1 #include <unistd.h>
2 #include <stdlib.h>
3 #include <stdio.h>
4 #include <string.h>
5 #include <inttypes.h>
6 #include <getopt.h>
7 #include <sys/types.h>
8 #include <sys/stat.h>
9 #include <fcntl.h>
10 #include <ctype.h>
11 #include <errno.h>
12 #include <sys/ioctl.h>
13 #include <sys/time.h>
14 #include <dirent.h>
15 #include <math.h>
16 #include <cstdlib>
17
18 #if defined(__DragonFly__)
19 #include <sys/endian.h> // for le32toh()
20 #endif
21
22 #include "v4l2-ctl.h"
23
24 static struct v4l2_format vfmt; /* set_format/get_format */
25
meta_usage()26 void meta_usage()
27 {
28 printf("\nMetadata Formats options:\n"
29 " --list-formats-meta display supported metadata capture formats [VIDIOC_ENUM_FMT]\n"
30 " --get-fmt-meta query the metadata capture format [VIDIOC_G_FMT]\n"
31 " --set-fmt-meta <f> set the metadata capture format [VIDIOC_S_FMT]\n"
32 " parameter is either the format index as reported by\n"
33 " --list-formats-meta, or the fourcc value as a string\n"
34 " --try-fmt-meta <f> try the metadata capture format [VIDIOC_TRY_FMT]\n"
35 " parameter is either the format index as reported by\n"
36 " --list-formats-meta, or the fourcc value as a string\n"
37 " --list-formats-meta-out display supported metadata output formats [VIDIOC_ENUM_FMT]\n"
38 " --get-fmt-meta-out query the metadata output format [VIDIOC_G_FMT]\n"
39 " --set-fmt-meta-out <f> set the metadata output format [VIDIOC_S_FMT]\n"
40 " parameter is either the format index as reported by\n"
41 " --list-formats-meta-out, or the fourcc value as a string\n"
42 " --try-fmt-meta-out <f> try the metadata output format [VIDIOC_TRY_FMT]\n"
43 " parameter is either the format index as reported by\n"
44 " --list-formats-meta-out, or the fourcc value as a string\n"
45 );
46 }
47
meta_cmd(int ch,char * optarg)48 void meta_cmd(int ch, char *optarg)
49 {
50 switch (ch) {
51 case OptSetMetaFormat:
52 case OptTryMetaFormat:
53 case OptSetMetaOutFormat:
54 case OptTryMetaOutFormat:
55 if (strlen(optarg) == 0) {
56 meta_usage();
57 std::exit(EXIT_FAILURE);
58 } else if (strlen(optarg) == 4) {
59 vfmt.fmt.meta.dataformat = v4l2_fourcc(optarg[0],
60 optarg[1], optarg[2], optarg[3]);
61 } else {
62 vfmt.fmt.meta.dataformat = strtol(optarg, 0L, 0);
63 }
64 break;
65 }
66 }
67
__meta_set(cv4l_fd & _fd,bool set,bool _try,uint32_t type)68 static void __meta_set(cv4l_fd &_fd, bool set, bool _try, uint32_t type)
69 {
70 struct v4l2_format in_vfmt;
71 int fd = _fd.g_fd();
72 int ret;
73
74 if (!set && !_try)
75 return;
76
77 in_vfmt.type = type;
78 in_vfmt.fmt.meta.dataformat = vfmt.fmt.meta.dataformat;
79
80 if (in_vfmt.fmt.meta.dataformat < 256) {
81 struct v4l2_fmtdesc fmt = {};
82
83 fmt.index = in_vfmt.fmt.meta.dataformat;
84 fmt.type = in_vfmt.type;
85
86 if (doioctl(fd, VIDIOC_ENUM_FMT, &fmt))
87 fmt.pixelformat = 0;
88
89 in_vfmt.fmt.meta.dataformat = fmt.pixelformat;
90 }
91
92 if (set)
93 ret = doioctl(fd, VIDIOC_S_FMT, &in_vfmt);
94 else
95 ret = doioctl(fd, VIDIOC_TRY_FMT, &in_vfmt);
96 if (ret == 0 && (verbose || _try))
97 printfmt(fd, in_vfmt);
98 }
99
meta_set(cv4l_fd & _fd)100 void meta_set(cv4l_fd &_fd)
101 {
102 __meta_set(_fd, options[OptSetMetaFormat], options[OptTryMetaFormat],
103 V4L2_BUF_TYPE_META_CAPTURE);
104 __meta_set(_fd, options[OptSetMetaOutFormat],
105 options[OptTryMetaOutFormat], V4L2_BUF_TYPE_META_OUTPUT);
106 }
107
__meta_get(cv4l_fd & fd,uint32_t type)108 void __meta_get(cv4l_fd &fd, uint32_t type)
109 {
110 vfmt.type = type;
111 if (doioctl(fd.g_fd(), VIDIOC_G_FMT, &vfmt) == 0)
112 printfmt(fd.g_fd(), vfmt);
113 }
114
meta_get(cv4l_fd & fd)115 void meta_get(cv4l_fd &fd)
116 {
117 if (options[OptGetMetaFormat])
118 __meta_get(fd, V4L2_BUF_TYPE_META_CAPTURE);
119 if (options[OptGetMetaOutFormat])
120 __meta_get(fd, V4L2_BUF_TYPE_META_OUTPUT);
121 }
122
meta_list(cv4l_fd & fd)123 void meta_list(cv4l_fd &fd)
124 {
125 if (options[OptListMetaFormats]) {
126 printf("ioctl: VIDIOC_ENUM_FMT\n");
127 print_video_formats(fd, V4L2_BUF_TYPE_META_CAPTURE);
128 }
129
130 if (options[OptListMetaOutFormats]) {
131 printf("ioctl: VIDIOC_ENUM_FMT\n");
132 print_video_formats(fd, V4L2_BUF_TYPE_META_OUTPUT);
133 }
134 }
135
136 struct vivid_uvc_meta_buf {
137 uint64_t ns;
138 uint16_t sof;
139 uint8_t length;
140 uint8_t flags;
141 uint8_t buf[10];
142 };
143
144 #define UVC_STREAM_SCR (1 << 3)
145 #define UVC_STREAM_PTS (1 << 2)
146
147 struct vivid_meta_out_buf {
148 uint16_t brightness;
149 uint16_t contrast;
150 uint16_t saturation;
151 int16_t hue;
152 };
153
print_meta_buffer(FILE * f,cv4l_buffer & buf,cv4l_fmt & fmt,cv4l_queue & q)154 void print_meta_buffer(FILE *f, cv4l_buffer &buf, cv4l_fmt &fmt, cv4l_queue &q)
155 {
156 struct vivid_uvc_meta_buf *vbuf;
157 int buf_off = 0;
158 struct vivid_meta_out_buf *vbuf_out;
159
160 switch (fmt.g_pixelformat()) {
161 case V4L2_META_FMT_UVC:
162 fprintf(f, "UVC: ");
163 vbuf = static_cast<vivid_uvc_meta_buf *>(q.g_dataptr(buf.g_index(), 0));
164
165 fprintf(f, "%.6fs sof: %4d len: %u flags: 0x%02x",
166 static_cast<double>(vbuf->ns) / 1000000000.0,
167 vbuf->sof,
168 vbuf->length,
169 vbuf->flags);
170 if (vbuf->flags & UVC_STREAM_PTS) {
171 fprintf(f, " PTS: %u", le32toh(*(uint32_t*)(vbuf->buf)));
172 buf_off = 4;
173 }
174 if (vbuf->flags & UVC_STREAM_SCR)
175 fprintf(f, " STC: %u SOF counter: %u",
176 le32toh(*(uint32_t*)(vbuf->buf + buf_off)),
177 le16toh(*(uint16_t*)(vbuf->buf + buf_off + 4)));
178 fprintf(f, "\n");
179 break;
180 case V4L2_META_FMT_VIVID:
181 fprintf(f, "VIVID:");
182 vbuf_out = static_cast<vivid_meta_out_buf *>(q.g_dataptr(buf.g_index(), 0));
183
184 fprintf(f, " brightness: %u contrast: %u saturation: %u hue: %d\n",
185 vbuf_out->brightness, vbuf_out->contrast,
186 vbuf_out->saturation, vbuf_out->hue);
187 break;
188 }
189 }
190
meta_fillbuffer(cv4l_buffer & buf,cv4l_fmt & fmt,cv4l_queue & q)191 void meta_fillbuffer(cv4l_buffer &buf, cv4l_fmt &fmt, cv4l_queue &q)
192 {
193 struct vivid_meta_out_buf *vbuf;
194
195 switch (fmt.g_pixelformat()) {
196 case V4L2_META_FMT_VIVID:
197 vbuf = static_cast<vivid_meta_out_buf *>(q.g_dataptr(buf.g_index(), 0));
198 vbuf->brightness = buf.g_sequence() % 192 + 64;
199 vbuf->contrast = (buf.g_sequence() + 10) % 192 + 64;
200 vbuf->saturation = (buf.g_sequence() + 20) % 256;
201 vbuf->hue = buf.g_sequence() % 257 - 128;
202 break;
203 }
204 }
205