git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/VLIB/P264/p264.c

   1 #include <VLIB/Platform/video_utils.h>
   2 #include <VLIB/video_packetizer.h>
   3
   4 #include <VP_Os/vp_os_assert.h>
   5
   6 #include <VP_Os/vp_os_types.h>
   7
   8 #include <VLIB/P264/p264.h>
   9
  10 #ifndef HAS_P264_ENCODE
  11 //TODO: rename this file and this function (something like p264_entropic.c)
  12
  13 #define PACK_BITS( bits_out, length_out, bits_in, length_in ) \
  14   bits_out  <<= length_in;                                    \
  15   length_out += length_in;                                    \
  16   bits_out   |= bits_in;
  17
  18 void p264_encode_int(video_stream_t* const stream, int32_t code)
  19 {
  20   int32_t sign, length, data, value_code, value_length;
  21
  22   value_code = 0;
  23   value_length = 0;
  24   /// Encode level
  25   data = code;
  26
  27   if (data != 0)
  28   {
  29     // sign handling
  30     sign = 0;
  31     if( data < 0 )
  32     {
  33       data = -data;
  34       sign = 1;
  35     }
  36
  37     // TODO Check saturation & if level == -128
  38     length = 32 - clz(data);  // number of bits used in level ( = length of level )
  39     if( length > 1 )
  40     {
  41       data   -= 1 << (length - 1);
  42       length += 1;
  43     }
  44
  45     PACK_BITS( value_code, value_length, 1, length );
  46
  47     length -= 2;
  48     if(length > 0)
  49     {
  50       PACK_BITS( value_code, value_length, data, length );
  51     }
  52
  53     PACK_BITS( value_code, value_length, sign, 1 );
  54
  55     /// Write output
  56     video_write_data( stream, value_code, value_length );
  57   }
  58   else
  59   {
  60      video_write_data( stream, 0x01, 2 );
  61   }
  62 }
  63
  64 void p264_decode_int(video_stream_t* const stream, int32_t *code)
  65 {
  66   uint32_t stream_code, stream_length;
  67   int32_t r = 0, z, sign;
  68
  69   stream_code = stream_length = 0;
  70
  71   // Peek 32 bits from stream because we know our datas fit in
  72   video_peek_data( stream, &stream_code, 32 );
  73
  74   /// Decode level / last
  75   z = clz(stream_code);
  76
  77   stream_code  <<= z + 1; // Skip all zeros & 1
  78   stream_length += z + 1;
  79
  80   if( z == 1 )
  81   {
  82     *code = 0;
  83   }
  84   else
  85   {
  86     if( z == 0 )
  87     {
  88       z = 1;
  89       r = 1;
  90     }
  91
  92     stream_length  += z;
  93
  94     stream_code >>= (32 - z);
  95     sign = stream_code & 1;
  96
  97     if( z != 0 )
  98     {
  99       r  = stream_code >> 1;
 100       r += 1 << (z-1);
 101     }
 102
 103     *code = sign ? -r : r;
 104   }
 105
 106   // Do the real Read in stream to consume what we used
 107   video_read_data( stream, &stream_code, stream_length );
 108
 109 }
 110
 111 void p264_encode( video_stream_t* const stream, int32_t level, int32_t run, int32_t not_last )
 112 {
 113   int32_t sign, length, data, value_code, value_length;
 114
 115   /// Encode number of zeros
 116   data = run;
 117
 118   length      = 0;
 119   value_code  = 1;
 120
 121   if( data > 0 )
 122   {
 123     length = 32 - clz(data);      // compute number of bits used in run ( = length of run )
 124     data  -= 1 << ( length - 1 ); // compute value of run
 125   }
 126
 127   value_length  = length + 1;
 128
 129   length -= 1;
 130   if( length > 0 )
 131   {
 132     PACK_BITS( value_code, value_length, data, length );
 133   }
 134
 135   /// Encode level
 136   data = level;
 137
 138   // sign handling
 139   sign = 0;
 140   if( data < 0 )
 141   {
 142     data = -data;
 143     sign = 1;
 144   }
 145
 146   // TODO Check saturation & if level == -128
 147   length = 32 - clz(data);  // number of bits used in level ( = length of level )
 148   if( length > 1 )
 149   {
 150     data   -= 1 << (length - 1);
 151     length += 1;
 152   }
 153
 154   PACK_BITS( value_code, value_length, 1, length );
 155
 156   VP_OS_ASSERT( length != 2 );
 157
 158   length -= 2;
 159   if(length > 0)
 160   {
 161     PACK_BITS( value_code, value_length, data, length );
 162   }
 163
 164   PACK_BITS( value_code, value_length, sign, 1 );
 165
 166   /// Encode last
 167   // add sequence for end of block if required
 168   if( not_last == 0 )
 169   {
 170     PACK_BITS( value_code, value_length, 0x5, 3 );
 171   }
 172
 173   /// Write output
 174   video_write_data( stream, value_code, value_length );
 175 }
 176
 177 #endif // HAS_UVLC_ENCODE
 178
 179 C_RESULT p264_decode( video_stream_t* const stream, int32_t* run, uint32_t* level, int32_t* last)
 180 {
 181   uint32_t stream_code, stream_length;
 182   int32_t r = 0, z, sign;
 183
 184   stream_code = stream_length = 0;
 185
 186   // Peek 32 bits from stream because we know our datas fit in
 187   video_peek_data( stream, &stream_code, 32 );
 188
 189
 190   /// Decode number of zeros
 191   z = clz(stream_code);
 192
 193   stream_code  <<= z + 1; // Skip all zeros & 1
 194   stream_length += z + 1;
 195
 196   if( z > 1 )
 197   {
 198     r = stream_code >> (32 - (z-1));
 199
 200     stream_code   <<= (z-1);
 201     stream_length  += (z-1);
 202
 203     *run = r + (1 << (z-1));
 204   }
 205   else
 206   {
 207     *run = z;
 208   }
 209
 210
 211   /// Decode level / last
 212   z = clz(stream_code);
 213
 214   stream_code  <<= z + 1; // Skip all zeros & 1
 215   stream_length += z + 1;
 216
 217   if( z == 1 )
 218   {
 219     *run  = 0;
 220     *last = 1;
 221   }
 222   else
 223   {
 224     if( z == 0 )
 225     {
 226       z = 1;
 227       r = 1;
 228     }
 229
 230     stream_length  += z;
 231
 232     stream_code >>= (32 - z);
 233     sign = stream_code & 1;
 234
 235     if( z != 0 )
 236     {
 237       r  = stream_code >> 1;
 238       r += 1 << (z-1);
 239     }
 240
 241     *level = sign ? -r : r;
 242     *last  = 0;
 243   }
 244
 245   // Do the real Read in stream to consume what we used
 246   video_read_data( stream, &stream_code, stream_length );
 247
 248   return C_OK;
 249 }