blob: 3a37f5aa448f7fb363c0b3cb9c5f4e4426b5422c
1 | #include <stdio.h> |
2 | #include <stdlib.h> |
3 | #include <unistd.h> |
4 | #include <sys/ioctl.h> |
5 | #include <sys/types.h> |
6 | #include <sys/stat.h> |
7 | #include <string.h> |
8 | #include "util.h" |
9 | |
10 | #ifdef AMLOGIC_USB_CAMERA_SUPPORT |
11 | #define swap_cbcr |
12 | static void convert_rgb16_to_nv21(uint8_t *rgb, uint8_t *yuv, int width, int height) |
13 | { |
14 | int iy =0, iuv = 0; |
15 | uint8_t* buf_y = yuv; |
16 | uint8_t* buf_uv = buf_y + width * height; |
17 | uint16_t* buf_rgb = (uint16_t *)rgb; |
18 | int h,w,val_rgb,val_r,val_g,val_b; |
19 | int y,u,v; |
20 | for (h = 0; h < height; h++) { |
21 | for (w = 0; w < width; w++) { |
22 | val_rgb = buf_rgb[h * width + w]; |
23 | val_r = ((val_rgb & (0x1f << 11)) >> 11)<<3; |
24 | val_g = ((val_rgb & (0x3f << 5)) >> 5)<<2; |
25 | val_b = ((val_rgb & (0x1f << 0)) >> 0)<<3; |
26 | y = 0.30078 * val_r + 0.5859 * val_g + 0.11328 * val_b; |
27 | if (y > 255) { |
28 | y = 255; |
29 | } else if (y < 0) { |
30 | y = 0; |
31 | } |
32 | buf_y[iy++] = y; |
33 | if (0 == h % 2 && 0 == w % 2) { |
34 | u = -0.11328 * val_r - 0.33984 * val_g + 0.51179 * val_b + 128; |
35 | if (u > 255) { |
36 | u = 255; |
37 | } else if (u < 0) { |
38 | u = 0; |
39 | } |
40 | v = 0.51179 * val_r - 0.429688 * val_g - 0.08203 * val_b + 128; |
41 | if (v > 255) { |
42 | v = 255; |
43 | } else if (v < 0) { |
44 | v = 0; |
45 | } |
46 | #ifdef swap_cbcr |
47 | buf_uv[iuv++] = v; |
48 | buf_uv[iuv++] = u; |
49 | #else |
50 | buf_uv[iuv++] = u; |
51 | buf_uv[iuv++] = v; |
52 | #endif |
53 | } |
54 | } |
55 | } |
56 | } |
57 | |
58 | static inline void yuv_to_rgb16(unsigned char y,unsigned char u,unsigned char v,unsigned char *rgb) |
59 | { |
60 | register int r,g,b; |
61 | int rgb16; |
62 | |
63 | r = (1192 * (y - 16) + 1634 * (v - 128) ) >> 10; |
64 | g = (1192 * (y - 16) - 833 * (v - 128) - 400 * (u -128) ) >> 10; |
65 | b = (1192 * (y - 16) + 2066 * (u - 128) ) >> 10; |
66 | |
67 | r = r > 255 ? 255 : r < 0 ? 0 : r; |
68 | g = g > 255 ? 255 : g < 0 ? 0 : g; |
69 | b = b > 255 ? 255 : b < 0 ? 0 : b; |
70 | |
71 | rgb16 = (int)(((r >> 3)<<11) | ((g >> 2) << 5)| ((b >> 3) << 0)); |
72 | |
73 | *rgb = (unsigned char)(rgb16 & 0xFF); |
74 | rgb++; |
75 | *rgb = (unsigned char)((rgb16 & 0xFF00) >> 8); |
76 | |
77 | } |
78 | |
79 | void yuyv422_to_rgb16(unsigned char *from, unsigned char *to, int size) |
80 | { |
81 | int x,y,z=0; |
82 | |
83 | for (y = 0; y < size; y+=4) { |
84 | unsigned char Y1, Y2, U, V; |
85 | |
86 | Y1 = from[y + 0]; |
87 | U = from[y + 1]; |
88 | Y2 = from[y + 2]; |
89 | V = from[y + 3]; |
90 | |
91 | yuv_to_rgb16(Y1, U, V, &to[y]); |
92 | yuv_to_rgb16(Y2, U, V, &to[y + 2]); |
93 | } |
94 | } |
95 | |
96 | void yuyv422_to_rgb16(unsigned char *from, unsigned char *to, int width, int height) |
97 | { |
98 | yuyv422_to_rgb16(from,to,(width * height) * 2); |
99 | } |
100 | |
101 | void yuyv422_to_nv21(unsigned char *bufsrc, unsigned char *bufdest, int width, int height) |
102 | { |
103 | unsigned char *ptrsrcy1, *ptrsrcy2; |
104 | unsigned char *ptrsrcy3, *ptrsrcy4; |
105 | unsigned char *ptrsrccb1, *ptrsrccb2; |
106 | unsigned char *ptrsrccb3, *ptrsrccb4; |
107 | unsigned char *ptrsrccr1, *ptrsrccr2; |
108 | unsigned char *ptrsrccr3, *ptrsrccr4; |
109 | int srcystride, srcccstride; |
110 | |
111 | ptrsrcy1 = bufsrc ; |
112 | ptrsrcy2 = bufsrc + (width<<1) ; |
113 | ptrsrcy3 = bufsrc + (width<<1)*2 ; |
114 | ptrsrcy4 = bufsrc + (width<<1)*3 ; |
115 | |
116 | ptrsrccb1 = bufsrc + 1; |
117 | ptrsrccb2 = bufsrc + (width<<1) + 1; |
118 | ptrsrccb3 = bufsrc + (width<<1)*2 + 1; |
119 | ptrsrccb4 = bufsrc + (width<<1)*3 + 1; |
120 | |
121 | ptrsrccr1 = bufsrc + 3; |
122 | ptrsrccr2 = bufsrc + (width<<1) + 3; |
123 | ptrsrccr3 = bufsrc + (width<<1)*2 + 3; |
124 | ptrsrccr4 = bufsrc + (width<<1)*3 + 3; |
125 | |
126 | srcystride = (width<<1)*3; |
127 | srcccstride = (width<<1)*3; |
128 | |
129 | unsigned char *ptrdesty1, *ptrdesty2; |
130 | unsigned char *ptrdesty3, *ptrdesty4; |
131 | unsigned char *ptrdestcb1, *ptrdestcb2; |
132 | unsigned char *ptrdestcr1, *ptrdestcr2; |
133 | int destystride, destccstride; |
134 | |
135 | ptrdesty1 = bufdest; |
136 | ptrdesty2 = bufdest + width; |
137 | ptrdesty3 = bufdest + width*2; |
138 | ptrdesty4 = bufdest + width*3; |
139 | |
140 | ptrdestcb1 = bufdest + width*height; |
141 | ptrdestcb2 = bufdest + width*height + width; |
142 | |
143 | ptrdestcr1 = bufdest + width*height + 1; |
144 | ptrdestcr2 = bufdest + width*height + width + 1; |
145 | |
146 | destystride = (width)*3; |
147 | destccstride = width; |
148 | |
149 | int i, j; |
150 | |
151 | for(j=0; j<(height/4); j++) |
152 | { |
153 | for(i=0;i<(width/2);i++) |
154 | { |
155 | (*ptrdesty1++) = (*ptrsrcy1); |
156 | (*ptrdesty2++) = (*ptrsrcy2); |
157 | (*ptrdesty3++) = (*ptrsrcy3); |
158 | (*ptrdesty4++) = (*ptrsrcy4); |
159 | |
160 | ptrsrcy1 += 2; |
161 | ptrsrcy2 += 2; |
162 | ptrsrcy3 += 2; |
163 | ptrsrcy4 += 2; |
164 | |
165 | (*ptrdesty1++) = (*ptrsrcy1); |
166 | (*ptrdesty2++) = (*ptrsrcy2); |
167 | (*ptrdesty3++) = (*ptrsrcy3); |
168 | (*ptrdesty4++) = (*ptrsrcy4); |
169 | |
170 | ptrsrcy1 += 2; |
171 | ptrsrcy2 += 2; |
172 | ptrsrcy3 += 2; |
173 | ptrsrcy4 += 2; |
174 | |
175 | #if 0 |
176 | (*ptrdestcb1) = (*ptrsrccb1); |
177 | (*ptrdestcb2) = (*ptrsrccb3); |
178 | #else |
179 | (*ptrdestcb1) = (*ptrsrccr1); |
180 | (*ptrdestcb2) = (*ptrsrccr3); |
181 | #endif |
182 | |
183 | #if 0 |
184 | (*ptrdestcr1) = (*ptrsrccr1); |
185 | (*ptrdestcr2) = (*ptrsrccr3); |
186 | #else |
187 | (*ptrdestcr1) = (*ptrsrccb1); |
188 | (*ptrdestcr2) = (*ptrsrccb3); |
189 | #endif |
190 | |
191 | ptrdestcb1 += 2; |
192 | ptrdestcb2 += 2; |
193 | ptrdestcr1 += 2; |
194 | ptrdestcr2 += 2; |
195 | |
196 | ptrsrccb1 += 4; |
197 | ptrsrccb3 += 4; |
198 | ptrsrccr1 += 4; |
199 | ptrsrccr3 += 4; |
200 | |
201 | } |
202 | |
203 | |
204 | /* Update src pointers */ |
205 | ptrsrcy1 += srcystride; |
206 | ptrsrcy2 += srcystride; |
207 | ptrsrcy3 += srcystride; |
208 | ptrsrcy4 += srcystride; |
209 | |
210 | ptrsrccb1 += srcccstride; |
211 | ptrsrccb3 += srcccstride; |
212 | |
213 | ptrsrccr1 += srcccstride; |
214 | ptrsrccr3 += srcccstride; |
215 | |
216 | |
217 | /* Update dest pointers */ |
218 | ptrdesty1 += destystride; |
219 | ptrdesty2 += destystride; |
220 | ptrdesty3 += destystride; |
221 | ptrdesty4 += destystride; |
222 | |
223 | ptrdestcb1 += destccstride; |
224 | ptrdestcb2 += destccstride; |
225 | |
226 | ptrdestcr1 += destccstride; |
227 | ptrdestcr2 += destccstride; |
228 | |
229 | } |
230 | } |
231 | |
232 | static inline void yuv_to_rgb24(unsigned char y,unsigned char u,unsigned char v,unsigned char *rgb) |
233 | { |
234 | register int r,g,b; |
235 | int rgb24; |
236 | |
237 | r = (1192 * (y - 16) + 1634 * (v - 128) ) >> 10; |
238 | g = (1192 * (y - 16) - 833 * (v - 128) - 400 * (u -128) ) >> 10; |
239 | b = (1192 * (y - 16) + 2066 * (u - 128) ) >> 10; |
240 | |
241 | r = r > 255 ? 255 : r < 0 ? 0 : r; |
242 | g = g > 255 ? 255 : g < 0 ? 0 : g; |
243 | b = b > 255 ? 255 : b < 0 ? 0 : b; |
244 | |
245 | rgb24 = (int)((r <<16) | (g << 8)| b); |
246 | |
247 | *rgb = (unsigned char)r; |
248 | rgb++; |
249 | *rgb = (unsigned char)g; |
250 | rgb++; |
251 | *rgb = (unsigned char)b; |
252 | } |
253 | |
254 | void yuyv422_to_rgb24(unsigned char *buf, unsigned char *rgb, int width, int height) |
255 | { |
256 | int x,y,z=0; |
257 | int blocks; |
258 | |
259 | blocks = (width * height) * 2; |
260 | |
261 | for (y = 0,z=0; y < blocks; y+=4,z+=6) { |
262 | unsigned char Y1, Y2, U, V; |
263 | |
264 | Y1 = buf[y + 0]; |
265 | U = buf[y + 1]; |
266 | Y2 = buf[y + 2]; |
267 | V = buf[y + 3]; |
268 | |
269 | yuv_to_rgb24(Y1, U, V, &rgb[z]); |
270 | yuv_to_rgb24(Y2, U, V, &rgb[z + 3]); |
271 | |
272 | } |
273 | } |
274 | |
275 | void convert_rgb24_to_rgb16(uint8_t *src, uint8_t *dst, int width, int height) |
276 | { |
277 | int src_len = width*height*3; |
278 | int i = 0; |
279 | int j = 0; |
280 | |
281 | for (i = 0; i < src_len; i += 3) |
282 | { |
283 | dst[j] = (src[i]&0x1f) | (src[i+1]>>5); |
284 | dst[j+1] = ((src[i+1]>>2)<<5) | (src[i+2]>>3); |
285 | j += 2; |
286 | } |
287 | } |
288 | void yuyv_to_yv12(unsigned char *src, unsigned char *dst, int width, int height) |
289 | { |
290 | //width should be an even number. |
291 | //uv ALIGN 32. |
292 | int i,j,stride,c_stride,c_size,y_size,cb_offset,cr_offset; |
293 | unsigned char *dst_copy,*src_copy; |
294 | |
295 | dst_copy = dst; |
296 | src_copy = src; |
297 | |
298 | y_size = width*height; |
299 | c_stride = ALIGN(width/2, 16); |
300 | c_size = c_stride * height/2; |
301 | cr_offset = y_size; |
302 | cb_offset = y_size+c_size; |
303 | |
304 | for(i=0;i< y_size;i++){ |
305 | *dst++ = *src; |
306 | src += 2; |
307 | } |
308 | |
309 | dst = dst_copy; |
310 | src = src_copy; |
311 | |
312 | for(i=0;i<height;i+=2){ |
313 | for(j=1;j<width*2;j+=4){//one line has 2*width bytes for yuyv. |
314 | //ceil(u1+u2)/2 |
315 | *(dst+cr_offset+j/4)= (*(src+j+2) + *(src+j+2+width*2) + 1)/2; |
316 | *(dst+cb_offset+j/4)= (*(src+j) + *(src+j+width*2) + 1)/2; |
317 | } |
318 | dst += c_stride; |
319 | src += width*4; |
320 | } |
321 | } |
322 | #endif |
323 | void rgb24_memcpy(unsigned char *dst, unsigned char *src, int width, int height) |
324 | { |
325 | int stride = (width + 31) & ( ~31); |
326 | int w, h; |
327 | for (h=0; h<height; h++) |
328 | { |
329 | memcpy( dst, src, width*3); |
330 | dst += width*3; |
331 | src += stride*3; |
332 | } |
333 | } |
334 | |
335 | void nv21_memcpy_align32(unsigned char *dst, unsigned char *src, int width, int height) |
336 | { |
337 | int stride = (width + 31) & ( ~31); |
338 | int w, h; |
339 | for (h=0; h<height*3/2; h++) |
340 | { |
341 | memcpy( dst, src, width); |
342 | dst += width; |
343 | src += stride; |
344 | } |
345 | } |
346 | |
347 | void yv12_memcpy_align32(unsigned char *dst, unsigned char *src, int width, int height) |
348 | { |
349 | int new_width = (width + 63) & ( ~63); |
350 | int stride; |
351 | int w, h; |
352 | for (h=0; h<height; h++) |
353 | { |
354 | memcpy( dst, src, width); |
355 | dst += width; |
356 | src += new_width; |
357 | } |
358 | |
359 | stride = ALIGN(width/2, 16); |
360 | for (h=0; h<height; h++) |
361 | { |
362 | memcpy( dst, src, width/2); |
363 | dst += stride; |
364 | src += new_width/2; |
365 | } |
366 | } |
367 | |
368 | void yv12_adjust_memcpy(unsigned char *dst, unsigned char *src, int width, int height) |
369 | { |
370 | //width should be an even number. |
371 | int i,stride; |
372 | memcpy( dst, src, width*height); |
373 | src += width*height; |
374 | dst += width*height; |
375 | |
376 | stride = ALIGN(width/2, 16); |
377 | for(i =0; i< height; i++) |
378 | { |
379 | memcpy(dst,src, stride); |
380 | src+=width/2; |
381 | dst+=stride; |
382 | } |
383 | } |
384 | |
385 | void nv21_memcpy_canvas1080(unsigned char *dst, unsigned char *src, int width, int height) |
386 | { |
387 | int h; |
388 | for (h=0; h<height; h++){ |
389 | memcpy( dst, src, width); |
390 | dst += width; |
391 | src += width; |
392 | } |
393 | src+=width*8; |
394 | for (h=0; h<height/2; h++){ |
395 | memcpy( dst, src, width); |
396 | dst += width; |
397 | src += width; |
398 | } |
399 | } |
400 | |
401 | void yv12_memcpy_canvas1080(unsigned char *dst, unsigned char *src, int width, int height) |
402 | { |
403 | int h; |
404 | for (h=0; h<height; h++){ |
405 | memcpy( dst, src, width); |
406 | dst += width; |
407 | src += width; |
408 | } |
409 | src+=width*8; |
410 | for (h=0; h<height/2; h++){ |
411 | memcpy( dst, src, width/2); |
412 | dst += width/2; |
413 | src += width/2; |
414 | } |
415 | src+=width*2; |
416 | for (h=0; h<height/2; h++){ |
417 | memcpy( dst, src, width/2); |
418 | dst += width/2; |
419 | src += width/2; |
420 | } |
421 | } |
422 |