1 /*############################################################################
2 # Copyright (C) 2005 Intel Corporation
3 #
4 # SPDX-License-Identifier: MIT
5 ############################################################################*/
6
7 #if defined(ENABLE_V4L2_SUPPORT)
8
9 #include "v4l2_util.h"
10 #include <assert.h>
11 #include <fcntl.h>
12 #include <linux/videodev2.h>
13 #include <poll.h>
14 #include <pthread.h>
15 #include <signal.h>
16 #include <stdio.h>
17 #include <stdlib.h>
18 #include <sys/ioctl.h>
19
20 /* Global Declaration */
21 Buffer *buffers, *CurBuffers;
22 bool CtrlFlag = false;
23 int m_q[5], m_first = 0, m_last = 0, m_numInQ = 0;
24 pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
25 pthread_mutex_t empty = PTHREAD_MUTEX_INITIALIZER;
26
v4l2Device(const char * devname,uint32_t width,uint32_t height,uint32_t num_buffers,enum AtomISPMode MipiMode,enum V4L2PixelFormat v4l2Format)27 v4l2Device::v4l2Device(const char* devname,
28 uint32_t width,
29 uint32_t height,
30 uint32_t num_buffers,
31 enum AtomISPMode MipiMode,
32 enum V4L2PixelFormat v4l2Format)
33 : m_devname(devname),
34 m_height(height),
35 m_width(width),
36 m_num_buffers(num_buffers),
37 m_MipiPort(0),
38 m_MipiMode(MipiMode),
39 m_v4l2Format(v4l2Format),
40 m_fd(-1) {}
41
~v4l2Device()42 v4l2Device::~v4l2Device() {
43 if (m_fd > -1) {
44 BYE_ON(close(m_fd) < 0, "V4L2 device close failed: %s\n", ERRSTR);
45 }
46 }
47
blockIOCTL(int handle,int request,void * args)48 int v4l2Device::blockIOCTL(int handle, int request, void* args) {
49 int ioctlStatus;
50 do {
51 ioctlStatus = ioctl(handle, request, args);
52 } while (-1 == ioctlStatus && EINTR == errno);
53 return ioctlStatus;
54 }
55
GetAtomISPModes(enum AtomISPMode mode)56 int v4l2Device::GetAtomISPModes(enum AtomISPMode mode) {
57 switch (mode) {
58 case VIDEO:
59 return _ISP_MODE_VIDEO;
60 case PREVIEW:
61 return _ISP_MODE_PREVIEW;
62 case CONTINUOUS:
63 return _ISP_MODE_CONTINUOUS;
64 case STILL:
65 return _ISP_MODE_STILL;
66 case NONE:
67
68 default:
69 return _ISP_MODE_NONE;
70 }
71 }
72
ConvertToMFXFourCC(enum V4L2PixelFormat v4l2Format)73 int v4l2Device::ConvertToMFXFourCC(enum V4L2PixelFormat v4l2Format) {
74 switch (v4l2Format) {
75 case UYVY:
76 return MFX_FOURCC_UYVY;
77 case YUY2:
78 return MFX_FOURCC_YUY2;
79 case NO_FORMAT:
80
81 default:
82 assert(!"Unsupported mfx fourcc");
83 return 0;
84 }
85 }
86
ConvertToV4L2FourCC()87 int v4l2Device::ConvertToV4L2FourCC() {
88 switch (m_v4l2Format) {
89 case UYVY:
90 return V4L2_PIX_FMT_UYVY;
91 case YUY2:
92 return V4L2_PIX_FMT_YUYV;
93 case NO_FORMAT:
94
95 default:
96 assert(!"Unsupported v4l2 fourcc");
97 return 0;
98 }
99 }
100
Init(const char * devname,uint32_t width,uint32_t height,uint32_t num_buffers,enum V4L2PixelFormat v4l2Format,enum AtomISPMode MipiMode,int MipiPort)101 void v4l2Device::Init(const char* devname,
102 uint32_t width,
103 uint32_t height,
104 uint32_t num_buffers,
105 enum V4L2PixelFormat v4l2Format,
106 enum AtomISPMode MipiMode,
107 int MipiPort) {
108 (devname != NULL) ? m_devname = devname : m_devname;
109 (m_width != width) ? m_width = width : m_width;
110 (m_height != height) ? m_height = height : m_height;
111 (m_num_buffers != num_buffers) ? m_num_buffers = num_buffers : m_num_buffers;
112 (m_v4l2Format != v4l2Format) ? m_v4l2Format = v4l2Format : m_v4l2Format;
113 (m_MipiMode != MipiMode) ? m_MipiMode = MipiMode : m_MipiMode;
114 (m_MipiPort != MipiPort) ? m_MipiPort = MipiPort : m_MipiPort;
115
116 memset(&m_format, 0, sizeof m_format);
117 m_format.width = m_width;
118 m_format.height = m_height;
119 m_format.pixelformat = ConvertToV4L2FourCC();
120
121 V4L2Init();
122 }
123
V4L2Init()124 void v4l2Device::V4L2Init() {
125 int ret;
126 struct v4l2_format fmt;
127 struct v4l2_capability caps;
128 struct v4l2_streamparm parm;
129 struct v4l2_requestbuffers rqbufs;
130 CLEAR(parm);
131
132 m_fd = open(m_devname, O_RDWR);
133 BYE_ON(m_fd < 0, "failed to open %s: %s\n", m_devname, ERRSTR);
134 CLEAR(caps);
135
136 /* Specifically for setting up mipi configuration. DMABUFF is
137 * also enable by default here.
138 */
139 if (m_MipiPort > -1 && m_MipiMode != NONE) {
140 parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
141 parm.parm.capture.capturemode = GetAtomISPModes(m_MipiMode);
142
143 ret = blockIOCTL(m_fd, VIDIOC_S_INPUT, &m_MipiPort);
144 BYE_ON(ret < 0, "VIDIOC_S_INPUT failed: %s\n", ERRSTR);
145
146 ret = blockIOCTL(m_fd, VIDIOC_S_PARM, &parm);
147 BYE_ON(ret < 0, "VIDIOC_S_PARAM failed: %s\n", ERRSTR);
148 }
149
150 ret = blockIOCTL(m_fd, VIDIOC_QUERYCAP, &caps);
151 msdk_printf("Driver Caps:\n"
152 " Driver: \"%s\"\n"
153 " Card: \"%s\"\n"
154 " Bus: \"%s\"\n"
155 " Version: %d.%d\n"
156 " Capabilities: %08x\n",
157 caps.driver,
158 caps.card,
159 caps.bus_info,
160 (caps.version >> 16) && 0xff,
161 (caps.version >> 24) && 0xff,
162 caps.capabilities);
163
164 BYE_ON(ret, "VIDIOC_QUERYCAP failed: %s\n", ERRSTR);
165 BYE_ON(~caps.capabilities & V4L2_CAP_VIDEO_CAPTURE,
166 "video: singleplanar capture is not supported\n");
167
168 CLEAR(fmt);
169 fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
170 ret = blockIOCTL(m_fd, VIDIOC_G_FMT, &fmt);
171
172 BYE_ON(ret < 0, "VIDIOC_G_FMT failed: %s\n", ERRSTR);
173
174 msdk_printf(
175 "G_FMT(start): width = %u, height = %u, 4cc = %.4s, BPP = %u sizeimage = %d field = %d\n",
176 fmt.fmt.pix.width,
177 fmt.fmt.pix.height,
178 (char*)&fmt.fmt.pix.pixelformat,
179 fmt.fmt.pix.bytesperline,
180 fmt.fmt.pix.sizeimage,
181 fmt.fmt.pix.field);
182
183 fmt.fmt.pix = m_format;
184
185 msdk_printf(
186 "G_FMT(pre): width = %u, height = %u, 4cc = %.4s, BPP = %u sizeimage = %d field = %d\n",
187 fmt.fmt.pix.width,
188 fmt.fmt.pix.height,
189 (char*)&fmt.fmt.pix.pixelformat,
190 fmt.fmt.pix.bytesperline,
191 fmt.fmt.pix.sizeimage,
192 fmt.fmt.pix.field);
193
194 ret = blockIOCTL(m_fd, VIDIOC_S_FMT, &fmt);
195 BYE_ON(ret < 0, "VIDIOC_S_FMT failed: %s\n", ERRSTR);
196
197 ret = blockIOCTL(m_fd, VIDIOC_G_FMT, &fmt);
198 BYE_ON(ret < 0, "VIDIOC_G_FMT failed: %s\n", ERRSTR);
199 msdk_printf("G_FMT(final): width = %u, height = %u, 4cc = %.4s, BPP = %u\n",
200 fmt.fmt.pix.width,
201 fmt.fmt.pix.height,
202 (char*)&fmt.fmt.pix.pixelformat,
203 fmt.fmt.pix.bytesperline);
204
205 CLEAR(rqbufs);
206 rqbufs.count = m_num_buffers;
207 rqbufs.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
208 rqbufs.memory = V4L2_MEMORY_DMABUF;
209
210 ret = blockIOCTL(m_fd, VIDIOC_REQBUFS, &rqbufs);
211 BYE_ON(ret < 0, "VIDIOC_REQBUFS failed: %s\n", ERRSTR);
212 BYE_ON(rqbufs.count < m_num_buffers,
213 "video node allocated only "
214 "%u of %u buffers\n",
215 rqbufs.count,
216 m_num_buffers);
217
218 m_format = fmt.fmt.pix;
219 }
220
V4L2Alloc()221 void v4l2Device::V4L2Alloc() {
222 buffers = (Buffer*)malloc(sizeof(Buffer) * (int)m_num_buffers);
223 }
224
V4L2QueueBuffer(Buffer * buffer)225 void v4l2Device::V4L2QueueBuffer(Buffer* buffer) {
226 struct v4l2_buffer buf;
227 int ret;
228
229 memset(&buf, 0, sizeof buf);
230 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
231 buf.memory = V4L2_MEMORY_DMABUF;
232 buf.index = buffer->index;
233 buf.m.fd = buffer->fd;
234
235 ret = blockIOCTL(m_fd, VIDIOC_QBUF, &buf);
236 BYE_ON(ret < 0,
237 "VIDIOC_QBUF for buffer %d failed: %s (fd %u) (i %u)\n",
238 buf.index,
239 ERRSTR,
240 buffer->fd,
241 buffer->index);
242 }
243
V4L2DeQueueBuffer(Buffer * buffer)244 Buffer* v4l2Device::V4L2DeQueueBuffer(Buffer* buffer) {
245 struct v4l2_buffer buf;
246 int ret;
247
248 memset(&buf, 0, sizeof buf);
249
250 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
251 buf.memory = V4L2_MEMORY_DMABUF;
252
253 ret = blockIOCTL(m_fd, VIDIOC_DQBUF, &buf);
254 BYE_ON(ret, "VIDIOC_DQBUF failed: %s\n", ERRSTR);
255
256 return &buffer[buf.index];
257 }
258
V4L2StartCapture()259 void v4l2Device::V4L2StartCapture() {
260 int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
261 int ret = 0;
262
263 ret = blockIOCTL(m_fd, VIDIOC_STREAMON, &type);
264 BYE_ON(ret < 0, "STREAMON failed: %s\n", ERRSTR);
265 }
266
V4L2StopCapture()267 void v4l2Device::V4L2StopCapture() {
268 int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
269 int ret = 0;
270
271 ret = blockIOCTL(m_fd, VIDIOC_STREAMOFF, &type);
272 BYE_ON(ret < 0, "STREAMOFF failed: %s\n", ERRSTR);
273 }
274
PutOnQ(int x)275 void v4l2Device::PutOnQ(int x) {
276 pthread_mutex_lock(&mutex);
277 m_q[m_first] = x;
278 m_first = (m_first + 1) % 5;
279 m_numInQ++;
280 pthread_mutex_unlock(&mutex);
281 pthread_mutex_unlock(&empty);
282 }
283
GetOffQ()284 int v4l2Device::GetOffQ() {
285 int thing;
286
287 /* wait if the queue is empty. */
288 while (m_numInQ == 0)
289 pthread_mutex_lock(&empty);
290
291 pthread_mutex_lock(&mutex);
292 thing = m_q[m_last];
293 m_last = (m_last + 1) % 5;
294 m_numInQ--;
295 pthread_mutex_unlock(&mutex);
296
297 return thing;
298 }
299
GetV4L2TerminationSignal()300 int v4l2Device::GetV4L2TerminationSignal() {
301 return (CtrlFlag && m_numInQ == 0) ? 1 : 0;
302 }
303
CtrlCTerminationHandler(int s)304 static void CtrlCTerminationHandler(int s) {
305 CtrlFlag = true;
306 }
307
PollingThread(void * data)308 void* PollingThread(void* data) {
309 v4l2Device* v4l2 = (v4l2Device*)data;
310
311 struct sigaction sigIntHandler;
312 sigIntHandler.sa_handler = CtrlCTerminationHandler;
313 sigemptyset(&sigIntHandler.sa_mask);
314 sigIntHandler.sa_flags = 0;
315 sigaction(SIGINT, &sigIntHandler, NULL);
316
317 struct pollfd fd;
318 fd.fd = v4l2->GetV4L2DisplayID();
319 fd.events = POLLIN;
320
321 while (1) {
322 if (poll(&fd, 1, 5000) > 0) {
323 if (fd.revents & POLLIN) {
324 CurBuffers = v4l2->V4L2DeQueueBuffer(buffers);
325 v4l2->PutOnQ(CurBuffers->index);
326
327 if (CtrlFlag)
328 break;
329
330 if (CurBuffers)
331 v4l2->V4L2QueueBuffer(&buffers[CurBuffers->index]);
332 }
333 }
334 }
335 }
336
337 #endif // ifdef ENABLE_V4L2_SUPPORT
338