1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 *  Copyright (C) 2010-2012 Ken Tossell
5 *  All rights reserved.
6 *
7 *  Redistribution and use in source and binary forms, with or without
8 *  modification, are permitted provided that the following conditions
9 *  are met:
10 *
11 *   * Redistributions of source code must retain the above copyright
12 *     notice, this list of conditions and the following disclaimer.
13 *   * Redistributions in binary form must reproduce the above
14 *     copyright notice, this list of conditions and the following
15 *     disclaimer in the documentation and/or other materials provided
16 *     with the distribution.
17 *   * Neither the name of the author nor other contributors may be
18 *     used to endorse or promote products derived from this software
19 *     without specific prior written permission.
20 *
21 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 *  POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 /**
35  * @defgroup frame Frame processing
36  * @brief Tools for managing frame buffers and converting between image formats
37  */
38 #include "libuvc/libuvc.h"
39 #include "libuvc/libuvc_internal.h"
40 
41 /** @internal */
uvc_ensure_frame_size(uvc_frame_t * frame,size_t need_bytes)42 uvc_error_t uvc_ensure_frame_size(uvc_frame_t *frame, size_t need_bytes) {
43   if (frame->library_owns_data) {
44     if (!frame->data || frame->data_bytes != need_bytes) {
45       frame->data_bytes = need_bytes;
46       frame->data = realloc(frame->data, frame->data_bytes);
47     }
48     if (!frame->data)
49       return UVC_ERROR_NO_MEM;
50     return UVC_SUCCESS;
51   } else {
52     if (!frame->data || frame->data_bytes < need_bytes)
53       return UVC_ERROR_NO_MEM;
54     return UVC_SUCCESS;
55   }
56 }
57 
58 /** @brief Allocate a frame structure
59  * @ingroup frame
60  *
61  * @param data_bytes Number of bytes to allocate, or zero
62  * @return New frame, or NULL on error
63  */
uvc_allocate_frame(size_t data_bytes)64 uvc_frame_t *uvc_allocate_frame(size_t data_bytes) {
65   uvc_frame_t *frame = malloc(sizeof(*frame));
66 
67   if (!frame)
68     return NULL;
69 
70   memset(frame, 0, sizeof(*frame));
71 
72   frame->library_owns_data = 1;
73 
74   if (data_bytes > 0) {
75     frame->data_bytes = data_bytes;
76     frame->data = malloc(data_bytes);
77 
78     if (!frame->data) {
79       free(frame);
80       return NULL;
81     }
82   }
83 
84   return frame;
85 }
86 
87 /** @brief Free a frame structure
88  * @ingroup frame
89  *
90  * @param frame Frame to destroy
91  */
uvc_free_frame(uvc_frame_t * frame)92 void uvc_free_frame(uvc_frame_t *frame) {
93   if (frame->library_owns_data)
94   {
95     if (frame->data_bytes > 0)
96       free(frame->data);
97     if (frame->metadata_bytes > 0)
98       free(frame->metadata);
99   }
100 
101   free(frame);
102 }
103 
sat(int i)104 static inline unsigned char sat(int i) {
105   return (unsigned char)( i >= 255 ? 255 : (i < 0 ? 0 : i));
106 }
107 
108 /** @brief Duplicate a frame, preserving color format
109  * @ingroup frame
110  *
111  * @param in Original frame
112  * @param out Duplicate frame
113  */
uvc_duplicate_frame(uvc_frame_t * in,uvc_frame_t * out)114 uvc_error_t uvc_duplicate_frame(uvc_frame_t *in, uvc_frame_t *out) {
115   if (uvc_ensure_frame_size(out, in->data_bytes) < 0)
116     return UVC_ERROR_NO_MEM;
117 
118   out->width = in->width;
119   out->height = in->height;
120   out->frame_format = in->frame_format;
121   out->step = in->step;
122   out->sequence = in->sequence;
123   out->capture_time = in->capture_time;
124   out->capture_time_finished = in->capture_time_finished;
125   out->source = in->source;
126 
127   memcpy(out->data, in->data, in->data_bytes);
128 
129   if (in->metadata && in->metadata_bytes > 0)
130   {
131       if (out->metadata_bytes < in->metadata_bytes)
132       {
133           out->metadata = realloc(out->metadata, in->metadata_bytes);
134       }
135       out->metadata_bytes = in->metadata_bytes;
136       memcpy(out->metadata, in->metadata, in->metadata_bytes);
137   }
138 
139   return UVC_SUCCESS;
140 }
141 
142 #define YUYV2RGB_2(pyuv, prgb) { \
143     float r = 1.402f * ((pyuv)[3]-128); \
144     float g = -0.34414f * ((pyuv)[1]-128) - 0.71414f * ((pyuv)[3]-128); \
145     float b = 1.772f * ((pyuv)[1]-128); \
146     (prgb)[0] = sat(pyuv[0] + r); \
147     (prgb)[1] = sat(pyuv[0] + g); \
148     (prgb)[2] = sat(pyuv[0] + b); \
149     (prgb)[3] = sat(pyuv[2] + r); \
150     (prgb)[4] = sat(pyuv[2] + g); \
151     (prgb)[5] = sat(pyuv[2] + b); \
152     }
153 #define IYUYV2RGB_2(pyuv, prgb) { \
154     int r = (22987 * ((pyuv)[3] - 128)) >> 14; \
155     int g = (-5636 * ((pyuv)[1] - 128) - 11698 * ((pyuv)[3] - 128)) >> 14; \
156     int b = (29049 * ((pyuv)[1] - 128)) >> 14; \
157     (prgb)[0] = sat(*(pyuv) + r); \
158     (prgb)[1] = sat(*(pyuv) + g); \
159     (prgb)[2] = sat(*(pyuv) + b); \
160     (prgb)[3] = sat((pyuv)[2] + r); \
161     (prgb)[4] = sat((pyuv)[2] + g); \
162     (prgb)[5] = sat((pyuv)[2] + b); \
163     }
164 #define IYUYV2RGB_16(pyuv, prgb) IYUYV2RGB_8(pyuv, prgb); IYUYV2RGB_8(pyuv + 16, prgb + 24);
165 #define IYUYV2RGB_8(pyuv, prgb) IYUYV2RGB_4(pyuv, prgb); IYUYV2RGB_4(pyuv + 8, prgb + 12);
166 #define IYUYV2RGB_4(pyuv, prgb) IYUYV2RGB_2(pyuv, prgb); IYUYV2RGB_2(pyuv + 4, prgb + 6);
167 
168 /** @brief Convert a frame from YUYV to RGB
169  * @ingroup frame
170  *
171  * @param in YUYV frame
172  * @param out RGB frame
173  */
uvc_yuyv2rgb(uvc_frame_t * in,uvc_frame_t * out)174 uvc_error_t uvc_yuyv2rgb(uvc_frame_t *in, uvc_frame_t *out) {
175   if (in->frame_format != UVC_FRAME_FORMAT_YUYV)
176     return UVC_ERROR_INVALID_PARAM;
177 
178   if (uvc_ensure_frame_size(out, in->width * in->height * 3) < 0)
179     return UVC_ERROR_NO_MEM;
180 
181   out->width = in->width;
182   out->height = in->height;
183   out->frame_format = UVC_FRAME_FORMAT_RGB;
184   out->step = in->width * 3;
185   out->sequence = in->sequence;
186   out->capture_time = in->capture_time;
187   out->capture_time_finished = in->capture_time_finished;
188   out->source = in->source;
189 
190   uint8_t *pyuv = in->data;
191   uint8_t *prgb = out->data;
192   uint8_t *prgb_end = prgb + out->data_bytes;
193 
194   while (prgb < prgb_end) {
195     IYUYV2RGB_8(pyuv, prgb);
196 
197     prgb += 3 * 8;
198     pyuv += 2 * 8;
199   }
200 
201   return UVC_SUCCESS;
202 }
203 
204 #define IYUYV2BGR_2(pyuv, pbgr) { \
205     int r = (22987 * ((pyuv)[3] - 128)) >> 14; \
206     int g = (-5636 * ((pyuv)[1] - 128) - 11698 * ((pyuv)[3] - 128)) >> 14; \
207     int b = (29049 * ((pyuv)[1] - 128)) >> 14; \
208     (pbgr)[0] = sat(*(pyuv) + b); \
209     (pbgr)[1] = sat(*(pyuv) + g); \
210     (pbgr)[2] = sat(*(pyuv) + r); \
211     (pbgr)[3] = sat((pyuv)[2] + b); \
212     (pbgr)[4] = sat((pyuv)[2] + g); \
213     (pbgr)[5] = sat((pyuv)[2] + r); \
214     }
215 #define IYUYV2BGR_16(pyuv, pbgr) IYUYV2BGR_8(pyuv, pbgr); IYUYV2BGR_8(pyuv + 16, pbgr + 24);
216 #define IYUYV2BGR_8(pyuv, pbgr) IYUYV2BGR_4(pyuv, pbgr); IYUYV2BGR_4(pyuv + 8, pbgr + 12);
217 #define IYUYV2BGR_4(pyuv, pbgr) IYUYV2BGR_2(pyuv, pbgr); IYUYV2BGR_2(pyuv + 4, pbgr + 6);
218 
219 /** @brief Convert a frame from YUYV to BGR
220  * @ingroup frame
221  *
222  * @param in YUYV frame
223  * @param out BGR frame
224  */
uvc_yuyv2bgr(uvc_frame_t * in,uvc_frame_t * out)225 uvc_error_t uvc_yuyv2bgr(uvc_frame_t *in, uvc_frame_t *out) {
226   if (in->frame_format != UVC_FRAME_FORMAT_YUYV)
227     return UVC_ERROR_INVALID_PARAM;
228 
229   if (uvc_ensure_frame_size(out, in->width * in->height * 3) < 0)
230     return UVC_ERROR_NO_MEM;
231 
232   out->width = in->width;
233   out->height = in->height;
234   out->frame_format = UVC_FRAME_FORMAT_BGR;
235   out->step = in->width * 3;
236   out->sequence = in->sequence;
237   out->capture_time = in->capture_time;
238   out->capture_time_finished = in->capture_time_finished;
239   out->source = in->source;
240 
241   uint8_t *pyuv = in->data;
242   uint8_t *pbgr = out->data;
243   uint8_t *pbgr_end = pbgr + out->data_bytes;
244 
245   while (pbgr < pbgr_end) {
246     IYUYV2BGR_8(pyuv, pbgr);
247 
248     pbgr += 3 * 8;
249     pyuv += 2 * 8;
250   }
251 
252   return UVC_SUCCESS;
253 }
254 
255 #define IYUYV2Y(pyuv, py) { \
256     (py)[0] = (pyuv[0]); \
257     }
258 
259 /** @brief Convert a frame from YUYV to Y (GRAY8)
260  * @ingroup frame
261  *
262  * @param in YUYV frame
263  * @param out GRAY8 frame
264  */
uvc_yuyv2y(uvc_frame_t * in,uvc_frame_t * out)265 uvc_error_t uvc_yuyv2y(uvc_frame_t *in, uvc_frame_t *out) {
266   if (in->frame_format != UVC_FRAME_FORMAT_YUYV)
267     return UVC_ERROR_INVALID_PARAM;
268 
269   if (uvc_ensure_frame_size(out, in->width * in->height) < 0)
270     return UVC_ERROR_NO_MEM;
271 
272   out->width = in->width;
273   out->height = in->height;
274   out->frame_format = UVC_FRAME_FORMAT_GRAY8;
275   out->step = in->width;
276   out->sequence = in->sequence;
277   out->capture_time = in->capture_time;
278   out->capture_time_finished = in->capture_time_finished;
279   out->source = in->source;
280 
281   uint8_t *pyuv = in->data;
282   uint8_t *py = out->data;
283   uint8_t *py_end = py + out->data_bytes;
284 
285   while (py < py_end) {
286     IYUYV2Y(pyuv, py);
287 
288     py += 1;
289     pyuv += 2;
290   }
291 
292   return UVC_SUCCESS;
293 }
294 
295 #define IYUYV2UV(pyuv, puv) { \
296     (puv)[0] = (pyuv[1]); \
297     }
298 
299 /** @brief Convert a frame from YUYV to UV (GRAY8)
300  * @ingroup frame
301  *
302  * @param in YUYV frame
303  * @param out GRAY8 frame
304  */
uvc_yuyv2uv(uvc_frame_t * in,uvc_frame_t * out)305 uvc_error_t uvc_yuyv2uv(uvc_frame_t *in, uvc_frame_t *out) {
306   if (in->frame_format != UVC_FRAME_FORMAT_YUYV)
307     return UVC_ERROR_INVALID_PARAM;
308 
309   if (uvc_ensure_frame_size(out, in->width * in->height) < 0)
310     return UVC_ERROR_NO_MEM;
311 
312   out->width = in->width;
313   out->height = in->height;
314   out->frame_format = UVC_FRAME_FORMAT_GRAY8;
315   out->step = in->width;
316   out->sequence = in->sequence;
317   out->capture_time = in->capture_time;
318   out->capture_time_finished = in->capture_time_finished;
319   out->source = in->source;
320 
321   uint8_t *pyuv = in->data;
322   uint8_t *puv = out->data;
323   uint8_t *puv_end = puv + out->data_bytes;
324 
325   while (puv < puv_end) {
326     IYUYV2UV(pyuv, puv);
327 
328     puv += 1;
329     pyuv += 2;
330   }
331 
332   return UVC_SUCCESS;
333 }
334 
335 #define IUYVY2RGB_2(pyuv, prgb) { \
336     int r = (22987 * ((pyuv)[2] - 128)) >> 14; \
337     int g = (-5636 * ((pyuv)[0] - 128) - 11698 * ((pyuv)[2] - 128)) >> 14; \
338     int b = (29049 * ((pyuv)[0] - 128)) >> 14; \
339     (prgb)[0] = sat((pyuv)[1] + r); \
340     (prgb)[1] = sat((pyuv)[1] + g); \
341     (prgb)[2] = sat((pyuv)[1] + b); \
342     (prgb)[3] = sat((pyuv)[3] + r); \
343     (prgb)[4] = sat((pyuv)[3] + g); \
344     (prgb)[5] = sat((pyuv)[3] + b); \
345     }
346 #define IUYVY2RGB_16(pyuv, prgb) IUYVY2RGB_8(pyuv, prgb); IUYVY2RGB_8(pyuv + 16, prgb + 24);
347 #define IUYVY2RGB_8(pyuv, prgb) IUYVY2RGB_4(pyuv, prgb); IUYVY2RGB_4(pyuv + 8, prgb + 12);
348 #define IUYVY2RGB_4(pyuv, prgb) IUYVY2RGB_2(pyuv, prgb); IUYVY2RGB_2(pyuv + 4, prgb + 6);
349 
350 /** @brief Convert a frame from UYVY to RGB
351  * @ingroup frame
352  * @param ini UYVY frame
353  * @param out RGB frame
354  */
uvc_uyvy2rgb(uvc_frame_t * in,uvc_frame_t * out)355 uvc_error_t uvc_uyvy2rgb(uvc_frame_t *in, uvc_frame_t *out) {
356   if (in->frame_format != UVC_FRAME_FORMAT_UYVY)
357     return UVC_ERROR_INVALID_PARAM;
358 
359   if (uvc_ensure_frame_size(out, in->width * in->height * 3) < 0)
360     return UVC_ERROR_NO_MEM;
361 
362   out->width = in->width;
363   out->height = in->height;
364   out->frame_format = UVC_FRAME_FORMAT_RGB;
365   out->step = in->width *3;
366   out->sequence = in->sequence;
367   out->capture_time = in->capture_time;
368   out->capture_time_finished = in->capture_time_finished;
369   out->source = in->source;
370 
371   uint8_t *pyuv = in->data;
372   uint8_t *prgb = out->data;
373   uint8_t *prgb_end = prgb + out->data_bytes;
374 
375   while (prgb < prgb_end) {
376     IUYVY2RGB_8(pyuv, prgb);
377 
378     prgb += 3 * 8;
379     pyuv += 2 * 8;
380   }
381 
382   return UVC_SUCCESS;
383 }
384 
385 #define IUYVY2BGR_2(pyuv, pbgr) { \
386     int r = (22987 * ((pyuv)[2] - 128)) >> 14; \
387     int g = (-5636 * ((pyuv)[0] - 128) - 11698 * ((pyuv)[2] - 128)) >> 14; \
388     int b = (29049 * ((pyuv)[0] - 128)) >> 14; \
389     (pbgr)[0] = sat((pyuv)[1] + b); \
390     (pbgr)[1] = sat((pyuv)[1] + g); \
391     (pbgr)[2] = sat((pyuv)[1] + r); \
392     (pbgr)[3] = sat((pyuv)[3] + b); \
393     (pbgr)[4] = sat((pyuv)[3] + g); \
394     (pbgr)[5] = sat((pyuv)[3] + r); \
395     }
396 #define IUYVY2BGR_16(pyuv, pbgr) IUYVY2BGR_8(pyuv, pbgr); IUYVY2BGR_8(pyuv + 16, pbgr + 24);
397 #define IUYVY2BGR_8(pyuv, pbgr) IUYVY2BGR_4(pyuv, pbgr); IUYVY2BGR_4(pyuv + 8, pbgr + 12);
398 #define IUYVY2BGR_4(pyuv, pbgr) IUYVY2BGR_2(pyuv, pbgr); IUYVY2BGR_2(pyuv + 4, pbgr + 6);
399 
400 /** @brief Convert a frame from UYVY to BGR
401  * @ingroup frame
402  * @param ini UYVY frame
403  * @param out BGR frame
404  */
uvc_uyvy2bgr(uvc_frame_t * in,uvc_frame_t * out)405 uvc_error_t uvc_uyvy2bgr(uvc_frame_t *in, uvc_frame_t *out) {
406   if (in->frame_format != UVC_FRAME_FORMAT_UYVY)
407     return UVC_ERROR_INVALID_PARAM;
408 
409   if (uvc_ensure_frame_size(out, in->width * in->height * 3) < 0)
410     return UVC_ERROR_NO_MEM;
411 
412   out->width = in->width;
413   out->height = in->height;
414   out->frame_format = UVC_FRAME_FORMAT_BGR;
415   out->step = in->width *3;
416   out->sequence = in->sequence;
417   out->capture_time = in->capture_time;
418   out->capture_time_finished = in->capture_time_finished;
419   out->source = in->source;
420 
421   uint8_t *pyuv = in->data;
422   uint8_t *pbgr = out->data;
423   uint8_t *pbgr_end = pbgr + out->data_bytes;
424 
425   while (pbgr < pbgr_end) {
426     IUYVY2BGR_8(pyuv, pbgr);
427 
428     pbgr += 3 * 8;
429     pyuv += 2 * 8;
430   }
431 
432   return UVC_SUCCESS;
433 }
434 
435 /** @brief Convert a frame to RGB
436  * @ingroup frame
437  *
438  * @param in non-RGB frame
439  * @param out RGB frame
440  */
uvc_any2rgb(uvc_frame_t * in,uvc_frame_t * out)441 uvc_error_t uvc_any2rgb(uvc_frame_t *in, uvc_frame_t *out) {
442   switch (in->frame_format) {
443 #ifdef LIBUVC_HAS_JPEG
444     case UVC_FRAME_FORMAT_MJPEG:
445       return uvc_mjpeg2rgb(in, out);
446 #endif
447     case UVC_FRAME_FORMAT_YUYV:
448       return uvc_yuyv2rgb(in, out);
449     case UVC_FRAME_FORMAT_UYVY:
450       return uvc_uyvy2rgb(in, out);
451     case UVC_FRAME_FORMAT_RGB:
452       return uvc_duplicate_frame(in, out);
453     default:
454       return UVC_ERROR_NOT_SUPPORTED;
455   }
456 }
457 
458 /** @brief Convert a frame to BGR
459  * @ingroup frame
460  *
461  * @param in non-BGR frame
462  * @param out BGR frame
463  */
uvc_any2bgr(uvc_frame_t * in,uvc_frame_t * out)464 uvc_error_t uvc_any2bgr(uvc_frame_t *in, uvc_frame_t *out) {
465   switch (in->frame_format) {
466     case UVC_FRAME_FORMAT_YUYV:
467       return uvc_yuyv2bgr(in, out);
468     case UVC_FRAME_FORMAT_UYVY:
469       return uvc_uyvy2bgr(in, out);
470     case UVC_FRAME_FORMAT_BGR:
471       return uvc_duplicate_frame(in, out);
472     default:
473       return UVC_ERROR_NOT_SUPPORTED;
474   }
475 }
476