v4l2_test.c 50 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658165916601661166216631664166516661667166816691670167116721673167416751676167716781679168016811682168316841685168616871688168916901691169216931694169516961697169816991700170117021703170417051706170717081709171017111712171317141715171617171718171917201721172217231724172517261727172817291730173117321733173417351736173717381739174017411742174317441745174617471748174917501751175217531754175517561757175817591760176117621763
  1. /***************************************************************************
  2. * v4l2grab Version 0.3 *
  3. * Copyright (C) 2012 by Tobias Müller *
  4. * Tobias_Mueller@twam.info *
  5. * *
  6. * based on V4L2 Specification, Appendix B: Video Capture Example *
  7. * (http://v4l2spec.bytesex.org/spec/capture-example.html) *
  8. * *
  9. * This program is free software; you can redistribute it and/or modify *
  10. * it under the terms of the GNU General Public License as published by *
  11. * the Free Software Foundation; either version 2 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 General Public License for more details. *
  18. * *
  19. * You should have received a copy of the GNU General Public License *
  20. * along with this program; if not, write to the *
  21. * Free Software Foundation, Inc., *
  22. * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
  23. ***************************************************************************/
  24. /**************************************************************************
  25. * Modification History *
  26. * *
  27. * Matthew Witherwax 21AUG2013 *
  28. * Added ability to change frame interval (ie. frame rate/fps) *
  29. * Martin Savc 7JUL2015
  30. * Added support for continuous capture using SIGINT to stop.
  31. ***************************************************************************/
  32. // compile with all three access methods
  33. #if !defined(IO_READ) && !defined(IO_MMAP) && !defined(IO_USERPTR)
  34. #define IO_READ
  35. #define IO_MMAP
  36. #define IO_USERPTR
  37. #endif
  38. #include <stdio.h>
  39. #include <stdlib.h>
  40. #include <string.h>
  41. #include <assert.h>
  42. #include <getopt.h>
  43. #include <fcntl.h>
  44. #include <unistd.h>
  45. #include <errno.h>
  46. #include <malloc.h>
  47. #include <sys/stat.h>
  48. #include <sys/types.h>
  49. #include <sys/time.h>
  50. #include <time.h>
  51. #include <sys/mman.h>
  52. #include <sys/ioctl.h>
  53. #include <asm/types.h>
  54. #include <linux/videodev2.h>
  55. #include <jpeglib.h>
  56. #include <libv4l2.h>
  57. #include <signal.h>
  58. #include <stdint.h>
  59. #include <inttypes.h>
  60. #include <linux/fb.h>
  61. #include <stdbool.h>
  62. #include "config.h"
  63. #include "yuv.h"
  64. #include "convert.h"
  65. #define CLEAR(x) memset (&(x), 0, sizeof (x))
  66. #ifndef VERSION
  67. #define VERSION "v0.0.1"
  68. #endif
  69. #define FILENAME_MAX_LEN 30
  70. struct stfisp_fw_info {
  71. char filename[FILENAME_MAX_LEN];
  72. };
  73. struct v4l2_subdev_frame_size_enum {
  74. __u32 index;
  75. __u32 pad;
  76. __u32 code;
  77. __u32 min_width;
  78. __u32 max_width;
  79. __u32 min_height;
  80. __u32 max_height;
  81. __u32 which;
  82. __u32 reserved[8];
  83. };
  84. #define VIDIOC_STFISP_LOAD_FW \
  85. _IOW('V', BASE_VIDIOC_PRIVATE + 1, struct stfisp_fw_info)
  86. #define VIDIOC_SUBDEV_ENUM_FRAME_SIZE \
  87. _IOWR('V', 74, struct v4l2_subdev_frame_size_enum)
  88. #define MEDIA_BUS_FMT_SRGGB10_1X10 0x300f
  89. #define FBIOPAN_GET_PP_MODE 0x4609
  90. #define FBIOPAN_SET_PP_MODE 0x460a
  91. enum COLOR_FORMAT{
  92. COLOR_YUV422_UYVY = 0, // 00={Y1,V0,Y0,U0}
  93. COLOR_YUV422_VYUY = 1, // 01={Y1,U0,Y0,V0}
  94. COLOR_YUV422_YUYV = 2, // 10={V0,Y1,U0,Y0}
  95. COLOR_YUV422_YVYU = 3, // 11={U0,Y1,V0,Y0}
  96. COLOR_YUV420P, // 4
  97. COLOR_YUV420_NV21, // 5
  98. COLOR_YUV420_NV12, // 6
  99. COLOR_RGB888_ARGB, // 7
  100. COLOR_RGB888_ABGR, // 8
  101. COLOR_RGB888_RGBA, // 9
  102. COLOR_RGB888_BGRA, // 10
  103. COLOR_RGB565, // 11
  104. };
  105. struct pp_video_mode {
  106. enum COLOR_FORMAT format;
  107. unsigned int height;
  108. unsigned int width;
  109. unsigned int addr;
  110. };
  111. struct pp_mode {
  112. char pp_id;
  113. bool bus_out; /*out to ddr*/
  114. bool fifo_out; /*out to lcdc*/
  115. bool inited;
  116. struct pp_video_mode src;
  117. struct pp_video_mode dst;
  118. };
  119. #if defined(IO_MMAP) || defined(IO_USERPTR)
  120. // minimum number of buffer to request in VIDIOC_REQBUFS call
  121. #define VIDIOC_REQBUFS_COUNT 5
  122. #endif
  123. typedef enum {
  124. #ifdef IO_READ
  125. IO_METHOD_READ,
  126. #endif
  127. #ifdef IO_MMAP
  128. IO_METHOD_MMAP,
  129. #endif
  130. #ifdef IO_USERPTR
  131. IO_METHOD_USERPTR,
  132. #endif
  133. } io_method;
  134. struct buffer {
  135. void * start;
  136. size_t length;
  137. };
  138. static io_method g_io_mthd = IO_METHOD_MMAP;
  139. static int g_fd = -1;
  140. static int g_fb_fd = -1;
  141. static int g_stfbc_fd = -1;
  142. struct buffer * g_buffers = NULL;
  143. static unsigned int n_buffers = 0;
  144. struct fb_var_screeninfo g_vinfo;
  145. struct fb_fix_screeninfo g_finfo;
  146. static unsigned char *g_fb_buf = NULL;
  147. long g_screensize = 1920 * 1080 * 2;
  148. long g_imagesize = 1920 * 1080 * 2;
  149. // global settings
  150. static unsigned int format= V4L2_PIX_FMT_RGB565;
  151. static int pixformat = COLOR_RGB565;
  152. static unsigned int width = 1920;
  153. static unsigned int height = 1080;
  154. static unsigned int left = 0;
  155. static unsigned int up = 0;
  156. static unsigned int right = 0;
  157. static unsigned int down = 0;
  158. static unsigned int crop_flag = 0;
  159. //static int istride = 1280;
  160. static unsigned int fps = 30;
  161. static unsigned int test_fps = 0;
  162. static int continuous = 0;
  163. static unsigned char jpegQuality = 70;
  164. static char* jpegFilename = NULL;
  165. static char* deviceName = "/dev/video0";
  166. static unsigned int fps_count = 0;
  167. static struct v4l2_subdev_frame_size_enum frame_size;
  168. //static const char* const continuousFilenameFmt = "%s_%010"PRIu32"_%"PRId64".jpg";
  169. inline int clip(int value, int min, int max) {
  170. return (value > max ? max : value < min ? min : value);
  171. }
  172. static int v4l2fmt_to_fbfmt(unsigned int format)
  173. {
  174. int pixformat = COLOR_RGB565;
  175. switch (format) {
  176. case V4L2_PIX_FMT_RGB565:
  177. pixformat = COLOR_RGB565;
  178. break;
  179. case V4L2_PIX_FMT_RGB24:
  180. pixformat = COLOR_RGB888_ARGB;
  181. break;
  182. case V4L2_PIX_FMT_YUV420:
  183. pixformat = COLOR_YUV420P;
  184. break;
  185. case V4L2_PIX_FMT_YUYV:
  186. pixformat = COLOR_YUV422_YUYV;
  187. break;
  188. case V4L2_PIX_FMT_NV21:
  189. pixformat = COLOR_YUV420_NV21;
  190. break;
  191. case V4L2_PIX_FMT_NV12:
  192. pixformat = COLOR_YUV420_NV12;
  193. break;
  194. case V4L2_PIX_FMT_YVYU:
  195. pixformat = COLOR_YUV422_YVYU;
  196. break;
  197. default:
  198. pixformat = COLOR_RGB565;
  199. break;
  200. }
  201. return pixformat;
  202. }
  203. void test_float_1()
  204. {
  205. int count = 1920 * 1080 * 2;
  206. float r, g, b;
  207. float y = 10, u = 20, v = 30;
  208. int int_r, int_g, int_b;
  209. int int_y = 10, int_u = 20, int_v = 30;
  210. struct timeval tv1, tv2, tv3;
  211. long long elapse = 0;
  212. int i;
  213. gettimeofday(&tv1, NULL);
  214. while (count--) {
  215. b = 1.164 * (y - 16) + 2.018 * (u - 128);
  216. g = 1.164 * (y - 16) - 0.813 * (v - 128) - 0.391 * (u - 128);
  217. r = 1.164 * (y - 16) + 1.596 * (v - 128);
  218. y ++;
  219. u ++;
  220. v ++;
  221. }
  222. gettimeofday(&tv2, NULL);
  223. elapse = (tv2.tv_sec - tv1.tv_sec) * 1000 + (tv2.tv_usec - tv1.tv_usec) / 1000;
  224. printf("elapse: %lldms, out: r=%f, g=%f, b=%f\n",
  225. elapse ,r, g, b);
  226. count = 1920 * 1080 * 2;
  227. gettimeofday(&tv1, NULL);
  228. while (count--) {
  229. int_b = 1164 * (y - 16) + 2018 * (u - 128);
  230. int_g = 1164 * (y - 16) - 813 * (v - 128) - 391 * (u - 128);
  231. int_r = 1164 * (y - 16) + 1596 * (v - 128);
  232. int_y ++;
  233. int_u ++;
  234. int_v ++;
  235. }
  236. gettimeofday(&tv2, NULL);
  237. elapse = (tv2.tv_sec - tv1.tv_sec) + (tv2.tv_usec - tv1.tv_usec);
  238. printf("elapse: %lldus, out: r=%d, g=%d, b=%d\n",
  239. elapse , int_r, int_g, int_b);
  240. count = 1920 * 1080 * 2;
  241. unsigned char* arraybuf = NULL;
  242. arraybuf = (unsigned char*)malloc(count);
  243. if (!arraybuf) {
  244. printf("arraybuf malloc error\n");
  245. return;
  246. }
  247. unsigned char* arraybuf2 = NULL;
  248. arraybuf2 = (unsigned char*)malloc(count);
  249. if (!arraybuf2) {
  250. printf("arraybuf2 malloc error\n");
  251. return;
  252. }
  253. gettimeofday(&tv1, NULL);
  254. for (i = 0; i < count; i++) {
  255. arraybuf[i] = i + 1;
  256. }
  257. gettimeofday(&tv2, NULL);
  258. memcpy(arraybuf2, arraybuf, count);
  259. gettimeofday(&tv3, NULL);
  260. elapse = (tv2.tv_sec - tv1.tv_sec) * 1000 + (tv2.tv_usec - tv1.tv_usec) / 1000;
  261. printf("for() elapse: %lldms\n", elapse);
  262. elapse = (tv3.tv_sec - tv2.tv_sec) * 1000000 + (tv3.tv_usec - tv2.tv_usec);
  263. printf("memcpy() run elapse: %lldus\n", elapse);
  264. convert_nv21_to_rgb(arraybuf, arraybuf2, 1920, 1080, 1);
  265. free(arraybuf);
  266. free(arraybuf2);
  267. }
  268. /**
  269. SIGINT interput handler
  270. */
  271. void StopContCapture(int sig_id) {
  272. printf("stoping continuous capture\n");
  273. continuous = 0;
  274. }
  275. void InstallSIGINTHandler() {
  276. struct sigaction sa;
  277. CLEAR(sa);
  278. sa.sa_handler = StopContCapture;
  279. if(sigaction(SIGINT, &sa, 0) != 0)
  280. {
  281. fprintf(stderr,"could not install SIGINT handler, continuous capture disabled");
  282. continuous = 0;
  283. }
  284. }
  285. /**
  286. Print error message and terminate programm with EXIT_FAILURE return code.
  287. \param s error message to print
  288. */
  289. static void errno_exit(const char* s)
  290. {
  291. fprintf(stderr, "%s error %d, %s\n", s, errno, strerror(errno));
  292. exit(EXIT_FAILURE);
  293. }
  294. /**
  295. Do ioctl and retry if error was EINTR ("A signal was caught during the ioctl() operation."). Parameters are the same as on ioctl.
  296. \param fd file descriptor
  297. \param request request
  298. \param argp argument
  299. \returns result from ioctl
  300. */
  301. static int xioctl(int fd, int request, void* argp)
  302. {
  303. int r;
  304. do r = v4l2_ioctl(fd, request, argp);
  305. while (-1 == r && EINTR == errno);
  306. return r;
  307. }
  308. /**
  309. Write image to jpeg file.
  310. \param img image to write
  311. */
  312. int write_file (char * filename,unsigned char *image_buffer, int size)
  313. {
  314. /* More stuff */
  315. FILE * outfile; /* target file */
  316. if ((outfile = fopen(filename, "w+")) == NULL) {
  317. fprintf(stderr, "can't open %s\n", filename);
  318. return -1;
  319. }
  320. fwrite(image_buffer, size, 1, outfile);
  321. fclose(outfile);
  322. return 0 ;
  323. }
  324. int write_JPEG_file (char * filename,unsigned char *image_buffer, int image_width, int image_height, int quality )
  325. {
  326. struct jpeg_compress_struct cinfo;
  327. struct jpeg_error_mgr jerr;
  328. /* More stuff */
  329. FILE * outfile; /* target file */
  330. JSAMPROW row_pointer[1]; /* pointer to JSAMPLE row[s] */
  331. int row_stride; /* physical row width in image buffer */
  332. /* Step 1: allocate and initialize JPEG compression object */
  333. cinfo.err = jpeg_std_error(&jerr);
  334. /* Now we can initialize the JPEG compression object. */
  335. jpeg_create_compress(&cinfo);
  336. /* Step 2: specify data destination (eg, a file) */
  337. /* Note: steps 2 and 3 can be done in either order. */
  338. if ((outfile = fopen(filename, "w+")) == NULL) {
  339. fprintf(stderr, "can't open %s\n", filename);
  340. return -1;
  341. }
  342. jpeg_stdio_dest(&cinfo, outfile);
  343. /* Step 3: set parameters for compression */
  344. cinfo.image_width = image_width; /* image width and height, in pixels */
  345. cinfo.image_height = image_height;
  346. cinfo.input_components = 3; /* # of color components per pixel */
  347. cinfo.in_color_space = JCS_RGB; /* colorspace of input image */
  348. jpeg_set_defaults(&cinfo);
  349. jpeg_set_quality(&cinfo, quality, TRUE /* limit to baseline-JPEG values */);
  350. /* Step 4: Start compressor */
  351. jpeg_start_compress(&cinfo, TRUE);
  352. /* Step 5: while (scan lines remain to be written) */
  353. row_stride = image_width * 3; /* JSAMPLEs per row in image_buffer */
  354. while (cinfo.next_scanline < cinfo.image_height) {
  355. row_pointer[0] = & image_buffer[cinfo.next_scanline * row_stride];
  356. (void) jpeg_write_scanlines(&cinfo, row_pointer, 1);
  357. }
  358. /* Step 6: Finish compression */
  359. jpeg_finish_compress(&cinfo);
  360. /* After finish_compress, we can close the output file. */
  361. fclose(outfile);
  362. /* Step 7: release JPEG compression object */
  363. /* This is an important step since it will release a good deal of memory. */
  364. jpeg_destroy_compress(&cinfo);
  365. /* And we're done! */
  366. return 0 ;
  367. }
  368. static void jpegWrite(unsigned char* img, char* jpegFilename)
  369. {
  370. struct jpeg_compress_struct cinfo;
  371. struct jpeg_error_mgr jerr;
  372. JSAMPROW row_pointer[1];
  373. FILE *outfile = fopen( jpegFilename, "wb" );
  374. // try to open file for saving
  375. if (!outfile) {
  376. errno_exit("jpeg");
  377. }
  378. // create jpeg data
  379. cinfo.err = jpeg_std_error( &jerr );
  380. jpeg_create_compress(&cinfo);
  381. jpeg_stdio_dest(&cinfo, outfile);
  382. // set image parameters
  383. cinfo.image_width = width;
  384. cinfo.image_height = height;
  385. cinfo.input_components = 3;
  386. cinfo.in_color_space = JCS_YCbCr;
  387. // set jpeg compression parameters to default
  388. jpeg_set_defaults(&cinfo);
  389. // and then adjust quality setting
  390. jpeg_set_quality(&cinfo, jpegQuality, TRUE);
  391. // start compress
  392. jpeg_start_compress(&cinfo, TRUE);
  393. // feed data
  394. while (cinfo.next_scanline < cinfo.image_height) {
  395. row_pointer[0] = &img[cinfo.next_scanline * cinfo.image_width * cinfo.input_components];
  396. jpeg_write_scanlines(&cinfo, row_pointer, 1);
  397. }
  398. // finish compression
  399. jpeg_finish_compress(&cinfo);
  400. // destroy jpeg data
  401. jpeg_destroy_compress(&cinfo);
  402. // close output file
  403. fclose(outfile);
  404. }
  405. /**
  406. process image read
  407. */
  408. static void imageProcess(const void* p, struct timeval timestamp)
  409. {
  410. //timestamp.tv_sec
  411. //timestamp.tv_usec
  412. unsigned char* src = (unsigned char*)p;
  413. unsigned char* dst = malloc(width*height*3*sizeof(char));
  414. static int count = 0;
  415. if (test_fps)
  416. return;
  417. // write jpeg
  418. char filename[512];
  419. switch (format) {
  420. case V4L2_PIX_FMT_YUV420:
  421. if (jpegFilename) {
  422. // sprintf(filename, "%d-yuv420-%s", count, jpegFilename);
  423. // YUV420toYUV444(width, height, src, dst);
  424. // jpegWrite(dst, filename);
  425. sprintf(filename, "raw-%d-yuv420-%s", count, jpegFilename);
  426. write_file(filename, src, g_imagesize);
  427. count++;
  428. }
  429. break;
  430. case V4L2_PIX_FMT_YUYV:
  431. case V4L2_PIX_FMT_YVYU:
  432. if (jpegFilename) {
  433. // sprintf(filename, "%d-yuv422-%s", count, jpegFilename);
  434. // YUV422toYUV444(width, height, src, dst);
  435. // jpegWrite(dst, filename);
  436. sprintf(filename, "raw-%d-yuv422-%s", count, jpegFilename);
  437. write_file(filename, src, g_imagesize);
  438. count++;
  439. }
  440. if (pixformat == v4l2fmt_to_fbfmt(format)) {
  441. yuyv_resize(src, g_fb_buf, width, height);
  442. } else if (g_vinfo.grayscale) {
  443. convert_yuyv_to_nv12(src, g_fb_buf, width, height, 1);
  444. } else {
  445. convert_yuyv_to_rgb(src, g_fb_buf, width, height, 0);
  446. }
  447. break;
  448. case V4L2_PIX_FMT_NV21:
  449. if (jpegFilename) {
  450. // sprintf(filename, "%d-nv21-%s", count, jpegFilename);
  451. // YUV420NV21toYUV444(width, height, src, dst);
  452. // jpegWrite(dst, filename);
  453. sprintf(filename, "raw-%d-nv21-%s", count, jpegFilename);
  454. write_file(filename, src, g_imagesize);
  455. count++;
  456. }
  457. if (pixformat == v4l2fmt_to_fbfmt(format))
  458. convert_nv21_to_nv12(src, g_fb_buf, width, height, 0);
  459. else if (g_vinfo.grayscale)
  460. convert_nv21_to_nv12(src, g_fb_buf, width, height, 1);
  461. else
  462. convert_nv21_to_rgb(src, g_fb_buf, width, height, 1);
  463. break;
  464. case V4L2_PIX_FMT_NV12:
  465. if (jpegFilename) {
  466. // sprintf(filename, "%d-nv12-%s", count, jpegFilename);
  467. // YUV420NV12toYUV444(width, height, src, dst);
  468. // jpegWrite(dst, filename);
  469. sprintf(filename, "raw-%d-nv12-%s", count, jpegFilename);
  470. write_file(filename, src, g_imagesize);
  471. count++;
  472. }
  473. if (pixformat == v4l2fmt_to_fbfmt(format))
  474. convert_nv21_to_nv12(src, g_fb_buf, width, height, 0);
  475. else if (g_vinfo.grayscale)
  476. convert_nv21_to_nv12(src, g_fb_buf, width, height, 0);
  477. else
  478. convert_nv21_to_rgb(src, g_fb_buf, width, height, 0);
  479. break;
  480. case V4L2_PIX_FMT_RGB24:
  481. if (jpegFilename) {
  482. // sprintf(filename, "%d-rgb-%s", count, jpegFilename);
  483. // RGB565toRGB888(width, height, src, dst);
  484. // write_JPEG_file(filename, src, width, height, jpegQuality);
  485. sprintf(filename, "raw-%d-rgb-%s", count, jpegFilename);
  486. write_file(filename, src, g_imagesize);
  487. count++;
  488. }
  489. convert_rgb888_to_rgb(src, g_fb_buf, width, height, 0);
  490. break;
  491. case V4L2_PIX_FMT_RGB565:
  492. if (jpegFilename) {
  493. // sprintf(filename, "%d-rgb565-%s", count, jpegFilename);
  494. // RGB565toRGB888(width, height, src, dst);
  495. // write_JPEG_file(filename, dst, width, height, jpegQuality);
  496. sprintf(filename, "raw-%d-rgb565-%s", count, jpegFilename);
  497. write_file(filename, src, g_imagesize);
  498. count++;
  499. }
  500. if (pixformat == v4l2fmt_to_fbfmt(format))
  501. convert_rgb565_to_rgb(src, g_fb_buf, width, height, 0);
  502. else if (g_vinfo.grayscale)
  503. convert_rgb565_to_nv12(src, g_fb_buf, width, height, 0);
  504. else
  505. convert_rgb565_to_rgb(src, g_fb_buf, width, height, 0);
  506. break;
  507. case V4L2_PIX_FMT_SRGGB12:
  508. if (jpegFilename)
  509. sprintf(filename, "raw-%d-RGGB12-%s", count, jpegFilename);
  510. else
  511. sprintf(filename, "raw-%d-RGGB12.raw", count);
  512. write_file(filename, src, g_imagesize);
  513. RAW12toRAW16(width, height, src, dst);
  514. if (jpegFilename)
  515. sprintf(filename, "raw-%d-RGGB16-%s", count, jpegFilename);
  516. else
  517. sprintf(filename, "raw-%d-RGGB16.raw", count);
  518. write_file(filename, dst, width * height * 2);
  519. count++;
  520. break;
  521. case V4L2_PIX_FMT_SGRBG12:
  522. if (jpegFilename)
  523. sprintf(filename, "raw-%d-GRBG12-%s", count, jpegFilename);
  524. else
  525. sprintf(filename, "raw-%d-GRBG12.raw", count);
  526. write_file(filename, src, g_imagesize);
  527. RAW12toRAW16(width, height, src, dst);
  528. if (jpegFilename)
  529. sprintf(filename, "raw-%d-GRBG16-%s", count, jpegFilename);
  530. else
  531. sprintf(filename, "raw-%d-GRBG16.raw", count);
  532. write_file(filename, dst, width * height * 2);
  533. count++;
  534. break;
  535. case V4L2_PIX_FMT_SGBRG12:
  536. if (jpegFilename)
  537. sprintf(filename, "raw-%d-GBRG12-%s", count, jpegFilename);
  538. else
  539. sprintf(filename, "raw-%d-GBRG12.raw", count);
  540. write_file(filename, src, g_imagesize);
  541. RAW12toRAW16(width, height, src, dst);
  542. if (jpegFilename)
  543. sprintf(filename, "raw-%d-GBRG16-%s", count, jpegFilename);
  544. else
  545. sprintf(filename, "raw-%d-GBRG16.raw", count);
  546. write_file(filename, dst, width * height * 2);
  547. count++;
  548. break;
  549. case V4L2_PIX_FMT_SBGGR12:
  550. if (jpegFilename)
  551. sprintf(filename, "raw-%d-BGGR12-%s", count, jpegFilename);
  552. else
  553. sprintf(filename, "raw-%d-BGGR12.raw", count);
  554. write_file(filename, src, g_imagesize);
  555. RAW12toRAW16(width, height, src, dst);
  556. if (jpegFilename)
  557. sprintf(filename, "raw-%d-BGGR16-%s", count, jpegFilename);
  558. else
  559. sprintf(filename, "raw-%d-BGGR16.raw", count);
  560. write_file(filename, dst, width * height * 2);
  561. count++;
  562. break;
  563. default:
  564. printf("unknow format\n");
  565. break;
  566. }
  567. // free temporary image
  568. free(dst);
  569. }
  570. /**
  571. read single frame
  572. */
  573. static int frameRead(void)
  574. {
  575. struct v4l2_buffer buf;
  576. #ifdef IO_USERPTR
  577. unsigned int i;
  578. #endif
  579. switch (g_io_mthd) {
  580. #ifdef IO_READ
  581. case IO_METHOD_READ:
  582. if (-1 == v4l2_read(g_fd, g_buffers[0].start, g_buffers[0].length)) {
  583. switch (errno) {
  584. case EAGAIN:
  585. return 0;
  586. case EIO:
  587. // Could ignore EIO, see spec.
  588. // fall through
  589. default:
  590. errno_exit("read");
  591. }
  592. }
  593. struct timespec ts;
  594. struct timeval timestamp;
  595. clock_gettime(CLOCK_MONOTONIC,&ts);
  596. timestamp.tv_sec = ts.tv_sec;
  597. timestamp.tv_usec = ts.tv_nsec/1000;
  598. imageProcess(g_buffers[0].start,timestamp);
  599. break;
  600. #endif
  601. #ifdef IO_MMAP
  602. case IO_METHOD_MMAP:
  603. CLEAR(buf);
  604. buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  605. buf.memory = V4L2_MEMORY_MMAP;
  606. if (-1 == xioctl(g_fd, VIDIOC_DQBUF, &buf)) {
  607. switch (errno) {
  608. case EAGAIN:
  609. return 0;
  610. case EIO:
  611. // Could ignore EIO, see spec
  612. // fall through
  613. default:
  614. errno_exit("VIDIOC_DQBUF");
  615. }
  616. }
  617. assert(buf.index < n_buffers);
  618. imageProcess(g_buffers[buf.index].start, buf.timestamp);
  619. if (-1 == xioctl(g_fd, VIDIOC_QBUF, &buf))
  620. errno_exit("VIDIOC_QBUF");
  621. break;
  622. #endif
  623. #ifdef IO_USERPTR
  624. case IO_METHOD_USERPTR:
  625. CLEAR (buf);
  626. buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  627. buf.memory = V4L2_MEMORY_USERPTR;
  628. if (-1 == xioctl(g_fd, VIDIOC_DQBUF, &buf)) {
  629. switch (errno) {
  630. case EAGAIN:
  631. return 0;
  632. case EIO:
  633. // Could ignore EIO, see spec.
  634. // fall through
  635. default:
  636. errno_exit("VIDIOC_DQBUF");
  637. }
  638. }
  639. for (i = 0; i < n_buffers; ++i) {
  640. if (buf.m.userptr == (unsigned long)g_buffers[i].start
  641. && buf.length == g_buffers[i].length) {
  642. break;
  643. }
  644. }
  645. assert (i < n_buffers);
  646. imageProcess((void *)buf.m.userptr, buf.timestamp);
  647. if (-1 == xioctl(g_fd, VIDIOC_QBUF, &buf))
  648. errno_exit("VIDIOC_QBUF");
  649. break;
  650. #endif
  651. }
  652. static unsigned int start_timems;
  653. unsigned int time_ms, tmp_ms;
  654. struct timespec ts;
  655. clock_gettime(CLOCK_MONOTONIC, &ts);
  656. tmp_ms = ts.tv_sec * 1000 + ts.tv_nsec/1000000;
  657. time_ms = tmp_ms - start_timems;
  658. start_timems = tmp_ms;
  659. if (!(++fps_count%5)) {
  660. fps_count = 0;
  661. printf("%s format = 0x%x\n", __func__, format);
  662. printf("fps: %d\n", 1000/time_ms);
  663. }
  664. return 1;
  665. }
  666. /**
  667. mainloop: read frames and process them
  668. */
  669. static void mainLoop(void)
  670. {
  671. int count, i;
  672. unsigned int numberOfTimeouts;
  673. numberOfTimeouts = 0;
  674. count = 3;
  675. while (count-- > 0) {
  676. for (i = 0; i < 1; i++) {
  677. fd_set fds;
  678. struct timeval tv;
  679. int r;
  680. FD_ZERO(&fds);
  681. FD_SET(g_fd, &fds);
  682. /* Timeout. */
  683. tv.tv_sec = 1;
  684. tv.tv_usec = 0;
  685. r = select(g_fd + 1, &fds, NULL, NULL, &tv);
  686. if (-1 == r) {
  687. if (EINTR == errno)
  688. continue;
  689. errno_exit("select");
  690. }
  691. if (0 == r) {
  692. if (numberOfTimeouts <= 0) {
  693. // count++;
  694. } else {
  695. fprintf(stderr, "select timeout\n");
  696. exit(EXIT_FAILURE);
  697. }
  698. }
  699. if(continuous == 1) {
  700. count = 3;
  701. }
  702. if (frameRead())
  703. break;
  704. /* EAGAIN - continue select loop. */
  705. }
  706. }
  707. }
  708. /**
  709. stop capturing
  710. */
  711. static void captureStop(void)
  712. {
  713. enum v4l2_buf_type type;
  714. switch (g_io_mthd) {
  715. #ifdef IO_READ
  716. case IO_METHOD_READ:
  717. /* Nothing to do. */
  718. break;
  719. #endif
  720. #ifdef IO_MMAP
  721. case IO_METHOD_MMAP:
  722. #endif
  723. #ifdef IO_USERPTR
  724. case IO_METHOD_USERPTR:
  725. #endif
  726. #if defined(IO_MMAP) || defined(IO_USERPTR)
  727. type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  728. if (-1 == xioctl(g_fd, VIDIOC_STREAMOFF, &type))
  729. errno_exit("VIDIOC_STREAMOFF");
  730. break;
  731. #endif
  732. }
  733. }
  734. /**
  735. start capturing
  736. */
  737. static void captureStart(void)
  738. {
  739. unsigned int i;
  740. enum v4l2_buf_type type;
  741. switch (g_io_mthd) {
  742. #ifdef IO_READ
  743. case IO_METHOD_READ:
  744. /* Nothing to do. */
  745. break;
  746. #endif
  747. #ifdef IO_MMAP
  748. case IO_METHOD_MMAP:
  749. for (i = 0; i < n_buffers; ++i) {
  750. struct v4l2_buffer buf;
  751. CLEAR(buf);
  752. buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  753. buf.memory = V4L2_MEMORY_MMAP;
  754. buf.index = i;
  755. if (-1 == xioctl(g_fd, VIDIOC_QBUF, &buf))
  756. errno_exit("VIDIOC_QBUF");
  757. }
  758. type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  759. if (-1 == xioctl(g_fd, VIDIOC_STREAMON, &type))
  760. errno_exit("VIDIOC_STREAMON");
  761. break;
  762. #endif
  763. #ifdef IO_USERPTR
  764. case IO_METHOD_USERPTR:
  765. for (i = 0; i < n_buffers; ++i) {
  766. struct v4l2_buffer buf;
  767. CLEAR (buf);
  768. buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  769. buf.memory = V4L2_MEMORY_USERPTR;
  770. buf.index = i;
  771. buf.m.userptr = (unsigned long) g_buffers[i].start;
  772. buf.length = g_buffers[i].length;
  773. if (-1 == xioctl(g_fd, VIDIOC_QBUF, &buf))
  774. errno_exit("VIDIOC_QBUF");
  775. }
  776. type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  777. if (-1 == xioctl(g_fd, VIDIOC_STREAMON, &type))
  778. errno_exit("VIDIOC_STREAMON");
  779. break;
  780. #endif
  781. }
  782. }
  783. static void deviceUninit(void)
  784. {
  785. unsigned int i;
  786. switch (g_io_mthd) {
  787. #ifdef IO_READ
  788. case IO_METHOD_READ:
  789. free(g_buffers[0].start);
  790. break;
  791. #endif
  792. #ifdef IO_MMAP
  793. case IO_METHOD_MMAP:
  794. for (i = 0; i < n_buffers; ++i)
  795. if (-1 == v4l2_munmap(g_buffers[i].start, g_buffers[i].length))
  796. errno_exit("munmap");
  797. break;
  798. #endif
  799. #ifdef IO_USERPTR
  800. case IO_METHOD_USERPTR:
  801. for (i = 0; i < n_buffers; ++i)
  802. free(g_buffers[i].start);
  803. break;
  804. #endif
  805. }
  806. free(g_buffers);
  807. if (!test_fps) {
  808. if (-1 == munmap((void *)g_fb_buf, g_screensize)) {
  809. printf(" Error: framebuffer device munmap() failed.\n");
  810. exit (EXIT_FAILURE) ;
  811. }
  812. }
  813. }
  814. #ifdef IO_READ
  815. static void readInit(unsigned int buffer_size)
  816. {
  817. g_buffers = calloc(1, sizeof(*g_buffers));
  818. if (!g_buffers) {
  819. fprintf(stderr, "Out of memory\n");
  820. exit(EXIT_FAILURE);
  821. }
  822. g_buffers[0].length = buffer_size;
  823. g_buffers[0].start = malloc(buffer_size);
  824. if (!g_buffers[0].start) {
  825. fprintf (stderr, "Out of memory\n");
  826. exit(EXIT_FAILURE);
  827. }
  828. }
  829. #endif
  830. #ifdef IO_MMAP
  831. static void mmapInit(void)
  832. {
  833. struct v4l2_requestbuffers req;
  834. CLEAR(req);
  835. req.count = VIDIOC_REQBUFS_COUNT;
  836. req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  837. req.memory = V4L2_MEMORY_MMAP;
  838. if (-1 == xioctl(g_fd, VIDIOC_REQBUFS, &req)) {
  839. if (EINVAL == errno) {
  840. fprintf(stderr, "%s does not support memory mapping\n", deviceName);
  841. exit(EXIT_FAILURE);
  842. } else {
  843. errno_exit("VIDIOC_REQBUFS");
  844. }
  845. }
  846. if (req.count < 2) {
  847. fprintf(stderr, "Insufficient buffer memory on %s\n", deviceName);
  848. exit(EXIT_FAILURE);
  849. }
  850. g_buffers = calloc(req.count, sizeof(*g_buffers));
  851. if (!g_buffers) {
  852. fprintf(stderr, "Out of memory\n");
  853. exit(EXIT_FAILURE);
  854. }
  855. for (n_buffers = 0; n_buffers < req.count; ++n_buffers) {
  856. struct v4l2_buffer buf;
  857. CLEAR(buf);
  858. buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  859. buf.memory = V4L2_MEMORY_MMAP;
  860. buf.index = n_buffers;
  861. if (-1 == xioctl(g_fd, VIDIOC_QUERYBUF, &buf))
  862. errno_exit("VIDIOC_QUERYBUF");
  863. g_buffers[n_buffers].length = buf.length;
  864. g_buffers[n_buffers].start = v4l2_mmap(NULL, /* start anywhere */
  865. buf.length, PROT_READ | PROT_WRITE, /* required */
  866. MAP_SHARED, /* recommended */
  867. g_fd, buf.m.offset);
  868. if (MAP_FAILED == g_buffers[n_buffers].start)
  869. errno_exit("mmap");
  870. g_imagesize = buf.length;
  871. }
  872. }
  873. #endif
  874. #ifdef IO_USERPTR
  875. static void userptrInit(unsigned int buffer_size)
  876. {
  877. struct v4l2_requestbuffers req;
  878. unsigned int page_size;
  879. page_size = getpagesize();
  880. buffer_size = (buffer_size + page_size - 1) & ~(page_size - 1);
  881. CLEAR(req);
  882. req.count = VIDIOC_REQBUFS_COUNT;
  883. req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  884. req.memory = V4L2_MEMORY_USERPTR;
  885. if (-1 == xioctl(g_fd, VIDIOC_REQBUFS, &req)) {
  886. if (EINVAL == errno) {
  887. fprintf(stderr, "%s does not support user pointer i/o\n", deviceName);
  888. exit(EXIT_FAILURE);
  889. } else {
  890. errno_exit("VIDIOC_REQBUFS");
  891. }
  892. }
  893. g_buffers = calloc(4, sizeof(*g_buffers));
  894. if (!g_buffers) {
  895. fprintf(stderr, "Out of memory\n");
  896. exit(EXIT_FAILURE);
  897. }
  898. for (n_buffers = 0; n_buffers < 4; ++n_buffers) {
  899. g_buffers[n_buffers].length = buffer_size;
  900. g_buffers[n_buffers].start = memalign(/* boundary */ page_size, buffer_size);
  901. if (!g_buffers[n_buffers].start) {
  902. fprintf(stderr, "Out of memory\n");
  903. exit(EXIT_FAILURE);
  904. }
  905. }
  906. }
  907. #endif
  908. /**
  909. initialize device
  910. */
  911. static void deviceInit(void)
  912. {
  913. struct v4l2_capability cap;
  914. struct v4l2_cropcap cropcap;
  915. struct v4l2_crop crop;
  916. struct v4l2_format fmt;
  917. struct v4l2_streamparm frameint;
  918. //struct v4l2_streamparm frameget;
  919. unsigned int min;
  920. if (-1 == xioctl(g_fd, VIDIOC_QUERYCAP, &cap)) {
  921. if (EINVAL == errno) {
  922. fprintf(stderr, "%s is no V4L2 device\n",deviceName);
  923. exit(EXIT_FAILURE);
  924. } else {
  925. errno_exit("VIDIOC_QUERYCAP");
  926. }
  927. }
  928. if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
  929. fprintf(stderr, "%s is no video capture device\n",deviceName);
  930. exit(EXIT_FAILURE);
  931. }
  932. switch (g_io_mthd) {
  933. #ifdef IO_READ
  934. case IO_METHOD_READ:
  935. if (!(cap.capabilities & V4L2_CAP_READWRITE)) {
  936. fprintf(stderr, "%s does not support read i/o\n",deviceName);
  937. exit(EXIT_FAILURE);
  938. }
  939. break;
  940. #endif
  941. #ifdef IO_MMAP
  942. case IO_METHOD_MMAP:
  943. #endif
  944. #ifdef IO_USERPTR
  945. case IO_METHOD_USERPTR:
  946. #endif
  947. #if defined(IO_MMAP) || defined(IO_USERPTR)
  948. if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
  949. fprintf(stderr, "%s does not support streaming i/o\n",deviceName);
  950. exit(EXIT_FAILURE);
  951. }
  952. break;
  953. #endif
  954. }
  955. /* Select video input, video standard and tune here. */
  956. CLEAR(cropcap);
  957. cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  958. if (0 == xioctl(g_fd, VIDIOC_CROPCAP, &cropcap)) {
  959. crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  960. crop.c = cropcap.defrect; /* reset to default */
  961. if (-1 == xioctl(g_fd, VIDIOC_S_CROP, &crop)) {
  962. switch (errno) {
  963. case EINVAL:
  964. /* Cropping not supported. */
  965. break;
  966. default:
  967. /* Errors ignored. */
  968. break;
  969. }
  970. }
  971. } else {
  972. /* Errors ignored. */
  973. }
  974. CLEAR(fmt);
  975. // v4l2_format
  976. fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  977. fmt.fmt.pix.width = width;
  978. fmt.fmt.pix.height = height;
  979. fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
  980. fmt.fmt.pix.pixelformat = format;
  981. /* If the user has set the fps to -1, don't try to set the frame interval */
  982. if (fps != -1)
  983. {
  984. CLEAR(frameint);
  985. /* Attempt to set the frame interval. */
  986. frameint.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  987. frameint.parm.capture.timeperframe.numerator = 1;
  988. frameint.parm.capture.timeperframe.denominator = fps;
  989. if (-1 == xioctl(g_fd, VIDIOC_S_PARM, &frameint))
  990. fprintf(stderr,"Unable to set frame interval.\n");
  991. fprintf(stderr,"set frame interval = %d.\n", frameint.parm.capture.timeperframe.denominator);
  992. }
  993. if (-1 == xioctl(g_fd, VIDIOC_S_FMT, &fmt))
  994. errno_exit("VIDIOC_S_FMT");
  995. if (crop_flag) {
  996. struct v4l2_selection sel_crop = {
  997. V4L2_BUF_TYPE_VIDEO_CAPTURE,
  998. V4L2_SEL_TGT_CROP,
  999. 0,
  1000. { left, up, right, down }
  1001. };
  1002. struct v4l2_selection get_crop = {
  1003. V4L2_BUF_TYPE_VIDEO_CAPTURE,
  1004. V4L2_SEL_TGT_CROP,
  1005. };
  1006. fprintf(stderr, "sel_crop.left = %d, %d, %d, %d\n",
  1007. sel_crop.r.left, sel_crop.r.top, sel_crop.r.width, sel_crop.r.height);
  1008. if (-1 == xioctl(g_fd, VIDIOC_S_SELECTION, &sel_crop)) {
  1009. fprintf(stderr,"S_SELECTION Failed.\n");
  1010. }
  1011. fprintf(stderr, "sel_crop.left = %d, %d, %d, %d\n",
  1012. sel_crop.r.left, sel_crop.r.top, sel_crop.r.width, sel_crop.r.height);
  1013. if (-1 == xioctl(g_fd, VIDIOC_G_SELECTION, &get_crop)) {
  1014. fprintf(stderr,"G_SELECTION Failed.\n");
  1015. }
  1016. fprintf(stderr, "get_crop.left = %d, %d, %d, %d\n",
  1017. get_crop.r.left, get_crop.r.top, get_crop.r.width, get_crop.r.height);
  1018. if (memcmp(&sel_crop, &get_crop, sizeof(sel_crop))) {
  1019. fprintf(stderr,"set/get selection diff.\n");
  1020. }
  1021. memset(&fmt, 0, sizeof(struct v4l2_format));
  1022. fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  1023. fmt.fmt.pix.field = V4L2_FIELD_ANY;
  1024. if (-1 == ioctl(g_fd, VIDIOC_G_FMT, &fmt))
  1025. errno_exit("VIDIOC_G_FMT");
  1026. }
  1027. //if (fmt.fmt.pix.pixelformat != V4L2_PIX_FMT_YUV420) {
  1028. //if (fmt.fmt.pix.pixelformat != V4L2_PIX_FMT_RGB565) {
  1029. if (fmt.fmt.pix.pixelformat != format) {
  1030. fprintf(stderr,"Libv4l didn't accept format. Can't proceed.\n");
  1031. exit(EXIT_FAILURE);
  1032. }
  1033. /* Note VIDIOC_S_FMT may change width and height. */
  1034. if (width != fmt.fmt.pix.width) {
  1035. width = fmt.fmt.pix.width;
  1036. fprintf(stderr,"Image width set to %i by device %s.\n", width, deviceName);
  1037. }
  1038. if (height != fmt.fmt.pix.height) {
  1039. height = fmt.fmt.pix.height;
  1040. fprintf(stderr,"Image height set to %i by device %s.\n", height, deviceName);
  1041. }
  1042. /* Buggy driver paranoia. */
  1043. min = fmt.fmt.pix.width * 2;
  1044. if (fmt.fmt.pix.bytesperline < min)
  1045. fmt.fmt.pix.bytesperline = min;
  1046. min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
  1047. if (fmt.fmt.pix.sizeimage < min)
  1048. fmt.fmt.pix.sizeimage = min;
  1049. switch (g_io_mthd) {
  1050. #ifdef IO_READ
  1051. case IO_METHOD_READ:
  1052. readInit(fmt.fmt.pix.sizeimage);
  1053. break;
  1054. #endif
  1055. #ifdef IO_MMAP
  1056. case IO_METHOD_MMAP:
  1057. mmapInit();
  1058. break;
  1059. #endif
  1060. #ifdef IO_USERPTR
  1061. case IO_METHOD_USERPTR:
  1062. userptrInit(fmt.fmt.pix.sizeimage);
  1063. break;
  1064. #endif
  1065. }
  1066. if (!test_fps) {
  1067. struct pp_mode pp_info[3];
  1068. pixformat = v4l2fmt_to_fbfmt(format);
  1069. if (-1 == ioctl(g_stfbc_fd, FBIOPAN_GET_PP_MODE, &pp_info[0])) {
  1070. printf("Error reading variable information.\n");
  1071. exit (EXIT_FAILURE);
  1072. }
  1073. printf("get pp format :%d, %d\n", pp_info[1].src.format, __LINE__);
  1074. pp_info[1].src.format = pixformat;
  1075. if (-1 == ioctl(g_stfbc_fd, FBIOPAN_SET_PP_MODE, &pp_info[0])) {
  1076. printf("Error reading variable information.\n");
  1077. exit (EXIT_FAILURE);
  1078. }
  1079. if (-1 == ioctl(g_stfbc_fd, FBIOPAN_GET_PP_MODE, &pp_info[0])) {
  1080. printf("Error reading variable information.\n");
  1081. exit (EXIT_FAILURE);
  1082. }
  1083. printf("get pp format :%d, %d\n", pp_info[1].src.format, __LINE__);
  1084. pixformat = pp_info[1].src.format;
  1085. // Get fixed screen information
  1086. if (-1 == xioctl(g_fb_fd, FBIOGET_FSCREENINFO, &g_finfo)) {
  1087. printf("Error reading fixed information.\n");
  1088. exit (EXIT_FAILURE);
  1089. }
  1090. // Get variable screen information
  1091. if (-1 == xioctl(g_fb_fd, FBIOGET_VSCREENINFO, &g_vinfo)) {
  1092. printf("Error reading variable information.\n");
  1093. exit (EXIT_FAILURE);
  1094. }
  1095. printf("g_vinfo.xres = %d, g_vinfo.yres = %d, grayscale = %d\n", g_vinfo.xres, g_vinfo.yres, g_vinfo.grayscale);
  1096. printf("g_vinfo.xoffset = %d, g_vinfo.yoffset = %d\n", g_vinfo.xoffset, g_vinfo.yoffset);
  1097. printf("g_vinfo.bits_per_pixel = %d, g_finfo.line_length = %d\n", g_vinfo.bits_per_pixel, g_finfo.line_length);
  1098. // if (ioctl(g_fb_fd, FBIOPUT_VSCREENINFO, &g_vinfo) < 0) {
  1099. // printf("FBIOPUT_VSCREENINFO.\n");
  1100. // exit (EXIT_FAILURE);
  1101. // }
  1102. // printf("g_vinfo.bits_per_pixel = %d, g_finfo.line_length = %d\n", g_vinfo.bits_per_pixel, g_finfo.line_length);
  1103. g_screensize = g_vinfo.xres * g_vinfo.yres * g_vinfo.bits_per_pixel / 8;
  1104. // g_screensize = g_vinfo.xres * g_vinfo.yres * 32 / 8;
  1105. //mmap framebuffer
  1106. g_fb_buf = (unsigned char *)mmap(NULL, g_screensize, PROT_READ | PROT_WRITE, MAP_SHARED, g_fb_fd, 0);
  1107. if (g_fb_buf == (void *)(-1)) {
  1108. printf("Error: failed to map framebuffer device to memory.\n");
  1109. exit (EXIT_FAILURE) ;
  1110. }
  1111. memset(g_fb_buf, 0x00, g_screensize);
  1112. }
  1113. }
  1114. /**
  1115. close device
  1116. */
  1117. static void deviceClose(void)
  1118. {
  1119. if (-1 == v4l2_close(g_fd))
  1120. errno_exit("close");
  1121. g_fd = -1;
  1122. if (!test_fps) {
  1123. close(g_fb_fd);
  1124. close(g_stfbc_fd);
  1125. }
  1126. }
  1127. /**
  1128. open device
  1129. */
  1130. static void deviceOpen(void)
  1131. {
  1132. struct stat st;
  1133. //struct v4l2_capability cap;
  1134. // stat file
  1135. if (-1 == stat(deviceName, &st)) {
  1136. fprintf(stderr, "Cannot identify '%s': %d, %s\n", deviceName, errno, strerror(errno));
  1137. exit(EXIT_FAILURE);
  1138. }
  1139. // check if its device
  1140. if (!S_ISCHR(st.st_mode)) {
  1141. fprintf(stderr, "%s is no device\n", deviceName);
  1142. exit(EXIT_FAILURE);
  1143. }
  1144. // open device
  1145. g_fd = v4l2_open(deviceName, O_RDWR /* required */ | O_NONBLOCK, 0);
  1146. // g_fd = v4l2_open(deviceName, O_RDWR, 0);
  1147. // check if opening was successfull
  1148. if (-1 == g_fd) {
  1149. fprintf(stderr, "Cannot open '%s': %d, %s\n", deviceName, errno, strerror(errno));
  1150. exit(EXIT_FAILURE);
  1151. }
  1152. //open framebuffer
  1153. if (!test_fps) {
  1154. g_fb_fd = open("/dev/fb0", O_RDWR);
  1155. if (g_fb_fd == -1) {
  1156. printf("Error: cannot open framebuffer device.\n");
  1157. exit (EXIT_FAILURE);
  1158. }
  1159. g_stfbc_fd = open("/dev/stfbcdev", O_RDWR);
  1160. if (g_stfbc_fd == -1) {
  1161. printf("Error: cannot open stfbcdev device.\n");
  1162. exit (EXIT_FAILURE);
  1163. }
  1164. }
  1165. }
  1166. void loadfw_start(char *filename)
  1167. {
  1168. struct stfisp_fw_info fw_info = {0};
  1169. // open device
  1170. g_fd = open(deviceName, O_RDWR /* required */ | O_NONBLOCK, 0);
  1171. // check if opening was successfull
  1172. if (-1 == g_fd) {
  1173. fprintf(stderr, "Cannot open '%s': %d, %s\n", deviceName, errno, strerror(errno));
  1174. exit(EXIT_FAILURE);
  1175. }
  1176. if (filename && (strlen(filename) < FILENAME_MAX_LEN))
  1177. memcpy(fw_info.filename, filename, strlen(filename) + 1);
  1178. fprintf(stderr, "VIDIOC_STFISP_LOAD_FW = 0x%lx, filename = %s, size = %lu\n",
  1179. VIDIOC_STFISP_LOAD_FW, fw_info.filename, sizeof(struct stfisp_fw_info));
  1180. if (-1 == ioctl(g_fd, VIDIOC_STFISP_LOAD_FW, &fw_info)) {
  1181. if (EINVAL == errno) {
  1182. fprintf(stderr, "%s is no V4L2 device\n",deviceName);
  1183. exit(EXIT_FAILURE);
  1184. } else {
  1185. errno_exit("VIDIOC_STFISP_LOAD_FW");
  1186. }
  1187. }
  1188. }
  1189. void sensor_image_size_info(void)
  1190. {
  1191. unsigned int i = 0;
  1192. fprintf(stderr, "go in sensor_image_size_info....\n");
  1193. // open device
  1194. g_fd = open(deviceName, O_RDWR /* required */ | O_NONBLOCK, 0);
  1195. // check if opening was successfull
  1196. if (-1 == g_fd) {
  1197. fprintf(stderr, "Cannot open '%s': %d, %s\n", deviceName, errno, strerror(errno));
  1198. exit(EXIT_FAILURE);
  1199. }
  1200. for (i = 0; i < 4; i++)
  1201. {
  1202. frame_size.index = i;
  1203. frame_size.code = MEDIA_BUS_FMT_SRGGB10_1X10;
  1204. if (-1 == ioctl(g_fd, VIDIOC_SUBDEV_ENUM_FRAME_SIZE, &frame_size)) {
  1205. errno_exit("VIDIOC_SIZE_INFO");
  1206. }
  1207. printf("image_size: width[%d] = %d, height[%d] = %d \n",
  1208. i, frame_size.min_width, i, frame_size.min_height);
  1209. }
  1210. }
  1211. /**
  1212. print usage information
  1213. */
  1214. static void usage(FILE* fp, int argc, char** argv)
  1215. {
  1216. fprintf(fp,
  1217. "Usage: %s [options]\n\n"
  1218. "Options:\n"
  1219. "-d | --device name Video device name [/dev/video0]\n"
  1220. "-h | --help Print this message\n"
  1221. "-o | --output Set JPEG output filename\n"
  1222. "-q | --quality Set JPEG quality (0-100)\n"
  1223. "-m | --mmap Use memory mapped buffer\n"
  1224. "-r | --read Use read() calls\n"
  1225. "-u | --userptr Use application allocated buffer\n"
  1226. "-W | --width Set image width\n"
  1227. "-H | --height Set image height\n"
  1228. "-X | --left Set image x start\n"
  1229. "-Y | --up Set image y start\n"
  1230. "-R | --right Set image x width\n"
  1231. "-D | --down Set image y height\n"
  1232. "-I | --interval Set frame interval (fps) (-1 to skip)\n"
  1233. "-c | --continuous Do continous capture, stop with SIGINT.\n"
  1234. "-v | --version Print version\n"
  1235. "-f | --format image format\n"
  1236. " 0:V4L2_PIX_FMT_RGB565\n"
  1237. " 1:V4L2_PIX_FMT_RGB24\n"
  1238. " 2:V4L2_PIX_FMT_YUV420\n"
  1239. " 3:V4L2_PIX_FMT_YUYV\n"
  1240. " 4:V4L2_PIX_FMT_NV21\n"
  1241. " 5:V4L2_PIX_FMT_NV12\n"
  1242. " 6:V4L2_PIX_FMT_YVYU\n"
  1243. " 7:V4L2_PIX_FMT_SRGGB12\n"
  1244. " 8:V4L2_PIX_FMT_SGRBG12\n"
  1245. " 9:V4L2_PIX_FMT_SGBRG12\n"
  1246. " 10:V4L2_PIX_FMT_SBGGR12\n"
  1247. " default:V4L2_PIX_FMT_RGB565\n"
  1248. "-t | --testfps test fps\n"
  1249. "-l | --loadfw load stfisp fw image\n"
  1250. "-s | --g_imagesize print image size\n"
  1251. "",
  1252. argv[0]);
  1253. }
  1254. static const char short_options [] = "d:ho:q:mruW:H:I:vcf:tX:Y:R:D:l:s";
  1255. static const struct option
  1256. long_options [] = {
  1257. { "device", required_argument, NULL, 'd' },
  1258. { "help", no_argument, NULL, 'h' },
  1259. { "output", required_argument, NULL, 'o' },
  1260. { "quality", required_argument, NULL, 'q' },
  1261. { "mmap", no_argument, NULL, 'm' },
  1262. { "read", no_argument, NULL, 'r' },
  1263. { "userptr", no_argument, NULL, 'u' },
  1264. { "width", required_argument, NULL, 'W' },
  1265. { "height", required_argument, NULL, 'H' },
  1266. { "left", required_argument, NULL, 'X' },
  1267. { "up", required_argument, NULL, 'Y' },
  1268. { "right", required_argument, NULL, 'R' },
  1269. { "down", required_argument, NULL, 'D' },
  1270. { "interval", required_argument, NULL, 'I' },
  1271. { "version", no_argument, NULL, 'v' },
  1272. { "continuous", no_argument, NULL, 'c' },
  1273. { "format", required_argument, NULL, 'f' },
  1274. { "testfps", no_argument, NULL, 't' },
  1275. { "loadfw", required_argument, NULL, 'l' },
  1276. { "g_imagesize", no_argument, NULL, 's' },
  1277. { 0, 0, 0, 0 }
  1278. };
  1279. int main(int argc, char **argv)
  1280. {
  1281. for (;;) {
  1282. int index, c = 0;
  1283. c = getopt_long(argc, argv, short_options, long_options, &index);
  1284. if (-1 == c)
  1285. break;
  1286. switch (c) {
  1287. case 0: /* getopt_long() flag */
  1288. break;
  1289. case 'd':
  1290. deviceName = optarg;
  1291. break;
  1292. case 'h':
  1293. // print help
  1294. usage(stdout, argc, argv);
  1295. exit(EXIT_SUCCESS);
  1296. case 'o':
  1297. // set jpeg filename
  1298. jpegFilename = optarg;
  1299. break;
  1300. case 'q':
  1301. // set jpeg quality
  1302. jpegQuality = atoi(optarg);
  1303. break;
  1304. case 'm':
  1305. #ifdef IO_MMAP
  1306. g_io_mthd = IO_METHOD_MMAP;
  1307. #else
  1308. fprintf(stderr, "You didn't compile for mmap support.\n");
  1309. exit(EXIT_FAILURE);
  1310. #endif
  1311. break;
  1312. case 'r':
  1313. #ifdef IO_READ
  1314. g_io_mthd = IO_METHOD_READ;
  1315. #else
  1316. fprintf(stderr, "You didn't compile for read support.\n");
  1317. exit(EXIT_FAILURE);
  1318. #endif
  1319. break;
  1320. case 'u':
  1321. #ifdef IO_USERPTR
  1322. g_io_mthd = IO_METHOD_USERPTR;
  1323. #else
  1324. fprintf(stderr, "You didn't compile for userptr support.\n");
  1325. exit(EXIT_FAILURE);
  1326. #endif
  1327. break;
  1328. case 'W':
  1329. // set width
  1330. width = atoi(optarg);
  1331. break;
  1332. case 'H':
  1333. // set height
  1334. height = atoi(optarg);
  1335. break;
  1336. case 'X':
  1337. // set x start
  1338. left = atoi(optarg);
  1339. crop_flag = 1;
  1340. break;
  1341. case 'Y':
  1342. // set y start
  1343. up = atoi(optarg);
  1344. crop_flag = 1;
  1345. break;
  1346. case 'R':
  1347. // set x width
  1348. right = atoi(optarg);
  1349. crop_flag = 1;
  1350. break;
  1351. case 'D':
  1352. // set y height
  1353. down = atoi(optarg);
  1354. crop_flag = 1;
  1355. break;
  1356. case 'I':
  1357. // set fps
  1358. fps = atoi(optarg);
  1359. break;
  1360. case 'c':
  1361. // set flag for continuous capture, interuptible by sigint
  1362. continuous = 1;
  1363. InstallSIGINTHandler();
  1364. break;
  1365. case 'v':
  1366. printf("Version: %s\n", VERSION);
  1367. exit(EXIT_SUCCESS);
  1368. break;
  1369. case 'f':
  1370. printf("format: %s\n", optarg);
  1371. format = atoi(optarg);
  1372. switch (format) {
  1373. case 0:
  1374. format = V4L2_PIX_FMT_RGB565;
  1375. break;
  1376. case 1:
  1377. format = V4L2_PIX_FMT_RGB24;
  1378. break;
  1379. case 2:
  1380. format = V4L2_PIX_FMT_YUV420;
  1381. break;
  1382. case 3:
  1383. format = V4L2_PIX_FMT_YUYV;
  1384. break;
  1385. case 4:
  1386. format = V4L2_PIX_FMT_NV21;
  1387. break;
  1388. case 5:
  1389. format = V4L2_PIX_FMT_NV12;
  1390. break;
  1391. case 6:
  1392. format = V4L2_PIX_FMT_YVYU;
  1393. break;
  1394. case 7:
  1395. format = V4L2_PIX_FMT_SRGGB12;
  1396. break;
  1397. case 8:
  1398. format = V4L2_PIX_FMT_SGRBG12;
  1399. break;
  1400. case 9:
  1401. format = V4L2_PIX_FMT_SGBRG12;
  1402. break;
  1403. case 10:
  1404. format = V4L2_PIX_FMT_SBGGR12;
  1405. break;
  1406. default:
  1407. format = V4L2_PIX_FMT_RGB565;
  1408. break;
  1409. }
  1410. break;
  1411. case 't':
  1412. test_fps = 1;
  1413. break;
  1414. case 'l':
  1415. loadfw_start(optarg);
  1416. exit(EXIT_SUCCESS);
  1417. break;
  1418. case 's':
  1419. sensor_image_size_info();
  1420. exit(EXIT_SUCCESS);
  1421. break;
  1422. default:
  1423. usage(stderr, argc, argv);
  1424. exit(EXIT_FAILURE);
  1425. }
  1426. }
  1427. // open and initialize device
  1428. deviceOpen();
  1429. deviceInit();
  1430. test_float_1();
  1431. // start capturing
  1432. captureStart();
  1433. // process frames
  1434. mainLoop();
  1435. // stop capturing
  1436. captureStop();
  1437. // close device
  1438. deviceUninit();
  1439. deviceClose();
  1440. exit(EXIT_SUCCESS);
  1441. return 0;
  1442. }