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, 0);
129 if (ctx->fd < 0) {
130 mpp_err_f("Cannot open device\n");
131 goto FAIL;
132 }
133
134 // Determine if fd is a V4L2 Device
135 if (0 != camera_source_ioctl(ctx->fd, VIDIOC_QUERYCAP, &cap)) {
136 mpp_err_f("Not v4l2 compatible\n");
137 goto FAIL;
138 }
139
140 if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) && !(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE)) {
141 mpp_err_f("Capture not supported\n");
142 goto FAIL;
143 }
144
145 if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
146 mpp_err_f("Streaming IO Not Supported\n");
147 goto FAIL;
148 }
149
150 // Preserve original settings as set by v4l2-ctl for example
151 vfmt = (struct v4l2_format) {0};
152 vfmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
153
154 if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE)
155 vfmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
156
157 vfmt.fmt.pix.width = width;
158 vfmt.fmt.pix.height = height;
159
160 if (MPP_FRAME_FMT_IS_YUV(format)) {
161 vfmt.fmt.pix.pixelformat = V4L2_yuv_cfg[format - MPP_FRAME_FMT_YUV];
162 } else if (MPP_FRAME_FMT_IS_RGB(format)) {
163 vfmt.fmt.pix.pixelformat = V4L2_RGB_cfg[format - MPP_FRAME_FMT_RGB];
164 }
165
166 if (!vfmt.fmt.pix.pixelformat)
167 vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_NV12;
168
169 type = vfmt.type;
170 ctx->type = vfmt.type;
171
172 if (-1 == camera_source_ioctl(ctx->fd, VIDIOC_S_FMT, &vfmt)) {
173 mpp_err_f("VIDIOC_S_FMT\n");
174 goto FAIL;
175 }
176
177 if (-1 == camera_source_ioctl(ctx->fd, VIDIOC_G_FMT, &vfmt)) {
178 mpp_err_f("VIDIOC_G_FMT\n");
179 goto FAIL;
180 }
181
182 mpp_log("get width %d height %d", vfmt.fmt.pix.width, vfmt.fmt.pix.height);
183
184 // Request memory-mapped buffers
185 req = (struct v4l2_requestbuffers) {0};
186 req.count = ctx->bufcnt;
187 req.type = type;
188 req.memory = V4L2_MEMORY_MMAP;
189 if (-1 == camera_source_ioctl(ctx->fd, VIDIOC_REQBUFS, &req)) {
190 mpp_err_f("Device does not support mmap\n");
191 goto FAIL;
192 }
193
194 if (req.count != ctx->bufcnt) {
195 mpp_err_f("Device buffer count mismatch\n");
196 goto FAIL;
197 }
198
199 // mmap() the buffers into userspace memory
200 for (i = 0 ; i < ctx->bufcnt; i++) {
201 buf = (struct v4l2_buffer) {0};
202 buf.type = type;
203 buf.memory = V4L2_MEMORY_MMAP;
204 buf.index = i;
205 struct v4l2_plane planes[FMT_NUM_PLANES];
206 buf.memory = V4L2_MEMORY_MMAP;
207 if (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == type) {
208 buf.m.planes = planes;
209 buf.length = FMT_NUM_PLANES;
210 }
211
212 if (-1 == camera_source_ioctl(ctx->fd, VIDIOC_QUERYBUF, &buf)) {
213 mpp_err_f("ERROR: VIDIOC_QUERYBUF\n");
214 goto FAIL;
215 }
216
217 ctx->fbuf[i].start = mmap(NULL, buf.length,
218 PROT_READ | PROT_WRITE, MAP_SHARED,
219 ctx->fd, buf.m.offset);
220 if (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == buf.type) {
221 // tmp_buffers[n_buffers].length = buf.m.planes[0].length;
222 buf_len = buf.m.planes[0].length;
223 ctx->fbuf[i].start =
224 mmap(NULL /* start anywhere */,
225 buf.m.planes[0].length,
226 PROT_READ | PROT_WRITE /* required */,
227 MAP_SHARED /* recommended */,
228 ctx->fd, buf.m.planes[0].m.mem_offset);
229 } else {
230 buf_len = buf.length;
231 ctx->fbuf[i].start =
232 mmap(NULL /* start anywhere */,
233 buf.length,
234 PROT_READ | PROT_WRITE /* required */,
235 MAP_SHARED /* recommended */,
236 ctx->fd, buf.m.offset);
237 }
238 if (MAP_FAILED == ctx->fbuf[i].start) {
239 mpp_err_f("ERROR: Failed to map device frame buffers\n");
240 goto FAIL;
241 }
242 struct v4l2_exportbuffer expbuf = (struct v4l2_exportbuffer) {0} ;
243 // xcam_mem_clear (expbuf);
244 expbuf.type = type;
245 expbuf.index = i;
246 expbuf.flags = O_CLOEXEC;
247 if (camera_source_ioctl(ctx->fd, VIDIOC_EXPBUF, &expbuf) < 0) {
248 mpp_err_f("get dma buf failed\n");
249 goto FAIL;
250 } else {
251 mpp_log("get dma buf(%d)-fd: %d\n", i, expbuf.fd);
252 MppBufferInfo info;
253 memset(&info, 0, sizeof(MppBufferInfo));
254 info.type = MPP_BUFFER_TYPE_EXT_DMA;
255 info.fd = expbuf.fd;
256 info.size = buf_len & 0x07ffffff;
257 info.index = (buf_len & 0xf8000000) >> 27;
258 mpp_buffer_import(&ctx->fbuf[i].buffer, &info);
259 }
260 ctx->fbuf[i].export_fd = expbuf.fd;
261 }
262
263 for (i = 0; i < ctx->bufcnt; i++ ) {
264 struct v4l2_plane planes[FMT_NUM_PLANES];
265
266 buf = (struct v4l2_buffer) {0};
267 buf.type = type;
268 buf.memory = V4L2_MEMORY_MMAP;
269 buf.index = i;
270 buf.memory = V4L2_MEMORY_MMAP;
271
272 if (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == type) {
273 buf.m.planes = planes;
274 buf.length = FMT_NUM_PLANES;
275 }
276
277 if (-1 == camera_source_ioctl(ctx->fd, VIDIOC_QBUF, &buf)) {
278 mpp_err_f("ERROR: VIDIOC_QBUF %d\n", i);
279 camera_source_deinit(ctx);
280 goto FAIL;
281 }
282 }
283
284 // Start capturing
285 if (-1 == camera_source_ioctl(ctx->fd, VIDIOC_STREAMON, &type)) {
286 mpp_err_f("ERROR: VIDIOC_STREAMON\n");
287 camera_source_deinit(ctx);
288 goto FAIL;
289 }
290
291 //skip some frames at start
292 for (i = 0; i < ctx->bufcnt; i++ ) {
293 RK_S32 idx = camera_source_get_frame(ctx);
294 if (idx >= 0)
295 camera_source_put_frame(ctx, idx);
296 }
297
298 return ctx;
299
300 FAIL:
301 camera_source_deinit(ctx);
302 return NULL;
303 }
304
305 // Free a context to capture frames from <fname>.
306 // Returns NULL on error.
camera_source_deinit(CamSource * ctx)307 MPP_RET camera_source_deinit(CamSource *ctx)
308 {
309 struct v4l2_buffer buf;
310 enum v4l2_buf_type type;
311 RK_U32 i;
312
313 if (NULL == ctx)
314 return MPP_OK;
315
316 if (ctx->fd < 0)
317 return MPP_OK;
318
319 // Stop capturing
320 type = ctx->type;
321
322 camera_source_ioctl(ctx->fd, VIDIOC_STREAMOFF, &type);
323
324 // un-mmap() buffers
325 for (i = 0 ; i < ctx->bufcnt; i++) {
326 buf = (struct v4l2_buffer) {0};
327 buf.type = type;
328 buf.memory = V4L2_MEMORY_MMAP;
329 buf.index = i;
330 camera_source_ioctl(ctx->fd, VIDIOC_QUERYBUF, &buf);
331 if (ctx->fbuf[buf.index].buffer) {
332 mpp_buffer_put(ctx->fbuf[buf.index].buffer);
333 }
334 munmap(ctx->fbuf[buf.index].start, buf.length);
335 close(ctx->fbuf[i].export_fd);
336 }
337
338 // Close v4l2 device
339 close(ctx->fd);
340 MPP_FREE(ctx);
341 return MPP_OK;
342 }
343
344 // Returns a pointer to a captured frame and its meta-data. NOT thread-safe.
camera_source_get_frame(CamSource * ctx)345 RK_S32 camera_source_get_frame(CamSource *ctx)
346 {
347 struct v4l2_buffer buf;
348 enum v4l2_buf_type type;
349
350 type = ctx->type;
351 buf = (struct v4l2_buffer) {0};
352 buf.type = type;
353 buf.memory = V4L2_MEMORY_MMAP;
354
355 struct v4l2_plane planes[FMT_NUM_PLANES];
356 if (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == type) {
357 buf.m.planes = planes;
358 buf.length = FMT_NUM_PLANES;
359 }
360
361 if (-1 == camera_source_ioctl(ctx->fd, VIDIOC_DQBUF, &buf)) {
362 mpp_err_f("VIDIOC_DQBUF\n");
363 return -1;
364 }
365
366 if (buf.index > ctx->bufcnt) {
367 mpp_err_f("buffer index out of bounds\n");
368 return -1;
369 }
370
371 if (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == type)
372 buf.bytesused = buf.m.planes[0].bytesused;
373
374 return buf.index;
375 }
376
377 // It's OK to capture into this framebuffer now
camera_source_put_frame(CamSource * ctx,RK_S32 idx)378 MPP_RET camera_source_put_frame(CamSource *ctx, RK_S32 idx)
379 {
380 struct v4l2_buffer buf;
381 enum v4l2_buf_type type;
382
383 if (idx < 0)
384 return MPP_OK;
385
386 type = ctx->type;
387 buf = (struct v4l2_buffer) {0};
388 buf.type = type;
389 buf.memory = V4L2_MEMORY_MMAP;
390 buf.index = idx;
391
392 struct v4l2_plane planes[FMT_NUM_PLANES];
393 if (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == type) {
394 buf.m.planes = planes;
395 buf.length = FMT_NUM_PLANES;
396 }
397
398 // Tell kernel it's ok to overwrite this frame
399 if (-1 == camera_source_ioctl(ctx->fd, VIDIOC_QBUF, &buf)) {
400 mpp_err_f("VIDIOC_QBUF\n");
401 return MPP_OK;
402 }
403
404 return MPP_OK;
405 }
406
camera_frame_to_buf(CamSource * ctx,RK_S32 idx)407 MppBuffer camera_frame_to_buf(CamSource *ctx, RK_S32 idx)
408 {
409 if (idx < 0)
410 return NULL;
411
412 return ctx->fbuf[idx].buffer;
413 }
414