Fix byte alignment for right aligned functions.

4 files changed, 66 insertions, 15 deletions

diff --git a/src/bitfield/8byte.c b/src/bitfield/8byte.c
index f08f227d..e3eddf20 100644
--- a/src/bitfield/8byte.c
+++ b/src/bitfield/8byte.c
@@ -10,14 +10,6 @@ uint64_t bitmask(const uint8_t bit_count) {
     return (((uint64_t)0x1) << bit_count) - 1;
 }
 
-static 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;
-}
-
 uint8_t eightbyte_get_nibble(const uint64_t source, const uint8_t nibble_index,
         const bool big_endian) {
     return get_bit_field(source, NIBBLE_SIZE * nibble_index, NIBBLE_SIZE,
diff --git a/src/bitfield/bitarray.c b/src/bitfield/bitarray.c
index 35fc2d9e..dcb9a08f 100644
--- a/src/bitfield/bitarray.c
+++ b/src/bitfield/bitarray.c
@@ -108,6 +108,14 @@ bool copy_bits(const uint8_t* source_origin, const uint16_t source_length,
     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.
  *
@@ -125,5 +133,13 @@ bool copy_bits_right_aligned(const uint8_t source[], const uint16_t source_lengt
             destination_length,
             // provide a proper destination offset so the result is right
             // aligned
-            CHAR_BIT - find_end_bit(bit_count));
+            (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/src/bitfield/bitfield.h b/src/bitfield/bitfield.h
index 990482fc..ef04d9ca 100644
--- a/src/bitfield/bitfield.h
+++ b/src/bitfield/bitfield.h
@@ -110,9 +110,36 @@ bool copy_bits_right_aligned(const uint8_t source[], const uint16_t source_lengt
                 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);
+
 bool set_nibble(const uint16_t nibble_index, const uint8_t value,
                 uint8_t* destination, const uint16_t destination_length);
 
+uint16_t bits_to_bytes(uint32_t bits);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/tests/bitfield_tests.c b/tests/bitfield_tests.c
index e0646e6a..b03882a2 100644
--- a/tests/bitfield_tests.c
+++ b/tests/bitfield_tests.c
@@ -42,7 +42,8 @@ START_TEST (test_get_bits_out_of_range)
 {
     uint8_t data[4] = {0x12, 0x34, 0x56, 0x78};
     uint8_t result[4];
-    fail_if(copy_bits_right_aligned(data, 4, 25, 16, result, 4));
+    fail_if(copy_bits_right_aligned(data, sizeof(data), 25, 16, result,
+                sizeof(result)));
 }
 END_TEST
 
@@ -50,9 +51,22 @@ START_TEST (test_get_bits)
 {
     uint8_t data[4] = {0x12, 0x34, 0x56, 0x78};
     uint8_t result[4] = {0};
-    fail_unless(copy_bits_right_aligned(data, 4, 0, 16, result, 4));
-    ck_assert_int_eq(result[0], 0x12);
+    fail_unless(copy_bits_right_aligned(data, sizeof(data), 0, 16, result,
+                sizeof(result)));
+    ck_assert_int_eq(result[2], 0x12);
+    ck_assert_int_eq(result[3], 0x34);
+}
+END_TEST
+
+START_TEST (test_copy_bytes)
+{
+    uint8_t data[4] = {0x12, 0x34, 0x56, 0x78};
+    uint8_t result[4] = {0};
+    fail_unless(copy_bytes_right_aligned(data, sizeof(data), 1, 3, result,
+                sizeof(result)));
     ck_assert_int_eq(result[1], 0x34);
+    ck_assert_int_eq(result[2], 0x56);
+    ck_assert_int_eq(result[3], 0x78);
 }
 END_TEST
 
@@ -60,9 +74,10 @@ START_TEST (test_get_uneven_bits)
 {
     uint8_t data[4] = {0x12, 0x34, 0x56, 0x78};
     uint8_t result[4] = {0};
-    fail_unless(copy_bits_right_aligned(data, 4, 4, 12, result, 4));
-    ck_assert_int_eq(result[0], 0x2);
-    ck_assert_int_eq(result[1], 0x34);
+    fail_unless(copy_bits_right_aligned(data, sizeof(data), 4, 12, result,
+                sizeof(result)));
+    ck_assert_int_eq(result[2], 0x2);
+    ck_assert_int_eq(result[3], 0x34);
 }
 END_TEST
 
@@ -73,6 +88,7 @@ Suite* bitfieldSuite(void) {
     tcase_add_test(tc_core, test_get_nibble);
     tcase_add_test(tc_core, test_set_nibble);
     tcase_add_test(tc_core, test_get_bits);
+    tcase_add_test(tc_core, test_copy_bytes);
     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);