getframe.c 7.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293
  1. #include <libmegapixels.h>
  2. #include <stdio.h>
  3. #include <limits.h>
  4. #include <string.h>
  5. #include <linux/videodev2.h>
  6. #include <sys/ioctl.h>
  7. #include <stdlib.h>
  8. #include <sys/types.h>
  9. #include <errno.h>
  10. #include <sys/mman.h>
  11. #include <getopt.h>
  12. #include <ctype.h>
  13. struct buffer {
  14. void *start;
  15. size_t length;
  16. };
  17. struct buffer *buffers;
  18. int
  19. xioctl(int fd, int request, void *arg)
  20. {
  21. int r;
  22. do {
  23. r = ioctl(fd, request, arg);
  24. } while (r == -1 && errno == EINTR);
  25. return r;
  26. }
  27. void
  28. usage(char *name)
  29. {
  30. fprintf(stderr, "Usage: %s [-h] [-n offset] [-c camera] [-o file]\n", name);
  31. fprintf(stderr, "Get a frame using libmegapixels\n\n");
  32. fprintf(stderr, "Arguments:\n");
  33. fprintf(stderr, " -n offset Get the Nth frame from the output (defaults to 5)\n");
  34. fprintf(stderr, " -c camera Use a specific camera number\n");
  35. fprintf(stderr, " -m modenum Use another camera mode than the first\n");
  36. fprintf(stderr, " -o file File to store the frame in\n");
  37. fprintf(stderr, " -v Show verbose debug info\n");
  38. fprintf(stderr, " -h Display this help text\n");
  39. }
  40. int
  41. main(int argc, char *argv[])
  42. {
  43. int c;
  44. int camera_id = 0;
  45. long res;
  46. char *end;
  47. char *outfile = NULL;
  48. int count = 5;
  49. int mode_idx = 0;
  50. int verbose = 0;
  51. while ((c = getopt(argc, argv, "hvc:n:o:m:")) != -1) {
  52. switch (c) {
  53. case 'c':
  54. res = strtol(optarg, &end, 10);
  55. if (end == optarg || end == NULL || *end != '\0') {
  56. fprintf(stderr, "Invalid number for -c\n");
  57. return 1;
  58. }
  59. camera_id = (int) res;
  60. break;
  61. case 'o':
  62. outfile = optarg;
  63. break;
  64. case 'n':
  65. res = strtol(optarg, &end, 10);
  66. if (end == optarg || end == NULL || *end != '\0') {
  67. fprintf(stderr, "Invalid number for -n\n");
  68. return 1;
  69. }
  70. count = (int) res;
  71. break;
  72. case 'm':
  73. res = strtol(optarg, &end, 10);
  74. if (end == optarg || end == NULL || *end != '\0') {
  75. fprintf(stderr, "Invalid number for -m\n");
  76. return 1;
  77. }
  78. mode_idx = (int) res;
  79. break;
  80. case 'h':
  81. usage(argv[0]);
  82. return 0;
  83. break;
  84. case 'v':
  85. verbose = 1;
  86. break;
  87. case '?':
  88. if (optopt == 'd' || optopt == 'l') {
  89. fprintf(stderr, "Option -%c requires an argument.\n", optopt);
  90. } else if (isprint(optopt)) {
  91. fprintf(stderr, "Unknown option '-%c'\n", optopt);
  92. } else {
  93. fprintf(stderr, "Unknown option character x%x\n", optopt);
  94. }
  95. return 1;
  96. default:
  97. usage(argv[0]);
  98. return 1;
  99. }
  100. }
  101. char configpath[PATH_MAX];
  102. int ret = libmegapixels_find_config(configpath);
  103. libmegapixels_devconfig *config = {0};
  104. libmegapixels_init(&config);
  105. if (verbose) {
  106. libmegapixels_loglevel(2);
  107. }
  108. if (ret) {
  109. printf("Using config: %s\n", configpath);
  110. libmegapixels_load_file(config, configpath);
  111. } else {
  112. fprintf(stderr, "No config found for this device\n");
  113. }
  114. libmegapixels_load_uvc(config);
  115. if (config->count == 0) {
  116. fprintf(stderr, "No valid camera configuration\n");
  117. return 1;
  118. }
  119. if (camera_id > config->count - 1) {
  120. fprintf(stderr, "Camera id %d does not exist\n", camera_id);
  121. return 1;
  122. }
  123. libmegapixels_camera *camera = config->cameras[camera_id];
  124. if (libmegapixels_open(camera) != 0) {
  125. fprintf(stderr, "Could not open default camera\n");
  126. return 1;
  127. }
  128. if (mode_idx > camera->num_modes) {
  129. fprintf(stderr, "Invalid mode index: %d\n", mode_idx);
  130. }
  131. libmegapixels_mode *mode = camera->modes[mode_idx];
  132. struct v4l2_format format = {0};
  133. unsigned int frame_size = libmegapixels_select_mode(camera, mode, &format);
  134. if (frame_size == 0) {
  135. fprintf(stderr, "Could not select mode\n");
  136. return 1;
  137. }
  138. // Do the reqular V4L2 stuff to get a frame
  139. struct v4l2_capability cap;
  140. int mplanes = 0;
  141. enum v4l2_buf_type buftype = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  142. if (ioctl(camera->video_fd, VIDIOC_QUERYCAP, &cap) != 0) {
  143. fprintf(stderr, "VIDIOC_QUERYCAP failed: %s\n", strerror(errno));
  144. return 1;
  145. }
  146. if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
  147. fprintf(stderr, "Device does not support streaming i/o\n");
  148. return 1;
  149. }
  150. if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
  151. if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE) {
  152. mplanes = 1;
  153. buftype = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
  154. } else {
  155. fprintf(stderr, "Device does not support V4L2_CAP_VIDEO_CAPTURE\n");
  156. return 1;
  157. }
  158. }
  159. struct v4l2_requestbuffers req = {0};
  160. req.count = 4;
  161. req.type = buftype;
  162. req.memory = V4L2_MEMORY_MMAP;
  163. if (xioctl(camera->video_fd, VIDIOC_REQBUFS, &req) == -1) {
  164. fprintf(stderr, "VIDIOC_REQBUFS failed: %s\n", strerror(errno));
  165. return 1;
  166. }
  167. buffers = calloc(req.count, sizeof(*buffers));
  168. for (int i = 0; i < req.count; i++) {
  169. struct v4l2_buffer buf = {0};
  170. buf.type = buftype;
  171. buf.memory = V4L2_MEMORY_MMAP;
  172. buf.index = i;
  173. struct v4l2_plane planes[1];
  174. if (mplanes) {
  175. buf.m.planes = planes;
  176. buf.length = 1;
  177. }
  178. if (xioctl(camera->video_fd, VIDIOC_QUERYBUF, &buf) == -1) {
  179. fprintf(stderr, "VIDIOC_QUERYBUF failed: %s\n", strerror(errno));
  180. return 1;
  181. }
  182. unsigned int offset;
  183. if (mplanes) {
  184. buffers[i].length = planes[0].length;
  185. offset = planes[0].m.mem_offset;
  186. } else {
  187. buffers[i].length = buf.length;
  188. offset = buf.m.offset;
  189. }
  190. buffers[i].start = mmap(NULL, buffers[i].length, PROT_READ | PROT_WRITE, MAP_SHARED, camera->video_fd, offset);
  191. if (buffers[i].start == MAP_FAILED) {
  192. fprintf(stderr, "mmap() failed\n");
  193. return 1;
  194. }
  195. }
  196. for (int i = 0; i < req.count; i++) {
  197. struct v4l2_buffer qbuf = {0};
  198. qbuf.type = buftype;
  199. qbuf.memory = V4L2_MEMORY_MMAP;
  200. qbuf.index = i;
  201. if (mplanes) {
  202. struct v4l2_plane qplanes[1];
  203. qbuf.m.planes = qplanes;
  204. qbuf.length = 1;
  205. }
  206. if (xioctl(camera->video_fd, VIDIOC_QBUF, &qbuf) == -1) {
  207. fprintf(stderr, "VIDIOC_QBUF failed: %s\n", strerror(errno));
  208. return 1;
  209. }
  210. }
  211. enum v4l2_buf_type type = buftype;
  212. if (xioctl(camera->video_fd, VIDIOC_STREAMON, &type) == -1) {
  213. fprintf(stderr, "VIDIOC_STREAMON failed: %s\n", strerror(errno));
  214. return 1;
  215. }
  216. while (count-- > 0) {
  217. while (1) {
  218. fd_set(fds);
  219. FD_ZERO(&fds);
  220. FD_SET(camera->video_fd, &fds);
  221. int sr = select(FD_SETSIZE, &fds, NULL, NULL, NULL);
  222. if (sr == -1) {
  223. if (errno == EINTR) {
  224. count++;
  225. continue;
  226. }
  227. fprintf(stderr, "select() failed: %s\n", strerror(errno));
  228. return 1;
  229. }
  230. struct v4l2_buffer buf = {0};
  231. buf.type = buftype;
  232. buf.memory = V4L2_MEMORY_MMAP;
  233. if (mplanes) {
  234. struct v4l2_plane dqplanes[1];
  235. buf.m.planes = dqplanes;
  236. buf.length = 1;
  237. }
  238. if (xioctl(camera->video_fd, VIDIOC_DQBUF, &buf) == -1) {
  239. fprintf(stderr, "VIDIOC_DQBUF failed\n");
  240. return 1;
  241. }
  242. fprintf(stderr, "received frame\n");
  243. if (count == 0 && outfile != NULL) {
  244. FILE *fp = fopen(outfile, "w");
  245. fwrite(buffers[buf.index].start, buffers[buf.index].length, 1, fp);
  246. fclose(fp);
  247. printf("Stored frame to: %s\n", outfile);
  248. printf("Format: %dx%d\n", mode->width, mode->height);
  249. char fourcc[5] = {0};
  250. fourcc[0] = (char) (mode->v4l_pixfmt & 0xff);
  251. fourcc[1] = (char) ((mode->v4l_pixfmt >> 8) & 0xff);
  252. fourcc[2] = (char) ((mode->v4l_pixfmt >> 16) & 0xff);
  253. fourcc[3] = (char) ((mode->v4l_pixfmt >> 24) & 0xff);
  254. printf("Pixfmt: %s\n", fourcc);
  255. }
  256. if (xioctl(camera->video_fd, VIDIOC_QBUF, &buf) == -1) {
  257. fprintf(stderr, "VIDIOC_DQBUF failed\n");
  258. return 1;
  259. }
  260. break;
  261. }
  262. }
  263. return 0;
  264. }