git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/VP_SDK/VP_Stages/vp_stages_yuv2rgb.c

   1 /**
   2  *  @file   vp_stages_yuv2rgb.c
   3  *  @brief  VP Stages. YUV to RGB converter stage declaration
   4 */
   5
   6 #ifdef _INCLUDED_FOR_DOXYGEN_
   7 #else // ! _INCLUDED_FOR_DOXYGEN_
   8
   9 ///////////////////////////////////////////////
  10 // INCLUDES
  11
  12 #include <VP_Stages/vp_stages_yuv2rgb.h>
  13 #include <VP_Api/vp_api_config.h>
  14 #include <VP_Api/vp_api_picture.h>
  15 #include <VP_Os/vp_os_print.h>
  16 #include <VP_Os/vp_os_malloc.h>
  17 #include <VP_Os/vp_os_assert.h>
  18
  19 #endif // < _INCLUDED_FOR_DOXYGEN_
  20
  21
  22 #ifdef USE_YUV2RGB_STRETCH
  23
  24 static C_RESULT vp_stages_yuv2rgb_open(vp_stages_yuv2rgb_config_t *cfg);
  25 static C_RESULT vp_stages_yuv2rgb_close(vp_stages_yuv2rgb_config_t *cfg);
  26 static void vp_stages_yuv2rgb_hresample(uint32_t sample0, uint32_t sample1, int8_t cov, int8_t* frac, uint32_t* line0, uint32_t* line1, int* dx);
  27 static void vp_stages_yuv2rgb_hstretch(uint8_t* y_base, int32_t y_rbytes, uint8_t* cb_base, uint8_t* cr_base, uint32_t* line0, uint32_t* line1);
  28 static void vp_stages_yuv2rgb_vstretch(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes, int8_t cov, int8_t* frac, uint32_t* line, int* dy);
  29 static void vp_stages_yuv2rgb_flushline_565(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes, int y);
  30
  31
  32 #define SRC_WIDTH   176
  33 #define SRC_HEIGHT  144
  34 #define DST_WIDTH   320
  35 #define DST_HEIGHT  240
  36
  37 /******************************************************************************/
  38 /*                                                                            */
  39 /*          176*144 (YCbCr, 4:2:0) -> 320*240 (RGB-16bits, 5:6:5)             */
  40 /*                                                                            */
  41 /******************************************************************************/
  42
  43 static int8_t htable[SRC_WIDTH] =
  44 {
  45   0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E,
  46   0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F,
  47   0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F,
  48   0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E,
  49   0x0F, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0F, 0x0E, 0x0F, 0x0E,
  50   0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F,
  51   0x0E, 0x0F, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0F, 0x0E, 0x0F,
  52   0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E,
  53   0x0F, 0x0E, 0x0F, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0F, 0x0E,
  54   0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F,
  55   0x0E, 0x0F, 0x0E, 0x0F, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0F
  56 };
  57
  58 static int8_t vtable[SRC_HEIGHT] =
  59 {
  60   0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D,
  61   0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D,
  62   0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E,
  63   0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D,
  64   0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D,
  65   0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E,
  66   0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D,
  67   0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D,
  68   0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E
  69 };
  70
  71 /******************************************************************************/
  72
  73 #endif  // < USE_YUV2RGB_STRETCH
  74
  75
  76 /**
  77  *  @def      VP_STAGES_YUV2RGB_LIMIT(x)
  78  *  @brief    Macro used for ARGB32 saturation
  79  *
  80  *  Moves x in a limited range of values (between 0 and 0xff).
  81  */
  82 //#define VP_STAGES_YUV2ARGB_LIMIT(x)  ( (x) > 0xffff ? 0xff : ( (x) <= 0xff ? 0 : ( (x) >> 8 ) ) )
  83 #define VP_STAGES_YUV2ARGB_LIMIT(dst, x) \
  84   dst = x; \
  85   dst = (dst > 0 ? ( ((dst) >> 8) > 0xff ? 0xff : ((dst) >> 8) ) : 0);
  86
  87 /**
  88  *  @def      VP_STAGES_YUV2RGB_SAT5U(a)
  89  *  @brief    5 bits saturation
  90  */
  91 #define VP_STAGES_YUV2RGB_SAT5U(a)  \
  92   if((a) < 0) (a) = 0;              \
  93   else if((a) > 0x1F) (a) = 0x1F;
  94
  95 /**
  96  *  @def      VP_STAGES_YUV2RGB_SAT6U(a)
  97  *  @brief    6 bits saturation
  98  */
  99 #define VP_STAGES_YUV2RGB_SAT6U(a)  \
 100   if((a) < 0) (a) = 0;              \
 101   else if((a) > 0x3F) (a) = 0x3F;
 102
 103 /** Pointer to a YUV to RGB conversion function
 104  */
 105 static vp_stages_YUV_to_RGB_t vp_stages_YUV_to_RGB;
 106
 107 #ifndef QCIF_TO_QVGA
 108 /** YUV to RGB conversion functions
 109  */
 110   void vp_stages_YUV420P_to_RGB565(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes);
 111   void vp_stages_YUV420P_to_RGB24(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes);
 112   void vp_stages_YUV420P_to_ARGB32(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes);
 113 #else // ! QCIF_TO_QVGA
 114 /** YUV to RGB conversion + resizing
 115  */
 116 #ifdef USE_YUV2RGB_STRETCH
 117   static void vp_stages_YUV420P_to_RGB565_QCIF_to_QVGA_stretch(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes);
 118 #else  // ! USE_YUV2RGB_STRETCH
 119   static void vp_stages_YUV420P_to_RGB565_QCIF_to_QVGA(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes);
 120   static void vp_stages_YUV420P_to_RGB24_QCIF_to_QVGA(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes);
 121   static void vp_stages_YUV420P_to_ARGB32_QCIF_to_QVGA(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes);
 122 #endif  // < USE_YUV2RGB_STRETCH
 123 #endif // < QCIF_TO_QVGA
 124
 125
 126 #ifndef QCIF_TO_QVGA
 127 void vp_stages_YUV420P_to_RGB565(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes)
 128 {
 129   uint32_t  w, width;
 130   uint32_t  h, height;
 131
 132   width = picture->width >> 1;
 133   height = picture->height >> 1;
 134
 135   for (h = 0; h < height; h++)
 136   {
 137     uint8_t*  s0;
 138     uint8_t*  s1;
 139     uint8_t*  s2;
 140     uint16_t* d0;
 141     uint16_t* d1;
 142
 143     s0 = picture->y_buf  + (h * picture->y_line_size * 2);
 144     s1 = picture->cb_buf + (h * picture->cb_line_size);
 145     s2 = picture->cr_buf + (h * picture->cr_line_size);
 146     d0 = (uint16_t*) (dst + (h * dst_rbytes * 2));
 147     d1 = (uint16_t*) (dst + (h * dst_rbytes * 2) + dst_rbytes);
 148
 149     for (w = 0; w < width; w++)
 150     {
 151       int32_t   y, cb, cr;
 152       int32_t   c_r, c_g, c_b;
 153       int16_t   r, g, b;
 154
 155       cb = ((uint32_t) *(s1++)) - 0x80L;
 156       cr = ((uint32_t) *(s2++)) - 0x80L;
 157       c_r = cr * 359L;
 158       c_g = (cb * -88L) + (cr * -183L);
 159       c_b = cb * 454L;
 160
 161       y = ((uint32_t) s0[0]) << 8;
 162       r = (int16_t) ((y + c_r + 0x000) >> 11);      VP_STAGES_YUV2RGB_SAT5U(r);
 163       g = (int16_t) ((y + c_g + 0x000) >> 10);      VP_STAGES_YUV2RGB_SAT6U(g);
 164       b = (int16_t) ((y + c_b + 0x000) >> 11);      VP_STAGES_YUV2RGB_SAT5U(b);
 165       d0[0] = (r << 11) | (g << 5) | (b << 0);
 166
 167       y = ((uint32_t) s0[1]) << 8;
 168       r = (int16_t) ((y + c_r + 0x400) >> 11);      VP_STAGES_YUV2RGB_SAT5U(r);
 169       g = (int16_t) ((y + c_g + 0x200) >> 10);      VP_STAGES_YUV2RGB_SAT6U(g);
 170       b = (int16_t) ((y + c_b + 0x400) >> 11);      VP_STAGES_YUV2RGB_SAT5U(b);
 171       d0[1] = (r << 11) | (g << 5) | (b << 0);
 172
 173       y = ((uint32_t) s0[picture->y_line_size + 0]) << 8;
 174       r = (int16_t) ((y + c_r + 0x600) >> 11);      VP_STAGES_YUV2RGB_SAT5U(r);
 175       g = (int16_t) ((y + c_g + 0x300) >> 10);      VP_STAGES_YUV2RGB_SAT6U(g);
 176       b = (int16_t) ((y + c_b + 0x600) >> 11);      VP_STAGES_YUV2RGB_SAT5U(b);
 177       d1[0] = (r << 11) | (g << 5) | (b << 0);
 178
 179       y = ((uint32_t) s0[picture->y_line_size + 1]) << 8;
 180       r = (int16_t) ((y + c_r + 0x200) >> 11);      VP_STAGES_YUV2RGB_SAT5U(r);
 181       g = (int16_t) ((y + c_g + 0x100) >> 10);      VP_STAGES_YUV2RGB_SAT6U(g);
 182       b = (int16_t) ((y + c_b + 0x200) >> 11);      VP_STAGES_YUV2RGB_SAT5U(b);
 183       d1[1] = (r << 11) | (g << 5) | (b << 0);
 184
 185       s0 += 2;
 186       d0 += 2;
 187       d1 += 2;
 188     }
 189   }
 190 }
 191 #endif // < QCIF_TO_QVGA
 192
 193
 194 #ifndef QCIF_TO_QVGA
 195 void vp_stages_YUV420P_to_RGB24(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes)
 196 {
 197   uint32_t width, height;
 198   int32_t line, col, linewidth;
 199   int32_t y, u, v, r, g, b;
 200   int32_t vr, ug, vg, ub;
 201   uint8_t *py, *pu, *pv;
 202
 203   int32_t lineSz0 = picture->y_line_size;
 204   int32_t lineSz1 = picture->cb_line_size;
 205   int32_t lineSz2 = picture->cr_line_size;
 206
 207   width = picture->width;
 208   height = picture->height;
 209
 210   linewidth = width - (width >> 1);
 211
 212   if(cfg->mode == VP_STAGES_YUV2RGB_MODE_UPSIDE_DOWN)
 213     {
 214       py = picture->y_buf+lineSz0*(height-1)+width-1;
 215       pu = picture->cb_buf+lineSz1*((height>>1)-1)+((width>>1)-1);
 216       pv = picture->cr_buf+lineSz2*((height>>1)-1)+((width>>1)-1);
 217
 218       for (line = height-1; line >= 0; line--) {
 219         for (col = width-1; col >= 0; col--) {
 220           y   = *py;
 221           y   = y << 8;
 222           u   = *pu - 128;
 223           ug  = 88 * u;
 224           ub  = 454 * u;
 225           v   = *pv - 128;
 226           vg  = 183 * v;
 227           vr  = 359 * v;
 228
 229           VP_STAGES_YUV2ARGB_LIMIT(r, y +      vr);
 230           VP_STAGES_YUV2ARGB_LIMIT(g, y - ug - vg);
 231           VP_STAGES_YUV2ARGB_LIMIT(b, y + ub     );
 232
 233           *dst = r;
 234           dst++;
 235           *dst = g;
 236           dst++;
 237           *dst = b;
 238           dst++;
 239
 240           py--;
 241
 242           if (col & 1)
 243             {
 244               pu--;
 245               pv--;
 246             } // No else
 247         }
 248
 249         pu += linewidth;
 250         pv += linewidth;
 251
 252         if (line & 1)
 253           {
 254             pu -= lineSz1;
 255             pv -= lineSz2;
 256           } // No else
 257         py -= lineSz0 - width;
 258       }
 259     }
 260   else
 261     {
 262       py = picture->y_buf;
 263       pu = picture->cb_buf;
 264       pv = picture->cr_buf;
 265
 266       for (line = 0; line < (int32_t)height; line++) {
 267         for (col = 0; col < (int32_t)width; col++) {
 268           y   = *py;
 269           y   = y << 8;
 270           u   = *pu - 128;
 271           ug  = 88 * u;
 272           ub  = 454 * u;
 273           v   = *pv - 128;
 274           vg  = 183 * v;
 275           vr  = 359 * v;
 276
 277           VP_STAGES_YUV2ARGB_LIMIT(r, y +      vr);
 278           VP_STAGES_YUV2ARGB_LIMIT(g, y - ug - vg);
 279           VP_STAGES_YUV2ARGB_LIMIT(b, y + ub     );
 280
 281           *dst = r;
 282           dst++;
 283           *dst = g;
 284           dst++;
 285           *dst = b;
 286           dst++;
 287
 288           py++;
 289
 290           if (col & 1)
 291             {
 292               pu++;
 293               pv++;
 294             } // No else
 295         }
 296
 297         pu -= linewidth;
 298         pv -= linewidth;
 299
 300         if (line & 1)
 301           {
 302             pu += lineSz1;
 303             pv += lineSz2;
 304           } // No else
 305         py += lineSz0 - width;
 306       }
 307     }
 308 }
 309 #endif // < QCIF_TO_QVGA
 310
 311
 312 #ifndef QCIF_TO_QVGA
 313 void vp_stages_YUV420P_to_ARGB32(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes)
 314 {
 315   uint32_t width, height;
 316   int32_t line, col, linewidth;
 317   int32_t y, u, v, r, g, b;
 318   int32_t vr, ug, vg, ub;
 319   uint8_t *py, *pu, *pv;
 320
 321   int32_t lineSz0 = picture->y_line_size;
 322   int32_t lineSz1 = picture->cb_line_size;
 323   int32_t lineSz2 = picture->cr_line_size;
 324
 325   width = picture->width;
 326   height = picture->height;
 327
 328   linewidth = width - (width >> 1);
 329
 330   py = picture->y_buf;
 331   pu = picture->cb_buf;
 332   pv = picture->cr_buf;
 333
 334   for (line = 0; line < (int32_t)height; line++) {
 335     for (col = 0; col < (int32_t)width; col++) {
 336       y   = *py;
 337       y   = y << 8;
 338       u   = *pu - 128;
 339       ug  = 88 * u;
 340       ub  = 454 * u;
 341       v   = *pv - 128;
 342       vg  = 183 * v;
 343       vr  = 359 * v;
 344
 345       VP_STAGES_YUV2ARGB_LIMIT(r, y + vr);
 346       VP_STAGES_YUV2ARGB_LIMIT(g, y - ug - vg);
 347       VP_STAGES_YUV2ARGB_LIMIT(b, y + ub);
 348
 349       *dst = b;
 350       dst++;
 351       *dst = g;
 352       dst++;
 353       *dst = r;
 354       dst+=2;        //skip Alpha value
 355
 356       py++;
 357
 358       if (col & 1)
 359       {
 360         pu++;
 361         pv++;
 362       } // No else
 363     }
 364
 365     pu -= linewidth;
 366     pv -= linewidth;
 367
 368     if (line & 1)
 369     {
 370       pu += lineSz1;
 371       pv += lineSz2;
 372     } // No else
 373     py += lineSz0 - width;
 374   }
 375 }
 376 #endif // < QCIF_TO_QVGA
 377
 378
 379 #ifdef QCIF_TO_QVGA
 380 #ifdef USE_YUV2RGB_STRETCH
 381 static void vp_stages_YUV420P_to_RGB565_QCIF_to_QVGA_stretch(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes)
 382 {
 383   int     h;
 384   uint8_t*  y;
 385   uint8_t*  cb;
 386   uint8_t*  cr;
 387   int     dy;
 388   int8_t    vfrac;
 389
 390   vp_os_memset(cfg->vline, 0, dst_rbytes);
 391
 392   y = picture->y_buf;
 393   cb = picture->cb_buf;
 394   cr = picture->cr_buf;
 395   vfrac = 8;
 396   dy = 0;
 397   for (h = 0; h < SRC_HEIGHT; h += 2)
 398   {
 399     vp_stages_yuv2rgb_hstretch(y, picture->y_line_size, cb, cr, cfg->hline0, cfg->hline1);
 400     vp_stages_yuv2rgb_vstretch(cfg, picture, dst, dst_rbytes, vtable[h + 0], &vfrac, cfg->hline0, &dy);
 401     vp_stages_yuv2rgb_vstretch(cfg, picture, dst, dst_rbytes, vtable[h + 1], &vfrac, cfg->hline1, &dy);
 402     y += (picture->y_line_size * 2);
 403     cb += picture->cb_line_size;
 404     cr += picture->cr_line_size;
 405   }
 406 }
 407 #endif  // < USE_YUV2RGB_STRETCH
 408 #endif  // <  QCIF_TO_QVGA
 409
 410
 411 #ifdef QCIF_TO_QVGA
 412 #ifndef USE_YUV2RGB_STRETCH
 413 static void vp_stages_YUV420P_to_RGB565_QCIF_to_QVGA(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes)
 414 {
 415   uint32_t  w, width;
 416   uint32_t  h, height;
 417   uint8_t*  s0;
 418   uint8_t*  s1;
 419   uint8_t*  s2;
 420   uint16_t* d0;
 421   uint16_t* d1;
 422   int32_t   y, cb, cr;
 423   int32_t   c_r, c_g, c_b;
 424   int16_t   r, g, b;
 425
 426   width = QVGA_WIDTH >> 2;
 427   height = QVGA_HEIGHT >> 2;
 428
 429   for (h = 0; h < height; h++)
 430   {
 431     s0 = picture->y_buf  + ((h * picture->y_line_size)<<1);
 432     s1 = picture->cb_buf + (h * picture->cb_line_size);
 433     s2 = picture->cr_buf + (h * picture->cr_line_size);
 434     d0 = (uint16_t*) (dst + ((h * QVGA_WIDTH) << 3));
 435     d1 = (uint16_t*) (dst + ((h * QVGA_WIDTH) << 3) + (QVGA_WIDTH << 2));
 436
 437     for (w = 0; w < width; w++)
 438     {
 439       cb = ((uint32_t) *(s1++)) - 0x80L;
 440       cr = ((uint32_t) *(s2++)) - 0x80L;
 441       c_r = cr * 359L;
 442       c_g = (cb * -88L) + (cr * -183L);
 443       c_b = cb * 454L;
 444
 445       y = ((uint32_t) s0[0]) << 8;
 446       r = (int16_t) ((y + c_r + 0x000) >> 11);      VP_STAGES_YUV2RGB_SAT5U(r);
 447       g = (int16_t) ((y + c_g + 0x000) >> 10);      VP_STAGES_YUV2RGB_SAT6U(g);
 448       b = (int16_t) ((y + c_b + 0x000) >> 11);      VP_STAGES_YUV2RGB_SAT5U(b);
 449       d0[0] = (r << 11) | (g << 5) | (b << 0);
 450       d0[1] = d0[0];
 451
 452       y = ((uint32_t) s0[1]) << 8;
 453       r = (int16_t) ((y + c_r + 0x400) >> 11);      VP_STAGES_YUV2RGB_SAT5U(r);
 454       g = (int16_t) ((y + c_g + 0x200) >> 10);      VP_STAGES_YUV2RGB_SAT6U(g);
 455       b = (int16_t) ((y + c_b + 0x400) >> 11);      VP_STAGES_YUV2RGB_SAT5U(b);
 456       d0[2] = (r << 11) | (g << 5) | (b << 0);
 457       d0[3] = d0[2];
 458
 459       y = ((uint32_t) s0[picture->y_line_size + 0]) << 8;
 460       r = (int16_t) ((y + c_r + 0x600) >> 11);      VP_STAGES_YUV2RGB_SAT5U(r);
 461       g = (int16_t) ((y + c_g + 0x300) >> 10);      VP_STAGES_YUV2RGB_SAT6U(g);
 462       b = (int16_t) ((y + c_b + 0x600) >> 11);      VP_STAGES_YUV2RGB_SAT5U(b);
 463       d1[0] = (r << 11) | (g << 5) | (b << 0);
 464       d1[1] = d1[0];
 465
 466       y = ((uint32_t) s0[picture->y_line_size + 1]) << 8;
 467       r = (int16_t) ((y + c_r + 0x200) >> 11);      VP_STAGES_YUV2RGB_SAT5U(r);
 468       g = (int16_t) ((y + c_g + 0x100) >> 10);      VP_STAGES_YUV2RGB_SAT6U(g);
 469       b = (int16_t) ((y + c_b + 0x200) >> 11);      VP_STAGES_YUV2RGB_SAT5U(b);
 470       d1[2] = (r << 11) | (g << 5) | (b << 0);
 471       d1[3] = d1[2];
 472
 473       s0 += 2;
 474       d0 += 4;
 475       d1 += 4;
 476     }
 477     vp_os_memcpy(dst + ((h * QVGA_WIDTH) << 3) + (QVGA_WIDTH<<1),    dst + ((h * QVGA_WIDTH) << 3),                    QVGA_WIDTH<<1);
 478     vp_os_memcpy(dst + ((h * QVGA_WIDTH) << 3) + (QVGA_WIDTH<<1)*3,  dst + ((h * QVGA_WIDTH) << 3) + (QVGA_WIDTH<<2),  QVGA_WIDTH<<1);
 479   }
 480 }
 481 #endif  // < USE_YUV2RGB_STRETCH
 482 #endif  // <  QCIF_TO_QVGA
 483
 484
 485 #ifdef QCIF_TO_QVGA
 486 #ifndef USE_YUV2RGB_STRETCH
 487 static void vp_stages_YUV420P_to_RGB24_QCIF_to_QVGA(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes)
 488 {
 489 }
 490 #endif  // < USE_YUV2RGB_STRETCH
 491 #endif  // <  QCIF_TO_QVGA
 492
 493
 494 #ifdef QCIF_TO_QVGA
 495 #ifndef USE_YUV2RGB_STRETCH
 496 static void vp_stages_YUV420P_to_ARGB32_QCIF_to_QVGA(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes)
 497 {
 498   uint32_t width, height;
 499   int32_t line, col, linewidth;
 500   int32_t y, u, v, r, g, b;
 501   int32_t vr, ug, vg, ub;
 502   uint8_t *py, *pu, *pv;
 503
 504   int32_t lineSz0 = picture->y_line_size;
 505   int32_t lineSz1 = picture->cb_line_size;
 506   int32_t lineSz2 = picture->cr_line_size;
 507
 508   width = picture->width;
 509   height = picture->height;
 510
 511   linewidth = width - (width >> 1);
 512
 513   py = picture->y_buf;
 514   pu = picture->cb_buf;
 515   pv = picture->cr_buf;
 516
 517   for (line = 0; line < (QVGA_HEIGHT >> 1); line++) {
 518     for (col = 0; col < (QVGA_WIDTH >> 1); col++) {
 519       y   = *py;
 520       y   = y << 8;
 521       u   = *pu - 128;
 522       ug  = 88 * u;
 523       ub  = 454 * u;
 524       v   = *pv - 128;
 525       vg  = 183 * v;
 526       vr  = 359 * v;
 527
 528       VP_STAGES_YUV2ARGB_LIMIT(r, y + vr);
 529       VP_STAGES_YUV2ARGB_LIMIT(g, y - ug - vg);
 530       VP_STAGES_YUV2ARGB_LIMIT(b, y + ub);
 531
 532       *dst = b;
 533       dst++;
 534       *dst = g;
 535       dst++;
 536       *dst = r;
 537       dst+=2;        //skip Alpha value
 538       *dst = b;
 539       dst++;
 540       *dst = g;
 541       dst++;
 542       *dst = r;
 543       dst+=2;
 544
 545       py++;
 546
 547       if (col & 1)
 548       {
 549         pu++;
 550         pv++;
 551       } // No else
 552     }
 553
 554     pu -= (QVGA_WIDTH >> 2);
 555     pv -= (QVGA_WIDTH >> 2);
 556
 557     if (line & 1)
 558     {
 559       pu += lineSz1;
 560       pv += lineSz2;
 561     } // No else
 562     py += lineSz0 - (QVGA_WIDTH >> 1);
 563
 564     vp_os_memcpy(dst, dst - (QVGA_WIDTH<<2), QVGA_WIDTH<<2);
 565     dst += (QVGA_WIDTH<<2);
 566   }
 567 }
 568 #endif  // < USE_YUV2RGB_STRETCH
 569 #endif  // <  QCIF_TO_QVGA
 570
 571
 572 /******************************************************************************/
 573
 574 #ifdef USE_YUV2RGB_STRETCH
 575 static C_RESULT vp_stages_yuv2rgb_open(vp_stages_yuv2rgb_config_t *cfg)
 576 {
 577   C_RESULT res = VP_SUCCESS;
 578
 579   cfg->hline0 = (uint32_t*) vp_os_malloc(DST_WIDTH * sizeof(uint32_t));
 580   cfg->hline1 = (uint32_t*) vp_os_malloc(DST_WIDTH * sizeof(uint32_t));
 581   cfg->vline  = (uint32_t*) vp_os_malloc(DST_WIDTH * sizeof(uint32_t));
 582   if (cfg->hline0 && cfg->hline1 && cfg->vline)
 583   {
 584 //    vfrac = 8;
 585   }
 586   else
 587   {
 588     vp_stages_yuv2rgb_close(cfg);
 589     res = VP_FAILURE;
 590   }
 591
 592   return res;
 593 }
 594
 595 /******************************************************************************/
 596
 597 static C_RESULT vp_stages_yuv2rgb_close(vp_stages_yuv2rgb_config_t *cfg)
 598 {
 599   if (cfg->hline0)
 600   {
 601     vp_os_free(cfg->hline0);
 602     cfg->hline0 = NULL;
 603   }
 604   if (cfg->hline1)
 605   {
 606     vp_os_free(cfg->hline1);
 607     cfg->hline1 = NULL;
 608   }
 609   if (cfg->vline)
 610   {
 611     vp_os_free(cfg->vline);
 612     cfg->vline = NULL;
 613   }
 614   return(VP_SUCCESS);
 615 }
 616
 617 /******************************************************************************/
 618
 619 static void vp_stages_yuv2rgb_hresample(uint32_t sample0, uint32_t sample1, int8_t cov, int8_t* frac, uint32_t* line0, uint32_t* line1, int* dx)
 620 {
 621   int8_t  f;
 622   int   i;
 623
 624   f = *frac;
 625   i = *dx;
 626   do
 627   {
 628     int8_t  c;
 629
 630     if (f < cov)
 631     {
 632       c = f;
 633     }
 634     else
 635     {
 636       c = cov;
 637     }
 638     cov -= c;
 639     f -= c;
 640     switch (c)
 641     {
 642       case 0:
 643       assert(0);
 644       break;
 645
 646       case 1:
 647       line0[i] += (sample0 >> 3) & 0x1F1F1F1FUL;
 648       line1[i] += (sample1 >> 3) & 0x1F1F1F1FUL;
 649       break;
 650
 651       case 2:
 652       line0[i] += (sample0 >> 2) & 0x3F3F3F3FUL;
 653       line1[i] += (sample1 >> 2) & 0x3F3F3F3FUL;
 654       break;
 655
 656       case 3:
 657       line0[i] += ((sample0 >> 1) & 0x7F7F7F7FUL) - ((sample0 >> 3) & 0x1F1F1F1FUL);
 658       line1[i] += ((sample1 >> 1) & 0x7F7F7F7FUL) - ((sample1 >> 3) & 0x1F1F1F1FUL);
 659       break;
 660
 661       case 4:
 662       line0[i] += (sample0 >> 1) & 0x7F7F7F7FUL;
 663       line1[i] += (sample1 >> 1) & 0x7F7F7F7FUL;
 664       break;
 665
 666       case 5:
 667       line0[i] += ((sample0 >> 1) & 0x7F7F7F7FUL) + ((sample0 >> 3) & 0x1F1F1F1FUL);
 668       line1[i] += ((sample1 >> 1) & 0x7F7F7F7FUL) + ((sample1 >> 3) & 0x1F1F1F1FUL);
 669       break;
 670
 671       case 6:
 672       line0[i] += sample0 - ((sample0 >> 2) & 0x3F3F3F3FUL);
 673       line1[i] += sample1 - ((sample1 >> 2) & 0x3F3F3F3FUL);
 674       break;
 675
 676       case 7:
 677       line0[i] += sample0 - ((sample0 >> 3) & 0x1F1F1F1FUL);
 678       line1[i] += sample1 - ((sample1 >> 3) & 0x1F1F1F1FUL);
 679       break;
 680
 681       case 8:
 682       line0[i] += sample0;
 683       line1[i] += sample1;
 684       break;
 685     }
 686
 687     if (f == 0)
 688     {
 689       f = 8;
 690       i++;
 691       if (i < DST_WIDTH)
 692       {
 693         line0[i] = 0UL;
 694         line1[i] = 0UL;
 695       }
 696     }
 697   } while (cov);
 698   *dx = i;
 699   *frac = f;
 700 }
 701
 702 /*----------------------------------------------------------------------------*/
 703
 704 static void vp_stages_yuv2rgb_hstretch(uint8_t* y_base, int32_t y_rbytes, uint8_t* cb_base, uint8_t* cr_base, uint32_t* line0, uint32_t* line1)
 705 {
 706   int     sx, dx;
 707   int8_t    frac;
 708   uint8_t*  s0;
 709   uint8_t*  s1;
 710   uint8_t*  s2;
 711
 712   frac = 8;
 713   dx = 0;
 714   line0[0] = 0UL;
 715   line1[0] = 0UL;
 716   s0 = y_base;
 717   s1 = cb_base;
 718   s2 = cr_base;
 719   for (sx = 0; sx < 176; sx += 2)
 720   {
 721     int32_t   cb, cr;
 722     uint32_t  cr_cg_cb, smp0, smp1;
 723
 724     cb = (int32_t) ((uint32_t) *(s1++));
 725     cr = (int32_t) ((uint32_t) *(s2++));
 726     cr_cg_cb = ((cr * 180L) << 8) & 0xFF0000UL;
 727     cr_cg_cb |= ((cb * 44L) + (cr * 91L)) & 0xFF00UL;
 728     cr_cg_cb |= ((cb * 227L) >> 8) & 0xFFUL;
 729
 730     smp0 = cr_cg_cb | (((uint32_t) s0[0]) << 24);
 731     smp1 = cr_cg_cb | (((uint32_t) s0[y_rbytes + 0]) << 24);
 732     vp_stages_yuv2rgb_hresample(smp0, smp1, htable[sx], &frac, line0, line1, &dx);
 733
 734     smp0 = cr_cg_cb | (((uint32_t) s0[1]) << 24);
 735     smp1 = cr_cg_cb | (((uint32_t) s0[y_rbytes + 1]) << 24);
 736     vp_stages_yuv2rgb_hresample(smp0, smp1, htable[sx], &frac, line0, line1, &dx);
 737     s0 += 2;
 738   }
 739 }
 740
 741 /*----------------------------------------------------------------------------*/
 742
 743 static void vp_stages_yuv2rgb_vstretch(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst_base, uint32_t dst_rbytes, int8_t cov, int8_t* frac, uint32_t* line, int* dy)
 744 {
 745   int8_t    f;
 746   uint32_t* src;
 747   uint32_t* dst;
 748   int     y;
 749
 750   f = *frac;
 751   y = *dy;
 752   do
 753   {
 754     int8_t  c;
 755
 756     if (f < cov)
 757     {
 758       c = f;
 759     }
 760     else
 761     {
 762       c = cov;
 763     }
 764     cov -= c;
 765     f -= c;
 766     src = line;
 767     dst = cfg->vline;
 768     switch (c)
 769     {
 770       int   dx;
 771
 772       case 8:
 773       for (dx = 0; dx < DST_WIDTH; dx++)
 774       {
 775         uint32_t  ycc;
 776
 777         ycc = src[dx];
 778         dst[dx] = ycc;
 779       }
 780       break;
 781
 782       case 7:
 783       for (dx = 0; dx < DST_WIDTH; dx++)
 784       {
 785         uint32_t  ycc;
 786
 787         ycc = src[dx];
 788         dst[dx] += (ycc) - ((ycc >> 3) & 0x1F1F1F1FUL);
 789       }
 790       break;
 791
 792       case 6:
 793       for (dx = 0; dx < DST_WIDTH; dx++)
 794       {
 795         uint32_t  ycc;
 796
 797         ycc = src[dx];
 798         dst[dx] += (ycc) - ((ycc >> 2) & 0x3F3F3F3FUL);
 799       }
 800       break;
 801
 802       case 5:
 803       for (dx = 0; dx < DST_WIDTH; dx++)
 804       {
 805         uint32_t  ycc;
 806
 807         ycc = src[dx];
 808         dst[dx] += ((ycc >> 1) & 0x7F7F7F7FUL) + ((ycc >> 3) & 0x1F1F1F1FUL);
 809       }
 810       break;
 811
 812       case 4:
 813       for (dx = 0; dx < DST_WIDTH; dx++)
 814       {
 815         uint32_t  ycc;
 816
 817         ycc = src[dx];
 818         dst[dx] += ((ycc >> 1) & 0x7F7F7F7FUL);
 819       }
 820       break;
 821
 822       case 3:
 823       for (dx = 0; dx < DST_WIDTH; dx++)
 824       {
 825         uint32_t  ycc;
 826
 827         ycc = src[dx];
 828         dst[dx] += ((ycc >> 1) & 0x7F7F7F7FUL) - ((ycc >> 3) & 0x1F1F1F1FUL);
 829       }
 830       break;
 831
 832       case 2:
 833       for (dx = 0; dx < DST_WIDTH; dx++)
 834       {
 835         uint32_t  ycc;
 836
 837         ycc = src[dx];
 838         dst[dx] += ((ycc >> 2) & 0x3F3F3F3FUL);
 839       }
 840       break;
 841
 842       case 1:
 843       for (dx = 0; dx < DST_WIDTH; dx++)
 844       {
 845         uint32_t  ycc;
 846
 847         ycc = src[dx];
 848         dst[dx] += ((ycc >> 3) & 0x1F1F1F1FUL);
 849       }
 850       break;
 851
 852       case 0:
 853       assert(0);
 854       break;
 855     }
 856
 857     if (f == 0)
 858     {
 859       vp_stages_yuv2rgb_flushline_565(cfg, picture, dst_base, dst_rbytes, y++);
 860       f = 8;
 861     }
 862   } while (cov);
 863
 864   *dy = y;
 865   *frac = f;
 866 }
 867
 868 /*----------------------------------------------------------------------------*/
 869
 870 static void vp_stages_yuv2rgb_flushline_565(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes, int y)
 871 {
 872 #ifdef __linux__
 873   uint32_t* s;
 874   uint16_t* d;
 875   int     i, even, odd;
 876
 877   s = cfg->vline;
 878   d = (uint16_t*) (dst + (dst_rbytes * y));
 879   even = 0;
 880   odd = 4;
 881   if (y & 1)
 882   {
 883     even ^= 6;
 884     odd ^= 6;
 885   }
 886   for (i = 0; i < DST_WIDTH; i += 2)
 887   {
 888     int32_t   ycc;
 889     int32_t   y;
 890     int32_t   r, g, b;
 891     int32_t   c_r, c_g, c_b;
 892
 893     ycc = s[0];
 894     y = ((uint32_t) ycc) >> 24;
 895     c_r = ycc << 8;           c_r = ((uint32_t) c_r) >> 23;   c_r  -= 180;
 896     c_g = ycc << 16;          c_g = ((uint32_t) c_g) >> 23;   c_g  -= 135;
 897     c_b = ycc << 24;          c_b = ((uint32_t) c_b) >> 23;   c_b  -= 227;
 898     r = (even + y + c_r) >> 3;      VP_STAGES_YUV2RGB_SAT5U(r);
 899     g = ((even >> 1) + y - c_g) >> 2; VP_STAGES_YUV2RGB_SAT6U(g);
 900     b = (even + y + c_b) >> 3;      VP_STAGES_YUV2RGB_SAT5U(b);
 901     d[0] = (r << 11) | (g << 5) | (b << 0);
 902
 903     ycc = s[1];
 904     y = ((uint32_t) ycc) >> 24;
 905     c_r = ycc << 8;           c_r = ((uint32_t) c_r) >> 23;   c_r  -= 180;
 906     c_g = ycc << 16;          c_g = ((uint32_t) c_g) >> 23;   c_g  -= 135;
 907     c_b = ycc << 24;          c_b = ((uint32_t) c_b) >> 23;   c_b  -= 227;
 908     r = (odd + y + c_r) >> 3;     VP_STAGES_YUV2RGB_SAT5U(r);
 909     g = ((odd >> 1) + y - c_g) >> 2;  VP_STAGES_YUV2RGB_SAT6U(g);
 910     b = (odd + y + c_b) >> 3;     VP_STAGES_YUV2RGB_SAT5U(b);
 911     d[1] = (r << 11) | (g << 5) | (b << 0);
 912
 913     s[0] = 0U;
 914     s[1] = 0U;
 915     s += 2;
 916     d += 2;
 917   }
 918 #else
 919   asm_yuvrgb565(dst, dst_rbytes, cfg->vline, y);
 920 //static void flushline_565(uint16_t* dst, int32_t dst_rbytes, uint32_t* src, int y);
 921 #endif
 922 }
 923
 924 /******************************************************************************/
 925 #endif  // < USE_YUV2RGB_STRETCH
 926
 927
 928 C_RESULT vp_stages_yuv2rgb_stage_open(vp_stages_yuv2rgb_config_t *cfg)
 929 {
 930   C_RESULT res = VP_SUCCESS;
 931
 932 #ifdef USE_YUV2RGB_STRETCH
 933   if(VP_FAILED(vp_stages_yuv2rgb_open(cfg))) {
 934     res = VP_FAILURE;
 935   }
 936 #endif  // < USE_YUV2RGB_STRETCH
 937
 938   return res;
 939 }
 940
 941
 942 C_RESULT vp_stages_yuv2rgb_stage_transform(vp_stages_yuv2rgb_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
 943 {
 944   uint32_t width, height;
 945   static uint32_t bytesPerPixel = 0;
 946   uint8_t *dst;
 947   vp_api_picture_t *picture;
 948
 949   vp_os_mutex_lock(&out->lock);
 950
 951   if( in->size > 0 )
 952   {
 953     picture = (vp_api_picture_t *) in->buffers; //&in->buffers[in->indexBuffer];
 954
 955 #ifndef QCIF_TO_QVGA
 956     width = picture->width;
 957     height = picture->height;
 958 #else   // QCIF_TO_QVGA
 959     width = QVGA_WIDTH;
 960     height = QVGA_HEIGHT;
 961 #endif  // <  QCIF_TO_QVGA
 962
 963     if(out->status == VP_API_STATUS_INIT)
 964     {
 965       switch(cfg->rgb_format)
 966       {
 967         case VP_STAGES_RGB_FORMAT_RGB565:
 968           bytesPerPixel = 2;
 969 #ifndef QCIF_TO_QVGA
 970           vp_stages_YUV_to_RGB = vp_stages_YUV420P_to_RGB565;
 971 #else   // QCIF_TO_QVGA
 972 # ifndef USE_YUV2RGB_STRETCH
 973           vp_stages_YUV_to_RGB = vp_stages_YUV420P_to_RGB565_QCIF_to_QVGA;
 974 # else   // USE_YUV2RGB_STRETCH
 975           vp_stages_YUV_to_RGB = vp_stages_YUV420P_to_RGB565_QCIF_to_QVGA_stretch;
 976 # endif  // <  USE_YUV2RGB_STRETCH
 977 #endif  // <  QCIF_TO_QVGA
 978           break;
 979
 980         case VP_STAGES_RGB_FORMAT_RGB24:
 981           bytesPerPixel = 3;
 982 #ifndef QCIF_TO_QVGA
 983           vp_stages_YUV_to_RGB = vp_stages_YUV420P_to_RGB24;
 984 #else   // QCIF_TO_QVGA
 985           vp_stages_YUV_to_RGB = vp_stages_YUV420P_to_RGB24_QCIF_to_QVGA;
 986 #endif  // <  QCIF_TO_QVGA
 987           break;
 988
 989         case VP_STAGES_RGB_FORMAT_ARGB32:
 990           bytesPerPixel = 4;
 991 #ifndef QCIF_TO_QVGA
 992           vp_stages_YUV_to_RGB = vp_stages_YUV420P_to_ARGB32;
 993 #else   // QCIF_TO_QVGA
 994           vp_stages_YUV_to_RGB = vp_stages_YUV420P_to_ARGB32_QCIF_to_QVGA;
 995 #endif  // <  QCIF_TO_QVGA
 996           break;
 997
 998         default :
 999           vp_stages_YUV_to_RGB = NULL;
1000           break;
1001       }
1002
1003       VP_OS_ASSERT(vp_stages_YUV_to_RGB != NULL);
1004       VP_OS_ASSERT(bytesPerPixel != 0);
1005
1006       out->numBuffers   = 1;
1007       out->indexBuffer  = 0;
1008       out->size         = width * height * bytesPerPixel;
1009       out->buffers      = (int8_t **) vp_os_malloc(sizeof(int8_t *)+out->size*sizeof(int8_t));
1010       out->buffers[out->indexBuffer] = (int8_t *)(out->buffers+1);
1011       out->lineSize     = (int32_t *) vp_os_malloc(out->numBuffers * sizeof(int32_t *));
1012       out->lineSize[out->indexBuffer] = width*bytesPerPixel;
1013       vp_os_memset(out->buffers[out->indexBuffer], 0, out->size);
1014
1015       out->status = VP_API_STATUS_PROCESSING;
1016     } // No else
1017
1018     if( out->status == VP_API_STATUS_PROCESSING )
1019     {
1020       dst = (uint8_t*)out->buffers[out->indexBuffer];
1021
1022       vp_stages_YUV_to_RGB(cfg, picture, dst, width * bytesPerPixel);
1023       out->size = width * height * bytesPerPixel;
1024     } // No else
1025   }
1026   else
1027   {
1028     out->size = 0;
1029   }
1030
1031   if(in->status == VP_API_STATUS_STILL_RUNNING) {
1032     out->status = VP_API_STATUS_PROCESSING;
1033   }
1034   else {
1035     out->status = in->status;
1036   }
1037
1038   vp_os_mutex_unlock(&out->lock);
1039
1040   return (VP_SUCCESS);
1041 }
1042
1043
1044 C_RESULT vp_stages_yuv2rgb_stage_close(vp_stages_yuv2rgb_config_t *cfg)
1045 {
1046   C_RESULT res = VP_SUCCESS;
1047
1048 #ifdef USE_YUV2RGB_STRETCH
1049   if(VP_FAILED(vp_stages_yuv2rgb_close(cfg))) {
1050     res = VP_FAILURE;
1051   }
1052 #endif  // < USE_YUV2RGB_STRETCH
1053
1054   return res;
1055 }