diff options
| -rw-r--r-- | src/bitfield/bitarray.c | 129 | ||||
| -rw-r--r-- | src/bitfield/bitfield.c | 167 | ||||
| -rw-r--r-- | src/bitfield/bitfield.h | 65 | ||||
| -rw-r--r-- | tests/bitfield_tests.c | 25 |
4 files changed, 221 insertions, 165 deletions
diff --git a/src/bitfield/bitarray.c b/src/bitfield/bitarray.c new file mode 100644 index 00000000..8fbc941d --- /dev/null +++ b/src/bitfield/bitarray.c @@ -0,0 +1,129 @@ +#include <bitfield/bitfield.h> +#include <stddef.h> +#include <limits.h> +#include <string.h> + +#define PREPARE_FIRST_COPY() \ + do { \ + if (bit_count >= (CHAR_BIT - destination_offset_modulo)) { \ + *destination &= reverse_mask[destination_offset_modulo]; \ + bit_count -= CHAR_BIT - destination_offset_modulo; \ + } else { \ + *destination &= reverse_mask[destination_offset_modulo] \ + | reverse_mask_xor[destination_offset_modulo + bit_count + 1];\ + c &= reverse_mask[destination_offset_modulo + bit_count ];\ + bit_count = 0; \ + } } while (0) + +static const uint8_t reverse_mask[] = + { 0x55, 0x80, 0xc0, 0xe0, 0xf0, 0xf8, 0xfc, 0xfe, 0xff }; +static const uint8_t reverse_mask_xor[] = + { 0xff, 0x7f, 0x3f, 0x1f, 0x0f, 0x07, 0x03, 0x01, 0x00 }; + +bool copyBits(const uint8_t* source_origin, const uint16_t source_length, + const uint16_t source_offset, uint16_t bit_count, + uint8_t* destination_origin, const uint16_t destination_length, + const uint16_t destination_offset) { + if(bit_count < 1) { + return false; + } + + if(source_offset + bit_count > source_length * CHAR_BIT || + destination_offset + bit_count > destination_length * CHAR_BIT ) { + return false; + } + + const uint8_t* source = source_origin + (source_offset / CHAR_BIT); + uint8_t* destination = destination_origin + (destination_offset / CHAR_BIT); + int source_offset_modulo = source_offset % CHAR_BIT; + int destination_offset_modulo = destination_offset % CHAR_BIT; + + if(source_offset_modulo == destination_offset_modulo) { + if(source_offset_modulo > 0) { + uint8_t c = reverse_mask_xor[destination_offset_modulo] & *source++; + PREPARE_FIRST_COPY(); + *destination++ |= c; + } + + int byte_len = bit_count / CHAR_BIT; + int bit_count_modulo = bit_count % CHAR_BIT; + + if(byte_len > 0) { + memcpy(destination, source, byte_len); + source += byte_len; + destination += byte_len; + } + + if(bit_count_modulo > 0) { + *destination &= reverse_mask_xor[bit_count_modulo]; + *destination |= reverse_mask[bit_count_modulo] & *source; + } + } else { + int bit_diff_left_shift; + int bit_diff_right_shift; + uint8_t c; + /* + * Begin: Line things up on destination. + */ + if(source_offset_modulo > destination_offset_modulo) { + bit_diff_left_shift = source_offset_modulo - destination_offset_modulo; + bit_diff_right_shift = CHAR_BIT - bit_diff_left_shift; + + c = *source++ << bit_diff_left_shift; + c |= *source >> bit_diff_right_shift; + c &= reverse_mask_xor[destination_offset_modulo]; + } else { + bit_diff_right_shift = destination_offset_modulo - source_offset_modulo; + bit_diff_left_shift = CHAR_BIT - bit_diff_right_shift; + + c = *source >> bit_diff_right_shift & + reverse_mask_xor[destination_offset_modulo]; + } + PREPARE_FIRST_COPY(); + *destination++ |= c; + + /* + * Middle: copy with only shifting the source. + */ + int byte_len = bit_count / CHAR_BIT; + while(--byte_len >= 0) { + c = *source++ << bit_diff_left_shift; + c |= *source >> bit_diff_right_shift; + *destination++ = c; + } + + /* + * End: copy the remaing bits; + */ + int bit_count_modulo = bit_count % CHAR_BIT; + if(bit_count_modulo > 0) { + c = *source++ << bit_diff_left_shift; + c |= *source >> bit_diff_right_shift; + c &= reverse_mask[bit_count_modulo]; + + *destination &= reverse_mask_xor[bit_count_modulo]; + *destination |= c; + } + } + return true; +} + +/** + * Find the ending bit of a bitfield within the final byte. + * + * Returns: a bit position from 0 to 7. + */ +static uint8_t findEndBit(const uint16_t startBit, const uint16_t numBits) { + int endBit = numBits % CHAR_BIT; + return endBit == 0 ? CHAR_BIT : endBit; +} + +bool copyBitsRightAligned(const uint8_t source[], const uint16_t source_length, + const uint16_t offset, const uint16_t bit_count, + uint8_t* destination, const uint16_t destination_length) { + return copyBits(source, source_length, offset, bit_count, destination, + destination_length, + // provide a proper destination offset so the result is right + // aligned + CHAR_BIT - findEndBit(offset, bit_count)); +} diff --git a/src/bitfield/bitfield.c b/src/bitfield/bitfield.c index 834e443e..b0396413 100644 --- a/src/bitfield/bitfield.c +++ b/src/bitfield/bitfield.c @@ -5,124 +5,16 @@ #define NIBBLE_SIZE (CHAR_BIT / 2) -#define PREPARE_FIRST_COPY() \ - do { \ - if (source_length >= (CHAR_BIT - destination_offset_modulo)) { \ - *destination &= reverse_mask[destination_offset_modulo]; \ - source_length -= CHAR_BIT - destination_offset_modulo; \ - } else { \ - *destination &= reverse_mask[destination_offset_modulo] \ - | reverse_mask_xor[destination_offset_modulo + source_length + 1];\ - c &= reverse_mask[destination_offset_modulo + source_length ];\ - source_length = 0; \ - } } while (0) - -/** - * Find the ending bit of a bitfield within the final byte. - * - * Returns: a bit position from 0 to 7. - */ -static uint8_t findEndBit(const uint16_t startBit, const uint16_t numBits) { - int endBit = numBits % CHAR_BIT; - return endBit == 0 ? CHAR_BIT : endBit; -} - - -// TODO can probably remove this -static int byteForBit(const uint16_t startBit) { - return startBit / CHAR_BIT; +uint64_t bitmask(const uint8_t numBits) { + return (((uint64_t)0x1) << numBits) - 1; } -/* Thanks to - * http://stackoverflow.com/questions/3534535/whats-a-time-efficient-algorithm-to-copy-unaligned-bit-arrays - */ -static void bitarray_copy(const uint8_t* source_origin, int source_offset, - int source_length, uint8_t* destination_origin, int destination_offset) { - static const uint8_t reverse_mask[] = - { 0x55, 0x80, 0xc0, 0xe0, 0xf0, 0xf8, 0xfc, 0xfe, 0xff }; - static const uint8_t reverse_mask_xor[] = - { 0xff, 0x7f, 0x3f, 0x1f, 0x0f, 0x07, 0x03, 0x01, 0x00 }; - - if(source_length < 1) { - return; - } - - const uint8_t* source = source_origin + byteForBit(source_offset); - uint8_t* destination = destination_origin + byteForBit(destination_offset); - int source_offset_modulo = source_offset % CHAR_BIT; - int destination_offset_modulo = destination_offset % CHAR_BIT; - - if(source_offset_modulo == destination_offset_modulo) { - if(source_offset_modulo > 0) { - uint8_t c = reverse_mask_xor[destination_offset_modulo] & *source++; - PREPARE_FIRST_COPY(); - *destination++ |= c; - } - - int byte_len = source_length / CHAR_BIT; - int source_length_modulo = source_length % CHAR_BIT; - - if(byte_len > 0) { - memcpy(destination, source, byte_len); - source += byte_len; - destination += byte_len; - } - - if(source_length_modulo > 0) { - *destination &= reverse_mask_xor[source_length_modulo]; - *destination |= reverse_mask[source_length_modulo] & *source; - } - } else { - int bit_diff_left_shift; - int bit_diff_right_shift; - uint8_t c; - /* - * Begin: Line things up on destination. - */ - if(source_offset_modulo > destination_offset_modulo) { - bit_diff_left_shift = source_offset_modulo - destination_offset_modulo; - bit_diff_right_shift = CHAR_BIT - bit_diff_left_shift; - - c = *source++ << bit_diff_left_shift; - c |= *source >> bit_diff_right_shift; - c &= reverse_mask_xor[destination_offset_modulo]; - } else { - bit_diff_right_shift = destination_offset_modulo - source_offset_modulo; - bit_diff_left_shift = CHAR_BIT - bit_diff_right_shift; - - c = *source >> bit_diff_right_shift & - reverse_mask_xor[destination_offset_modulo]; - } - PREPARE_FIRST_COPY(); - *destination++ |= c; - - /* - * Middle: copy with only shifting the source. - */ - int byte_len = source_length / CHAR_BIT; - while(--byte_len >= 0) { - c = *source++ << bit_diff_left_shift; - c |= *source >> bit_diff_right_shift; - *destination++ = c; - } - - /* - * End: copy the remaing bits; - */ - int source_length_modulo = source_length % CHAR_BIT; - if(source_length_modulo > 0) { - c = *source++ << bit_diff_left_shift; - c |= *source >> bit_diff_right_shift; - c &= reverse_mask[source_length_modulo]; - - *destination &= reverse_mask_xor[source_length_modulo]; - *destination |= c; - } +static uint16_t bitsToBytes(uint32_t bits) { + uint8_t byte_count = bits / CHAR_BIT; + if(bits % CHAR_BIT != 0) { + ++byte_count; } -} - -uint64_t bitmask(const uint8_t numBits) { - return (((uint64_t)0x1) << numBits) - 1; + return byte_count; } uint64_t getBitField(uint64_t data, const uint16_t startBit, @@ -131,24 +23,18 @@ uint64_t getBitField(uint64_t data, const uint16_t startBit, if(!bigEndian) { data = __builtin_bswap64(data); } - getBits(startBit, numBits, (const uint8_t*)&data, - CHAR_BIT * sizeof(uint64_t), - bigEndian ? ENDIANNESS_BIG_ENDIAN : ENDIANNESS_LITTLE_ENDIAN, - result); - // TODO the result has already been shifted to be aligned right, so if we - // try and bswap here it's going to be screwed up unless it was byte aligned + copyBitsRightAligned((const uint8_t*)&data, sizeof(data), startBit, numBits, + result, sizeof(result)); uint64_t int_result = 0; - // TODO should the API return the byte length of data in the result array? - // i think yes. - uint8_t byte_count = numBits / CHAR_BIT; - if(numBits % CHAR_BIT != 0) { - ++byte_count; - } - // TODO wow, can't believe this works, but something is clearly wrong with - // the API! - for(int i = 0; i < byte_count; i++) { - int_result |= result[byte_count - i - 1] << (CHAR_BIT * i); + if(!bigEndian) { + // we need to swap the byte order of the array to get it into a + // uint64_t, but it's been right aligned so we have to be more careful + for(int i = 0; i < bitsToBytes(numBits); i++) { + int_result |= result[bitsToBytes(numBits) - i - 1] << (CHAR_BIT * i); + } + } else { + int_result = *(uint64_t*)result; } return int_result; } @@ -157,19 +43,20 @@ uint64_t getBitField(uint64_t data, const uint16_t startBit, * TODO it would be nice to have a warning if you call with this a value that * won't fit in the number of bits you've specified it should use. */ -void setBitField(uint64_t* data, uint64_t value, int startBit, int numBits) { - int shiftDistance = 64 - startBit - numBits; +void setBitField(uint64_t* data, uint64_t value, const uint16_t startPos, + const uint16_t numBits) { + int shiftDistance = 64 - startPos - numBits; value <<= shiftDistance; *data &= ~(bitmask(numBits) << shiftDistance); *data |= value; } -uint8_t nthByte(uint64_t source, int byteNum) { +uint8_t nthByte(const uint64_t source, const uint16_t byteNum) { return (source >> (64 - ((byteNum + 1) * CHAR_BIT))) & 0xFF; } uint8_t getNibble(const uint8_t nibble_index, const uint8_t data[], - const uint8_t length, Endianness endianness) { + const uint8_t length) { uint8_t byte_index = nibble_index / 2; uint8_t result; if(byte_index < length) { @@ -182,18 +69,10 @@ uint8_t getNibble(const uint8_t nibble_index, const uint8_t data[], return result; } -// TODO getBytes, return status and store in output parameter uint8_t getByte(const uint8_t byte_index, const uint8_t data[], - const uint8_t length, Endianness endianness) { + const uint8_t length) { if(byte_index < length) { return data[byte_index]; } return 0; } - -void getBits(const uint16_t start_index, const uint16_t field_size, - const uint8_t data[], const uint8_t length, Endianness endianness, - uint8_t* result) { - bitarray_copy(data, start_index, field_size, result, - CHAR_BIT - findEndBit(start_index, field_size)); -} diff --git a/src/bitfield/bitfield.h b/src/bitfield/bitfield.h index ce68b949..bf5c9b7d 100644 --- a/src/bitfield/bitfield.h +++ b/src/bitfield/bitfield.h @@ -8,20 +8,57 @@ extern "C" { #endif -typedef enum { - ENDIANNESS_LITTLE_ENDIAN, - ENDIANNESS_BIG_ENDIAN -} Endianness; - uint8_t getNibble(const uint8_t nibble_index, const uint8_t data[], - const uint8_t length, Endianness endianness); + const uint8_t length); uint8_t getByte(const uint8_t byte_index, const uint8_t data[], - const uint8_t length, Endianness endianness); + const uint8_t length); + +/* Public: Copy a range of bits from one bit array to another. + * + * The range does not need to be byte aligned, and the source and destination do + * not have to be the same size (as long as the desitnation has enough room to + * fit the range). + * + * A bit array with regards to this function always has the leftmost bit in byte + * 0, i.e. bit index is the leftmost bit of byte 0. Endianness does not matter. + * + * Thanks to + * http://stackoverflow.com/questions/3534535/whats-a-time-efficient-algorithm-to-copy-unaligned-bit-arrays + * for the implementation of the algorithm. + * + * source_origin - the source array. + * source_length - the total length of the source array in bytes, + * for range checking. + * source_offset - an offset in bits to start the copy from the source array. + * Specify 0 to start from source_origin. + * bit_count - the number of bits to copy. + * destination_origin - the destination array. + * desitnation_length - the total length of the destination array in bytes, + * for range checking. + * destination_offset - an offset in bits to start placing the copied range into + * the destination array. Specify 0 to start from the beginning of the + * destination. If you are copying a range not aligned on a byte, you + * probably want to set this to a positive offset to right the resulting + * bits in the destination. + * + * Returns true if the copy was successful and false if the range exceeded the + * size of the source or destination, or if the range size negative or 0. + */ +bool copyBits(const uint8_t* source_origin, const uint16_t source_length, + const uint16_t source_offset, uint16_t bit_count, + uint8_t* destination_origin, const uint16_t destination_length, + const uint16_t destination_offset); + +bool copyBitsRightAligned(const uint8_t source[], const uint16_t source_length, + const uint16_t offset, const uint16_t bit_count, + uint8_t* destination, const uint16_t destination_length); -void getBits(const uint16_t start_index, const uint16_t field_size, - const uint8_t data[], const uint8_t length, Endianness endianness, - uint8_t* result); +// TODO using uint64_t everywhere for CAN message payload is kind of cute, but +// in actuality a CAN message may have a smaller payload, and it makes all of +// these functions not applicable to other data sizes. It's also fairly +// inefficient on 32-bit platforms. how much work is it to switch vi-firmware +// to using uint8_t*? /* Public: Reads a subset of bits from a byte array. * @@ -55,7 +92,8 @@ void getBits(const uint16_t start_index, const uint16_t field_size, * * Returns the value of the requested bit field. */ -uint64_t getBitField(uint64_t data, const uint16_t startPos, const uint16_t numBits, bool bigEndian); +uint64_t getBitField(uint64_t data, const uint16_t startPos, + const uint16_t numBits, bool bigEndian); /* Public: Set the bit field in the given data array to the new value. * @@ -63,7 +101,8 @@ uint64_t getBitField(uint64_t data, const uint16_t startPos, const uint16_t numB * value - the value to set in the bit field. * startPos - the starting index of the bit field (beginning from 0). */ -void setBitField(uint64_t* data, uint64_t value, int startPos, int numBits); +void setBitField(uint64_t* data, uint64_t value, const uint16_t startPos, + const uint16_t numBits); /* Public: Retreive the nth byte out of 8 bytes in a uint64_t. * @@ -72,7 +111,7 @@ void setBitField(uint64_t* data, uint64_t value, int startPos, int numBits); * * Returns the requested byte from the source bytes. */ -uint8_t nthByte(uint64_t source, int byteNum); +uint8_t nthByte(const uint64_t source, const uint16_t byteNum); #ifdef __cplusplus } diff --git a/tests/bitfield_tests.c b/tests/bitfield_tests.c index 056f8c13..e2304ea0 100644 --- a/tests/bitfield_tests.c +++ b/tests/bitfield_tests.c @@ -193,9 +193,9 @@ END_TEST START_TEST (test_get_byte) { uint8_t data[4] = {0x12, 0x34, 0x56, 0x78}; - uint8_t result = getByte(0, data, sizeof(data), ENDIANNESS_BIG_ENDIAN); + uint8_t result = getByte(0, data, sizeof(data)); ck_assert_int_eq(result, 0x12); - result = getByte(3, data, sizeof(data), ENDIANNESS_BIG_ENDIAN); + result = getByte(3, data, sizeof(data)); ck_assert_int_eq(result, 0x78); } END_TEST @@ -203,20 +203,28 @@ END_TEST START_TEST (test_get_nibble) { uint8_t data[4] = {0x12, 0x34, 0x56, 0x78}; - uint8_t result = getNibble(0, data, sizeof(data), ENDIANNESS_BIG_ENDIAN); + uint8_t result = getNibble(0, data, sizeof(data)); ck_assert_int_eq(result, 0x1); - result = getNibble(1, data, sizeof(data), ENDIANNESS_BIG_ENDIAN); + result = getNibble(1, data, sizeof(data)); ck_assert_int_eq(result, 0x2); - result = getNibble(2, data, sizeof(data), ENDIANNESS_BIG_ENDIAN); + result = getNibble(2, data, sizeof(data)); ck_assert_int_eq(result, 0x3); } END_TEST -START_TEST (test_get_bits) +START_TEST (test_get_bits_out_of_range) { uint8_t data[4] = {0x12, 0x34, 0x56, 0x78}; uint8_t result[4]; - getBits(0, 16, data, 36, ENDIANNESS_BIG_ENDIAN, result); + fail_if(copyBitsRightAligned(data, 4, 25, 16, result, 4)); +} +END_TEST + +START_TEST (test_get_bits) +{ + uint8_t data[4] = {0x12, 0x34, 0x56, 0x78}; + uint8_t result[4] = {0}; + fail_unless(copyBitsRightAligned(data, 4, 0, 16, result, 4)); ck_assert_int_eq(result[0], 0x12); ck_assert_int_eq(result[1], 0x34); } @@ -226,7 +234,7 @@ START_TEST (test_get_uneven_bits) { uint8_t data[4] = {0x12, 0x34, 0x56, 0x78}; uint8_t result[4] = {0}; - getBits(4, 12, data, 36, ENDIANNESS_BIG_ENDIAN, result); + fail_unless(copyBitsRightAligned(data, 4, 4, 12, result, 4)); ck_assert_int_eq(result[0], 0x2); ck_assert_int_eq(result[1], 0x34); } @@ -252,6 +260,7 @@ Suite* bitfieldSuite(void) { tcase_add_test(tc_core, test_get_byte); tcase_add_test(tc_core, test_get_nibble); tcase_add_test(tc_core, test_get_bits); + tcase_add_test(tc_core, test_get_bits_out_of_range); tcase_add_test(tc_core, test_get_uneven_bits); suite_add_tcase(s, tc_core); |