425 lines
15 KiB
C++
425 lines
15 KiB
C++
/**************************************************************************
|
|
* (C) copyright Fraunhofer - IIS (2024)
|
|
* All Rights Reserved
|
|
*
|
|
* This software and/or program is protected by copyright law and
|
|
* international treaties. Any unauthorized reproduction or distribution
|
|
* of this software and/or program, or any portion of it, may result in
|
|
* severe civil and criminal penalties, and will be prosecuted to the
|
|
* maximum extent possible under law.
|
|
*
|
|
\*************************************************************************/
|
|
|
|
#include <algorithm>
|
|
#include <cassert>
|
|
#include <cstring>
|
|
|
|
#include "bitbuffer.h"
|
|
|
|
const uint32_t BitMask[32 + 1] = {
|
|
0x0, 0x1, 0x3, 0x7, 0xf, 0x1f, 0x3f,
|
|
0x7f, 0xff, 0x1ff, 0x3ff, 0x7ff, 0xfff, 0x1fff,
|
|
0x3fff, 0x7fff, 0xffff, 0x1ffff, 0x3ffff, 0x7ffff, 0xfffff,
|
|
0x1fffff, 0x3fffff, 0x7fffff, 0xffffff, 0x1ffffff, 0x3ffffff, 0x7ffffff,
|
|
0xfffffff, 0x1fffffff, 0x3fffffff, 0x7fffffff, 0xffffffff};
|
|
|
|
void bitbuffer_create(HANDLE_BITBUFFER* hBitBuf, uint8_t* pBuffer, uint32_t bufSize) {
|
|
bitbuffer_init(*hBitBuf, pBuffer, bufSize, 0);
|
|
|
|
memset((*hBitBuf)->Buffer, 0, bufSize * sizeof(uint8_t));
|
|
}
|
|
|
|
void bitbuffer_delete(HANDLE_BITBUFFER hBitBuf) {
|
|
;
|
|
}
|
|
|
|
void bitbuffer_init(HANDLE_BITBUFFER hBitBuf, uint8_t* pBuffer, uint32_t bufSize,
|
|
uint32_t validBits) {
|
|
hBitBuf->ValidBits = validBits;
|
|
hBitBuf->ReadOffset = 0;
|
|
hBitBuf->WriteOffset = 0;
|
|
#ifdef BITCNT_ENABLE
|
|
hBitBuf->BitCnt = 0;
|
|
#endif
|
|
hBitBuf->BitNdx = 0;
|
|
|
|
hBitBuf->Buffer = pBuffer;
|
|
hBitBuf->bufSize = bufSize;
|
|
hBitBuf->bufBits = (bufSize << 3);
|
|
/*assure bufsize (2^n) */
|
|
assert(hBitBuf->ValidBits <= (int32_t)hBitBuf->bufBits);
|
|
assert((bufSize > 0) && (bufSize <= MAX_BUFSIZE_BYTES));
|
|
{
|
|
uint32_t x = 0, n = bufSize;
|
|
for (x = 0; n > 0; x++, n >>= 1) {
|
|
}
|
|
if (bufSize != ((uint32_t)1 << (x - 1))) {
|
|
assert(0);
|
|
}
|
|
}
|
|
}
|
|
|
|
void bitbuffer_reset(HANDLE_BITBUFFER hBitBuf) {
|
|
hBitBuf->ValidBits = 0;
|
|
hBitBuf->ReadOffset = 0;
|
|
#ifdef BITCNT_ENABLE
|
|
hBitBuf->BitCnt = 0;
|
|
#endif
|
|
hBitBuf->WriteOffset = 0;
|
|
hBitBuf->BitNdx = 0;
|
|
}
|
|
|
|
int32_t bitbuffer_get(HANDLE_BITBUFFER hBitBuf, const uint32_t numberOfBits) {
|
|
uint32_t byteOffset = hBitBuf->BitNdx >> 3;
|
|
uint32_t bitOffset = hBitBuf->BitNdx & 0x07;
|
|
|
|
hBitBuf->BitNdx = (hBitBuf->BitNdx + numberOfBits) & (hBitBuf->bufBits - 1);
|
|
#ifdef BITCNT_ENABLE
|
|
hBitBuf->BitCnt += numberOfBits;
|
|
#endif
|
|
hBitBuf->ValidBits -= numberOfBits;
|
|
|
|
uint32_t byteMask = hBitBuf->bufSize - 1;
|
|
|
|
uint32_t tx = (hBitBuf->Buffer[byteOffset & byteMask] << 24) |
|
|
(hBitBuf->Buffer[(byteOffset + 1) & byteMask] << 16) |
|
|
(hBitBuf->Buffer[(byteOffset + 2) & byteMask] << 8) |
|
|
hBitBuf->Buffer[(byteOffset + 3) & byteMask];
|
|
|
|
if (bitOffset) {
|
|
tx <<= bitOffset;
|
|
tx |= hBitBuf->Buffer[(byteOffset + 4) & byteMask] >> (8 - bitOffset);
|
|
}
|
|
|
|
return (tx >> (32 - numberOfBits));
|
|
}
|
|
|
|
#if defined(__clang__) && (__clang_major__ >= 12)
|
|
__attribute__((no_sanitize("unsigned-shift-base")))
|
|
#endif
|
|
int32_t
|
|
bitbuffer_get32(HANDLE_BITBUFFER hBitBuf) {
|
|
uint32_t BitNdx = hBitBuf->BitNdx + 32;
|
|
hBitBuf->BitNdx = BitNdx & (hBitBuf->bufBits - 1);
|
|
#ifdef BITCNT_ENABLE
|
|
hBitBuf->BitCnt += 32;
|
|
#endif
|
|
hBitBuf->ValidBits -= 32;
|
|
|
|
uint32_t byteOffset = (BitNdx - 1) >> 3;
|
|
if (BitNdx <= hBitBuf->bufBits) {
|
|
uint32_t cache = (hBitBuf->Buffer[(byteOffset - 3)] << 24) |
|
|
(hBitBuf->Buffer[(byteOffset - 2)] << 16) |
|
|
(hBitBuf->Buffer[(byteOffset - 1)] << 8) | hBitBuf->Buffer[(byteOffset - 0)];
|
|
|
|
if ((BitNdx = (BitNdx & 7)) != 0) {
|
|
cache =
|
|
(cache >> (8 - BitNdx)) | ((uint32_t)hBitBuf->Buffer[byteOffset - 4] << (24 + BitNdx));
|
|
}
|
|
return (cache);
|
|
} else {
|
|
uint32_t byte_mask = hBitBuf->bufSize - 1;
|
|
uint32_t cache = (hBitBuf->Buffer[(byteOffset - 3) & byte_mask] << 24) |
|
|
(hBitBuf->Buffer[(byteOffset - 2) & byte_mask] << 16) |
|
|
(hBitBuf->Buffer[(byteOffset - 1) & byte_mask] << 8) |
|
|
hBitBuf->Buffer[(byteOffset - 0) & byte_mask];
|
|
|
|
if ((BitNdx = (BitNdx & 7)) != 0) {
|
|
cache = (cache >> (8 - BitNdx)) |
|
|
((uint32_t)hBitBuf->Buffer[(byteOffset - 4) & byte_mask] << (24 + BitNdx));
|
|
}
|
|
return (cache);
|
|
}
|
|
}
|
|
|
|
int32_t bitbuffer_getBwd(HANDLE_BITBUFFER hBitBuf, const uint32_t numberOfBits) {
|
|
uint32_t byteOffset = hBitBuf->BitNdx >> 3;
|
|
uint32_t bitOffset = hBitBuf->BitNdx & 0x07;
|
|
uint32_t byteMask = hBitBuf->bufSize - 1;
|
|
int32_t i;
|
|
|
|
hBitBuf->BitNdx = (hBitBuf->BitNdx - numberOfBits) & (hBitBuf->bufBits - 1);
|
|
#ifdef BITCNT_ENABLE
|
|
hBitBuf->BitCnt -= numberOfBits;
|
|
#endif
|
|
hBitBuf->ValidBits += numberOfBits;
|
|
|
|
uint32_t tx = hBitBuf->Buffer[(byteOffset - 3) & byteMask] << 24 |
|
|
hBitBuf->Buffer[(byteOffset - 2) & byteMask] << 16 |
|
|
hBitBuf->Buffer[(byteOffset - 1) & byteMask] << 8 |
|
|
hBitBuf->Buffer[byteOffset & byteMask];
|
|
uint32_t txa = 0x0;
|
|
|
|
tx >>= (8 - bitOffset);
|
|
|
|
if (bitOffset && numberOfBits > 24) {
|
|
tx |= hBitBuf->Buffer[(byteOffset - 4) & byteMask] << (24 + bitOffset);
|
|
}
|
|
|
|
/* in place turn around */
|
|
for (i = 0; i < 16; i++) {
|
|
uint32_t bitMaskR = 0x00000001 << i;
|
|
uint32_t bitMaskL = 0x80000000 >> i;
|
|
|
|
txa |= (tx & bitMaskR) << (31 - (i << 1));
|
|
txa |= (tx & bitMaskL) >> (31 - (i << 1));
|
|
}
|
|
|
|
return (txa >> (32 - numberOfBits));
|
|
}
|
|
|
|
void bitbuffer_put(HANDLE_BITBUFFER hBitBuf, uint32_t value, const uint32_t numberOfBits) {
|
|
if (numberOfBits != 0) {
|
|
uint32_t byteOffset0 = hBitBuf->BitNdx >> 3;
|
|
uint32_t bitOffset = hBitBuf->BitNdx & 0x7;
|
|
|
|
hBitBuf->BitNdx = (hBitBuf->BitNdx + numberOfBits) & (hBitBuf->bufBits - 1);
|
|
#ifdef BITCNT_ENABLE
|
|
hBitBuf->BitCnt += numberOfBits;
|
|
#endif
|
|
hBitBuf->ValidBits += numberOfBits;
|
|
|
|
uint32_t byteMask = hBitBuf->bufSize - 1;
|
|
|
|
uint32_t byteOffset1 = (byteOffset0 + 1) & byteMask;
|
|
uint32_t byteOffset2 = (byteOffset0 + 2) & byteMask;
|
|
uint32_t byteOffset3 = (byteOffset0 + 3) & byteMask;
|
|
|
|
// Create tmp containing free bits at the left border followed by bits to write, LSB's are
|
|
// cleared, if available Create mask to apply upon all buffer bytes
|
|
uint32_t tmp = (value << (32 - numberOfBits)) >> bitOffset;
|
|
uint32_t mask = ~((BitMask[numberOfBits] << (32 - numberOfBits)) >> bitOffset);
|
|
|
|
// read all 4 bytes from buffer and create a 32-bit cache
|
|
uint32_t cache = (((uint32_t)hBitBuf->Buffer[byteOffset0]) << 24) |
|
|
(((uint32_t)hBitBuf->Buffer[byteOffset1]) << 16) |
|
|
(((uint32_t)hBitBuf->Buffer[byteOffset2]) << 8) |
|
|
(((uint32_t)hBitBuf->Buffer[byteOffset3]) << 0);
|
|
|
|
cache = (cache & mask) | tmp;
|
|
hBitBuf->Buffer[byteOffset0] = (uint8_t)(cache >> 24);
|
|
hBitBuf->Buffer[byteOffset1] = (uint8_t)(cache >> 16);
|
|
hBitBuf->Buffer[byteOffset2] = (uint8_t)(cache >> 8);
|
|
hBitBuf->Buffer[byteOffset3] = (uint8_t)(cache >> 0);
|
|
|
|
if ((bitOffset + numberOfBits) > 32) {
|
|
uint32_t byteOffset4 = (byteOffset0 + 4) & byteMask;
|
|
// remaining bits: in range 1..7
|
|
// replace MSBits of next byte in buffer by LSBits of "value"
|
|
int32_t bits = (bitOffset + numberOfBits) & 7;
|
|
cache = (uint32_t)hBitBuf->Buffer[byteOffset4] & (~(BitMask[bits] << (8 - bits)));
|
|
cache |= value << (8 - bits);
|
|
hBitBuf->Buffer[byteOffset4] = (uint8_t)cache;
|
|
}
|
|
}
|
|
}
|
|
|
|
void bitbuffer_putBwd(HANDLE_BITBUFFER hBitBuf, uint32_t value, const uint32_t numberOfBits) {
|
|
uint32_t byteOffset = hBitBuf->BitNdx >> 3;
|
|
uint32_t bitOffset = 7 - (hBitBuf->BitNdx & 0x07);
|
|
uint32_t byteMask = hBitBuf->bufSize - 1;
|
|
|
|
uint32_t mask = ~(BitMask[numberOfBits] << bitOffset);
|
|
uint32_t tmp = 0x0000;
|
|
int32_t i;
|
|
|
|
hBitBuf->BitNdx = (hBitBuf->BitNdx - numberOfBits) & (hBitBuf->bufBits - 1);
|
|
#ifdef BITCNT_ENABLE
|
|
hBitBuf->BitCnt -= numberOfBits;
|
|
#endif
|
|
hBitBuf->ValidBits -= numberOfBits;
|
|
|
|
/* in place turn around */
|
|
for (i = 0; i < 16; i++) {
|
|
uint32_t bitMaskR = 0x00000001 << i;
|
|
uint32_t bitMaskL = 0x80000000 >> i;
|
|
|
|
tmp |= (value & bitMaskR) << (31 - (i << 1));
|
|
tmp |= (value & bitMaskL) >> (31 - (i << 1));
|
|
}
|
|
value = tmp;
|
|
tmp = value >> (32 - numberOfBits) << bitOffset;
|
|
|
|
hBitBuf->Buffer[byteOffset & byteMask] =
|
|
(hBitBuf->Buffer[byteOffset & byteMask] & (mask)) | (uint8_t)(tmp);
|
|
hBitBuf->Buffer[(byteOffset - 1) & byteMask] =
|
|
(hBitBuf->Buffer[(byteOffset - 1) & byteMask] & (mask >> 8)) | (uint8_t)(tmp >> 8);
|
|
hBitBuf->Buffer[(byteOffset - 2) & byteMask] =
|
|
(hBitBuf->Buffer[(byteOffset - 2) & byteMask] & (mask >> 16)) | (uint8_t)(tmp >> 16);
|
|
hBitBuf->Buffer[(byteOffset - 3) & byteMask] =
|
|
(hBitBuf->Buffer[(byteOffset - 3) & byteMask] & (mask >> 24)) | (uint8_t)(tmp >> 24);
|
|
|
|
if ((bitOffset + numberOfBits) > 32) {
|
|
hBitBuf->Buffer[(byteOffset - 4) & byteMask] =
|
|
(uint8_t)(value >> (64 - numberOfBits - bitOffset)) |
|
|
(hBitBuf->Buffer[(byteOffset - 4) & byteMask] &
|
|
~(BitMask[bitOffset] >> (32 - numberOfBits)));
|
|
}
|
|
}
|
|
|
|
void bitbuffer_pushBack(HANDLE_BITBUFFER hBitBuf, const uint32_t numberOfBits, uint8_t config) {
|
|
#ifdef BITCNT_ENABLE
|
|
hBitBuf->BitCnt -= numberOfBits;
|
|
#endif
|
|
hBitBuf->ValidBits = (config == 0) ? (hBitBuf->ValidBits + (int32_t)numberOfBits)
|
|
: (hBitBuf->ValidBits - (int32_t)numberOfBits);
|
|
hBitBuf->BitNdx = ((uint32_t)((int32_t)hBitBuf->BitNdx - (int32_t)numberOfBits)) &
|
|
((int32_t)hBitBuf->bufBits - 1);
|
|
}
|
|
|
|
void bitbuffer_pushForward(HANDLE_BITBUFFER hBitBuf, const uint32_t numberOfBits, uint8_t config) {
|
|
#ifdef BITCNT_ENABLE
|
|
hBitBuf->BitCnt += numberOfBits;
|
|
#endif
|
|
hBitBuf->ValidBits = (config == 0) ? (hBitBuf->ValidBits - (int32_t)numberOfBits)
|
|
: (hBitBuf->ValidBits + (int32_t)numberOfBits);
|
|
hBitBuf->BitNdx =
|
|
(uint32_t)((int32_t)hBitBuf->BitNdx + (int32_t)numberOfBits) & (hBitBuf->bufBits - 1);
|
|
}
|
|
|
|
int32_t bitbuffer_getValidBits(HANDLE_BITBUFFER hBitBuf) {
|
|
return hBitBuf->ValidBits;
|
|
}
|
|
|
|
int32_t bitbuffer_getFreeBits(HANDLE_BITBUFFER hBitBuf) {
|
|
return (hBitBuf->bufBits - hBitBuf->ValidBits);
|
|
}
|
|
|
|
#ifdef BITCNT_ENABLE
|
|
void bitbuffer_byteAlign(HANDLE_BITBUFFER hBitBuf, uint8_t config) {
|
|
int32_t alignment = hBitBuf->BitCnt & 0x07;
|
|
if (alignment) {
|
|
if (config == 0)
|
|
bitbuffer_pushForward(hBitBuf, 8 - alignment, config); /* BS_READER */
|
|
else
|
|
bitbuffer_put(hBitBuf, 0, 8 - alignment); /* BS_WRITER */
|
|
}
|
|
hBitBuf->BitCnt = 0;
|
|
}
|
|
|
|
void bitbuffer_setBitCnt(HANDLE_BITBUFFER hBitBuf, const uint32_t value) {
|
|
hBitBuf->BitCnt = value;
|
|
}
|
|
|
|
int32_t bitbuffer_getBitCnt(HANDLE_BITBUFFER hBitBuf) {
|
|
return hBitBuf->BitCnt;
|
|
}
|
|
#endif
|
|
|
|
void bitbuffer_feed(HANDLE_BITBUFFER hBitBuf, const uint8_t* inputBuffer, const uint32_t bufferSize,
|
|
uint32_t* bytesValid) {
|
|
inputBuffer = &inputBuffer[bufferSize - *bytesValid];
|
|
|
|
uint32_t bTotal = 0;
|
|
|
|
uint32_t bToRead =
|
|
std::min(hBitBuf->bufBits,
|
|
(uint32_t)std::max(0, ((int32_t)hBitBuf->bufBits - hBitBuf->ValidBits))) >>
|
|
3;
|
|
uint32_t noOfBytes =
|
|
std::min(bToRead, *bytesValid); //(bToRead < *bytesValid) ? bToRead : *bytesValid ;
|
|
|
|
while (noOfBytes > 0) {
|
|
/* split read to buffer size */
|
|
bToRead = hBitBuf->bufSize - hBitBuf->ReadOffset;
|
|
bToRead = std::min(bToRead, noOfBytes); //(bToRead < noOfBytes) ? bToRead : noOfBytes ;
|
|
|
|
/* copy 'bToRead' bytes from 'ptr' to inputbuffer */
|
|
memcpy(&hBitBuf->Buffer[hBitBuf->ReadOffset], inputBuffer, bToRead * sizeof(uint8_t));
|
|
|
|
/* add noOfBits to number of valid bits in buffer */
|
|
hBitBuf->ValidBits += (int32_t)(bToRead << 3);
|
|
bTotal += bToRead;
|
|
inputBuffer += bToRead;
|
|
|
|
hBitBuf->ReadOffset = (hBitBuf->ReadOffset + bToRead) & (hBitBuf->bufSize - 1);
|
|
noOfBytes -= bToRead;
|
|
}
|
|
|
|
*bytesValid -= bTotal;
|
|
}
|
|
|
|
static void bitbuffer_copyAlignedBlock(HANDLE_BITBUFFER h_BitBufSrc, uint8_t* dstBuffer,
|
|
uint32_t bToRead) {
|
|
uint32_t byteOffset = h_BitBufSrc->BitNdx >> 3;
|
|
const uint32_t byteMask = h_BitBufSrc->bufSize - 1;
|
|
|
|
uint8_t* pBBB = h_BitBufSrc->Buffer;
|
|
for (uint32_t i = 0; i < bToRead; i++) {
|
|
dstBuffer[i] = pBBB[(byteOffset + i) & byteMask];
|
|
}
|
|
|
|
bToRead <<= 3;
|
|
|
|
h_BitBufSrc->BitNdx = (h_BitBufSrc->BitNdx + bToRead) & (h_BitBufSrc->bufBits - 1);
|
|
#ifdef BITCNT_ENABLE
|
|
h_BitBufSrc->BitCnt += bToRead;
|
|
#endif
|
|
h_BitBufSrc->ValidBits -= bToRead;
|
|
}
|
|
|
|
void bitbuffer_copy(HANDLE_BITBUFFER h_BitBufDst, HANDLE_BITBUFFER h_BitBufSrc,
|
|
uint32_t* bytesValid) {
|
|
int32_t bTotal = 0;
|
|
|
|
/* limit noOfBytes to valid bytes in src buffer and available bytes in dst buffer */
|
|
uint32_t bToRead = h_BitBufSrc->ValidBits >> 3;
|
|
uint32_t noOfBytes =
|
|
std::min(bToRead, *bytesValid); //(*bytesValid < bToRead) ? *bytesValid : bToRead ;
|
|
bToRead = bitbuffer_getFreeBits(h_BitBufDst);
|
|
noOfBytes = std::min(bToRead, noOfBytes); //(bToRead < noOfBytes) ? bToRead : noOfBytes;
|
|
|
|
while (noOfBytes > 0) {
|
|
/* Split Read to buffer size */
|
|
bToRead = h_BitBufDst->bufSize - h_BitBufDst->ReadOffset;
|
|
bToRead = std::min(noOfBytes, bToRead); //(noOfBytes < bToRead) ? noOfBytes : bToRead ;
|
|
|
|
/* copy 'bToRead' bytes from buffer to buffer */
|
|
if (!(h_BitBufSrc->BitNdx & 0x07)) {
|
|
bitbuffer_copyAlignedBlock(h_BitBufSrc, h_BitBufDst->Buffer + h_BitBufDst->ReadOffset,
|
|
bToRead);
|
|
} else {
|
|
for (uint32_t i = 0; i < bToRead; i++) {
|
|
h_BitBufDst->Buffer[h_BitBufDst->ReadOffset + i] = (uint8_t)bitbuffer_get(h_BitBufSrc, 8);
|
|
}
|
|
}
|
|
|
|
/* add noOfBits to number of valid bits in buffer */
|
|
h_BitBufDst->ValidBits += bToRead << 3;
|
|
bTotal += bToRead;
|
|
|
|
h_BitBufDst->ReadOffset = (h_BitBufDst->ReadOffset + bToRead) & (h_BitBufDst->bufSize - 1);
|
|
noOfBytes -= bToRead;
|
|
}
|
|
|
|
*bytesValid -= bTotal;
|
|
}
|
|
|
|
void bitbuffer_fetch(HANDLE_BITBUFFER hBitBuf, uint8_t* outBuf, uint32_t* writeBytes) {
|
|
uint8_t* outputBuffer = outBuf;
|
|
uint32_t bTotal = 0;
|
|
|
|
uint32_t bToWrite = (hBitBuf->ValidBits) >> 3;
|
|
uint32_t noOfBytes =
|
|
std::min(bToWrite, *writeBytes); //(bToWrite < *writeBytes) ? bToWrite : *writeBytes ;
|
|
|
|
while (noOfBytes > 0) {
|
|
/* split write to buffer size */
|
|
bToWrite = hBitBuf->bufSize - hBitBuf->WriteOffset;
|
|
bToWrite = std::min(bToWrite, noOfBytes); //(bToWrite < noOfBytes) ? bToWrite : noOfBytes ;
|
|
|
|
/* copy 'bToWrite' bytes from bitbuffer to outputbuffer */
|
|
memcpy(outputBuffer, &hBitBuf->Buffer[hBitBuf->WriteOffset], bToWrite * sizeof(uint8_t));
|
|
|
|
/* sub noOfBits from number of valid bits in buffer */
|
|
hBitBuf->ValidBits -= bToWrite << 3;
|
|
bTotal += bToWrite;
|
|
outputBuffer += bToWrite;
|
|
|
|
hBitBuf->WriteOffset = (hBitBuf->WriteOffset + bToWrite) & (hBitBuf->bufSize - 1);
|
|
noOfBytes -= bToWrite;
|
|
}
|
|
|
|
*writeBytes = bTotal;
|
|
}
|