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 = 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 reverse_mask[] =
  42         { 0x55, 0x80, 0xc0, 0xe0, 0xf0, 0xf8, 0xfc, 0xfe, 0xff };
  43     static const uint8_t reverse_mask_xor[] =
  44         { 0xff, 0x7f, 0x3f, 0x1f, 0x0f, 0x07, 0x03, 0x01, 0x00 };
  45
  46     if(source_length < 1) {
  47         return;
  48     }
  49
  50     const uint8_t* source = source_origin + byteForBit(source_offset);
  51     uint8_t* destination = destination_origin + byteForBit(destination_offset);
  52     int source_offset_modulo = source_offset % CHAR_BIT;
  53     int destination_offset_modulo = destination_offset % CHAR_BIT;
  54
  55     if(source_offset_modulo == destination_offset_modulo) {
  56         if(source_offset_modulo > 0) {
  57             uint8_t c = reverse_mask_xor[destination_offset_modulo] & *source++;
  58             PREPARE_FIRST_COPY();
  59             *destination++ |= c;
  60         }
  61
  62         int byte_len = source_length / CHAR_BIT;
  63         int source_length_modulo = source_length % CHAR_BIT;
  64
  65         if(byte_len > 0) {
  66             memcpy(destination, source, byte_len);
  67             source += byte_len;
  68             destination += byte_len;
  69         }
  70
  71         if(source_length_modulo > 0) {
  72             *destination &= reverse_mask_xor[source_length_modulo];
  73             *destination |= reverse_mask[source_length_modulo] & *source;
  74         }
  75     } else {
  76         int bit_diff_left_shift;
  77         int bit_diff_right_shift;
  78         uint8_t c;
  79         /*
  80          * Begin: Line things up on destination.
  81          */
  82         if(source_offset_modulo > destination_offset_modulo) {
  83             bit_diff_left_shift = source_offset_modulo - destination_offset_modulo;
  84             bit_diff_right_shift = CHAR_BIT - bit_diff_left_shift;
  85
  86             c = *source++ << bit_diff_left_shift;
  87             c |= *source >> bit_diff_right_shift;
  88             c &= reverse_mask_xor[destination_offset_modulo];
  89         } else {
  90             bit_diff_right_shift = destination_offset_modulo - source_offset_modulo;
  91             bit_diff_left_shift = CHAR_BIT - bit_diff_right_shift;
  92
  93             c = *source >> bit_diff_right_shift &
  94                     reverse_mask_xor[destination_offset_modulo];
  95         }
  96         PREPARE_FIRST_COPY();
  97         *destination++ |= c;
  98
  99         /*
 100          * Middle: copy with only shifting the source.
 101          */
 102         int byte_len = source_length / CHAR_BIT;
 103         while(--byte_len >= 0) {
 104             c = *source++ << bit_diff_left_shift;
 105             c |= *source >> bit_diff_right_shift;
 106             *destination++ = c;
 107         }
 108
 109         /*
 110          * End: copy the remaing bits;
 111          */
 112         int source_length_modulo = source_length % CHAR_BIT;
 113         if(source_length_modulo > 0) {
 114             c = *source++ << bit_diff_left_shift;
 115             c |= *source >> bit_diff_right_shift;
 116             c &= reverse_mask[source_length_modulo];
 117
 118             *destination &= reverse_mask_xor[source_length_modulo];
 119             *destination |= c;
 120         }
 121     }
 122 }
 123
 124 uint64_t bitmask(const uint8_t numBits) {
 125     return (((uint64_t)0x1) << numBits) - 1;
 126 }
 127
 128 uint64_t getBitField(uint64_t data, const uint16_t startBit,
 129         const uint16_t numBits, bool bigEndian) {
 130     uint8_t result[8] = {0};
 131     if(!bigEndian) {
 132         data = __builtin_bswap64(data);
 133     }
 134     getBits(startBit, numBits, (const uint8_t*)&data,
 135             CHAR_BIT * sizeof(uint64_t),
 136             bigEndian ? ENDIANNESS_BIG_ENDIAN : ENDIANNESS_LITTLE_ENDIAN,
 137             result);
 138     // TODO the result has already been shifted to be aligned right, so if we
 139     // try and bswap here it's going to be screwed up unless it was byte aligned
 140     uint64_t int_result = 0;
 141     // TODO should the API return the byte length of data in the result array?
 142     // i think yes.
 143     uint8_t byte_count = numBits / CHAR_BIT;
 144     if(numBits % CHAR_BIT != 0) {
 145         ++byte_count;
 146     }
 147
 148     // TODO wow, can't believe this works, but something is clearly wrong with
 149     // the API!
 150     for(int i = 0; i < byte_count; i++) {
 151         int_result |= result[byte_count - i - 1] << (CHAR_BIT * i);
 152     }
 153     return int_result;
 154 }
 155
 156 /**
 157  * TODO it would be nice to have a warning if you call with this a value that
 158  * won't fit in the number of bits you've specified it should use.
 159  */
 160 void setBitField(uint64_t* data, uint64_t value, int startBit, int numBits) {
 161     int shiftDistance = 64 - startBit - numBits;
 162     value <<= shiftDistance;
 163     *data &= ~(bitmask(numBits) << shiftDistance);
 164     *data |= value;
 165 }
 166
 167 uint8_t nthByte(uint64_t source, int byteNum) {
 168     return (source >> (64 - ((byteNum + 1) * CHAR_BIT))) & 0xFF;
 169 }
 170
 171 uint8_t getNibble(const uint8_t nibble_index, const uint8_t data[],
 172         const uint8_t length, Endianness endianness) {
 173     uint8_t byte_index = nibble_index / 2;
 174     uint8_t result;
 175     if(byte_index < length) {
 176         result = data[byte_index];
 177         if(nibble_index % 2 == 0) {
 178             result >>= NIBBLE_SIZE;
 179         }
 180     }
 181     result &= bitmask(NIBBLE_SIZE);
 182     return result;
 183 }
 184
 185 // TODO getBytes, return status and store in output parameter
 186 uint8_t getByte(const uint8_t byte_index, const uint8_t data[],
 187         const uint8_t length, Endianness endianness) {
 188     if(byte_index < length) {
 189         return data[byte_index];
 190     }
 191     return 0;
 192 }
 193
 194 void getBits(const uint16_t start_index, const uint16_t field_size,
 195         const uint8_t data[], const uint8_t length, Endianness endianness,
 196         uint8_t* result) {
 197     bitarray_copy(data, start_index, field_size, result,
 198             CHAR_BIT - findEndBit(start_index, field_size));
 199 }