1 #include <bitfield/bitfield.h>
6 #define PREPARE_FIRST_COPY() \
8 if (bit_count >= (CHAR_BIT - destination_offset_modulo)) { \
9 *destination &= reverse_mask[destination_offset_modulo]; \
10 bit_count -= CHAR_BIT - destination_offset_modulo; \
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 ];\
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 };
23 bool copyBits(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) {
31 if(source_offset + bit_count > source_length * CHAR_BIT ||
32 destination_offset + bit_count > destination_length * CHAR_BIT ) {
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;
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++;
48 int byte_len = bit_count / CHAR_BIT;
49 int bit_count_modulo = bit_count % CHAR_BIT;
52 memcpy(destination, source, byte_len);
54 destination += byte_len;
57 if(bit_count_modulo > 0) {
58 *destination &= reverse_mask_xor[bit_count_modulo];
59 *destination |= reverse_mask[bit_count_modulo] & *source;
62 int bit_diff_left_shift;
63 int bit_diff_right_shift;
66 * Begin: Line things up on destination.
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;
72 c = *source++ << bit_diff_left_shift;
73 c |= *source >> bit_diff_right_shift;
74 c &= reverse_mask_xor[destination_offset_modulo];
76 bit_diff_right_shift = destination_offset_modulo - source_offset_modulo;
77 bit_diff_left_shift = CHAR_BIT - bit_diff_right_shift;
79 c = *source >> bit_diff_right_shift &
80 reverse_mask_xor[destination_offset_modulo];
86 * Middle: copy with only shifting the source.
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;
96 * End: copy the remaing bits;
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];
104 *destination &= reverse_mask_xor[bit_count_modulo];
112 * Find the ending bit of a bitfield within the final byte.
114 * Returns: a bit position from 0 to 7.
116 uint8_t find_end_bit(const uint16_t numBits) {
117 int endBit = numBits % CHAR_BIT;
118 return endBit == 0 ? CHAR_BIT : endBit;
121 bool copyBitsRightAligned(const uint8_t source[], const uint16_t source_length,
122 const uint16_t offset, const uint16_t bit_count,
123 uint8_t* destination, const uint16_t destination_length) {
124 return copyBits(source, source_length, offset, bit_count, destination,
126 // provide a proper destination offset so the result is right
128 CHAR_BIT - find_end_bit(bit_count));