variable length bit packing in C - bit-manipulation

Question

I am trying to write c functions ReadInt32, WriteInt32 for packing bits sequentially in a stream buffer with size optimisation in mind.
But my functions do not work as expected, I don't find the same values back after writing them in the buffer. I need a little bit of help to point what and where I have misunderstood.
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#define BITSET(x,n) (x | ( 1 << n))
#define BITCLEAR(x,n) (x & ~(1 << n))
#define ISBITSET(x,n) ((x & (1 << n)) != 0)
/**
#param nbits The number of bits to write
#param val the integer value to write
#param bufptr a pointer on the buffer to write in
#param nbitswritten the number of bits already written in the previous write.
#return the number of bits actually written.
*/
int WriteInt32(int nbits, int val, uint8_t* bufptr, int nbitswritten)
{
uint8_t* p=(uint8_t*)&val;
uint8_t* ptr = &(*(bufptr+nbitswritten));
for (int i=0; i<nbits; i++)
{
int bpos = (i&0x7);
*(ptr+(i>>3)) = ISBITSET(*(p+(i>>3)), bpos) ? (BITSET(*(ptr+(i>>3)), bpos)) : (BITCLEAR(*(ptr+(i>>3)), bpos));
}
return (nbitswritten + nbits);
}
int ReadInt32(int nbits, int& val, uint8_t* ptr, int nbitsread)
{
val = 0;
uint8_t* p = &(*(ptr+nbitsread));
for (int i=0; i<nbits; i++)
{
uint8_t ch = *(p+(i>>3));
int bpos = (i&0x7);
val = (ISBITSET(ch, bpos) ? BITSET(val, bpos) : BITCLEAR(val, bpos));
}
return (nbitsread+nbits);
}
int main (int argc, const char * argv[])
{
int value = 289; // for example I want to encode this value on 10 bits
unsigned char buf[50];
// packing
int nbitswritten = 0;
nbitswritten = WriteInt32 (10, value, buf, 50, nbitswritten);
// unpacking - read from buffer
int nbitsread = 0;
int rvalue;
nbitsread = ReadInt32(10, rvalue, buf, nbitsread);
if ( value == rvalue)
printf("encoding & decoding ok\n");
else
printf("encoding or decoding failed\n");
return 0;
};
Thank you
Olivier

Answer 1

This is an answer to my own question. How to write/read an integer encoded in smaller size than its natural size.
If it can help someone. That's good. ;)
exemple:
struct dd
{
short var1:5; // This variable is encoded on 5 bits
int var2:11; // This variable is encoded on 11 bits
uint8_t var3:3; // This variable is encoded on 3 bits
};
uint8_t buffer[100];
bzero(buffer, 100);
int nbitswritten = 0;
struct dd vardd = { 16, 452, 3 };
// write stuff
nbitswritten = WriteBits(5, vardd.var1, buffer, 100, nbitswritten);
nbitswritten = WriteBits(11, vardd.var2, buffer, 100, nbitswritten);
nbitswritten = WriteBits(3, vardd.var3, buffer, 100, nbitswritten);
// read back
int v1, v2, v3;
int nbitsread = 0;
struct dd result;
nbitsread = ReadInt(5, v1, buffer, 100, nbitsread);
nbitsread = ReadInt(11, v2, buffer, 100, nbitsread);
nbitsread = ReadInt(3, v3, buffer, 100, nbitsread);
result.var1 = (short)v1;
result.var2 = v2;
result.var3 = (uint8_t)v3;
Actually the Read functions comes from the Torque Engine library!!
This is my slightly modified version :
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#define BITSET(x,n) (x | ( 1 << n))
#define BITCLEAR(x,n) (x & ~(1 << n))
#define ISBITSET(x,n) ((x & (1 << n)) != 0)
/**
#param nbits The number of bits to write
#param val the value to write
#param bufptr the buffer to write to/in
#param buflen the maximum buffer length in byte/octet
#param nbitswritten the number of bits in bufptr already written.
#return the number of bits written;
*/
int WriteBits(int nbits, const void * val, uint8_t * bufptr, int buflen, int nbitswritten)
{
if (!bufptr || !val || !nbits || !buflen)
return 0;
if ( nbitswritten >= buflen<<8L )
return 0;
if ( nbits+nbitswritten>buflen<<8L )
return 0;
uint8_t* p=(uint8_t*)val;
int n = nbitswritten;
for (int i=0; i<nbits; i++)
{
int sbpos = (i&0x7);
int dbpos = (n&0x7);
*(bufptr+(n>>3)) = ISBITSET(*(p+(i>>3)), sbpos) ? (BITSET(*(bufptr+(n>>3)), dbpos)) : (BITCLEAR(*(bufptr+(n>>3)), dbpos));
n++;
}
nbitswritten = n;
return (n);
}
int ReadBits(int nbits, void * val, uint8_t * bufptr, int buflen, int nbitsread)
{
if (!nbits)
return 0;
uint8_t * stPtr = (bufptr + (nbitsread >> 3));
int byteCount = (nbits + 7) >> 3;
uint8_t * ptr = (TUint8*)val;
int downShift = nbitsread & 0x7;
int upShift = 8 - downShift;
uint8_t curB = *stPtr;
const uint8_t *stEnd = bufptr + buflen;
while(byteCount--)
{
stPtr++;
uint8_t nextB = stPtr < stEnd ? *stPtr : 0;
*ptr++ = (curB >> downShift) | (nextB << upShift);
curB = nextB;
}
nbitsread += nbits;
return nbitsread;
}
int ReadInt(int nbits, int &val, uint8_t * bufptr, int buflen, int nbitsread)
{
int ret = ReadBits(nbits, &val, bufptr, buflen, nbitsread);
if (nbits != 32) {
val &= (1<<nbits)-1;
}
return ret;
}
Olive