1. /*
  2.     Project  : H.264 encoder test    
  3.     Module   : Bitstream functions
  4.  
  5.     Author   : Marko 'Fador' Viitanen
  6.  
  7.     Date     : 9/5/2009
  8.     Modified : 10/5/2009
  9.  
  10. */
  11.  
  12. #include <stdio.h>
  13. #include <stdlib.h>
  14. #include <math.h>
  15. #include <stdarg.h>
  16.  
  17. #include "common.h"
  18. #include "bitstream.h"
  19.  
  20.  
  21. void printf_bitstream(char *msg, ...)
  22. {
  23.  
  24. va_list fmtargs;
  25. char buffer[1024];
  26.  
  27.   va_start(fmtargs,msg);
  28.   vsnprintf(buffer,sizeof(buffer)-1,msg,fmtargs);
  29.   va_end(fmtargs);
  30.   printf("%s",buffer);
  31.  
  32. }
  33.  
  34.  
  35. /*
  36.  * Clear bitstream
  37.  */
  38. void bitstream_init(bitstream* stream)
  39. {
  40.     stream->cur_byte=0;
  41.     stream->cur_bit=0;
  42.     memset(stream->data, 0, sizeof(uint32)*32);
  43.  
  44. }
  45.  
  46.  
  47. /*
  48.  * Put bits to bitstream
  49.  * Input:
  50.  *          stream = pointer bitstream to put the data
  51.  *          data   = pointer to actual data
  52.  *          bits   = number of bits to write      
  53.  */
  54.  
  55. void bitstream_put(bitstream* stream, uint32 data, uint8 bits)
  56. {
  57.     uint8 i=0;
  58.     uint32 bitsleft=32-stream->cur_bit;
  59.     printf_bitstream("put: ");
  60.     for(i=0;i<bits;i++)
  61.     {
  62.         printf("%i",(data&(1<<(bits-i-1)))?1:0);
  63.     }
  64.     printf_bitstream("\n");
  65.     //printf_bitstream(" count: %i\n",bits);
  66.  
  67.     //Theres space for all the bits
  68.     if(bits<=bitsleft)
  69.     {
  70.         stream->data[stream->cur_byte] |= (data<<((bitsleft-bits)));
  71.         stream->cur_bit+=bits;
  72.         bits=0;
  73.     }
  74.     //No space for everything, store the bits we can and continue later
  75.     else
  76.     {
  77.         stream->data[stream->cur_byte] |= (data>>(bits-bitsleft));
  78.         stream->cur_bit=32;
  79.         bits-=bitsleft;        
  80.     }
  81.  
  82.     //Check if the buffer is full
  83.     if(stream->cur_bit==32)
  84.     {
  85.         stream->cur_byte++;
  86.         bitsleft=32;
  87.         if(stream->cur_byte==32)
  88.         {
  89.             //Flush data out
  90.             bitstream_flush(stream);
  91.         }
  92.     }
  93.  
  94.     //..still some writing to do
  95.     if(bits!=0)
  96.     {
  97.         stream->data[stream->cur_byte] |= (data<<(bitsleft-bits));
  98.         stream->cur_bit+=bits;
  99.     }
  100.  
  101.  
  102.  
  103. }
  104.  
  105. /*
  106.  *  Align the bitstream
  107.  */
  108. void bitstream_align(bitstream* stream)
  109. {  
  110.     if(stream->cur_byte==32)
  111.     {
  112.         //Stream flushed, zero out the values
  113.         bitstream_init(stream);
  114.     }
  115.     else
  116.     {
  117.         stream->cur_byte++;
  118.     }
  119. }
  120.  
  121. void bitstream_flush(bitstream* stream)
  122. {
  123.    
  124.     /*
  125.      *  SAVE DATA TO OUTPUT
  126.      */
  127.  
  128.     //Stream flushed, zero out the values
  129.     bitstream_init(stream);
  130. }
  131.