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

   1 #include <VP_Os/vp_os_print.h>
   2
   3 #include <VLIB/Platform/video_utils.h>
   4 #include <VLIB/video_picture.h>
   5 #include <VP_Os/vp_os_malloc.h>
   6
   7 #ifndef HAS_VIDEO_BLOCKLINE_TO_MACRO_BLOCKS
   8
   9 // Convert a 8x8 block of 8 bits data to a 8x8 block of 16 bits data
  10 static void copy_block_8_16(int16_t* dst, int32_t dst_offset, uint8_t* src, int32_t src_offset)
  11 {
  12   uint32_t* src32 = (uint32_t*) src;
  13   uint32_t* dst32 = (uint32_t*) dst;
  14
  15   uint32_t  src_offset32 = src_offset >> 2;
  16   uint32_t  dst_offset32 = dst_offset >> 1;
  17
  18   uint32_t  temp;
  19
  20   int32_t i;
  21
  22   for( i = 0; i < MCU_BLOCK_SIZE; i += MCU_WIDTH, src32 += src_offset32, dst32 += dst_offset32 )
  23   {
  24     temp = *src32++;
  25
  26     *dst32++ = ((temp << 8) & 0x00FF0000) | (temp & 0x000000FF);
  27     *dst32++ = ((temp >> 8) & 0x00FF0000) | ((temp >> 16) & 0x000000FF);
  28
  29     temp = *src32++;
  30
  31     *dst32++ = ((temp << 8) & 0x00FF0000) | (temp & 0x000000FF);
  32     *dst32++ = ((temp >> 8) & 0x00FF0000) | ((temp >> 16) & 0x000000FF);
  33   }
  34
  35 }
  36
  37 #endif
  38
  39 // Convert a 8x8 block of 16 bits data to a 8x8 block of 8 bits data
  40 static void copy_block_16_8(uint8_t* dst, int32_t dst_offset, int16_t* src, int32_t src_offset)
  41 {
  42   int32_t i;
  43   int16_t temp;
  44
  45   for( i = 0; i < MCU_BLOCK_SIZE; i += MCU_WIDTH, dst += dst_offset, src += src_offset )
  46   {
  47     temp = *src++; if( temp > 0xff ) temp = 0xff; if(temp < 0) temp = 0; temp &= 0xff; *dst++ = (uint8_t) temp;
  48     temp = *src++; if( temp > 0xff ) temp = 0xff; if(temp < 0) temp = 0; temp &= 0xff; *dst++ = (uint8_t) temp;
  49     temp = *src++; if( temp > 0xff ) temp = 0xff; if(temp < 0) temp = 0; temp &= 0xff; *dst++ = (uint8_t) temp;
  50     temp = *src++; if( temp > 0xff ) temp = 0xff; if(temp < 0) temp = 0; temp &= 0xff; *dst++ = (uint8_t) temp;
  51     temp = *src++; if( temp > 0xff ) temp = 0xff; if(temp < 0) temp = 0; temp &= 0xff; *dst++ = (uint8_t) temp;
  52     temp = *src++; if( temp > 0xff ) temp = 0xff; if(temp < 0) temp = 0; temp &= 0xff; *dst++ = (uint8_t) temp;
  53     temp = *src++; if( temp > 0xff ) temp = 0xff; if(temp < 0) temp = 0; temp &= 0xff; *dst++ = (uint8_t) temp;
  54     temp = *src++; if( temp > 0xff ) temp = 0xff; if(temp < 0) temp = 0; temp &= 0xff; *dst++ = (uint8_t) temp;
  55   }
  56 }
  57
  58 //
  59 // Transform blockline in macro blocks
  60 //
  61 // Blockline:
  62 //  _______
  63 // | 1 | 2 |
  64 // |___|___|  Y
  65 // | 3 | 4 |
  66 // |___|___|
  67 //  ___
  68 // | 5 |
  69 // |___| Cb
  70 //  ___
  71 // | 6 |
  72 // |___| Cr
  73 //
  74 // Dct cache:
  75 //  _______________________
  76 // | 1 | 2 | 3 | 4 | 5 | 6 | ...
  77 // |___|___|___|___|___|___|
  78 //
  79
  80 #ifndef HAS_VIDEO_BLOCKLINE_TO_MACRO_BLOCKS
  81
  82 C_RESULT video_blockline_to_macro_blocks(video_picture_context_t* ctx, int16_t* dst, int32_t num_macro_blocks)
  83 {
  84   uint8_t* y_src        = ctx->y_src;
  85   uint8_t* cb_src       = ctx->cb_src;
  86   uint8_t* cr_src       = ctx->cr_src;
  87
  88   while( num_macro_blocks > 0 )
  89   {
  90     // Current MB
  91     copy_block_8_16( dst,
  92                      0,
  93                      y_src,
  94                      ctx->y_woffset - MCU_WIDTH );
  95
  96     dst += MCU_BLOCK_SIZE; // skip block 1
  97
  98     copy_block_8_16( dst,
  99                      0,
 100                      y_src + MCU_WIDTH,
 101                      ctx->y_woffset - MCU_WIDTH );
 102
 103     dst += MCU_BLOCK_SIZE; // skip block 2
 104
 105     copy_block_8_16( dst,
 106                      0,
 107                      y_src + ctx->y_hoffset,
 108                      ctx->y_woffset - MCU_WIDTH );
 109
 110     dst += MCU_BLOCK_SIZE; // skip block 3
 111
 112     copy_block_8_16( dst,
 113                      0,
 114                      y_src + ctx->y_hoffset + MCU_WIDTH,
 115                      ctx->y_woffset - MCU_WIDTH );
 116
 117     dst += MCU_BLOCK_SIZE; // skip block 4
 118
 119     copy_block_8_16( dst,
 120                      0,
 121                      cb_src,
 122                      ctx->c_woffset - MCU_WIDTH );
 123
 124     dst += MCU_BLOCK_SIZE;  // skip blocks 5
 125
 126     copy_block_8_16( dst,
 127                      0,
 128                      cr_src,
 129                      ctx->c_woffset - MCU_WIDTH );
 130
 131     dst += MCU_BLOCK_SIZE;  // skip blocks 6
 132
 133     y_src   += MCU_WIDTH*2;
 134     cb_src  += MCU_WIDTH;
 135     cr_src  += MCU_WIDTH;
 136
 137     num_macro_blocks --;
 138   }
 139
 140   ctx->y_src  = y_src;
 141   ctx->cb_src = cb_src;
 142   ctx->cr_src = cr_src;
 143
 144   return C_OK;
 145 }
 146
 147 #endif
 148
 149 // Transform macro blocks in picture of specified format
 150 static C_RESULT video_blockline_from_macro_blocks_yuv420(video_picture_context_t* ctx, int16_t* src, int32_t num_macro_blocks);
 151 static C_RESULT video_blockline_from_macro_blocks_rgb565(video_picture_context_t* ctx, int16_t* src, int32_t num_macro_blocks);
 152
 153 C_RESULT video_blockline_from_macro_blocks(video_picture_context_t* ctx, int16_t* src, int32_t num_macro_blocks, enum PixelFormat format)
 154 {
 155   C_RESULT res;
 156
 157   switch(format)
 158   {
 159     case PIX_FMT_YUV420P:
 160       res = video_blockline_from_macro_blocks_yuv420(ctx, src, num_macro_blocks);
 161       break;
 162     case PIX_FMT_RGB565:
 163       res = video_blockline_from_macro_blocks_rgb565(ctx, src, num_macro_blocks);
 164       break;
 165
 166     default:
 167       PRINT("In file %s, in function %s, format %d not supported\n", __FILE__, __FUNCTION__, format);
 168       res = C_FAIL;
 169       break;
 170   }
 171
 172   return res;
 173 }
 174
 175 C_RESULT video_blockline_from_macro_blocks_yuv420(video_picture_context_t* ctx, int16_t* src, int32_t num_macro_blocks)
 176 {
 177   uint8_t *y_dst, *cb_dst, *cr_dst;
 178
 179   y_dst   = ctx->y_src;
 180   cb_dst  = ctx->cb_src;
 181   cr_dst  = ctx->cr_src;
 182
 183   while( num_macro_blocks > 0 )
 184   {
 185     // Current MB
 186     copy_block_16_8( y_dst,
 187                      ctx->y_woffset - MCU_WIDTH,
 188                      src,
 189                      0 );
 190
 191     src += MCU_BLOCK_SIZE;
 192
 193     copy_block_16_8( y_dst + MCU_WIDTH,
 194                      ctx->y_woffset - MCU_WIDTH,
 195                      src,
 196                      0 );
 197
 198     src += MCU_BLOCK_SIZE;
 199
 200     copy_block_16_8( y_dst + ctx->y_hoffset,
 201                      ctx->y_woffset - MCU_WIDTH,
 202                      src,
 203                      0 );
 204
 205     src += MCU_BLOCK_SIZE;
 206
 207     copy_block_16_8( y_dst + ctx->y_hoffset + MCU_WIDTH,
 208                      ctx->y_woffset - MCU_WIDTH,
 209                      src,
 210                      0 );
 211
 212     src += MCU_BLOCK_SIZE;
 213
 214     copy_block_16_8( cb_dst,
 215                      ctx->c_woffset - MCU_WIDTH,
 216                      src,
 217                      0 );
 218
 219     src += MCU_BLOCK_SIZE;
 220
 221     copy_block_16_8( cr_dst,
 222                      ctx->c_woffset - MCU_WIDTH,
 223                      src,
 224                      0 );
 225
 226     src += MCU_BLOCK_SIZE;
 227
 228     y_dst   += MCU_WIDTH*2;
 229     cb_dst  += MCU_WIDTH;
 230     cr_dst  += MCU_WIDTH;
 231
 232     num_macro_blocks--;
 233   }
 234
 235   ctx->y_src  = y_dst;
 236   ctx->cb_src = cb_dst;
 237   ctx->cr_src = cr_dst;
 238
 239   return C_OK;
 240 }
 241
 242 #define MAKE_RGBA_565(r, g, b) ( ((r) << 11) | ((g) << 5) | (b) )
 243
 244 #if TARGET_CPU_ARM == 1 && defined(TARGET_OS_IPHONE)
 245 static inline int32_t saturate8(int32_t x)
 246 {
 247         usat(x, 7, 8);
 248
 249         return x;
 250 }
 251
 252 static inline uint32_t saturate5(int32_t x)
 253 {
 254         usat(x, 5, 11);
 255
 256         return x;
 257 }
 258
 259 static inline uint32_t saturate6(int32_t x)
 260 {
 261         usat(x, 6, 10);
 262
 263         return x;
 264 }
 265
 266 #else
 267
 268 // To make sure that you are bounding your inputs in the range of 0 & 255
 269
 270 static inline int32_t saturate8(int32_t x)
 271 {
 272   if( x < 0 )
 273   {
 274     x = 0;
 275   }
 276
 277   x >>= 8;
 278
 279   return x > 0xFF ? 0xFF : x;
 280 }
 281
 282 static inline uint16_t saturate5(int32_t x)
 283 {
 284   if( x < 0 )
 285   {
 286     x = 0;
 287   }
 288
 289   x >>= 11;
 290
 291   return x > 0x1F ? 0x1F : x;
 292 }
 293
 294 static inline uint16_t saturate6(int32_t x)
 295 {
 296   if( x < 0 )
 297   {
 298     x = 0;
 299   }
 300
 301   x >>= 10;
 302
 303   return x > 0x3F ? 0x3F : x;
 304 }
 305 #endif
 306
 307
 308 static C_RESULT video_blockline_from_macro_blocks_rgb565(video_picture_context_t* ctx, int16_t* src, int32_t num_macro_blocks)
 309 {
 310   uint32_t y_up_read, y_down_read, cr_current, cb_current;
 311   int32_t u, v, vr, ug, vg, ub, r, g, b;
 312   int16_t *y_buf1, *y_buf2, *cr_buf, *cb_buf;
 313   uint16_t *dst_up, *dst_down;
 314
 315   // Control variables
 316   int32_t line_size, block_size, y_woffset, y_hoffset;
 317
 318   y_buf1  = src;
 319   y_buf2  = y_buf1 + MCU_WIDTH;
 320
 321   cb_buf  = y_buf1 + MCU_BLOCK_SIZE * 4;
 322   cr_buf  = cb_buf + MCU_BLOCK_SIZE;
 323
 324   // Our ptrs are 16 bits
 325   y_woffset = ctx->y_woffset / 2;
 326   y_hoffset = ctx->y_hoffset / 2;
 327
 328   dst_up    = (uint16_t*) ctx->y_src;
 329   dst_down  = dst_up + y_woffset;
 330
 331   line_size             = MCU_WIDTH / 2; // We compute two pixels at a time
 332   block_size    = MCU_HEIGHT / 2; // We compute two lines at a time
 333
 334   while( num_macro_blocks > 0 )
 335   {
 336     // First quarter
 337     cb_current  = cb_buf[0];
 338     cr_current  = cr_buf[0];
 339
 340     u   = cb_current - 128;
 341     ug  = 88 * u;
 342     ub  = 454 * u;
 343     v   = cr_current - 128;
 344     vg  = 183 * v;
 345     vr  = 359 * v;
 346
 347     y_up_read   = y_buf1[0] << 8;
 348     y_down_read = y_buf2[0] << 8;
 349
 350     r = saturate5((y_up_read + vr));
 351     g = saturate6((y_up_read - ug - vg));
 352     b = saturate5((y_up_read + ub));
 353
 354     dst_up[0] = MAKE_RGBA_565(r, g, b);
 355
 356     r = saturate5((y_down_read + vr));
 357     g = saturate6((y_down_read - ug - vg));
 358     b = saturate5((y_down_read + ub));
 359
 360     dst_down[0] = MAKE_RGBA_565(r, g, b);
 361
 362     y_up_read   = y_buf1[1] << 8;
 363     y_down_read = y_buf2[1] << 8;
 364
 365     r = saturate5((y_up_read + vr));
 366     g = saturate6((y_up_read - ug - vg));
 367     b = saturate5((y_up_read + ub));
 368
 369     dst_up[1] = MAKE_RGBA_565(r, g, b);
 370
 371     r = saturate5((y_down_read + vr));
 372     g = saturate6((y_down_read - ug - vg));
 373     b = saturate5((y_down_read + ub));
 374
 375     dst_down[1] = MAKE_RGBA_565(r, g, b);
 376
 377     // Second quarter
 378     cr_current  = cr_buf[MCU_WIDTH / 2];
 379     cb_current  = cb_buf[MCU_WIDTH / 2];
 380
 381     u   = cb_current - 128;
 382     ug  = 88 * u;
 383     ub  = 454 * u;
 384     v   = cr_current - 128;
 385     vg  = 183 * v;
 386     vr  = 359 * v;
 387
 388     y_up_read   = y_buf1[MCU_BLOCK_SIZE] << 8;
 389     y_down_read = y_buf2[MCU_BLOCK_SIZE] << 8;
 390
 391     r = saturate5((y_up_read + vr));
 392     g = saturate6((y_up_read - ug - vg));
 393     b = saturate5((y_up_read + ub));
 394
 395     dst_up[MCU_WIDTH] = MAKE_RGBA_565(r, g, b);
 396
 397     r = saturate5((y_down_read + vr));
 398     g = saturate6((y_down_read - ug - vg));
 399     b = saturate5((y_down_read + ub));
 400
 401     dst_down[MCU_WIDTH] = MAKE_RGBA_565(r, g, b);
 402
 403     y_up_read   = y_buf1[MCU_BLOCK_SIZE + 1] << 8;
 404     y_down_read = y_buf2[MCU_BLOCK_SIZE + 1] << 8;
 405
 406     r = saturate5((y_up_read + vr));
 407     g = saturate6((y_up_read - ug - vg));
 408     b = saturate5((y_up_read + ub));
 409
 410     dst_up[MCU_WIDTH+1] = MAKE_RGBA_565(r, g, b);
 411
 412     r = saturate5((y_down_read + vr));
 413     g = saturate6((y_down_read - ug - vg));
 414     b = saturate5((y_down_read + ub));
 415
 416     dst_down[MCU_WIDTH+1] = MAKE_RGBA_565(r, g, b);
 417
 418     // Third quarter
 419     cr_current  = cr_buf[MCU_BLOCK_SIZE/2];
 420     cb_current  = cb_buf[MCU_BLOCK_SIZE/2];
 421
 422     u   = cb_current - 128;
 423     ug  = 88 * u;
 424     ub  = 454 * u;
 425     v   = cr_current - 128;
 426     vg  = 183 * v;
 427     vr  = 359 * v;
 428
 429     y_up_read   = y_buf1[MCU_BLOCK_SIZE*2] << 8;
 430     y_down_read = y_buf2[MCU_BLOCK_SIZE*2] << 8;
 431
 432     r = saturate5((y_up_read + vr));
 433     g = saturate6((y_up_read - ug - vg));
 434     b = saturate5((y_up_read + ub));
 435
 436     dst_up[y_hoffset] = MAKE_RGBA_565(r, g, b);
 437
 438     r = saturate5((y_down_read + vr));
 439     g = saturate6((y_down_read - ug - vg));
 440     b = saturate5((y_down_read + ub));
 441
 442     dst_down[y_hoffset] = MAKE_RGBA_565(r, g, b);
 443
 444     y_up_read   = y_buf1[MCU_BLOCK_SIZE*2 + 1] << 8;
 445     y_down_read = y_buf2[MCU_BLOCK_SIZE*2 + 1] << 8;
 446
 447     r = saturate5((y_up_read + vr));
 448     g = saturate6((y_up_read - ug - vg));
 449     b = saturate5((y_up_read + ub));
 450
 451     dst_up[y_hoffset + 1] = MAKE_RGBA_565(r, g, b);
 452
 453     r = saturate5((y_down_read + vr));
 454     g = saturate6((y_down_read - ug - vg));
 455     b = saturate5((y_down_read + ub));
 456
 457     dst_down[y_hoffset + 1] = MAKE_RGBA_565(r, g, b);
 458
 459     // Fourth quarter
 460     cr_current  = cr_buf[MCU_BLOCK_SIZE/2 + MCU_WIDTH/2];
 461     cb_current  = cb_buf[MCU_BLOCK_SIZE/2 + MCU_WIDTH/2];
 462
 463     u   = cb_current - 128;
 464     ug  = 88 * u;
 465     ub  = 454 * u;
 466     v   = cr_current - 128;
 467     vg  = 183 * v;
 468     vr  = 359 * v;
 469
 470     y_up_read   = y_buf1[MCU_BLOCK_SIZE*3] << 8;
 471     y_down_read = y_buf2[MCU_BLOCK_SIZE*3] << 8;
 472
 473     r = saturate5((y_up_read + vr));
 474     g = saturate6((y_up_read - ug - vg));
 475     b = saturate5((y_up_read + ub));
 476
 477     dst_up[y_hoffset + MCU_WIDTH] = MAKE_RGBA_565(r, g, b);
 478
 479     r = saturate5((y_down_read + vr));
 480     g = saturate6((y_down_read - ug - vg));
 481     b = saturate5((y_down_read + ub));
 482
 483     dst_down[y_hoffset + MCU_WIDTH] = MAKE_RGBA_565(r, g, b);
 484
 485     y_up_read   = y_buf1[MCU_BLOCK_SIZE*3 + 1] << 8;
 486     y_down_read = y_buf2[MCU_BLOCK_SIZE*3 + 1] << 8;
 487
 488     r = saturate5((y_up_read + vr));
 489     g = saturate6((y_up_read - ug - vg));
 490     b = saturate5((y_up_read + ub));
 491
 492     dst_up[y_hoffset + MCU_WIDTH + 1] = MAKE_RGBA_565(r, g, b);
 493
 494     r = saturate5((y_down_read + vr));
 495     g = saturate6((y_down_read - ug - vg));
 496     b = saturate5((y_down_read + ub));
 497
 498     dst_down[y_hoffset + MCU_WIDTH + 1] = MAKE_RGBA_565(r, g, b);
 499
 500     cr_buf    += 1;
 501     cb_buf    += 1;
 502     y_buf1    += 2;
 503     y_buf2    += 2;
 504     dst_up    += 2;
 505     dst_down  += 2;
 506
 507     line_size--;
 508
 509     if( line_size == 0 ) // We computed one line of a luma-block
 510     {
 511       dst_up    += y_woffset*2 - MCU_WIDTH;
 512       dst_down  += y_woffset*2 - MCU_WIDTH;
 513
 514       block_size--;
 515
 516       if( block_size == 0 )
 517       {
 518         y_buf1  = cr_buf + MCU_BLOCK_SIZE/2 + MCU_WIDTH/2;
 519         y_buf2  = y_buf1 + MCU_WIDTH;
 520
 521         cb_buf  = y_buf1 + MCU_BLOCK_SIZE * 4;
 522         cr_buf  = cb_buf + MCU_BLOCK_SIZE;
 523
 524         block_size = MCU_WIDTH / 2; // We compute two lines at a time
 525
 526         dst_up  += 2*MCU_WIDTH - y_hoffset;
 527         dst_down = dst_up + y_woffset;
 528
 529         num_macro_blocks--;
 530       }
 531       else
 532       {
 533         y_buf1  = y_buf2;
 534         y_buf2 += MCU_WIDTH;
 535
 536         cr_buf += MCU_WIDTH / 2;
 537         cb_buf += MCU_WIDTH / 2;
 538       }
 539
 540       line_size = MCU_WIDTH / 2; // We compute two pixels at a time
 541     }
 542   }
 543
 544   ctx->y_src = (uint8_t*) dst_up;
 545
 546   return C_OK;
 547 }
 548
 549
 550
 551 // Transform a blockline YUV 4:2:0 in picture of specified format
 552 static C_RESULT video_blockline_from_blockline_yuv420(video_picture_context_t* ctx, video_picture_context_t* src, int32_t num_macro_blocks);
 553 static C_RESULT video_blockline_from_blockline_rgb565(video_picture_context_t* ctx, video_picture_context_t* src, int32_t num_macro_blocks);
 554
 555 C_RESULT video_blockline_from_blockline(video_picture_context_t* ctx, video_picture_context_t* src, int32_t num_macro_blocks, enum PixelFormat format)
 556 {
 557   C_RESULT res;
 558
 559   switch(format)
 560   {
 561     case PIX_FMT_YUV420P:
 562       res = video_blockline_from_blockline_yuv420(ctx, src, num_macro_blocks);
 563       break;
 564     case PIX_FMT_RGB565:
 565       res = video_blockline_from_blockline_rgb565(ctx, src, num_macro_blocks);
 566       break;
 567
 568     default:
 569       PRINT("In file %s, in function %s, format %d not supported\n", __FILE__, __FUNCTION__, format);
 570       res = C_FAIL;
 571       break;
 572   }
 573
 574   return res;
 575 }
 576
 577 static C_RESULT video_blockline_from_blockline_yuv420(video_picture_context_t* blockline_ctx, video_picture_context_t* blockline_src, int32_t num_macro_blocks)
 578 {
 579   uint32_t line;
 580   uint8_t* dest;
 581   uint8_t* src;
 582   uint32_t copy_line_size;
 583
 584   // copy luma
 585   copy_line_size = num_macro_blocks * MB_HEIGHT_Y;
 586   dest = blockline_ctx->y_src;
 587   src  = blockline_src->y_src;
 588   line = MB_HEIGHT_Y;
 589   while (line--)
 590   {
 591     vp_os_memcpy(dest,src,copy_line_size);
 592     dest += blockline_ctx->y_woffset;
 593     src  += blockline_src->y_woffset;
 594   }
 595
 596   // copy cb
 597   copy_line_size = num_macro_blocks * MB_HEIGHT_C;
 598   dest = blockline_ctx->cb_src;
 599   src  = blockline_src->cb_src;
 600   line = MB_HEIGHT_C;
 601   while (line--)
 602   {
 603     vp_os_memcpy(dest,src,copy_line_size);
 604     dest += blockline_ctx->c_woffset;
 605     src  += blockline_src->c_woffset;
 606   }
 607
 608   // copy cr
 609   copy_line_size = num_macro_blocks * MB_HEIGHT_C;
 610   dest = blockline_ctx->cr_src;
 611   src  = blockline_src->cr_src;
 612   line = MB_HEIGHT_C;
 613   while (line--)
 614   {
 615     vp_os_memcpy(dest,src,copy_line_size);
 616     dest += blockline_ctx->c_woffset;
 617     src  += blockline_src->c_woffset;
 618   }
 619
 620   return C_OK;
 621 }
 622
 623
 624 static C_RESULT
 625 video_blockline_from_blockline_rgb565(video_picture_context_t* ctx,
 626                                                                           video_picture_context_t* src, int32_t num_macro_blocks)
 627 {
 628         uint32_t y_up_read, y_down_read, cr_current, cb_current;
 629         int32_t u, v, vr, ug, vg, ub, r, g, b;
 630         uint8_t *y_buf_up, *y_buf_down, *cr_buf, *cb_buf;
 631         uint16_t *dst_up, *dst_down;
 632         int32_t line_size,dest_y_woffset;
 633
 634         // Control variables
 635         uint32_t pixel,line;
 636
 637         // In ptrs
 638         y_buf_up = src->y_src;
 639         y_buf_down = y_buf_up + src->y_woffset;
 640
 641         cb_buf = src->cb_src;
 642         cr_buf = src->cr_src;
 643
 644         // Out ptrs are 16 bits
 645         dest_y_woffset = ctx->y_woffset / 2;
 646
 647         dst_up = (uint16_t*) ctx->y_src;
 648         dst_down = dst_up + dest_y_woffset;
 649
 650         // We compute two pixels at a time
 651         line_size = MB_WIDTH_Y*num_macro_blocks;
 652
 653         pixel = line_size>>1;
 654         line = MB_WIDTH_Y>>1;
 655
 656         while (line)
 657         {
 658                 // load cb cr
 659                 cb_current = *cb_buf++;
 660                 cr_current = *cr_buf++;
 661
 662                 u = cb_current - 128;
 663                 ug = 88 * u;
 664                 ub = 454 * u;
 665                 v = cr_current - 128;
 666                 vg = 183 * v;
 667                 vr = 359 * v;
 668
 669                 // compute pixel(0,0)
 670                 y_up_read = (*y_buf_up++) << 8;
 671                 r = saturate5((y_up_read + vr));
 672                 g = saturate6((y_up_read - ug - vg));
 673                 b = saturate5((y_up_read + ub));
 674
 675                 *dst_up++ = MAKE_RGBA_565(r, g, b);
 676
 677                 // compute pixel(1,0)
 678                 y_up_read = (*y_buf_up++) << 8;
 679
 680                 r = saturate5((y_up_read + vr));
 681                 g = saturate6((y_up_read - ug - vg));
 682                 b = saturate5((y_up_read + ub));
 683
 684                 *dst_up++ = MAKE_RGBA_565(r, g, b);
 685
 686                 // compute pixel (0,1)
 687                 y_down_read = (*y_buf_down++) << 8;
 688
 689                 r = saturate5((y_down_read + vr));
 690                 g = saturate6((y_down_read - ug - vg));
 691                 b = saturate5((y_down_read + ub));
 692
 693                 *dst_down++ = MAKE_RGBA_565(r, g, b);
 694
 695                 // compute pixel (1,1)
 696                 y_down_read = (*y_buf_down++) << 8;
 697
 698                 r = saturate5((y_down_read + vr));
 699                 g = saturate6((y_down_read - ug - vg));
 700                 b = saturate5((y_down_read + ub));
 701
 702                 *dst_down++ = MAKE_RGBA_565(r, g, b);
 703
 704                 pixel--;
 705                 if (pixel == 0)
 706                 {
 707                         // jump to next line
 708                         y_buf_up += 2*src->y_woffset - line_size;
 709                         y_buf_down += 2*src->y_woffset - line_size;
 710                         cb_buf += src->c_woffset - (line_size>>1);
 711                         cr_buf += src->c_woffset - (line_size>>1);
 712
 713                         dst_up += 2*dest_y_woffset - line_size;
 714                         dst_down += 2*dest_y_woffset - line_size;
 715
 716                         pixel = line_size>>1;
 717                         line--;
 718                 }
 719         }
 720
 721         return C_OK;
 722 }