1 /*
2 * Copyright 2015 Rockchip Electronics Co. LTD
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #define MODULE_TAG "camera_source"
18
19 #include <stdio.h>
20 #include <fcntl.h>
21 #include <unistd.h>
22 #include <errno.h>
23 #include <stdlib.h>
24 #include <string.h>
25 #include <time.h>
26
27 #include <sys/select.h>
28 #include <sys/mman.h>
29 #include <sys/ioctl.h>
30 #include <linux/videodev2.h>
31
32 #include "mpp_log.h"
33 #include "mpp_mem.h"
34 #include "camera_source.h"
35
36 typedef struct CamFrame_t {
37 void *start;
38 size_t length;
39 RK_S32 export_fd;
40 RK_S32 sequence;
41 MppBuffer buffer;
42 } CamFrame;
43
44 struct CamSource {
45 RK_S32 fd; // Device handle
46 RK_U32 bufcnt; // # of buffers
47 enum v4l2_buf_type type;
48 MppFrameFormat fmt;
49 CamFrame fbuf[10];// frame buffers
50 };
51
52 static RK_U32 V4L2_yuv_cfg[MPP_FMT_YUV_BUTT] = {
53 V4L2_PIX_FMT_NV12,
54 0,
55 V4L2_PIX_FMT_NV16,
56 0,
57 V4L2_PIX_FMT_YVU420,
58 V4L2_PIX_FMT_NV21,
59 V4L2_PIX_FMT_YUV422P,
60 V4L2_PIX_FMT_NV61,
61 V4L2_PIX_FMT_YUYV,
62 V4L2_PIX_FMT_YVYU,
63 V4L2_PIX_FMT_UYVY,
64 V4L2_PIX_FMT_VYUY,
65 V4L2_PIX_FMT_GREY,
66 0,
67 0,
68 0,
69 };
70
71 static RK_U32 V4L2_RGB_cfg[MPP_FMT_RGB_BUTT - MPP_FRAME_FMT_RGB] = {
72 V4L2_PIX_FMT_RGB565,
73 0,
74 V4L2_PIX_FMT_RGB555,
75 0,
76 V4L2_PIX_FMT_RGB444,
77 0,
78 V4L2_PIX_FMT_RGB24,
79 V4L2_PIX_FMT_BGR24,
80 0,
81 0,
82 V4L2_PIX_FMT_RGB32,
83 V4L2_PIX_FMT_BGR32,
84 0,
85 0,
86 };
87
88 #define FMT_NUM_PLANES 1
89
90 // Wrap ioctl() to spin on EINTR
camera_source_ioctl(RK_S32 fd,RK_S32 req,void * arg)91 static RK_S32 camera_source_ioctl(RK_S32 fd, RK_S32 req, void* arg)
92 {
93 struct timespec poll_time;
94 RK_S32 ret;
95
96 while ((ret = ioctl(fd, req, arg))) {
97 if (ret == -1 && (EINTR != errno && EAGAIN != errno)) {
98 // mpp_err("ret = %d, errno %d", ret, errno);
99 break;
100 }
101 // 10 milliseconds
102 poll_time.tv_sec = 0;
103 poll_time.tv_nsec = 10000000;
104 nanosleep(&poll_time, NULL);
105 }
106
107 return ret;
108 }
109
110 // Create a new context to capture frames from <fname>.
111 // Returns NULL on error.
camera_source_init(const char * device,RK_U32 bufcnt,RK_U32 width,RK_U32 height,MppFrameFormat format)112 CamSource *camera_source_init(const char *device, RK_U32 bufcnt, RK_U32 width, RK_U32 height, MppFrameFormat format)
113 {
114 struct v4l2_capability cap;
115 struct v4l2_format vfmt;
116 struct v4l2_requestbuffers req;
117 struct v4l2_buffer buf;
118 enum v4l2_buf_type type;
119 RK_U32 i;
120 RK_U32 buf_len = 0;
121 CamSource *ctx;
122
123 ctx = mpp_calloc(CamSource, 1);
124 if (!ctx)
125 return NULL;
126
127 ctx->bufcnt = bufcnt;
128 ctx->fd = open(device, O_RDWR | O_CLOEXEC, 0);
129 if (ctx->fd < 0) {
130 mpp_err_f("Cannot open device\n");
131 goto FAIL;
132 }
133
134 {
135 struct v4l2_input input;
136
137 input.index = 0;
138 while (!camera_source_ioctl(ctx->fd, VIDIOC_ENUMINPUT, &input)) {
139 mpp_log("input devices:%s\n", input.name);
140 ++input.index;
141 }
142 }
143
144 // Determine if fd is a V4L2 Device
145 if (0 != camera_source_ioctl(ctx->fd, VIDIOC_QUERYCAP, &cap)) {
146 mpp_err_f("Not v4l2 compatible\n");
147 goto FAIL;
148 }
149
150 if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) && !(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE)) {
151 mpp_err_f("Capture not supported\n");
152 goto FAIL;
153 }
154
155 if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
156 mpp_err_f("Streaming IO Not Supported\n");
157 goto FAIL;
158 }
159
160 // Preserve original settings as set by v4l2-ctl for example
161 vfmt = (struct v4l2_format) {0};
162 vfmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
163
164 if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE)
165 vfmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
166
167 vfmt.fmt.pix.width = width;
168 vfmt.fmt.pix.height = height;
169
170 {
171 struct v4l2_fmtdesc fmtdesc;
172
173 fmtdesc.index = 0;
174 fmtdesc.type = vfmt.type;
175 while (!camera_source_ioctl(ctx->fd, VIDIOC_ENUM_FMT, &fmtdesc)) {
176 mpp_log("fmt name: [%s]\n", fmtdesc.description);
177 mpp_log("fmt pixelformat: '%c%c%c%c', description = '%s'\n", fmtdesc.pixelformat & 0xFF,
178 (fmtdesc.pixelformat >> 8) & 0xFF, (fmtdesc.pixelformat >> 16) & 0xFF,
179 (fmtdesc.pixelformat >> 24) & 0xFF, fmtdesc.description);
180 fmtdesc.index++;
181 }
182 }
183
184 if (MPP_FRAME_FMT_IS_YUV(format)) {
185 vfmt.fmt.pix.pixelformat = V4L2_yuv_cfg[format - MPP_FRAME_FMT_YUV];
186 } else if (MPP_FRAME_FMT_IS_RGB(format)) {
187 vfmt.fmt.pix.pixelformat = V4L2_RGB_cfg[format - MPP_FRAME_FMT_RGB];
188 }
189
190 if (!vfmt.fmt.pix.pixelformat)
191 vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_NV12;
192
193 type = vfmt.type;
194 ctx->type = vfmt.type;
195
196 if (-1 == camera_source_ioctl(ctx->fd, VIDIOC_S_FMT, &vfmt)) {
197 mpp_err_f("VIDIOC_S_FMT\n");
198 goto FAIL;
199 }
200
201 if (-1 == camera_source_ioctl(ctx->fd, VIDIOC_G_FMT, &vfmt)) {
202 mpp_err_f("VIDIOC_G_FMT\n");
203 goto FAIL;
204 }
205
206 mpp_log("get width %d height %d", vfmt.fmt.pix.width, vfmt.fmt.pix.height);
207
208 // Request memory-mapped buffers
209 req = (struct v4l2_requestbuffers) {0};
210 req.count = ctx->bufcnt;
211 req.type = type;
212 req.memory = V4L2_MEMORY_MMAP;
213 if (-1 == camera_source_ioctl(ctx->fd, VIDIOC_REQBUFS, &req)) {
214 mpp_err_f("Device does not support mmap\n");
215 goto FAIL;
216 }
217
218 if (req.count != ctx->bufcnt) {
219 mpp_err_f("Device buffer count mismatch\n");
220 goto FAIL;
221 }
222
223 // mmap() the buffers into userspace memory
224 for (i = 0 ; i < ctx->bufcnt; i++) {
225 buf = (struct v4l2_buffer) {0};
226 buf.type = type;
227 buf.memory = V4L2_MEMORY_MMAP;
228 buf.index = i;
229 struct v4l2_plane planes[FMT_NUM_PLANES];
230 if (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == type) {
231 buf.m.planes = planes;
232 buf.length = FMT_NUM_PLANES;
233 }
234
235 if (-1 == camera_source_ioctl(ctx->fd, VIDIOC_QUERYBUF, &buf)) {
236 mpp_err_f("ERROR: VIDIOC_QUERYBUF\n");
237 goto FAIL;
238 }
239
240 if (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == buf.type) {
241 // tmp_buffers[n_buffers].length = buf.m.planes[0].length;
242 buf_len = buf.m.planes[0].length;
243 ctx->fbuf[i].start =
244 mmap(NULL /* start anywhere */,
245 buf.m.planes[0].length,
246 PROT_READ | PROT_WRITE /* required */,
247 MAP_SHARED /* recommended */,
248 ctx->fd, buf.m.planes[0].m.mem_offset);
249 } else {
250 buf_len = buf.length;
251 ctx->fbuf[i].start =
252 mmap(NULL /* start anywhere */,
253 buf.length,
254 PROT_READ | PROT_WRITE /* required */,
255 MAP_SHARED /* recommended */,
256 ctx->fd, buf.m.offset);
257 }
258 if (MAP_FAILED == ctx->fbuf[i].start) {
259 mpp_err_f("ERROR: Failed to map device frame buffers\n");
260 goto FAIL;
261 }
262
263 ctx->fbuf[i].length = buf_len; // record buffer length for unmap
264
265 struct v4l2_exportbuffer expbuf = (struct v4l2_exportbuffer) {0} ;
266 // xcam_mem_clear (expbuf);
267 expbuf.type = type;
268 expbuf.index = i;
269 expbuf.flags = O_CLOEXEC;
270 if (camera_source_ioctl(ctx->fd, VIDIOC_EXPBUF, &expbuf) < 0) {
271 mpp_err_f("get dma buf failed\n");
272 goto FAIL;
273 } else {
274 mpp_log("get dma buf(%d)-fd: %d\n", i, expbuf.fd);
275 MppBufferInfo info;
276 memset(&info, 0, sizeof(MppBufferInfo));
277 info.type = MPP_BUFFER_TYPE_EXT_DMA;
278 info.fd = expbuf.fd;
279 info.size = buf_len & 0x07ffffff;
280 info.index = (buf_len & 0xf8000000) >> 27;
281 mpp_buffer_import(&ctx->fbuf[i].buffer, &info);
282 }
283 ctx->fbuf[i].export_fd = expbuf.fd;
284 }
285
286 for (i = 0; i < ctx->bufcnt; i++ ) {
287 struct v4l2_plane planes[FMT_NUM_PLANES];
288
289 buf = (struct v4l2_buffer) {0};
290 buf.type = type;
291 buf.memory = V4L2_MEMORY_MMAP;
292 buf.index = i;
293
294 if (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == type) {
295 buf.m.planes = planes;
296 buf.length = FMT_NUM_PLANES;
297 }
298
299 if (-1 == camera_source_ioctl(ctx->fd, VIDIOC_QBUF, &buf)) {
300 mpp_err_f("ERROR: VIDIOC_QBUF %d\n", i);
301 goto FAIL;
302 }
303 }
304
305 // Start capturing
306 if (-1 == camera_source_ioctl(ctx->fd, VIDIOC_STREAMON, &type)) {
307 mpp_err_f("ERROR: VIDIOC_STREAMON\n");
308 goto FAIL;
309 }
310
311 //skip some frames at start
312 for (i = 0; i < ctx->bufcnt; i++ ) {
313 RK_S32 idx = camera_source_get_frame(ctx);
314 if (idx >= 0)
315 camera_source_put_frame(ctx, idx);
316 }
317
318 return ctx;
319
320 FAIL:
321 camera_source_deinit(ctx);
322 return NULL;
323 }
324
325 // Free a context to capture frames from <fname>.
326 // Returns NULL on error.
camera_source_deinit(CamSource * ctx)327 MPP_RET camera_source_deinit(CamSource *ctx)
328 {
329 struct v4l2_buffer buf;
330 enum v4l2_buf_type type;
331 RK_U32 i;
332
333 if (NULL == ctx)
334 return MPP_OK;
335
336 if (ctx->fd < 0)
337 return MPP_OK;
338
339 // Stop capturing
340 type = ctx->type;
341
342 camera_source_ioctl(ctx->fd, VIDIOC_STREAMOFF, &type);
343
344 // un-mmap() buffers
345 for (i = 0 ; i < ctx->bufcnt; i++) {
346 buf = (struct v4l2_buffer) {0};
347 buf.type = type;
348 buf.memory = V4L2_MEMORY_MMAP;
349 buf.index = i;
350 camera_source_ioctl(ctx->fd, VIDIOC_QUERYBUF, &buf);
351 if (ctx->fbuf[buf.index].buffer) {
352 mpp_buffer_put(ctx->fbuf[buf.index].buffer);
353 }
354 munmap(ctx->fbuf[buf.index].start, ctx->fbuf[buf.index].length);
355 close(ctx->fbuf[i].export_fd);
356 }
357
358 // Close v4l2 device
359 close(ctx->fd);
360 MPP_FREE(ctx);
361 return MPP_OK;
362 }
363
364 // Returns a pointer to a captured frame and its meta-data. NOT thread-safe.
camera_source_get_frame(CamSource * ctx)365 RK_S32 camera_source_get_frame(CamSource *ctx)
366 {
367 struct v4l2_buffer buf;
368 enum v4l2_buf_type type;
369
370 type = ctx->type;
371 buf = (struct v4l2_buffer) {0};
372 buf.type = type;
373 buf.memory = V4L2_MEMORY_MMAP;
374
375 struct v4l2_plane planes[FMT_NUM_PLANES];
376 if (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == type) {
377 buf.m.planes = planes;
378 buf.length = FMT_NUM_PLANES;
379 }
380
381 if (-1 == camera_source_ioctl(ctx->fd, VIDIOC_DQBUF, &buf)) {
382 mpp_err_f("VIDIOC_DQBUF\n");
383 return -1;
384 }
385
386 if (buf.index > ctx->bufcnt) {
387 mpp_err_f("buffer index out of bounds\n");
388 return -1;
389 }
390
391 if (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == type)
392 buf.bytesused = buf.m.planes[0].bytesused;
393
394 return buf.index;
395 }
396
397 // It's OK to capture into this framebuffer now
camera_source_put_frame(CamSource * ctx,RK_S32 idx)398 MPP_RET camera_source_put_frame(CamSource *ctx, RK_S32 idx)
399 {
400 struct v4l2_buffer buf;
401 enum v4l2_buf_type type;
402
403 if (idx < 0)
404 return MPP_OK;
405
406 type = ctx->type;
407 buf = (struct v4l2_buffer) {0};
408 buf.type = type;
409 buf.memory = V4L2_MEMORY_MMAP;
410 buf.index = idx;
411
412 struct v4l2_plane planes[FMT_NUM_PLANES];
413 if (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == type) {
414 buf.m.planes = planes;
415 buf.length = FMT_NUM_PLANES;
416 }
417
418 // Tell kernel it's ok to overwrite this frame
419 if (-1 == camera_source_ioctl(ctx->fd, VIDIOC_QBUF, &buf)) {
420 mpp_err_f("VIDIOC_QBUF\n");
421 return MPP_OK;
422 }
423
424 return MPP_OK;
425 }
426
camera_frame_to_buf(CamSource * ctx,RK_S32 idx)427 MppBuffer camera_frame_to_buf(CamSource *ctx, RK_S32 idx)
428 {
429 MppBuffer buf = NULL;
430
431 if (idx < 0)
432 return buf;
433
434 buf = ctx->fbuf[idx].buffer;
435 if (buf)
436 mpp_buffer_sync_end(buf);
437
438 return buf;
439 }
440