Add raw bayer to bitmap conversion tool
[omap4-v4l2-camera:takashi-suzukis-yavta.git] / raw2bmp.c
1 /*                 Raw to bmp converter
2 #
3 #              Paulo Assis <pj.assis@gmail.com
4 #
5 # This program is free software; you can redistribute it and/or modify
6 # it under the terms of the GNU General Public License as published by
7 # the Free Software Foundation; either version 2 of the License, or
8 # (at your option) any later version. 
9 #
10 # This program is distributed in the hope that it will be useful,
11 # but WITHOUT ANY WARRANTY; without even the implied warranty of
12 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 
13 # GNU General Public License for more details.
14 */
15
16 #include <stdio.h>
17 #include <stdlib.h>
18 #include <string.h>
19 #include <fcntl.h>
20 #include <inttypes.h>
21
22 #define TRUE -1
23 #define FALSE 0
24
25 typedef struct tagBITMAPFILEHEADER 
26
27     uint16_t    bfType; //Specifies the file type, must be BM
28     uint32_t   bfSize; //Specifies the size, in bytes, of the bitmap file
29     uint16_t    bfReserved1; //Reserved; must be zero
30     uint16_t    bfReserved2; //Reserved; must be zero
31     uint32_t   bfOffBits; /*Specifies the offset, in bytes, 
32                 from the beginning of the BITMAPFILEHEADER structure 
33                 to the bitmap bits= FileHeader+InfoHeader+RGBQUAD(0 for 24bit BMP)=64*/
34 }   __attribute__ ((packed)) BITMAPFILEHEADER, *PBITMAPFILEHEADER;
35
36
37 typedef struct tagBITMAPINFOHEADER
38 {
39     uint32_t  biSize; 
40     uint32_t   biWidth; 
41     uint32_t   biHeight; 
42     uint16_t   biPlanes; 
43     uint16_t   biBitCount; 
44     uint32_t  biCompression; 
45     uint32_t  biSizeImage; 
46     uint32_t   biXPelsPerMeter; 
47     uint32_t   biYPelsPerMeter; 
48     uint32_t  biClrUsed; 
49     uint32_t  biClrImportant; 
50 }  __attribute__ ((packed)) BITMAPINFOHEADER, *PBITMAPINFOHEADER;
51
52 static void convert_border_bayer_line_to_bgr24( uint8_t* bayer, uint8_t* adjacent_bayer,
53     uint8_t *bgr, int width, int start_with_green, int blue_line)
54 {
55     int t0, t1;
56
57     if (start_with_green) 
58     {
59     /* First pixel */
60         if (blue_line) 
61         {
62             *bgr++ = bayer[1];
63             *bgr++ = bayer[0];
64             *bgr++ = adjacent_bayer[0];
65         } 
66         else 
67         {
68             *bgr++ = adjacent_bayer[0];
69             *bgr++ = bayer[0];
70             *bgr++ = bayer[1];
71         }
72         /* Second pixel */
73         t0 = (bayer[0] + bayer[2] + adjacent_bayer[1] + 1) / 3;
74         t1 = (adjacent_bayer[0] + adjacent_bayer[2] + 1) >> 1;
75         if (blue_line) 
76         {
77             *bgr++ = bayer[1];
78             *bgr++ = t0;
79             *bgr++ = t1;
80         } 
81         else 
82         {
83             *bgr++ = t1;
84             *bgr++ = t0;
85             *bgr++ = bayer[1];
86         }
87         bayer++;
88         adjacent_bayer++;
89         width -= 2;
90     } 
91     else 
92     {
93         /* First pixel */
94         t0 = (bayer[1] + adjacent_bayer[0] + 1) >> 1;
95         if (blue_line) 
96         {
97             *bgr++ = bayer[0];
98             *bgr++ = t0;
99             *bgr++ = adjacent_bayer[1];
100         } 
101         else 
102         {
103             *bgr++ = adjacent_bayer[1];
104             *bgr++ = t0;
105             *bgr++ = bayer[0];
106         }
107         width--;
108     }
109
110     if (blue_line) 
111     {
112         for ( ; width > 2; width -= 2) 
113         {
114             t0 = (bayer[0] + bayer[2] + 1) >> 1;
115             *bgr++ = t0;
116             *bgr++ = bayer[1];
117             *bgr++ = adjacent_bayer[1];
118             bayer++;
119             adjacent_bayer++;
120
121             t0 = (bayer[0] + bayer[2] + adjacent_bayer[1] + 1) / 3;
122             t1 = (adjacent_bayer[0] + adjacent_bayer[2] + 1) >> 1;
123             *bgr++ = bayer[1];
124             *bgr++ = t0;
125             *bgr++ = t1;
126             bayer++;
127             adjacent_bayer++;
128         }
129     } 
130     else 
131     {
132         for ( ; width > 2; width -= 2) 
133         {
134             t0 = (bayer[0] + bayer[2] + 1) >> 1;
135             *bgr++ = adjacent_bayer[1];
136             *bgr++ = bayer[1];
137             *bgr++ = t0;
138             bayer++;
139             adjacent_bayer++;
140
141             t0 = (bayer[0] + bayer[2] + adjacent_bayer[1] + 1) / 3;
142             t1 = (adjacent_bayer[0] + adjacent_bayer[2] + 1) >> 1;
143             *bgr++ = t1;
144             *bgr++ = t0;
145             *bgr++ = bayer[1];
146             bayer++;
147             adjacent_bayer++;
148         }
149     }
150
151     if (width == 2) 
152     {
153         /* Second to last pixel */
154         t0 = (bayer[0] + bayer[2] + 1) >> 1;
155         if (blue_line) 
156         {
157             *bgr++ = t0;
158             *bgr++ = bayer[1];
159             *bgr++ = adjacent_bayer[1];
160         } 
161         else 
162         {
163             *bgr++ = adjacent_bayer[1];
164             *bgr++ = bayer[1];
165             *bgr++ = t0;
166         }
167         /* Last pixel */
168         t0 = (bayer[1] + adjacent_bayer[2] + 1) >> 1;
169         if (blue_line) 
170         {
171             *bgr++ = bayer[2];
172             *bgr++ = t0;
173             *bgr++ = adjacent_bayer[1];
174         }
175         else 
176         {
177             *bgr++ = adjacent_bayer[1];
178             *bgr++ = t0;
179             *bgr++ = bayer[2];
180         }
181     } 
182     else 
183     {
184         /* Last pixel */
185         if (blue_line) 
186         {
187             *bgr++ = bayer[0];
188             *bgr++ = bayer[1];
189             *bgr++ = adjacent_bayer[1];
190         } 
191         else 
192         {
193             *bgr++ = adjacent_bayer[1];
194             *bgr++ = bayer[1];
195             *bgr++ = bayer[0];
196         }
197     }
198 }
199
200 static void bayer_to_rgbbgr24(uint8_t *bayer,
201     uint8_t *bgr, int width, int height,
202     int start_with_green, int blue_line)
203 {
204     /* render the first line */
205     convert_border_bayer_line_to_bgr24(bayer, bayer + width, bgr, width,
206         start_with_green, blue_line);
207     bgr += width * 3;
208
209     /* reduce height by 2 because of the special case top/bottom line */
210     for (height -= 2; height; height--) 
211     {
212         int t0, t1;
213         /* (width - 2) because of the border */
214         uint8_t *bayerEnd = bayer + (width - 2);
215
216         if (start_with_green) 
217         {
218             /* OpenCV has a bug in the next line, which was
219             t0 = (bayer[0] + bayer[width * 2] + 1) >> 1; */
220             t0 = (bayer[1] + bayer[width * 2 + 1] + 1) >> 1;
221             /* Write first pixel */
222             t1 = (bayer[0] + bayer[width * 2] + bayer[width + 1] + 1) / 3;
223             if (blue_line) 
224             {
225                 *bgr++ = t0;
226                 *bgr++ = t1;
227                 *bgr++ = bayer[width];
228             } 
229             else 
230             {
231                 *bgr++ = bayer[width];
232                 *bgr++ = t1;
233                 *bgr++ = t0;
234             }
235
236             /* Write second pixel */
237             t1 = (bayer[width] + bayer[width + 2] + 1) >> 1;
238             if (blue_line) 
239             {
240                 *bgr++ = t0;
241                 *bgr++ = bayer[width + 1];
242                 *bgr++ = t1;
243             } 
244             else 
245             {
246                 *bgr++ = t1;
247                 *bgr++ = bayer[width + 1];
248                 *bgr++ = t0;
249             }
250             bayer++;
251         } 
252         else 
253         {
254             /* Write first pixel */
255             t0 = (bayer[0] + bayer[width * 2] + 1) >> 1;
256             if (blue_line) 
257             {
258                 *bgr++ = t0;
259                 *bgr++ = bayer[width];
260                 *bgr++ = bayer[width + 1];
261             } 
262             else 
263             {
264                 *bgr++ = bayer[width + 1];
265                 *bgr++ = bayer[width];
266                 *bgr++ = t0;
267             }
268         }
269
270         if (blue_line) 
271         {
272             for (; bayer <= bayerEnd - 2; bayer += 2) 
273             {
274                 t0 = (bayer[0] + bayer[2] + bayer[width * 2] +
275                     bayer[width * 2 + 2] + 2) >> 2;
276                 t1 = (bayer[1] + bayer[width] +
277                     bayer[width + 2] + bayer[width * 2 + 1] +
278                     2) >> 2;
279                 *bgr++ = t0;
280                 *bgr++ = t1;
281                 *bgr++ = bayer[width + 1];
282
283                 t0 = (bayer[2] + bayer[width * 2 + 2] + 1) >> 1;
284                 t1 = (bayer[width + 1] + bayer[width + 3] +
285                     1) >> 1;
286                 *bgr++ = t0;
287                 *bgr++ = bayer[width + 2];
288                 *bgr++ = t1;
289             }
290         } 
291         else 
292         {
293             for (; bayer <= bayerEnd - 2; bayer += 2) 
294             {
295                 t0 = (bayer[0] + bayer[2] + bayer[width * 2] +
296                     bayer[width * 2 + 2] + 2) >> 2;
297                 t1 = (bayer[1] + bayer[width] +
298                     bayer[width + 2] + bayer[width * 2 + 1] +
299                     2) >> 2;
300                 *bgr++ = bayer[width + 1];
301                 *bgr++ = t1;
302                 *bgr++ = t0;
303
304                 t0 = (bayer[2] + bayer[width * 2 + 2] + 1) >> 1;
305                 t1 = (bayer[width + 1] + bayer[width + 3] +
306                     1) >> 1;
307                 *bgr++ = t1;
308                 *bgr++ = bayer[width + 2];
309                 *bgr++ = t0;
310             }
311         }
312
313         if (bayer < bayerEnd) 
314         {
315             /* write second to last pixel */
316             t0 = (bayer[0] + bayer[2] + bayer[width * 2] +
317                 bayer[width * 2 + 2] + 2) >> 2;
318             t1 = (bayer[1] + bayer[width] +
319                 bayer[width + 2] + bayer[width * 2 + 1] +
320                 2) >> 2;
321             if (blue_line) 
322             {
323                 *bgr++ = t0;
324                 *bgr++ = t1;
325                 *bgr++ = bayer[width + 1];
326             } 
327             else 
328             {
329                 *bgr++ = bayer[width + 1];
330                 *bgr++ = t1;
331                 *bgr++ = t0;
332             }
333             /* write last pixel */
334             t0 = (bayer[2] + bayer[width * 2 + 2] + 1) >> 1;
335             if (blue_line) 
336             {
337                 *bgr++ = t0;
338                 *bgr++ = bayer[width + 2];
339                 *bgr++ = bayer[width + 1];
340             } 
341             else 
342             {
343                 *bgr++ = bayer[width + 1];
344                 *bgr++ = bayer[width + 2];
345                 *bgr++ = t0;
346             }
347             bayer++;
348         } 
349         else
350         {
351             /* write last pixel */
352             t0 = (bayer[0] + bayer[width * 2] + 1) >> 1;
353             t1 = (bayer[1] + bayer[width * 2 + 1] + bayer[width] + 1) / 3;
354             if (blue_line) 
355             {
356                 *bgr++ = t0;
357                 *bgr++ = t1;
358                 *bgr++ = bayer[width + 1];
359             } 
360             else 
361             {
362                 *bgr++ = bayer[width + 1];
363                 *bgr++ = t1;
364                 *bgr++ = t0;
365             }
366         }
367
368         /* skip 2 border pixels */
369         bayer += 2;
370
371         blue_line = !blue_line;
372         start_with_green = !start_with_green;
373     }
374
375     /* render the last line */
376     convert_border_bayer_line_to_bgr24(bayer + width, bayer, bgr, width,
377         !start_with_green, !blue_line);
378 }
379
380 /*convert bayer raw data to rgb24
381 * args: 
382 *      pBay: pointer to buffer containing Raw bayer data data
383 *      pRGB24: pointer to buffer containing rgb24 data
384 *      width: picture width
385 *      height: picture height
386 *      pix_order: bayer pixel order (0=gb/rg   1=gr/bg  2=bg/gr  3=rg/bg)
387 */
388 static void 
389 bayer_to_rgb24(uint8_t *pBay, uint8_t *pRGB24, int width, int height, int pix_order)
390 {
391     switch (pix_order) 
392     {
393         /*conversion functions are build for bgr, by switching b and r lines we get rgb*/
394         case 0: /* gbgbgb... | rgrgrg... (V4L2_PIX_FMT_SGBRG8)*/
395             bayer_to_rgbbgr24(pBay, pRGB24, width, height, TRUE, FALSE);
396             break;
397         
398         case 1: /* grgrgr... | bgbgbg... (V4L2_PIX_FMT_SGRBG8)*/
399             bayer_to_rgbbgr24(pBay, pRGB24, width, height, TRUE, TRUE);
400             break;
401         
402         case 2: /* bgbgbg... | grgrgr... (V4L2_PIX_FMT_SBGGR8)*/
403             bayer_to_rgbbgr24(pBay, pRGB24, width, height, FALSE, FALSE);
404             break;
405         
406         case 3: /* rgrgrg... ! gbgbgb... (V4L2_PIX_FMT_SRGGB8)*/
407             bayer_to_rgbbgr24(pBay, pRGB24, width, height, FALSE, TRUE);
408             break;
409             
410         default: /* default is 0*/
411             bayer_to_rgbbgr24(pBay, pRGB24, width, height, TRUE, FALSE);
412             break;
413     }
414 }
415
416 static void
417 flip_bgr_image(uint8_t *bgr_buff, int width, int height)
418 {
419     int i =0;
420     /*alloc a temp buffer*/
421     uint8_t *tmp_buff = calloc(width*height*3, 1);
422     uint8_t *t1 = bgr_buff + (width*height*3); /*point to the end of buffer*/
423
424     for(i=0;i<height;i++)
425     {
426         t1 -= width * 3; /*decrement a line*/
427         memcpy(tmp_buff+(i*width*3), t1, width*3); /* copy the line to temp buffer*/
428     }
429     /*get the fliped buffer*/
430     memcpy(bgr_buff, tmp_buff, height*width*3);
431     free(tmp_buff);
432 }
433
434 static int 
435 SaveBPM(const char *Filename, int width, int height, int BitCount, uint8_t *ImagePix) 
436 {
437     int ret=0;
438     BITMAPFILEHEADER BmpFileh;
439     BITMAPINFOHEADER BmpInfoh;
440     uint32_t imgsize;
441     FILE *fp;
442
443     imgsize=width*height*BitCount/8;
444
445     BmpFileh.bfType=0x4d42;//must be BM (x4d42) 
446     /*Specifies the size, in bytes, of the bitmap file*/
447     BmpFileh.bfSize=sizeof(BITMAPFILEHEADER)+sizeof(BITMAPINFOHEADER)+imgsize;
448     BmpFileh.bfReserved1=0; //Reserved; must be zero
449     BmpFileh.bfReserved2=0; //Reserved; must be zero
450     /*Specifies the offset, in bytes,                      */
451     /*from the beginning of the BITMAPFILEHEADER structure */
452     /* to the bitmap bits                                  */
453     BmpFileh.bfOffBits=sizeof(BITMAPFILEHEADER)+sizeof(BITMAPINFOHEADER); 
454
455     BmpInfoh.biSize=40;
456     BmpInfoh.biWidth=width; 
457     BmpInfoh.biHeight=height; 
458     BmpInfoh.biPlanes=1; 
459     BmpInfoh.biBitCount=BitCount; 
460     BmpInfoh.biCompression=0;  
461     BmpInfoh.biSizeImage=imgsize; 
462     BmpInfoh.biXPelsPerMeter=0; 
463     BmpInfoh.biYPelsPerMeter=0; 
464     BmpInfoh.biClrUsed=0; 
465     BmpInfoh.biClrImportant=0;
466
467     if ((fp = fopen(Filename,"wb"))!=NULL) 
468     {    
469         ret=fwrite(&BmpFileh, sizeof(BITMAPFILEHEADER), 1, fp);
470         ret+=fwrite(&BmpInfoh, sizeof(BITMAPINFOHEADER),1,fp);
471         ret+=fwrite(ImagePix,imgsize,1,fp);
472         if (ret<3) ret=1;
473         else ret=0;
474
475         fflush(fp); 
476         if(fsync(fileno(fp)) || fclose(fp))
477         {
478             perror("BMP ERROR - couldn't write to file");
479             ret=1;
480         }
481     } 
482     else 
483     {
484         ret=1;
485         printf("ERROR: Could not open file %s for write \n",Filename);
486     }
487     return ret;
488 }
489
490 int main(int argc, char *argv[])
491 {
492
493     if(argc < 5)
494         printf("usage: %s filename.raw filename.bmp width height [pixel_size_in_bits] [pix_order]\n", argv[0]);
495
496     char* in_filename = argv[1];
497     char* out_filename = argv[2];
498     
499     int width = atoi(argv[3]);
500     int height = atoi(argv[4]);
501     int pix_size = 8;
502     int pix_order = 0;
503     if(argc > 5) 
504         pix_size = atoi(argv[5]);
505     if(argc > 6)
506         pix_order = atoi(argv[6]);
507         
508     if((pix_size != 8) && (pix_size != 10))
509     {
510         printf("requested %i bits per pixel but only 8 and 10 bit bayer is supported\n", pix_size);
511         return(-1);
512     }
513     
514     /*round by excess*/
515     size_t in_buff_size = ((width*height*pix_size)/8) + 0.49;
516     uint8_t* in_buff = calloc(in_buff_size, 1);
517     
518     /*bgr buffer*/
519     uint8_t* bgr_buff = NULL;
520     
521     FILE *fp;
522     if ((fp = fopen(in_filename,"rb"))!=NULL) { 
523         printf("opening %s\n", in_filename);
524     } else {
525         printf("couldn't open %s for read\n",in_filename);
526         free(in_buff);
527         return (-1);
528     }
529     
530     size_t bytes_read = fread(in_buff, 1, in_buff_size, fp);
531     printf("read %i bytes from file %s\n", bytes_read, in_filename);
532     
533     if(pix_size == 10)
534     {
535         /*alocate a temp buffer*/
536         uint8_t *tmp_buff = calloc(width*height, 1);
537         uint8_t *t1 = tmp_buff;
538         uint8_t *t2 = in_buff;
539         
540         uint16_t pixel;
541     
542         /*process 4 pixel group (5 bytes)*/
543         int i =0;
544         for(i=0;i<=in_buff_size;i+=5)
545         {
546             
547             pixel = t2[0] | (t2[1] & 0x03)<<8;
548             *t1++ = (uint8_t) (pixel >> 2);
549             
550             pixel = (t2[1] & 0xFC)>>2 | (t2[2] & 0x0F)<<6 ;
551             *t1++ = (uint8_t) (pixel >> 2);
552             
553             pixel = (t2[2] & 0xF0)>>4 | (t2[3] & 0x3F)<<4;
554             *t1++ = (uint8_t) (pixel >> 2);
555             
556             pixel = (t2[3] & 0xC0)>>6 | (t2[4])<<2;
557             *t1++ = (uint8_t) (pixel >> 2);
558           
559             t2+=5;
560         }
561         
562         memcpy(in_buff, tmp_buff, width*height);
563         free(tmp_buff);
564     }
565     
566     //convert to bgr
567     bgr_buff = calloc(width*height*3, 1);
568     
569     bayer_to_rgb24(in_buff, bgr_buff, width, height, pix_order);
570     
571     flip_bgr_image(bgr_buff, width, height);
572     SaveBPM(out_filename, width, height, 24, bgr_buff);
573     
574     fclose(fp);
575     free(in_buff);
576     free(bgr_buff);
577     return 0;
578 }