uc_bit_array_copy.cpp

00001 /*
00002  * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <uavcan/marshal/bit_stream.hpp>
00006 #include <climits>
00007 #include <cstring>
00008 #include <cstddef>
00009 
00010 namespace uavcan
00011 {
00012 void bitarrayCopy(const unsigned char* src, std::size_t src_offset, std::size_t src_len,
00013                   unsigned char* dst, std::size_t dst_offset)
00014 {
00015     /*
00016      * Should never be called on a zero-length buffer. The caller will also ensure that the bit
00017      * offsets never exceed one byte.
00018      */
00019 
00020     UAVCAN_ASSERT(src_len > 0U);
00021     UAVCAN_ASSERT(src_offset < 8U && dst_offset < 8U);
00022 
00023     const std::size_t last_bit = src_offset + src_len;
00024     while (last_bit - src_offset)
00025     {
00026         const uint8_t src_bit_offset = src_offset % 8U;
00027         const uint8_t dst_bit_offset = dst_offset % 8U;
00028 
00029         // The number of bits to copy
00030         const uint8_t max_offset = uavcan::max(src_bit_offset, dst_bit_offset);
00031         const std::size_t copy_bits = uavcan::min(last_bit - src_offset, std::size_t(8U - max_offset));
00032 
00033         /*
00034          * The mask indicating which bits of dest to update:
00035          * dst_byte_offset           copy_bits          write_mask
00036          * 0                         8                  11111111
00037          * 0                         7                  11111110
00038          * ...
00039          * 0                         1                  10000000
00040          * ...
00041          * 4                         4                  00001111
00042          * 4                         3                  00001110
00043          * 4                         2                  00001100
00044          * 4                         1                  00001000
00045          * ...
00046          * 7                         1                  00000001
00047          */
00048         const uint8_t write_mask = uint8_t(uint8_t(0xFF00U >> copy_bits) >> dst_bit_offset);
00049 
00050         // The value to be extracted from src, shifted into the dst location
00051         const uint8_t src_data = uint8_t((src[src_offset / 8U] << src_bit_offset) >> dst_bit_offset);
00052 
00053         dst[dst_offset / 8U] = uint8_t((dst[dst_offset / 8U] & ~write_mask) | (src_data & write_mask));
00054 
00055         src_offset += copy_bits;
00056         dst_offset += copy_bits;
00057     }
00058 }
00059 }