vcs.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/VLIB/UVLC/uvlc_codec.c

   1 #include <VLIB/Platform/video_utils.h>
   2 #include <VLIB/Platform/video_config.h>
   3
   4 #include <VLIB/video_quantizer.h>
   5 #include <VLIB/video_dct.h>
   6 #include <VLIB/video_packetizer.h>
   7 #include "uvlc_codec.h"
   8
   9 #include <VP_Os/vp_os_malloc.h>
  10 #include <VP_Os/vp_os_assert.h>
  11 #include <VP_Os/vp_os_types.h>
  12
  13 const uvlc_codec_t uvlc_codec = {
  14   uvlc_encode_blockline,
  15   uvlc_decode_blockline,
  16   uvlc_update,
  17   uvlc_cache,
  18   { 0 }
  19 };
  20
  21 void uvlc_codec_alloc( video_controller_t* controller )
  22 {
  23   video_codec_t* video_codec;
  24
  25   video_codec = (video_codec_t*) vp_os_malloc( sizeof(uvlc_codec) );
  26
  27   vp_os_memcpy(video_codec, &uvlc_codec, sizeof(uvlc_codec));
  28
  29   controller->video_codec = video_codec;
  30 }
  31
  32 void uvlc_codec_free( video_controller_t* controller )
  33 {
  34   uvlc_codec_t* uvlc_codec = (uvlc_codec_t*) controller->video_codec;
  35
  36   if( uvlc_codec != NULL )
  37   {
  38     vp_os_free( uvlc_codec );
  39     controller->video_codec = NULL;
  40   }
  41 }
  42
  43 static C_RESULT uvlc_flush_stream( video_stream_t* out, video_stream_t* in )
  44 {
  45   // They are still data in cache
  46   // Always copy a number of bytes that is a times of 4.
  47   // Only for the last copy, we can have exactly the number of bytes left
  48   int32_t offset, size;
  49   uint32_t out_stream_size;
  50
  51   if( in->length != 32 )
  52   {
  53     // flush & reset internal stream
  54     video_write_data( in, 0, in->length+1 );
  55     in->length = 32;
  56   }
  57
  58   out_stream_size = out->size & ~3; // Round to the highest times of 4 available
  59
  60   offset = in->index - (in->used >> 2);
  61   size = ( in->used < out_stream_size ) ? in->used : out_stream_size;
  62
  63   vp_os_memcpy(out->bytes, in->bytes + offset, size);
  64
  65   out->index  = size >> 2;
  66   out->used   = size;
  67
  68   in->used -= size;
  69
  70   return C_OK;
  71 }
  72
  73 static C_RESULT uvlc_load_stream( video_stream_t* out, video_stream_t* in )
  74 {
  75   // We cache as many blockline as possible
  76   C_RESULT res;
  77   bool_t found, last_zero, last_zero_temp;
  78   uint8_t *dst, *src;
  79
  80   int32_t value, nb_bytes;
  81   uint32_t in_index = (in->used >> 2) - 1;
  82
  83   // -> start looking for last blockline's end
  84   found = FALSE;
  85
  86   if( in->index == 0 ) // First call, we look for full blocklines
  87   {
  88     last_zero = FALSE;
  89
  90     while( (in_index > in->index) && !found )
  91     {
  92       value = in->bytes[in_index];
  93
  94       last_zero_temp = (value & 0xFF) == 0; // 0x??????00
  95       found = last_zero_temp & last_zero;
  96
  97       if( !found )
  98       {
  99         last_zero = last_zero_temp;
 100         value >>= 8;
 101
 102         last_zero_temp = (value & 0xFF) == 0; // 0x????00??
 103         found = last_zero_temp & last_zero;
 104
 105         if( !found )
 106         {
 107           last_zero = last_zero_temp;
 108           value >>= 8;
 109
 110           last_zero_temp = (value & 0xFF) == 0; // 0x??00????
 111           found = last_zero_temp & last_zero;
 112
 113           if( !found )
 114           {
 115             in_index--; // Handle both the special case where blockline is dword aligned &
 116                         // blockline start is still not found
 117
 118             last_zero = last_zero_temp;
 119             value >>= 8;
 120
 121             last_zero_temp = (value & 0xFF) == 0; // 0x00??????
 122             found = last_zero_temp & last_zero;
 123
 124             if( !found )
 125             {
 126               last_zero = last_zero_temp;
 127             }
 128           }
 129         }
 130       }
 131     }
 132   }
 133
 134   in_index++;
 135
 136   // configure parameters for memcpy
 137   if( !found )
 138   {
 139     // cache all data
 140     nb_bytes = in->used - in->index * 4;
 141
 142     res = C_FAIL;
 143   }
 144   else
 145   {
 146     // cache only data containing full blocklines
 147     nb_bytes = (in_index - in->index) * 4;
 148
 149     res = C_OK;
 150   }
 151
 152   // Realloc internal stream to have enough space to hold all required data
 153   while( out->used + nb_bytes >= out->size )
 154   {
 155     out->bytes = vp_os_realloc( out->bytes, out->size + 2048 ); // Add 2KB to internal stream
 156     out->size += 2048;
 157   }
 158
 159   dst   = (uint8_t*)&out->bytes[0];
 160   dst  += out->used;
 161
 162   src   = (uint8_t*)&in->bytes[0];
 163   src  += in->index*4;
 164
 165   vp_os_memcpy( dst, src, nb_bytes );
 166
 167   out->used += nb_bytes;
 168   in->index  = in_index;
 169
 170   VP_OS_ASSERT( out->used <= out->size );
 171
 172   return res;
 173 }
 174
 175 C_RESULT uvlc_pack_controller( video_controller_t* controller )
 176 {
 177   video_stream_t* stream = &controller->in_stream;
 178   uvlc_codec_t* uvlc_codec = (uvlc_codec_t*) controller->video_codec;
 179   uvlc_picture_layer_t* picture_layer;
 180   uvlc_gob_layer_t* gob;
 181
 182   gob = NULL;
 183   picture_layer = &uvlc_codec->picture_layer;
 184
 185   video_stuff8( stream );
 186
 187   picture_layer->gobs = (uvlc_gob_layer_t*) controller->gobs;
 188   gob = &picture_layer->gobs[controller->blockline];
 189
 190   video_write_data( stream, MAKE_START_CODE(controller->blockline), 22 );
 191
 192   if( controller->blockline == 0 )
 193   {
 194     picture_layer->quant = gob->quant;
 195     uvlc_write_picture_layer( controller, stream );
 196   }
 197   else
 198   {
 199     uvlc_write_gob_layer( stream, gob );
 200   }
 201
 202   return C_OK;
 203 }
 204
 205 C_RESULT uvlc_unpack_controller( video_controller_t* controller )
 206 {
 207   uint32_t start_code = 0;
 208   video_stream_t* stream = &controller->in_stream;
 209   uvlc_codec_t* uvlc_codec = (uvlc_codec_t*) controller->video_codec;
 210   uvlc_picture_layer_t* picture_layer;
 211   uvlc_gob_layer_t* gob;
 212
 213   gob = NULL;
 214   picture_layer = &uvlc_codec->picture_layer;
 215
 216   video_align8( stream );
 217   video_read_data( stream, &start_code, 22 );
 218
 219   controller->blockline = start_code & 0x1F;
 220   start_code &= ~0x1F; // TODO Check if compiler use arm instruction bic
 221
 222   VP_OS_ASSERT( controller->blockline == 0x1F ||
 223                 controller->num_blockline == 0 || // Check if cache is allocated for current picture
 224                 (controller->num_blockline > 0 && controller->blockline < controller->num_blockline) );
 225
 226   if( start_code == PICTURE_START_CODE )
 227   {
 228     if( controller->blockline == 0x1F )
 229     {
 230       controller->picture_complete = TRUE;
 231     }
 232     else
 233     {
 234       if( controller->blockline == 0 )
 235       {
 236         uvlc_read_picture_layer( controller, stream );
 237
 238         picture_layer->gobs = (uvlc_gob_layer_t*) controller->gobs;
 239         gob = &picture_layer->gobs[controller->blockline];
 240
 241         gob->quant = picture_layer->quant;
 242       }
 243       else
 244       {
 245         picture_layer->gobs = (uvlc_gob_layer_t*) controller->gobs;
 246         gob = &picture_layer->gobs[controller->blockline];
 247
 248         uvlc_read_gob_layer( stream, gob );
 249       }
 250     }
 251   }
 252
 253   return C_OK;
 254 }
 255
 256 C_RESULT uvlc_encode_blockline( video_controller_t* controller, const vp_api_picture_t* blockline, bool_t picture_complete )
 257 {
 258   video_codec_t* video_codec;
 259   int16_t *in = NULL, *out = NULL;
 260   int32_t num_macro_blocks = 0;
 261   video_macroblock_t* macroblock = NULL;
 262   video_picture_context_t blockline_ctx;
 263   video_gob_t*  gobs;
 264   static uint32_t mean_Q = 2;
 265
 266   video_stream_t* stream = &controller->in_stream;
 267
 268   if( stream->used*2 >= stream->size )
 269   {
 270         uint32_t add = 32 - clz(stream->used/controller->blockline);        // estimate the log2 size of a blockline in the stream
 271         add = 1<<(add+1);                                                                                                   // major and compute addition buffer size
 272     stream->bytes = vp_os_realloc( stream->bytes, stream->size + add ); // Add some byte to internal stream
 273     stream->size += add;
 274   }
 275
 276   video_codec                   = controller->video_codec;
 277   controller->picture_complete  = picture_complete;
 278   controller->blockline         = blockline->blockline;
 279
 280   blockline_ctx.y_src     = blockline->y_buf;
 281   blockline_ctx.cb_src    = blockline->cb_buf;
 282   blockline_ctx.cr_src    = blockline->cr_buf;
 283   blockline_ctx.y_woffset = blockline->y_line_size;
 284   blockline_ctx.c_woffset = blockline->cb_line_size;
 285   blockline_ctx.y_hoffset = blockline->y_line_size * MCU_HEIGHT;
 286
 287   gobs        = &controller->gobs[controller->blockline];
 288   gobs->quant = controller->quant;
 289   macroblock  = &gobs->macroblocks[0];
 290
 291   uvlc_pack_controller( controller );
 292
 293   in  = controller->blockline_cache;
 294   out = macroblock->data;
 295
 296   num_macro_blocks = controller->mb_blockline;
 297
 298   ///> Cache blockline in dct format & perform dct
 299   while( num_macro_blocks > MAX_NUM_MACRO_BLOCKS_PER_CALL )
 300   {
 301     RTMON_USTART(VIDEO_VLIB_BLOCKLINE_TO_MB);
 302     video_blockline_to_macro_blocks(&blockline_ctx, in, MAX_NUM_MACRO_BLOCKS_PER_CALL);
 303     RTMON_USTOP(VIDEO_VLIB_BLOCKLINE_TO_MB);
 304
 305
 306 #ifdef HAS_FDCT_QUANT_COMPUTE
 307     out = video_fdct_quant_compute(in, out, MAX_NUM_MACRO_BLOCKS_PER_CALL,gobs->quant);
 308 #else
 309     out = video_fdct_compute(in, out, MAX_NUM_MACRO_BLOCKS_PER_CALL);
 310 #endif
 311     if( in == controller->blockline_cache )
 312       in += DCT_BUFFER_SIZE;
 313     else
 314       in -= DCT_BUFFER_SIZE;
 315
 316     num_macro_blocks -= MAX_NUM_MACRO_BLOCKS_PER_CALL;
 317   }
 318
 319   RTMON_USTART(VIDEO_VLIB_BLOCKLINE_TO_MB);
 320   video_blockline_to_macro_blocks(&blockline_ctx, in, num_macro_blocks);
 321   RTMON_USTOP(VIDEO_VLIB_BLOCKLINE_TO_MB);
 322
 323   RTMON_USTOP(VIDEO_VLIB_BLOCKLINE_TO_MB);
 324
 325 #ifdef HAS_FDCT_QUANT_COMPUTE
 326   video_fdct_quant_compute(in, out, num_macro_blocks,gobs->quant);
 327 #else
 328   video_fdct_compute(in, out, num_macro_blocks);
 329 #endif
 330   ///<
 331
 332   ///> Do quantification on each macroblock
 333   RTMON_USTART(VIDEO_VLIB_QUANTIZE);
 334   video_quantize( controller, &controller->gobs[controller->blockline].macroblocks[0], controller->mb_blockline );
 335   RTMON_USTOP(VIDEO_VLIB_QUANTIZE);
 336   ///<
 337
 338   ///> Packetize Data to output buffer
 339   RTMON_USTART(VIDEO_VLIB_PACKET);
 340   uvlc_write_mb_layer( stream, macroblock, controller->mb_blockline );
 341
 342   ///> Control Stream size
 343
 344   if (controller->target_size > 0)
 345   {
 346     if ((controller->blockline+1) == controller->num_blockline)
 347     {
 348       if ((controller->target_size > stream->used) && mean_Q > 2)
 349       {
 350         // last frame was too small increase overall quality (i.e. decrease quant)
 351         mean_Q--;
 352       }
 353       else if ((controller->target_size < stream->used) && mean_Q < 31)
 354       {
 355         // last frame was too large decrease overall quality (i.e. increase quant)
 356         mean_Q++;
 357       }
 358         }
 359
 360     if (stream->used > ((controller->blockline+1)*controller->target_size*1.05/controller->num_blockline))
 361     {
 362       // stream too large, reduce quality
 363       controller->quant = mean_Q + 1;
 364     }
 365     else if (stream->used < ((controller->blockline+1)*controller->target_size*0.95/controller->num_blockline))
 366     {
 367       // stream too low, increase quality
 368       controller->quant = mean_Q - 1;
 369     }
 370     else
 371       controller->quant = mean_Q;
 372
 373     // quant saturation, 31 is reserved for old TABLE_QUANTIZATION mode (backward compatibility)
 374     // TODO: quant == 1 doesn't work, find why
 375     if (controller->quant > 30)
 376       controller->quant=30;
 377     else if (controller->quant < 2)
 378             controller->quant=2;
 379   }
 380   else
 381           controller->quant= DEFAULT_QUANTIZATION;
 382   RTMON_USTOP(VIDEO_VLIB_PACKET);
 383   ///<
 384
 385   if( controller->picture_complete )
 386   {
 387     video_stuff8( stream );
 388     video_write_data( stream, PICTURE_END_CODE, 22 );
 389   }
 390
 391   // Update controller according to user inputs & video statistics
 392   video_controller_update( controller, picture_complete );
 393
 394   return C_OK;
 395 }
 396
 397 #ifndef HAS_UVLC_DECODE_BLOCKLINE
 398 C_RESULT uvlc_decode_blockline( video_controller_t* controller, vp_api_picture_t* picture, bool_t* got_image )
 399 {
 400   video_codec_t* video_codec;
 401   vp_api_picture_t blockline = { 0 };
 402   int16_t *in = NULL, *out = NULL;
 403   int32_t num_macro_blocks = 0;
 404   video_macroblock_t* macroblock = NULL;
 405   video_picture_context_t blockline_ctx;
 406   video_gob_t*  gobs;
 407
 408   controller->mode  = VIDEO_DECODE;
 409   video_codec       = controller->video_codec;
 410
 411   blockline                   = *picture;
 412   blockline.height            = MB_HEIGHT_Y;
 413   blockline.complete          = 1;
 414   blockline.vision_complete   = 0;
 415
 416   picture->complete  = controller->picture_complete;
 417
 418   blockline_ctx.y_woffset = blockline.y_line_size;
 419   blockline_ctx.c_woffset = blockline.cb_line_size;
 420   blockline_ctx.y_hoffset = blockline.y_line_size * MCU_HEIGHT;
 421
 422   // At least a complete blockline is found
 423   while( !controller->picture_complete && controller->in_stream.index <= (controller->in_stream.used >> 2) )
 424   {
 425     uvlc_unpack_controller( controller );
 426
 427     if( !controller->picture_complete )
 428     {
 429       blockline.blockline  = controller->blockline;
 430
 431       blockline_ctx.y_src     = picture->y_buf + blockline.blockline * MB_HEIGHT_Y * picture->y_line_size;
 432       blockline_ctx.cb_src    = picture->cb_buf + blockline.blockline * MB_HEIGHT_C * picture->cb_line_size;
 433       blockline_ctx.cr_src    = picture->cr_buf + blockline.blockline * MB_HEIGHT_C * picture->cr_line_size;
 434
 435       picture->blockline  = controller->blockline;
 436       num_macro_blocks    = controller->mb_blockline;
 437
 438       macroblock  = &controller->cache_mbs[0];
 439       gobs        = &controller->gobs[controller->blockline];
 440       out         = gobs->macroblocks->data;
 441
 442       if( gobs->quant != controller->quant )
 443       {
 444         controller->quant = gobs->quant;
 445         video_quantizer_update( controller );
 446       }
 447
 448       while( num_macro_blocks > MAX_NUM_MACRO_BLOCKS_PER_CALL )
 449       {
 450         in = &macroblock->data[0];
 451
 452         uvlc_read_mb_layer( &controller->in_stream, macroblock, MAX_NUM_MACRO_BLOCKS_PER_CALL );
 453
 454         video_unquantize( controller, macroblock, MAX_NUM_MACRO_BLOCKS_PER_CALL );
 455
 456         out = video_idct_compute( in, out, MAX_NUM_MACRO_BLOCKS_PER_CALL );
 457
 458         if( macroblock == &controller->cache_mbs[0] )
 459           macroblock += MAX_NUM_MACRO_BLOCKS_PER_CALL;
 460         else
 461           macroblock -= MAX_NUM_MACRO_BLOCKS_PER_CALL;
 462
 463         num_macro_blocks -= MAX_NUM_MACRO_BLOCKS_PER_CALL;
 464       }
 465
 466       in = macroblock->data;
 467
 468       uvlc_read_mb_layer( &controller->in_stream, macroblock, num_macro_blocks );
 469
 470       video_unquantize( controller, macroblock, num_macro_blocks );
 471
 472       video_idct_compute( in, out, num_macro_blocks );
 473
 474       video_blockline_from_macro_blocks(&blockline_ctx, gobs->macroblocks->data, controller->mb_blockline, picture->format);
 475
 476       // Update controller according to video statistics
 477       video_controller_update( controller, controller->picture_complete );
 478     }
 479   }
 480
 481   if( controller->picture_complete )
 482   {
 483     picture->complete   = controller->picture_complete;
 484     picture->blockline  = 0;
 485
 486     controller->picture_complete  = 0;
 487     controller->in_stream.length  = 32;
 488     controller->num_frames++;
 489
 490     *got_image = TRUE;
 491   }
 492   else
 493   {
 494     controller->in_stream.used  = 0;
 495     controller->in_stream.index = 0;
 496   }
 497
 498   return C_OK;
 499 }
 500 #endif
 501
 502 C_RESULT uvlc_update( video_controller_t* controller )
 503 {
 504   return C_OK;
 505 }
 506
 507 C_RESULT uvlc_cache( video_controller_t* controller, video_stream_t* ex_stream)
 508 {
 509   C_RESULT res;
 510
 511   video_stream_t* in_stream = &controller->in_stream;
 512
 513   switch( controller->mode )
 514   {
 515   case VIDEO_ENCODE:
 516     res = uvlc_flush_stream( ex_stream, in_stream );
 517     break;
 518
 519   case VIDEO_DECODE:
 520     res = uvlc_load_stream( in_stream, ex_stream );
 521     break;
 522
 523   default:
 524     res = C_FAIL;
 525     break;
 526   }
 527
 528   return res;
 529 }