1 #include <bitfield/bitfield.h>
6 #define NIBBLE_SIZE (CHAR_BIT / 2)
8 #define PREPARE_FIRST_COPY() \
10 if (source_length >= (CHAR_BIT - destination_offset_modulo)) { \
11 *destination &= reverse_mask[destination_offset_modulo]; \
12 source_length -= CHAR_BIT - destination_offset_modulo; \
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 ];\
21 * Find the ending bit of a bitfield within the final byte.
23 * Returns: a bit position from 0 to 7.
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;
31 // TODO can probably remove this
32 static int byteForBit(const uint16_t startBit) {
33 return startBit / CHAR_BIT;
37 * http://stackoverflow.com/questions/3534535/whats-a-time-efficient-algorithm-to-copy-unaligned-bit-arrays
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 };
46 if(source_length < 1) {
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;
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++;
62 int byte_len = source_length / CHAR_BIT;
63 int source_length_modulo = source_length % CHAR_BIT;
66 memcpy(destination, source, byte_len);
68 destination += byte_len;
71 if(source_length_modulo > 0) {
72 *destination &= reverse_mask_xor[source_length_modulo];
73 *destination |= reverse_mask[source_length_modulo] & *source;
76 int bit_diff_left_shift;
77 int bit_diff_right_shift;
80 * Begin: Line things up on destination.
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;
86 c = *source++ << bit_diff_left_shift;
87 c |= *source >> bit_diff_right_shift;
88 c &= reverse_mask_xor[destination_offset_modulo];
90 bit_diff_right_shift = destination_offset_modulo - source_offset_modulo;
91 bit_diff_left_shift = CHAR_BIT - bit_diff_right_shift;
93 c = *source >> bit_diff_right_shift &
94 reverse_mask_xor[destination_offset_modulo];
100 * Middle: copy with only shifting the source.
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;
110 * End: copy the remaing bits;
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];
118 *destination &= reverse_mask_xor[source_length_modulo];
124 uint64_t bitmask(const uint8_t numBits) {
125 return (((uint64_t)0x1) << numBits) - 1;
128 uint64_t getBitField(uint64_t data, const uint16_t startBit,
129 const uint16_t numBits, bool bigEndian) {
130 uint8_t result[8] = {0};
132 data = __builtin_bswap64(data);
134 getBits(startBit, numBits, (const uint8_t*)&data,
135 CHAR_BIT * sizeof(uint64_t),
136 bigEndian ? ENDIANNESS_BIG_ENDIAN : ENDIANNESS_LITTLE_ENDIAN,
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?
143 uint8_t byte_count = numBits / CHAR_BIT;
144 if(numBits % CHAR_BIT != 0) {
148 // TODO wow, can't believe this works, but something is clearly wrong with
150 for(int i = 0; i < byte_count; i++) {
151 int_result |= result[byte_count - i - 1] << (CHAR_BIT * i);
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.
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);
167 uint8_t nthByte(uint64_t source, int byteNum) {
168 return (source >> (64 - ((byteNum + 1) * CHAR_BIT))) & 0xFF;
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;
175 if(byte_index < length) {
176 result = data[byte_index];
177 if(nibble_index % 2 == 0) {
178 result >>= NIBBLE_SIZE;
181 result &= bitmask(NIBBLE_SIZE);
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];
194 void getBits(const uint16_t start_index, const uint16_t field_size,
195 const uint8_t data[], const uint8_t length, Endianness endianness,
197 bitarray_copy(data, start_index, field_size, result,
198 CHAR_BIT - findEndBit(start_index, field_size));