Fix byte alignment for right aligned functions.

3 files changed, 44 insertions, 9 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