inserts a series of bits into a byte buffer at the specified bit offset

gistfile1.h

/// inserts a series of bits into a byte buffer at the specified bit offset
/// @param[in]  dst      pointer to buffer
/// @param[in]  src      array of data to insert into the dst buffer
/// @param[in]  len      length in bytes of the buffer
/// @param[in]  offset   number of bits to offset the data in the buffer
/// @param[in]  n        number of bits to insert into the buffer
void bitops_copy(uint8_t * dst, uint8_t * src, uint32_t len, uint32_t offset,
   uint32_t n)
{
   uint32_t byte_required;
   uint32_t byte_offset;
   uint32_t bit_offset;
   uint32_t byte_n;
   uint32_t bit_n;
   uint32_t mask;
   uint32_t pos;
   uint32_t u;

   // preliminary calculations to determine limits
   byte_required  = ((offset + n) & 0x07) ? 1 : 0;
   byte_required  = ((offset + n) / 0x08) + byte_required;
   byte_offset    = (offset / 0x08);
   bit_offset     = (offset & 0x07);
   byte_n         = (n / 0x08);
   bit_n          = (n & 0x07);

   if (dst == src)
      return;
   if (byte_required > len)
      return;

   pos = 0;
   if (byte_n)
   {
      // generates a mask used for copying bits accross bit boundaries
      mask = 0xFF;
      for(u = 0; u < bit_offset; u++)
         mask = (mask << 1) & 0xFF;

      // copies first byte of data
      dst[byte_offset]     = ((src[0] << bit_offset) & ( mask)) |
                             ((dst[byte_offset])     & (~mask));

      // copies middle bytes
      for(pos = 1; pos < (byte_n); pos++)
         dst[byte_offset+pos] = ((src[pos]   << (bit_offset))   &  (mask)) |
                                ((src[pos-1] >> (8-bit_offset)) & (~mask));

      // copies last byte on an even bit boundary
      dst[byte_offset+pos] = ((dst[byte_offset+pos])         &  (mask)) |
                             ((src[pos-1] >> (8-bit_offset)) & (~mask));

   };

   if ((bit_offset) || (bit_n))
   {
      // generates a mask used to copy remaining least significant
      // bits into the remaining least significant byte
      mask = 0x00;
      for(u = 0; u < bit_n; u++)
         mask = ((mask << 1) | 0x01) & 0xFF;
      for(u = 0; u < bit_offset; u++)
         mask = (mask << 1) & 0xFF;

      // copies remaining bits into the least significant
      // byte (of remaining two bytes)
      dst[byte_offset+pos]   = ((src[pos] << bit_offset) &  (mask)) |
                               ((dst[byte_offset+pos])   & (~mask));

      if ((bit_offset + bit_n) > 8)
      {
         // generates a mask used to copy remaining most significant
         // bits into the remaining most significant byte
         mask = 0x00;
         for(u = 0; u < ((bit_n+bit_offset)%8); u++)
            mask = ((mask << 1) | 0x01) & 0xFF;

         // copies remaining bits into the most significant
         // byte (of remaining two bytes)
         u = 8-bit_offset;
         dst[byte_offset+pos+1] = ((src[pos] >> u)          &  (mask)) |
                                  ((dst[byte_offset+pos+1]) & (~mask));
      };
   };

   return;
}