src/bitfield/bitfield.c

   1 #include <bitfield/bitfield.h>
   2 #include <limits.h>
   3 #include <string.h>
   4 #include <stddef.h>
   5
   6 #define NIBBLE_SIZE (CHAR_BIT / 2)
   7
   8 uint64_t bitmask(const uint8_t numBits) {
   9     return (((uint64_t)0x1) << numBits) - 1;
  10 }
  11
  12 static uint16_t bitsToBytes(uint32_t bits) {
  13     uint8_t byte_count = bits / CHAR_BIT;
  14     if(bits % CHAR_BIT != 0) {
  15         ++byte_count;
  16     }
  17     return byte_count;
  18 }
  19
  20 uint64_t getBitField(uint64_t data, const uint16_t startBit,
  21         const uint16_t numBits, bool bigEndian) {
  22     uint8_t result[8] = {0};
  23     if(!bigEndian) {
  24         data = __builtin_bswap64(data);
  25     }
  26     copyBitsRightAligned((const uint8_t*)&data, sizeof(data), startBit, numBits,
  27             result, sizeof(result));
  28     uint64_t int_result = 0;
  29
  30     if(!bigEndian) {
  31         // we need to swap the byte order of the array to get it into a
  32         // uint64_t, but it's been right aligned so we have to be more careful
  33         for(int i = 0; i < bitsToBytes(numBits); i++) {
  34             int_result |= result[bitsToBytes(numBits) - i - 1] << (CHAR_BIT * i);
  35         }
  36     } else {
  37         int_result = *(uint64_t*)result;
  38     }
  39     return int_result;
  40 }
  41
  42 /**
  43  * TODO it would be nice to have a warning if you call with this a value that
  44  * won't fit in the number of bits you've specified it should use.
  45  */
  46 void setBitField(uint64_t* data, uint64_t value, const uint16_t startPos,
  47         const uint16_t numBits) {
  48     int shiftDistance = 64 - startPos - numBits;
  49     value <<= shiftDistance;
  50     *data &= ~(bitmask(numBits) << shiftDistance);
  51     *data |= value;
  52 }
  53
  54 uint8_t nthByte(const uint64_t source, const uint16_t byteNum) {
  55     return (source >> (64 - ((byteNum + 1) * CHAR_BIT))) & 0xFF;
  56 }
  57
  58 uint8_t getNibble(const uint8_t nibble_index, const uint8_t data[],
  59         const uint8_t length) {
  60     uint8_t byte_index = nibble_index / 2;
  61     uint8_t result;
  62     if(byte_index < length) {
  63         result = data[byte_index];
  64         if(nibble_index % 2 == 0) {
  65             result >>= NIBBLE_SIZE;
  66         }
  67     }
  68     result &= bitmask(NIBBLE_SIZE);
  69     return result;
  70 }
  71
  72 uint8_t getByte(const uint8_t byte_index, const uint8_t data[],
  73         const uint8_t length) {
  74     if(byte_index < length) {
  75         return data[byte_index];
  76     }
  77     return 0;
  78 }