diff options
Diffstat (limited to 'CAN-binder/libs/bitfield-c/src/bitfield')
| -rw-r--r-- | CAN-binder/libs/bitfield-c/src/bitfield/8byte.c | 61 | ||||
| -rw-r--r-- | CAN-binder/libs/bitfield-c/src/bitfield/8byte.h | 88 | ||||
| -rw-r--r-- | CAN-binder/libs/bitfield-c/src/bitfield/bitarray.c | 145 | ||||
| -rw-r--r-- | CAN-binder/libs/bitfield-c/src/bitfield/bitfield.c | 74 | ||||
| -rw-r--r-- | CAN-binder/libs/bitfield-c/src/bitfield/bitfield.h | 220 |
5 files changed, 588 insertions, 0 deletions
diff --git a/CAN-binder/libs/bitfield-c/src/bitfield/8byte.c b/CAN-binder/libs/bitfield-c/src/bitfield/8byte.c new file mode 100644 index 00000000..9325ed1b --- /dev/null +++ b/CAN-binder/libs/bitfield-c/src/bitfield/8byte.c @@ -0,0 +1,61 @@ +#include <bitfield/bitfield.h> +#include <bitfield/8byte.h> +#include <stddef.h> +#include <limits.h> +#include <string.h> + +#define EIGHTBYTE_BIT (8 * sizeof(uint64_t)) + +uint8_t eightbyte_get_nibble(const uint64_t source, const uint8_t nibble_index, + const bool data_is_big_endian) { + return (uint8_t) eightbyte_get_bitfield(source, NIBBLE_SIZE * nibble_index, + NIBBLE_SIZE, data_is_big_endian); +} + +uint8_t eightbyte_get_byte(uint64_t source, const uint8_t byte_index, + const bool data_is_big_endian) { + if(data_is_big_endian) { + source = __builtin_bswap64(source); + } + return (source >> (EIGHTBYTE_BIT - ((byte_index + 1) * CHAR_BIT))) & 0xFF; +} + +// TODO is this funciton necessary anymore? is it any faster for uint64_t than +// get_bitfield(data[], ...)? is the performance better on a 32 bit platform +// like the PIC32? +uint64_t eightbyte_get_bitfield(uint64_t source, const uint16_t offset, + const uint16_t bit_count, const bool data_is_big_endian) { + int startByte = offset / CHAR_BIT; + int endByte = (offset + bit_count - 1) / CHAR_BIT; + + if(!data_is_big_endian) { + source = __builtin_bswap64(source); + } + + uint8_t* bytes = (uint8_t*)&source; + uint64_t ret = bytes[startByte]; + if(startByte != endByte) { + // The lowest byte address contains the most significant bit. + uint8_t i; + for(i = startByte + 1; i <= endByte; i++) { + ret = ret << 8; + ret = ret | bytes[i]; + } + } + + ret >>= 8 - find_end_bit(offset + bit_count); + return ret & bitmask(bit_count); +} + +bool eightbyte_set_bitfield(uint64_t value, const uint16_t offset, + const uint16_t bit_count, uint64_t* destination) { + if(value > bitmask(bit_count)) { + return false; + } + + int shiftDistance = EIGHTBYTE_BIT - offset - bit_count; + value <<= shiftDistance; + *destination &= ~(bitmask(bit_count) << shiftDistance); + *destination |= value; + return true; +} diff --git a/CAN-binder/libs/bitfield-c/src/bitfield/8byte.h b/CAN-binder/libs/bitfield-c/src/bitfield/8byte.h new file mode 100644 index 00000000..04512690 --- /dev/null +++ b/CAN-binder/libs/bitfield-c/src/bitfield/8byte.h @@ -0,0 +1,88 @@ +#ifndef __8BYTE_H__ +#define __8BYTE_H__ + +#include <stdint.h> +#include <stdbool.h> + +#ifdef __cplusplus +extern "C" { +#endif + +/* Public: Reads a subset of bits into a uint64_t. + * + * source - the bytes in question. + * offset - the starting index of the bit field (beginning from 0). + * bit_count - the width of the bit field to extract. + * data_is_big_endian - if the data passed in is little endian, set this to false and it + * will be flipped before grabbing the bit field. + * + * Bit fields are positioned according to big-endian bit layout. + * + * For example, the bit layout of the value "42" (i.e. 00101010 set at position + * 14 with length 6 is: + * + * 000000000000001010100000000000000000000000000000000000000000000 + * + * and the same value and position but with length 8 is: + * + * 000000000000000010101000000000000000000000000000000000000000000 + * + * If the architecture where is code is running is little-endian, the input data + * will be swapped before grabbing the bit field. + * + * Examples + * + * uint64_t value = get_bitfield(data, 2, 4); + * + * Returns the value of the requested bit field, right aligned in a uint64_t. + */ +uint64_t eightbyte_get_bitfield(uint64_t source, const uint16_t offset, + const uint16_t bit_count, const bool data_is_big_endian); + +/* Public: Return a single nibble from the payload, with range checking. + * + * source - the source payload. + * nibble_index - the index of the nibble to retreive. The leftmost nibble is + * index 0. + * data_is_big_endian - if the data passed in is little endian, set this to false and it + * will be flipped before grabbing the bit field. + * + * Returns the retreived nibble, right aligned in a uint8_t. + */ +uint8_t eightbyte_get_nibble(const uint64_t source, const uint8_t nibble_index, + const bool data_is_big_endian); + +/* Public: Return a single byte from the payload, with range checking. + * + * source - the source byte array. + * byte_index - the index of the byte to retreive. The leftmost byte is index 0. + * data_is_big_endian - if the data passed in is little endian, set this to false and it + * will be flipped before grabbing the bit field. + * + * Returns the retreived byte. + */ +uint8_t eightbyte_get_byte(const uint64_t source, const uint8_t byte_index, + const bool data_is_big_endian); + +/* Public: Set the bit field in the given data array to the new value. + * + * destination - a byte array with size at least offset + bit_count. + * value - the value to set in the bit field. + * offset - the starting index of the bit field (beginning from 0). + * bit_count - the number of bits to set in the data. + * + * Returns true if the bit_count is enough to fully represent the value, and + * false if it will not fit. + */ +bool eightbyte_set_bitfield(uint64_t value, + const uint16_t offset, const uint16_t bit_count, uint64_t* destination); + +/* Private: Determine the index of the last bit used. + */ +uint8_t find_end_bit(const uint16_t num_bits); + +#ifdef __cplusplus +} +#endif + +#endif // __8BYTE_H__ diff --git a/CAN-binder/libs/bitfield-c/src/bitfield/bitarray.c b/CAN-binder/libs/bitfield-c/src/bitfield/bitarray.c new file mode 100644 index 00000000..dcb9a08f --- /dev/null +++ b/CAN-binder/libs/bitfield-c/src/bitfield/bitarray.c @@ -0,0 +1,145 @@ +#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 copy_bits(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; +} + +uint16_t bits_to_bytes(uint32_t bits) { + uint8_t byte_count = bits / CHAR_BIT; + if(bits % CHAR_BIT != 0) { + ++byte_count; + } + return byte_count; +} + +/** + * Find the ending bit of a bitfield within the final byte. + * + * Returns: a bit position from 0 to 7. + */ +uint8_t find_end_bit(const uint16_t numBits) { + int endBit = numBits % CHAR_BIT; + return endBit == 0 ? CHAR_BIT : endBit; +} + +bool copy_bits_right_aligned(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 copy_bits(source, source_length, offset, bit_count, destination, + destination_length, + // provide a proper destination offset so the result is right + // aligned + (destination_length - bits_to_bytes(bit_count)) * CHAR_BIT + + CHAR_BIT - find_end_bit(bit_count)); +} + +bool copy_bytes_right_aligned(const uint8_t source[], const uint16_t source_length, + const uint16_t offset, const uint16_t byte_count, + uint8_t* destination, const uint16_t destination_length) { + return copy_bits_right_aligned(source, source_length, offset * CHAR_BIT, + byte_count * CHAR_BIT, destination, destination_length); +} diff --git a/CAN-binder/libs/bitfield-c/src/bitfield/bitfield.c b/CAN-binder/libs/bitfield-c/src/bitfield/bitfield.c new file mode 100644 index 00000000..795f0208 --- /dev/null +++ b/CAN-binder/libs/bitfield-c/src/bitfield/bitfield.c @@ -0,0 +1,74 @@ +#include <bitfield/bitfield.h> +#include <limits.h> +#include <string.h> +#include <stddef.h> +#include <sys/param.h> + +uint64_t bitmask(const uint8_t bit_count) { + return (((uint64_t)0x1) << bit_count) - 1; +} + +uint8_t get_nibble(const uint8_t source[], const uint8_t source_length, + const uint8_t nibble_index) { + uint8_t byte_index = nibble_index / 2; + uint8_t result = get_byte(source, source_length, byte_index); + if(nibble_index % 2 == 0) { + result >>= NIBBLE_SIZE; + } + result &= bitmask(NIBBLE_SIZE); + return result; +} + +uint8_t get_byte(const uint8_t source[], const uint8_t source_length, + const uint8_t byte_index) { + if(byte_index < source_length) { + return source[byte_index]; + } + return 0; +} + +uint64_t get_bitfield(const uint8_t source[], const uint8_t source_length, + const uint16_t offset, const uint16_t bit_count) { + if(bit_count > 64 || bit_count < 1) { + // TODO error reporting? + return 0; + } + + ArrayOrBytes combined; + memset(combined.bytes, 0, sizeof(combined.bytes)); + if(copy_bits_right_aligned(source, source_length, offset, bit_count, + combined.bytes, sizeof(combined.bytes))) { + if(BYTE_ORDER == LITTLE_ENDIAN) { + combined.whole = __builtin_bswap64(combined.whole); + } + } else { + // debug("couldn't copy enough bits from source") + } + return combined.whole; +} + +bool set_nibble(const uint16_t nibble_index, const uint8_t value, + uint8_t* destination, const uint16_t destination_length) { + return copy_bits(&value, CHAR_BIT, NIBBLE_SIZE, NIBBLE_SIZE, destination, + destination_length, nibble_index * NIBBLE_SIZE); +} + +bool set_bitfield(const uint64_t value, const uint16_t offset, + const uint16_t bit_count, uint8_t destination[], + uint16_t destination_length) { + if(value > bitmask(bit_count)) { + return false; + } + + ArrayOrBytes combined = { + whole: value + }; + + if(BYTE_ORDER == LITTLE_ENDIAN) { + combined.whole = __builtin_bswap64(combined.whole); + } + + return copy_bits(combined.bytes, sizeof(combined.bytes), + sizeof(combined.bytes) * CHAR_BIT - bit_count, bit_count, + destination, destination_length, offset); +} diff --git a/CAN-binder/libs/bitfield-c/src/bitfield/bitfield.h b/CAN-binder/libs/bitfield-c/src/bitfield/bitfield.h new file mode 100644 index 00000000..df92639a --- /dev/null +++ b/CAN-binder/libs/bitfield-c/src/bitfield/bitfield.h @@ -0,0 +1,220 @@ +#ifndef __BITFIELD_H__ +#define __BITFIELD_H__ + +#include <stdint.h> +#include <stdbool.h> + +#define NIBBLE_SIZE (CHAR_BIT / 2) + +#ifdef __cplusplus +extern "C" { +#endif + +/* Public: Reads a subset of bits into a uint64_t, right aligned so they may be + * interpreted as a number. + * + * source - the bytes in question. + * source_size - the number of bytes in the source. + * offset - the starting index of the bit field (beginning from 0). + * bit_count - the width of the bit field to extract. This must be less than or + * equal to 64. + * + * Bit fields are positioned according to big-endian bit layout and the data is + * swapped automatically as necessary depending on the compiled architecture. + * + * For example, the bit layout of the value "42" (i.e. 00101010 set at position + * 14 with length 6 is: + * + * 000000000000001010100000000000000000000000000000000000000000000 + * + * and the same value and position but with length 8 is: + * + * 000000000000000010101000000000000000000000000000000000000000000 + * + * Examples + * + * uint64_t value = get_bitfield(data, data_size, 2, 4); + * + * Returns the value of the requested bit field, right aligned in a uint64_t. + */ +uint64_t get_bitfield(const uint8_t source[], const uint8_t source_length, + const uint16_t offset, const uint16_t bit_count); + +/* Public: Return a single nibble from the byte array, with range checking. + * + * source - the source byte array. + * source_length - the total length of the source array. + * nibble_index - the index of the nibble to retreive. The leftmost nibble is + * index 0. + * + * Returns the retreived nibble, right aligned in a uint8_t. + */ +uint8_t get_nibble(const uint8_t source[], const uint8_t source_length, + const uint8_t nibble_index); + +/* Public: Return a single byte from the byte array, with range checking. + * + * source - the source byte array. + * source_length - the total length of the source array. + * byte_index - the index of the byte to retreive. The leftmost byte is index 0. + * + * Returns the retreived byte. + */ +uint8_t get_byte(const uint8_t source[], const uint8_t source_length, + const uint8_t byte_index); + +/* 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. + * + * For example: + * + * uint8_t source[4] = {0x11, 0x22, 0x33, 0x44}; + * uint8_t destination[4] = {0}; + * copy_bits(source, sizeof(source), 8, 8, destination, + * sizeof(destination), 0); + * // destination[0] == 0x22 + * // destination[1] == 0x0 + * // destination[2] == 0x0 + * // destination[3] == 0x0 + * + * 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 copy_bits(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); + +/* Public: Copy a range of bits from one array to another, right aligning the + * result. + * + * This is mostly useful if you want to cast the result to an integer type + * instead of a byte array. + * + * For example: + * + * uint8_t source[4] = {0x11, 0x22, 0x33, 0x44}; + * uint8_t destination[4] = {0}; + * copy_bits_right_aligned(source, sizeof(source), 8, 8, destination, + * sizeof(destination)); + * // destination[0] == 0x0 + * // destination[1] == 0x0 + * // destination[2] == 0x0 + * // destination[3] == 0x22 + * + * int value = (int)destination; + * // value == 0x22 == 32 + * + * The arguments are the same as copy_bits, but without the destination_offset + * option - that's set automatically to right align the result. + * + * 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 copy_bits_right_aligned(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); + +/* Public: Copy a range of bytes from one byte array to another. + * + * The source and destination do not have to be the same size (as long as the + * desitnation has enough room to fit the range). + * + * source_origin - the source array. + * source_length - the total length of the source array in bytes, + * for range checking. + * source_offset - a byte offset to start the copy from the source array. + * Specify 0 to start from source_origin. + * byte_count - the number of bytes 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 bytes to start placing the copied range into + * the destination array. Specify 0 to start from the beginning of 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 copy_bytes_right_aligned(const uint8_t source[], const uint16_t source_length, + const uint16_t offset, const uint16_t byte_count, + uint8_t* destination, const uint16_t destination_length); + +/* Public: Set the a nibble in the given data array to the new value. + * + * nibble_index - the index of the nibble to retreive. The leftmost nibble is + * index 0. + * value - the value to set in the bit field. + * destination - the destination array. + * destination_length - the total length of the destination array in bytes, + * for range checking. + * + * Returns true if the bit_count is enough to fully represent the value, and + * false if it will not fit. + */ +bool set_nibble(const uint16_t nibble_index, const uint8_t value, + uint8_t* destination, const uint16_t destination_length); + +/* Public: Set the bit field in the given data array to the new value. + * + * value - the value to set in the bit field. + * offset - the starting index of the bit field (beginning from 0). + * bit_count - the number of bits to set in the data. + * destination - the destination array. + * destination_length - the total length of the destination array in bytes, + * for range checking. + * + * Returns true if the bit_count is enough to fully represent the value, and + * false if it will not fit. + */ +bool set_bitfield(const uint64_t value, const uint16_t offset, + const uint16_t bit_count, uint8_t destination[], + uint16_t destination_length); + +/* Public: Return a right aligned bitmask for a uint64_t. + * + * bit_count - the number of bits to mask, right aligned. + */ +uint64_t bitmask(const uint8_t bit_count); + +/* Private: + */ +uint16_t bits_to_bytes(uint32_t bits); + +/* Private: A union to assist swapping between uint64_t and a uint8_t array. + */ +typedef union { + uint64_t whole; + uint8_t bytes[sizeof(uint64_t)]; +} ArrayOrBytes; + +#ifdef __cplusplus +} +#endif + +#endif // __BITFIELD_H__ |