/* Cesar project {{{ * * Copyright (C) 2007 Spidcom * * <<>> * * }}} */ /** * \file lib/src/bitstream.c * \brief Bit stream access using word access. * \ingroup lib */ #include "common/std.h" #include "lib/bitstream.h" #include "lib/swap.h" static u32 bitstream_rol32 (u32 val, uint n) { return (n == bitsizeof (u32) ? 0x0 : (val << n)); } static uint bitstream_read (bitstream_t *ctx, u32 *value, uint nb_bit) { uint ret = 0; u32 buf, mask, x; for(buf = 0; nb_bit; nb_bit -= x, ret += x) { /* x is bits_left_in_ctx_buf or nb_bit */ x = MIN ((bitsizeof(ctx->buf) - ctx->bit_offset), nb_bit); /* create bitmask */ mask = bitstream_rol32 (1ul, x) - 1; /* fill local buf */ buf |= (((ctx->buf >> ctx->bit_offset) & mask) << ret); if(x < nb_bit) { /* not enough bits in ctx->buf, get next word */ ctx->buf = *ctx->stream++; ctx->bit_offset = 0; /* if no more bits are available */ if(!bitstream_available_bits (ctx)) break; }else /* move ctx bit offset forward */ ctx->bit_offset += x; } *value = buf; return ret; } static uint bitstream_write (bitstream_t *ctx, u32 *value, uint nb_bit) { u32 mask, x; uint ret = 0; for(; nb_bit; nb_bit -= x, ret += x) { /* x is bits_left_in_ctx_buf or nb_bit */ x = MIN ((bitsizeof(ctx->buf) - ctx->bit_offset), nb_bit); /* create bitmask */ mask = bitstream_rol32 (1ul, x) - 1; /* fill ctx buf */ ctx->buf |= (((*value >> ret) & mask) << ctx->bit_offset); if(x < nb_bit) { /* Write ctx buf to stream */ *ctx->stream++ = ctx->buf; ctx->bit_offset = 0; ctx->buf = 0; /* if no more bits are available */ if(!bitstream_available_bits (ctx)) break; } else /* move ctx bit offset forward */ ctx->bit_offset += x; } return ret; } void bitstream_init (bitstream_t *ctx, void *data, uint nb_bytes, bitstream_type_t type) { ctx->start = (u8*)data; /* ensure 32-bit alignment */ ctx->stream = (u32*)((u8*)data - ((u32)data & 0x03)); ctx->bit_offset = ((u32)data & 0x03) * 8; ctx->buf = ((1ul << ctx->bit_offset) - 1) & *ctx->stream; ctx->nb_bytes = nb_bytes; ctx->type = type; /* In read mode, load first word */ if(ctx->type == BITSTREAM_READ) ctx->buf = *ctx->stream++; } uint bitstream_finalise (bitstream_t *ctx) { if(ctx->type == BITSTREAM_WRITE && ctx->bit_offset) { /* In write mode, write last buffered word */ /* create bitmask */ u32 mask = ~(bitstream_rol32 (1ul, ctx->bit_offset) - 1); /* write last word */ *ctx->stream = (*ctx->stream & mask) | ctx->buf; return ctx->bit_offset; } return 0; } uint bitstream_available_bits (bitstream_t *ctx) { dbg_assert (ctx); uint n = (((u32)ctx->stream <= ((u32)ctx->start + ctx->nb_bytes)) ? ((((u32)ctx->start + ctx->nb_bytes) - (u32)(ctx->stream)) * 8) : 0x00000000); return (ctx->type == BITSTREAM_READ ? n + (bitsizeof(ctx->buf) - ctx->bit_offset) : n - ctx->bit_offset); } uint bitstream_access_8 (bitstream_t *ctx, void *value, uint nb_bit) { dbg_assert (ctx); dbg_assert (nb_bit <= 8); u32 buf; uint ret; if(ctx->type == BITSTREAM_READ) { ret = bitstream_read (ctx, &buf, nb_bit); *(u8*)value = 0xff & buf; return ret; } else { buf = *(u8*)value; return bitstream_write (ctx, &buf, nb_bit); } } uint bitstream_access_16 (bitstream_t *ctx, void *value, uint nb_bit) { dbg_assert (ctx); dbg_assert (nb_bit <= 16); u32 buf; uint ret; if(ctx->type == BITSTREAM_READ) { ret = bitstream_read (ctx, &buf, nb_bit); *(u16*)value = 0xffff & buf; return ret; } else { buf = *(u16*)value; return bitstream_write (ctx, &buf, nb_bit); } } uint bitstream_access_32 (bitstream_t *ctx, void *value, uint nb_bit) { dbg_assert (ctx); dbg_assert (nb_bit <= 32); u32 buf; uint ret; if(ctx->type == BITSTREAM_READ) { ret = bitstream_read (ctx, &buf, nb_bit); *(u32*)value = buf; return ret; } else { buf = *(u32*)value; return bitstream_write (ctx, &buf, nb_bit); } } uint bitstream_access_64 (bitstream_t *ctx, void *value, uint nb_bit) { dbg_assert (ctx); dbg_assert (nb_bit <= 64); u64 val; u32 buf = 0; uint ret = 0, x; if(ctx->type == BITSTREAM_READ) { for(val = 0; nb_bit; nb_bit -= x, ret += x) { x = MIN (bitsizeof(buf), nb_bit); bitstream_read (ctx, &buf, x); val |= (u64)buf << ret; } *(u64*)value = val; } else { for(val = *(u64*)value; nb_bit; nb_bit -= x, ret += x) { x = MIN (bitsizeof(buf), nb_bit); buf = (0xffffffff & (val >> ret)); bitstream_write (ctx, &buf, x); } } return ret; } /* Direct access ops */ uint bitstream_direct_read (void *data, uint bit_offset, uint nb_bit) { u32 *stream; uint ret = 0; u32 buf, mask, x, y = 0; /* ensure 32-bit alignment */ data = (u32*)data + (bit_offset >> 5); stream = (u32*)((u8*)data - ((u32)data & 0x03)); bit_offset = ((u32)data & 0x03) * 8 + (bit_offset & 0x1f); dbg_assert (nb_bit <= 32); for(; nb_bit; nb_bit -= x, y += x, bit_offset += x) { /* bit_offset mod bitsizeof (buf) */ bit_offset &= (bitsizeof (buf) - 1); /* direct access to buf */ buf = *stream++; /* x is bits_left_in_local_buf or nb_bit */ x = MIN ((bitsizeof(buf) - bit_offset), nb_bit); /* create bitmask */ mask = bitstream_rol32 (1ul, x) - 1; /* set return value */ ret |= ((((buf >> bit_offset) & mask) << y)); } return ret; } u64 bitstream_direct_read_large (u8 *data, uint bit_offset, uint nb_bit) { u64 ret; u32 buf, x, y = 0; dbg_assert (nb_bit <= 64); for(ret = 0; nb_bit; nb_bit -= x, bit_offset += x, y += x) { x = MIN (bitsizeof(buf), nb_bit); buf = bitstream_direct_read (data, bit_offset, x); ret |= (u64)buf << y; } return ret; } void bitstream_direct_write (void *data, uint bit_offset, uint value, uint nb_bit) { u32 *stream; u32 buf, mask, x, y = 0; dbg_assert (nb_bit <= 32); /* ensure 32-bit alignment */ data = (u32*)data + (bit_offset >> 5); stream = (u32*)((u8*)data - ((u32)data & 0x03)); bit_offset = ((u32)data & 0x03) * 8 + (bit_offset & 0x1f); for(buf = 0; nb_bit; nb_bit -= x, y += x, bit_offset += x) { /* bit_offset mod bitsizeof (buf) */ bit_offset &= (bitsizeof (buf) - 1); /* x is bits_left_in_local_buf or nb_bit */ x = MIN ((bitsizeof(buf) - bit_offset), nb_bit); /* create bitmask */ mask = bitstream_rol32 (1ul, x) - 1; /* fill local buf */ buf |= (((value >> y) & mask) << bit_offset); /* direct access to stream */ *stream = (*stream & ~(mask << bit_offset)) | buf; stream++; buf = 0; } } void bitstream_direct_write_large (u8 *data, uint bit_offset, u64 value, uint nb_bit) { u32 buf, x, y = 0; dbg_assert (nb_bit <= 64); for(buf = 0; nb_bit; nb_bit -= x, bit_offset += x, y += x) { x = MIN (bitsizeof(buf), nb_bit); buf = 0xffffffff & (value >> y); bitstream_direct_write (data, bit_offset, buf, x); } } void* bitstream_memcpy (void *dest, void *src, size_t len) { u32 tmp, x = 0; bitstream_t ctx_r, ctx_w; bitstream_init (&ctx_r, src, len, BITSTREAM_READ); bitstream_init (&ctx_w, dest, len, BITSTREAM_WRITE); for(len *= 8; len; len -= x) { x = MIN (bitsizeof (u32), len); bitstream_access (&ctx_r, &tmp, x); bitstream_access (&ctx_w, &tmp, x); } bitstream_finalise (&ctx_r); bitstream_finalise (&ctx_w); return dest; }