v4l2-ruby 0.9.0

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
data/ext/v4l2/camera.c ADDED
@@ -0,0 +1,1428 @@
1
+ /*
2
+ *
3
+ * Video 4 Linux V2 driver library (camera utility).
4
+ *
5
+ * Copyright (C) 2015 Hiroshi Kuwagata. All rights reserved.
6
+ *
7
+ */
8
+
9
+ /*
10
+ * $Id: camera.c 158 2017-10-26 08:51:55Z pi $
11
+ */
12
+
13
+ #include <stdio.h>
14
+ #include <stdlib.h>
15
+ #include <stdint.h>
16
+ #include <strings.h>
17
+ #include <memory.h>
18
+ #include <errno.h>
19
+ #include <fcntl.h>
20
+ #include <unistd.h>
21
+ #include <pthread.h>
22
+ #include <sys/mman.h>
23
+ #include <sys/time.h>
24
+ #include <sys/types.h>
25
+ #include <sys/ioctl.h>
26
+ #include <sys/stat.h>
27
+
28
+ #include "camera.h"
29
+
30
+ #ifdef RUBY_EXTLIB
31
+ #include <ruby.h>
32
+ #else /* defined(RUBY_EXTLIB) */
33
+ #include <sys/select.h>
34
+ #endif /* defined(RUBY_EXTLIB) */
35
+
36
+ #define BZERO(x) bzero(&x, sizeof(x))
37
+ #define HEXDUMP(x) hexdump(&x, sizeof(x))
38
+ #define N(x) (sizeof(x)/sizeof(*x))
39
+ #define DBG_LOG(...) fprintf(stderr, __VA_ARGS__)
40
+
41
+ #define DEFAULT_FORMAT V4L2_PIX_FMT_MJPEG
42
+ #define DEFAULT_WIDTH 640
43
+ #define DEFAULT_HEIGHT 480
44
+ #define DEFAULT_FRAMERATE_NUM 30
45
+ #define DEFAULT_FRAMERATE_DENOM 1
46
+
47
+ #define NEED_CAPABILITY (V4L2_CAP_VIDEO_CAPTURE|V4L2_CAP_STREAMING)
48
+ #define IS_CAPABLE(x) (((x) & NEED_CAPABILITY) == NEED_CAPABILITY)
49
+
50
+ #define NUM_PLANE 3
51
+ #define CAMERA_UP_PERIOD 15
52
+ #define COPIED 0x8000
53
+
54
+ #define ST_ERROR (-1)
55
+ #define ST_NONE (0) /* fialized */
56
+ #define ST_INITIALIZED (1)
57
+ #define ST_PREPARE (2)
58
+ #define ST_READY (3)
59
+ #define ST_REQUESTED (4)
60
+ #define ST_BREAK (5)
61
+ #define ST_STOPPING (6)
62
+ #define ST_FINALIZED (7)
63
+
64
+ static int xioctl(int fh, int request, void *arg)
65
+ {
66
+ int r;
67
+
68
+ do {
69
+ r = ioctl(fh, request, arg);
70
+ } while (r == -1 && EINTR == errno);
71
+
72
+ return r;
73
+ }
74
+
75
+ static int
76
+ device_open(char* path, int *_fd, char* name, char* driver, char* bus)
77
+ {
78
+ int ret;
79
+ int err;
80
+ int fd;
81
+ struct v4l2_capability cap;
82
+
83
+ do {
84
+ /*
85
+ * entry process
86
+ */
87
+ ret = !0;
88
+ fd = -1;
89
+
90
+ /*
91
+ * device open
92
+ */
93
+ fd = open(path, O_RDWR);
94
+ if (fd < 0) {
95
+ perror("open()");
96
+ break;
97
+ }
98
+
99
+ /*
100
+ * get device capability info
101
+ */
102
+ err = xioctl(fd, VIDIOC_QUERYCAP, &cap);
103
+ if (err < 0) {
104
+ perror("ioctl(VIDIOC_QUERYCAP)");
105
+ break;
106
+ }
107
+
108
+ /*
109
+ * check capability
110
+ */
111
+ if (!IS_CAPABLE(cap.capabilities)) {
112
+ fprintf(stderr, "This device not have camera function that required.");
113
+ break;
114
+ }
115
+
116
+ /*
117
+ * set return parameters
118
+ */
119
+ strncpy( name, (char*)cap.card, N(cap.card));
120
+ strncpy( driver, (char*)cap.driver, N(cap.driver));
121
+ strncpy( bus, (char*)cap.bus_info, N(cap.bus_info));
122
+
123
+ *_fd = fd;
124
+
125
+ /*
126
+ * mark succeed
127
+ */
128
+ ret = 0;
129
+
130
+ } while(0);
131
+
132
+ /*
133
+ * fallback for error occured
134
+ */
135
+ if (ret) {
136
+ if (fd >= 0) close(fd);
137
+ }
138
+
139
+ return ret;
140
+ }
141
+
142
+ static int
143
+ set_format(int fd, uint32_t fcc, int wd, int ht)
144
+ {
145
+ int ret;
146
+ int err;
147
+ struct v4l2_format fmt;
148
+
149
+ ret = 0;
150
+
151
+ BZERO(fmt);
152
+
153
+ fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
154
+ fmt.fmt.pix.width = wd;
155
+ fmt.fmt.pix.height = ht;
156
+ fmt.fmt.pix.pixelformat = fcc;
157
+ fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
158
+
159
+ err = xioctl(fd, VIDIOC_S_FMT, &fmt);
160
+ if (err < 0) {
161
+ perror("ioctl(VIDIOC_S_FMT)");
162
+ ret = !0;
163
+ }
164
+
165
+ return ret;
166
+ }
167
+
168
+ static int
169
+ set_param(int fd, int num, int denom)
170
+ {
171
+ int ret;
172
+ int err;
173
+ struct v4l2_streamparm param;
174
+ struct v4l2_fract* tpf;
175
+
176
+ ret = 0;
177
+
178
+ BZERO(param);
179
+ param.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
180
+ tpf = &param.parm.capture.timeperframe;
181
+ tpf->numerator = num;
182
+ tpf->denominator = denom;
183
+
184
+ err = xioctl(fd, VIDIOC_S_PARM, &param);
185
+ if (err < 0) {
186
+ perror("ioctl(VIDIOC_S_PARM:V4L2_BUF_TYPE_VIDEO_CAPTURE)");
187
+ ret = !0;
188
+ }
189
+
190
+ return ret;
191
+ }
192
+
193
+ static int
194
+ mb_init(mblock_t* mb, int fd, struct v4l2_buffer* buf)
195
+ {
196
+ int ret;
197
+ void* ptr;
198
+
199
+ ret = 0;
200
+ ptr = mmap(NULL, buf->length, PROT_READ, MAP_SHARED, fd, buf->m.offset);
201
+
202
+ if (ptr != NULL) {
203
+ mb->ptr = ptr;
204
+ mb->size = buf->length;
205
+ } else{
206
+ ret = !0;
207
+ }
208
+
209
+ return ret;
210
+ }
211
+
212
+ static void
213
+ mb_copyto(mblock_t* src, void* dst, size_t* used)
214
+ {
215
+ memcpy(dst, src->ptr, src->used);
216
+ *used = src->used;
217
+ }
218
+
219
+ static void
220
+ mb_discard(mblock_t* mb)
221
+ {
222
+ if (mb->ptr != NULL) {
223
+ munmap(mb->ptr, mb->size);
224
+ mb->ptr = NULL;
225
+ }
226
+ }
227
+
228
+ static int
229
+ request_buffer(int fd, int n, mblock_t* mb)
230
+ {
231
+ int ret;
232
+ int err;
233
+ int i;
234
+
235
+ struct v4l2_requestbuffers req;
236
+ struct v4l2_buffer buf;
237
+
238
+ /*
239
+ * initialize
240
+ */
241
+ ret = 0;
242
+
243
+ bzero(mb, sizeof(*mb) * n);
244
+
245
+ /*
246
+ * body
247
+ */
248
+ do {
249
+ /*
250
+ * request for camera buffer allocation
251
+ */
252
+ BZERO(req);
253
+
254
+ req.count = n;
255
+ req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
256
+ req.memory = V4L2_MEMORY_MMAP;
257
+
258
+ err = xioctl(fd, VIDIOC_REQBUFS, &req);
259
+ if (err < 0) {
260
+ perror("ioctl(VIDIOC_REQBUFS)");
261
+ ret = !0;
262
+ break;
263
+ }
264
+
265
+ /*
266
+ * get camera buffer and mapping to user area
267
+ */
268
+ for (i = 0; i < n; i++) {
269
+ BZERO(buf);
270
+
271
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
272
+ buf.memory = V4L2_MEMORY_MMAP;
273
+ buf.index = i;
274
+
275
+ err = xioctl(fd, VIDIOC_QUERYBUF, &buf);
276
+ if (err < 0) {
277
+ perror( "ioctl(VIDIOC_QUERYBUF)");
278
+ ret = !0;
279
+ break;
280
+ }
281
+
282
+ err = mb_init(mb + i, fd, &buf);
283
+ if (err) {
284
+ ret = !0;
285
+ break;
286
+ }
287
+ }
288
+
289
+ if (ret) break;
290
+
291
+ } while (0);
292
+
293
+ /*
294
+ * post process
295
+ */
296
+ if (ret) {
297
+ for (i = 0; i < n; i++) {
298
+ if (mb[i].ptr != NULL) mb_discard(mb + i);
299
+ }
300
+ }
301
+
302
+ return ret;
303
+ }
304
+
305
+ static int
306
+ enqueue_buffer(int fd, int i)
307
+ {
308
+ int err;
309
+ int ret;
310
+ struct v4l2_buffer buf;
311
+
312
+ ret = 0;
313
+
314
+ BZERO(buf);
315
+
316
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
317
+ buf.memory = V4L2_MEMORY_MMAP;
318
+ buf.index = i;
319
+
320
+ err = xioctl(fd, VIDIOC_QBUF, &buf);
321
+ if (err) {
322
+ perror("ioctl(VIDIOC_QBUF)");
323
+ ret = !0;
324
+ }
325
+
326
+ return ret;
327
+ }
328
+
329
+ static int
330
+ start(int fd)
331
+ {
332
+ int ret;
333
+ int err;
334
+ enum v4l2_buf_type typ;
335
+
336
+ ret = 0;
337
+ typ = V4L2_BUF_TYPE_VIDEO_CAPTURE;
338
+
339
+ err = xioctl(fd, VIDIOC_STREAMON, &typ);
340
+ if (err < 0) {
341
+ perror("ioctl(VIDIOC_STREAMON)");
342
+ ret = !0;
343
+ }
344
+
345
+ return ret;
346
+ }
347
+
348
+ static int
349
+ stop(int fd)
350
+ {
351
+ int ret;
352
+ int err;
353
+ enum v4l2_buf_type typ;
354
+
355
+ ret = 0;
356
+ typ = V4L2_BUF_TYPE_VIDEO_CAPTURE;
357
+
358
+ err = xioctl(fd, VIDIOC_STREAMOFF, &typ);
359
+ if (err < 0) {
360
+ perror("ioctl(VIDIOC_STREAMOFF)");
361
+ ret = !0;
362
+ }
363
+
364
+ return ret;
365
+ }
366
+
367
+ static int
368
+ query_captured_buffer(int fd, int* plane, size_t* used)
369
+ {
370
+ int ret;
371
+ int err;
372
+ struct v4l2_buffer buf;
373
+
374
+ ret = 0;
375
+
376
+ BZERO(buf);
377
+
378
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
379
+ buf.memory = V4L2_MEMORY_MMAP;
380
+
381
+ err = xioctl(fd, VIDIOC_DQBUF, &buf);
382
+ if (err < 0) {
383
+ perror( "ioctl(VIDIOC_DQBUF)");
384
+ ret = !0;
385
+ }
386
+
387
+ if (!ret) {
388
+ *plane = buf.index;
389
+ *used = buf.bytesused;
390
+ }
391
+
392
+ return ret;
393
+ }
394
+
395
+ static int
396
+ update_image_size(camera_t* cam)
397
+ {
398
+ int ret;
399
+ size_t size;
400
+
401
+ ret = 0;
402
+
403
+ switch (cam->format) {
404
+ case V4L2_PIX_FMT_NV21:
405
+ size = (cam->width * cam->height * 3) / 2;
406
+ break;
407
+
408
+ case V4L2_PIX_FMT_YUYV:
409
+ case V4L2_PIX_FMT_NV16:
410
+ case V4L2_PIX_FMT_RGB565:
411
+ size = cam->width * cam->height * 2;
412
+ break;
413
+
414
+ case V4L2_PIX_FMT_MJPEG:
415
+ size = cam->width * cam->height;
416
+ break;
417
+
418
+ case V4L2_PIX_FMT_H264:
419
+ size = cam->width * cam->height * 3;
420
+ break;
421
+
422
+ default:
423
+ fprintf(stderr, "update_image_size():unknown image format.\n");
424
+ ret = !0;
425
+ }
426
+
427
+ if (!ret) cam->image_size = size;
428
+
429
+ return ret;
430
+ }
431
+
432
+ static VALUE
433
+ wait_fd(VALUE arg)
434
+ {
435
+ camera_t* cam = (camera_t*)arg;
436
+
437
+ rb_thread_wait_fd(cam->fd);
438
+
439
+ return Qundef;
440
+ }
441
+
442
+ #ifdef RUBY_EXTLIB
443
+ static void
444
+ capture_frame(camera_t* cam)
445
+ {
446
+ int err;
447
+ int plane;
448
+ size_t used;
449
+ int state;
450
+
451
+ errno = 0;
452
+
453
+ // これはwarning対応
454
+ used = 0;
455
+ plane = 0;
456
+
457
+ rb_protect(wait_fd, (VALUE)cam, &state);
458
+
459
+ if (state == 8) {
460
+ cam->state = ST_ERROR;
461
+ rb_jump_tag(state);
462
+
463
+ } else if (state != 0) {
464
+ cam->state = ST_READY;
465
+ rb_jump_tag(state);
466
+
467
+ } else if (errno == EINTR) {
468
+ cam->state = ST_BREAK;
469
+
470
+ } else {
471
+ do {
472
+ err = query_captured_buffer(cam->fd, &plane, &used);
473
+ if (err) {
474
+ cam->state = ST_ERROR;
475
+ break;
476
+ }
477
+
478
+ cam->mb[plane].used = used;
479
+
480
+ if (cam->latest >= 0) {
481
+ err = enqueue_buffer(cam->fd, cam->latest & ~COPIED);
482
+ if (err) {
483
+ cam->state = ST_ERROR;
484
+ break;
485
+ }
486
+ }
487
+
488
+ cam->latest = plane;
489
+
490
+ } while(0);
491
+ }
492
+ }
493
+ #else /* defined(RUBY_EXTLIB_ */
494
+ static void
495
+ capture_frame(camera_t* cam)
496
+ {
497
+ int err;
498
+ int plane;
499
+ size_t used;
500
+ int state;
501
+ fd_set fds;
502
+ size_t used;
503
+
504
+ do {
505
+ // これはwarning対応
506
+ used = 0;
507
+ plane = 0;
508
+
509
+ FD_ZERO(&fds);
510
+ FD_SET(cam->fd, &fds);
511
+ select(cam->fd + 1, &fds, NULL, NULL, NULL);
512
+
513
+ if (errno == EINTR) continue;
514
+
515
+ err = query_captured_buffer(cam->fd, &plane, &used);
516
+ if (err) {
517
+ cam->state = ST_ERROR;
518
+ break;
519
+ }
520
+
521
+ cam->mb[plane].used = used;
522
+
523
+ if (cam->latest >= 0) {
524
+ err = enqueue_buffer(cam->fd, cam->latest & ~COPIED);
525
+ if (err) {
526
+ cam->state = ST_ERROR;
527
+ break;
528
+ }
529
+ }
530
+
531
+ cam->latest = plane;
532
+ } while(0);
533
+ }
534
+ #endif /* defined(RUBY_EXTLIB_ */
535
+
536
+ /*
537
+ * ここからパブリックな関数
538
+ */
539
+
540
+ int
541
+ camera_initialize(camera_t* cam, char* dev)
542
+ {
543
+ int ret;
544
+ int err;
545
+
546
+ do {
547
+ /*
548
+ * entry process
549
+ */
550
+ ret = !0;
551
+
552
+ /*
553
+ * check arguments
554
+ */
555
+ if (cam == NULL) break;
556
+ if (strnlen(dev, N(cam->device)) >= N(cam->device)) break;
557
+
558
+ /*
559
+ * initailize context data
560
+ */
561
+ bzero(cam, sizeof(*cam));
562
+ cam->fd = -1;
563
+
564
+ /*
565
+ * camera open
566
+ */
567
+ err = device_open(dev, &cam->fd, cam->name, cam->driver, cam->bus);
568
+ if (err) break;
569
+
570
+ /*
571
+ * initialize camera context
572
+ */
573
+ strcpy(cam->device, dev);
574
+
575
+ cam->format = DEFAULT_FORMAT;
576
+ cam->width = DEFAULT_WIDTH;
577
+ cam->height = DEFAULT_HEIGHT;
578
+ cam->framerate.num = DEFAULT_FRAMERATE_NUM;
579
+ cam->framerate.denom = DEFAULT_FRAMERATE_DENOM;
580
+
581
+ cam->state = ST_INITIALIZED;
582
+ cam->latest = -1;
583
+
584
+ update_image_size(cam);
585
+
586
+ /*
587
+ * mark succeed
588
+ */
589
+ ret = 0;
590
+
591
+ } while (0);
592
+
593
+ /*
594
+ * fallback for error occured
595
+ */
596
+ if (ret) {
597
+ if (cam->fd >= 0) close(cam->fd);
598
+ }
599
+
600
+ return ret;
601
+ }
602
+
603
+ int
604
+ camera_get_format_desc(camera_t* cam, int i, struct v4l2_fmtdesc* dst)
605
+ {
606
+ int ret;
607
+ int err;
608
+
609
+ do {
610
+ /*
611
+ * entry process
612
+ */
613
+ ret = !0;
614
+
615
+ /*
616
+ * check argument
617
+ */
618
+ if (cam == NULL) break;
619
+ if (dst == NULL) break;
620
+
621
+ /*
622
+ * do query
623
+ */
624
+ memset(dst, 0, sizeof(*dst));
625
+
626
+ dst->index = i;
627
+ dst->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
628
+
629
+ err = xioctl(cam->fd, VIDIOC_ENUM_FMT, dst);
630
+
631
+ if (err) break;
632
+
633
+ /*
634
+ * mark succeed
635
+ */
636
+ ret = 0;
637
+
638
+ } while(0);
639
+
640
+ return ret;
641
+ }
642
+
643
+ int
644
+ camera_get_control_info(camera_t* cam, int ctrl, struct v4l2_queryctrl* dst)
645
+ {
646
+ int ret;
647
+ int err;
648
+
649
+ do {
650
+ /*
651
+ * entry process
652
+ */
653
+ ret = !0;
654
+
655
+ /*
656
+ * check argument
657
+ */
658
+ if (cam == NULL) break;
659
+ if (dst == NULL) break;
660
+
661
+ /*
662
+ * do query
663
+ */
664
+ memset(dst, 0, sizeof(*dst));
665
+
666
+ dst->id = ctrl;
667
+
668
+ err = xioctl(cam->fd, VIDIOC_QUERYCTRL, dst);
669
+ if (err) break;
670
+
671
+ /*
672
+ * mark succeed
673
+ */
674
+ ret = 0;
675
+
676
+ } while(0);
677
+
678
+ return ret;
679
+ }
680
+
681
+ int
682
+ camera_get_frame_size(camera_t* cam, uint32_t fmt, uint32_t idx,
683
+ struct v4l2_frmsizeenum* dst)
684
+ {
685
+ int ret;
686
+ int err;
687
+
688
+ do {
689
+ /*
690
+ * entry process
691
+ */
692
+ ret = !0;
693
+
694
+ /*
695
+ * check argument
696
+ */
697
+ if (cam == NULL) break;
698
+ if (dst == NULL) break;
699
+
700
+ /*
701
+ * do query
702
+ */
703
+ memset(dst, 0, sizeof(*dst));
704
+
705
+ dst->index = idx;
706
+ dst->pixel_format = fmt;
707
+
708
+ err = xioctl(cam->fd, VIDIOC_ENUM_FRAMESIZES, dst);
709
+ if (err) break;
710
+
711
+ /*
712
+ * mark succeed
713
+ */
714
+ ret = 0;
715
+
716
+ } while(0);
717
+
718
+ return ret;
719
+ }
720
+
721
+ int
722
+ camera_get_frame_rate(camera_t* cam, uint32_t fmt,
723
+ uint32_t wd, uint32_t ht, uint32_t idx,
724
+ struct v4l2_frmivalenum* dst)
725
+ {
726
+ int ret;
727
+ int err;
728
+
729
+ do {
730
+ /*
731
+ * entry process
732
+ */
733
+ ret = !0;
734
+
735
+ /*
736
+ * check argument
737
+ */
738
+ if (cam == NULL) break;
739
+ if (dst == NULL) break;
740
+
741
+ /*
742
+ * do query
743
+ */
744
+ memset(dst, 0, sizeof(*dst));
745
+
746
+ dst->index = idx;
747
+ dst->pixel_format = fmt;
748
+ dst->width = wd;
749
+ dst->height = ht;
750
+
751
+ err = xioctl(cam->fd, VIDIOC_ENUM_FRAMEINTERVALS, dst);
752
+ if (err) break;
753
+
754
+ /*
755
+ * mark succeed
756
+ */
757
+ ret = 0;
758
+
759
+ } while(0);
760
+
761
+ return ret;
762
+ }
763
+
764
+ int
765
+ camera_get_menu_item(camera_t* cam, int ctrl, int index,
766
+ struct v4l2_querymenu* dst)
767
+ {
768
+ int ret;
769
+ int err;
770
+
771
+ do {
772
+ /*
773
+ * entry process
774
+ */
775
+ ret = !0;
776
+
777
+ /*
778
+ * check argument
779
+ */
780
+ if (cam == NULL) break;
781
+ if (dst == NULL) break;
782
+
783
+ /*
784
+ * do query
785
+ */
786
+ memset(dst, 0, sizeof(*dst));
787
+
788
+ dst->id = ctrl;
789
+ dst->index = index;
790
+
791
+ err = xioctl(cam->fd, VIDIOC_QUERYMENU, dst);
792
+ if (err) break;
793
+
794
+ /*
795
+ * mark succeed
796
+ */
797
+ ret = 0;
798
+
799
+ } while(0);
800
+
801
+ return ret;
802
+ }
803
+
804
+ int
805
+ camera_get_control(camera_t* cam, uint32_t id, int32_t* dst)
806
+ {
807
+ int ret;
808
+ int err;
809
+ struct v4l2_control ctrl;
810
+
811
+ do {
812
+ /*
813
+ * entry process
814
+ */
815
+ ret = !0;
816
+
817
+ /*
818
+ * check argument
819
+ */
820
+ if (cam == NULL) break;
821
+ if (dst == NULL) break;
822
+
823
+ /*
824
+ * do ioctl
825
+ */
826
+ memset(&ctrl, 0, sizeof(ctrl));
827
+
828
+ ctrl.id = id;
829
+
830
+ err = xioctl(cam->fd, VIDIOC_G_CTRL, &ctrl);
831
+ if (err) {
832
+ perror("ioctl(VIDIOC_S_CTRL)");
833
+ break;
834
+ }
835
+
836
+ /*
837
+ * set return parameter
838
+ */
839
+ *dst = ctrl.value;
840
+
841
+ /*
842
+ * mark succeed
843
+ */
844
+ ret = 0;
845
+
846
+ } while(0);
847
+
848
+ return ret;
849
+ }
850
+
851
+ int
852
+ camera_set_control(camera_t* cam, uint32_t id, int32_t src)
853
+ {
854
+ int ret;
855
+ int err;
856
+ struct v4l2_control ctrl;
857
+
858
+ do {
859
+ /*
860
+ * entry process
861
+ */
862
+ ret = !0;
863
+
864
+ /*
865
+ * check argument
866
+ */
867
+ if (cam == NULL) break;
868
+
869
+ /*
870
+ * do ioctl
871
+ */
872
+ ctrl.id = id;
873
+ ctrl.value = src;
874
+
875
+ err = xioctl(cam->fd, VIDIOC_S_CTRL, &ctrl);
876
+ if (err) {
877
+ perror("ioctl(VIDIOC_S_CTRL)");
878
+ break;
879
+ }
880
+
881
+ /*
882
+ * mark succeed
883
+ */
884
+ ret = 0;
885
+
886
+ } while(0);
887
+
888
+ return ret;
889
+ }
890
+
891
+ int
892
+ camera_set_format(camera_t* cam, uint32_t format)
893
+ {
894
+ int ret;
895
+
896
+ do {
897
+ /*
898
+ * entry process
899
+ */
900
+ ret = !0;
901
+
902
+ /*
903
+ * check argument
904
+ */
905
+ if (cam == NULL) break;
906
+ if (cam->state != ST_INITIALIZED) break;
907
+
908
+ switch (format) {
909
+ case V4L2_PIX_FMT_NV21:
910
+ case V4L2_PIX_FMT_YUYV:
911
+ case V4L2_PIX_FMT_NV16:
912
+ case V4L2_PIX_FMT_RGB565:
913
+ case V4L2_PIX_FMT_MJPEG:
914
+ case V4L2_PIX_FMT_H264:
915
+ break;
916
+
917
+ default:
918
+ fprintf(stderr, "camera_set_format():unknown image format.\n");
919
+ goto out;
920
+ }
921
+
922
+ /*
923
+ * update camera context
924
+ */
925
+ cam->format = format;
926
+ update_image_size(cam);
927
+
928
+ /*
929
+ * mark succeed
930
+ */
931
+ ret = 0;
932
+ } while(0);
933
+
934
+ out:
935
+
936
+ return ret;
937
+ }
938
+
939
+ int
940
+ camera_set_framerate(camera_t* cam, int num, int denom)
941
+ {
942
+ int ret;
943
+
944
+ do {
945
+ /*
946
+ * entry process
947
+ */
948
+ ret = !0;
949
+
950
+ /*
951
+ * check argunments
952
+ */
953
+ if (cam == NULL) break;
954
+ if (cam->state != ST_INITIALIZED) break;
955
+
956
+ /*
957
+ * update camera context
958
+ */
959
+ cam->framerate.num = num;
960
+ cam->framerate.denom = denom;
961
+
962
+ /*
963
+ * mark succeed
964
+ */
965
+ ret = 0;
966
+
967
+ } while (0);
968
+
969
+ return ret;
970
+ }
971
+
972
+ int
973
+ camera_get_image_width(camera_t* cam, int* width)
974
+ {
975
+ int ret;
976
+
977
+ do {
978
+ /*
979
+ * entry process
980
+ */
981
+ ret = !0;
982
+
983
+ /*
984
+ * check argunments
985
+ */
986
+ if (cam == NULL) break;
987
+
988
+ /*
989
+ * set return paramater
990
+ */
991
+ *width = cam->width;
992
+
993
+ /*
994
+ * mark succeed
995
+ */
996
+ ret = 0;
997
+
998
+ } while (0);
999
+
1000
+ return ret;
1001
+ }
1002
+
1003
+ int
1004
+ camera_set_image_width(camera_t* cam, int width)
1005
+ {
1006
+ int ret;
1007
+
1008
+ do {
1009
+ /*
1010
+ * entry process
1011
+ */
1012
+ ret = !0;
1013
+
1014
+ /*
1015
+ * check argunments
1016
+ */
1017
+ if (cam == NULL) break;
1018
+ if (cam->state != ST_INITIALIZED) break;
1019
+
1020
+ /*
1021
+ * update camera context
1022
+ */
1023
+ cam->width = width;
1024
+ update_image_size(cam);
1025
+
1026
+ /*
1027
+ * mark succeed
1028
+ */
1029
+ ret = 0;
1030
+
1031
+ } while (0);
1032
+
1033
+ return ret;
1034
+ }
1035
+
1036
+ int
1037
+ camera_get_image_height(camera_t* cam, int* height)
1038
+ {
1039
+ int ret;
1040
+
1041
+ do {
1042
+ /*
1043
+ * entry process
1044
+ */
1045
+ ret = !0;
1046
+
1047
+ /*
1048
+ * check argunments
1049
+ */
1050
+ if (cam == NULL) break;
1051
+
1052
+ /*
1053
+ * set return paramater
1054
+ */
1055
+ *height = cam->height;
1056
+
1057
+ /*
1058
+ * mark succeed
1059
+ */
1060
+ ret = 0;
1061
+
1062
+ } while (0);
1063
+
1064
+ return ret;
1065
+ }
1066
+
1067
+
1068
+ int
1069
+ camera_set_image_height(camera_t* cam, int height)
1070
+ {
1071
+ int ret;
1072
+
1073
+ do {
1074
+ /*
1075
+ * entry process
1076
+ */
1077
+ ret = !0;
1078
+
1079
+ /*
1080
+ * check argunments
1081
+ */
1082
+ if (cam == NULL) break;
1083
+ if (cam->state != ST_INITIALIZED) break;
1084
+
1085
+ /*
1086
+ * update camera context
1087
+ */
1088
+ cam->height = height;
1089
+ update_image_size(cam);
1090
+
1091
+ /*
1092
+ * mark succeed
1093
+ */
1094
+ ret = 0;
1095
+
1096
+ } while (0);
1097
+
1098
+ return ret;
1099
+ }
1100
+
1101
+ int
1102
+ camera_finalize(camera_t* cam)
1103
+ {
1104
+ int ret;
1105
+ int i;
1106
+
1107
+ do {
1108
+ /*
1109
+ * entry process
1110
+ */
1111
+ ret = !0;
1112
+
1113
+ /*
1114
+ * check argument
1115
+ */
1116
+ if (cam == NULL) break;
1117
+ if (cam->state != ST_INITIALIZED && cam->state != ST_ERROR) break;
1118
+
1119
+ if (cam->state != ST_ERROR) {
1120
+ /*
1121
+ * closing camera device
1122
+ */
1123
+ close(cam->fd);
1124
+ for (i = 0; i < NUM_PLANE; i++) mb_discard(cam->mb + i);
1125
+ }
1126
+
1127
+ /*
1128
+ * destroy camera context
1129
+ */
1130
+ bzero(cam, sizeof(*cam));
1131
+
1132
+ cam->fd = -1;
1133
+ cam->state = ST_FINALIZED;
1134
+
1135
+ /*
1136
+ * mark succeed
1137
+ */
1138
+ ret = 0;
1139
+
1140
+ } while (0);
1141
+
1142
+ return ret;
1143
+ }
1144
+
1145
+ int
1146
+ camera_start(camera_t* cam)
1147
+ {
1148
+ int ret;
1149
+ int err;
1150
+ int i;
1151
+
1152
+ do {
1153
+ /*
1154
+ * entry process
1155
+ */
1156
+ ret = !0;
1157
+
1158
+ /*
1159
+ * check argunments
1160
+ */
1161
+ if (cam == NULL) break;
1162
+ if (cam->state != ST_INITIALIZED) break;
1163
+
1164
+ /*
1165
+ * setup for camera device
1166
+ */
1167
+ err = set_format(cam->fd, cam->format, cam->width, cam->height);
1168
+ if (err) break;
1169
+
1170
+ err = set_param(cam->fd, cam->framerate.num, cam->framerate.denom);
1171
+ if (err) break;
1172
+
1173
+ err = request_buffer(cam->fd, NUM_PLANE, cam->mb);
1174
+ if (err) break;
1175
+
1176
+ for (i = 0; i < NUM_PLANE; i++) {
1177
+ err = enqueue_buffer(cam->fd, i);
1178
+ if (err) break;
1179
+ }
1180
+ if (err) break;
1181
+
1182
+
1183
+ /*
1184
+ * start image capture
1185
+ */
1186
+ cam->state = ST_PREPARE;
1187
+
1188
+ err = start(cam->fd);
1189
+ if (err) break;
1190
+
1191
+ cam->state = ST_READY;
1192
+
1193
+ /*
1194
+ * mark succeed
1195
+ */
1196
+ ret = 0;
1197
+
1198
+ } while (0);
1199
+
1200
+ if (ret) {
1201
+ /*
1202
+ * post process on error occured.
1203
+ */
1204
+ if (cam->state == ST_PREPARE) {
1205
+ cam->state = ST_INITIALIZED;
1206
+ }
1207
+
1208
+ /*
1209
+ * 以下の資源を確保したままになっているかもしれないので解放しておく
1210
+ * - ファイルハンドル
1211
+ * - リクエストバッファ
1212
+ */
1213
+ if (cam->fd >= 0) {
1214
+ close(cam->fd);
1215
+ cam->fd = -1;
1216
+ }
1217
+
1218
+ for (i = 0; i < NUM_PLANE; i++) mb_discard(cam->mb + i);
1219
+ }
1220
+
1221
+ return ret;
1222
+ }
1223
+
1224
+ int
1225
+ camera_stop(camera_t* cam)
1226
+ {
1227
+ int ret;
1228
+ int err;
1229
+ int i;
1230
+
1231
+ do {
1232
+ /*
1233
+ * entry process
1234
+ */
1235
+ ret = !0;
1236
+
1237
+ /*
1238
+ * check argument
1239
+ */
1240
+ if (cam == NULL) break;
1241
+
1242
+ /*
1243
+ * stop image capture
1244
+ */
1245
+ if (cam->state != ST_PREPARE && cam->state != ST_READY &&
1246
+ cam->state != ST_ERROR) {
1247
+ break;
1248
+ }
1249
+
1250
+ if (cam->state != ST_ERROR) {
1251
+ cam->state = ST_STOPPING;
1252
+
1253
+ if (cam->state != ST_ERROR) {
1254
+ err = stop(cam->fd);
1255
+ if (err) break;
1256
+ }
1257
+
1258
+ /*
1259
+ * device reset
1260
+ */
1261
+ close(cam->fd);
1262
+ for (i = 0; i < NUM_PLANE; i++) mb_discard(cam->mb + i);
1263
+
1264
+ cam->fd = open(cam->device, O_RDWR);
1265
+ cam->state = ST_INITIALIZED;
1266
+ cam->latest = -1;
1267
+
1268
+ } else {
1269
+ /*
1270
+ * エラーの場合はオブジェクトの再利用を許さない。
1271
+ * 再オープンも初期化も行わず、デバイスのクロー
1272
+ * ズとリソースの解放のみを行う。
1273
+ */
1274
+ close(cam->fd);
1275
+ for (i = 0; i < NUM_PLANE; i++) mb_discard(cam->mb + i);
1276
+
1277
+ cam->latest = -1;
1278
+ }
1279
+
1280
+ /*
1281
+ * mark succeed
1282
+ */
1283
+ ret = 0;
1284
+
1285
+ } while (0);
1286
+
1287
+ return ret;
1288
+ }
1289
+
1290
+ int
1291
+ camera_get_image(camera_t* cam, void* ptr, size_t* used)
1292
+ {
1293
+ int ret;
1294
+
1295
+ do {
1296
+ /*
1297
+ * entry process
1298
+ */
1299
+ ret = !0;
1300
+
1301
+ /*
1302
+ * check arguments
1303
+ */
1304
+ if (cam == NULL) break;
1305
+
1306
+ /*
1307
+ * check statement
1308
+ */
1309
+ if (cam->state != ST_PREPARE && cam->state != ST_READY) {
1310
+ break;
1311
+ }
1312
+
1313
+ /*
1314
+ * image copy
1315
+ */
1316
+
1317
+ // 状態を変更
1318
+ cam->state = ST_REQUESTED;
1319
+
1320
+ // フレームをキャプチャー。フレームが届くまで待つ。
1321
+ capture_frame(cam);
1322
+
1323
+ if (cam->state == ST_ERROR) {
1324
+ // エラーが発生した場合はエラー状態を保持したまま処理をスキップする
1325
+ break;
1326
+ }
1327
+
1328
+ if (cam->state == ST_BREAK) {
1329
+ // 割り込まれていた場合は、状態を元に戻して処理をスキップする
1330
+ cam->state = ST_READY;
1331
+ break;
1332
+ }
1333
+
1334
+ // フレームをコピー
1335
+ mb_copyto(cam->mb + cam->latest, ptr, used);
1336
+
1337
+ // コピー済みであることをマーク
1338
+ cam->latest |= COPIED;
1339
+
1340
+ // 状態を元に戻す
1341
+ cam->state = ST_READY;
1342
+
1343
+ /*
1344
+ * mark succeed
1345
+ */
1346
+ ret = 0;
1347
+
1348
+ } while (0);
1349
+
1350
+ return ret;
1351
+ }
1352
+
1353
+ int
1354
+ camera_check_busy(camera_t* cam, int* busy)
1355
+ {
1356
+ int ret;
1357
+ int err;
1358
+ struct v4l2_format fmt;
1359
+
1360
+ do {
1361
+ /*
1362
+ * entry process
1363
+ */
1364
+ ret = !0;
1365
+
1366
+ /*
1367
+ * check arguments
1368
+ */
1369
+ if (cam == NULL) break;
1370
+ if (busy == NULL) break;
1371
+
1372
+ /*
1373
+ * do check (try set format)
1374
+ */
1375
+ BZERO(fmt);
1376
+
1377
+ fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1378
+ fmt.fmt.pix.width = cam->width;
1379
+ fmt.fmt.pix.height = cam->height;
1380
+ fmt.fmt.pix.pixelformat = cam->format;
1381
+ fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
1382
+
1383
+ err = xioctl(cam->fd, VIDIOC_S_FMT, &fmt);
1384
+ if (err >= 0) {
1385
+ *busy = 0;
1386
+ ret = 0;
1387
+
1388
+ } else if (errno == EBUSY) {
1389
+ *busy = !0;
1390
+ ret = 0;
1391
+
1392
+ } else {
1393
+ perror("ioctl(VIDIOC_S_FMT)");
1394
+ }
1395
+ } while(0);
1396
+
1397
+ return ret;
1398
+ }
1399
+
1400
+ int
1401
+ camera_check_error(camera_t* cam, int* error)
1402
+ {
1403
+ int ret;
1404
+ int err;
1405
+ struct v4l2_format fmt;
1406
+
1407
+ do {
1408
+ /*
1409
+ * entry process
1410
+ */
1411
+ ret = !0;
1412
+
1413
+ /*
1414
+ * check arguments
1415
+ */
1416
+ if (cam == NULL) break;
1417
+ if (error == NULL) break;
1418
+
1419
+ /*
1420
+ * do check (check state)
1421
+ */
1422
+ *error = (cam->state == ST_ERROR);
1423
+
1424
+ ret = 0;
1425
+ } while(0);
1426
+
1427
+ return ret;
1428
+ }