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 yvu)95 void v4lconvert_yuv420_to_bgr24(const unsigned char *src, unsigned char *dest,
96 int width, int height, 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 + width * height;
105 usrc = vsrc + (width * height) / 4;
106 } else {
107 usrc = src + width * height;
108 vsrc = usrc + (width * 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 /* Rewind u and v for next line */
142 if (!(i & 1)) {
143 usrc -= width / 2;
144 vsrc -= width / 2;
145 }
146 }
147 }
148
v4lconvert_yuv420_to_rgb24(const unsigned char * src,unsigned char * dest,int width,int height,int yvu)149 void v4lconvert_yuv420_to_rgb24(const unsigned char *src, unsigned char *dest,
150 int width, int height, int yvu)
151 {
152 int i, j;
153
154 const unsigned char *ysrc = src;
155 const unsigned char *usrc, *vsrc;
156
157 if (yvu) {
158 vsrc = src + width * height;
159 usrc = vsrc + (width * height) / 4;
160 } else {
161 usrc = src + width * height;
162 vsrc = usrc + (width * height) / 4;
163 }
164
165 for (i = 0; i < height; i++) {
166 for (j = 0; j < width; j += 2) {
167 #if 1 /* fast slightly less accurate multiplication free code */
168 int u1 = (((*usrc - 128) << 7) + (*usrc - 128)) >> 6;
169 int rg = (((*usrc - 128) << 1) + (*usrc - 128) +
170 ((*vsrc - 128) << 2) + ((*vsrc - 128) << 1)) >> 3;
171 int v1 = (((*vsrc - 128) << 1) + (*vsrc - 128)) >> 1;
172
173 *dest++ = CLIP(*ysrc + v1);
174 *dest++ = CLIP(*ysrc - rg);
175 *dest++ = CLIP(*ysrc + u1);
176 ysrc++;
177
178 *dest++ = CLIP(*ysrc + v1);
179 *dest++ = CLIP(*ysrc - rg);
180 *dest++ = CLIP(*ysrc + u1);
181 #else
182 *dest++ = YUV2R(*ysrc, *usrc, *vsrc);
183 *dest++ = YUV2G(*ysrc, *usrc, *vsrc);
184 *dest++ = YUV2B(*ysrc, *usrc, *vsrc);
185 ysrc++;
186
187 *dest++ = YUV2R(*ysrc, *usrc, *vsrc);
188 *dest++ = YUV2G(*ysrc, *usrc, *vsrc);
189 *dest++ = YUV2B(*ysrc, *usrc, *vsrc);
190 #endif
191 ysrc++;
192 usrc++;
193 vsrc++;
194 }
195 /* Rewind u and v for next line */
196 if (!(i&1)) {
197 usrc -= width / 2;
198 vsrc -= width / 2;
199 }
200 }
201 }
202
v4lconvert_yuyv_to_bgr24(const unsigned char * src,unsigned char * dest,int width,int height,int stride)203 void v4lconvert_yuyv_to_bgr24(const unsigned char *src, unsigned char *dest,
204 int width, int height, int stride)
205 {
206 int j;
207
208 while (--height >= 0) {
209 for (j = 0; j + 1 < width; j += 2) {
210 int u = src[1];
211 int v = src[3];
212 int u1 = (((u - 128) << 7) + (u - 128)) >> 6;
213 int rg = (((u - 128) << 1) + (u - 128) +
214 ((v - 128) << 2) + ((v - 128) << 1)) >> 3;
215 int v1 = (((v - 128) << 1) + (v - 128)) >> 1;
216
217 *dest++ = CLIP(src[0] + u1);
218 *dest++ = CLIP(src[0] - rg);
219 *dest++ = CLIP(src[0] + v1);
220
221 *dest++ = CLIP(src[2] + u1);
222 *dest++ = CLIP(src[2] - rg);
223 *dest++ = CLIP(src[2] + v1);
224 src += 4;
225 }
226 src += stride - width * 2;
227 }
228 }
229
v4lconvert_yuyv_to_rgb24(const unsigned char * src,unsigned char * dest,int width,int height,int stride)230 void v4lconvert_yuyv_to_rgb24(const unsigned char *src, unsigned char *dest,
231 int width, int height, int stride)
232 {
233 int j;
234
235 while (--height >= 0) {
236 for (j = 0; j + 1 < width; j += 2) {
237 int u = src[1];
238 int v = src[3];
239 int u1 = (((u - 128) << 7) + (u - 128)) >> 6;
240 int rg = (((u - 128) << 1) + (u - 128) +
241 ((v - 128) << 2) + ((v - 128) << 1)) >> 3;
242 int v1 = (((v - 128) << 1) + (v - 128)) >> 1;
243
244 *dest++ = CLIP(src[0] + v1);
245 *dest++ = CLIP(src[0] - rg);
246 *dest++ = CLIP(src[0] + u1);
247
248 *dest++ = CLIP(src[2] + v1);
249 *dest++ = CLIP(src[2] - rg);
250 *dest++ = CLIP(src[2] + u1);
251 src += 4;
252 }
253 src += stride - (width * 2);
254 }
255 }
256
v4lconvert_yuyv_to_yuv420(const unsigned char * src,unsigned char * dest,int width,int height,int stride,int yvu)257 void v4lconvert_yuyv_to_yuv420(const unsigned char *src, unsigned char *dest,
258 int width, int height, int stride, int yvu)
259 {
260 int i, j;
261 const unsigned char *src1;
262 unsigned char *udest, *vdest;
263
264 /* copy the Y values */
265 src1 = src;
266 for (i = 0; i < height; i++) {
267 for (j = 0; j + 1 < width; j += 2) {
268 *dest++ = src1[0];
269 *dest++ = src1[2];
270 src1 += 4;
271 }
272 src1 += stride - width * 2;
273 }
274
275 /* copy the U and V values */
276 src++; /* point to V */
277 src1 = src + stride; /* next line */
278 if (yvu) {
279 vdest = dest;
280 udest = dest + width * height / 4;
281 } else {
282 udest = dest;
283 vdest = dest + width * height / 4;
284 }
285 for (i = 0; i < height; i += 2) {
286 for (j = 0; j + 1 < width; j += 2) {
287 *udest++ = ((int) src[0] + src1[0]) / 2; /* U */
288 *vdest++ = ((int) src[2] + src1[2]) / 2; /* V */
289 src += 4;
290 src1 += 4;
291 }
292 src1 += stride - width * 2;
293 src = src1;
294 src1 += stride;
295 }
296 }
297
v4lconvert_nv16_to_yuyv(const unsigned char * src,unsigned char * dest,int width,int height)298 void v4lconvert_nv16_to_yuyv(const unsigned char *src, unsigned char *dest,
299 int width, int height)
300 {
301 const unsigned char *y, *cbcr;
302 int count = 0;
303
304 y = src;
305 cbcr = src + width*height;
306
307 while (count++ < width*height) {
308 *dest++ = *y++;
309 *dest++ = *cbcr++;
310 }
311 }
312
v4lconvert_yvyu_to_bgr24(const unsigned char * src,unsigned char * dest,int width,int height,int stride)313 void v4lconvert_yvyu_to_bgr24(const unsigned char *src, unsigned char *dest,
314 int width, int height, int stride)
315 {
316 int j;
317
318 while (--height >= 0) {
319 for (j = 0; j + 1 < width; j += 2) {
320 int u = src[3];
321 int v = src[1];
322 int u1 = (((u - 128) << 7) + (u - 128)) >> 6;
323 int rg = (((u - 128) << 1) + (u - 128) +
324 ((v - 128) << 2) + ((v - 128) << 1)) >> 3;
325 int v1 = (((v - 128) << 1) + (v - 128)) >> 1;
326
327 *dest++ = CLIP(src[0] + u1);
328 *dest++ = CLIP(src[0] - rg);
329 *dest++ = CLIP(src[0] + v1);
330
331 *dest++ = CLIP(src[2] + u1);
332 *dest++ = CLIP(src[2] - rg);
333 *dest++ = CLIP(src[2] + v1);
334 src += 4;
335 }
336 src += stride - (width * 2);
337 }
338 }
339
v4lconvert_yvyu_to_rgb24(const unsigned char * src,unsigned char * dest,int width,int height,int stride)340 void v4lconvert_yvyu_to_rgb24(const unsigned char *src, unsigned char *dest,
341 int width, int height, int stride)
342 {
343 int j;
344
345 while (--height >= 0) {
346 for (j = 0; j + 1 < width; j += 2) {
347 int u = src[3];
348 int v = src[1];
349 int u1 = (((u - 128) << 7) + (u - 128)) >> 6;
350 int rg = (((u - 128) << 1) + (u - 128) +
351 ((v - 128) << 2) + ((v - 128) << 1)) >> 3;
352 int v1 = (((v - 128) << 1) + (v - 128)) >> 1;
353
354 *dest++ = CLIP(src[0] + v1);
355 *dest++ = CLIP(src[0] - rg);
356 *dest++ = CLIP(src[0] + u1);
357
358 *dest++ = CLIP(src[2] + v1);
359 *dest++ = CLIP(src[2] - rg);
360 *dest++ = CLIP(src[2] + u1);
361 src += 4;
362 }
363 src += stride - (width * 2);
364 }
365 }
366
v4lconvert_uyvy_to_bgr24(const unsigned char * src,unsigned char * dest,int width,int height,int stride)367 void v4lconvert_uyvy_to_bgr24(const unsigned char *src, unsigned char *dest,
368 int width, int height, int stride)
369 {
370 int j;
371
372 while (--height >= 0) {
373 for (j = 0; j + 1 < width; j += 2) {
374 int u = src[0];
375 int v = src[2];
376 int u1 = (((u - 128) << 7) + (u - 128)) >> 6;
377 int rg = (((u - 128) << 1) + (u - 128) +
378 ((v - 128) << 2) + ((v - 128) << 1)) >> 3;
379 int v1 = (((v - 128) << 1) + (v - 128)) >> 1;
380
381 *dest++ = CLIP(src[1] + u1);
382 *dest++ = CLIP(src[1] - rg);
383 *dest++ = CLIP(src[1] + v1);
384
385 *dest++ = CLIP(src[3] + u1);
386 *dest++ = CLIP(src[3] - rg);
387 *dest++ = CLIP(src[3] + v1);
388 src += 4;
389 }
390 src += stride - width * 2;
391 }
392 }
393
v4lconvert_uyvy_to_rgb24(const unsigned char * src,unsigned char * dest,int width,int height,int stride)394 void v4lconvert_uyvy_to_rgb24(const unsigned char *src, unsigned char *dest,
395 int width, int height, int stride)
396 {
397 int j;
398
399 while (--height >= 0) {
400 for (j = 0; j + 1 < width; j += 2) {
401 int u = src[0];
402 int v = src[2];
403 int u1 = (((u - 128) << 7) + (u - 128)) >> 6;
404 int rg = (((u - 128) << 1) + (u - 128) +
405 ((v - 128) << 2) + ((v - 128) << 1)) >> 3;
406 int v1 = (((v - 128) << 1) + (v - 128)) >> 1;
407
408 *dest++ = CLIP(src[1] + v1);
409 *dest++ = CLIP(src[1] - rg);
410 *dest++ = CLIP(src[1] + u1);
411
412 *dest++ = CLIP(src[3] + v1);
413 *dest++ = CLIP(src[3] - rg);
414 *dest++ = CLIP(src[3] + u1);
415 src += 4;
416 }
417 src += stride - width * 2;
418 }
419 }
420
v4lconvert_uyvy_to_yuv420(const unsigned char * src,unsigned char * dest,int width,int height,int stride,int yvu)421 void v4lconvert_uyvy_to_yuv420(const unsigned char *src, unsigned char *dest,
422 int width, int height, int stride, int yvu)
423 {
424 int i, j;
425 const unsigned char *src1;
426 unsigned char *udest, *vdest;
427
428 /* copy the Y values */
429 src1 = src;
430 for (i = 0; i < height; i++) {
431 for (j = 0; j + 1 < width; j += 2) {
432 *dest++ = src1[1];
433 *dest++ = src1[3];
434 src1 += 4;
435 }
436 src1 += stride - width * 2;
437 }
438
439 /* copy the U and V values */
440 src1 = src + stride; /* next line */
441 if (yvu) {
442 vdest = dest;
443 udest = dest + width * height / 4;
444 } else {
445 udest = dest;
446 vdest = dest + width * height / 4;
447 }
448 for (i = 0; i < height; i += 2) {
449 for (j = 0; j + 1 < width; j += 2) {
450 *udest++ = ((int) src[0] + src1[0]) / 2; /* U */
451 *vdest++ = ((int) src[2] + src1[2]) / 2; /* V */
452 src += 4;
453 src1 += 4;
454 }
455 src1 += stride - width * 2;
456 src = src1;
457 src1 += stride;
458 }
459 }
460
v4lconvert_swap_rgb(const unsigned char * src,unsigned char * dst,int width,int height)461 void v4lconvert_swap_rgb(const unsigned char *src, unsigned char *dst,
462 int width, int height)
463 {
464 int i;
465
466 for (i = 0; i < (width * height); i++) {
467 unsigned char tmp0, tmp1;
468 tmp0 = *src++;
469 tmp1 = *src++;
470 *dst++ = *src++;
471 *dst++ = tmp1;
472 *dst++ = tmp0;
473 }
474 }
475
v4lconvert_swap_uv(const unsigned char * src,unsigned char * dest,const struct v4l2_format * src_fmt)476 void v4lconvert_swap_uv(const unsigned char *src, unsigned char *dest,
477 const struct v4l2_format *src_fmt)
478 {
479 int y;
480
481 /* Copy Y */
482 for (y = 0; y < src_fmt->fmt.pix.height; y++) {
483 memcpy(dest, src, src_fmt->fmt.pix.width);
484 dest += src_fmt->fmt.pix.width;
485 src += src_fmt->fmt.pix.bytesperline;
486 }
487
488 /* Copy component 2 */
489 src += src_fmt->fmt.pix.height * src_fmt->fmt.pix.bytesperline / 4;
490 for (y = 0; y < src_fmt->fmt.pix.height / 2; y++) {
491 memcpy(dest, src, src_fmt->fmt.pix.width / 2);
492 dest += src_fmt->fmt.pix.width / 2;
493 src += src_fmt->fmt.pix.bytesperline / 2;
494 }
495
496 /* Copy component 1 */
497 src -= src_fmt->fmt.pix.height * src_fmt->fmt.pix.bytesperline / 2;
498 for (y = 0; y < src_fmt->fmt.pix.height / 2; y++) {
499 memcpy(dest, src, src_fmt->fmt.pix.width / 2);
500 dest += src_fmt->fmt.pix.width / 2;
501 src += src_fmt->fmt.pix.bytesperline / 2;
502 }
503 }
504
v4lconvert_rgb565_to_rgb24(const unsigned char * src,unsigned char * dest,int width,int height)505 void v4lconvert_rgb565_to_rgb24(const unsigned char *src, unsigned char *dest,
506 int width, int height)
507 {
508 int j;
509 while (--height >= 0) {
510 for (j = 0; j < width; j++) {
511 unsigned short tmp = *(unsigned short *)src;
512
513 /* Original format: rrrrrggg gggbbbbb */
514 *dest++ = 0xf8 & (tmp >> 8);
515 *dest++ = 0xfc & (tmp >> 3);
516 *dest++ = 0xf8 & (tmp << 3);
517
518 src += 2;
519 }
520 }
521 }
522
v4lconvert_rgb565_to_bgr24(const unsigned char * src,unsigned char * dest,int width,int height)523 void v4lconvert_rgb565_to_bgr24(const unsigned char *src, unsigned char *dest,
524 int width, int height)
525 {
526 int j;
527 while (--height >= 0) {
528 for (j = 0; j < width; j++) {
529 unsigned short tmp = *(unsigned short *)src;
530
531 /* Original format: rrrrrggg gggbbbbb */
532 *dest++ = 0xf8 & (tmp << 3);
533 *dest++ = 0xfc & (tmp >> 3);
534 *dest++ = 0xf8 & (tmp >> 8);
535
536 src += 2;
537 }
538 }
539 }
540
v4lconvert_rgb565_to_yuv420(const unsigned char * src,unsigned char * dest,const struct v4l2_format * src_fmt,int yvu)541 void v4lconvert_rgb565_to_yuv420(const unsigned char *src, unsigned char *dest,
542 const struct v4l2_format *src_fmt, int yvu)
543 {
544 int x, y;
545 unsigned short tmp;
546 unsigned char *udest, *vdest;
547 unsigned r[4], g[4], b[4];
548 int avg_src[3];
549
550 /* Y */
551 for (y = 0; y < src_fmt->fmt.pix.height; y++) {
552 for (x = 0; x < src_fmt->fmt.pix.width; x++) {
553 tmp = *(unsigned short *)src;
554 r[0] = 0xf8 & (tmp << 3);
555 g[0] = 0xfc & (tmp >> 3);
556 b[0] = 0xf8 & (tmp >> 8);
557 RGB2Y(r[0], g[0], b[0], *dest++);
558 src += 2;
559 }
560 src += src_fmt->fmt.pix.bytesperline - 2 * src_fmt->fmt.pix.width;
561 }
562 src -= src_fmt->fmt.pix.height * src_fmt->fmt.pix.bytesperline;
563
564 /* U + V */
565 if (yvu) {
566 vdest = dest;
567 udest = dest + src_fmt->fmt.pix.width * src_fmt->fmt.pix.height / 4;
568 } else {
569 udest = dest;
570 vdest = dest + src_fmt->fmt.pix.width * src_fmt->fmt.pix.height / 4;
571 }
572
573 for (y = 0; y < src_fmt->fmt.pix.height / 2; y++) {
574 for (x = 0; x < src_fmt->fmt.pix.width / 2; x++) {
575 tmp = *(unsigned short *)src;
576 r[0] = 0xf8 & (tmp << 3);
577 g[0] = 0xfc & (tmp >> 3);
578 b[0] = 0xf8 & (tmp >> 8);
579
580 tmp = *(((unsigned short *)src) + 1);
581 r[1] = 0xf8 & (tmp << 3);
582 g[1] = 0xfc & (tmp >> 3);
583 b[1] = 0xf8 & (tmp >> 8);
584
585 tmp = *(((unsigned short *)src) + src_fmt->fmt.pix.bytesperline);
586 r[2] = 0xf8 & (tmp << 3);
587 g[2] = 0xfc & (tmp >> 3);
588 b[2] = 0xf8 & (tmp >> 8);
589
590 tmp = *(((unsigned short *)src) + src_fmt->fmt.pix.bytesperline + 1);
591 r[3] = 0xf8 & (tmp << 3);
592 g[3] = 0xfc & (tmp >> 3);
593 b[3] = 0xf8 & (tmp >> 8);
594
595 avg_src[0] = (r[0] + r[1] + r[2] + r[3]) / 4;
596 avg_src[1] = (g[0] + g[1] + g[2] + g[3]) / 4;
597 avg_src[2] = (b[0] + b[1] + b[2] + b[3]) / 4;
598 RGB2UV(avg_src[0], avg_src[1], avg_src[2], *udest++, *vdest++);
599 src += 4;
600 }
601 src += 2 * src_fmt->fmt.pix.bytesperline - 2 * src_fmt->fmt.pix.width;
602 }
603 }
604
v4lconvert_y16_to_rgb24(const unsigned char * src,unsigned char * dest,int width,int height,int little_endian)605 void v4lconvert_y16_to_rgb24(const unsigned char *src, unsigned char *dest,
606 int width, int height, int little_endian)
607 {
608 int j;
609
610 if (little_endian)
611 src++;
612
613 while (--height >= 0) {
614 for (j = 0; j < width; j++) {
615 *dest++ = *src;
616 *dest++ = *src;
617 *dest++ = *src;
618 src+=2;
619 }
620 }
621 }
622
v4lconvert_y16_to_yuv420(const unsigned char * src,unsigned char * dest,const struct v4l2_format * src_fmt,int little_endian)623 void v4lconvert_y16_to_yuv420(const unsigned char *src, unsigned char *dest,
624 const struct v4l2_format *src_fmt, int little_endian)
625 {
626 int x, y;
627
628 if (little_endian)
629 src++;
630
631 /* Y */
632 for (y = 0; y < src_fmt->fmt.pix.height; y++)
633 for (x = 0; x < src_fmt->fmt.pix.width; x++){
634 *dest++ = *src;
635 src+=2;
636 }
637
638 /* Clear U/V */
639 memset(dest, 0x80, src_fmt->fmt.pix.width * src_fmt->fmt.pix.height / 2);
640 }
641
v4lconvert_grey_to_rgb24(const unsigned char * src,unsigned char * dest,int width,int height)642 void v4lconvert_grey_to_rgb24(const unsigned char *src, unsigned char *dest,
643 int width, int height)
644 {
645 int j;
646 while (--height >= 0) {
647 for (j = 0; j < width; j++) {
648 *dest++ = *src;
649 *dest++ = *src;
650 *dest++ = *src;
651 src++;
652 }
653 }
654 }
655
v4lconvert_grey_to_yuv420(const unsigned char * src,unsigned char * dest,const struct v4l2_format * src_fmt)656 void v4lconvert_grey_to_yuv420(const unsigned char *src, unsigned char *dest,
657 const struct v4l2_format *src_fmt)
658 {
659 int x, y;
660
661 /* Y */
662 for (y = 0; y < src_fmt->fmt.pix.height; y++)
663 for (x = 0; x < src_fmt->fmt.pix.width; x++)
664 *dest++ = *src++;
665
666 /* Clear U/V */
667 memset(dest, 0x80, src_fmt->fmt.pix.width * src_fmt->fmt.pix.height / 2);
668 }
669
670 /* 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)671 static inline void convert_packed_to_16bit(const uint8_t *raw, uint16_t *unpacked,
672 int vw, int unpacked_len)
673 {
674 int mask = (1 << vw) - 1;
675 uint32_t buffer = 0;
676 int bitsIn = 0;
677 while (unpacked_len--) {
678 while (bitsIn < vw) {
679 buffer = (buffer << 8) | *(raw++);
680 bitsIn += 8;
681 }
682 bitsIn -= vw;
683 *(unpacked++) = (buffer >> bitsIn) & mask;
684 }
685 }
686
v4lconvert_y10b_to_rgb24(struct v4lconvert_data * data,const unsigned char * src,unsigned char * dest,int width,int height)687 int v4lconvert_y10b_to_rgb24(struct v4lconvert_data *data,
688 const unsigned char *src, unsigned char *dest, int width, int height)
689 {
690 unsigned char *unpacked_buffer;
691
692 unpacked_buffer = v4lconvert_alloc_buffer(width * height * 2,
693 &data->convert_pixfmt_buf,
694 &data->convert_pixfmt_buf_size);
695 if (!unpacked_buffer)
696 return v4lconvert_oom_error(data);
697
698 convert_packed_to_16bit((uint8_t *)src, (uint16_t *)unpacked_buffer,
699 10, width * height);
700
701 int j;
702 unsigned short *tmp = (unsigned short *)unpacked_buffer;
703 while (--height >= 0) {
704 for (j = 0; j < width; j++) {
705
706 /* Only 10 useful bits, so we discard the LSBs */
707 *dest++ = (*tmp & 0x3ff) >> 2;
708 *dest++ = (*tmp & 0x3ff) >> 2;
709 *dest++ = (*tmp & 0x3ff) >> 2;
710
711 /* +1 means two bytes as we are dealing with (unsigned short) */
712 tmp += 1;
713 }
714 }
715 return 0;
716 }
717
v4lconvert_y10b_to_yuv420(struct v4lconvert_data * data,const unsigned char * src,unsigned char * dest,int width,int height)718 int v4lconvert_y10b_to_yuv420(struct v4lconvert_data *data,
719 const unsigned char *src, unsigned char *dest, int width, int height)
720 {
721 unsigned char *unpacked_buffer;
722
723 unpacked_buffer = v4lconvert_alloc_buffer(width * height * 2,
724 &data->convert_pixfmt_buf,
725 &data->convert_pixfmt_buf_size);
726 if (!unpacked_buffer)
727 return v4lconvert_oom_error(data);
728
729 convert_packed_to_16bit((uint8_t *)src, (uint16_t *)unpacked_buffer,
730 10, width * height);
731
732 int x, y;
733 unsigned short *tmp = (unsigned short *)unpacked_buffer;
734
735 /* Y */
736 for (y = 0; y < height; y++)
737 for (x = 0; x < width; x++) {
738
739 /* Only 10 useful bits, so we discard the LSBs */
740 *dest++ = (*tmp & 0x3ff) >> 2;
741
742 /* +1 means two bytes as we are dealing with (unsigned short) */
743 tmp += 1;
744 }
745
746 /* Clear U/V */
747 memset(dest, 0x80, width * height / 2);
748
749 return 0;
750 }
751
v4lconvert_rgb32_to_rgb24(const unsigned char * src,unsigned char * dest,int width,int height,int bgr)752 void v4lconvert_rgb32_to_rgb24(const unsigned char *src, unsigned char *dest,
753 int width, int height,int bgr)
754 {
755 int j;
756 while (--height >= 0) {
757 for (j = 0; j < width; j++) {
758 if (bgr){
759 *dest++ = src[2];
760 *dest++ = src[1];
761 *dest++ = src[0];
762 src+=4;
763 }
764 else{
765 *dest++ = *src++;
766 *dest++ = *src++;
767 *dest++ = *src++;
768 src+=1;
769 }
770 }
771 }
772 }
773
hsvtorgb(const unsigned char * hsv,unsigned char * rgb,unsigned char hsv_enc)774 static void hsvtorgb(const unsigned char *hsv, unsigned char *rgb,
775 unsigned char hsv_enc)
776 {
777 /* From http://stackoverflow.com/questions/3018313/ */
778 uint8_t region;
779 uint8_t remain;
780 uint8_t p, q, t;
781
782 if (!hsv[1]) {
783 rgb[0] = rgb[1] = rgb[2] = hsv[2];
784 return;
785 }
786
787 if (hsv_enc == V4L2_HSV_ENC_256) {
788 region = hsv[0] / 43;
789 remain = (hsv[0] - (region * 43)) * 6;
790
791 } else {
792 int aux;
793
794 region = hsv[0] / (180/6);
795
796 /* Remain must be scaled to 0..255 */
797 aux = (hsv[0] % (180/6)) * 6 * 256;
798 aux /= 180;
799 remain = aux;
800 }
801
802 p = (hsv[2] * (255 - hsv[1])) >> 8;
803 q = (hsv[2] * (255 - ((hsv[1] * remain) >> 8))) >> 8;
804 t = (hsv[2] * (255 - ((hsv[1] * (255 - remain)) >> 8))) >> 8;
805
806 switch (region) {
807 case 0:
808 rgb[0] = hsv[2]; rgb[1] = t; rgb[2] = p;
809 break;
810 case 1:
811 rgb[0] = q; rgb[1] = hsv[2]; rgb[2] = p;
812 break;
813 case 2:
814 rgb[0] = p; rgb[1] = hsv[2]; rgb[2] = t;
815 break;
816 case 3:
817 rgb[0] = p; rgb[1] = q; rgb[2] = hsv[2];
818 break;
819 case 4:
820 rgb[0] = t; rgb[1] = p; rgb[2] = hsv[2];
821 break;
822 default:
823 rgb[0] = hsv[2]; rgb[1] = p; rgb[2] = q;
824 break;
825 }
826
827 }
828
v4lconvert_hsv_to_rgb24(const unsigned char * src,unsigned char * dest,int width,int height,int bgr,int Xin,unsigned char hsv_enc)829 void v4lconvert_hsv_to_rgb24(const unsigned char *src, unsigned char *dest,
830 int width, int height, int bgr, int Xin, unsigned char hsv_enc){
831 int j, k;
832 int bppIN = Xin / 8;
833 unsigned char rgb[3];
834
835 src += bppIN - 3;
836
837 while (--height >= 0)
838 for (j = 0; j < width; j++) {
839 hsvtorgb(src, rgb, hsv_enc);
840 for (k = 0; k < 3; k++)
841 if (bgr && k < 3)
842 *dest++ = rgb[2-k];
843 else
844 *dest++ = rgb[k];
845 src += bppIN;
846 }
847 }
848
v4lconvert_nv12_to_rgb24(const unsigned char * src,unsigned char * dest,int width,int height,int bgr)849 void v4lconvert_nv12_to_rgb24(const unsigned char *src, unsigned char *dest,
850 int width, int height, int bgr)
851 {
852 int i, j;
853 const unsigned char *ysrc = src;
854 const unsigned char *uvsrc = src + width * height;
855
856 for (i = 0; i < height; i++) {
857 for (j = 0; j < width; j ++) {
858 if (bgr) {
859 *dest++ = YUV2B(*ysrc, *uvsrc, *(uvsrc + 1));
860 *dest++ = YUV2G(*ysrc, *uvsrc, *(uvsrc + 1));
861 *dest++ = YUV2R(*ysrc, *uvsrc, *(uvsrc + 1));
862 } else {
863 *dest++ = YUV2R(*ysrc, *uvsrc, *(uvsrc + 1));
864 *dest++ = YUV2G(*ysrc, *uvsrc, *(uvsrc + 1));
865 *dest++ = YUV2B(*ysrc, *uvsrc, *(uvsrc + 1));
866 }
867 ysrc++;
868 if (j&1)
869 uvsrc += 2;
870 }
871
872 /* Rewind u and v for next line */
873 if (!(i&1))
874 uvsrc -= width;
875 }
876 }
877
v4lconvert_nv12_to_yuv420(const unsigned char * src,unsigned char * dest,int width,int height,int yvu)878 void v4lconvert_nv12_to_yuv420(const unsigned char *src, unsigned char *dest,
879 int width, int height, int yvu)
880 {
881 int i, j;
882 const unsigned char *ysrc = src;
883 const unsigned char *uvsrc = src + width * height;
884 unsigned char *ydst = dest;
885 unsigned char *udst, *vdst;
886
887 if (yvu) {
888 vdst = ydst + width * height;
889 udst = vdst + ((width / 2) * (height / 2));
890 } else {
891 udst = ydst + width * height;
892 vdst = udst + ((width / 2) * (height / 2));
893 }
894
895 for (i = 0; i < height; i++)
896 for (j = 0; j < width; j++) {
897 *ydst++ = *ysrc++;
898 if (((i % 2) == 0) && ((j % 2) == 0)) {
899 *udst++ = *uvsrc++;
900 *vdst++ = *uvsrc++;
901 }
902 }
903 }
904