libs/bitfield-c/src/bitfield/bitarray.c

   1 #include <bitfield/bitfield.h>
   2 #include <stddef.h>
   3 #include <limits.h>
   4 #include <string.h>
   5
   6 #define PREPARE_FIRST_COPY()                                      \
   7     do {                                                          \
   8     if (bit_count >= (CHAR_BIT - destination_offset_modulo)) {              \
   9         *destination &= reverse_mask[destination_offset_modulo];              \
  10         bit_count -= CHAR_BIT - destination_offset_modulo;                  \
  11     } else {                                                      \
  12         *destination &= reverse_mask[destination_offset_modulo]               \
  13               | reverse_mask_xor[destination_offset_modulo + bit_count + 1];\
  14          c &= reverse_mask[destination_offset_modulo + bit_count    ];\
  15         bit_count = 0;                                              \
  16     } } while (0)
  17
  18 static const uint8_t reverse_mask[] =
  19     { 0x55, 0x80, 0xc0, 0xe0, 0xf0, 0xf8, 0xfc, 0xfe, 0xff };
  20 static const uint8_t reverse_mask_xor[] =
  21     { 0xff, 0x7f, 0x3f, 0x1f, 0x0f, 0x07, 0x03, 0x01, 0x00 };
  22
  23 bool copy_bits(const uint8_t* source_origin, const uint16_t source_length,
  24         const uint16_t source_offset, uint16_t bit_count,
  25         uint8_t* destination_origin, const uint16_t destination_length,
  26         const uint16_t destination_offset) {
  27     if(bit_count < 1) {
  28         return false;
  29     }
  30
  31     if(source_offset + bit_count > source_length * CHAR_BIT ||
  32             destination_offset + bit_count > destination_length * CHAR_BIT ) {
  33         return false;
  34     }
  35
  36     const uint8_t* source = source_origin + (source_offset / CHAR_BIT);
  37     uint8_t* destination = destination_origin + (destination_offset / CHAR_BIT);
  38     int source_offset_modulo = source_offset % CHAR_BIT;
  39     int destination_offset_modulo = destination_offset % CHAR_BIT;
  40
  41     if(source_offset_modulo == destination_offset_modulo) {
  42         if(source_offset_modulo > 0) {
  43             uint8_t c = reverse_mask_xor[destination_offset_modulo] & *source++;
  44             PREPARE_FIRST_COPY();
  45             *destination++ |= c;
  46         }
  47
  48         int byte_len = bit_count / CHAR_BIT;
  49         int bit_count_modulo = bit_count % CHAR_BIT;
  50
  51         if(byte_len > 0) {
  52             memcpy(destination, source, byte_len);
  53             source += byte_len;
  54             destination += byte_len;
  55         }
  56
  57         if(bit_count_modulo > 0) {
  58             *destination &= reverse_mask_xor[bit_count_modulo];
  59             *destination |= reverse_mask[bit_count_modulo] & *source;
  60         }
  61     } else {
  62         int bit_diff_left_shift;
  63         int bit_diff_right_shift;
  64         uint8_t c;
  65         /*
  66          * Begin: Line things up on destination.
  67          */
  68         if(source_offset_modulo > destination_offset_modulo) {
  69             bit_diff_left_shift = source_offset_modulo - destination_offset_modulo;
  70             bit_diff_right_shift = CHAR_BIT - bit_diff_left_shift;
  71
  72             c = *source++ << bit_diff_left_shift;
  73             c |= *source >> bit_diff_right_shift;
  74             c &= reverse_mask_xor[destination_offset_modulo];
  75         } else {
  76             bit_diff_right_shift = destination_offset_modulo - source_offset_modulo;
  77             bit_diff_left_shift = CHAR_BIT - bit_diff_right_shift;
  78
  79             c = *source >> bit_diff_right_shift &
  80                     reverse_mask_xor[destination_offset_modulo];
  81         }
  82         PREPARE_FIRST_COPY();
  83         *destination++ |= c;
  84
  85         /*
  86          * Middle: copy with only shifting the source.
  87          */
  88         int byte_len = bit_count / CHAR_BIT;
  89         while(--byte_len >= 0) {
  90             c = *source++ << bit_diff_left_shift;
  91             c |= *source >> bit_diff_right_shift;
  92             *destination++ = c;
  93         }
  94
  95         /*
  96          * End: copy the remaing bits;
  97          */
  98         int bit_count_modulo = bit_count % CHAR_BIT;
  99         if(bit_count_modulo > 0) {
 100             c = *source++ << bit_diff_left_shift;
 101             c |= *source >> bit_diff_right_shift;
 102             c &= reverse_mask[bit_count_modulo];
 103
 104             *destination &= reverse_mask_xor[bit_count_modulo];
 105             *destination |= c;
 106         }
 107     }
 108     return true;
 109 }
 110
 111 uint16_t bits_to_bytes(uint32_t bits) {
 112     uint8_t byte_count = bits / CHAR_BIT;
 113     if(bits % CHAR_BIT != 0) {
 114         ++byte_count;
 115     }
 116     return byte_count;
 117 }
 118
 119 /**
 120  * Find the ending bit of a bitfield within the final byte.
 121  *
 122  * Returns: a bit position from 0 to 7.
 123  */
 124 uint8_t find_end_bit(const uint16_t numBits) {
 125     int endBit = numBits % CHAR_BIT;
 126     return endBit == 0 ? CHAR_BIT : endBit;
 127 }
 128
 129 bool copy_bits_right_aligned(const uint8_t source[], const uint16_t source_length,
 130                 const uint16_t offset, const uint16_t bit_count,
 131                 uint8_t* destination, const uint16_t destination_length) {
 132     return copy_bits(source, source_length, offset, bit_count, destination,
 133             destination_length,
 134             // provide a proper destination offset so the result is right
 135             // aligned
 136             (destination_length - bits_to_bytes(bit_count)) * CHAR_BIT +
 137                  CHAR_BIT - find_end_bit(bit_count));
 138 }
 139
 140 bool copy_bytes_right_aligned(const uint8_t source[], const uint16_t source_length,
 141                 const uint16_t offset, const uint16_t byte_count,
 142                 uint8_t* destination, const uint16_t destination_length) {
 143     return copy_bits_right_aligned(source, source_length, offset * CHAR_BIT,
 144             byte_count * CHAR_BIT, destination, destination_length);
 145 }