From: Christopher Peplin Date: Sat, 28 Dec 2013 20:42:10 +0000 (-0500) Subject: Draft work making the bitfield functions more generic. X-Git-Tag: 5.0.2~277^2~33 X-Git-Url: https://gerrit.automotivelinux.org/gerrit/gitweb?a=commitdiff_plain;h=0b00bb6c3c5df70317775efd12b6fff418ff4540;p=apps%2Fagl-service-can-low-level.git Draft work making the bitfield functions more generic. --- diff --git a/src/bitfield/bitfield.c b/src/bitfield/bitfield.c index d383db04..b3aef4b2 100644 --- a/src/bitfield/bitfield.c +++ b/src/bitfield/bitfield.c @@ -1,47 +1,147 @@ #include +#include +#include +#include + +#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. */ -int findEndBit(int startBit, int numBits) { - int endBit = (startBit + numBits) % 8; - return endBit == 0 ? 8 : endBit; -} - -uint64_t bitmask(int numBits) { - return (((uint64_t)0x1) << numBits) - 1; +static uint8_t findEndBit(const uint16_t startBit, const uint16_t numBits) { + int endBit = (startBit + numBits) % CHAR_BIT; + return endBit == 0 ? CHAR_BIT : endBit; } -int startingByte(int startBit) { - return startBit / 8; -} -int endingByte(int startBit, int numBits) { - return (startBit + numBits - 1) / 8; +// TODO can probably remove this +static int byteForBit(const uint16_t startBit) { + return startBit / CHAR_BIT; } -uint64_t getBitField(uint64_t data, int startBit, int numBits, bool bigEndian) { - int startByte = startingByte(startBit); - int endByte = endingByte(startBit, numBits); +/* 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 mask[] = + { 0x55, 0x01, 0x03, 0x07, 0x0f, 0x1f, 0x3f, 0x7f, 0xff }; + 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(!bigEndian) { - data = __builtin_bswap64(data); + if(source_length < 1) { + return; } - uint8_t* bytes = (uint8_t*)&data; - uint64_t ret = bytes[startByte]; - if(startByte != endByte) { - // The lowest byte address contains the most significant bit. - int i; - for(i = startByte + 1; i <= endByte; i++) { - ret = ret << 8; - ret = ret | bytes[i]; + + int source_offset_modulo; + int destination_offset_modulo; + + const uint8_t* source = source_origin + byteForBit(source_offset); + uint8_t* destination = destination_origin + byteForBit(destination_offset); + + source_offset_modulo = source_offset % CHAR_BIT; + destination_offset_modulo = destination_offset % CHAR_BIT; + + if(source_offset_modulo == destination_offset_modulo) { + if(source_offset_modulo) { + uint8_t c; + + 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) { + memcpy(destination, source, byte_len); + source += byte_len; + destination += byte_len; + } + if(source_length_modulo) { + *destination &= reverse_mask_xor[source_length_modulo]; + *destination |= reverse_mask[source_length_modulo] & *source; + } + } else { + int bit_diff_ls; + int bit_diff_rs; + uint8_t c; + /* + * Begin: Line things up on destination. + */ + if (source_offset_modulo > destination_offset_modulo) { + bit_diff_ls = source_offset_modulo - destination_offset_modulo; + bit_diff_rs = CHAR_BIT - bit_diff_ls; + + c = *source++ << bit_diff_ls; + c |= *source >> bit_diff_rs; + c &= reverse_mask_xor[destination_offset_modulo]; + } else { + bit_diff_rs = destination_offset_modulo - source_offset_modulo; + bit_diff_ls = CHAR_BIT - bit_diff_rs; + + c = *source >> bit_diff_rs & + 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_ls; + c |= *source >> bit_diff_rs; + *destination++ = c; + } + + /* + * End: copy the remaing bits; + */ + int source_length_modulo = source_length % CHAR_BIT; + if(source_length_modulo) { + c = *source++ << bit_diff_ls; + c |= *source >> bit_diff_rs; + c &= reverse_mask[source_length_modulo]; + + *destination &= reverse_mask_xor[source_length_modulo]; + *destination |= c; } } +} + +uint64_t bitmask(const uint8_t numBits) { + return (((uint64_t)0x1) << numBits) - 1; +} - ret >>= 8 - findEndBit(startBit, numBits); - return ret & bitmask(numBits); +uint64_t getBitField(uint64_t data, const uint16_t startBit, + const uint16_t numBits, bool bigEndian) { + uint64_t result; + getBits(startBit, numBits, (const uint8_t*)&data, + CHAR_BIT * sizeof(uint64_t), + bigEndian ? ENDIANNESS_BIG_ENDIAN : ENDIANNESS_LITTLE_ENDIAN, + &result); + return result; } /** @@ -56,5 +156,34 @@ void setBitField(uint64_t* data, uint64_t value, int startBit, int numBits) { } uint8_t nthByte(uint64_t source, int byteNum) { - return (source >> (64 - ((byteNum + 1) * 8))) & 0xFF; + 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) { + uint8_t byte_index = nibble_index / 2; + uint8_t result; + if(byte_index < length) { + result = data[byte_index]; + if(nibble_index % 2 == 0) { + result >>= NIBBLE_SIZE; + } + } + result &= bitmask(NIBBLE_SIZE); + 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) { + 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, 0); } diff --git a/src/bitfield/bitfield.h b/src/bitfield/bitfield.h index 7d9f3995..ce68b949 100644 --- a/src/bitfield/bitfield.h +++ b/src/bitfield/bitfield.h @@ -8,6 +8,21 @@ 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); + +uint8_t getByte(const uint8_t byte_index, const uint8_t data[], + const uint8_t length, Endianness endianness); + +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); + /* Public: Reads a subset of bits from a byte array. * * data - the bytes in question. @@ -40,7 +55,7 @@ extern "C" { * * Returns the value of the requested bit field. */ -uint64_t getBitField(uint64_t data, int startPos, int 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. * diff --git a/tests/bitfield_tests.c b/tests/bitfield_tests.c index d248989b..f321e5f2 100644 --- a/tests/bitfield_tests.c +++ b/tests/bitfield_tests.c @@ -190,6 +190,48 @@ START_TEST(test_nth_byte) } 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); + ck_assert_int_eq(result, 0x12); + result = getByte(3, data, sizeof(data), ENDIANNESS_BIG_ENDIAN); + ck_assert_int_eq(result, 0x78); +} +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); + ck_assert_int_eq(result, 0x1); + result = getNibble(1, data, sizeof(data), ENDIANNESS_BIG_ENDIAN); + ck_assert_int_eq(result, 0x2); + result = getNibble(2, data, sizeof(data), ENDIANNESS_BIG_ENDIAN); + ck_assert_int_eq(result, 0x3); +} +END_TEST + +START_TEST (test_get_bits) +{ + uint8_t data[4] = {0x12, 0x34, 0x56, 0x78}; + uint8_t result[4]; + getBits(0, 16, data, 36, ENDIANNESS_BIG_ENDIAN, result); + ck_assert_int_eq(result[0], 0x12); + ck_assert_int_eq(result[1], 0x34); +} +END_TEST + +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); + ck_assert_int_eq(result[0], 0x2); + ck_assert_int_eq(result[1], 0x34); +} +END_TEST + Suite* bitfieldSuite(void) { Suite* s = suite_create("bitfield"); TCase *tc_core = tcase_create("core"); @@ -207,6 +249,10 @@ Suite* bitfieldSuite(void) { tcase_add_test(tc_core, test_set_off_byte_boundary); tcase_add_test(tc_core, test_set_odd_number_of_bits); tcase_add_test(tc_core, test_nth_byte); + 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_uneven_bits); suite_add_tcase(s, tc_core); return s;