stf_v4l2.c 21 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698
  1. // SPDX-License-Identifier: GPL-2.0
  2. /*
  3. * Copyright (C) 2021 StarFive Technology Co., Ltd.
  4. */
  5. #include <sys/mman.h>
  6. #include <libv4l2.h>
  7. #include <sys/ioctl.h>
  8. #include "common.h"
  9. #include "stf_v4l2.h"
  10. #include "stf_log.h"
  11. #define FILENAME_MAX_LEN 30
  12. struct stfisp_fw_info {
  13. char filename[FILENAME_MAX_LEN];
  14. };
  15. struct v4l2_subdev_frame_size_enum {
  16. __u32 index;
  17. __u32 pad;
  18. __u32 code;
  19. __u32 min_width;
  20. __u32 max_width;
  21. __u32 min_height;
  22. __u32 max_height;
  23. __u32 which;
  24. __u32 reserved[8];
  25. };
  26. #define VIDIOC_STFISP_LOAD_FW \
  27. _IOW('V', BASE_VIDIOC_PRIVATE + 1, struct stfisp_fw_info)
  28. #define VIDIOC_SUBDEV_ENUM_FRAME_SIZE \
  29. _IOWR('V', 74, struct v4l2_subdev_frame_size_enum)
  30. #define MEDIA_BUS_FMT_SRGGB10_1X10 0x300f
  31. /**
  32. Do ioctl and retry if error was EINTR ("A signal was caught during the ioctl() operation."). Parameters are the same as on ioctl.
  33. \param fd file descriptor
  34. \param request request
  35. \param argp argument
  36. \returns result from ioctl
  37. */
  38. int xioctl(int fd, int request, void* argp)
  39. {
  40. int r;
  41. // TODO: the orign is v4l2_ioctl()
  42. do r = ioctl(fd, request, argp);
  43. while (-1 == r && EINTR == errno);
  44. return r;
  45. }
  46. void sensor_image_size_info(V4l2Param_t *param)
  47. {
  48. int fd = 0;
  49. uint32_t i = 0;
  50. struct v4l2_subdev_frame_size_enum frame_size;
  51. LOG(STF_LEVEL_DEBUG, "go in sensor_image_size_info....\n");
  52. fd = open(param->device_name, O_RDWR /* required */ | O_NONBLOCK, 0);
  53. if (-1 == fd) {
  54. LOG(STF_LEVEL_ERR, "Cannot open '%s': %d, %s\n", param->device_name, errno, strerror(errno));
  55. close(fd);
  56. exit(EXIT_FAILURE);
  57. }
  58. for (i = 0; i < 4; i++) {
  59. frame_size.index = i;
  60. frame_size.code = MEDIA_BUS_FMT_SRGGB10_1X10;
  61. if (-1 == ioctl(fd, VIDIOC_SUBDEV_ENUM_FRAME_SIZE, &frame_size)) {
  62. close(fd);
  63. errno_exit("VIDIOC_SIZE_INFO");
  64. }
  65. LOG(STF_LEVEL_DEBUG, "image_size: width[%d] = %d, height[%d] = %d \n",
  66. i, frame_size.min_width, i, frame_size.min_height);
  67. }
  68. close(fd);
  69. }
  70. void loadfw_start(char *filename, V4l2Param_t *param)
  71. {
  72. struct stfisp_fw_info fw_info = {0};
  73. int fd = 0;
  74. LOG(STF_LEVEL_TRACE, "Enter\n");
  75. fd = open(param->device_name, O_RDWR /* required */ | O_NONBLOCK, 0);
  76. if (-1 == fd) {
  77. LOG(STF_LEVEL_ERR, "Cannot open '%s': %d, %s\n", param->device_name, errno, strerror(errno));
  78. exit(EXIT_FAILURE);
  79. }
  80. if (filename && (strlen(filename) < FILENAME_MAX_LEN)) {
  81. memcpy(fw_info.filename, filename, strlen(filename) + 1);
  82. }
  83. LOG(STF_LEVEL_INFO, "VIDIOC_STFISP_LOAD_FW = 0x%lx, filename = %s, size = %lu, device=%s\n",
  84. VIDIOC_STFISP_LOAD_FW, fw_info.filename, sizeof(struct stfisp_fw_info), param->device_name);
  85. if (-1 == ioctl(fd, VIDIOC_STFISP_LOAD_FW, &fw_info)) {
  86. if (EINVAL == errno) {
  87. close(fd);
  88. LOG(STF_LEVEL_ERR, "%s is no V4L2 device\n", param->device_name);
  89. exit(EXIT_FAILURE);
  90. } else {
  91. close(fd);
  92. errno_exit("VIDIOC_STFISP_LOAD_FW");
  93. }
  94. }
  95. close(fd);
  96. LOG(STF_LEVEL_TRACE, "Exit\n");
  97. }
  98. static void convert_v4l2_mem_type(int iomthd, enum v4l2_memory *mem_type)
  99. {
  100. if (iomthd < IO_METHOD_MMAP || iomthd > IO_METHOD_READ) {
  101. LOG(STF_LEVEL_ERR, "iomthd %d out of range\n", iomthd);
  102. exit(EXIT_FAILURE);
  103. }
  104. switch (iomthd) {
  105. case IO_METHOD_MMAP:
  106. *mem_type = V4L2_MEMORY_MMAP;
  107. break;
  108. case IO_METHOD_USERPTR:
  109. *mem_type = V4L2_MEMORY_USERPTR;
  110. break;
  111. case IO_METHOD_DMABUF:
  112. *mem_type = V4L2_MEMORY_DMABUF;
  113. break;
  114. case IO_METHOD_READ:
  115. *mem_type = 0; // not use memory machanism
  116. break;
  117. default:
  118. *mem_type = 0; // not use memory machanism
  119. break;
  120. }
  121. }
  122. void stf_v4l2_open(V4l2Param_t *param, char *device_name)
  123. {
  124. struct stat st;
  125. //struct v4l2_capability cap;
  126. LOG(STF_LEVEL_TRACE, "Enter\n");
  127. // stat file
  128. if (-1 == stat(device_name, &st)) {
  129. LOG(STF_LEVEL_ERR, "Cannot identify '%s': %d, %s\n", device_name, errno, strerror(errno));
  130. exit(EXIT_FAILURE);
  131. }
  132. // check if is device
  133. if (!S_ISCHR(st.st_mode)) {
  134. LOG(STF_LEVEL_ERR, "%s is no device\n", device_name);
  135. exit(EXIT_FAILURE);
  136. }
  137. // open device
  138. param->fd = v4l2_open(device_name, O_RDWR /* required */ | O_NONBLOCK, 0);
  139. // param->fd = v4l2_open(device_name, O_RDWR, 0);
  140. if (-1 == param->fd) {
  141. LOG(STF_LEVEL_ERR, "Cannot open '%s': %d, %s\n", device_name, errno, strerror(errno));
  142. exit(EXIT_FAILURE);
  143. }
  144. LOG(STF_LEVEL_TRACE, "Exit\n");
  145. }
  146. void stf_v4l2_close(V4l2Param_t *param)
  147. {
  148. LOG(STF_LEVEL_TRACE, "Enter\n");
  149. if (-1 == v4l2_close(param->fd)) {
  150. errno_exit("close");
  151. }
  152. param->fd = -1;
  153. LOG(STF_LEVEL_TRACE, "Exit\n");
  154. }
  155. void stf_v4l2_init(V4l2Param_t *param)
  156. {
  157. struct v4l2_capability cap;
  158. struct v4l2_cropcap cropcap;
  159. struct v4l2_crop crop;
  160. struct v4l2_format fmt;
  161. struct v4l2_streamparm frameint;
  162. //struct v4l2_streamparm frameget;
  163. unsigned int min;
  164. LOG(STF_LEVEL_TRACE, "Enter\n");
  165. // query capability
  166. if (-1 == xioctl(param->fd, VIDIOC_QUERYCAP, &cap)) {
  167. if (EINVAL == errno) {
  168. LOG(STF_LEVEL_ERR, "%s is no V4L2 device\n", param->device_name);
  169. exit(EXIT_FAILURE);
  170. } else {
  171. errno_exit("VIDIOC_QUERYCAP");
  172. }
  173. }
  174. if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
  175. LOG(STF_LEVEL_ERR, "%s is no video capture device\n", param->device_name);
  176. exit(EXIT_FAILURE);
  177. }
  178. switch (param->io_mthd) {
  179. case IO_METHOD_READ:
  180. if (!(cap.capabilities & V4L2_CAP_READWRITE)) {
  181. LOG(STF_LEVEL_ERR, "%s does not support read i/o\n", param->device_name);
  182. exit(EXIT_FAILURE);
  183. }
  184. break;
  185. case IO_METHOD_MMAP:
  186. case IO_METHOD_USERPTR:
  187. case IO_METHOD_DMABUF:
  188. if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
  189. LOG(STF_LEVEL_ERR, "%s does not support streaming i/o\n", param->device_name);
  190. exit(EXIT_FAILURE);
  191. }
  192. break;
  193. default:
  194. LOG(STF_LEVEL_ERR, "%s does not specify streaming i/o\n", param->device_name);
  195. exit(EXIT_FAILURE);
  196. break;
  197. }
  198. /* Select video input, video standard and tune here. */
  199. CLEAR(cropcap);
  200. cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  201. if (0 == xioctl(param->fd, VIDIOC_CROPCAP, &cropcap)) {
  202. crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  203. crop.c = cropcap.defrect; /* reset to default */
  204. if (-1 == xioctl(param->fd, VIDIOC_S_CROP, &crop)) {
  205. switch (errno) {
  206. case EINVAL:
  207. /* Cropping not supported. */
  208. break;
  209. default:
  210. /* Errors ignored. */
  211. break;
  212. }
  213. }
  214. } else {
  215. /* Errors ignored. */
  216. }
  217. /* If the user has set the fps to -1, don't try to set the frame interval */
  218. if (param->fps != -1) {
  219. CLEAR(frameint);
  220. /* Attempt to set the frame interval. */
  221. frameint.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  222. frameint.parm.capture.timeperframe.numerator = 1;
  223. frameint.parm.capture.timeperframe.denominator = param->fps;
  224. if (-1 == xioctl(param->fd, VIDIOC_S_PARM, &frameint)) {
  225. LOG(STF_LEVEL_WARN, "Unable to set frame interval.\n");
  226. }
  227. LOG(STF_LEVEL_INFO, "set frame interval = %d.\n", frameint.parm.capture.timeperframe.denominator);
  228. }
  229. // set v4l2_format
  230. CLEAR(fmt);
  231. fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  232. fmt.fmt.pix.width = param->width;
  233. fmt.fmt.pix.height = param->height;
  234. fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
  235. fmt.fmt.pix.pixelformat = param->format;
  236. if (-1 == xioctl(param->fd, VIDIOC_S_FMT, &fmt)) {
  237. errno_exit("VIDIOC_S_FMT");
  238. }
  239. if (param->crop_flag) {
  240. struct v4l2_selection sel_crop = {
  241. V4L2_BUF_TYPE_VIDEO_CAPTURE,
  242. V4L2_SEL_TGT_CROP,
  243. 0,
  244. {param->crop_info.left, param->crop_info.top,
  245. param->crop_info.width, param->crop_info.height} // TODO: opt here
  246. };
  247. struct v4l2_selection get_crop = {
  248. V4L2_BUF_TYPE_VIDEO_CAPTURE,
  249. V4L2_SEL_TGT_CROP,
  250. };
  251. LOG(STF_LEVEL_DEBUG, "sel_crop.left = %d, %d, %d, %d\n",
  252. sel_crop.r.left, sel_crop.r.top, sel_crop.r.width, sel_crop.r.height);
  253. if (-1 == xioctl(param->fd, VIDIOC_S_SELECTION, &sel_crop)) {
  254. LOG(STF_LEVEL_ERR, "S_SELECTION Failed.\n");
  255. }
  256. LOG(STF_LEVEL_DEBUG, "sel_crop.left = %d, %d, %d, %d\n",
  257. sel_crop.r.left, sel_crop.r.top, sel_crop.r.width, sel_crop.r.height);
  258. if (-1 == xioctl(param->fd, VIDIOC_G_SELECTION, &get_crop)) {
  259. LOG(STF_LEVEL_ERR, "G_SELECTION Failed.\n");
  260. }
  261. LOG(STF_LEVEL_DEBUG, "get_crop.left = %d, %d, %d, %d\n",
  262. get_crop.r.left, get_crop.r.top, get_crop.r.width, get_crop.r.height);
  263. if (memcmp(&sel_crop, &get_crop, sizeof(sel_crop))) {
  264. LOG(STF_LEVEL_WARN, "set/get selection diff.\n");
  265. }
  266. }
  267. // get v4l2_format
  268. memset(&fmt, 0, sizeof(struct v4l2_format));
  269. fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  270. fmt.fmt.pix.field = V4L2_FIELD_ANY;
  271. if (-1 == ioctl(param->fd, VIDIOC_G_FMT, &fmt)) {
  272. errno_exit("VIDIOC_G_FMT");
  273. }
  274. LOG(STF_LEVEL_INFO, "VIDIOC_G_FMT: type=%d, Fourcc format=%c%c%c%c\n",
  275. fmt.type, fmt.fmt.pix.pixelformat & 0xff,
  276. (fmt.fmt.pix.pixelformat >> 8) &0xff,
  277. (fmt.fmt.pix.pixelformat >> 16) &0xff,
  278. (fmt.fmt.pix.pixelformat >> 24) &0xff);
  279. LOG(STF_LEVEL_INFO, " \t width=%d, height=%d, field=%d, bytesperline=%d, sizeimage=%d\n",
  280. fmt.fmt.pix.width, fmt.fmt.pix.height, fmt.fmt.pix.field,
  281. fmt.fmt.pix.bytesperline, fmt.fmt.pix.sizeimage);
  282. //if (fmt.fmt.pix.pixelformat != V4L2_PIX_FMT_YUV420) {
  283. //if (fmt.fmt.pix.pixelformat != V4L2_PIX_FMT_RGB565) {
  284. if (fmt.fmt.pix.pixelformat != param->format) {
  285. LOG(STF_LEVEL_ERR, "v4l2 didn't accept format %d. Can't proceed.\n", param->format);
  286. exit(EXIT_FAILURE);
  287. }
  288. /* Note VIDIOC_S_FMT may change width and height. */
  289. if (param->width != fmt.fmt.pix.width) {
  290. param->width = fmt.fmt.pix.width;
  291. LOG(STF_LEVEL_WARN, "Correct image width set to %i by device %s.\n", param->width, param->device_name);
  292. }
  293. if (param->height != fmt.fmt.pix.height) {
  294. param->height = fmt.fmt.pix.height;
  295. LOG(STF_LEVEL_WARN, "Correct image height set to %i by device %s.\n", param->height, param->device_name);
  296. }
  297. // TODO: who and why add the below here? Change bytesperline and sizeimage
  298. /* Buggy driver paranoia. */
  299. min = fmt.fmt.pix.width * 2;
  300. if (fmt.fmt.pix.bytesperline < min) {
  301. fmt.fmt.pix.bytesperline = min;
  302. }
  303. min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
  304. if (fmt.fmt.pix.sizeimage < min) {
  305. fmt.fmt.pix.sizeimage = min;
  306. }
  307. // param->image_size = fmt.fmt.pix.sizeimage; // wrong here
  308. LOG(STF_LEVEL_LOG, "fmt.fmt.pix.sizeimage=%d, fmt.fmt.pix.bytesperline=%d\n",
  309. fmt.fmt.pix.sizeimage, fmt.fmt.pix.bytesperline);
  310. // convert mem type
  311. convert_v4l2_mem_type(param->io_mthd, &param->mem_type);
  312. LOG(STF_LEVEL_TRACE, "Exit\n");
  313. }
  314. void stf_v4l2_uninit(V4l2Param_t *param)
  315. {
  316. uint32_t i;
  317. LOG(STF_LEVEL_TRACE, "Enter\n");
  318. switch (param->io_mthd) {
  319. case IO_METHOD_READ:
  320. free(param->pBuffers[0].start);
  321. break;
  322. case IO_METHOD_MMAP:
  323. for (i = 0; i < param->n_buffers; ++i) {
  324. if (-1 == v4l2_munmap(param->pBuffers[i].start,
  325. param->pBuffers[i].length)) {
  326. errno_exit("munmap");
  327. }
  328. }
  329. break;
  330. case IO_METHOD_USERPTR:
  331. for (i = 0; i < param->n_buffers; ++i) {
  332. free(param->pBuffers[i].start);
  333. }
  334. break;
  335. case IO_METHOD_DMABUF:
  336. break;
  337. default:
  338. break;
  339. }
  340. free(param->pBuffers);
  341. LOG(STF_LEVEL_TRACE, "Exit\n");
  342. }
  343. static void stf_v4l2_readInit(V4l2Param_t *param)
  344. {
  345. LOG(STF_LEVEL_TRACE, "Enter\n");
  346. param->n_buffers = 1;
  347. param->pBuffers = calloc(param->n_buffers, sizeof(*param->pBuffers));
  348. if (!param->pBuffers) {
  349. LOG(STF_LEVEL_ERR, "Out of memory\n");
  350. exit(EXIT_FAILURE);
  351. }
  352. param->pBuffers[0].length = param->image_size;
  353. param->pBuffers[0].start = malloc(param->image_size);
  354. if (!param->pBuffers[0].start) {
  355. LOG(STF_LEVEL_ERR, "Out of memory\n");
  356. exit(EXIT_FAILURE);
  357. }
  358. LOG(STF_LEVEL_TRACE, "Exit\n");
  359. }
  360. static void stf_v4l2_mmapInit(V4l2Param_t *param)
  361. {
  362. int i = 0;
  363. struct v4l2_requestbuffers req;
  364. CLEAR(req);
  365. LOG(STF_LEVEL_TRACE, "Enter\n");
  366. req.count = BUFCOUNT;
  367. req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  368. req.memory = V4L2_MEMORY_MMAP;
  369. if (-1 == xioctl(param->fd, VIDIOC_REQBUFS, &req)) {
  370. if (EINVAL == errno) {
  371. LOG(STF_LEVEL_ERR, "%s does not support memory mapping\n", param->device_name);
  372. exit(EXIT_FAILURE);
  373. } else {
  374. errno_exit("VIDIOC_REQBUFS");
  375. }
  376. }
  377. if (req.count < 2) {
  378. LOG(STF_LEVEL_ERR, "Insufficient buffer memory on %s\n", param->device_name);
  379. exit(EXIT_FAILURE);
  380. }
  381. param->n_buffers = req.count;
  382. param->pBuffers = calloc(param->n_buffers, sizeof(*param->pBuffers));
  383. if (!param->pBuffers) {
  384. LOG(STF_LEVEL_ERR, "Out of memory\n");
  385. exit(EXIT_FAILURE);
  386. }
  387. for (i = 0; i < param->n_buffers; ++i) {
  388. struct v4l2_buffer buf;
  389. CLEAR(buf);
  390. buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  391. buf.memory = V4L2_MEMORY_MMAP;
  392. buf.index = i;
  393. if (-1 == xioctl(param->fd, VIDIOC_QUERYBUF, &buf))
  394. errno_exit("VIDIOC_QUERYBUF");
  395. param->pBuffers[i].length = buf.length;
  396. param->pBuffers[i].start = v4l2_mmap(NULL, /* start anywhere */
  397. buf.length, PROT_READ | PROT_WRITE, /* required */
  398. MAP_SHARED, /* recommended */
  399. param->fd, buf.m.offset);
  400. if (MAP_FAILED == param->pBuffers[i].start) {
  401. errno_exit("mmap");
  402. }
  403. param->image_size = buf.length; // NOTE: updated value
  404. }
  405. LOG(STF_LEVEL_TRACE, "Exit\n");
  406. }
  407. static void stf_v4l2_dmabufInit(V4l2Param_t *param, int *dmabufs, int count)
  408. {
  409. int i = 0;
  410. struct v4l2_requestbuffers req;
  411. LOG(STF_LEVEL_TRACE, "Enter\n");
  412. CLEAR(req);
  413. req.count = BUFCOUNT; // TODO: modify later
  414. req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  415. req.memory = V4L2_MEMORY_DMABUF;
  416. if (-1 == xioctl(param->fd, VIDIOC_REQBUFS, &req)) {
  417. if (EINVAL == errno) {
  418. fprintf(stderr, "does not support dmabuf\n");
  419. exit(EXIT_FAILURE);
  420. } else {
  421. errno_print("VIDIOC_REQBUFS");
  422. }
  423. }
  424. if (req.count < 2) {
  425. fprintf(stderr, "Insufficient buffer memory\n");
  426. exit(EXIT_FAILURE);
  427. }
  428. param->n_buffers = req.count;
  429. param->pBuffers = calloc(param->n_buffers, sizeof(*param->pBuffers));
  430. if (!param->pBuffers) {
  431. LOG(STF_LEVEL_ERR, "Out of memory\n");
  432. exit(EXIT_FAILURE);
  433. }
  434. for (i = 0; i < req.count; ++i) {
  435. struct v4l2_buffer buf;
  436. CLEAR(buf);
  437. buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  438. buf.memory = V4L2_MEMORY_DMABUF;
  439. buf.index = i;
  440. if (-1 == xioctl(param->fd, VIDIOC_QUERYBUF, &buf)) {
  441. errno_print("VIDIOC_QUERYBUF");
  442. }
  443. param->pBuffers[i].index = buf.index;
  444. param->pBuffers[i].dmabuf_fd = dmabufs[i];
  445. #if 0
  446. param->pBuffers[i].length = buf.length;
  447. param->pBuffers[i].start = mmap(NULL /* start anywhere */,
  448. buf.length,
  449. PROT_READ | PROT_WRITE /* required */,
  450. MAP_SHARED /* recommended */,
  451. dmabufs[i], 0);
  452. if (MAP_FAILED == param->pBuffers[i].start)
  453. errno_print("mmap");
  454. #endif
  455. }
  456. LOG(STF_LEVEL_TRACE, "Exit\n");
  457. }
  458. static void stf_v4l2_userptrInit(V4l2Param_t *param)
  459. {
  460. struct v4l2_requestbuffers req;
  461. unsigned int page_size;
  462. unsigned int buffer_size = param->image_size;
  463. int i = 0;
  464. page_size = getpagesize();
  465. buffer_size = (buffer_size + page_size - 1) & ~(page_size - 1);
  466. CLEAR(req);
  467. req.count = BUFCOUNT;
  468. req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  469. req.memory = V4L2_MEMORY_USERPTR;
  470. if (-1 == xioctl(param->fd, VIDIOC_REQBUFS, &req)) {
  471. if (EINVAL == errno) {
  472. LOG(STF_LEVEL_ERR, "%s does not support user pointer i/o\n", param->device_name);
  473. exit(EXIT_FAILURE);
  474. } else {
  475. errno_exit("VIDIOC_REQBUFS");
  476. }
  477. }
  478. param->n_buffers = 4;
  479. param->pBuffers = calloc(param->n_buffers, sizeof(*param->pBuffers));
  480. if (!param->pBuffers) {
  481. LOG(STF_LEVEL_ERR, "Out of memory\n");
  482. exit(EXIT_FAILURE);
  483. }
  484. for (i = 0; i < param->n_buffers; ++i) {
  485. param->pBuffers[i].length = buffer_size;
  486. param->pBuffers[i].start = memalign(/* boundary */ page_size, buffer_size);
  487. if (!param->pBuffers[i].start) {
  488. LOG(STF_LEVEL_ERR, "Out of memory\n");
  489. exit(EXIT_FAILURE);
  490. }
  491. }
  492. param->image_size = buffer_size;
  493. }
  494. void sft_v4l2_prepare_capturing(V4l2Param_t *param, int *dmabufs, int count)
  495. {
  496. LOG(STF_LEVEL_TRACE, "Enter\n");
  497. switch (param->io_mthd) {
  498. case IO_METHOD_READ:
  499. stf_v4l2_readInit(param);
  500. break;
  501. case IO_METHOD_MMAP:
  502. stf_v4l2_mmapInit(param);
  503. break;
  504. case IO_METHOD_USERPTR:
  505. stf_v4l2_userptrInit(param);
  506. break;
  507. case IO_METHOD_DMABUF:
  508. stf_v4l2_dmabufInit(param, dmabufs, count);
  509. break;
  510. default:
  511. LOG(STF_LEVEL_ERR, "%s does not specify streaming i/o\n", param->device_name);
  512. exit(EXIT_FAILURE);
  513. break;
  514. }
  515. LOG(STF_LEVEL_TRACE, "Exit\n");
  516. }
  517. void sft_v4l2_start_capturing(V4l2Param_t *param)
  518. {
  519. unsigned int i;
  520. enum v4l2_buf_type type;
  521. LOG(STF_LEVEL_TRACE, "Enter\n");
  522. switch (param->io_mthd) {
  523. case IO_METHOD_READ:
  524. /* Nothing to do. */
  525. break;
  526. case IO_METHOD_MMAP:
  527. case IO_METHOD_USERPTR:
  528. case IO_METHOD_DMABUF:
  529. for (i = 0; i < param->n_buffers; ++i) {
  530. stf_v4l2_queue_buffer(param, i);
  531. }
  532. type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  533. if (-1 == xioctl(param->fd, VIDIOC_STREAMON, &type)) {
  534. errno_exit("VIDIOC_STREAMON");
  535. }
  536. break;
  537. default:
  538. break;
  539. }
  540. LOG(STF_LEVEL_TRACE, "Exit\n");
  541. }
  542. void stf_v4l2_stop_capturing(V4l2Param_t *param)
  543. {
  544. enum v4l2_buf_type type;
  545. LOG(STF_LEVEL_TRACE, "Enter\n");
  546. switch (param->io_mthd) {
  547. case IO_METHOD_READ:
  548. /* Nothing to do. */
  549. break;
  550. case IO_METHOD_MMAP:
  551. case IO_METHOD_USERPTR:
  552. case IO_METHOD_DMABUF:
  553. type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  554. if (-1 == xioctl(param->fd, VIDIOC_STREAMOFF, &type))
  555. errno_exit("VIDIOC_STREAMOFF");
  556. break;
  557. default:
  558. break;
  559. }
  560. LOG(STF_LEVEL_TRACE, "Exit\n");
  561. }
  562. // support V4L2_MEMORY_MMAP / V4L2_MEMORY_USERPTR / V4L2_MEMORY_DMABUF
  563. // NOTE: for V4L2_MEMORY_USERPTR, index is pBuffers index
  564. void stf_v4l2_queue_buffer(V4l2Param_t *param, int index)
  565. {
  566. struct v4l2_buffer buf;
  567. LOG(STF_LEVEL_TRACE, "Enter\n");
  568. assert(index < param->n_buffers);
  569. CLEAR(buf);
  570. buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  571. buf.memory = param->mem_type;
  572. buf.index = index; // NOTE: for V4L2_MEMORY_USERPTR not used here?
  573. if (param->mem_type == V4L2_MEMORY_DMABUF) {
  574. //buf.m.fd = param->dmabuf_fd;
  575. buf.m.fd = param->pBuffers[index].dmabuf_fd;
  576. } else if (param->mem_type == V4L2_MEMORY_USERPTR) {
  577. buf.m.userptr = (unsigned long)param->pBuffers[index].start;
  578. buf.length = param->pBuffers[index].length;
  579. }
  580. if (-1 == xioctl(param->fd, VIDIOC_QBUF, &buf)) {
  581. errno_print("VIDIOC_QBUF");
  582. }
  583. LOG(STF_LEVEL_TRACE, "Exit\n");
  584. }
  585. // support V4L2_MEMORY_MMAP / V4L2_MEMORY_USERPTR / V4L2_MEMORY_DMABUF
  586. int stf_v4l2_dequeue_buffer(V4l2Param_t *param, struct v4l2_buffer *buf)
  587. {
  588. int index = 0;
  589. LOG(STF_LEVEL_TRACE, "Enter\n");
  590. PCLEAR(buf);
  591. buf->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  592. buf->memory = param->mem_type;
  593. if (-1 == xioctl(param->fd, VIDIOC_DQBUF, buf)) {
  594. switch (errno) {
  595. case EAGAIN:
  596. return 0;
  597. case EIO:
  598. /* Could ignore EIO, see spec. */
  599. /* fall through */
  600. default:
  601. errno_print("VIDIOC_DQBUF");
  602. }
  603. }
  604. index = buf->index;
  605. if (param->mem_type == V4L2_MEMORY_USERPTR) {
  606. for (index = 0; index < param->n_buffers; ++index) {
  607. if (buf->m.userptr == (unsigned long)param->pBuffers[index].start
  608. && buf->length == param->pBuffers[index].length) {
  609. break;
  610. }
  611. }
  612. }
  613. assert(index < param->n_buffers);
  614. LOG(STF_LEVEL_TRACE, "Exit\n");
  615. return 1;
  616. }