v4l2_test.c 33 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919
  1. // SPDX-License-Identifier: GPL-2.0
  2. /*
  3. * Copyright (C) 2021 StarFive Technology Co., Ltd.
  4. */
  5. #include <getopt.h>
  6. #include <signal.h>
  7. #include <inttypes.h>
  8. #include <stdbool.h>
  9. #include <libv4l2.h>
  10. #include <poll.h>
  11. #include "common.h"
  12. #include "yuv.h"
  13. #include "convert.h"
  14. #include "stf_v4l2.h"
  15. #include "stf_framebuffer.h"
  16. #include "stf_drm.h"
  17. #include "stf_log.h"
  18. #define FB_DEVICE_NAME "/dev/fb0"
  19. #define STFBC_DEVICE_NAME "/dev/stfbcdev"
  20. #define DRM_DEVICE_NAME "/dev/dri/card0"
  21. #define V4L2_DFT_DEVICE_NAME "/dev/video0"
  22. typedef struct enum_value_t {
  23. int value;
  24. const char *name;
  25. } enum_value_t;
  26. static const enum_value_t g_disp_values[] = {
  27. { STF_DISP_NONE, "NONE"},
  28. { STF_DISP_FB, "FB"},
  29. { STF_DISP_DRM, "DRM"}
  30. };
  31. static const enum_value_t g_iomthd_values[] = {
  32. { IO_METHOD_MMAP, "MMAP"},
  33. { IO_METHOD_USERPTR, "USERPTR"},
  34. { IO_METHOD_DMABUF, "DMABUF"},
  35. { IO_METHOD_READ, "READ"}
  36. };
  37. typedef struct {
  38. V4l2Param_t v4l2_param;
  39. FBParam_t fb_param;
  40. DRMParam_t drm_param;
  41. enum STF_DISP_TYPE disp_type;
  42. enum IOMethod io_mthd;
  43. int continuous;
  44. uint8_t jpegQuality;
  45. char* jpegFilename;
  46. FILE *rec_fp;
  47. int dmabufs[BUFCOUNT]; // for dmabuf use, mmap not use it
  48. } ConfigParam_t;
  49. ConfigParam_t *gp_cfg_param = NULL;
  50. static int g_drm_buf_next_idx = -1;
  51. static int g_drm_buf_curr_idx = 0;
  52. static void alloc_default_config(ConfigParam_t **pp_data)
  53. {
  54. ConfigParam_t *cfg_param = NULL;
  55. cfg_param = malloc(sizeof(*cfg_param));
  56. if (!cfg_param) {
  57. errno_exit("malloc");
  58. }
  59. memset(cfg_param, 0, sizeof(*cfg_param));
  60. cfg_param->disp_type = STF_DISP_NONE;
  61. cfg_param->continuous = 0;
  62. cfg_param->jpegQuality = 70;
  63. cfg_param->io_mthd = IO_METHOD_MMAP;
  64. cfg_param->v4l2_param.device_name = V4L2_DFT_DEVICE_NAME;
  65. cfg_param->v4l2_param.fd = -1;
  66. cfg_param->v4l2_param.io_mthd = cfg_param->io_mthd;
  67. cfg_param->v4l2_param.width = 1920;
  68. cfg_param->v4l2_param.height = 1080;
  69. cfg_param->v4l2_param.image_size = cfg_param->v4l2_param.width *
  70. cfg_param->v4l2_param.height * 3 / 2;
  71. cfg_param->v4l2_param.format = V4L2_PIX_FMT_NV12; // V4L2_PIX_FMT_RGB565
  72. cfg_param->v4l2_param.fps = 30;
  73. // the fb param will be updated after fb init
  74. cfg_param->fb_param.fd = -1;
  75. cfg_param->fb_param.pixformat = COLOR_YUV420_NV21; // COLOR_RGB565
  76. cfg_param->fb_param.width = 1920;
  77. cfg_param->fb_param.height = 1080;
  78. cfg_param->fb_param.bpp = 16;
  79. cfg_param->fb_param.screen_size = cfg_param->fb_param.width *
  80. cfg_param->fb_param.height * cfg_param->fb_param.bpp / 8;
  81. *pp_data = cfg_param;
  82. }
  83. static void check_cfg_params(ConfigParam_t *cfg_param)
  84. {
  85. LOG(STF_LEVEL_TRACE, "Enter\n");
  86. int disp_type = cfg_param->disp_type;
  87. int io_mthd = cfg_param->io_mthd;
  88. int ret = EXIT_FAILURE;
  89. assert(disp_type >= STF_DISP_NONE && disp_type <= STF_DISP_DRM);
  90. assert(io_mthd >= IO_METHOD_MMAP && io_mthd <= IO_METHOD_READ);
  91. // when mmap, support display NONE, DRM, FB
  92. if (IO_METHOD_MMAP == io_mthd) {
  93. ret = EXIT_SUCCESS;
  94. }
  95. // when dmabuf, only support DRM, and not save file
  96. if (IO_METHOD_DMABUF == io_mthd
  97. && STF_DISP_DRM == disp_type
  98. && !cfg_param->jpegFilename) {
  99. ret = EXIT_SUCCESS;
  100. }
  101. if (EXIT_FAILURE == ret) {
  102. LOG(STF_LEVEL_ERR, "Not support: io method is %s, display type is %s\n",
  103. g_iomthd_values[io_mthd].name, g_disp_values[disp_type].name);
  104. exit(EXIT_FAILURE);
  105. }
  106. LOG(STF_LEVEL_TRACE, "Exit\n");
  107. }
  108. /**
  109. SIGINT interput handler
  110. */
  111. void StopContCapture(int sig_id) {
  112. LOG(STF_LEVEL_INFO, "stoping continuous capture\n");
  113. gp_cfg_param->continuous = 0;
  114. }
  115. void InstallSIGINTHandler() {
  116. struct sigaction sa;
  117. CLEAR(sa);
  118. sa.sa_handler = StopContCapture;
  119. if (sigaction(SIGINT, &sa, 0) != 0) {
  120. LOG(STF_LEVEL_ERR, "could not install SIGINT handler, continuous capture disabled\n");
  121. gp_cfg_param->continuous = 0;
  122. }
  123. }
  124. /**
  125. process image read, recommand NV21 or NV12
  126. TODO: use ffmpeg or opencv to convert the image format
  127. */
  128. static void imageProcess(const uint8_t* inbuf, uint8_t* outbuf,
  129. struct timeval timestamp)
  130. {
  131. //timestamp.tv_sec
  132. //timestamp.tv_usec
  133. int in_width = gp_cfg_param->v4l2_param.width;
  134. int in_height = gp_cfg_param->v4l2_param.height;
  135. int in_imagesize = gp_cfg_param->v4l2_param.image_size;
  136. uint32_t in_format = gp_cfg_param->v4l2_param.format;
  137. char* jpegFilename = gp_cfg_param->jpegFilename;
  138. int disp_type = gp_cfg_param->disp_type;
  139. int new_in_format = 0;
  140. int out_format = 0;
  141. int out_size = 0;
  142. uint8_t* dst = malloc(in_width * in_height * 3);
  143. int is_yuv420sp = 0; // NOTE: NV21 or NV12, it is special for starfive framebuffer
  144. static int s_frmcnt = 0;
  145. if (STF_DISP_FB == disp_type) {
  146. new_in_format = v4l2fmt_to_fbfmt(in_format);
  147. out_format = gp_cfg_param->fb_param.pixformat;
  148. out_size = gp_cfg_param->fb_param.screen_size;
  149. is_yuv420sp = gp_cfg_param->fb_param.vinfo.grayscale;
  150. } else if (STF_DISP_DRM == disp_type) {
  151. new_in_format = v4l2fmt_to_drmfmt(in_format);
  152. out_format = gp_cfg_param->drm_param.dev_head->drm_format;
  153. out_size = gp_cfg_param->drm_param.dev_head->bufs[0].size;
  154. }
  155. LOG(STF_LEVEL_LOG, "in_width=%d, in_height=%d, in_imagesize=%d, out_size=%d, p=%p\n",
  156. in_width, in_height, in_imagesize, out_size, inbuf);
  157. // write jpeg
  158. char filename[512];
  159. switch (in_format) {
  160. case V4L2_PIX_FMT_YUV420:
  161. // if (jpegFilename) {
  162. // // sprintf(filename, "%d-yuv420-%s", s_frmcnt, jpegFilename);
  163. // // YUV420toYUV444(in_width, in_height, inbuf, dst);
  164. // // jpegWrite(dst, filename);
  165. // sprintf(filename, "raw-%d-yuv420-%s", s_frmcnt, jpegFilename);
  166. // write_file(filename, inbuf, in_imagesize);
  167. // s_frmcnt++;
  168. // }
  169. if (gp_cfg_param->jpegFilename && gp_cfg_param->rec_fp) {
  170. fwrite(inbuf, in_imagesize, 1, gp_cfg_param->rec_fp);
  171. }
  172. break;
  173. case V4L2_PIX_FMT_YUYV:
  174. case V4L2_PIX_FMT_YVYU:
  175. // if (jpegFilename) {
  176. // // sprintf(filename, "%d-yuv422-%s", s_frmcnt, jpegFilename);
  177. // // YUV422toYUV444(in_width, in_height, inbuf, dst);
  178. // // jpegWrite(dst, filename);
  179. // sprintf(filename, "raw-%d-yuv422-%s", s_frmcnt, jpegFilename);
  180. // write_file(filename, inbuf, in_imagesize);
  181. // s_frmcnt++;
  182. // }
  183. if (gp_cfg_param->jpegFilename && gp_cfg_param->rec_fp) {
  184. fwrite(inbuf, in_imagesize, 1, gp_cfg_param->rec_fp);
  185. }
  186. if (out_format == new_in_format) {
  187. yuyv_resize(inbuf, outbuf, in_width, in_height);
  188. } else if ((STF_DISP_FB == disp_type) && is_yuv420sp) {
  189. convert_yuyv_to_nv12(inbuf, outbuf, in_width, in_height, 1);
  190. } else if ((STF_DISP_DRM == disp_type) && (out_format == V4L2_PIX_FMT_NV12)) {
  191. convert_yuyv_to_nv12(inbuf, outbuf, in_width, in_height, 1);
  192. } else {
  193. convert_yuyv_to_rgb(inbuf, outbuf, in_width, in_height, 0);
  194. }
  195. break;
  196. case V4L2_PIX_FMT_NV21:
  197. // if (gp_cfg_param->jpegFilename) {
  198. // // sprintf(filename, "%d-nv21-%s", s_frmcnt, gp_cfg_param->jpegFilename);
  199. // // YUV420NV21toYUV444(in_width, in_height, inbuf, dst, 0);
  200. // // jpegWrite(dst, filename);
  201. // sprintf(filename, "raw-nv21-%s", gp_cfg_param->jpegFilename);
  202. // write_file(filename, inbuf, in_imagesize);
  203. // s_frmcnt++;
  204. // }
  205. if (gp_cfg_param->jpegFilename && gp_cfg_param->rec_fp) {
  206. fwrite(inbuf, in_imagesize, 1, gp_cfg_param->rec_fp);
  207. }
  208. LOG(STF_LEVEL_LOG, "out_format=%d, new_in_format=%d, is_yuv420sp=%d\n", out_format,
  209. new_in_format, is_yuv420sp);
  210. if (outbuf) {
  211. if (out_format == new_in_format) {
  212. convert_nv21_to_nv12(inbuf, outbuf, in_width, in_height, 0);
  213. } else if ((STF_DISP_FB == disp_type) && is_yuv420sp) {
  214. convert_nv21_to_nv12(inbuf, outbuf, in_width, in_height, 1);
  215. } else {
  216. convert_nv21_to_nv12(inbuf, outbuf, in_width, in_height, 1);
  217. }
  218. }
  219. break;
  220. case V4L2_PIX_FMT_NV12:
  221. // if (jpegFilename) {
  222. // // sprintf(filename, "%d-nv12-%s", s_frmcnt, jpegFilename);
  223. // // YUV420NV12toYUV444(in_width, in_height, inbuf, dst);
  224. // // jpegWrite(dst, filename);
  225. // sprintf(filename, "raw-%d-nv12-%s", s_frmcnt, jpegFilename);
  226. // write_file(filename, inbuf, in_imagesize);
  227. // s_frmcnt++;
  228. // }
  229. if (gp_cfg_param->jpegFilename && gp_cfg_param->rec_fp) {
  230. fwrite(inbuf, in_imagesize, 1, gp_cfg_param->rec_fp);
  231. }
  232. LOG(STF_LEVEL_DEBUG, "out_format=%d, new_in_format=%d, is_yuv420sp=%d\n", out_format,
  233. new_in_format, is_yuv420sp);
  234. if (out_format == new_in_format) {
  235. convert_nv21_to_nv12(inbuf, outbuf, in_width, in_height, 0);
  236. } else if ((STF_DISP_FB == disp_type) && is_yuv420sp) {
  237. convert_nv21_to_nv12(inbuf, outbuf, in_width, in_height, 1);
  238. } else {
  239. convert_nv21_to_rgb(inbuf, outbuf, in_width, in_height, 0);
  240. }
  241. break;
  242. case V4L2_PIX_FMT_RGB24:
  243. // if (jpegFilename) {
  244. // // sprintf(filename, "%d-rgb-%s", s_frmcnt, jpegFilename);
  245. // // RGB565toRGB888(in_width, in_height, inbuf, dst);
  246. // // write_JPEG_file(filename, inbuf, in_width, in_height, jpegQuality);
  247. // sprintf(filename, "raw-%d-rgb-%s", s_frmcnt, jpegFilename);
  248. // write_file(filename, inbuf, in_imagesize);
  249. // s_frmcnt++;
  250. // }
  251. if (gp_cfg_param->jpegFilename && gp_cfg_param->rec_fp) {
  252. fwrite(inbuf, in_imagesize, 1, gp_cfg_param->rec_fp);
  253. }
  254. convert_rgb888_to_rgb(inbuf, outbuf, in_width, in_height, 0);
  255. break;
  256. case V4L2_PIX_FMT_RGB565:
  257. // if (jpegFilename) {
  258. // // sprintf(filename, "%d-rgb565-%s", s_frmcnt, jpegFilename);
  259. // // RGB565toRGB888(in_width, in_height, inbuf, dst);
  260. // // write_JPEG_file(filename, dst, in_width, in_height, jpegQuality);
  261. // sprintf(filename, "raw-%d-rgb565-%s", s_frmcnt, jpegFilename);
  262. // write_file(filename, inbuf, in_imagesize);
  263. // s_frmcnt++;
  264. // }
  265. if (gp_cfg_param->jpegFilename && gp_cfg_param->rec_fp) {
  266. fwrite(inbuf, in_imagesize, 1, gp_cfg_param->rec_fp);
  267. }
  268. if (out_format == new_in_format)
  269. convert_rgb565_to_rgb(inbuf, outbuf, in_width, in_height, 0);
  270. else if ((STF_DISP_FB == disp_type) && is_yuv420sp)
  271. convert_rgb565_to_nv12(inbuf, outbuf, in_width, in_height, 0);
  272. else
  273. convert_rgb565_to_rgb(inbuf, outbuf, in_width, in_height, 0);
  274. break;
  275. case V4L2_PIX_FMT_SRGGB12:
  276. if (jpegFilename)
  277. sprintf(filename, "raw-%d-RGGB12-%s", s_frmcnt, jpegFilename);
  278. else
  279. sprintf(filename, "raw-%d-RGGB12.raw", s_frmcnt);
  280. write_file(filename, inbuf, in_imagesize);
  281. RAW12toRAW16(in_width, in_height, inbuf, dst);
  282. if (jpegFilename)
  283. sprintf(filename, "raw-%d-RGGB16-%s", s_frmcnt, jpegFilename);
  284. else
  285. sprintf(filename, "raw-%d-RGGB16.raw", s_frmcnt);
  286. write_file(filename, (const uint8_t *)dst, in_width * in_height * 2);
  287. s_frmcnt++;
  288. break;
  289. case V4L2_PIX_FMT_SGRBG12:
  290. if (jpegFilename)
  291. sprintf(filename, "raw-%d-GRBG12-%s", s_frmcnt, jpegFilename);
  292. else
  293. sprintf(filename, "raw-%d-GRBG12.raw", s_frmcnt);
  294. write_file(filename, inbuf, in_imagesize);
  295. RAW12toRAW16(in_width, in_height, inbuf, dst);
  296. if (jpegFilename)
  297. sprintf(filename, "raw-%d-GRBG16-%s", s_frmcnt, jpegFilename);
  298. else
  299. sprintf(filename, "raw-%d-GRBG16.raw", s_frmcnt);
  300. write_file(filename, (const uint8_t *)dst, in_width * in_height * 2);
  301. s_frmcnt++;
  302. break;
  303. case V4L2_PIX_FMT_SGBRG12:
  304. if (jpegFilename)
  305. sprintf(filename, "raw-%d-GBRG12-%s", s_frmcnt, jpegFilename);
  306. else
  307. sprintf(filename, "raw-%d-GBRG12.raw", s_frmcnt);
  308. write_file(filename, inbuf, in_imagesize);
  309. RAW12toRAW16(in_width, in_height, inbuf, dst);
  310. if (jpegFilename)
  311. sprintf(filename, "raw-%d-GBRG16-%s", s_frmcnt, jpegFilename);
  312. else
  313. sprintf(filename, "raw-%d-GBRG16.raw", s_frmcnt);
  314. write_file(filename, (const uint8_t *)dst, in_width * in_height * 2);
  315. s_frmcnt++;
  316. break;
  317. case V4L2_PIX_FMT_SBGGR12:
  318. if (jpegFilename)
  319. sprintf(filename, "raw-%d-BGGR12-%s", s_frmcnt, jpegFilename);
  320. else
  321. sprintf(filename, "raw-%d-BGGR12.raw", s_frmcnt);
  322. write_file(filename, inbuf, in_imagesize);
  323. RAW12toRAW16(in_width, in_height, inbuf, dst);
  324. if (jpegFilename)
  325. sprintf(filename, "raw-%d-BGGR16-%s", s_frmcnt, jpegFilename);
  326. else
  327. sprintf(filename, "raw-%d-BGGR16.raw", s_frmcnt);
  328. write_file(filename, (const uint8_t *)dst, in_width * in_height * 2);
  329. s_frmcnt++;
  330. break;
  331. default:
  332. LOG(STF_LEVEL_ERR, "unknow in_format\n");
  333. break;
  334. }
  335. // free temporary image
  336. free(dst);
  337. }
  338. void calc_frame_fps()
  339. {
  340. static uint32_t frm_cnt = 0;
  341. static struct timespec ts_old;
  342. struct timespec ts;
  343. uint32_t fps = 0;
  344. uint32_t diff_ms = 0;
  345. if (frm_cnt == 0) {
  346. clock_gettime(CLOCK_MONOTONIC, &ts_old);
  347. }
  348. if (frm_cnt++ >= 50) {
  349. clock_gettime(CLOCK_MONOTONIC, &ts);
  350. diff_ms = (ts.tv_sec - ts_old.tv_sec) * 1000 + (ts.tv_nsec - ts_old.tv_nsec) / 1000000;
  351. fps = 1000 * (frm_cnt - 1) / diff_ms;
  352. frm_cnt = 0;
  353. LOG(STF_LEVEL_INFO, "pipeline display fps=%d\n", fps);
  354. }
  355. }
  356. /**
  357. read single frame
  358. */
  359. static int frameRead(void)
  360. {
  361. struct v4l2_buffer buf;
  362. V4l2Param_t *pv4l2_param = &gp_cfg_param->v4l2_param;
  363. uint8_t *dst = NULL;
  364. if (STF_DISP_FB == gp_cfg_param->disp_type) {
  365. dst = gp_cfg_param->fb_param.screen_buf;
  366. } else if (STF_DISP_DRM == gp_cfg_param->disp_type &&
  367. IO_METHOD_DMABUF != gp_cfg_param->io_mthd) {
  368. dst = gp_cfg_param->drm_param.dev_head->bufs[0].buf;
  369. } else {
  370. LOG(STF_LEVEL_LOG, "Not display\n");
  371. }
  372. switch (pv4l2_param->io_mthd) {
  373. case IO_METHOD_READ:
  374. if (-1 == v4l2_read(pv4l2_param->fd, pv4l2_param->pBuffers[0].start,
  375. pv4l2_param->pBuffers[0].length)) {
  376. switch (errno) {
  377. case EAGAIN:
  378. return 0;
  379. case EIO:
  380. // Could ignore EIO, see spec.
  381. // fall through
  382. default:
  383. errno_exit("read");
  384. }
  385. }
  386. struct timespec ts;
  387. struct timeval timestamp;
  388. clock_gettime(CLOCK_MONOTONIC,&ts);
  389. timestamp.tv_sec = ts.tv_sec;
  390. timestamp.tv_usec = ts.tv_nsec/1000;
  391. imageProcess((uint8_t *)(pv4l2_param->pBuffers[0].start), dst, timestamp);
  392. break;
  393. case IO_METHOD_MMAP:
  394. stf_v4l2_dequeue_buffer(pv4l2_param, &buf);
  395. LOG(STF_LEVEL_LOG, "buf.index=%d, n_buffers=%d\n",
  396. buf.index, gp_cfg_param->v4l2_param.n_buffers);
  397. imageProcess((uint8_t *)(pv4l2_param->pBuffers[buf.index].start), dst, buf.timestamp);
  398. stf_v4l2_queue_buffer(pv4l2_param, buf.index);
  399. if (STF_DISP_DRM == gp_cfg_param->disp_type) {
  400. drmModePageFlip(gp_cfg_param->drm_param.fd, gp_cfg_param->drm_param.dev_head->crtc_id,
  401. gp_cfg_param->drm_param.dev_head->bufs[0].fb_id,
  402. DRM_MODE_PAGE_FLIP_EVENT, gp_cfg_param->drm_param.dev_head);
  403. }
  404. LOG(STF_LEVEL_LOG, "buf.index: %d, buf.bytesused=%d\n", buf.index, buf.bytesused);
  405. break;
  406. case IO_METHOD_USERPTR:
  407. stf_v4l2_dequeue_buffer(pv4l2_param, &buf);
  408. imageProcess((uint8_t *)(buf.m.userptr), dst, buf.timestamp);
  409. stf_v4l2_queue_buffer(pv4l2_param, buf.index);
  410. break;
  411. case IO_METHOD_DMABUF:
  412. default:
  413. break;
  414. }
  415. return 1;
  416. }
  417. /**
  418. * mainloop_select: read frames with select() and process them
  419. */
  420. static void mainloop_select(void)
  421. {
  422. int count, i;
  423. uint32_t numberOfTimeouts;
  424. numberOfTimeouts = 0;
  425. count = 3;
  426. while (count-- > 0) {
  427. for (i = 0; i < 1; i++) {
  428. fd_set fds;
  429. struct timeval tv;
  430. int r;
  431. FD_ZERO(&fds);
  432. FD_SET(gp_cfg_param->v4l2_param.fd, &fds);
  433. /* Timeout. */
  434. tv.tv_sec = 1;
  435. tv.tv_usec = 0;
  436. r = select(gp_cfg_param->v4l2_param.fd + 1, &fds, NULL, NULL, &tv);
  437. if (-1 == r) {
  438. if (EINTR == errno) {
  439. continue;
  440. }
  441. errno_exit("select");
  442. } else if (0 == r) {
  443. if (numberOfTimeouts <= 0) {
  444. // count++;
  445. } else {
  446. LOG(STF_LEVEL_ERR, "select timeout\n");
  447. exit(EXIT_FAILURE);
  448. }
  449. }
  450. if (gp_cfg_param->continuous == 1) {
  451. count = 3;
  452. }
  453. if (frameRead())
  454. break;
  455. /* EAGAIN - continue select loop. */
  456. }
  457. }
  458. }
  459. static void page_flip_handler(int fd, unsigned int frame,
  460. unsigned int sec, unsigned int usec,
  461. void *data)
  462. {
  463. struct drm_dev_t *dev = data;
  464. /* If we have a next buffer, then let's return the current one,
  465. * and grab the next one.
  466. */
  467. if (g_drm_buf_next_idx > 0) {
  468. stf_v4l2_queue_buffer(&gp_cfg_param->v4l2_param, g_drm_buf_curr_idx);
  469. g_drm_buf_curr_idx = g_drm_buf_next_idx;
  470. g_drm_buf_next_idx = -1;
  471. }
  472. drmModePageFlip(fd, dev->crtc_id, dev->bufs[g_drm_buf_curr_idx].fb_id,
  473. DRM_MODE_PAGE_FLIP_EVENT, dev);
  474. }
  475. static void mainloop()
  476. {
  477. struct v4l2_buffer buf;
  478. int r;
  479. int count = 3;
  480. drmEventContext ev;
  481. struct pollfd* fds = NULL;
  482. uint32_t nfds = 0;
  483. LOG(STF_LEVEL_TRACE, "Enter\n");
  484. if (STF_DISP_FB == gp_cfg_param->disp_type ||
  485. IO_METHOD_MMAP == gp_cfg_param->io_mthd) {
  486. // fb or (drm + mmap)
  487. nfds = 1;
  488. fds = (struct pollfd*)malloc(sizeof(struct pollfd) * nfds);
  489. memset(fds, 0, sizeof(struct pollfd) * nfds);
  490. fds[0].fd = gp_cfg_param->v4l2_param.fd;
  491. fds[0].events = POLLIN;
  492. } else if (STF_DISP_DRM == gp_cfg_param->disp_type &&
  493. IO_METHOD_DMABUF == gp_cfg_param->io_mthd) {
  494. // (drm + dmabuf)
  495. nfds = 2;
  496. fds = (struct pollfd*)malloc(sizeof(struct pollfd) * nfds);
  497. memset(fds, 0, sizeof(struct pollfd) * nfds);
  498. fds[0].fd = gp_cfg_param->v4l2_param.fd;
  499. fds[0].events = POLLIN;
  500. fds[1].fd = gp_cfg_param->drm_param.fd;
  501. fds[1].events = POLLIN;
  502. memset(&ev, 0, sizeof ev);
  503. ev.version = DRM_EVENT_CONTEXT_VERSION;
  504. ev.vblank_handler = NULL;
  505. ev.page_flip_handler = page_flip_handler;
  506. } else {
  507. LOG(STF_LEVEL_ERR, "Display type %d and io method type %d not support\n",
  508. gp_cfg_param->disp_type, gp_cfg_param->io_mthd);
  509. exit(EXIT_FAILURE);
  510. }
  511. if (!gp_cfg_param->rec_fp && gp_cfg_param->jpegFilename) {
  512. gp_cfg_param->rec_fp = fopen(gp_cfg_param->jpegFilename, "w+");
  513. if (!gp_cfg_param->rec_fp) {
  514. LOG(STF_LEVEL_ERR, "can't open %s\n", gp_cfg_param->jpegFilename);
  515. exit(EXIT_FAILURE);
  516. }
  517. }
  518. while (count-- > 0) {
  519. r = poll(fds, nfds, 3000);
  520. if (-1 == r) {
  521. if (EINTR == errno) {
  522. continue;
  523. }
  524. LOG(STF_LEVEL_ERR, "error in poll %d", errno);
  525. break;
  526. }
  527. if (0 == r) {
  528. LOG(STF_LEVEL_ERR, "poll timeout, %d\n", errno);
  529. break;
  530. }
  531. if (STF_DISP_FB == gp_cfg_param->disp_type ||
  532. IO_METHOD_MMAP == gp_cfg_param->io_mthd) {
  533. // fb or (drm + mmap)
  534. if (fds[0].revents & POLLIN) {
  535. frameRead();
  536. calc_frame_fps();
  537. }
  538. } else if (STF_DISP_DRM == gp_cfg_param->disp_type &&
  539. IO_METHOD_DMABUF == gp_cfg_param->io_mthd) {
  540. // drm + dmabuf
  541. if (fds[0].revents & POLLIN) {
  542. int dequeued = stf_v4l2_dequeue_buffer(&gp_cfg_param->v4l2_param, &buf);
  543. if (dequeued) {
  544. g_drm_buf_next_idx = buf.index;
  545. frameRead(); // TODO: add support for save file later
  546. calc_frame_fps();
  547. }
  548. }
  549. if (fds[1].revents & POLLIN) {
  550. if (g_drm_buf_next_idx > 0) {
  551. drmHandleEvent(gp_cfg_param->drm_param.fd, &ev);
  552. }
  553. }
  554. }
  555. if (gp_cfg_param->continuous == 1) {
  556. count = 3;
  557. }
  558. usleep(1 * 1000);
  559. }
  560. if (fds) {
  561. free(fds);
  562. fds = NULL;
  563. }
  564. if (gp_cfg_param->rec_fp) {
  565. fclose(gp_cfg_param->rec_fp);
  566. gp_cfg_param->rec_fp = NULL;
  567. }
  568. LOG(STF_LEVEL_TRACE, "Exit\n");
  569. }
  570. static void usage(FILE* fp, int argc, char** argv)
  571. {
  572. fprintf(fp,
  573. "Usage: %s [options]\n\n"
  574. "Options:\n"
  575. "-d | --device name Video device name [default /dev/video0]\n"
  576. "-h | --help Print this message\n"
  577. "-o | --output Save raw data to filename\n"
  578. //"-q | --quality Set JPEG quality (0-100)\n"
  579. "-m | --method Set V4L2 videobuf2 memory type, default 0\n"
  580. " 0: IO_METHOD_MMAP\n"
  581. " 1: IO_METHOD_USERPTR\n"
  582. " 2: IO_METHOD_DMABUF\n"
  583. " 3: IO_METHOD_READ\n"
  584. "-W | --width Set v4l2 image width, default 1920\n"
  585. "-H | --height Set v4l2 image height, default 1080\n"
  586. "-X | --left Set v4l2 image crop x start\n"
  587. "-Y | --up Set v4l2 image crop y start\n"
  588. "-R | --right Set v4l2 image crop x width\n"
  589. "-D | --down Set v4l2 image crop y height\n"
  590. "-I | --interval Set frame interval (fps) (-1 to skip)\n"
  591. "-c | --continuous Do continous capture, stop with SIGINT.\n"
  592. "-v | --version Print version\n"
  593. "-f | --format image format, default 5\n"
  594. " 0: V4L2_PIX_FMT_RGB565\n"
  595. " 1: V4L2_PIX_FMT_RGB24\n"
  596. " 2: V4L2_PIX_FMT_YUV420\n"
  597. " 3: V4L2_PIX_FMT_YUYV\n"
  598. " 4: V4L2_PIX_FMT_NV21\n"
  599. " 5: V4L2_PIX_FMT_NV12\n"
  600. " 6: V4L2_PIX_FMT_YVYU\n"
  601. " 7: V4L2_PIX_FMT_SRGGB12\n"
  602. " 8: V4L2_PIX_FMT_SGRBG12\n"
  603. " 9: V4L2_PIX_FMT_SGBRG12\n"
  604. " 10: V4L2_PIX_FMT_SBGGR12\n"
  605. " default: V4L2_PIX_FMT_RGB565\n"
  606. "-t | --distype set display type, default 0\n"
  607. " 0: Not display\n"
  608. " 1: Use Framebuffer Display\n"
  609. " 2: Use DRM Display\n"
  610. "-l | --loadfw load stfisp fw image\n"
  611. "-s | --g_imagesize print image size\n"
  612. "\n"
  613. "Eg:\n"
  614. "\t drm: v4l2test -d /dev/video2 -f 5 -c -W 1920 -H 1080 -m 2 -t 2\n"
  615. "\t fb: v4l2test -d /dev/video2 -f 5 -c -W 1920 -H 1080 -m 0 -t 1\n"
  616. "\n"
  617. "Open debug log level: \n"
  618. "\t export V4L2_DEBUG=3\n"
  619. "\t default level 1, level range 0 ~ 7\n"
  620. "",
  621. argv[0]);
  622. }
  623. static const char short_options [] = "d:ho:q:m:W:H:I:vcf:t:X:Y:R:D:l:s";
  624. static const struct option long_options [] = {
  625. { "device", required_argument, NULL, 'd' },
  626. { "help", no_argument, NULL, 'h' },
  627. { "output", required_argument, NULL, 'o' },
  628. { "quality", required_argument, NULL, 'q' },
  629. { "method", required_argument, NULL, 'm' },
  630. { "width", required_argument, NULL, 'W' },
  631. { "height", required_argument, NULL, 'H' },
  632. { "left", required_argument, NULL, 'X' },
  633. { "up", required_argument, NULL, 'Y' },
  634. { "right", required_argument, NULL, 'R' },
  635. { "down", required_argument, NULL, 'D' },
  636. { "interval", required_argument, NULL, 'I' },
  637. { "version", no_argument, NULL, 'v' },
  638. { "continuous", no_argument, NULL, 'c' },
  639. { "format", required_argument, NULL, 'f' },
  640. { "distype", required_argument, NULL, 't' },
  641. { "loadfw", required_argument, NULL, 'l' },
  642. { "g_imagesize",no_argument, NULL, 's' },
  643. { 0, 0, 0, 0 }
  644. };
  645. void parse_options(int argc, char **argv, ConfigParam_t *cfg_param)
  646. {
  647. int index, c = 0;
  648. int value = 0;
  649. while ((c = getopt_long(argc, argv, short_options, long_options, &index)) != -1) {
  650. switch (c) {
  651. case 0: /* getopt_long() flag */
  652. break;
  653. case 'd':
  654. cfg_param->v4l2_param.device_name = strdup(optarg);
  655. break;
  656. case 'h':
  657. usage(stdout, argc, argv);
  658. exit(EXIT_SUCCESS);
  659. case 'o':
  660. // set jpeg filename
  661. cfg_param->jpegFilename = strdup(optarg);
  662. break;
  663. case 'q':
  664. // set jpeg quality
  665. cfg_param->jpegQuality = atoi(optarg);
  666. break;
  667. case 'm':
  668. value = atoi(optarg);
  669. if (value < IO_METHOD_MMAP || value > IO_METHOD_READ) {
  670. LOG(STF_LEVEL_ERR, "io method %d is out of range [%d, %d]\n", value,
  671. IO_METHOD_MMAP, IO_METHOD_READ);
  672. exit(EXIT_FAILURE);
  673. }
  674. LOG(STF_LEVEL_INFO, "io method: %s\n", g_iomthd_values[value].name);
  675. cfg_param->io_mthd = value;
  676. cfg_param->v4l2_param.io_mthd = cfg_param->io_mthd;
  677. break;
  678. case 'W':
  679. // set v4l2 width
  680. cfg_param->v4l2_param.width = atoi(optarg);
  681. break;
  682. case 'H':
  683. // set v4l2 height
  684. cfg_param->v4l2_param.height = atoi(optarg);
  685. break;
  686. case 'X':
  687. // set x start
  688. cfg_param->v4l2_param.crop_info.left = atoi(optarg);
  689. cfg_param->v4l2_param.crop_flag = 1;
  690. break;
  691. case 'Y':
  692. // set y start
  693. cfg_param->v4l2_param.crop_info.top = atoi(optarg);
  694. cfg_param->v4l2_param.crop_flag = 1;
  695. break;
  696. case 'R':
  697. // set x width
  698. cfg_param->v4l2_param.crop_info.width = atoi(optarg);
  699. cfg_param->v4l2_param.crop_flag = 1;
  700. break;
  701. case 'D':
  702. // set y height
  703. cfg_param->v4l2_param.crop_info.height = atoi(optarg);
  704. cfg_param->v4l2_param.crop_flag = 1;
  705. break;
  706. case 'I':
  707. // set fps
  708. cfg_param->v4l2_param.fps = atoi(optarg);
  709. break;
  710. case 'c':
  711. // set flag for continuous capture, interuptible by sigint
  712. cfg_param->continuous = 1;
  713. InstallSIGINTHandler();
  714. break;
  715. case 'v':
  716. printf("Version: %s\n", TEST_VERSION);
  717. exit(EXIT_SUCCESS);
  718. break;
  719. case 'f':
  720. LOG(STF_LEVEL_INFO, "v4l2 format: %s\n", optarg);
  721. value = atoi(optarg);
  722. LOG(STF_LEVEL_INFO, "v4l2 format: %d\n", value);
  723. switch (value) {
  724. case 0:
  725. value = V4L2_PIX_FMT_RGB565;
  726. break;
  727. case 1:
  728. value = V4L2_PIX_FMT_RGB24;
  729. break;
  730. case 2:
  731. value = V4L2_PIX_FMT_YUV420;
  732. break;
  733. case 3:
  734. value = V4L2_PIX_FMT_YUYV;
  735. break;
  736. case 4:
  737. value = V4L2_PIX_FMT_NV21;
  738. break;
  739. case 5:
  740. value = V4L2_PIX_FMT_NV12;
  741. break;
  742. case 6:
  743. value = V4L2_PIX_FMT_YVYU;
  744. break;
  745. case 7:
  746. value = V4L2_PIX_FMT_SRGGB12;
  747. break;
  748. case 8:
  749. value = V4L2_PIX_FMT_SGRBG12;
  750. break;
  751. case 9:
  752. value = V4L2_PIX_FMT_SGBRG12;
  753. break;
  754. case 10:
  755. value = V4L2_PIX_FMT_SBGGR12;
  756. break;
  757. default:
  758. value = V4L2_PIX_FMT_RGB565;
  759. break;
  760. }
  761. cfg_param->v4l2_param.format = value;
  762. break;
  763. case 't':
  764. value = atoi(optarg);
  765. if (value < STF_DISP_NONE || value > STF_DISP_DRM) {
  766. LOG(STF_LEVEL_ERR, "Display Type %d is out of range [%d, %d]\n", value,
  767. STF_DISP_NONE, STF_DISP_DRM);
  768. exit(EXIT_FAILURE);
  769. }
  770. LOG(STF_LEVEL_INFO, "Display Type: %s\n", g_disp_values[value].name);
  771. cfg_param->disp_type = value;
  772. break;
  773. case 'l':
  774. loadfw_start(optarg, &(cfg_param->v4l2_param));
  775. exit(EXIT_SUCCESS);
  776. break;
  777. case 's':
  778. sensor_image_size_info(&(cfg_param->v4l2_param));
  779. exit(EXIT_SUCCESS);
  780. break;
  781. default:
  782. usage(stderr, argc, argv);
  783. exit(EXIT_FAILURE);
  784. }
  785. }
  786. }
  787. int main(int argc, char **argv)
  788. {
  789. init_log();
  790. alloc_default_config(&gp_cfg_param);
  791. parse_options(argc, argv, gp_cfg_param);
  792. check_cfg_params(gp_cfg_param);
  793. // open and initialize v4l2 device
  794. stf_v4l2_open(&gp_cfg_param->v4l2_param, gp_cfg_param->v4l2_param.device_name);
  795. stf_v4l2_init(&gp_cfg_param->v4l2_param);
  796. if (STF_DISP_FB == gp_cfg_param->disp_type) {
  797. stf_fb_open(&gp_cfg_param->fb_param, FB_DEVICE_NAME, STFBC_DEVICE_NAME);
  798. stf_fb_init(&gp_cfg_param->fb_param, gp_cfg_param->v4l2_param.format);
  799. update_videocvt_param(gp_cfg_param->disp_type, gp_cfg_param->fb_param.width,
  800. gp_cfg_param->fb_param.height, gp_cfg_param->fb_param.bpp,
  801. gp_cfg_param->fb_param.screen_size);
  802. } else if (STF_DISP_DRM == gp_cfg_param->disp_type) {
  803. stf_drm_open(&gp_cfg_param->drm_param, DRM_DEVICE_NAME, gp_cfg_param->io_mthd);
  804. stf_drm_init(&gp_cfg_param->drm_param, gp_cfg_param->v4l2_param.width,
  805. gp_cfg_param->v4l2_param.height, gp_cfg_param->v4l2_param.format,
  806. gp_cfg_param->io_mthd, gp_cfg_param->dmabufs,
  807. sizeof(gp_cfg_param->dmabufs) / sizeof(gp_cfg_param->dmabufs[0]));
  808. update_videocvt_param(gp_cfg_param->disp_type, gp_cfg_param->drm_param.dev_head->width,
  809. gp_cfg_param->drm_param.dev_head->height, 32,
  810. gp_cfg_param->drm_param.dev_head->bufs[0].size);
  811. }
  812. // prepare and start v4l2 capturing
  813. sft_v4l2_prepare_capturing(&gp_cfg_param->v4l2_param, gp_cfg_param->dmabufs, BUFCOUNT);
  814. sft_v4l2_start_capturing(&(gp_cfg_param->v4l2_param));
  815. // process frames
  816. mainloop();
  817. stf_v4l2_stop_capturing(&gp_cfg_param->v4l2_param);
  818. stf_v4l2_uninit(&gp_cfg_param->v4l2_param);
  819. stf_v4l2_close(&gp_cfg_param->v4l2_param);
  820. if (STF_DISP_FB == gp_cfg_param->disp_type) {
  821. stf_fb_uninit(&gp_cfg_param->fb_param);
  822. stf_fb_close(&gp_cfg_param->fb_param);
  823. } else if (STF_DISP_DRM == gp_cfg_param->disp_type) {
  824. stf_drm_close(&gp_cfg_param->drm_param);
  825. }
  826. deinit_log();
  827. free(gp_cfg_param);
  828. return 0;
  829. }