--- /dev/null
+#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));
+}
#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,
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;
}
* 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) {
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));
-}
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.
*
*
* 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.
*
* 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.
*
*
* 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
}
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
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);
}
{
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);
}
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);
Code Review / apps / agl-service-can-low-level.git / commitdiff