app/src/main/jni/libuvc/src/frame.c

   1 /*********************************************************************
   2  *********************************************************************/
   3 /*********************************************************************
   4  * added and modified some function for support and help for Android
   5  * Copyright (C) 2014 saki@serenegiant All rights reserved.
   6  * add:
   7  *      added some helper functions for supporting rgb565 and rgbx8888
   8  * modified:
   9  *      modified for optimization with gcc
  10  *      modified macros that convert pixel format to reduce cpu cycles
  11  *      added boundary check of pointer in the converting function to avoid crash
  12  *********************************************************************/
  13
  14 /*********************************************************************
  15  * Software License Agreement (BSD License)
  16  *
  17  *  Copyright (C) 2010-2012 Ken Tossell
  18  *  All rights reserved.
  19  *
  20  *  Redistribution and use in source and binary forms, with or without
  21  *  modification, are permitted provided that the following conditions
  22  *  are met:
  23  *
  24  *   * Redistributions of source code must retain the above copyright
  25  *     notice, this list of conditions and the following disclaimer.
  26  *   * Redistributions in binary form must reproduce the above
  27  *     copyright notice, this list of conditions and the following
  28  *     disclaimer in the documentation and/or other materials provided
  29  *     with the distribution.
  30  *   * Neither the name of the author nor other contributors may be
  31  *     used to endorse or promote products derived from this software
  32  *     without specific prior written permission.
  33  *
  34  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  35  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  36  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  37  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  38  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  39  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  40  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  41  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  42  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  43  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  44  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  45  *  POSSIBILITY OF SUCH DAMAGE.
  46  *********************************************************************/
  47 /**
  48  * @defgroup frame Frame processing
  49  * @brief Tools for managing frame buffers and converting between image formats
  50  */
  51 #include "libuvc/libuvc.h"
  52 #include "libuvc/libuvc_internal.h"
  53
  54 #define USE_STRIDE 1
  55 /** @internal */
  56 uvc_error_t uvc_ensure_frame_size(uvc_frame_t *frame, size_t need_bytes) {
  57         if LIKELY(frame->library_owns_data) {
  58                 if UNLIKELY(!frame->data || frame->data_bytes != need_bytes) {
  59                         frame->actual_bytes = frame->data_bytes = need_bytes;   // XXX
  60                         frame->data = realloc(frame->data, frame->data_bytes);
  61                 }
  62                 if (UNLIKELY(!frame->data || !need_bytes))
  63                         return UVC_ERROR_NO_MEM;
  64                 return UVC_SUCCESS;
  65         } else {
  66                 if (UNLIKELY(!frame->data || frame->data_bytes < need_bytes))
  67                         return UVC_ERROR_NO_MEM;
  68                 return UVC_SUCCESS;
  69         }
  70 }
  71
  72 /** @brief Allocate a frame structure
  73  * @ingroup frame
  74  *
  75  * @param data_bytes Number of bytes to allocate, or zero
  76  * @return New frame, or NULL on error
  77  */
  78 uvc_frame_t *uvc_allocate_frame(size_t data_bytes) {
  79         uvc_frame_t *frame = malloc(sizeof(*frame));    // FIXME using buffer pool is better performance(5-30%) than directory use malloc everytime.
  80
  81         if (UNLIKELY(!frame))
  82                 return NULL;
  83
  84 #ifndef __ANDROID__
  85         // XXX in many case, it is not neccesary to clear because all fields are set before use
  86         // therefore we remove this to improve performace, but be care not to forget to set fields before use
  87         memset(frame, 0, sizeof(*frame));       // bzero(frame, sizeof(*frame)); // bzero is deprecated
  88 #endif
  89 //      frame->library_owns_data = 1;   // XXX moved to lower
  90
  91         if (LIKELY(data_bytes > 0)) {
  92                 frame->library_owns_data = 1;
  93                 frame->actual_bytes = frame->data_bytes = data_bytes;   // XXX
  94                 frame->data = malloc(data_bytes);
  95
  96                 if (UNLIKELY(!frame->data)) {
  97                         free(frame);
  98                         return NULL ;
  99                 }
 100         }
 101
 102         return frame;
 103 }
 104
 105 /** @brief Free a frame structure
 106  * @ingroup frame
 107  *
 108  * @param frame Frame to destroy
 109  */
 110 void uvc_free_frame(uvc_frame_t *frame) {
 111         if ((frame->data_bytes > 0) && frame->library_owns_data)
 112                 free(frame->data);
 113
 114         free(frame);
 115 }
 116
 117 static inline unsigned char sat(int i) {
 118         return (unsigned char) (i >= 255 ? 255 : (i < 0 ? 0 : i));
 119 }
 120
 121 /** @brief Duplicate a frame, preserving color format
 122  * @ingroup frame
 123  *
 124  * @param in Original frame
 125  * @param out Duplicate frame
 126  */
 127 uvc_error_t uvc_duplicate_frame(uvc_frame_t *in, uvc_frame_t *out) {
 128         if (UNLIKELY(uvc_ensure_frame_size(out, in->data_bytes) < 0))
 129                 return UVC_ERROR_NO_MEM;
 130
 131         out->width = in->width;
 132         out->height = in->height;
 133         out->frame_format = in->frame_format;
 134         if (out->library_owns_data)
 135                 out->step = in->step;
 136         out->sequence = in->sequence;
 137         out->capture_time = in->capture_time;
 138         out->source = in->source;
 139         out->actual_bytes = in->actual_bytes;   // XXX
 140
 141 #if USE_STRIDE   // XXX
 142         if (in->step && out->step) {
 143                 const int istep = in->step;
 144                 const int ostep = out->step;
 145                 const int hh = in->height < out->height ? in->height : out->height;
 146                 const int rowbytes = istep < ostep ? istep : ostep;
 147                 register void *ip = in->data;
 148                 register void *op = out->data;
 149                 int h;
 150                 for (h = 0; h < hh; h += 4) {
 151                         memcpy(op, ip, rowbytes);
 152                         ip += istep; op += ostep;
 153                         memcpy(op, ip, rowbytes);
 154                         ip += istep; op += ostep;
 155                         memcpy(op, ip, rowbytes);
 156                         ip += istep; op += ostep;
 157                         memcpy(op, ip, rowbytes);
 158                         ip += istep; op += ostep;
 159                 }
 160         } else {
 161                 // compressed format? XXX if only one of the frame in / out has step, this may lead to crash...
 162                 memcpy(out->data, in->data, in->actual_bytes);
 163         }
 164 #else
 165         memcpy(out->data, in->data, in->actual_bytes); // XXX
 166 #endif
 167         return UVC_SUCCESS;
 168 }
 169
 170 #define PIXEL_RGB565            2
 171 #define PIXEL_UYVY                      2
 172 #define PIXEL_YUYV                      2
 173 #define PIXEL_RGB                       3
 174 #define PIXEL_BGR                       3
 175 #define PIXEL_RGBX                      4
 176
 177 #define PIXEL2_RGB565           PIXEL_RGB565 * 2
 178 #define PIXEL2_UYVY                     PIXEL_UYVY * 2
 179 #define PIXEL2_YUYV                     PIXEL_YUYV * 2
 180 #define PIXEL2_RGB                      PIXEL_RGB * 2
 181 #define PIXEL2_BGR                      PIXEL_BGR * 2
 182 #define PIXEL2_RGBX                     PIXEL_RGBX * 2
 183
 184 #define PIXEL4_RGB565           PIXEL_RGB565 * 4
 185 #define PIXEL4_UYVY                     PIXEL_UYVY * 4
 186 #define PIXEL4_YUYV                     PIXEL_YUYV * 4
 187 #define PIXEL4_RGB                      PIXEL_RGB * 4
 188 #define PIXEL4_BGR                      PIXEL_BGR * 4
 189 #define PIXEL4_RGBX                     PIXEL_RGBX * 4
 190
 191 #define PIXEL8_RGB565           PIXEL_RGB565 * 8
 192 #define PIXEL8_UYVY                     PIXEL_UYVY * 8
 193 #define PIXEL8_YUYV                     PIXEL_YUYV * 8
 194 #define PIXEL8_RGB                      PIXEL_RGB * 8
 195 #define PIXEL8_BGR                      PIXEL_BGR * 8
 196 #define PIXEL8_RGBX                     PIXEL_RGBX * 8
 197
 198 #define PIXEL16_RGB565          PIXEL_RGB565 * 16
 199 #define PIXEL16_UYVY            PIXEL_UYVY * 16
 200 #define PIXEL16_YUYV            PIXEL_YUYV * 16
 201 #define PIXEL16_RGB                     PIXEL_RGB * 16
 202 #define PIXEL16_BGR                     PIXEL_BGR * 16
 203 #define PIXEL16_RGBX            PIXEL_RGBX * 16
 204
 205 #define RGB2RGBX_2(prgb, prgbx, ax, bx) { \
 206                 (prgbx)[bx+0] = (prgb)[ax+0]; \
 207                 (prgbx)[bx+1] = (prgb)[ax+1]; \
 208                 (prgbx)[bx+2] = (prgb)[ax+2]; \
 209                 (prgbx)[bx+3] = 0xff; \
 210                 (prgbx)[bx+4] = (prgb)[ax+3]; \
 211                 (prgbx)[bx+5] = (prgb)[ax+4]; \
 212                 (prgbx)[bx+6] = (prgb)[ax+5]; \
 213                 (prgbx)[bx+7] = 0xff; \
 214         }
 215 #define RGB2RGBX_16(prgb, prgbx, ax, bx) \
 216         RGB2RGBX_8(prgb, prgbx, ax, bx) \
 217         RGB2RGBX_8(prgb, prgbx, ax + PIXEL8_RGB, bx +PIXEL8_RGBX);
 218 #define RGB2RGBX_8(prgb, prgbx, ax, bx) \
 219         RGB2RGBX_4(prgb, prgbx, ax, bx) \
 220         RGB2RGBX_4(prgb, prgbx, ax + PIXEL4_RGB, bx + PIXEL4_RGBX);
 221 #define RGB2RGBX_4(prgb, prgbx, ax, bx) \
 222         RGB2RGBX_2(prgb, prgbx, ax, bx) \
 223         RGB2RGBX_2(prgb, prgbx, ax + PIXEL2_RGB, bx + PIXEL2_RGBX);
 224
 225 /** @brief Convert a frame from RGB888 to RGBX8888
 226  * @ingroup frame
 227  * @param ini RGB888 frame
 228  * @param out RGBX8888 frame
 229  */
 230 uvc_error_t uvc_rgb2rgbx(uvc_frame_t *in, uvc_frame_t *out) {
 231         if (UNLIKELY(in->frame_format != UVC_FRAME_FORMAT_RGB))
 232                 return UVC_ERROR_INVALID_PARAM;
 233
 234         if (UNLIKELY(uvc_ensure_frame_size(out, in->width * in->height * PIXEL_RGBX) < 0))
 235                 return UVC_ERROR_NO_MEM;
 236
 237         out->width = in->width;
 238         out->height = in->height;
 239         out->frame_format = UVC_FRAME_FORMAT_RGBX;
 240         if (out->library_owns_data)
 241                 out->step = in->width * PIXEL_RGBX;
 242         out->sequence = in->sequence;
 243         out->capture_time = in->capture_time;
 244         out->source = in->source;
 245
 246         uint8_t *prgb = in->data;
 247         const uint8_t *prgb_end = prgb + in->data_bytes - PIXEL8_RGB;
 248         uint8_t *prgbx = out->data;
 249         const uint8_t *prgbx_end = prgbx + out->data_bytes - PIXEL8_RGBX;
 250
 251         // RGB888 to RGBX8888
 252 #if USE_STRIDE
 253         if (in->step && out->step && (in->step != out->step)) {
 254                 const int hh = in->height < out->height ? in->height : out->height;
 255                 const int ww = in->width < out->width ? in->width : out->width;
 256                 int h, w;
 257                 for (h = 0; h < hh; h++) {
 258                         w = 0;
 259                         prgb = in->data + in->step * h;
 260                         prgbx = out->data + out->step * h;
 261                         for (; (prgbx <= prgbx_end) && (prgb <= prgb_end) && (w < ww) ;) {
 262                                 RGB2RGBX_8(prgb, prgbx, 0, 0);
 263
 264                                 prgb += PIXEL8_RGB;
 265                                 prgbx += PIXEL8_RGBX;
 266                                 w += 8;
 267                         }
 268                 }
 269         } else {
 270                 // compressed format? XXX if only one of the frame in / out has step, this may lead to crash...
 271                 for (; (prgbx <= prgbx_end) && (prgb <= prgb_end) ;) {
 272                         RGB2RGBX_8(prgb, prgbx, 0, 0);
 273
 274                         prgb += PIXEL8_RGB;
 275                         prgbx += PIXEL8_RGBX;
 276                 }
 277         }
 278 #else
 279         for (; (prgbx <= prgbx_end) && (prgb <= prgb_end) ;) {
 280                 RGB2RGBX_8(prgb, prgbx, 0, 0);
 281
 282                 prgb += PIXEL8_RGB;
 283                 prgbx += PIXEL8_RGBX;
 284         }
 285 #endif
 286         return UVC_SUCCESS;
 287 }
 288
 289 // prgb565[0] = ((g << 3) & 0b11100000) | ((b >> 3) & 0b00011111);      // low byte
 290 // prgb565[1] = ((r & 0b11111000) | ((g >> 5) & 0b00000111));           // high byte
 291 #define RGB2RGB565_2(prgb, prgb565, ax, bx) { \
 292                 (prgb565)[bx+0] = (((prgb)[ax+1] << 3) & 0b11100000) | (((prgb)[ax+2] >> 3) & 0b00011111); \
 293                 (prgb565)[bx+1] = (((prgb)[ax+0] & 0b11111000) | (((prgb)[ax+1] >> 5) & 0b00000111)); \
 294                 (prgb565)[bx+2] = (((prgb)[ax+4] << 3) & 0b11100000) | (((prgb)[ax+5] >> 3) & 0b00011111); \
 295                 (prgb565)[bx+3] = (((prgb)[ax+3] & 0b11111000) | (((prgb)[ax+4] >> 5) & 0b00000111)); \
 296     }
 297 #define RGB2RGB565_16(prgb, prgb565, ax, bx) \
 298         RGB2RGB565_8(prgb, prgb565, ax, bx) \
 299         RGB2RGB565_8(prgb, prgb565, ax + PIXEL8_RGB, bx + PIXEL8_RGB565);
 300 #define RGB2RGB565_8(prgb, prgb565, ax, bx) \
 301         RGB2RGB565_4(prgb, prgb565, ax, bx) \
 302         RGB2RGB565_4(prgb, prgb565, ax + PIXEL4_RGB, bx + PIXEL4_RGB565);
 303 #define RGB2RGB565_4(prgb, prgb565, ax, bx) \
 304         RGB2RGB565_2(prgb, prgb565, ax, bx) \
 305         RGB2RGB565_2(prgb, prgb565, ax + PIXEL2_RGB, bx + PIXEL2_RGB565);
 306
 307 /** @brief Convert a frame from RGB888 to RGB565
 308  * @ingroup frame
 309  * @param ini RGB888 frame
 310  * @param out RGB565 frame
 311  */
 312 uvc_error_t uvc_rgb2rgb565(uvc_frame_t *in, uvc_frame_t *out) {
 313         if (UNLIKELY(in->frame_format != UVC_FRAME_FORMAT_RGB))
 314                 return UVC_ERROR_INVALID_PARAM;
 315
 316         if (UNLIKELY(uvc_ensure_frame_size(out, in->width * in->height * PIXEL_RGB565) < 0))
 317                 return UVC_ERROR_NO_MEM;
 318
 319         out->width = in->width;
 320         out->height = in->height;
 321         out->frame_format = UVC_FRAME_FORMAT_RGB565;
 322         if (out->library_owns_data)
 323                 out->step = in->width * PIXEL_RGB565;
 324         out->sequence = in->sequence;
 325         out->capture_time = in->capture_time;
 326         out->source = in->source;
 327
 328         uint8_t *prgb = in->data;
 329         const uint8_t *prgb_end = prgb + in->data_bytes - PIXEL8_RGB;
 330         uint8_t *prgb565 = out->data;
 331         const uint8_t *prgb565_end = prgb565 + out->data_bytes - PIXEL8_RGB565;
 332
 333         // RGB888 to RGB565
 334 #if USE_STRIDE
 335         if (in->step && out->step && (in->step != out->step)) {
 336                 const int hh = in->height < out->height ? in->height : out->height;
 337                 const int ww = in->width < out->width ? in->width : out->width;
 338                 int h, w;
 339                 for (h = 0; h < hh; h++) {
 340                         w = 0;
 341                         prgb = in->data + in->step * h;
 342                         prgb565 = out->data + out->step * h;
 343                         for (; (prgb565 <= prgb565_end) && (prgb <= prgb_end) && (w < ww) ;) {
 344                                 RGB2RGB565_8(prgb, prgb565, 0, 0);
 345
 346                                 prgb += PIXEL8_RGB;
 347                                 prgb565 += PIXEL8_RGB565;
 348                                 w += 8;
 349                         }
 350                 }
 351         } else {
 352                 // compressed format? XXX if only one of the frame in / out has step, this may lead to crash...
 353                 for (; (prgb565 <= prgb565_end) && (prgb <= prgb_end) ;) {
 354                         RGB2RGB565_8(prgb, prgb565, 0, 0);
 355
 356                         prgb += PIXEL8_RGB;
 357                         prgb565 += PIXEL8_RGB565;
 358                 }
 359         }
 360 #else
 361         for (; (prgb565 <= prgb565_end) && (prgb <= prgb_end) ;) {
 362                 RGB2RGB565_8(prgb, prgb565, 0, 0);
 363
 364                 prgb += PIXEL8_RGB;
 365                 prgb565 += PIXEL8_RGB565;
 366         }
 367 #endif
 368         return UVC_SUCCESS;
 369 }
 370 /*
 371  #define YUYV2RGB_2(pyuv, prgb) { \
 372     float r = 1.402f * ((pyuv)[3]-128); \
 373     float g = -0.34414f * ((pyuv)[1]-128) - 0.71414f * ((pyuv)[3]-128); \
 374     float b = 1.772f * ((pyuv)[1]-128); \
 375     (prgb)[0] = sat(pyuv[0] + r); \
 376     (prgb)[1] = sat(pyuv[0] + g); \
 377     (prgb)[2] = sat(pyuv[0] + b); \
 378     (prgb)[3] = sat(pyuv[2] + r); \
 379     (prgb)[4] = sat(pyuv[2] + g); \
 380     (prgb)[5] = sat(pyuv[2] + b); \
 381     }
 382 */
 383
 384 #define IYUYV2RGB_2(pyuv, prgb, ax, bx) { \
 385                 const int d1 = (pyuv)[ax+1]; \
 386                 const int d3 = (pyuv)[ax+3]; \
 387                 const int r = (22987 * (d3/*(pyuv)[ax+3]*/ - 128)) >> 14; \
 388                 const int g = (-5636 * (d1/*(pyuv)[ax+1]*/ - 128) - 11698 * (d3/*(pyuv)[ax+3]*/ - 128)) >> 14; \
 389                 const int b = (29049 * (d1/*(pyuv)[ax+1]*/ - 128)) >> 14; \
 390                 const int y0 = (pyuv)[ax+0]; \
 391                 (prgb)[bx+0] = sat(y0 + r); \
 392                 (prgb)[bx+1] = sat(y0 + g); \
 393                 (prgb)[bx+2] = sat(y0 + b); \
 394                 const int y2 = (pyuv)[ax+2]; \
 395                 (prgb)[bx+3] = sat(y2 + r); \
 396                 (prgb)[bx+4] = sat(y2 + g); \
 397                 (prgb)[bx+5] = sat(y2 + b); \
 398     }
 399 #define IYUYV2RGB_16(pyuv, prgb, ax, bx) \
 400         IYUYV2RGB_8(pyuv, prgb, ax, bx) \
 401         IYUYV2RGB_8(pyuv, prgb, ax + PIXEL8_YUYV, bx + PIXEL8_RGB)
 402 #define IYUYV2RGB_8(pyuv, prgb, ax, bx) \
 403         IYUYV2RGB_4(pyuv, prgb, ax, bx) \
 404         IYUYV2RGB_4(pyuv, prgb, ax + PIXEL4_YUYV, bx + PIXEL4_RGB)
 405 #define IYUYV2RGB_4(pyuv, prgb, ax, bx) \
 406         IYUYV2RGB_2(pyuv, prgb, ax, bx) \
 407         IYUYV2RGB_2(pyuv, prgb, ax + PIXEL2_YUYV, bx + PIXEL2_RGB)
 408
 409 /** @brief Convert a frame from YUYV to RGB888
 410  * @ingroup frame
 411  *
 412  * @param in YUYV frame
 413  * @param out RGB888 frame
 414  */
 415 uvc_error_t uvc_yuyv2rgb(uvc_frame_t *in, uvc_frame_t *out) {
 416         if (UNLIKELY(in->frame_format != UVC_FRAME_FORMAT_YUYV))
 417                 return UVC_ERROR_INVALID_PARAM;
 418
 419         if (UNLIKELY(uvc_ensure_frame_size(out, in->width * in->height * PIXEL_RGB) < 0))
 420                 return UVC_ERROR_NO_MEM;
 421
 422         out->width = in->width;
 423         out->height = in->height;
 424         out->frame_format = UVC_FRAME_FORMAT_RGB;
 425         if (out->library_owns_data)
 426                 out->step = in->width * PIXEL_RGB;
 427         out->sequence = in->sequence;
 428         out->capture_time = in->capture_time;
 429         out->source = in->source;
 430
 431         uint8_t *pyuv = in->data;
 432         const uint8_t *pyuv_end = pyuv + in->data_bytes - PIXEL8_YUYV;
 433         uint8_t *prgb = out->data;
 434         const uint8_t *prgb_end = prgb + out->data_bytes - PIXEL8_RGB;
 435
 436 #if USE_STRIDE
 437         if (in->step && out->step && (in->step != out->step)) {
 438                 const int hh = in->height < out->height ? in->height : out->height;
 439                 const int ww = in->width < out->width ? in->width : out->width;
 440                 int h, w;
 441                 for (h = 0; h < hh; h++) {
 442                         w = 0;
 443                         pyuv = in->data + in->step * h;
 444                         prgb = out->data + out->step * h;
 445                         for (; (prgb <= prgb_end) && (pyuv <= pyuv_end) && (w < ww) ;) {
 446                                 IYUYV2RGB_8(pyuv, prgb, 0, 0);
 447
 448                                 prgb += PIXEL8_RGB;
 449                                 pyuv += PIXEL8_YUYV;
 450                                 w += 8;
 451                         }
 452                 }
 453         } else {
 454                 // compressed format? XXX if only one of the frame in / out has step, this may lead to crash...
 455                 for (; (prgb <= prgb_end) && (pyuv <= pyuv_end) ;) {
 456                         IYUYV2RGB_8(pyuv, prgb, 0, 0);
 457
 458                         prgb += PIXEL8_RGB;
 459                         pyuv += PIXEL8_YUYV;
 460                 }
 461         }
 462 #else
 463         // YUYV => RGB888
 464         for (; (prgb <= prgb_end) && (pyuv <= pyuv_end) ;) {
 465                 IYUYV2RGB_8(pyuv, prgb, 0, 0);
 466
 467                 prgb += PIXEL8_RGB;
 468                 pyuv += PIXEL8_YUYV;
 469         }
 470 #endif
 471         return UVC_SUCCESS;
 472 }
 473
 474 /** @brief Convert a frame from YUYV to RGB565
 475  * @ingroup frame
 476  * @param ini YUYV frame
 477  * @param out RGB565 frame
 478  */
 479 uvc_error_t uvc_yuyv2rgb565(uvc_frame_t *in, uvc_frame_t *out) {
 480         if (UNLIKELY(in->frame_format != UVC_FRAME_FORMAT_YUYV))
 481                 return UVC_ERROR_INVALID_PARAM;
 482
 483         if (UNLIKELY(uvc_ensure_frame_size(out, in->width * in->height * PIXEL_RGB565) < 0))
 484                 return UVC_ERROR_NO_MEM;
 485
 486         out->width = in->width;
 487         out->height = in->height;
 488         out->frame_format = UVC_FRAME_FORMAT_RGB565;
 489         if (out->library_owns_data)
 490                 out->step = in->width * PIXEL_RGB565;
 491         out->sequence = in->sequence;
 492         out->capture_time = in->capture_time;
 493         out->source = in->source;
 494
 495         uint8_t *pyuv = in->data;
 496         const uint8_t *pyuv_end = pyuv + in->data_bytes - PIXEL8_YUYV;
 497         uint8_t *prgb565 = out->data;
 498         const uint8_t *prgb565_end = prgb565 + out->data_bytes - PIXEL8_RGB565;
 499
 500         uint8_t tmp[PIXEL8_RGB];        // for temporary rgb888 data(8pixel)
 501
 502 #if USE_STRIDE
 503         if (in->step && out->step && (in->step != out->step)) {
 504                 const int hh = in->height < out->height ? in->height : out->height;
 505                 const int ww = in->width < out->width ? in->width : out->width;
 506                 int h, w;
 507                 for (h = 0; h < hh; h++) {
 508                         w = 0;
 509                         pyuv = in->data + in->step * h;
 510                         prgb565 = out->data + out->step * h;
 511                         for (; (prgb565 <= prgb565_end) && (pyuv <= pyuv_end) && (w < ww) ;) {
 512                                 IYUYV2RGB_8(pyuv, tmp, 0, 0);
 513                                 RGB2RGB565_8(tmp, prgb565, 0, 0);
 514
 515                                 prgb565 += PIXEL8_YUYV;
 516                                 pyuv += PIXEL8_RGB565;
 517                                 w += 8;
 518                         }
 519                 }
 520         } else {
 521                 // compressed format? XXX if only one of the frame in / out has step, this may lead to crash...
 522                 for (; (prgb565 <= prgb565_end) && (pyuv <= pyuv_end) ;) {
 523                         IYUYV2RGB_8(pyuv, tmp, 0, 0);
 524                         RGB2RGB565_8(tmp, prgb565, 0, 0);
 525
 526                         prgb565 += PIXEL8_YUYV;
 527                         pyuv += PIXEL8_RGB565;
 528                 }
 529         }
 530 #else
 531         // YUYV => RGB565
 532         for (; (prgb565 <= prgb565_end) && (pyuv <= pyuv_end) ;) {
 533                 IYUYV2RGB_8(pyuv, tmp, 0, 0);
 534                 RGB2RGB565_8(tmp, prgb565, 0, 0);
 535
 536                 prgb565 += PIXEL8_YUYV;
 537                 pyuv += PIXEL8_RGB565;
 538         }
 539 #endif
 540         return UVC_SUCCESS;
 541 }
 542
 543 #define IYUYV2RGBX_2(pyuv, prgbx, ax, bx) { \
 544                 const int d1 = (pyuv)[ax+1]; \
 545                 const int d3 = (pyuv)[ax+3]; \
 546                 const int r = (22987 * (d3/*(pyuv)[ax+3]*/ - 128)) >> 14; \
 547                 const int g = (-5636 * (d1/*(pyuv)[ax+1]*/ - 128) - 11698 * (d3/*(pyuv)[ax+3]*/ - 128)) >> 14; \
 548                 const int b = (29049 * (d1/*(pyuv)[ax+1]*/ - 128)) >> 14; \
 549                 const int y0 = (pyuv)[ax+0]; \
 550                 (prgbx)[bx+0] = sat(y0 + r); \
 551                 (prgbx)[bx+1] = sat(y0 + g); \
 552                 (prgbx)[bx+2] = sat(y0 + b); \
 553                 (prgbx)[bx+3] = 0xff; \
 554                 const int y2 = (pyuv)[ax+2]; \
 555                 (prgbx)[bx+4] = sat(y2 + r); \
 556                 (prgbx)[bx+5] = sat(y2 + g); \
 557                 (prgbx)[bx+6] = sat(y2 + b); \
 558                 (prgbx)[bx+7] = 0xff; \
 559     }
 560 #define IYUYV2RGBX_16(pyuv, prgbx, ax, bx) \
 561         IYUYV2RGBX_8(pyuv, prgbx, ax, bx) \
 562         IYUYV2RGBX_8(pyuv, prgbx, ax + PIXEL8_YUYV, bx + PIXEL8_RGBX);
 563 #define IYUYV2RGBX_8(pyuv, prgbx, ax, bx) \
 564         IYUYV2RGBX_4(pyuv, prgbx, ax, bx) \
 565         IYUYV2RGBX_4(pyuv, prgbx, ax + PIXEL4_YUYV, bx + PIXEL4_RGBX);
 566 #define IYUYV2RGBX_4(pyuv, prgbx, ax, bx) \
 567         IYUYV2RGBX_2(pyuv, prgbx, ax, bx) \
 568         IYUYV2RGBX_2(pyuv, prgbx, ax + PIXEL2_YUYV, bx + PIXEL2_RGBX);
 569
 570 /** @brief Convert a frame from YUYV to RGBX8888
 571  * @ingroup frame
 572  * @param ini YUYV frame
 573  * @param out RGBX8888 frame
 574  */
 575 uvc_error_t uvc_yuyv2rgbx(uvc_frame_t *in, uvc_frame_t *out) {
 576         if (UNLIKELY(in->frame_format != UVC_FRAME_FORMAT_YUYV))
 577                 return UVC_ERROR_INVALID_PARAM;
 578
 579         if (UNLIKELY(uvc_ensure_frame_size(out, in->width * in->height * PIXEL_RGBX) < 0))
 580                 return UVC_ERROR_NO_MEM;
 581
 582         out->width = in->width;
 583         out->height = in->height;
 584         out->frame_format = UVC_FRAME_FORMAT_RGBX;
 585         if (out->library_owns_data)
 586                 out->step = in->width * PIXEL_RGBX;
 587         out->sequence = in->sequence;
 588         out->capture_time = in->capture_time;
 589         out->source = in->source;
 590
 591         uint8_t *pyuv = in->data;
 592         const uint8_t *pyuv_end = pyuv + in->data_bytes - PIXEL8_YUYV;
 593         uint8_t *prgbx = out->data;
 594         const uint8_t *prgbx_end = prgbx + out->data_bytes - PIXEL8_RGBX;
 595
 596         // YUYV => RGBX8888
 597 #if USE_STRIDE
 598         if (in->step && out->step && (in->step != out->step)) {
 599                 const int hh = in->height < out->height ? in->height : out->height;
 600                 const int ww = in->width < out->width ? in->width : out->width;
 601                 int h, w;
 602                 for (h = 0; h < hh; h++) {
 603                         w = 0;
 604                         pyuv = in->data + in->step * h;
 605                         prgbx = out->data + out->step * h;
 606                         for (; (prgbx <= prgbx_end) && (pyuv <= pyuv_end) && (w < ww) ;) {
 607                                 IYUYV2RGBX_8(pyuv, prgbx, 0, 0);
 608
 609                                 prgbx += PIXEL8_RGBX;
 610                                 pyuv += PIXEL8_YUYV;
 611                                 w += 8;
 612                         }
 613                 }
 614         } else {
 615                 // compressed format? XXX if only one of the frame in / out has step, this may lead to crash...
 616                 for (; (prgbx <= prgbx_end) && (pyuv <= pyuv_end) ;) {
 617                         IYUYV2RGBX_8(pyuv, prgbx, 0, 0);
 618
 619                         prgbx += PIXEL8_RGBX;
 620                         pyuv += PIXEL8_YUYV;
 621                 }
 622         }
 623 #else
 624         for (; (prgbx <= prgbx_end) && (pyuv <= pyuv_end) ;) {
 625                 IYUYV2RGBX_8(pyuv, prgbx, 0, 0);
 626
 627                 prgbx += PIXEL8_RGBX;
 628                 pyuv += PIXEL8_YUYV;
 629         }
 630 #endif
 631         return UVC_SUCCESS;
 632 }
 633
 634 #define IYUYV2BGR_2(pyuv, pbgr, ax, bx) { \
 635                 const int d1 = (pyuv)[1]; \
 636                 const int d3 = (pyuv)[3]; \
 637             const int r = (22987 * (d3/*(pyuv)[3]*/ - 128)) >> 14; \
 638             const int g = (-5636 * (d1/*(pyuv)[1]*/ - 128) - 11698 * (d3/*(pyuv)[3]*/ - 128)) >> 14; \
 639             const int b = (29049 * (d1/*(pyuv)[1]*/ - 128)) >> 14; \
 640                 const int y0 = (pyuv)[ax+0]; \
 641                 (pbgr)[bx+0] = sat(y0 + b); \
 642                 (pbgr)[bx+1] = sat(y0 + g); \
 643                 (pbgr)[bx+2] = sat(y0 + r); \
 644                 const int y2 = (pyuv)[ax+2]; \
 645                 (pbgr)[bx+3] = sat(y2 + b); \
 646                 (pbgr)[bx+4] = sat(y2 + g); \
 647                 (pbgr)[bx+5] = sat(y2 + r); \
 648     }
 649 #define IYUYV2BGR_16(pyuv, pbgr, ax, bx) \
 650         IYUYV2BGR_8(pyuv, pbgr, ax, bx) \
 651         IYUYV2BGR_8(pyuv, pbgr, ax + PIXEL8_YUYV, bx + PIXEL8_BGR)
 652 #define IYUYV2BGR_8(pyuv, pbgr, ax, bx) \
 653         IYUYV2BGR_4(pyuv, pbgr, ax, bx) \
 654         IYUYV2BGR_4(pyuv, pbgr, ax + PIXEL4_YUYV, bx + PIXEL4_BGR)
 655 #define IYUYV2BGR_4(pyuv, pbgr, ax, bx) \
 656         IYUYV2BGR_2(pyuv, pbgr, ax, bx) \
 657         IYUYV2BGR_2(pyuv, pbgr, ax + PIXEL2_YUYV, bx + PIXEL2_BGR)
 658
 659 /** @brief Convert a frame from YUYV to BGR888
 660  * @ingroup frame
 661  *
 662  * @param in YUYV frame
 663  * @param out BGR888 frame
 664  */
 665 uvc_error_t uvc_yuyv2bgr(uvc_frame_t *in, uvc_frame_t *out) {
 666         if (UNLIKELY(in->frame_format != UVC_FRAME_FORMAT_YUYV))
 667                 return UVC_ERROR_INVALID_PARAM;
 668
 669         if (UNLIKELY(uvc_ensure_frame_size(out, in->width * in->height * PIXEL_BGR) < 0))
 670                 return UVC_ERROR_NO_MEM;
 671
 672         out->width = in->width;
 673         out->height = in->height;
 674         out->frame_format = UVC_FRAME_FORMAT_BGR;
 675         if (out->library_owns_data)
 676                 out->step = in->width * PIXEL_BGR;
 677         out->sequence = in->sequence;
 678         out->capture_time = in->capture_time;
 679         out->source = in->source;
 680
 681         uint8_t *pyuv = in->data;
 682         uint8_t *pyuv_end = pyuv + in->data_bytes - PIXEL8_YUYV;
 683         uint8_t *pbgr = out->data;
 684         uint8_t *pbgr_end = pbgr + out->data_bytes - PIXEL8_BGR;
 685
 686         // YUYV => BGR888
 687 #if USE_STRIDE
 688         if (in->step && out->step && (in->step != out->step)) {
 689                 const int hh = in->height < out->height ? in->height : out->height;
 690                 const int ww = in->width < out->width ? in->width : out->width;
 691                 int h, w;
 692                 for (h = 0; h < hh; h++) {
 693                         w = 0;
 694                         pyuv = in->data + in->step * h;
 695                         pbgr = out->data + out->step * h;
 696                         for (; (pbgr <= pbgr_end) && (pyuv <= pyuv_end) && (w < ww) ;) {
 697                                 IYUYV2BGR_8(pyuv, pbgr, 0, 0);
 698
 699                                 pbgr += PIXEL8_BGR;
 700                                 pyuv += PIXEL8_YUYV;
 701                                 w += 8;
 702                         }
 703                 }
 704         } else {
 705                 // compressed format? XXX if only one of the frame in / out has step, this may lead to crash...
 706                 for (; (pbgr <= pbgr_end) && (pyuv <= pyuv_end) ;) {
 707                         IYUYV2BGR_8(pyuv, pbgr, 0, 0);
 708
 709                         pbgr += PIXEL8_BGR;
 710                         pyuv += PIXEL8_YUYV;
 711                 }
 712         }
 713 #else
 714         for (; (pbgr <= pbgr_end) && (pyuv <= pyuv_end) ;) {
 715                 IYUYV2BGR_8(pyuv, pbgr, 0, 0);
 716
 717                 pbgr += PIXEL8_BGR;
 718                 pyuv += PIXEL8_YUYV;
 719         }
 720 #endif
 721         return UVC_SUCCESS;
 722 }
 723
 724 #define IUYVY2RGB_2(pyuv, prgb, ax, bx) { \
 725                 const int d0 = (pyuv)[ax+0]; \
 726                 const int d2 = (pyuv)[ax+2]; \
 727             const int r = (22987 * (d2/*(pyuv)[ax+2]*/ - 128)) >> 14; \
 728             const int g = (-5636 * (d0/*(pyuv)[ax+0]*/ - 128) - 11698 * (d2/*(pyuv)[ax+2]*/ - 128)) >> 14; \
 729             const int b = (29049 * (d0/*(pyuv)[ax+0]*/ - 128)) >> 14; \
 730                 const int y1 = (pyuv)[ax+1]; \
 731                 (prgb)[bx+0] = sat(y1 + r); \
 732                 (prgb)[bx+1] = sat(y1 + g); \
 733                 (prgb)[bx+2] = sat(y1 + b); \
 734                 const int y3 = (pyuv)[ax+3]; \
 735                 (prgb)[bx+3] = sat(y3 + r); \
 736                 (prgb)[bx+4] = sat(y3 + g); \
 737                 (prgb)[bx+5] = sat(y3 + b); \
 738     }
 739 #define IUYVY2RGB_16(pyuv, prgb, ax, bx) \
 740         IUYVY2RGB_8(pyuv, prgb, ax, bx) \
 741         IUYVY2RGB_8(pyuv, prgb, ax + 16, bx + 24)
 742 #define IUYVY2RGB_8(pyuv, prgb, ax, bx) \
 743         IUYVY2RGB_4(pyuv, prgb, ax, bx) \
 744         IUYVY2RGB_4(pyuv, prgb, ax + 8, bx + 12)
 745 #define IUYVY2RGB_4(pyuv, prgb, ax, bx) \
 746         IUYVY2RGB_2(pyuv, prgb, ax, bx) \
 747         IUYVY2RGB_2(pyuv, prgb, ax + 4, bx + 6)
 748
 749 /** @brief Convert a frame from UYVY to RGB888
 750  * @ingroup frame
 751  * @param ini UYVY frame
 752  * @param out RGB888 frame
 753  */
 754 uvc_error_t uvc_uyvy2rgb(uvc_frame_t *in, uvc_frame_t *out) {
 755         if (UNLIKELY(in->frame_format != UVC_FRAME_FORMAT_UYVY))
 756                 return UVC_ERROR_INVALID_PARAM;
 757
 758         if (UNLIKELY(uvc_ensure_frame_size(out, in->width * in->height * PIXEL_RGB) < 0))
 759                 return UVC_ERROR_NO_MEM;
 760
 761         out->width = in->width;
 762         out->height = in->height;
 763         out->frame_format = UVC_FRAME_FORMAT_RGB;
 764         if (out->library_owns_data)
 765                 out->step = in->width * PIXEL_RGB;
 766         out->sequence = in->sequence;
 767         out->capture_time = in->capture_time;
 768         out->source = in->source;
 769
 770         uint8_t *pyuv = in->data;
 771         const uint8_t *pyuv_end = pyuv + in->data_bytes - PIXEL8_UYVY;
 772         uint8_t *prgb = out->data;
 773         const uint8_t *prgb_end = prgb + out->data_bytes - PIXEL8_RGB;
 774
 775         // UYVY => RGB888
 776 #if USE_STRIDE
 777         if (in->step && out->step && (in->step != out->step)) {
 778                 const int hh = in->height < out->height ? in->height : out->height;
 779                 const int ww = in->width < out->width ? in->width : out->width;
 780                 int h, w;
 781                 for (h = 0; h < hh; h++) {
 782                         w = 0;
 783                         pyuv = in->data + in->step * h;
 784                         prgb = out->data + out->step * h;
 785                         for (; (prgb <= prgb_end) && (pyuv <= pyuv_end) && (w < ww) ;) {
 786                                 IUYVY2RGB_8(pyuv, prgb, 0, 0);
 787
 788                                 prgb += PIXEL8_RGB;
 789                                 pyuv += PIXEL8_UYVY;
 790                                 w += 8;
 791                         }
 792                 }
 793         } else {
 794                 // compressed format? XXX if only one of the frame in / out has step, this may lead to crash...
 795                 for (; (prgb <= prgb_end) && (pyuv <= pyuv_end) ;) {
 796                         IUYVY2RGB_8(pyuv, prgb, 0, 0);
 797
 798                         prgb += PIXEL8_RGB;
 799                         pyuv += PIXEL8_UYVY;
 800                 }
 801         }
 802 #else
 803         for (; ((prgb <= prgb_end) && (pyuv <= pyuv_end) ;) {
 804                 IUYVY2RGB_8(pyuv, prgb, 0, 0);
 805
 806                 prgb += PIXEL8_RGB;
 807                 pyuv += PIXEL8_UYVY;
 808         }
 809 #endif
 810         return UVC_SUCCESS;
 811 }
 812
 813 /** @brief Convert a frame from UYVY to RGB565
 814  * @ingroup frame
 815  * @param ini UYVY frame
 816  * @param out RGB565 frame
 817  */
 818 uvc_error_t uvc_uyvy2rgb565(uvc_frame_t *in, uvc_frame_t *out) {
 819         if (UNLIKELY(in->frame_format != UVC_FRAME_FORMAT_UYVY))
 820                 return UVC_ERROR_INVALID_PARAM;
 821
 822         if (UNLIKELY(uvc_ensure_frame_size(out, in->width * in->height * PIXEL_RGB565) < 0))
 823                 return UVC_ERROR_NO_MEM;
 824
 825         out->width = in->width;
 826         out->height = in->height;
 827         out->frame_format = UVC_FRAME_FORMAT_RGB565;
 828         if (out->library_owns_data)
 829                 out->step = in->width * PIXEL_RGB565;
 830         out->sequence = in->sequence;
 831         out->capture_time = in->capture_time;
 832         out->source = in->source;
 833
 834         uint8_t *pyuv = in->data;
 835         const uint8_t *pyuv_end = pyuv + in->data_bytes - PIXEL8_UYVY;
 836         uint8_t *prgb565 = out->data;
 837         const uint8_t *prgb565_end = prgb565 + out->data_bytes - PIXEL8_RGB565;
 838
 839         uint8_t tmp[PIXEL8_RGB];                // for temporary rgb888 data(8pixel)
 840
 841         // UYVY => RGB565
 842 #if USE_STRIDE
 843         if (in->step && out->step && (in->step != out->step)) {
 844                 const int hh = in->height < out->height ? in->height : out->height;
 845                 const int ww = in->width < out->width ? in->width : out->width;
 846                 int h, w;
 847                 for (h = 0; h < hh; h++) {
 848                         w = 0;
 849                         pyuv = in->data + in->step * h;
 850                         prgb565 = out->data + out->step * h;
 851                         for (; (prgb565 <= prgb565_end) && (pyuv <= pyuv_end) && (w < ww) ;) {
 852                                 IUYVY2RGB_8(pyuv, tmp, 0, 0);
 853                                 RGB2RGB565_8(tmp, prgb565, 0, 0);
 854
 855                                 prgb565 += PIXEL8_RGB565;
 856                                 pyuv += PIXEL8_UYVY;
 857                                 w += 8;
 858                         }
 859                 }
 860         } else {
 861                 // compressed format? XXX if only one of the frame in / out has step, this may lead to crash...
 862                 for (; (prgb565 <= prgb565_end) && (pyuv <= pyuv_end) ;) {
 863                         IUYVY2RGB_8(pyuv, tmp, 0, 0);
 864                         RGB2RGB565_8(tmp, prgb565, 0, 0);
 865
 866                         prgb565 += PIXEL8_RGB565;
 867                         pyuv += PIXEL8_UYVY;
 868                 }
 869         }
 870 #else
 871         for (; (prgb565 <= prgb565_end) && (pyuv <= pyuv_end) ;) {
 872                 IUYVY2RGB_8(pyuv, tmp, 0, 0);
 873                 RGB2RGB565_8(tmp, prgb565, 0, 0);
 874
 875                 prgb565 += PIXEL8_RGB565;
 876                 pyuv += PIXEL8_UYVY;
 877         }
 878 #endif
 879         return UVC_SUCCESS;
 880 }
 881
 882 #define IUYVY2RGBX_2(pyuv, prgbx, ax, bx) { \
 883                 const int d0 = (pyuv)[ax+0]; \
 884                 const int d2 = (pyuv)[ax+2]; \
 885             const int r = (22987 * (d2/*(pyuv)[ax+2]*/ - 128)) >> 14; \
 886             const int g = (-5636 * (d0/*(pyuv)[ax+0]*/ - 128) - 11698 * (d2/*(pyuv)[ax+2]*/ - 128)) >> 14; \
 887             const int b = (29049 * (d0/*(pyuv)[ax+0]*/ - 128)) >> 14; \
 888                 const int y1 = (pyuv)[ax+1]; \
 889                 (prgbx)[bx+0] = sat(y1 + r); \
 890                 (prgbx)[bx+1] = sat(y1 + g); \
 891                 (prgbx)[bx+2] = sat(y1 + b); \
 892                 (prgbx)[bx+3] = 0xff; \
 893                 const int y3 = (pyuv)[ax+3]; \
 894                 (prgbx)[bx+4] = sat(y3 + r); \
 895                 (prgbx)[bx+5] = sat(y3 + g); \
 896                 (prgbx)[bx+6] = sat(y3 + b); \
 897                 (prgbx)[bx+7] = 0xff; \
 898     }
 899 #define IUYVY2RGBX_16(pyuv, prgbx, ax, bx) \
 900         IUYVY2RGBX_8(pyuv, prgbx, ax, bx) \
 901         IUYVY2RGBX_8(pyuv, prgbx, ax + PIXEL8_UYVY, bx + PIXEL8_RGBX)
 902 #define IUYVY2RGBX_8(pyuv, prgbx, ax, bx) \
 903         IUYVY2RGBX_4(pyuv, prgbx, ax, bx) \
 904         IUYVY2RGBX_4(pyuv, prgbx, ax + PIXEL4_UYVY, bx + PIXEL4_RGBX)
 905 #define IUYVY2RGBX_4(pyuv, prgbx, ax, bx) \
 906         IUYVY2RGBX_2(pyuv, prgbx, ax, bx) \
 907         IUYVY2RGBX_2(pyuv, prgbx, ax + PIXEL2_UYVY, bx + PIXEL2_RGBX)
 908
 909 /** @brief Convert a frame from UYVY to RGBX8888
 910  * @ingroup frame
 911  * @param ini UYVY frame
 912  * @param out RGBX8888 frame
 913  */
 914 uvc_error_t uvc_uyvy2rgbx(uvc_frame_t *in, uvc_frame_t *out) {
 915         if (UNLIKELY(in->frame_format != UVC_FRAME_FORMAT_UYVY))
 916                 return UVC_ERROR_INVALID_PARAM;
 917
 918         if (UNLIKELY(uvc_ensure_frame_size(out, in->width * in->height * PIXEL_RGBX) < 0))
 919                 return UVC_ERROR_NO_MEM;
 920
 921         out->width = in->width;
 922         out->height = in->height;
 923         out->frame_format = UVC_FRAME_FORMAT_RGBX;
 924         if (out->library_owns_data)
 925                 out->step = in->width * PIXEL_RGBX;
 926         out->sequence = in->sequence;
 927         out->capture_time = in->capture_time;
 928         out->source = in->source;
 929
 930         uint8_t *pyuv = in->data;
 931         const uint8_t *pyuv_end = pyuv + in->data_bytes - PIXEL8_UYVY;
 932         uint8_t *prgbx = out->data;
 933         const uint8_t *prgbx_end = prgbx + out->data_bytes - PIXEL8_RGBX;
 934
 935         // UYVY => RGBX8888
 936 #if USE_STRIDE
 937         if (in->step && out->step && (in->step != out->step)) {
 938                 const int hh = in->height < out->height ? in->height : out->height;
 939                 const int ww = in->width < out->width ? in->width : out->width;
 940                 int h, w;
 941                 for (h = 0; h < hh; h++) {
 942                         w = 0;
 943                         pyuv = in->data + in->step * h;
 944                         prgbx = out->data + out->step * h;
 945                         for (; (prgbx <= prgbx_end) && (pyuv <= pyuv_end) && (w < ww) ;) {
 946                                 IUYVY2RGBX_8(pyuv, prgbx, 0, 0);
 947
 948                                 prgbx += PIXEL8_RGBX;
 949                                 pyuv += PIXEL8_UYVY;
 950                                 w += 8;
 951                         }
 952                 }
 953         } else {
 954                 // compressed format? XXX if only one of the frame in / out has step, this may lead to crash...
 955                 for (; (prgbx <= prgbx_end) && (pyuv <= pyuv_end) ;) {
 956                         IUYVY2RGBX_8(pyuv, prgbx, 0, 0);
 957
 958                         prgbx += PIXEL8_RGBX;
 959                         pyuv += PIXEL8_UYVY;
 960                 }
 961         }
 962 #else
 963         for (; (prgbx <= prgbx_end) && (pyuv <= pyuv_end) ;) {
 964                 IUYVY2RGBX_8(pyuv, prgbx, 0, 0);
 965
 966                 prgbx += PIXEL8_RGBX;
 967                 pyuv += PIXEL8_UYVY;
 968         }
 969 #endif
 970         return UVC_SUCCESS;
 971 }
 972
 973 #define IUYVY2BGR_2(pyuv, pbgr, ax, bx) { \
 974                 const int d0 = (pyuv)[ax+0]; \
 975                 const int d2 = (pyuv)[ax+2]; \
 976             const int r = (22987 * (d2/*(pyuv)[ax+2]*/ - 128)) >> 14; \
 977             const int g = (-5636 * (d0/*(pyuv)[ax+0]*/ - 128) - 11698 * (d2/*(pyuv)[ax+2]*/ - 128)) >> 14; \
 978             const int b = (29049 * (d0/*(pyuv)[ax+0]*/ - 128)) >> 14; \
 979                 const int y1 = (pyuv)[ax+1]; \
 980                 (pbgr)[bx+0] = sat(y1 + b); \
 981                 (pbgr)[bx+1] = sat(y1 + g); \
 982                 (pbgr)[bx+2] = sat(y1 + r); \
 983                 const int y3 = (pyuv)[ax+3]; \
 984                 (pbgr)[bx+3] = sat(y3 + b); \
 985                 (pbgr)[bx+4] = sat(y3 + g); \
 986                 (pbgr)[bx+5] = sat(y3 + r); \
 987     }
 988 #define IUYVY2BGR_16(pyuv, pbgr, ax, bx) \
 989         IUYVY2BGR_8(pyuv, pbgr, ax, bx) \
 990         IUYVY2BGR_8(pyuv, pbgr, ax + PIXEL8_UYVY, bx + PIXEL8_BGR)
 991 #define IUYVY2BGR_8(pyuv, pbgr, ax, bx) \
 992         IUYVY2BGR_4(pyuv, pbgr, ax, bx) \
 993         IUYVY2BGR_4(pyuv, pbgr, ax + PIXEL4_UYVY, bx + PIXEL4_BGR)
 994 #define IUYVY2BGR_4(pyuv, pbgr, ax, bx) \
 995         IUYVY2BGR_2(pyuv, pbgr, ax, bx) \
 996         IUYVY2BGR_2(pyuv, pbgr, ax + PIXEL2_UYVY, bx + PIXEL2_BGR)
 997
 998 /** @brief Convert a frame from UYVY to BGR888
 999  * @ingroup frame
1000  * @param ini UYVY frame
1001  * @param out BGR888 frame
1002  */
1003 uvc_error_t uvc_uyvy2bgr(uvc_frame_t *in, uvc_frame_t *out) {
1004         if (UNLIKELY(in->frame_format != UVC_FRAME_FORMAT_UYVY))
1005                 return UVC_ERROR_INVALID_PARAM;
1006
1007         if (UNLIKELY(uvc_ensure_frame_size(out, in->width * in->height * PIXEL_BGR) < 0))
1008                 return UVC_ERROR_NO_MEM;
1009
1010         out->width = in->width;
1011         out->height = in->height;
1012         out->frame_format = UVC_FRAME_FORMAT_BGR;
1013         if (out->library_owns_data)
1014                 out->step = in->width * PIXEL_BGR;
1015         out->sequence = in->sequence;
1016         out->capture_time = in->capture_time;
1017         out->source = in->source;
1018
1019         uint8_t *pyuv = in->data;
1020         const uint8_t *pyuv_end = pyuv + in->data_bytes - PIXEL8_UYVY;
1021         uint8_t *pbgr = out->data;
1022         const uint8_t *pbgr_end = pbgr + out->data_bytes - PIXEL8_BGR;
1023
1024         // UYVY => BGR888
1025 #if USE_STRIDE
1026         if (in->step && out->step && (in->step != out->step)) {
1027                 const int hh = in->height < out->height ? in->height : out->height;
1028                 const int ww = in->width < out->width ? in->width : out->width;
1029                 int h, w;
1030                 for (h = 0; h < hh; h++) {
1031                         w = 0;
1032                         pyuv = in->data + in->step * h;
1033                         pbgr = out->data + out->step * h;
1034                         for (; (pbgr <= pbgr_end) && (pyuv <= pyuv_end) && (w < ww) ;) {
1035                                 IUYVY2BGR_8(pyuv, pbgr, 0, 0);
1036
1037                                 pbgr += PIXEL8_BGR;
1038                                 pyuv += PIXEL8_UYVY;
1039                                 w += 8;
1040                         }
1041                 }
1042         } else {
1043                 // compressed format? XXX if only one of the frame in / out has step, this may lead to crash...
1044                 for (; (pbgr <= pbgr_end) && (pyuv <= pyuv_end) ;) {
1045                         IUYVY2BGR_8(pyuv, pbgr, 0, 0);
1046
1047                         pbgr += PIXEL8_BGR;
1048                         pyuv += PIXEL8_UYVY;
1049                 }
1050         }
1051 #else
1052         for (; (pbgr <= pbgr_end) && (pyuv <= pyuv_end) ;) {
1053                 IUYVY2BGR_8(pyuv, pbgr, 0, 0);
1054
1055                 pbgr += PIXEL8_BGR;
1056                 pyuv += PIXEL8_UYVY;
1057         }
1058 #endif
1059         return UVC_SUCCESS;
1060 }
1061
1062 int uvc_yuyv2yuv420P(uvc_frame_t *in, uvc_frame_t *out) {
1063
1064         ENTER();
1065
1066         if (UNLIKELY(in->frame_format != UVC_FRAME_FORMAT_YUYV))
1067                 RETURN(UVC_ERROR_INVALID_PARAM, uvc_error_t);
1068
1069         if (UNLIKELY(uvc_ensure_frame_size(out, (in->width * in->height * 3) / 2) < 0))
1070                 RETURN(UVC_ERROR_NO_MEM, uvc_error_t);
1071
1072         const uint8_t *src = in->data;
1073         uint8_t *dest = out->data;
1074         const int32_t width = in->width;
1075         const int32_t height = in->height;
1076         const int32_t src_width = in->step;
1077         const int32_t src_height = in->height;
1078         const int32_t dest_width = out->width = out->step = in->width;
1079         const int32_t dest_height = out->height = in->height;
1080         const uint32_t hh = src_height < dest_height ? src_height : dest_height;
1081         uint8_t *y = dest;
1082         uint8_t *v = dest + dest_width * dest_height;
1083         uint8_t *u = dest + dest_width * dest_height * 5 / 4;
1084         int h, w;
1085         for (h = 0; h < hh; h++) {
1086                 const uint8_t *yuv = src + src_width * h;
1087                 for (w = 0; w < width; w += 4) {
1088                         *(y++) = yuv[0];        // y
1089                         *(y++) = yuv[2];        // y'
1090                         *(y++) = yuv[4];        // y''
1091                         *(y++) = yuv[6];        // y'''
1092                         if ((h & 1) == 1) {
1093                                 *(u++) = yuv[3];        // u
1094                                 *(u++) = yuv[7];        // u
1095                         } else {
1096                                 *(v++) = yuv[1];        // v
1097                                 *(v++) = yuv[5];        // v
1098                         }
1099                         yuv += 8;       // (1pixel=2bytes)x4pixel=8bytes
1100                 }
1101         }
1102         RETURN(0, int);
1103 }
1104
1105 //--------------------------------------------------------------------------------
1106 int uvc_yuyv2iyuv420P(uvc_frame_t *in, uvc_frame_t *out) {
1107
1108         ENTER();
1109
1110         if (UNLIKELY(in->frame_format != UVC_FRAME_FORMAT_YUYV))
1111                 RETURN(UVC_ERROR_INVALID_PARAM, uvc_error_t);
1112
1113         if (UNLIKELY(uvc_ensure_frame_size(out, (in->width * in->height * 3) / 2) < 0))
1114                 RETURN(UVC_ERROR_NO_MEM, uvc_error_t);
1115
1116         const uint8_t *src = in->data;
1117         uint8_t *dest = out->data;
1118         const int32_t width = in->width;
1119         const int32_t height = in->height;
1120         const int32_t src_width = in->step;
1121         const int32_t src_height = in->height;
1122         const int32_t dest_width = out->width = out->step = in->width;
1123         const int32_t dest_height = out->height = in->height;
1124         const uint32_t hh = src_height < dest_height ? src_height : dest_height;
1125         uint8_t *y = dest;
1126         uint8_t *u = dest + dest_width * dest_height * 5 / 4;
1127         uint8_t *v = dest + dest_width * dest_height * 5 / 4;
1128         int h, w;
1129         for (h = 0; h < hh; h++) {
1130                 const uint8_t *yuv = src + src_width * h;
1131                 for (w = 0; w < width; w += 4) {
1132                         *(y++) = yuv[0];        // y
1133                         *(y++) = yuv[2];        // y'
1134                         *(y++) = yuv[4];        // y''
1135                         *(y++) = yuv[6];        // y'''
1136                         if ((h & 1) == 1) {
1137                                 *(u++) = yuv[3];        // u
1138                                 *(u++) = yuv[7];        // u
1139                         } else {
1140                                 *(v++) = yuv[1];        // v
1141                                 *(v++) = yuv[5];        // v
1142                         }
1143                         yuv += 8;       // (1pixel=2bytes)x4pixel=8bytes
1144                 }
1145         }
1146         RETURN(0, int);
1147 }
1148
1149 uvc_error_t uvc_yuyv2yuv420SP(uvc_frame_t *in, uvc_frame_t *out) {
1150         ENTER();
1151
1152         if (UNLIKELY(in->frame_format != UVC_FRAME_FORMAT_YUYV))
1153                 RETURN(UVC_ERROR_INVALID_PARAM, uvc_error_t);
1154
1155         if (UNLIKELY(uvc_ensure_frame_size(out, (in->width * in->height * 3) / 2) < 0))
1156                 RETURN(UVC_ERROR_NO_MEM, uvc_error_t);
1157
1158         const uint8_t *src = in->data;
1159         uint8_t *dest = out->data;
1160         const int32_t width = in->width;
1161         const int32_t height = in->height;
1162         const int32_t src_width = in->step;
1163         const int32_t src_height = in->height;
1164         const int32_t dest_width = out->width = out->step = in->width;
1165         const int32_t dest_height = out->height = in->height;
1166
1167         const uint32_t hh = src_height < dest_height ? src_height : dest_height;
1168         uint8_t *uv = dest + dest_width * dest_height;
1169         int h, w;
1170         for (h = 0; h < hh - 1; h += 2) {
1171                 uint8_t *y0 = dest + width * h;
1172                 uint8_t *y1 = y0 + width;
1173                 const uint8_t *yuv = src + src_width * h;
1174                 for (w = 0; w < width; w += 4) {
1175                         *(y0++) = yuv[0];       // y
1176                         *(y0++) = yuv[2];       // y'
1177                         *(y0++) = yuv[4];       // y''
1178                         *(y0++) = yuv[6];       // y'''
1179                         *(uv++) = yuv[1];       // u
1180                         *(uv++) = yuv[3];       // v
1181                         *(uv++) = yuv[5];       // u
1182                         *(uv++) = yuv[7];       // v
1183                         *(y1++) = yuv[src_width+0];     // y on next low
1184                         *(y1++) = yuv[src_width+2];     // y' on next low
1185                         *(y1++) = yuv[src_width+4];     // y''  on next low
1186                         *(y1++) = yuv[src_width+6];     // y'''  on next low
1187                         yuv += 8;       // (1pixel=2bytes)x4pixels=8bytes
1188                 }
1189         }
1190
1191         RETURN(UVC_SUCCESS, uvc_error_t);
1192 }
1193
1194 uvc_error_t uvc_yuyv2iyuv420SP(uvc_frame_t *in, uvc_frame_t *out) {
1195         ENTER();
1196
1197         if (UNLIKELY(in->frame_format != UVC_FRAME_FORMAT_YUYV))
1198                 RETURN(UVC_ERROR_INVALID_PARAM, uvc_error_t);
1199
1200         if (UNLIKELY(uvc_ensure_frame_size(out, (in->width * in->height * 3) / 2) < 0))
1201                 return UVC_ERROR_NO_MEM;
1202
1203         const uint8_t *src = in->data;
1204         uint8_t *dest =out->data;
1205         const int32_t width = in->width;
1206         const int32_t height = in->height;
1207         const int32_t src_width = in->step;
1208         const int32_t src_height = in->height;
1209         const int32_t dest_width = out->width = out->step = in->width;
1210         const int32_t dest_height = out->height = in->height;
1211
1212         const uint32_t hh = src_height < dest_height ? src_height : dest_height;
1213         uint8_t *uv = dest + dest_width * dest_height;
1214         int h, w;
1215         for (h = 0; h < hh - 1; h += 2) {
1216                 uint8_t *y0 = dest + width * h;
1217                 uint8_t *y1 = y0 + width;
1218                 const uint8_t *yuv = src + src_width * h;
1219                 for (w = 0; w < width; w += 4) {
1220                         *(y0++) = yuv[0];       // y
1221                         *(y0++) = yuv[2];       // y'
1222                         *(y0++) = yuv[4];       // y''
1223                         *(y0++) = yuv[6];       // y'''
1224                         *(uv++) = yuv[3];       // v
1225                         *(uv++) = yuv[1];       // u
1226                         *(uv++) = yuv[7];       // v
1227                         *(uv++) = yuv[5];       // u
1228                         *(y1++) = yuv[src_width+0];     // y on next low
1229                         *(y1++) = yuv[src_width+2];     // y' on next low
1230                         *(y1++) = yuv[src_width+4];     // y''  on next low
1231                         *(y1++) = yuv[src_width+6];     // y'''  on next low
1232                         yuv += 8;       // (1pixel=2bytes)x4pixels=8bytes
1233                 }
1234         }
1235
1236         RETURN(UVC_SUCCESS, uvc_error_t);
1237 }
1238
1239 /** @brief Convert a frame to RGB565
1240  * @ingroup frame
1241  *
1242  * @param in non-RGB565 frame
1243  * @param out RGB565 frame
1244  */
1245 uvc_error_t uvc_any2rgb565(uvc_frame_t *in, uvc_frame_t *out) {
1246
1247         switch (in->frame_format) {
1248 #ifdef LIBUVC_HAS_JPEG
1249         case UVC_FRAME_FORMAT_MJPEG:
1250                 return uvc_mjpeg2rgb565(in, out);
1251 #endif
1252         case UVC_FRAME_FORMAT_YUYV:
1253                 return uvc_yuyv2rgb565(in, out);
1254         case UVC_FRAME_FORMAT_UYVY:
1255                 return uvc_uyvy2rgb565(in, out);
1256         case UVC_FRAME_FORMAT_RGB565:
1257                 return uvc_duplicate_frame(in, out);
1258         case UVC_FRAME_FORMAT_RGB:
1259                 return uvc_rgb2rgb565(in, out);
1260         default:
1261                 return UVC_ERROR_NOT_SUPPORTED;
1262         }
1263 }
1264
1265 /** @brief Convert a frame to RGB888
1266  * @ingroup frame
1267  *
1268  * @param in non-RGB888 frame
1269  * @param out RGB888 frame
1270  */
1271 uvc_error_t uvc_any2rgb(uvc_frame_t *in, uvc_frame_t *out) {
1272
1273         switch (in->frame_format) {
1274 #ifdef LIBUVC_HAS_JPEG
1275         case UVC_FRAME_FORMAT_MJPEG:
1276                 return uvc_mjpeg2rgb(in, out);
1277 #endif
1278         case UVC_FRAME_FORMAT_YUYV:
1279                 return uvc_yuyv2rgb(in, out);
1280         case UVC_FRAME_FORMAT_UYVY:
1281                 return uvc_uyvy2rgb(in, out);
1282         case UVC_FRAME_FORMAT_RGB:
1283                 return uvc_duplicate_frame(in, out);
1284         default:
1285                 return UVC_ERROR_NOT_SUPPORTED;
1286         }
1287 }
1288
1289 /** @brief Convert a frame to BGR888
1290  * @ingroup frame
1291  *
1292  * @param in non-BGR888 frame
1293  * @param out BGR888 frame
1294  */
1295 uvc_error_t uvc_any2bgr(uvc_frame_t *in, uvc_frame_t *out) {
1296
1297         switch (in->frame_format) {
1298 #ifdef LIBUVC_HAS_JPEG
1299         case UVC_FRAME_FORMAT_MJPEG:
1300                 return uvc_mjpeg2bgr(in, out);
1301 #endif
1302         case UVC_FRAME_FORMAT_YUYV:
1303                 return uvc_yuyv2bgr(in, out);
1304         case UVC_FRAME_FORMAT_UYVY:
1305                 return uvc_uyvy2bgr(in, out);
1306         case UVC_FRAME_FORMAT_BGR:
1307                 return uvc_duplicate_frame(in, out);
1308         default:
1309                 return UVC_ERROR_NOT_SUPPORTED;
1310         }
1311 }
1312
1313 /** @brief Convert a frame to RGBX8888
1314  * @ingroup frame
1315  *
1316  * @param in non-rgbx frame
1317  * @param out rgbx frame
1318  */
1319 uvc_error_t uvc_any2rgbx(uvc_frame_t *in, uvc_frame_t *out) {
1320
1321         switch (in->frame_format) {
1322 #ifdef LIBUVC_HAS_JPEG
1323         case UVC_FRAME_FORMAT_MJPEG:
1324                 return uvc_mjpeg2rgbx(in, out);
1325 #endif
1326         case UVC_FRAME_FORMAT_YUYV:
1327                 return uvc_yuyv2rgbx(in, out);
1328         case UVC_FRAME_FORMAT_UYVY:
1329                 return uvc_uyvy2rgbx(in, out);
1330         case UVC_FRAME_FORMAT_RGBX:
1331                 return uvc_duplicate_frame(in, out);
1332         case UVC_FRAME_FORMAT_RGB:
1333                 return uvc_rgb2rgbx(in, out);
1334         default:
1335                 return UVC_ERROR_NOT_SUPPORTED;
1336         }
1337 }
1338
1339 /** @brief Convert a frame to yuyv
1340  * @ingroup frame
1341  *
1342  * @param in non-yuyv frame
1343  * @param out yuyv frame
1344  */
1345 uvc_error_t uvc_any2yuyv(uvc_frame_t *in, uvc_frame_t *out) {
1346
1347         switch (in->frame_format) {
1348 #ifdef LIBUVC_HAS_JPEG
1349         case UVC_FRAME_FORMAT_MJPEG:
1350                 return uvc_mjpeg2yuyv(in, out);
1351 #endif
1352         case UVC_FRAME_FORMAT_YUYV:
1353                 return uvc_duplicate_frame(in, out);
1354         default:
1355                 return UVC_ERROR_NOT_SUPPORTED;
1356         }
1357 }
1358
1359 /** @brief Convert a frame to yuv420sp
1360  * @ingroup frame
1361  *
1362  * @param in non-yuv420sp frame
1363  * @param out yuv420sp frame
1364  */
1365 uvc_error_t uvc_any2yuv420SP(uvc_frame_t *in, uvc_frame_t *out) {
1366         uvc_error_t result = UVC_ERROR_NO_MEM;
1367         uvc_frame_t *yuv = uvc_allocate_frame((in->width * in->height * 3) / 2);
1368         if (yuv) {
1369                 result = uvc_any2yuyv(in, yuv);
1370                 if (LIKELY(!result)) {
1371                         result = uvc_yuyv2yuv420SP(yuv, out);
1372                 }
1373                 uvc_free_frame(yuv);
1374         }
1375         return result;
1376 }
1377
1378 /** @brief Convert a frame to iyuv420sp(NV21)
1379  * @ingroup frame
1380  *
1381  * @param in non-iyuv420SP(NV21) frame
1382  * @param out iyuv420SP(NV21) frame
1383  */
1384 uvc_error_t uvc_any2iyuv420SP(uvc_frame_t *in, uvc_frame_t *out) {
1385         uvc_error_t result = UVC_ERROR_NO_MEM;
1386         uvc_frame_t *yuv = uvc_allocate_frame((in->width * in->height * 3) / 2);
1387         if (yuv) {
1388                 result = uvc_any2yuyv(in, yuv);
1389                 if (LIKELY(!result)) {
1390                         result = uvc_yuyv2iyuv420SP(yuv, out);
1391                 }
1392                 uvc_free_frame(yuv);
1393         }
1394         return result;
1395 }