1 /*
2
3 # RGB <-> YUV conversion routines
4 # (C) 2008 Hans de Goede <hdegoede@redhat.com>
5
6 # RGB565 conversion routines
7 # (C) 2009 Mauro Carvalho Chehab
8
9 # This program is free software; you can redistribute it and/or modify
10 # it under the terms of the GNU Lesser General Public License as published by
11 # the Free Software Foundation; either version 2.1 of the License, or
12 # (at your option) any later version.
13 #
14 # This program is distributed in the hope that it will be useful,
15 # but WITHOUT ANY WARRANTY; without even the implied warranty of
16 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 # GNU Lesser General Public License for more details.
18 #
19 # You should have received a copy of the GNU Lesser General Public License
20 # along with this program; if not, write to the Free Software
21 # Foundation, Inc., 51 Franklin Street, Suite 500, Boston, MA 02110-1335 USA
22
23 */
24
25 #include <string.h>
26 #include "libv4lconvert-priv.h"
27
28 #define RGB2Y(r, g, b, y) \
29 (y) = ((8453 * (r) + 16594 * (g) + 3223 * (b) + 524288) >> 15)
30
31 #define RGB2UV(r, g, b, u, v) \
32 do { \
33 (u) = ((-4878 * (r) - 9578 * (g) + 14456 * (b) + 4210688) >> 15); \
34 (v) = ((14456 * (r) - 12105 * (g) - 2351 * (b) + 4210688) >> 15); \
35 } while (0)
36
v4lconvert_rgb24_to_yuv420(const unsigned char * src,unsigned char * dest,const struct v4l2_format * src_fmt,int bgr,int yvu,int bpp)37 void v4lconvert_rgb24_to_yuv420(const unsigned char *src, unsigned char *dest,
38 const struct v4l2_format *src_fmt, int bgr, int yvu, int bpp)
39 {
40 int x, y;
41 unsigned char *udest, *vdest;
42
43 /* Y */
44 for (y = 0; y < src_fmt->fmt.pix.height; y++) {
45 for (x = 0; x < src_fmt->fmt.pix.width; x++) {
46 if (bgr)
47 RGB2Y(src[2], src[1], src[0], *dest++);
48 else
49 RGB2Y(src[0], src[1], src[2], *dest++);
50 src += bpp;
51 }
52
53 src += src_fmt->fmt.pix.bytesperline - bpp * src_fmt->fmt.pix.width;
54 }
55 src -= src_fmt->fmt.pix.height * src_fmt->fmt.pix.bytesperline;
56
57 /* U + V */
58 if (yvu) {
59 vdest = dest;
60 udest = dest + src_fmt->fmt.pix.width * src_fmt->fmt.pix.height / 4;
61 } else {
62 udest = dest;
63 vdest = dest + src_fmt->fmt.pix.width * src_fmt->fmt.pix.height / 4;
64 }
65
66 for (y = 0; y < src_fmt->fmt.pix.height / 2; y++) {
67 for (x = 0; x < src_fmt->fmt.pix.width / 2; x++) {
68 int avg_src[3];
69
70 avg_src[0] = (src[0] + src[bpp] + src[src_fmt->fmt.pix.bytesperline] +
71 src[src_fmt->fmt.pix.bytesperline + bpp]) / 4;
72 avg_src[1] = (src[1] + src[bpp + 1] + src[src_fmt->fmt.pix.bytesperline + 1] +
73 src[src_fmt->fmt.pix.bytesperline + bpp + 1]) / 4;
74 avg_src[2] = (src[2] + src[bpp + 2] + src[src_fmt->fmt.pix.bytesperline + 2] +
75 src[src_fmt->fmt.pix.bytesperline + bpp + 2]) / 4;
76 if (bgr)
77 RGB2UV(avg_src[2], avg_src[1], avg_src[0], *udest++, *vdest++);
78 else
79 RGB2UV(avg_src[0], avg_src[1], avg_src[2], *udest++, *vdest++);
80 src += 2 * bpp;
81 }
82 src += 2 * src_fmt->fmt.pix.bytesperline - bpp * src_fmt->fmt.pix.width;
83 }
84 }
85
86 #define YUV2R(y, u, v) ({ \
87 int r = (y) + ((((v) - 128) * 1436) >> 10); r > 255 ? 255 : r < 0 ? 0 : r; })
88 #define YUV2G(y, u, v) ({ \
89 int g = (y) - ((((u) - 128) * 352 + ((v) - 128) * 731) >> 10); g > 255 ? 255 : g < 0 ? 0 : g; })
90 #define YUV2B(y, u, v) ({ \
91 int b = (y) + ((((u) - 128) * 1814) >> 10); b > 255 ? 255 : b < 0 ? 0 : b; })
92
93 #define CLIP(color) (unsigned char)(((color) > 0xFF) ? 0xff : (((color) < 0) ? 0 : (color)))
94
v4lconvert_yuv420_to_bgr24(const unsigned char * src,unsigned char * dest,int width,int height,int stride,int yvu)95 void v4lconvert_yuv420_to_bgr24(const unsigned char *src, unsigned char *dest,
96 int width, int height, int stride, int yvu)
97 {
98 int i, j;
99
100 const unsigned char *ysrc = src;
101 const unsigned char *usrc, *vsrc;
102
103 if (yvu) {
104 vsrc = src + stride * height;
105 usrc = vsrc + (stride * height) / 4;
106 } else {
107 usrc = src + stride * height;
108 vsrc = usrc + (stride * height) / 4;
109 }
110
111 for (i = 0; i < height; i++) {
112 for (j = 0; j < width; j += 2) {
113 #if 1 /* fast slightly less accurate multiplication free code */
114 int u1 = (((*usrc - 128) << 7) + (*usrc - 128)) >> 6;
115 int rg = (((*usrc - 128) << 1) + (*usrc - 128) +
116 ((*vsrc - 128) << 2) + ((*vsrc - 128) << 1)) >> 3;
117 int v1 = (((*vsrc - 128) << 1) + (*vsrc - 128)) >> 1;
118
119 *dest++ = CLIP(*ysrc + u1);
120 *dest++ = CLIP(*ysrc - rg);
121 *dest++ = CLIP(*ysrc + v1);
122 ysrc++;
123
124 *dest++ = CLIP(*ysrc + u1);
125 *dest++ = CLIP(*ysrc - rg);
126 *dest++ = CLIP(*ysrc + v1);
127 #else
128 *dest++ = YUV2B(*ysrc, *usrc, *vsrc);
129 *dest++ = YUV2G(*ysrc, *usrc, *vsrc);
130 *dest++ = YUV2R(*ysrc, *usrc, *vsrc);
131 ysrc++;
132
133 *dest++ = YUV2B(*ysrc, *usrc, *vsrc);
134 *dest++ = YUV2G(*ysrc, *usrc, *vsrc);
135 *dest++ = YUV2R(*ysrc, *usrc, *vsrc);
136 #endif
137 ysrc++;
138 usrc++;
139 vsrc++;
140 }
141 ysrc += stride - width;
142 /* Rewind u and v for next line */
143 if (!(i & 1)) {
144 usrc -= width / 2;
145 vsrc -= width / 2;
146 } else {
147 usrc += (stride - width) / 2;
148 vsrc += (stride - width) / 2;
149 }
150 }
151 }
152
v4lconvert_yuv420_to_rgb24(const unsigned char * src,unsigned char * dest,int width,int height,int stride,int yvu)153 void v4lconvert_yuv420_to_rgb24(const unsigned char *src, unsigned char *dest,
154 int width, int height, int stride, int yvu)
155 {
156 int i, j;
157
158 const unsigned char *ysrc = src;
159 const unsigned char *usrc, *vsrc;
160
161 if (yvu) {
162 vsrc = src + stride * height;
163 usrc = vsrc + (stride * height) / 4;
164 } else {
165 usrc = src + stride * height;
166 vsrc = usrc + (stride * height) / 4;
167 }
168
169 for (i = 0; i < height; i++) {
170 for (j = 0; j < width; j += 2) {
171 #if 1 /* fast slightly less accurate multiplication free code */
172 int u1 = (((*usrc - 128) << 7) + (*usrc - 128)) >> 6;
173 int rg = (((*usrc - 128) << 1) + (*usrc - 128) +
174 ((*vsrc - 128) << 2) + ((*vsrc - 128) << 1)) >> 3;
175 int v1 = (((*vsrc - 128) << 1) + (*vsrc - 128)) >> 1;
176
177 *dest++ = CLIP(*ysrc + v1);
178 *dest++ = CLIP(*ysrc - rg);
179 *dest++ = CLIP(*ysrc + u1);
180 ysrc++;
181
182 *dest++ = CLIP(*ysrc + v1);
183 *dest++ = CLIP(*ysrc - rg);
184 *dest++ = CLIP(*ysrc + u1);
185 #else
186 *dest++ = YUV2R(*ysrc, *usrc, *vsrc);
187 *dest++ = YUV2G(*ysrc, *usrc, *vsrc);
188 *dest++ = YUV2B(*ysrc, *usrc, *vsrc);
189 ysrc++;
190
191 *dest++ = YUV2R(*ysrc, *usrc, *vsrc);
192 *dest++ = YUV2G(*ysrc, *usrc, *vsrc);
193 *dest++ = YUV2B(*ysrc, *usrc, *vsrc);
194 #endif
195 ysrc++;
196 usrc++;
197 vsrc++;
198 }
199 ysrc += stride - width;
200 /* Rewind u and v for next line */
201 if (!(i & 1)) {
202 usrc -= width / 2;
203 vsrc -= width / 2;
204 } else {
205 usrc += (stride - width) / 2;
206 vsrc += (stride - width) / 2;
207 }
208 }
209 }
210
v4lconvert_yuyv_to_bgr24(const unsigned char * src,unsigned char * dest,int width,int height,int stride)211 void v4lconvert_yuyv_to_bgr24(const unsigned char *src, unsigned char *dest,
212 int width, int height, int stride)
213 {
214 int j;
215
216 while (--height >= 0) {
217 for (j = 0; j + 1 < width; j += 2) {
218 int u = src[1];
219 int v = src[3];
220 int u1 = (((u - 128) << 7) + (u - 128)) >> 6;
221 int rg = (((u - 128) << 1) + (u - 128) +
222 ((v - 128) << 2) + ((v - 128) << 1)) >> 3;
223 int v1 = (((v - 128) << 1) + (v - 128)) >> 1;
224
225 *dest++ = CLIP(src[0] + u1);
226 *dest++ = CLIP(src[0] - rg);
227 *dest++ = CLIP(src[0] + v1);
228
229 *dest++ = CLIP(src[2] + u1);
230 *dest++ = CLIP(src[2] - rg);
231 *dest++ = CLIP(src[2] + v1);
232 src += 4;
233 }
234 src += stride - width * 2;
235 }
236 }
237
v4lconvert_yuyv_to_rgb24(const unsigned char * src,unsigned char * dest,int width,int height,int stride)238 void v4lconvert_yuyv_to_rgb24(const unsigned char *src, unsigned char *dest,
239 int width, int height, int stride)
240 {
241 int j;
242
243 while (--height >= 0) {
244 for (j = 0; j + 1 < width; j += 2) {
245 int u = src[1];
246 int v = src[3];
247 int u1 = (((u - 128) << 7) + (u - 128)) >> 6;
248 int rg = (((u - 128) << 1) + (u - 128) +
249 ((v - 128) << 2) + ((v - 128) << 1)) >> 3;
250 int v1 = (((v - 128) << 1) + (v - 128)) >> 1;
251
252 *dest++ = CLIP(src[0] + v1);
253 *dest++ = CLIP(src[0] - rg);
254 *dest++ = CLIP(src[0] + u1);
255
256 *dest++ = CLIP(src[2] + v1);
257 *dest++ = CLIP(src[2] - rg);
258 *dest++ = CLIP(src[2] + u1);
259 src += 4;
260 }
261 src += stride - (width * 2);
262 }
263 }
264
v4lconvert_yuyv_to_yuv420(const unsigned char * src,unsigned char * dest,int width,int height,int stride,int yvu)265 void v4lconvert_yuyv_to_yuv420(const unsigned char *src, unsigned char *dest,
266 int width, int height, int stride, int yvu)
267 {
268 int i, j;
269 const unsigned char *src1;
270 unsigned char *udest, *vdest;
271
272 /* copy the Y values */
273 src1 = src;
274 for (i = 0; i < height; i++) {
275 for (j = 0; j + 1 < width; j += 2) {
276 *dest++ = src1[0];
277 *dest++ = src1[2];
278 src1 += 4;
279 }
280 src1 += stride - width * 2;
281 }
282
283 /* copy the U and V values */
284 src++; /* point to V */
285 src1 = src + stride; /* next line */
286 if (yvu) {
287 vdest = dest;
288 udest = dest + width * height / 4;
289 } else {
290 udest = dest;
291 vdest = dest + width * height / 4;
292 }
293 for (i = 0; i < height; i += 2) {
294 for (j = 0; j + 1 < width; j += 2) {
295 *udest++ = ((int) src[0] + src1[0]) / 2; /* U */
296 *vdest++ = ((int) src[2] + src1[2]) / 2; /* V */
297 src += 4;
298 src1 += 4;
299 }
300 src1 += stride - width * 2;
301 src = src1;
302 src1 += stride;
303 }
304 }
305
v4lconvert_nv16_to_yuyv(const unsigned char * src,unsigned char * dest,int width,int height,int stride)306 void v4lconvert_nv16_to_yuyv(const unsigned char *src, unsigned char *dest,
307 int width, int height, int stride)
308 {
309 const unsigned char *y, *cbcr;
310 int i, j;
311
312 y = src;
313 cbcr = src + stride * height;
314
315 for (i = 0; i < height; i++) {
316 for (j = 0; j < width; j++) {
317 *dest++ = *y++;
318 *dest++ = *cbcr++;
319 }
320 y += stride - width;
321 cbcr += stride - width;
322 }
323 }
324
v4lconvert_yvyu_to_bgr24(const unsigned char * src,unsigned char * dest,int width,int height,int stride)325 void v4lconvert_yvyu_to_bgr24(const unsigned char *src, unsigned char *dest,
326 int width, int height, int stride)
327 {
328 int j;
329
330 while (--height >= 0) {
331 for (j = 0; j + 1 < width; j += 2) {
332 int u = src[3];
333 int v = src[1];
334 int u1 = (((u - 128) << 7) + (u - 128)) >> 6;
335 int rg = (((u - 128) << 1) + (u - 128) +
336 ((v - 128) << 2) + ((v - 128) << 1)) >> 3;
337 int v1 = (((v - 128) << 1) + (v - 128)) >> 1;
338
339 *dest++ = CLIP(src[0] + u1);
340 *dest++ = CLIP(src[0] - rg);
341 *dest++ = CLIP(src[0] + v1);
342
343 *dest++ = CLIP(src[2] + u1);
344 *dest++ = CLIP(src[2] - rg);
345 *dest++ = CLIP(src[2] + v1);
346 src += 4;
347 }
348 src += stride - (width * 2);
349 }
350 }
351
v4lconvert_yvyu_to_rgb24(const unsigned char * src,unsigned char * dest,int width,int height,int stride)352 void v4lconvert_yvyu_to_rgb24(const unsigned char *src, unsigned char *dest,
353 int width, int height, int stride)
354 {
355 int j;
356
357 while (--height >= 0) {
358 for (j = 0; j + 1 < width; j += 2) {
359 int u = src[3];
360 int v = src[1];
361 int u1 = (((u - 128) << 7) + (u - 128)) >> 6;
362 int rg = (((u - 128) << 1) + (u - 128) +
363 ((v - 128) << 2) + ((v - 128) << 1)) >> 3;
364 int v1 = (((v - 128) << 1) + (v - 128)) >> 1;
365
366 *dest++ = CLIP(src[0] + v1);
367 *dest++ = CLIP(src[0] - rg);
368 *dest++ = CLIP(src[0] + u1);
369
370 *dest++ = CLIP(src[2] + v1);
371 *dest++ = CLIP(src[2] - rg);
372 *dest++ = CLIP(src[2] + u1);
373 src += 4;
374 }
375 src += stride - (width * 2);
376 }
377 }
378
v4lconvert_uyvy_to_bgr24(const unsigned char * src,unsigned char * dest,int width,int height,int stride)379 void v4lconvert_uyvy_to_bgr24(const unsigned char *src, unsigned char *dest,
380 int width, int height, int stride)
381 {
382 int j;
383
384 while (--height >= 0) {
385 for (j = 0; j + 1 < width; j += 2) {
386 int u = src[0];
387 int v = src[2];
388 int u1 = (((u - 128) << 7) + (u - 128)) >> 6;
389 int rg = (((u - 128) << 1) + (u - 128) +
390 ((v - 128) << 2) + ((v - 128) << 1)) >> 3;
391 int v1 = (((v - 128) << 1) + (v - 128)) >> 1;
392
393 *dest++ = CLIP(src[1] + u1);
394 *dest++ = CLIP(src[1] - rg);
395 *dest++ = CLIP(src[1] + v1);
396
397 *dest++ = CLIP(src[3] + u1);
398 *dest++ = CLIP(src[3] - rg);
399 *dest++ = CLIP(src[3] + v1);
400 src += 4;
401 }
402 src += stride - width * 2;
403 }
404 }
405
v4lconvert_uyvy_to_rgb24(const unsigned char * src,unsigned char * dest,int width,int height,int stride)406 void v4lconvert_uyvy_to_rgb24(const unsigned char *src, unsigned char *dest,
407 int width, int height, int stride)
408 {
409 int j;
410
411 while (--height >= 0) {
412 for (j = 0; j + 1 < width; j += 2) {
413 int u = src[0];
414 int v = src[2];
415 int u1 = (((u - 128) << 7) + (u - 128)) >> 6;
416 int rg = (((u - 128) << 1) + (u - 128) +
417 ((v - 128) << 2) + ((v - 128) << 1)) >> 3;
418 int v1 = (((v - 128) << 1) + (v - 128)) >> 1;
419
420 *dest++ = CLIP(src[1] + v1);
421 *dest++ = CLIP(src[1] - rg);
422 *dest++ = CLIP(src[1] + u1);
423
424 *dest++ = CLIP(src[3] + v1);
425 *dest++ = CLIP(src[3] - rg);
426 *dest++ = CLIP(src[3] + u1);
427 src += 4;
428 }
429 src += stride - width * 2;
430 }
431 }
432
v4lconvert_uyvy_to_yuv420(const unsigned char * src,unsigned char * dest,int width,int height,int stride,int yvu)433 void v4lconvert_uyvy_to_yuv420(const unsigned char *src, unsigned char *dest,
434 int width, int height, int stride, int yvu)
435 {
436 int i, j;
437 const unsigned char *src1;
438 unsigned char *udest, *vdest;
439
440 /* copy the Y values */
441 src1 = src;
442 for (i = 0; i < height; i++) {
443 for (j = 0; j + 1 < width; j += 2) {
444 *dest++ = src1[1];
445 *dest++ = src1[3];
446 src1 += 4;
447 }
448 src1 += stride - width * 2;
449 }
450
451 /* copy the U and V values */
452 src1 = src + stride; /* next line */
453 if (yvu) {
454 vdest = dest;
455 udest = dest + width * height / 4;
456 } else {
457 udest = dest;
458 vdest = dest + width * height / 4;
459 }
460 for (i = 0; i < height; i += 2) {
461 for (j = 0; j + 1 < width; j += 2) {
462 *udest++ = ((int) src[0] + src1[0]) / 2; /* U */
463 *vdest++ = ((int) src[2] + src1[2]) / 2; /* V */
464 src += 4;
465 src1 += 4;
466 }
467 src1 += stride - width * 2;
468 src = src1;
469 src1 += stride;
470 }
471 }
472
v4lconvert_swap_rgb(const unsigned char * src,unsigned char * dst,int width,int height)473 void v4lconvert_swap_rgb(const unsigned char *src, unsigned char *dst,
474 int width, int height)
475 {
476 int i;
477
478 for (i = 0; i < (width * height); i++) {
479 unsigned char tmp0, tmp1;
480 tmp0 = *src++;
481 tmp1 = *src++;
482 *dst++ = *src++;
483 *dst++ = tmp1;
484 *dst++ = tmp0;
485 }
486 }
487
v4lconvert_swap_uv(const unsigned char * src,unsigned char * dest,const struct v4l2_format * src_fmt)488 void v4lconvert_swap_uv(const unsigned char *src, unsigned char *dest,
489 const struct v4l2_format *src_fmt)
490 {
491 int y;
492
493 /* Copy Y */
494 for (y = 0; y < src_fmt->fmt.pix.height; y++) {
495 memcpy(dest, src, src_fmt->fmt.pix.width);
496 dest += src_fmt->fmt.pix.width;
497 src += src_fmt->fmt.pix.bytesperline;
498 }
499
500 /* Copy component 2 */
501 src += src_fmt->fmt.pix.height * src_fmt->fmt.pix.bytesperline / 4;
502 for (y = 0; y < src_fmt->fmt.pix.height / 2; y++) {
503 memcpy(dest, src, src_fmt->fmt.pix.width / 2);
504 dest += src_fmt->fmt.pix.width / 2;
505 src += src_fmt->fmt.pix.bytesperline / 2;
506 }
507
508 /* Copy component 1 */
509 src -= src_fmt->fmt.pix.height * src_fmt->fmt.pix.bytesperline / 2;
510 for (y = 0; y < src_fmt->fmt.pix.height / 2; y++) {
511 memcpy(dest, src, src_fmt->fmt.pix.width / 2);
512 dest += src_fmt->fmt.pix.width / 2;
513 src += src_fmt->fmt.pix.bytesperline / 2;
514 }
515 }
516
v4lconvert_rgb565_to_rgb24(const unsigned char * src,unsigned char * dest,int width,int height,int stride)517 void v4lconvert_rgb565_to_rgb24(const unsigned char *src, unsigned char *dest,
518 int width, int height, int stride)
519 {
520 int j;
521 while (--height >= 0) {
522 for (j = 0; j < width; j++) {
523 unsigned short tmp = *(unsigned short *)src;
524
525 /* Original format: rrrrrggg gggbbbbb */
526 *dest++ = 0xf8 & (tmp >> 8);
527 *dest++ = 0xfc & (tmp >> 3);
528 *dest++ = 0xf8 & (tmp << 3);
529
530 src += 2;
531 }
532 src += stride - 2 * width;
533 }
534 }
535
v4lconvert_rgb565_to_bgr24(const unsigned char * src,unsigned char * dest,int width,int height,int stride)536 void v4lconvert_rgb565_to_bgr24(const unsigned char *src, unsigned char *dest,
537 int width, int height, int stride)
538 {
539 int j;
540 while (--height >= 0) {
541 for (j = 0; j < width; j++) {
542 unsigned short tmp = *(unsigned short *)src;
543
544 /* Original format: rrrrrggg gggbbbbb */
545 *dest++ = 0xf8 & (tmp << 3);
546 *dest++ = 0xfc & (tmp >> 3);
547 *dest++ = 0xf8 & (tmp >> 8);
548
549 src += 2;
550 }
551 src += stride - 2 * width;
552 }
553 }
554
v4lconvert_rgb565_to_yuv420(const unsigned char * src,unsigned char * dest,const struct v4l2_format * src_fmt,int yvu)555 void v4lconvert_rgb565_to_yuv420(const unsigned char *src, unsigned char *dest,
556 const struct v4l2_format *src_fmt, int yvu)
557 {
558 int x, y;
559 unsigned short tmp;
560 unsigned char *udest, *vdest;
561 unsigned r[4], g[4], b[4];
562 int avg_src[3];
563
564 /* Y */
565 for (y = 0; y < src_fmt->fmt.pix.height; y++) {
566 for (x = 0; x < src_fmt->fmt.pix.width; x++) {
567 tmp = *(unsigned short *)src;
568 r[0] = 0xf8 & (tmp << 3);
569 g[0] = 0xfc & (tmp >> 3);
570 b[0] = 0xf8 & (tmp >> 8);
571 RGB2Y(r[0], g[0], b[0], *dest++);
572 src += 2;
573 }
574 src += src_fmt->fmt.pix.bytesperline - 2 * src_fmt->fmt.pix.width;
575 }
576 src -= src_fmt->fmt.pix.height * src_fmt->fmt.pix.bytesperline;
577
578 /* U + V */
579 if (yvu) {
580 vdest = dest;
581 udest = dest + src_fmt->fmt.pix.width * src_fmt->fmt.pix.height / 4;
582 } else {
583 udest = dest;
584 vdest = dest + src_fmt->fmt.pix.width * src_fmt->fmt.pix.height / 4;
585 }
586
587 for (y = 0; y < src_fmt->fmt.pix.height / 2; y++) {
588 for (x = 0; x < src_fmt->fmt.pix.width / 2; x++) {
589 tmp = *(unsigned short *)src;
590 r[0] = 0xf8 & (tmp << 3);
591 g[0] = 0xfc & (tmp >> 3);
592 b[0] = 0xf8 & (tmp >> 8);
593
594 tmp = *(((unsigned short *)src) + 1);
595 r[1] = 0xf8 & (tmp << 3);
596 g[1] = 0xfc & (tmp >> 3);
597 b[1] = 0xf8 & (tmp >> 8);
598
599 tmp = *(((unsigned short *)src) + src_fmt->fmt.pix.bytesperline);
600 r[2] = 0xf8 & (tmp << 3);
601 g[2] = 0xfc & (tmp >> 3);
602 b[2] = 0xf8 & (tmp >> 8);
603
604 tmp = *(((unsigned short *)src) + src_fmt->fmt.pix.bytesperline + 1);
605 r[3] = 0xf8 & (tmp << 3);
606 g[3] = 0xfc & (tmp >> 3);
607 b[3] = 0xf8 & (tmp >> 8);
608
609 avg_src[0] = (r[0] + r[1] + r[2] + r[3]) / 4;
610 avg_src[1] = (g[0] + g[1] + g[2] + g[3]) / 4;
611 avg_src[2] = (b[0] + b[1] + b[2] + b[3]) / 4;
612 RGB2UV(avg_src[0], avg_src[1], avg_src[2], *udest++, *vdest++);
613 src += 4;
614 }
615 src += 2 * src_fmt->fmt.pix.bytesperline - 2 * src_fmt->fmt.pix.width;
616 }
617 }
618
v4lconvert_y16_to_rgb24(const unsigned char * src,unsigned char * dest,int width,int height,int little_endian)619 void v4lconvert_y16_to_rgb24(const unsigned char *src, unsigned char *dest,
620 int width, int height, int little_endian)
621 {
622 int j;
623
624 if (little_endian)
625 src++;
626
627 while (--height >= 0) {
628 for (j = 0; j < width; j++) {
629 *dest++ = *src;
630 *dest++ = *src;
631 *dest++ = *src;
632 src+=2;
633 }
634 }
635 }
636
v4lconvert_y16_to_yuv420(const unsigned char * src,unsigned char * dest,const struct v4l2_format * src_fmt,int little_endian)637 void v4lconvert_y16_to_yuv420(const unsigned char *src, unsigned char *dest,
638 const struct v4l2_format *src_fmt, int little_endian)
639 {
640 int x, y;
641
642 if (little_endian)
643 src++;
644
645 /* Y */
646 for (y = 0; y < src_fmt->fmt.pix.height; y++)
647 for (x = 0; x < src_fmt->fmt.pix.width; x++){
648 *dest++ = *src;
649 src+=2;
650 }
651
652 /* Clear U/V */
653 memset(dest, 0x80, src_fmt->fmt.pix.width * src_fmt->fmt.pix.height / 2);
654 }
655
v4lconvert_grey_to_rgb24(const unsigned char * src,unsigned char * dest,int width,int height,int stride)656 void v4lconvert_grey_to_rgb24(const unsigned char *src, unsigned char *dest,
657 int width, int height, int stride)
658 {
659 int j;
660 while (--height >= 0) {
661 for (j = 0; j < width; j++) {
662 *dest++ = *src;
663 *dest++ = *src;
664 *dest++ = *src;
665 src++;
666 }
667 src += stride - width;
668 }
669 }
670
v4lconvert_grey_to_yuv420(const unsigned char * src,unsigned char * dest,const struct v4l2_format * src_fmt)671 void v4lconvert_grey_to_yuv420(const unsigned char *src, unsigned char *dest,
672 const struct v4l2_format *src_fmt)
673 {
674 int x, y;
675
676 /* Y */
677 for (y = 0; y < src_fmt->fmt.pix.height; y++)
678 for (x = 0; x < src_fmt->fmt.pix.width; x++)
679 *dest++ = *src++;
680
681 /* Clear U/V */
682 memset(dest, 0x80, src_fmt->fmt.pix.width * src_fmt->fmt.pix.height / 2);
683 }
684
685 /* Unpack buffer of (vw bit) data into padded 16bit buffer. */
convert_packed_to_16bit(const uint8_t * raw,uint16_t * unpacked,int vw,int unpacked_len)686 static inline void convert_packed_to_16bit(const uint8_t *raw, uint16_t *unpacked,
687 int vw, int unpacked_len)
688 {
689 int mask = (1 << vw) - 1;
690 uint32_t buffer = 0;
691 int bitsIn = 0;
692 while (unpacked_len--) {
693 while (bitsIn < vw) {
694 buffer = (buffer << 8) | *(raw++);
695 bitsIn += 8;
696 }
697 bitsIn -= vw;
698 *(unpacked++) = (buffer >> bitsIn) & mask;
699 }
700 }
701
v4lconvert_y10b_to_rgb24(struct v4lconvert_data * data,const unsigned char * src,unsigned char * dest,int width,int height)702 int v4lconvert_y10b_to_rgb24(struct v4lconvert_data *data,
703 const unsigned char *src, unsigned char *dest, int width, int height)
704 {
705 unsigned char *unpacked_buffer;
706
707 unpacked_buffer = v4lconvert_alloc_buffer(width * height * 2,
708 &data->convert_pixfmt_buf,
709 &data->convert_pixfmt_buf_size);
710 if (!unpacked_buffer)
711 return v4lconvert_oom_error(data);
712
713 convert_packed_to_16bit((uint8_t *)src, (uint16_t *)unpacked_buffer,
714 10, width * height);
715
716 int j;
717 unsigned short *tmp = (unsigned short *)unpacked_buffer;
718 while (--height >= 0) {
719 for (j = 0; j < width; j++) {
720
721 /* Only 10 useful bits, so we discard the LSBs */
722 *dest++ = (*tmp & 0x3ff) >> 2;
723 *dest++ = (*tmp & 0x3ff) >> 2;
724 *dest++ = (*tmp & 0x3ff) >> 2;
725
726 /* +1 means two bytes as we are dealing with (unsigned short) */
727 tmp += 1;
728 }
729 }
730 return 0;
731 }
732
v4lconvert_y10b_to_yuv420(struct v4lconvert_data * data,const unsigned char * src,unsigned char * dest,int width,int height)733 int v4lconvert_y10b_to_yuv420(struct v4lconvert_data *data,
734 const unsigned char *src, unsigned char *dest, int width, int height)
735 {
736 unsigned char *unpacked_buffer;
737
738 unpacked_buffer = v4lconvert_alloc_buffer(width * height * 2,
739 &data->convert_pixfmt_buf,
740 &data->convert_pixfmt_buf_size);
741 if (!unpacked_buffer)
742 return v4lconvert_oom_error(data);
743
744 convert_packed_to_16bit((uint8_t *)src, (uint16_t *)unpacked_buffer,
745 10, width * height);
746
747 int x, y;
748 unsigned short *tmp = (unsigned short *)unpacked_buffer;
749
750 /* Y */
751 for (y = 0; y < height; y++)
752 for (x = 0; x < width; x++) {
753
754 /* Only 10 useful bits, so we discard the LSBs */
755 *dest++ = (*tmp & 0x3ff) >> 2;
756
757 /* +1 means two bytes as we are dealing with (unsigned short) */
758 tmp += 1;
759 }
760
761 /* Clear U/V */
762 memset(dest, 0x80, width * height / 2);
763
764 return 0;
765 }
766
v4lconvert_rgb32_to_rgb24(const unsigned char * src,unsigned char * dest,int width,int height,int bgr)767 void v4lconvert_rgb32_to_rgb24(const unsigned char *src, unsigned char *dest,
768 int width, int height,int bgr)
769 {
770 int j;
771 while (--height >= 0) {
772 for (j = 0; j < width; j++) {
773 if (bgr){
774 *dest++ = src[2];
775 *dest++ = src[1];
776 *dest++ = src[0];
777 src+=4;
778 }
779 else{
780 *dest++ = *src++;
781 *dest++ = *src++;
782 *dest++ = *src++;
783 src+=1;
784 }
785 }
786 }
787 }
788
hsvtorgb(const unsigned char * hsv,unsigned char * rgb,unsigned char hsv_enc)789 static void hsvtorgb(const unsigned char *hsv, unsigned char *rgb,
790 unsigned char hsv_enc)
791 {
792 /* From http://stackoverflow.com/questions/3018313/ */
793 uint8_t region;
794 uint8_t remain;
795 uint8_t p, q, t;
796
797 if (!hsv[1]) {
798 rgb[0] = rgb[1] = rgb[2] = hsv[2];
799 return;
800 }
801
802 if (hsv_enc == V4L2_HSV_ENC_256) {
803 region = hsv[0] / 43;
804 remain = (hsv[0] - (region * 43)) * 6;
805
806 } else {
807 int aux;
808
809 region = hsv[0] / (180/6);
810
811 /* Remain must be scaled to 0..255 */
812 aux = (hsv[0] % (180/6)) * 6 * 256;
813 aux /= 180;
814 remain = aux;
815 }
816
817 p = (hsv[2] * (255 - hsv[1])) >> 8;
818 q = (hsv[2] * (255 - ((hsv[1] * remain) >> 8))) >> 8;
819 t = (hsv[2] * (255 - ((hsv[1] * (255 - remain)) >> 8))) >> 8;
820
821 switch (region) {
822 case 0:
823 rgb[0] = hsv[2]; rgb[1] = t; rgb[2] = p;
824 break;
825 case 1:
826 rgb[0] = q; rgb[1] = hsv[2]; rgb[2] = p;
827 break;
828 case 2:
829 rgb[0] = p; rgb[1] = hsv[2]; rgb[2] = t;
830 break;
831 case 3:
832 rgb[0] = p; rgb[1] = q; rgb[2] = hsv[2];
833 break;
834 case 4:
835 rgb[0] = t; rgb[1] = p; rgb[2] = hsv[2];
836 break;
837 default:
838 rgb[0] = hsv[2]; rgb[1] = p; rgb[2] = q;
839 break;
840 }
841
842 }
843
v4lconvert_hsv_to_rgb24(const unsigned char * src,unsigned char * dest,int width,int height,int bgr,int Xin,unsigned char hsv_enc)844 void v4lconvert_hsv_to_rgb24(const unsigned char *src, unsigned char *dest,
845 int width, int height, int bgr, int Xin, unsigned char hsv_enc){
846 int j, k;
847 int bppIN = Xin / 8;
848 unsigned char rgb[3];
849
850 src += bppIN - 3;
851
852 while (--height >= 0)
853 for (j = 0; j < width; j++) {
854 hsvtorgb(src, rgb, hsv_enc);
855 for (k = 0; k < 3; k++)
856 if (bgr && k < 3)
857 *dest++ = rgb[2-k];
858 else
859 *dest++ = rgb[k];
860 src += bppIN;
861 }
862 }
863
v4lconvert_nv12_to_rgb24(const unsigned char * src,unsigned char * dest,int width,int height,int stride,int bgr)864 void v4lconvert_nv12_to_rgb24(const unsigned char *src, unsigned char *dest,
865 int width, int height, int stride, int bgr)
866 {
867 int i, j;
868 const unsigned char *ysrc = src;
869 const unsigned char *uvsrc = src + stride * height;
870
871 for (i = 0; i < height; i++) {
872 for (j = 0; j < width; j ++) {
873 if (bgr) {
874 *dest++ = YUV2B(*ysrc, *uvsrc, *(uvsrc + 1));
875 *dest++ = YUV2G(*ysrc, *uvsrc, *(uvsrc + 1));
876 *dest++ = YUV2R(*ysrc, *uvsrc, *(uvsrc + 1));
877 } else {
878 *dest++ = YUV2R(*ysrc, *uvsrc, *(uvsrc + 1));
879 *dest++ = YUV2G(*ysrc, *uvsrc, *(uvsrc + 1));
880 *dest++ = YUV2B(*ysrc, *uvsrc, *(uvsrc + 1));
881 }
882 ysrc++;
883 if (j&1)
884 uvsrc += 2;
885 }
886
887 ysrc += stride - width;
888 /* Rewind u and v for next line */
889 if (!(i&1))
890 uvsrc -= width;
891 else
892 uvsrc += stride - width;
893 }
894 }
895
v4lconvert_nv12_to_yuv420(const unsigned char * src,unsigned char * dest,int width,int height,int stride,int yvu)896 void v4lconvert_nv12_to_yuv420(const unsigned char *src, unsigned char *dest,
897 int width, int height, int stride, int yvu)
898 {
899 int i, j;
900 const unsigned char *ysrc = src;
901 const unsigned char *uvsrc = src + stride * height;
902 unsigned char *ydst = dest;
903 unsigned char *udst, *vdst;
904
905 if (yvu) {
906 vdst = ydst + width * height;
907 udst = vdst + ((width / 2) * (height / 2));
908 } else {
909 udst = ydst + width * height;
910 vdst = udst + ((width / 2) * (height / 2));
911 }
912
913 for (i = 0; i < height; i++) {
914 for (j = 0; j < width; j++) {
915 *ydst++ = *ysrc++;
916 if (((i % 2) == 0) && ((j % 2) == 0)) {
917 *udst++ = *uvsrc++;
918 *vdst++ = *uvsrc++;
919 }
920 }
921
922 ysrc += stride - width;
923 if ((i % 2) == 0)
924 uvsrc += stride - width;
925 }
926 }
927