Draft work making the bitfield functions more generic.

2 files changed, 173 insertions, 29 deletions

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 <bitfield/bitfield.h>
+#include <limits.h>
+#include <string.h>
+#include <stddef.h>
+
+#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.
  *