You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
219 lines
6.3 KiB
219 lines
6.3 KiB
#define RBUFP(p,pos) (((uint8_t*)(p)) + (pos)) |
|
#define RBUFB(p,pos) (*(uint8_t*)RBUFP((p),(pos))) |
|
#define RBUFW(p,pos) (*(uint16_t*)RBUFP((p),(pos))) |
|
#define RBUFL(p,pos) (*(uint32_t*)RBUFP((p),(pos))) |
|
#define RBUFF(p,pos) (*(float*)RBUFP((p),(pos))) |
|
|
|
#define WBUFP(p,pos) (((uint8_t*)(p)) + (pos)) |
|
#define WBUFB(p,pos) (*(uint8_t*)WBUFP((p),(pos))) |
|
#define WBUFW(p,pos) (*(uint16_t*)WBUFP((p),(pos))) |
|
#define WBUFL(p,pos) (*(uint32_t*)WBUFP((p),(pos))) |
|
#define WBUFF(p,pos) (*(float*)WBUFP((p),(pos))) |
|
|
|
#include <stdint.h> |
|
|
|
uint32_t packBitsBE(uint8_t* target, uint64_t value, int32_t byteOffset, int32_t bitOffset, uint8_t lengthInBit) |
|
{ |
|
byteOffset += (bitOffset >> 3); //correct bitOffsets>=8 |
|
bitOffset %= 8; |
|
|
|
uint64_t bitmask = 0xFFFFFFFFFFFFFFFFLL; //Generate bitmask |
|
|
|
bitmask >>= (64 - lengthInBit); |
|
bitmask <<= bitOffset; |
|
|
|
value <<= bitOffset; //shift value |
|
value &= bitmask; |
|
|
|
bitmask ^= 0xFFFFFFFFFFFFFFFFLL; //invert bitmask |
|
|
|
if ((lengthInBit + bitOffset) <= 8) //write shifted value to target |
|
{ |
|
uint8_t* dataPointer = (uint8_t*)&target[byteOffset]; |
|
|
|
uint8_t bitmaskUC = (uint8_t)bitmask; |
|
uint8_t valueUC = (uint8_t)value; |
|
|
|
*dataPointer &= bitmaskUC; |
|
*dataPointer |= valueUC; |
|
} |
|
else if ((lengthInBit + bitOffset) <= 16) |
|
{ |
|
uint16_t* dataPointer = (uint16_t*)&target[byteOffset]; |
|
|
|
uint16_t bitmaskUC = (uint16_t)bitmask; |
|
uint16_t valueUC = (uint16_t)value; |
|
|
|
*dataPointer &= bitmaskUC; |
|
*dataPointer |= valueUC; |
|
} |
|
else if ((lengthInBit + bitOffset) <= 32) |
|
{ |
|
uint32_t* dataPointer = (uint32_t*)&target[byteOffset]; |
|
|
|
uint32_t bitmaskUC = (uint32_t)bitmask; |
|
uint32_t valueUC = (uint32_t)value; |
|
|
|
*dataPointer &= bitmaskUC; |
|
*dataPointer |= valueUC; |
|
} |
|
else if ((lengthInBit + bitOffset) <= 64) |
|
{ |
|
uint64_t* dataPointer = (uint64_t*)&target[byteOffset]; |
|
|
|
*dataPointer &= bitmask; |
|
*dataPointer |= value; |
|
} |
|
else |
|
{ |
|
//ShowError("Pack Bits Error: packBitsBE(...) not implemented for targetsizes above 64 bits.\n Targetsize: %d\n", (lengthInBit + bitOffset)); |
|
} |
|
return ((byteOffset << 3) + bitOffset + lengthInBit); |
|
} |
|
|
|
uint32_t packBitsBE(uint8_t* target, uint64_t value, int32_t bitOffset, uint8_t lengthInBit) |
|
{ |
|
return packBitsBE(target, value, 0, bitOffset, lengthInBit); |
|
} |
|
|
|
uint64_t unpackBitsBE(uint8_t* target, int32_t byteOffset, int32_t bitOffset, uint8_t lengthInBit) |
|
{ |
|
byteOffset += (bitOffset >> 3); |
|
bitOffset %= 8; |
|
|
|
uint64_t bitmask = 0xFFFFFFFFFFFFFFFFLL; |
|
|
|
bitmask >>= (64 - lengthInBit); |
|
bitmask <<= bitOffset; |
|
|
|
uint64_t retVal; |
|
|
|
if ((lengthInBit + bitOffset) <= 8) |
|
{ |
|
uint8_t* dataPointer = (uint8_t*)&target[byteOffset]; |
|
|
|
retVal = ((*dataPointer)&(uint8_t)bitmask) >> bitOffset; |
|
} |
|
else if ((lengthInBit + bitOffset) <= 16) |
|
{ |
|
uint16_t* dataPointer = (uint16_t*)&target[byteOffset]; |
|
|
|
retVal = ((*dataPointer)&(uint16_t)bitmask) >> bitOffset; |
|
} |
|
else if ((lengthInBit + bitOffset) <= 32) |
|
{ |
|
uint32_t* dataPointer = (uint32_t*)&target[byteOffset]; |
|
|
|
retVal = ((*dataPointer)&(uint32_t)bitmask) >> bitOffset; |
|
} |
|
else if ((lengthInBit + bitOffset) <= 64) |
|
{ |
|
uint64_t* dataPointer = (uint64_t*)&target[byteOffset]; |
|
|
|
retVal = ((*dataPointer)&(uint64_t)bitmask) >> bitOffset; |
|
} |
|
else |
|
{ |
|
//ShowError("Unpack Bits Error: unpackBits(...) not implemented for targetsizes above 64 bits.\n Targetsize: %d\n", (lengthInBit + bitOffset)); |
|
return 0; |
|
} |
|
return retVal; |
|
} |
|
|
|
uint64_t unpackBitsBE(uint8_t* target, int32_t bitOffset, uint8_t lengthInBit) |
|
{ |
|
return unpackBitsBE(target, 0, bitOffset, lengthInBit); |
|
} |
|
|
|
uint32_t packBitsLE(uint8_t* target, uint64_t value, int32_t byteOffset, int32_t bitOffset, uint8_t lengthInBit) |
|
{ |
|
byteOffset += (bitOffset >> 3); //correct bitOffsets >= 8 |
|
bitOffset %= 8; |
|
|
|
uint8_t bytesNeeded; //calculate how many bytes are needed |
|
if ((bitOffset + lengthInBit) <= 8) |
|
bytesNeeded = 1; |
|
else if ((bitOffset + lengthInBit) <= 16) |
|
bytesNeeded = 2; |
|
else if ((bitOffset + lengthInBit) <= 32) |
|
bytesNeeded = 4; |
|
else if ((bitOffset + lengthInBit) <= 64) |
|
bytesNeeded = 8; |
|
else |
|
{ |
|
//ShowError("Pack Bits Error: packBitsLE(...) not implemented for targetsizes above 64 bits.\n Targetsize: %d\n", (lengthInBit + bitOffset)); |
|
return 0; |
|
} |
|
|
|
uint8_t* modifiedTarget = new uint8_t[bytesNeeded]; //convert byteOrder to Big Endian |
|
|
|
for (uint8_t curByte = 0; curByte < bytesNeeded; ++curByte) |
|
{ |
|
modifiedTarget[curByte] = target[byteOffset + (bytesNeeded - 1) - curByte]; |
|
} |
|
|
|
int32_t newBitOffset = (bytesNeeded << 3) - (bitOffset + lengthInBit); //calculate new bitOffset |
|
|
|
packBitsBE(&modifiedTarget[0], value, 0, newBitOffset, lengthInBit); //write data to modified array |
|
|
|
for (uint8_t curByte = 0; curByte < bytesNeeded; ++curByte) //copy back to target |
|
{ |
|
target[byteOffset + (bytesNeeded - 1) - curByte] = modifiedTarget[curByte]; |
|
} |
|
|
|
if (modifiedTarget) delete[] modifiedTarget; |
|
return ((byteOffset << 3) + bitOffset + lengthInBit); |
|
} |
|
|
|
uint32_t packBitsLE(uint8_t* target, uint64_t value, int32_t bitOffset, uint8_t lengthInBit) |
|
{ |
|
return packBitsLE(target, value, 0, bitOffset, lengthInBit); |
|
} |
|
|
|
uint64_t unpackBitsLE(uint8_t* target, int32_t byteOffset, int32_t bitOffset, uint8_t lengthInBit) |
|
{ |
|
byteOffset += (bitOffset >> 3); |
|
bitOffset %= 8; |
|
|
|
uint8_t bytesNeeded; |
|
if ((bitOffset + lengthInBit) <= 8) |
|
bytesNeeded = 1; |
|
else if ((bitOffset + lengthInBit) <= 16) |
|
bytesNeeded = 2; |
|
else if ((bitOffset + lengthInBit) <= 32) |
|
bytesNeeded = 4; |
|
else if ((bitOffset + lengthInBit) <= 64) |
|
bytesNeeded = 8; |
|
else |
|
{ |
|
//ShowError("Unpack Bits Error: packBitsLE(...) not implemented for targetsizes above 64 bits.\n Targetsize: %d\n", (lengthInBit + bitOffset)); |
|
return 0; |
|
} |
|
|
|
uint64_t retVal; |
|
|
|
uint8_t* modifiedTarget = new uint8_t[bytesNeeded]; |
|
|
|
for (uint8_t curByte = 0; curByte < bytesNeeded; ++curByte) |
|
{ |
|
modifiedTarget[curByte] = target[byteOffset + (bytesNeeded - 1) - curByte]; |
|
} |
|
if (bytesNeeded == 1) |
|
{ |
|
uint8_t bitmask = 0xFF >> bitOffset; |
|
retVal = (modifiedTarget[0] & bitmask) >> (8 - (lengthInBit + bitOffset)); |
|
} |
|
else |
|
{ |
|
int32_t newBitOffset = (bytesNeeded * 8) - (bitOffset + lengthInBit); |
|
retVal = unpackBitsBE(&modifiedTarget[0], 0, newBitOffset, lengthInBit); |
|
} |
|
|
|
if (modifiedTarget) delete[]modifiedTarget; |
|
return retVal; |
|
} |
|
|
|
uint64_t unpackBitsLE(uint8_t* target, int32_t bitOffset, uint8_t lengthInBit) |
|
{ |
|
return unpackBitsLE(target, 0, bitOffset, lengthInBit); |
|
}
|
|
|