/* Cesar project {{{ * * Copyright (C) 2008 Spidcom * * <<>> * * }}} */ /** * \file lib/src/bitstream.c * \brief Bit stream access using word access. * \ingroup lib */ #include "common/std.h" #include "lib/bitstream.h" /* Evaluates to a number composed of n LSB one bits, for n in 0-31. */ #define bit_mask(n) (~(0xffffffffu << (n))) /* Evaluates to a number composed of n LSB one bits, for n in 32-63. */ #define bit_mask64u(n) (~((u64) (0xffffffffu << ((n) - 32)) << 32)) /** Union for quick access to words inside a double word. */ union dwu { u64 dw; struct { #if DEFS_BIG_ENDIAN u32 high, low; #else u32 low, high; #endif } w; }; /** * Logical shift right of a 64 bit value of 1 to 32 bits. * \param a 64 bit value * \param s shift (1-32) * \return shifted value */ extern inline u64 lshrdu32 (u64 a, uint s) { dbg_assert (s > 0 && s <= 32); union dwu i = { .dw = a }; if (s == 32) { i.w.low = i.w.high; i.w.high = 0; } else { i.w.low = i.w.high << (32 - s) | i.w.low >> s; i.w.high = i.w.high >> s; } return i.dw; } /** * Logical shift left of a 32 bit value, or'ed with a 64 bit value. * \param a 64 bit value to be or'ed with * \param b 32 bit value to shift * \param s shift (0-31) * \return or result * * Equivalent of: * \code * a |= (u64) b << s; * \endcode */ extern inline u64 lshlwo64 (u64 a, u32 b, uint s) { dbg_assert (s < 32); union dwu i = { .dw = a }; if (s == 0) { i.w.low |= b; } else { i.w.low |= b << s; i.w.high |= b >> (32 - s); } return i.dw; } void bitstream_init (bitstream_t *ctx, void *data, uint nb_bytes, bitstream_direction_t direction) { dbg_assert (ctx); dbg_assert (direction == BITSTREAM_READ || direction == BITSTREAM_WRITE); /* Initialise context. */ ctx->direction = direction; ctx->buffer = 0; ctx->buffer_bits = 0; ctx->data = NULL; ctx->data_bits = 0; ctx->data_written_bits = 0; ctx->buffer_cb = NULL; /* Set first data buffer. */ bitstream_set_buffer (ctx, data, nb_bytes); } void bitstream_init_buffer_cb (bitstream_t *ctx, bitstream_buffer_cb_t cb, void *user_data) { dbg_assert (ctx); ctx->buffer_cb = cb; ctx->buffer_cb_user_data = user_data; } void bitstream_set_buffer (bitstream_t *ctx, void *data, uint nb_bytes) { dbg_assert (ctx); dbg_assert (ctx->direction == BITSTREAM_READ || ctx->direction == BITSTREAM_WRITE); dbg_assert (ctx->data == NULL && ctx->data_bits == 0); dbg_assert_ptr (data); dbg_assert (nb_bytes != 0); uint nb_bits = nb_bytes * 8; /* No problem with aligned buffers. */ if (((uint) data & 0x3) == 0) { ctx->data = data; ctx->data_bits = nb_bits; } else { u32 *adata = (void *) ((uint) data & ~0x3); uint una = ((uint) data - (uint) adata) * 8; if (ctx->direction == BITSTREAM_READ) { u32 w = *adata; /* Drop unused LSB. */ w >>= una; /* Mask unused MSB. */ if (nb_bits + una < 32) w &= bit_mask (nb_bits); /* Put in work buffer. */ ctx->buffer = lshlwo64 (ctx->buffer, w, ctx->buffer_bits); ctx->data = adata + 1; if (nb_bits + una < 32) { ctx->buffer_bits += nb_bits; ctx->data_bits = 0; } else { ctx->buffer_bits += 32 - una; ctx->data_bits = nb_bits + una - 32; } } else { /* If there is enough room in the work buffer, stuff data * preceding the unaligned buffer into it. */ if (ctx->buffer_bits <= 32) /* 40 will be OK too, who cares... */ { u32 w = *adata; /* Mask preceding data. */ w &= bit_mask (una); /* Stuff it into work buffer. */ ctx->buffer = (ctx->buffer << una) | w; ctx->buffer_bits += una; ctx->data = adata; ctx->data_bits = nb_bits + una; ctx->data_written_bits -= una; /* Fix written data. */ } else { /* No enough room, should write the first word now. */ u32 w = *adata; /* Mask preceding data. */ u32 mask; mask = bit_mask (una); if (nb_bits + una < 32) mask |= ~bit_mask (nb_bits + una); w &= mask; /* Add and store bits from work buffer. */ w |= ((u32) ctx->buffer << una) & ~mask; *adata = w; /* Update context. */ uint written_bits; if (nb_bits + una < 32) written_bits = nb_bits; else written_bits = 32 - una; ctx->buffer = lshrdu32 (ctx->buffer, written_bits); ctx->buffer_bits -= written_bits; ctx->data_bits = nb_bits - written_bits; ctx->data_written_bits += written_bits; ctx->data = adata + 1; } } } } uint bitstream_read_ (bitstream_t *ctx, uint nb_bits __FL) { dbg_assert (ctx); dbg_assert (ctx->direction == BITSTREAM_READ); dbg_blame (nb_bits > 0 && nb_bits <= 32); /* If input buffer exhausted, read the next word. */ while (nb_bits > ctx->buffer_bits) { if (ctx->data_bits) { /* Load bits from data buffer. */ u32 w = *ctx->data++; uint read_bits = ctx->data_bits; if (read_bits < 32) w &= bit_mask (read_bits); else read_bits = 32; ctx->buffer = lshlwo64 (ctx->buffer, w, ctx->buffer_bits); ctx->data_bits -= read_bits; ctx->buffer_bits += read_bits; } else { /* Data buffer exhausted. */ dbg_assert (ctx->buffer_cb); ctx->data = NULL; ctx->buffer_cb (ctx, ctx->buffer_cb_user_data); } } dbg_assert (nb_bits <= ctx->buffer_bits); /* Read from input buffer. */ u32 v = ctx->buffer; if (nb_bits < 32) v &= bit_mask (nb_bits); ctx->buffer = lshrdu32 (ctx->buffer, nb_bits); ctx->buffer_bits -= nb_bits; return v; } void bitstream_write_ (bitstream_t *ctx, uint value, uint nb_bits __FL) { dbg_assert (ctx); dbg_assert (ctx->direction == BITSTREAM_WRITE); dbg_blame (nb_bits > 0 && nb_bits <= 32); dbg_blame (nb_bits == 32 || (value & ~bit_mask (nb_bits)) == 0); /* Write to output buffer. */ ctx->buffer = lshlwo64 (ctx->buffer, value, ctx->buffer_bits); ctx->buffer_bits += nb_bits; /* If at least 32 bits, write it. */ while (ctx->buffer_bits >= 32) { if (ctx->data_bits) { /* Write bits to data buffer. */ uint written_bits = ctx->data_bits; /* Do no clobber data following the data buffer. */ if (written_bits >= 32) { written_bits = 32; *ctx->data++ = (u32) ctx->buffer; } else { u32 mask = bit_mask (written_bits); *ctx->data = (*ctx->data & ~mask) | ((u32) ctx->buffer & mask); /* Do not update ctx->data, it is empty anyway. */ } ctx->data_bits -= written_bits; ctx->data_written_bits += written_bits; ctx->buffer_bits -= written_bits; ctx->buffer = lshrdu32 (ctx->buffer, written_bits); } else { /* Data buffer exhausted. */ dbg_assert (ctx->buffer_cb); ctx->data = NULL; ctx->buffer_cb (ctx, ctx->buffer_cb_user_data); } } } uint bitstream_write_finalise (bitstream_t *ctx) { dbg_assert (ctx); dbg_assert (ctx->direction == BITSTREAM_WRITE); /* If at least 1 bit, write it. */ dbg_assert (ctx->buffer_bits < 32); while (ctx->buffer_bits) { /* Warning, differences with code in bitstream_write is subtle. */ if (ctx->data_bits) { /* Write bits to data buffer. */ uint written_bits; /* Do no clobber data following the data buffer. */ if (ctx->data_bits >= 32) { written_bits = MIN (32u, ctx->buffer_bits); *ctx->data++ = (u32) (ctx->buffer & 0xffffffff); } else { written_bits = MIN (ctx->data_bits, ctx->buffer_bits); u32 mask = bit_mask (ctx->data_bits); *ctx->data = (*ctx->data & ~mask) | ((u32) ctx->buffer & mask); /* Do not update ctx->data, it is empty anyway. */ } /* This is not futile if a new buffer is set: */ ctx->data_bits -= written_bits; ctx->data_written_bits += written_bits; ctx->buffer_bits -= written_bits; ctx->buffer = lshrdu32 (ctx->buffer, written_bits); } else { /* Data buffer exhausted. */ dbg_assert (ctx->buffer_cb); ctx->data = NULL; ctx->buffer_cb (ctx, ctx->buffer_cb_user_data); } } return ctx->data_written_bits; } u64 bitstream_read_large (bitstream_t *ctx, uint nb_bits) { dbg_assert (ctx); dbg_assert (nb_bits > 32 && nb_bits <= 64); u32 low = bitstream_read (ctx, 32); u32 hi = bitstream_read (ctx, nb_bits - 32); return ((u64) hi << 32) | low; } void bitstream_write_large (bitstream_t *ctx, u64 value, uint nb_bits) { dbg_assert (ctx); dbg_assert (nb_bits > 32 && nb_bits <= 64); bitstream_write (ctx, value, 32); bitstream_write (ctx, value >> 32, nb_bits - 32); } void bitstream_skip (bitstream_t *ctx, uint nb_bits) { dbg_assert (ctx); dbg_assert (ctx->direction == BITSTREAM_READ); while (nb_bits) { /* Skip from buffer. */ if (nb_bits < ctx->buffer_bits) { /* Enough data in buffer. */ dbg_assert (nb_bits < 32); ctx->buffer = lshrdu32 (ctx->buffer, nb_bits); ctx->buffer_bits -= nb_bits; nb_bits = 0; } else { /* Not enough, eat all. */ nb_bits -= ctx->buffer_bits; ctx->buffer = 0; ctx->buffer_bits = 0; /* Skip from data buffer. */ if (nb_bits <= ctx->data_bits) { /* Enough data, eat words. */ uint skip_words = nb_bits / 32; uint skip_bits = nb_bits % 32; nb_bits = 0; ctx->data += skip_words; ctx->data_bits -= skip_words * 32; if (skip_bits) { /* Eat bits. */ u32 w = *ctx->data++; uint read_bits = ctx->data_bits; if (read_bits < 32) w &= bit_mask (read_bits); else read_bits = 32; ctx->data_bits -= read_bits; ctx->buffer = lshrdu32 (w, skip_bits); ctx->buffer_bits = read_bits - skip_bits; } } else { /* Not enough, eat all. */ dbg_assert (ctx->buffer_cb); nb_bits -= ctx->data_bits; ctx->data = NULL; ctx->data_bits = 0; ctx->buffer_cb (ctx, ctx->buffer_cb_user_data); } } } } void bitstream_read_buf (bitstream_t *ctx, u8 *buf, uint size) { dbg_assert (ctx); dbg_assert_ptr (buf); while (size--) *buf++ = bitstream_read (ctx, 8); } void bitstream_write_buf (bitstream_t *ctx, const u8 *buf, uint size) { dbg_assert (ctx); dbg_assert_ptr (buf); while (size--) bitstream_write (ctx, *buf++, 8); } void bitstream_copy_buf (bitstream_t *dst, bitstream_t *src, uint size) { dbg_assert (dst); dbg_assert (src); while (size--) bitstream_write (dst, bitstream_read (src, 8), 8); } uint bitstream_read_str (bitstream_t *ctx, char *str, uint size) { char c; uint left; dbg_assert (ctx); dbg_assert_ptr (str); dbg_assert (size); left = size; while (1) { c = bitstream_read (ctx, 8); /* If read character is null, do not decrement left. */ if (c && --left) { *str++ = c; } else { /* Always terminate with a null character. */ *str = 0; break; } } return size - left; } uint bitstream_write_str (bitstream_t *ctx, const char *str, uint size) { char c; uint left; dbg_assert (ctx); dbg_assert_ptr (str); dbg_assert (size); left = size; do { c = *str++; /* If read character is null, do not decrement left. */ if (c && !--left) c = '\0'; bitstream_write (ctx, c, 8); } while (c); return size - left; } uint bitstream_available_bits (bitstream_t *ctx) { dbg_assert (ctx); dbg_assert (ctx->direction == BITSTREAM_READ || ctx->direction == BITSTREAM_WRITE); if (ctx->direction == BITSTREAM_READ) return ctx->data_bits + ctx->buffer_bits; else return ctx->data_bits - ctx->buffer_bits; } uint bitstream_written_bits (bitstream_t *ctx) { dbg_assert (ctx); dbg_assert (ctx->direction == BITSTREAM_READ || ctx->direction == BITSTREAM_WRITE); if (ctx->direction == BITSTREAM_READ) return 0; else return ctx->data_written_bits + ctx->buffer_bits; } void bitstream_access_8 (bitstream_t *ctx, u8 *value, uint nb_bits) { dbg_assert (ctx); dbg_assert (ctx->direction == BITSTREAM_READ || ctx->direction == BITSTREAM_WRITE); dbg_assert (value); dbg_assert (nb_bits <= 8); if (ctx->direction == BITSTREAM_READ) *value = bitstream_read (ctx, nb_bits); else bitstream_write (ctx, *value, nb_bits); } void bitstream_access_16 (bitstream_t *ctx, u16 *value, uint nb_bits) { dbg_assert (ctx); dbg_assert (ctx->direction == BITSTREAM_READ || ctx->direction == BITSTREAM_WRITE); dbg_assert (value); dbg_assert (nb_bits <= 16); if (ctx->direction == BITSTREAM_READ) *value = bitstream_read (ctx, nb_bits); else bitstream_write (ctx, *value, nb_bits); } void bitstream_access_32 (bitstream_t *ctx, u32 *value, uint nb_bits) { dbg_assert (ctx); dbg_assert (ctx->direction == BITSTREAM_READ || ctx->direction == BITSTREAM_WRITE); dbg_assert (value); if (ctx->direction == BITSTREAM_READ) *value = bitstream_read (ctx, nb_bits); else bitstream_write (ctx, *value, nb_bits); } void bitstream_access_64 (bitstream_t *ctx, u64 *value, uint nb_bits) { dbg_assert (ctx); dbg_assert (ctx->direction == BITSTREAM_READ || ctx->direction == BITSTREAM_WRITE); dbg_assert (value); if (ctx->direction == BITSTREAM_READ) { if (nb_bits > 32) *value = bitstream_read_large (ctx, nb_bits); else *value = bitstream_read (ctx, nb_bits); } else { if (nb_bits > 32) bitstream_write_large (ctx, *value, nb_bits); else bitstream_write (ctx, *value, nb_bits); } } uint bitstream_access_finalise (bitstream_t *ctx) { dbg_assert (ctx); dbg_assert (ctx->direction == BITSTREAM_READ || ctx->direction == BITSTREAM_WRITE); if (ctx->direction == BITSTREAM_READ) return 0; else return bitstream_write_finalise (ctx); } void bitstream_access_buf (bitstream_t *ctx, u8 *buf, uint size) { dbg_assert (ctx); dbg_assert (ctx->direction == BITSTREAM_READ || ctx->direction == BITSTREAM_WRITE); if (ctx->direction == BITSTREAM_READ) bitstream_read_buf (ctx, buf, size); else bitstream_write_buf (ctx, buf, size); } uint bitstream_access_str (bitstream_t *ctx, char *str, uint size) { dbg_assert (ctx); dbg_assert (ctx->direction == BITSTREAM_READ || ctx->direction == BITSTREAM_WRITE); if (ctx->direction == BITSTREAM_READ) return bitstream_read_str (ctx, str, size); else return bitstream_write_str (ctx, str, size); } uint bitstream_direct_read (const void *data, uint bit_offset, uint nb_bits) { const u32 *p; uint una; uint r; dbg_assert_ptr (data); dbg_assert (nb_bits > 0 && nb_bits <= 32); /* Normalise bit offset. */ p = (const u32 *) ((u32) data + bit_offset / 8); una = ((u32) p & 0x3) * 8 + bit_offset % 8; p = (const u32 *) ((u32) p & ~0x3); /* Read from first word. */ r = p[0] >> una; /* Read from second word if needed. */ if (una + nb_bits > 32) r |= p[1] << (32 - una); /* Mask if needed. */ if (nb_bits < 32) r &= bit_mask (nb_bits); return r; } u64 bitstream_direct_read_large (const void *data, uint bit_offset, uint nb_bits) { const u32 *p; uint una; u64 r; dbg_assert_ptr (data); dbg_assert (nb_bits > 32 && nb_bits <= 64); /* Normalise bit offset. */ p = (const u32 *) ((u32) data + bit_offset / 8); una = ((u32) p & 0x3) * 8 + bit_offset % 8; p = (const u32 *) ((u32) p & ~0x3); /* Read from first and second word. */ r = p[0] | (u64) p[1] << 32; if (una != 0) r = lshrdu32 (r, una); /* Read from third word if needed. */ if (una + nb_bits > 64) r |= (u64) (p[2] << (32 - una)) << 32; /* Mask if needed. */ if (nb_bits < 64) r &= bit_mask64u (nb_bits); return r; } mac_t bitstream_direct_read_mac (const void *data) { const u32 *p; uint una; union dwu r; /* No assert, too hurry. */ una = (u32) data & 0x3; p = (const u32 *) ((u32) data & ~0x3); /* Different code depending on alignment. */ if (una == 0) { r.w.low = p[0]; r.w.high = p[1] << 16 >> 16; return r.dw; } else if (una == 1) { r.w.low = p[0] >> 8 | p[1] << 24; r.w.high = p[1] << 8 >> 16; return r.dw; } else if (una == 2) { r.w.low = p[0] >> 16 | p[1] << 16; r.w.high = p[1] >> 16; return r.dw; } else { r.w.low = p[0] >> 24 | p[1] << 8; r.w.high = p[1] >> 24 | p[2] << 24 >> 16; return r.dw; } } void bitstream_direct_read_macs (const void *data, mac_t *dst, mac_t *src) { const u32 *p; uint una; union dwu d, s; /* No assert, too hurry. */ una = (u32) data & 0x3; p = (const u32 *) ((u32) data & ~0x3); /* Different code depending on alignment. */ if (una == 0) { d.w.low = p[0]; d.w.high = p[1] << 16 >> 16; s.w.low = p[1] >> 16 | p[2] << 16; s.w.high = p[2] >> 16; } else if (una == 1) { d.w.low = p[0] >> 8 | p[1] << 24; d.w.high = p[1] << 8 >> 16; s.w.low = p[1] >> 24 | p[2] << 8; s.w.high = p[2] >> 24 | p[3] << 24 >> 16; } else if (una == 2) { d.w.low = p[0] >> 16 | p[1] << 16; d.w.high = p[1] >> 16; s.w.low = p[2]; s.w.high = p[3] << 16 >> 16; } else { d.w.low = p[0] >> 24 | p[1] << 8; d.w.high = p[1] >> 24 | p[2] << 24 >> 16; s.w.low = p[2] >> 8 | p[3] << 24; s.w.high = p[3] << 8 >> 16; } *dst = d.dw; *src = s.dw; } void bitstream_direct_write (void *data, uint bit_offset, uint value, uint nb_bits) { u32 *p; uint una; dbg_assert_ptr (data); dbg_assert (nb_bits > 0 && nb_bits <= 32); dbg_assert (nb_bits == 32 || (value & ~bit_mask (nb_bits)) == 0); /* Normalise bit offset. */ p = (u32 *) ((u32) data + bit_offset / 8); una = ((u32) p & 0x3) * 8 + bit_offset % 8; p = (u32 *) ((u32) p & ~0x3); /* Is it aligned? */ if (!una) { /* Yes, only handle data after, only one word. */ if (nb_bits < 32) p[0] = (p[0] & ~bit_mask (nb_bits)) | value; else p[0] = value; } else { u32 mask; /* No, should mask data before, may use two words. */ mask = bit_mask (una); if (una + nb_bits < 32) mask |= ~bit_mask (una + nb_bits); p[0] = (p[0] & mask) | value << una; if (una + nb_bits > 32) p[1] = (p[1] & ~bit_mask (una + nb_bits - 32)) | value >> (32 - una); } } void bitstream_direct_write_large (void *data, uint bit_offset, u64 value, uint nb_bits) { u32 *p; uint una; dbg_assert_ptr (data); dbg_assert (nb_bits > 32 && nb_bits <= 64); dbg_assert (nb_bits == 64 || (value & ~bit_mask64u (nb_bits)) == 0); /* Normalise bit offset. */ p = (u32 *) ((u32) data + bit_offset / 8); una = ((u32) p & 0x3) * 8 + bit_offset % 8; p = (u32 *) ((u32) p & ~0x3); /* Is it aligned? */ if (!una) { /* Yes, only handle data after, only two words. */ p[0] = value; if (nb_bits < 64) p[1] = (p[1] & ~bit_mask (nb_bits - 32)) | (u32) (value >> 32); else p[1] = value >> 32; } else { /* No, should mask data before, may use three words. */ p[0] = (p[0] & bit_mask (una)) | (u32) value << una; u32 vm = lshrdu32 (value, 32 - una); if (una + nb_bits < 64) { p[1] = (p[1] & ~bit_mask (una + nb_bits - 32)) | vm; } else { p[1] = vm; if (una + nb_bits > 64) p[2] = (p[2] & ~bit_mask (una + nb_bits - 64)) | (u32) (value >> 32) >> (32 - una); } } } void bitstream_memcpy (void *dest, const void *src, uint size) { const u32 *s; u32 *d; u32 sw, dw; uint sb, db; uint duna, suna; dbg_assert_ptr (dest); dbg_assert_ptr (src); /* Setup destination. */ duna = (u32) dest & 0x3; if (duna) { d = (u32 *) ((u32) dest & ~0x3); dw = *d; dw &= bit_mask (duna * 8); db = duna; } else { d = dest; dw = 0; db = 0; } /* Setup source. */ suna = (u32) src & 0x3; if (suna) { s = (u32 *) ((u32) src & ~0x3); sw = *s++; sw >>= suna * 8; sb = 4 - suna; } else { s = src; sw = 0; /* Compiler warning. */ sb = 0; } /* Copy bytes. */ while (size) { if (sb == 0) { sw = *s++; sb = 4; } dw |= (sw & 0xff) << (db * 8); db++; sw >>= 8; sb--; if (db == 4) { *d++ = dw; db = 0; dw = 0; } size--; } /* Last destination word. */ if (db) { dw |= *d & ~bit_mask (db * 8); *d = dw; } } bool bitstream_memcmp (const void *s1, const void *s2, uint size) { const u32 *p1, *p2; u32 w1, w2; uint b1, b2; uint una1, una2; dbg_assert_ptr (s1); dbg_assert_ptr (s2); /* Setup s1. */ una1 = (u32) s1 & 0x3; if (una1) { p1 = (u32 *) ((u32) s1 & ~0x3); w1 = *p1++; w1 >>= una1 * 8; b1 = 4 - una1; } else { p1 = s1; w1 = 0; /* Compiler warning. */ b1 = 0; } /* Setup s2. */ una2 = (u32) s2 & 0x3; if (una2) { p2 = (u32 *) ((u32) s2 & ~0x3); w2 = *p2++; w2 >>= una2 * 8; b2 = 4 - una2; } else { p2 = s2; w2 = 0; /* Compiler warning. */ b2 = 0; } /* Compare bytes. */ while (size) { if (b1 == 0) { w1 = *p1++; b1 = 4; } if (b2 == 0) { w2 = *p2++; b2 = 4; } if ((w1 & 0xff) != (w2 & 0xff)) return false; w1 >>= 8; b1--; w2 >>= 8; b2--; size--; } return true; }