X-Git-Url: http://git.maemo.org/git/?a=blobdiff_plain;ds=sidebyside;f=mardrone%2FARDrone_SDK_Version_1_8_20110726%2FARDroneLib%2FVP_SDK%2FVP_Stages%2Fvp_stages_yuv2rgb.c;fp=mardrone%2FARDrone_SDK_Version_1_8_20110726%2FARDroneLib%2FVP_SDK%2FVP_Stages%2Fvp_stages_yuv2rgb.c;h=55347acd9f37e09758f13a3a3fb86b08bb77aa34;hb=9ec9bc13b75d30bc45535c54a652934debfcea92;hp=0000000000000000000000000000000000000000;hpb=ae0a3c2dc0898400aca0dd6b439c5db8044db7b2;p=mardrone diff --git a/mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/VP_SDK/VP_Stages/vp_stages_yuv2rgb.c b/mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/VP_SDK/VP_Stages/vp_stages_yuv2rgb.c new file mode 100644 index 0000000..55347ac --- /dev/null +++ b/mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/VP_SDK/VP_Stages/vp_stages_yuv2rgb.c @@ -0,0 +1,1055 @@ +/** + * @file vp_stages_yuv2rgb.c + * @brief VP Stages. YUV to RGB converter stage declaration +*/ + +#ifdef _INCLUDED_FOR_DOXYGEN_ +#else // ! _INCLUDED_FOR_DOXYGEN_ + +/////////////////////////////////////////////// +// INCLUDES + +#include +#include +#include +#include +#include +#include + +#endif // < _INCLUDED_FOR_DOXYGEN_ + + +#ifdef USE_YUV2RGB_STRETCH + +static C_RESULT vp_stages_yuv2rgb_open(vp_stages_yuv2rgb_config_t *cfg); +static C_RESULT vp_stages_yuv2rgb_close(vp_stages_yuv2rgb_config_t *cfg); +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); +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); +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); +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); + + +#define SRC_WIDTH 176 +#define SRC_HEIGHT 144 +#define DST_WIDTH 320 +#define DST_HEIGHT 240 + +/******************************************************************************/ +/* */ +/* 176*144 (YCbCr, 4:2:0) -> 320*240 (RGB-16bits, 5:6:5) */ +/* */ +/******************************************************************************/ + +static int8_t htable[SRC_WIDTH] = +{ + 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, + 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, + 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, + 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, + 0x0F, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0F, 0x0E, 0x0F, 0x0E, + 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, + 0x0E, 0x0F, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0F, 0x0E, 0x0F, + 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, + 0x0F, 0x0E, 0x0F, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0F, 0x0E, + 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, + 0x0E, 0x0F, 0x0E, 0x0F, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0E, 0x0F, 0x0F +}; + +static int8_t vtable[SRC_HEIGHT] = +{ + 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, + 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, + 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, + 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, + 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, + 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, + 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, + 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, + 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E, 0x0D, 0x0D, 0x0E +}; + +/******************************************************************************/ + +#endif // < USE_YUV2RGB_STRETCH + + +/** + * @def VP_STAGES_YUV2RGB_LIMIT(x) + * @brief Macro used for ARGB32 saturation + * + * Moves x in a limited range of values (between 0 and 0xff). + */ +//#define VP_STAGES_YUV2ARGB_LIMIT(x) ( (x) > 0xffff ? 0xff : ( (x) <= 0xff ? 0 : ( (x) >> 8 ) ) ) +#define VP_STAGES_YUV2ARGB_LIMIT(dst, x) \ + dst = x; \ + dst = (dst > 0 ? ( ((dst) >> 8) > 0xff ? 0xff : ((dst) >> 8) ) : 0); + +/** + * @def VP_STAGES_YUV2RGB_SAT5U(a) + * @brief 5 bits saturation + */ +#define VP_STAGES_YUV2RGB_SAT5U(a) \ + if((a) < 0) (a) = 0; \ + else if((a) > 0x1F) (a) = 0x1F; + +/** + * @def VP_STAGES_YUV2RGB_SAT6U(a) + * @brief 6 bits saturation + */ +#define VP_STAGES_YUV2RGB_SAT6U(a) \ + if((a) < 0) (a) = 0; \ + else if((a) > 0x3F) (a) = 0x3F; + +/** Pointer to a YUV to RGB conversion function + */ +static vp_stages_YUV_to_RGB_t vp_stages_YUV_to_RGB; + +#ifndef QCIF_TO_QVGA +/** YUV to RGB conversion functions + */ + void vp_stages_YUV420P_to_RGB565(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes); + void vp_stages_YUV420P_to_RGB24(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes); + void vp_stages_YUV420P_to_ARGB32(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes); +#else // ! QCIF_TO_QVGA +/** YUV to RGB conversion + resizing + */ +#ifdef USE_YUV2RGB_STRETCH + 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); +#else // ! USE_YUV2RGB_STRETCH + 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); + 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); + 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); +#endif // < USE_YUV2RGB_STRETCH +#endif // < QCIF_TO_QVGA + + +#ifndef QCIF_TO_QVGA +void vp_stages_YUV420P_to_RGB565(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes) +{ + uint32_t w, width; + uint32_t h, height; + + width = picture->width >> 1; + height = picture->height >> 1; + + for (h = 0; h < height; h++) + { + uint8_t* s0; + uint8_t* s1; + uint8_t* s2; + uint16_t* d0; + uint16_t* d1; + + s0 = picture->y_buf + (h * picture->y_line_size * 2); + s1 = picture->cb_buf + (h * picture->cb_line_size); + s2 = picture->cr_buf + (h * picture->cr_line_size); + d0 = (uint16_t*) (dst + (h * dst_rbytes * 2)); + d1 = (uint16_t*) (dst + (h * dst_rbytes * 2) + dst_rbytes); + + for (w = 0; w < width; w++) + { + int32_t y, cb, cr; + int32_t c_r, c_g, c_b; + int16_t r, g, b; + + cb = ((uint32_t) *(s1++)) - 0x80L; + cr = ((uint32_t) *(s2++)) - 0x80L; + c_r = cr * 359L; + c_g = (cb * -88L) + (cr * -183L); + c_b = cb * 454L; + + y = ((uint32_t) s0[0]) << 8; + r = (int16_t) ((y + c_r + 0x000) >> 11); VP_STAGES_YUV2RGB_SAT5U(r); + g = (int16_t) ((y + c_g + 0x000) >> 10); VP_STAGES_YUV2RGB_SAT6U(g); + b = (int16_t) ((y + c_b + 0x000) >> 11); VP_STAGES_YUV2RGB_SAT5U(b); + d0[0] = (r << 11) | (g << 5) | (b << 0); + + y = ((uint32_t) s0[1]) << 8; + r = (int16_t) ((y + c_r + 0x400) >> 11); VP_STAGES_YUV2RGB_SAT5U(r); + g = (int16_t) ((y + c_g + 0x200) >> 10); VP_STAGES_YUV2RGB_SAT6U(g); + b = (int16_t) ((y + c_b + 0x400) >> 11); VP_STAGES_YUV2RGB_SAT5U(b); + d0[1] = (r << 11) | (g << 5) | (b << 0); + + y = ((uint32_t) s0[picture->y_line_size + 0]) << 8; + r = (int16_t) ((y + c_r + 0x600) >> 11); VP_STAGES_YUV2RGB_SAT5U(r); + g = (int16_t) ((y + c_g + 0x300) >> 10); VP_STAGES_YUV2RGB_SAT6U(g); + b = (int16_t) ((y + c_b + 0x600) >> 11); VP_STAGES_YUV2RGB_SAT5U(b); + d1[0] = (r << 11) | (g << 5) | (b << 0); + + y = ((uint32_t) s0[picture->y_line_size + 1]) << 8; + r = (int16_t) ((y + c_r + 0x200) >> 11); VP_STAGES_YUV2RGB_SAT5U(r); + g = (int16_t) ((y + c_g + 0x100) >> 10); VP_STAGES_YUV2RGB_SAT6U(g); + b = (int16_t) ((y + c_b + 0x200) >> 11); VP_STAGES_YUV2RGB_SAT5U(b); + d1[1] = (r << 11) | (g << 5) | (b << 0); + + s0 += 2; + d0 += 2; + d1 += 2; + } + } +} +#endif // < QCIF_TO_QVGA + + +#ifndef QCIF_TO_QVGA +void vp_stages_YUV420P_to_RGB24(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes) +{ + uint32_t width, height; + int32_t line, col, linewidth; + int32_t y, u, v, r, g, b; + int32_t vr, ug, vg, ub; + uint8_t *py, *pu, *pv; + + int32_t lineSz0 = picture->y_line_size; + int32_t lineSz1 = picture->cb_line_size; + int32_t lineSz2 = picture->cr_line_size; + + width = picture->width; + height = picture->height; + + linewidth = width - (width >> 1); + + if(cfg->mode == VP_STAGES_YUV2RGB_MODE_UPSIDE_DOWN) + { + py = picture->y_buf+lineSz0*(height-1)+width-1; + pu = picture->cb_buf+lineSz1*((height>>1)-1)+((width>>1)-1); + pv = picture->cr_buf+lineSz2*((height>>1)-1)+((width>>1)-1); + + for (line = height-1; line >= 0; line--) { + for (col = width-1; col >= 0; col--) { + y = *py; + y = y << 8; + u = *pu - 128; + ug = 88 * u; + ub = 454 * u; + v = *pv - 128; + vg = 183 * v; + vr = 359 * v; + + VP_STAGES_YUV2ARGB_LIMIT(r, y + vr); + VP_STAGES_YUV2ARGB_LIMIT(g, y - ug - vg); + VP_STAGES_YUV2ARGB_LIMIT(b, y + ub ); + + *dst = r; + dst++; + *dst = g; + dst++; + *dst = b; + dst++; + + py--; + + if (col & 1) + { + pu--; + pv--; + } // No else + } + + pu += linewidth; + pv += linewidth; + + if (line & 1) + { + pu -= lineSz1; + pv -= lineSz2; + } // No else + py -= lineSz0 - width; + } + } + else + { + py = picture->y_buf; + pu = picture->cb_buf; + pv = picture->cr_buf; + + for (line = 0; line < (int32_t)height; line++) { + for (col = 0; col < (int32_t)width; col++) { + y = *py; + y = y << 8; + u = *pu - 128; + ug = 88 * u; + ub = 454 * u; + v = *pv - 128; + vg = 183 * v; + vr = 359 * v; + + VP_STAGES_YUV2ARGB_LIMIT(r, y + vr); + VP_STAGES_YUV2ARGB_LIMIT(g, y - ug - vg); + VP_STAGES_YUV2ARGB_LIMIT(b, y + ub ); + + *dst = r; + dst++; + *dst = g; + dst++; + *dst = b; + dst++; + + py++; + + if (col & 1) + { + pu++; + pv++; + } // No else + } + + pu -= linewidth; + pv -= linewidth; + + if (line & 1) + { + pu += lineSz1; + pv += lineSz2; + } // No else + py += lineSz0 - width; + } + } +} +#endif // < QCIF_TO_QVGA + + +#ifndef QCIF_TO_QVGA +void vp_stages_YUV420P_to_ARGB32(vp_stages_yuv2rgb_config_t *cfg, vp_api_picture_t *picture, uint8_t *dst, uint32_t dst_rbytes) +{ + uint32_t width, height; + int32_t line, col, linewidth; + int32_t y, u, v, r, g, b; + int32_t vr, ug, vg, ub; + uint8_t *py, *pu, *pv; + + int32_t lineSz0 = picture->y_line_size; + int32_t lineSz1 = picture->cb_line_size; + int32_t lineSz2 = picture->cr_line_size; + + width = picture->width; + height = picture->height; + + linewidth = width - (width >> 1); + + py = picture->y_buf; + pu = picture->cb_buf; + pv = picture->cr_buf; + + for (line = 0; line < (int32_t)height; line++) { + for (col = 0; col < (int32_t)width; col++) { + y = *py; + y = y << 8; + u = *pu - 128; + ug = 88 * u; + ub = 454 * u; + v = *pv - 128; + vg = 183 * v; + vr = 359 * v; + + VP_STAGES_YUV2ARGB_LIMIT(r, y + vr); + VP_STAGES_YUV2ARGB_LIMIT(g, y - ug - vg); + VP_STAGES_YUV2ARGB_LIMIT(b, y + ub); + + *dst = b; + dst++; + *dst = g; + dst++; + *dst = r; + dst+=2; //skip Alpha value + + py++; + + if (col & 1) + { + pu++; + pv++; + } // No else + } + + pu -= linewidth; + pv -= linewidth; + + if (line & 1) + { + pu += lineSz1; + pv += lineSz2; + } // No else + py += lineSz0 - width; + } +} +#endif // < QCIF_TO_QVGA + + +#ifdef QCIF_TO_QVGA +#ifdef USE_YUV2RGB_STRETCH +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) +{ + int h; + uint8_t* y; + uint8_t* cb; + uint8_t* cr; + int dy; + int8_t vfrac; + + vp_os_memset(cfg->vline, 0, dst_rbytes); + + y = picture->y_buf; + cb = picture->cb_buf; + cr = picture->cr_buf; + vfrac = 8; + dy = 0; + for (h = 0; h < SRC_HEIGHT; h += 2) + { + vp_stages_yuv2rgb_hstretch(y, picture->y_line_size, cb, cr, cfg->hline0, cfg->hline1); + vp_stages_yuv2rgb_vstretch(cfg, picture, dst, dst_rbytes, vtable[h + 0], &vfrac, cfg->hline0, &dy); + vp_stages_yuv2rgb_vstretch(cfg, picture, dst, dst_rbytes, vtable[h + 1], &vfrac, cfg->hline1, &dy); + y += (picture->y_line_size * 2); + cb += picture->cb_line_size; + cr += picture->cr_line_size; + } +} +#endif // < USE_YUV2RGB_STRETCH +#endif // < QCIF_TO_QVGA + + +#ifdef QCIF_TO_QVGA +#ifndef USE_YUV2RGB_STRETCH +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) +{ + uint32_t w, width; + uint32_t h, height; + uint8_t* s0; + uint8_t* s1; + uint8_t* s2; + uint16_t* d0; + uint16_t* d1; + int32_t y, cb, cr; + int32_t c_r, c_g, c_b; + int16_t r, g, b; + + width = QVGA_WIDTH >> 2; + height = QVGA_HEIGHT >> 2; + + for (h = 0; h < height; h++) + { + s0 = picture->y_buf + ((h * picture->y_line_size)<<1); + s1 = picture->cb_buf + (h * picture->cb_line_size); + s2 = picture->cr_buf + (h * picture->cr_line_size); + d0 = (uint16_t*) (dst + ((h * QVGA_WIDTH) << 3)); + d1 = (uint16_t*) (dst + ((h * QVGA_WIDTH) << 3) + (QVGA_WIDTH << 2)); + + for (w = 0; w < width; w++) + { + cb = ((uint32_t) *(s1++)) - 0x80L; + cr = ((uint32_t) *(s2++)) - 0x80L; + c_r = cr * 359L; + c_g = (cb * -88L) + (cr * -183L); + c_b = cb * 454L; + + y = ((uint32_t) s0[0]) << 8; + r = (int16_t) ((y + c_r + 0x000) >> 11); VP_STAGES_YUV2RGB_SAT5U(r); + g = (int16_t) ((y + c_g + 0x000) >> 10); VP_STAGES_YUV2RGB_SAT6U(g); + b = (int16_t) ((y + c_b + 0x000) >> 11); VP_STAGES_YUV2RGB_SAT5U(b); + d0[0] = (r << 11) | (g << 5) | (b << 0); + d0[1] = d0[0]; + + y = ((uint32_t) s0[1]) << 8; + r = (int16_t) ((y + c_r + 0x400) >> 11); VP_STAGES_YUV2RGB_SAT5U(r); + g = (int16_t) ((y + c_g + 0x200) >> 10); VP_STAGES_YUV2RGB_SAT6U(g); + b = (int16_t) ((y + c_b + 0x400) >> 11); VP_STAGES_YUV2RGB_SAT5U(b); + d0[2] = (r << 11) | (g << 5) | (b << 0); + d0[3] = d0[2]; + + y = ((uint32_t) s0[picture->y_line_size + 0]) << 8; + r = (int16_t) ((y + c_r + 0x600) >> 11); VP_STAGES_YUV2RGB_SAT5U(r); + g = (int16_t) ((y + c_g + 0x300) >> 10); VP_STAGES_YUV2RGB_SAT6U(g); + b = (int16_t) ((y + c_b + 0x600) >> 11); VP_STAGES_YUV2RGB_SAT5U(b); + d1[0] = (r << 11) | (g << 5) | (b << 0); + d1[1] = d1[0]; + + y = ((uint32_t) s0[picture->y_line_size + 1]) << 8; + r = (int16_t) ((y + c_r + 0x200) >> 11); VP_STAGES_YUV2RGB_SAT5U(r); + g = (int16_t) ((y + c_g + 0x100) >> 10); VP_STAGES_YUV2RGB_SAT6U(g); + b = (int16_t) ((y + c_b + 0x200) >> 11); VP_STAGES_YUV2RGB_SAT5U(b); + d1[2] = (r << 11) | (g << 5) | (b << 0); + d1[3] = d1[2]; + + s0 += 2; + d0 += 4; + d1 += 4; + } + vp_os_memcpy(dst + ((h * QVGA_WIDTH) << 3) + (QVGA_WIDTH<<1), dst + ((h * QVGA_WIDTH) << 3), QVGA_WIDTH<<1); + vp_os_memcpy(dst + ((h * QVGA_WIDTH) << 3) + (QVGA_WIDTH<<1)*3, dst + ((h * QVGA_WIDTH) << 3) + (QVGA_WIDTH<<2), QVGA_WIDTH<<1); + } +} +#endif // < USE_YUV2RGB_STRETCH +#endif // < QCIF_TO_QVGA + + +#ifdef QCIF_TO_QVGA +#ifndef USE_YUV2RGB_STRETCH +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) +{ +} +#endif // < USE_YUV2RGB_STRETCH +#endif // < QCIF_TO_QVGA + + +#ifdef QCIF_TO_QVGA +#ifndef USE_YUV2RGB_STRETCH +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) +{ + uint32_t width, height; + int32_t line, col, linewidth; + int32_t y, u, v, r, g, b; + int32_t vr, ug, vg, ub; + uint8_t *py, *pu, *pv; + + int32_t lineSz0 = picture->y_line_size; + int32_t lineSz1 = picture->cb_line_size; + int32_t lineSz2 = picture->cr_line_size; + + width = picture->width; + height = picture->height; + + linewidth = width - (width >> 1); + + py = picture->y_buf; + pu = picture->cb_buf; + pv = picture->cr_buf; + + for (line = 0; line < (QVGA_HEIGHT >> 1); line++) { + for (col = 0; col < (QVGA_WIDTH >> 1); col++) { + y = *py; + y = y << 8; + u = *pu - 128; + ug = 88 * u; + ub = 454 * u; + v = *pv - 128; + vg = 183 * v; + vr = 359 * v; + + VP_STAGES_YUV2ARGB_LIMIT(r, y + vr); + VP_STAGES_YUV2ARGB_LIMIT(g, y - ug - vg); + VP_STAGES_YUV2ARGB_LIMIT(b, y + ub); + + *dst = b; + dst++; + *dst = g; + dst++; + *dst = r; + dst+=2; //skip Alpha value + *dst = b; + dst++; + *dst = g; + dst++; + *dst = r; + dst+=2; + + py++; + + if (col & 1) + { + pu++; + pv++; + } // No else + } + + pu -= (QVGA_WIDTH >> 2); + pv -= (QVGA_WIDTH >> 2); + + if (line & 1) + { + pu += lineSz1; + pv += lineSz2; + } // No else + py += lineSz0 - (QVGA_WIDTH >> 1); + + vp_os_memcpy(dst, dst - (QVGA_WIDTH<<2), QVGA_WIDTH<<2); + dst += (QVGA_WIDTH<<2); + } +} +#endif // < USE_YUV2RGB_STRETCH +#endif // < QCIF_TO_QVGA + + +/******************************************************************************/ + +#ifdef USE_YUV2RGB_STRETCH +static C_RESULT vp_stages_yuv2rgb_open(vp_stages_yuv2rgb_config_t *cfg) +{ + C_RESULT res = VP_SUCCESS; + + cfg->hline0 = (uint32_t*) vp_os_malloc(DST_WIDTH * sizeof(uint32_t)); + cfg->hline1 = (uint32_t*) vp_os_malloc(DST_WIDTH * sizeof(uint32_t)); + cfg->vline = (uint32_t*) vp_os_malloc(DST_WIDTH * sizeof(uint32_t)); + if (cfg->hline0 && cfg->hline1 && cfg->vline) + { +// vfrac = 8; + } + else + { + vp_stages_yuv2rgb_close(cfg); + res = VP_FAILURE; + } + + return res; +} + +/******************************************************************************/ + +static C_RESULT vp_stages_yuv2rgb_close(vp_stages_yuv2rgb_config_t *cfg) +{ + if (cfg->hline0) + { + vp_os_free(cfg->hline0); + cfg->hline0 = NULL; + } + if (cfg->hline1) + { + vp_os_free(cfg->hline1); + cfg->hline1 = NULL; + } + if (cfg->vline) + { + vp_os_free(cfg->vline); + cfg->vline = NULL; + } + return(VP_SUCCESS); +} + +/******************************************************************************/ + +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) +{ + int8_t f; + int i; + + f = *frac; + i = *dx; + do + { + int8_t c; + + if (f < cov) + { + c = f; + } + else + { + c = cov; + } + cov -= c; + f -= c; + switch (c) + { + case 0: + assert(0); + break; + + case 1: + line0[i] += (sample0 >> 3) & 0x1F1F1F1FUL; + line1[i] += (sample1 >> 3) & 0x1F1F1F1FUL; + break; + + case 2: + line0[i] += (sample0 >> 2) & 0x3F3F3F3FUL; + line1[i] += (sample1 >> 2) & 0x3F3F3F3FUL; + break; + + case 3: + line0[i] += ((sample0 >> 1) & 0x7F7F7F7FUL) - ((sample0 >> 3) & 0x1F1F1F1FUL); + line1[i] += ((sample1 >> 1) & 0x7F7F7F7FUL) - ((sample1 >> 3) & 0x1F1F1F1FUL); + break; + + case 4: + line0[i] += (sample0 >> 1) & 0x7F7F7F7FUL; + line1[i] += (sample1 >> 1) & 0x7F7F7F7FUL; + break; + + case 5: + line0[i] += ((sample0 >> 1) & 0x7F7F7F7FUL) + ((sample0 >> 3) & 0x1F1F1F1FUL); + line1[i] += ((sample1 >> 1) & 0x7F7F7F7FUL) + ((sample1 >> 3) & 0x1F1F1F1FUL); + break; + + case 6: + line0[i] += sample0 - ((sample0 >> 2) & 0x3F3F3F3FUL); + line1[i] += sample1 - ((sample1 >> 2) & 0x3F3F3F3FUL); + break; + + case 7: + line0[i] += sample0 - ((sample0 >> 3) & 0x1F1F1F1FUL); + line1[i] += sample1 - ((sample1 >> 3) & 0x1F1F1F1FUL); + break; + + case 8: + line0[i] += sample0; + line1[i] += sample1; + break; + } + + if (f == 0) + { + f = 8; + i++; + if (i < DST_WIDTH) + { + line0[i] = 0UL; + line1[i] = 0UL; + } + } + } while (cov); + *dx = i; + *frac = f; +} + +/*----------------------------------------------------------------------------*/ + +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) +{ + int sx, dx; + int8_t frac; + uint8_t* s0; + uint8_t* s1; + uint8_t* s2; + + frac = 8; + dx = 0; + line0[0] = 0UL; + line1[0] = 0UL; + s0 = y_base; + s1 = cb_base; + s2 = cr_base; + for (sx = 0; sx < 176; sx += 2) + { + int32_t cb, cr; + uint32_t cr_cg_cb, smp0, smp1; + + cb = (int32_t) ((uint32_t) *(s1++)); + cr = (int32_t) ((uint32_t) *(s2++)); + cr_cg_cb = ((cr * 180L) << 8) & 0xFF0000UL; + cr_cg_cb |= ((cb * 44L) + (cr * 91L)) & 0xFF00UL; + cr_cg_cb |= ((cb * 227L) >> 8) & 0xFFUL; + + smp0 = cr_cg_cb | (((uint32_t) s0[0]) << 24); + smp1 = cr_cg_cb | (((uint32_t) s0[y_rbytes + 0]) << 24); + vp_stages_yuv2rgb_hresample(smp0, smp1, htable[sx], &frac, line0, line1, &dx); + + smp0 = cr_cg_cb | (((uint32_t) s0[1]) << 24); + smp1 = cr_cg_cb | (((uint32_t) s0[y_rbytes + 1]) << 24); + vp_stages_yuv2rgb_hresample(smp0, smp1, htable[sx], &frac, line0, line1, &dx); + s0 += 2; + } +} + +/*----------------------------------------------------------------------------*/ + +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) +{ + int8_t f; + uint32_t* src; + uint32_t* dst; + int y; + + f = *frac; + y = *dy; + do + { + int8_t c; + + if (f < cov) + { + c = f; + } + else + { + c = cov; + } + cov -= c; + f -= c; + src = line; + dst = cfg->vline; + switch (c) + { + int dx; + + case 8: + for (dx = 0; dx < DST_WIDTH; dx++) + { + uint32_t ycc; + + ycc = src[dx]; + dst[dx] = ycc; + } + break; + + case 7: + for (dx = 0; dx < DST_WIDTH; dx++) + { + uint32_t ycc; + + ycc = src[dx]; + dst[dx] += (ycc) - ((ycc >> 3) & 0x1F1F1F1FUL); + } + break; + + case 6: + for (dx = 0; dx < DST_WIDTH; dx++) + { + uint32_t ycc; + + ycc = src[dx]; + dst[dx] += (ycc) - ((ycc >> 2) & 0x3F3F3F3FUL); + } + break; + + case 5: + for (dx = 0; dx < DST_WIDTH; dx++) + { + uint32_t ycc; + + ycc = src[dx]; + dst[dx] += ((ycc >> 1) & 0x7F7F7F7FUL) + ((ycc >> 3) & 0x1F1F1F1FUL); + } + break; + + case 4: + for (dx = 0; dx < DST_WIDTH; dx++) + { + uint32_t ycc; + + ycc = src[dx]; + dst[dx] += ((ycc >> 1) & 0x7F7F7F7FUL); + } + break; + + case 3: + for (dx = 0; dx < DST_WIDTH; dx++) + { + uint32_t ycc; + + ycc = src[dx]; + dst[dx] += ((ycc >> 1) & 0x7F7F7F7FUL) - ((ycc >> 3) & 0x1F1F1F1FUL); + } + break; + + case 2: + for (dx = 0; dx < DST_WIDTH; dx++) + { + uint32_t ycc; + + ycc = src[dx]; + dst[dx] += ((ycc >> 2) & 0x3F3F3F3FUL); + } + break; + + case 1: + for (dx = 0; dx < DST_WIDTH; dx++) + { + uint32_t ycc; + + ycc = src[dx]; + dst[dx] += ((ycc >> 3) & 0x1F1F1F1FUL); + } + break; + + case 0: + assert(0); + break; + } + + if (f == 0) + { + vp_stages_yuv2rgb_flushline_565(cfg, picture, dst_base, dst_rbytes, y++); + f = 8; + } + } while (cov); + + *dy = y; + *frac = f; +} + +/*----------------------------------------------------------------------------*/ + +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) +{ +#ifdef __linux__ + uint32_t* s; + uint16_t* d; + int i, even, odd; + + s = cfg->vline; + d = (uint16_t*) (dst + (dst_rbytes * y)); + even = 0; + odd = 4; + if (y & 1) + { + even ^= 6; + odd ^= 6; + } + for (i = 0; i < DST_WIDTH; i += 2) + { + int32_t ycc; + int32_t y; + int32_t r, g, b; + int32_t c_r, c_g, c_b; + + ycc = s[0]; + y = ((uint32_t) ycc) >> 24; + c_r = ycc << 8; c_r = ((uint32_t) c_r) >> 23; c_r -= 180; + c_g = ycc << 16; c_g = ((uint32_t) c_g) >> 23; c_g -= 135; + c_b = ycc << 24; c_b = ((uint32_t) c_b) >> 23; c_b -= 227; + r = (even + y + c_r) >> 3; VP_STAGES_YUV2RGB_SAT5U(r); + g = ((even >> 1) + y - c_g) >> 2; VP_STAGES_YUV2RGB_SAT6U(g); + b = (even + y + c_b) >> 3; VP_STAGES_YUV2RGB_SAT5U(b); + d[0] = (r << 11) | (g << 5) | (b << 0); + + ycc = s[1]; + y = ((uint32_t) ycc) >> 24; + c_r = ycc << 8; c_r = ((uint32_t) c_r) >> 23; c_r -= 180; + c_g = ycc << 16; c_g = ((uint32_t) c_g) >> 23; c_g -= 135; + c_b = ycc << 24; c_b = ((uint32_t) c_b) >> 23; c_b -= 227; + r = (odd + y + c_r) >> 3; VP_STAGES_YUV2RGB_SAT5U(r); + g = ((odd >> 1) + y - c_g) >> 2; VP_STAGES_YUV2RGB_SAT6U(g); + b = (odd + y + c_b) >> 3; VP_STAGES_YUV2RGB_SAT5U(b); + d[1] = (r << 11) | (g << 5) | (b << 0); + + s[0] = 0U; + s[1] = 0U; + s += 2; + d += 2; + } +#else + asm_yuvrgb565(dst, dst_rbytes, cfg->vline, y); +//static void flushline_565(uint16_t* dst, int32_t dst_rbytes, uint32_t* src, int y); +#endif +} + +/******************************************************************************/ +#endif // < USE_YUV2RGB_STRETCH + + +C_RESULT vp_stages_yuv2rgb_stage_open(vp_stages_yuv2rgb_config_t *cfg) +{ + C_RESULT res = VP_SUCCESS; + +#ifdef USE_YUV2RGB_STRETCH + if(VP_FAILED(vp_stages_yuv2rgb_open(cfg))) { + res = VP_FAILURE; + } +#endif // < USE_YUV2RGB_STRETCH + + return res; +} + + +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) +{ + uint32_t width, height; + static uint32_t bytesPerPixel = 0; + uint8_t *dst; + vp_api_picture_t *picture; + + vp_os_mutex_lock(&out->lock); + + if( in->size > 0 ) + { + picture = (vp_api_picture_t *) in->buffers; //&in->buffers[in->indexBuffer]; + +#ifndef QCIF_TO_QVGA + width = picture->width; + height = picture->height; +#else // QCIF_TO_QVGA + width = QVGA_WIDTH; + height = QVGA_HEIGHT; +#endif // < QCIF_TO_QVGA + + if(out->status == VP_API_STATUS_INIT) + { + switch(cfg->rgb_format) + { + case VP_STAGES_RGB_FORMAT_RGB565: + bytesPerPixel = 2; +#ifndef QCIF_TO_QVGA + vp_stages_YUV_to_RGB = vp_stages_YUV420P_to_RGB565; +#else // QCIF_TO_QVGA +# ifndef USE_YUV2RGB_STRETCH + vp_stages_YUV_to_RGB = vp_stages_YUV420P_to_RGB565_QCIF_to_QVGA; +# else // USE_YUV2RGB_STRETCH + vp_stages_YUV_to_RGB = vp_stages_YUV420P_to_RGB565_QCIF_to_QVGA_stretch; +# endif // < USE_YUV2RGB_STRETCH +#endif // < QCIF_TO_QVGA + break; + + case VP_STAGES_RGB_FORMAT_RGB24: + bytesPerPixel = 3; +#ifndef QCIF_TO_QVGA + vp_stages_YUV_to_RGB = vp_stages_YUV420P_to_RGB24; +#else // QCIF_TO_QVGA + vp_stages_YUV_to_RGB = vp_stages_YUV420P_to_RGB24_QCIF_to_QVGA; +#endif // < QCIF_TO_QVGA + break; + + case VP_STAGES_RGB_FORMAT_ARGB32: + bytesPerPixel = 4; +#ifndef QCIF_TO_QVGA + vp_stages_YUV_to_RGB = vp_stages_YUV420P_to_ARGB32; +#else // QCIF_TO_QVGA + vp_stages_YUV_to_RGB = vp_stages_YUV420P_to_ARGB32_QCIF_to_QVGA; +#endif // < QCIF_TO_QVGA + break; + + default : + vp_stages_YUV_to_RGB = NULL; + break; + } + + VP_OS_ASSERT(vp_stages_YUV_to_RGB != NULL); + VP_OS_ASSERT(bytesPerPixel != 0); + + out->numBuffers = 1; + out->indexBuffer = 0; + out->size = width * height * bytesPerPixel; + out->buffers = (int8_t **) vp_os_malloc(sizeof(int8_t *)+out->size*sizeof(int8_t)); + out->buffers[out->indexBuffer] = (int8_t *)(out->buffers+1); + out->lineSize = (int32_t *) vp_os_malloc(out->numBuffers * sizeof(int32_t *)); + out->lineSize[out->indexBuffer] = width*bytesPerPixel; + vp_os_memset(out->buffers[out->indexBuffer], 0, out->size); + + out->status = VP_API_STATUS_PROCESSING; + } // No else + + if( out->status == VP_API_STATUS_PROCESSING ) + { + dst = (uint8_t*)out->buffers[out->indexBuffer]; + + vp_stages_YUV_to_RGB(cfg, picture, dst, width * bytesPerPixel); + out->size = width * height * bytesPerPixel; + } // No else + } + else + { + out->size = 0; + } + + if(in->status == VP_API_STATUS_STILL_RUNNING) { + out->status = VP_API_STATUS_PROCESSING; + } + else { + out->status = in->status; + } + + vp_os_mutex_unlock(&out->lock); + + return (VP_SUCCESS); +} + + +C_RESULT vp_stages_yuv2rgb_stage_close(vp_stages_yuv2rgb_config_t *cfg) +{ + C_RESULT res = VP_SUCCESS; + +#ifdef USE_YUV2RGB_STRETCH + if(VP_FAILED(vp_stages_yuv2rgb_close(cfg))) { + res = VP_FAILURE; + } +#endif // < USE_YUV2RGB_STRETCH + + return res; +}