diff --git a/libuavcan/include/uavcan/marshal/bit_stream.hpp b/libuavcan/include/uavcan/marshal/bit_stream.hpp
index ac4a36edc8..35586390f5 100644
--- a/libuavcan/include/uavcan/marshal/bit_stream.hpp
+++ b/libuavcan/include/uavcan/marshal/bit_stream.hpp
@@ -12,7 +12,14 @@
 
 namespace uavcan
 {
-
+/**
+ * This function implements fast copy of unaligned bit arrays. It isn't part of the library API, so it is not exported.
+ * @param src_org       Source array
+ * @param src_offset    Bit offset of the first source byte
+ * @param src_len       Number of bits to copy
+ * @param dst_org       Destination array
+ * @param dst_offset    Bit offset of the first destination byte
+ */
 void bitarrayCopy(const unsigned char* src_org, unsigned src_offset, unsigned src_len,
                   unsigned char* dst_org, unsigned dst_offset);
 
@@ -30,11 +37,18 @@ class UAVCAN_EXPORT BitStream
 
     static inline unsigned bitlenToBytelen(unsigned bits) { return (bits + 7) / 8; }
 
-    static inline void copyBitArray(const uint8_t* src_org, unsigned src_offset, unsigned src_len,
-                                    uint8_t* dst_org, unsigned dst_offset)
+    static inline void copyBitArrayAlignedToUnaligned(const uint8_t* src_org, unsigned src_len,
+                                                      uint8_t* dst_org, unsigned dst_offset)
+    {
+        bitarrayCopy(reinterpret_cast<const unsigned char*>(src_org), 0, src_len,
+                     reinterpret_cast<unsigned char*>(dst_org), dst_offset);
+    }
+
+    static inline void copyBitArrayUnalignedToAligned(const uint8_t* src_org, unsigned src_offset, unsigned src_len,
+                                                      uint8_t* dst_org)
     {
         bitarrayCopy(reinterpret_cast<const unsigned char*>(src_org), src_offset, src_len,
-                     reinterpret_cast<unsigned char*>(dst_org), dst_offset);
+                     reinterpret_cast<unsigned char*>(dst_org), 0);
     }
 
 public:
diff --git a/libuavcan/src/marshal/uc_bit_stream.cpp b/libuavcan/src/marshal/uc_bit_stream.cpp
index 156c39233a..0f4a523684 100644
--- a/libuavcan/src/marshal/uc_bit_stream.cpp
+++ b/libuavcan/src/marshal/uc_bit_stream.cpp
@@ -23,7 +23,7 @@ int BitStream::write(const uint8_t* bytes, const unsigned bitlen)
     tmp[0] = tmp[bytelen - 1] = 0;
 
     fill(tmp, tmp + bytelen, uint8_t(0));
-    copyBitArray(bytes, 0, bitlen, tmp, bit_offset_ % 8);
+    copyBitArrayAlignedToUnaligned(bytes, bitlen, tmp, bit_offset_ % 8);
 
     const unsigned new_bit_offset = bit_offset_ + bitlen;
 
@@ -70,7 +70,7 @@ int BitStream::read(uint8_t* bytes, const unsigned bitlen)
     }
 
     fill(bytes, bytes + bitlenToBytelen(bitlen), uint8_t(0));
-    copyBitArray(tmp, bit_offset_ % 8, bitlen, bytes, 0);
+    copyBitArrayUnalignedToAligned(tmp, bit_offset_ % 8, bitlen, bytes);
     bit_offset_ += bitlen;
     return ResultOk;
 }