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 #define PREPARE_FIRST_COPY()                                      \
   9     do {                                                          \
  10     if (source_length >= (CHAR_BIT - destination_offset_modulo)) {              \
  11         *destination     &= reverse_mask[destination_offset_modulo];              \
  12         source_length -= CHAR_BIT - destination_offset_modulo;                  \
  13     } else {                                                      \
  14         *destination     &= reverse_mask[destination_offset_modulo]               \
  15               | reverse_mask_xor[destination_offset_modulo + source_length + 1];\
  16          c       &= reverse_mask[destination_offset_modulo + source_length    ];\
  17         source_length = 0;                                              \
  18     } } while (0)
  19
  20 /**
  21  * Find the ending bit of a bitfield within the final byte.
  22  *
  23  * Returns: a bit position from 0 to 7.
  24  */
  25 static uint8_t findEndBit(const uint16_t startBit, const uint16_t numBits) {
  26     int endBit = (startBit + numBits) % CHAR_BIT;
  27     return endBit == 0 ? CHAR_BIT : endBit;
  28 }
  29
  30
  31 // TODO can probably remove this
  32 static int byteForBit(const uint16_t startBit) {
  33     return startBit / CHAR_BIT;
  34 }
  35
  36 /* Thanks to
  37  * http://stackoverflow.com/questions/3534535/whats-a-time-efficient-algorithm-to-copy-unaligned-bit-arrays
  38  */
  39 static void bitarray_copy(const uint8_t* source_origin, int source_offset,
  40         int source_length, uint8_t* destination_origin, int destination_offset) {
  41     static const uint8_t mask[] =
  42         { 0x55, 0x01, 0x03, 0x07, 0x0f, 0x1f, 0x3f, 0x7f, 0xff };
  43     static const uint8_t reverse_mask[] =
  44         { 0x55, 0x80, 0xc0, 0xe0, 0xf0, 0xf8, 0xfc, 0xfe, 0xff };
  45     static const uint8_t reverse_mask_xor[] =
  46         { 0xff, 0x7f, 0x3f, 0x1f, 0x0f, 0x07, 0x03, 0x01, 0x00 };
  47
  48     if(source_length < 1) {
  49         return;
  50     }
  51
  52     int source_offset_modulo;
  53     int destination_offset_modulo;
  54
  55     const uint8_t* source = source_origin + byteForBit(source_offset);
  56     uint8_t* destination = destination_origin + byteForBit(destination_offset);
  57
  58     source_offset_modulo = source_offset % CHAR_BIT;
  59     destination_offset_modulo = destination_offset % CHAR_BIT;
  60
  61     if(source_offset_modulo == destination_offset_modulo) {
  62         if(source_offset_modulo) {
  63             uint8_t c;
  64
  65             c = reverse_mask_xor[destination_offset_modulo]     & *source++;
  66
  67             PREPARE_FIRST_COPY();
  68             *destination++ |= c;
  69         }
  70
  71         int byte_len = source_length / CHAR_BIT;
  72         int source_length_modulo = source_length % CHAR_BIT;
  73
  74         if(byte_len) {
  75             memcpy(destination, source, byte_len);
  76             source += byte_len;
  77             destination += byte_len;
  78         }
  79         if(source_length_modulo) {
  80             *destination     &= reverse_mask_xor[source_length_modulo];
  81             *destination |= reverse_mask[source_length_modulo]     & *source;
  82         }
  83     } else {
  84         int bit_diff_ls;
  85         int bit_diff_rs;
  86         uint8_t c;
  87         /*
  88          * Begin: Line things up on destination.
  89          */
  90         if (source_offset_modulo > destination_offset_modulo) {
  91             bit_diff_ls = source_offset_modulo - destination_offset_modulo;
  92             bit_diff_rs = CHAR_BIT - bit_diff_ls;
  93
  94             c = *source++ << bit_diff_ls;
  95             c |= *source >> bit_diff_rs;
  96             c     &= reverse_mask_xor[destination_offset_modulo];
  97         } else {
  98             bit_diff_rs = destination_offset_modulo - source_offset_modulo;
  99             bit_diff_ls = CHAR_BIT - bit_diff_rs;
 100
 101             c = *source >> bit_diff_rs     &
 102                 reverse_mask_xor[destination_offset_modulo];
 103         }
 104         PREPARE_FIRST_COPY();
 105         *destination++ |= c;
 106
 107         /*
 108          * Middle: copy with only shifting the source.
 109          */
 110         int byte_len = source_length / CHAR_BIT;
 111
 112         while(--byte_len >= 0) {
 113             c = *source++ << bit_diff_ls;
 114             c |= *source >> bit_diff_rs;
 115             *destination++ = c;
 116         }
 117
 118         /*
 119          * End: copy the remaing bits;
 120          */
 121         int source_length_modulo = source_length % CHAR_BIT;
 122         if(source_length_modulo) {
 123             c = *source++ << bit_diff_ls;
 124             c |= *source >> bit_diff_rs;
 125             c &= reverse_mask[source_length_modulo];
 126
 127             *destination &= reverse_mask_xor[source_length_modulo];
 128             *destination |= c;
 129         }
 130     }
 131 }
 132
 133 uint64_t bitmask(const uint8_t numBits) {
 134     return (((uint64_t)0x1) << numBits) - 1;
 135 }
 136
 137 uint64_t getBitField(uint64_t data, const uint16_t startBit,
 138         const uint16_t numBits, bool bigEndian) {
 139     uint64_t result;
 140     getBits(startBit, numBits, (const uint8_t*)&data,
 141             CHAR_BIT * sizeof(uint64_t),
 142             bigEndian ? ENDIANNESS_BIG_ENDIAN : ENDIANNESS_LITTLE_ENDIAN,
 143             &result);
 144     return result;
 145 }
 146
 147 /**
 148  * TODO it would be nice to have a warning if you call with this a value that
 149  * won't fit in the number of bits you've specified it should use.
 150  */
 151 void setBitField(uint64_t* data, uint64_t value, int startBit, int numBits) {
 152     int shiftDistance = 64 - startBit - numBits;
 153     value <<= shiftDistance;
 154     *data &= ~(bitmask(numBits) << shiftDistance);
 155     *data |= value;
 156 }
 157
 158 uint8_t nthByte(uint64_t source, int byteNum) {
 159     return (source >> (64 - ((byteNum + 1) * CHAR_BIT))) & 0xFF;
 160 }
 161
 162 uint8_t getNibble(const uint8_t nibble_index, const uint8_t data[],
 163         const uint8_t length, Endianness endianness) {
 164     uint8_t byte_index = nibble_index / 2;
 165     uint8_t result;
 166     if(byte_index < length) {
 167         result = data[byte_index];
 168         if(nibble_index % 2 == 0) {
 169             result >>= NIBBLE_SIZE;
 170         }
 171     }
 172     result &= bitmask(NIBBLE_SIZE);
 173     return result;
 174 }
 175
 176 // TODO getBytes, return status and store in output parameter
 177 uint8_t getByte(const uint8_t byte_index, const uint8_t data[],
 178         const uint8_t length, Endianness endianness) {
 179     if(byte_index < length) {
 180         return data[byte_index];
 181     }
 182     return 0;
 183 }
 184
 185 void getBits(const uint16_t start_index, const uint16_t field_size,
 186         const uint8_t data[], const uint8_t length, Endianness endianness,
 187         uint8_t* result) {
 188     bitarray_copy(data, start_index, field_size, result, 0);
 189 }