1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * Broadcom BCM2835 V4L2 driver
4 *
5 * Copyright © 2013 Raspberry Pi (Trading) Ltd.
6 *
7 * Authors: Vincent Sanders @ Collabora
8 * Dave Stevenson @ Broadcom
9 * (now dave.stevenson@raspberrypi.org)
10 * Simon Mellor @ Broadcom
11 * Luke Diamand @ Broadcom
12 */
13
14 #include <linux/dma-mapping.h>
15 #include <linux/errno.h>
16 #include <linux/kernel.h>
17 #include <linux/module.h>
18 #include <linux/slab.h>
19 #include <media/videobuf2-vmalloc.h>
20 #include <media/videobuf2-dma-contig.h>
21 #include <media/v4l2-device.h>
22 #include <media/v4l2-ioctl.h>
23 #include <media/v4l2-ctrls.h>
24 #include <media/v4l2-fh.h>
25 #include <media/v4l2-event.h>
26 #include <media/v4l2-common.h>
27 #include <linux/delay.h>
28
29 #include "../interface/vchiq_arm/vchiq_bus.h"
30 #include "../vchiq-mmal/mmal-common.h"
31 #include "../vchiq-mmal/mmal-encodings.h"
32 #include "../vchiq-mmal/mmal-vchiq.h"
33 #include "../vchiq-mmal/mmal-msg.h"
34 #include "../vchiq-mmal/mmal-parameters.h"
35 #include "bcm2835-camera.h"
36
37 #define MIN_WIDTH 32
38 #define MIN_HEIGHT 32
39 #define MIN_BUFFER_SIZE (80 * 1024)
40
41 #define MAX_VIDEO_MODE_WIDTH 1280
42 #define MAX_VIDEO_MODE_HEIGHT 720
43
44 #define MAX_BCM2835_CAMERAS 2
45
46 int bcm2835_v4l2_debug;
47 module_param_named(debug, bcm2835_v4l2_debug, int, 0644);
48 MODULE_PARM_DESC(bcm2835_v4l2_debug, "Debug level 0-2");
49
50 #define UNSET (-1)
51 static int video_nr[] = {[0 ... (MAX_BCM2835_CAMERAS - 1)] = UNSET };
52 module_param_array(video_nr, int, NULL, 0644);
53 MODULE_PARM_DESC(video_nr, "videoX start numbers, -1 is autodetect");
54
55 static int max_video_width = MAX_VIDEO_MODE_WIDTH;
56 static int max_video_height = MAX_VIDEO_MODE_HEIGHT;
57 module_param(max_video_width, int, 0644);
58 MODULE_PARM_DESC(max_video_width, "Threshold for video mode");
59 module_param(max_video_height, int, 0644);
60 MODULE_PARM_DESC(max_video_height, "Threshold for video mode");
61
62 /* camera instance counter */
63 static atomic_t camera_instance = ATOMIC_INIT(0);
64
65 /* global device data array */
66 static struct bcm2835_mmal_dev *gdev[MAX_BCM2835_CAMERAS];
67
68 #define FPS_MIN 1
69 #define FPS_MAX 90
70
71 /* timeperframe: min/max and default */
72 static const struct v4l2_fract
73 tpf_min = {.numerator = 1, .denominator = FPS_MAX},
74 tpf_max = {.numerator = 1, .denominator = FPS_MIN},
75 tpf_default = {.numerator = 1000, .denominator = 30000};
76
77 /* Container for MMAL and VB2 buffers*/
78 struct vb2_mmal_buffer {
79 struct vb2_v4l2_buffer vb;
80 struct mmal_buffer mmal;
81 };
82
83 /* video formats */
84 static struct mmal_fmt formats[] = {
85 {
86 .fourcc = V4L2_PIX_FMT_YUV420,
87 .mmal = MMAL_ENCODING_I420,
88 .depth = 12,
89 .mmal_component = COMP_CAMERA,
90 .ybbp = 1,
91 .remove_padding = true,
92 }, {
93 .fourcc = V4L2_PIX_FMT_YUYV,
94 .mmal = MMAL_ENCODING_YUYV,
95 .depth = 16,
96 .mmal_component = COMP_CAMERA,
97 .ybbp = 2,
98 .remove_padding = false,
99 }, {
100 .fourcc = V4L2_PIX_FMT_RGB24,
101 .mmal = MMAL_ENCODING_RGB24,
102 .depth = 24,
103 .mmal_component = COMP_CAMERA,
104 .ybbp = 3,
105 .remove_padding = false,
106 }, {
107 .fourcc = V4L2_PIX_FMT_JPEG,
108 .flags = V4L2_FMT_FLAG_COMPRESSED,
109 .mmal = MMAL_ENCODING_JPEG,
110 .depth = 8,
111 .mmal_component = COMP_IMAGE_ENCODE,
112 .ybbp = 0,
113 .remove_padding = false,
114 }, {
115 .fourcc = V4L2_PIX_FMT_H264,
116 .flags = V4L2_FMT_FLAG_COMPRESSED,
117 .mmal = MMAL_ENCODING_H264,
118 .depth = 8,
119 .mmal_component = COMP_VIDEO_ENCODE,
120 .ybbp = 0,
121 .remove_padding = false,
122 }, {
123 .fourcc = V4L2_PIX_FMT_MJPEG,
124 .flags = V4L2_FMT_FLAG_COMPRESSED,
125 .mmal = MMAL_ENCODING_MJPEG,
126 .depth = 8,
127 .mmal_component = COMP_VIDEO_ENCODE,
128 .ybbp = 0,
129 .remove_padding = false,
130 }, {
131 .fourcc = V4L2_PIX_FMT_YVYU,
132 .mmal = MMAL_ENCODING_YVYU,
133 .depth = 16,
134 .mmal_component = COMP_CAMERA,
135 .ybbp = 2,
136 .remove_padding = false,
137 }, {
138 .fourcc = V4L2_PIX_FMT_VYUY,
139 .mmal = MMAL_ENCODING_VYUY,
140 .depth = 16,
141 .mmal_component = COMP_CAMERA,
142 .ybbp = 2,
143 .remove_padding = false,
144 }, {
145 .fourcc = V4L2_PIX_FMT_UYVY,
146 .mmal = MMAL_ENCODING_UYVY,
147 .depth = 16,
148 .mmal_component = COMP_CAMERA,
149 .ybbp = 2,
150 .remove_padding = false,
151 }, {
152 .fourcc = V4L2_PIX_FMT_NV12,
153 .mmal = MMAL_ENCODING_NV12,
154 .depth = 12,
155 .mmal_component = COMP_CAMERA,
156 .ybbp = 1,
157 .remove_padding = true,
158 }, {
159 .fourcc = V4L2_PIX_FMT_BGR24,
160 .mmal = MMAL_ENCODING_BGR24,
161 .depth = 24,
162 .mmal_component = COMP_CAMERA,
163 .ybbp = 3,
164 .remove_padding = false,
165 }, {
166 .fourcc = V4L2_PIX_FMT_YVU420,
167 .mmal = MMAL_ENCODING_YV12,
168 .depth = 12,
169 .mmal_component = COMP_CAMERA,
170 .ybbp = 1,
171 .remove_padding = true,
172 }, {
173 .fourcc = V4L2_PIX_FMT_NV21,
174 .mmal = MMAL_ENCODING_NV21,
175 .depth = 12,
176 .mmal_component = COMP_CAMERA,
177 .ybbp = 1,
178 .remove_padding = true,
179 }, {
180 .fourcc = V4L2_PIX_FMT_BGR32,
181 .mmal = MMAL_ENCODING_BGRA,
182 .depth = 32,
183 .mmal_component = COMP_CAMERA,
184 .ybbp = 4,
185 .remove_padding = false,
186 },
187 };
188
get_format(struct v4l2_format * f)189 static struct mmal_fmt *get_format(struct v4l2_format *f)
190 {
191 struct mmal_fmt *fmt;
192 unsigned int k;
193
194 for (k = 0; k < ARRAY_SIZE(formats); k++) {
195 fmt = &formats[k];
196 if (fmt->fourcc == f->fmt.pix.pixelformat)
197 return fmt;
198 }
199
200 return NULL;
201 }
202
203 /* ------------------------------------------------------------------
204 * Videobuf queue operations
205 * ------------------------------------------------------------------
206 */
207
queue_setup(struct vb2_queue * vq,unsigned int * nbuffers,unsigned int * nplanes,unsigned int sizes[],struct device * alloc_ctxs[])208 static int queue_setup(struct vb2_queue *vq,
209 unsigned int *nbuffers, unsigned int *nplanes,
210 unsigned int sizes[], struct device *alloc_ctxs[])
211 {
212 struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vq);
213 unsigned long size;
214
215 /* refuse queue setup if port is not configured */
216 if (!dev->capture.port) {
217 v4l2_err(&dev->v4l2_dev,
218 "%s: capture port not configured\n", __func__);
219 return -EINVAL;
220 }
221
222 /* Handle CREATE_BUFS situation - *nplanes != 0 */
223 if (*nplanes) {
224 if (*nplanes != 1 ||
225 sizes[0] < dev->capture.port->current_buffer.size) {
226 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
227 "%s: dev:%p Invalid buffer request from CREATE_BUFS, size %u < %u, nplanes %u != 1\n",
228 __func__, dev, sizes[0],
229 dev->capture.port->current_buffer.size,
230 *nplanes);
231 return -EINVAL;
232 } else {
233 return 0;
234 }
235 }
236
237 /* Handle REQBUFS situation */
238 size = dev->capture.port->current_buffer.size;
239 if (size == 0) {
240 v4l2_err(&dev->v4l2_dev,
241 "%s: capture port buffer size is zero\n", __func__);
242 return -EINVAL;
243 }
244
245 if (*nbuffers < dev->capture.port->minimum_buffer.num)
246 *nbuffers = dev->capture.port->minimum_buffer.num;
247
248 dev->capture.port->current_buffer.num = *nbuffers;
249
250 *nplanes = 1;
251
252 sizes[0] = size;
253
254 /*
255 * videobuf2-vmalloc allocator is context-less so no need to set
256 * alloc_ctxs array.
257 */
258
259 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n",
260 __func__, dev);
261
262 return 0;
263 }
264
buffer_init(struct vb2_buffer * vb)265 static int buffer_init(struct vb2_buffer *vb)
266 {
267 struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
268 struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
269 struct vb2_mmal_buffer *buf =
270 container_of(vb2, struct vb2_mmal_buffer, vb);
271
272 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p, vb %p\n",
273 __func__, dev, vb);
274 buf->mmal.buffer = vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
275 buf->mmal.buffer_size = vb2_plane_size(&buf->vb.vb2_buf, 0);
276
277 return mmal_vchi_buffer_init(dev->instance, &buf->mmal);
278 }
279
buffer_prepare(struct vb2_buffer * vb)280 static int buffer_prepare(struct vb2_buffer *vb)
281 {
282 struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
283 unsigned long size;
284
285 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p, vb %p\n",
286 __func__, dev, vb);
287
288 if (!dev->capture.port || !dev->capture.fmt)
289 return -ENODEV;
290
291 size = dev->capture.stride * dev->capture.height;
292 if (vb2_plane_size(vb, 0) < size) {
293 v4l2_err(&dev->v4l2_dev,
294 "%s data will not fit into plane (%lu < %lu)\n",
295 __func__, vb2_plane_size(vb, 0), size);
296 return -EINVAL;
297 }
298
299 return 0;
300 }
301
buffer_cleanup(struct vb2_buffer * vb)302 static void buffer_cleanup(struct vb2_buffer *vb)
303 {
304 struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
305 struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
306 struct vb2_mmal_buffer *buf =
307 container_of(vb2, struct vb2_mmal_buffer, vb);
308
309 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p, vb %p\n",
310 __func__, dev, vb);
311
312 mmal_vchi_buffer_cleanup(&buf->mmal);
313 }
314
is_capturing(struct bcm2835_mmal_dev * dev)315 static inline bool is_capturing(struct bcm2835_mmal_dev *dev)
316 {
317 return dev->capture.camera_port ==
318 &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE];
319 }
320
buffer_cb(struct vchiq_mmal_instance * instance,struct vchiq_mmal_port * port,int status,struct mmal_buffer * mmal_buf)321 static void buffer_cb(struct vchiq_mmal_instance *instance,
322 struct vchiq_mmal_port *port,
323 int status,
324 struct mmal_buffer *mmal_buf)
325 {
326 struct bcm2835_mmal_dev *dev = port->cb_ctx;
327 struct vb2_mmal_buffer *buf =
328 container_of(mmal_buf, struct vb2_mmal_buffer, mmal);
329
330 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
331 "%s: status:%d, buf:%p, length:%lu, flags %u, pts %lld\n",
332 __func__, status, buf, mmal_buf->length, mmal_buf->mmal_flags,
333 mmal_buf->pts);
334
335 if (status) {
336 /* error in transfer */
337 if (buf) {
338 /* there was a buffer with the error so return it */
339 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
340 }
341 return;
342 }
343
344 if (mmal_buf->length == 0) {
345 /* stream ended */
346 if (dev->capture.frame_count) {
347 /* empty buffer whilst capturing - expected to be an
348 * EOS, so grab another frame
349 */
350 if (is_capturing(dev)) {
351 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
352 "Grab another frame");
353 vchiq_mmal_port_parameter_set(
354 instance,
355 dev->capture.camera_port,
356 MMAL_PARAMETER_CAPTURE,
357 &dev->capture.frame_count,
358 sizeof(dev->capture.frame_count));
359 }
360 if (vchiq_mmal_submit_buffer(instance, port,
361 &buf->mmal))
362 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
363 "Failed to return EOS buffer");
364 } else {
365 /* stopping streaming.
366 * return buffer, and signal frame completion
367 */
368 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
369 complete(&dev->capture.frame_cmplt);
370 }
371 return;
372 }
373
374 if (!dev->capture.frame_count) {
375 /* signal frame completion */
376 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
377 complete(&dev->capture.frame_cmplt);
378 return;
379 }
380
381 if (dev->capture.vc_start_timestamp != -1 && mmal_buf->pts) {
382 ktime_t timestamp;
383 s64 runtime_us = mmal_buf->pts -
384 dev->capture.vc_start_timestamp;
385 timestamp = ktime_add_us(dev->capture.kernel_start_ts,
386 runtime_us);
387 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
388 "Convert start time %llu and %llu with offset %llu to %llu\n",
389 ktime_to_ns(dev->capture.kernel_start_ts),
390 dev->capture.vc_start_timestamp, mmal_buf->pts,
391 ktime_to_ns(timestamp));
392 buf->vb.vb2_buf.timestamp = ktime_to_ns(timestamp);
393 } else {
394 buf->vb.vb2_buf.timestamp = ktime_get_ns();
395 }
396 buf->vb.sequence = dev->capture.sequence++;
397 buf->vb.field = V4L2_FIELD_NONE;
398
399 vb2_set_plane_payload(&buf->vb.vb2_buf, 0, mmal_buf->length);
400 if (mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_KEYFRAME)
401 buf->vb.flags |= V4L2_BUF_FLAG_KEYFRAME;
402
403 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
404
405 if (mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_EOS &&
406 is_capturing(dev)) {
407 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
408 "Grab another frame as buffer has EOS");
409 vchiq_mmal_port_parameter_set(
410 instance,
411 dev->capture.camera_port,
412 MMAL_PARAMETER_CAPTURE,
413 &dev->capture.frame_count,
414 sizeof(dev->capture.frame_count));
415 }
416 }
417
enable_camera(struct bcm2835_mmal_dev * dev)418 static int enable_camera(struct bcm2835_mmal_dev *dev)
419 {
420 int ret;
421
422 if (!dev->camera_use_count) {
423 ret = vchiq_mmal_port_parameter_set(
424 dev->instance,
425 &dev->component[COMP_CAMERA]->control,
426 MMAL_PARAMETER_CAMERA_NUM, &dev->camera_num,
427 sizeof(dev->camera_num));
428 if (ret < 0) {
429 v4l2_err(&dev->v4l2_dev,
430 "Failed setting camera num, ret %d\n", ret);
431 return -EINVAL;
432 }
433
434 ret = vchiq_mmal_component_enable(dev->instance,
435 dev->component[COMP_CAMERA]);
436 if (ret < 0) {
437 v4l2_err(&dev->v4l2_dev,
438 "Failed enabling camera, ret %d\n", ret);
439 return -EINVAL;
440 }
441 }
442 dev->camera_use_count++;
443 v4l2_dbg(1, bcm2835_v4l2_debug,
444 &dev->v4l2_dev, "enabled camera (refcount %d)\n",
445 dev->camera_use_count);
446 return 0;
447 }
448
disable_camera(struct bcm2835_mmal_dev * dev)449 static int disable_camera(struct bcm2835_mmal_dev *dev)
450 {
451 int ret;
452
453 if (!dev->camera_use_count) {
454 v4l2_err(&dev->v4l2_dev,
455 "Disabled the camera when already disabled\n");
456 return -EINVAL;
457 }
458 dev->camera_use_count--;
459 if (!dev->camera_use_count) {
460 unsigned int i = 0xFFFFFFFF;
461
462 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
463 "Disabling camera\n");
464 ret = vchiq_mmal_component_disable(dev->instance,
465 dev->component[COMP_CAMERA]);
466 if (ret < 0) {
467 v4l2_err(&dev->v4l2_dev,
468 "Failed disabling camera, ret %d\n", ret);
469 return -EINVAL;
470 }
471 vchiq_mmal_port_parameter_set(
472 dev->instance,
473 &dev->component[COMP_CAMERA]->control,
474 MMAL_PARAMETER_CAMERA_NUM, &i,
475 sizeof(i));
476 }
477 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
478 "Camera refcount now %d\n", dev->camera_use_count);
479 return 0;
480 }
481
buffer_queue(struct vb2_buffer * vb)482 static void buffer_queue(struct vb2_buffer *vb)
483 {
484 struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
485 struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
486 struct vb2_mmal_buffer *buf =
487 container_of(vb2, struct vb2_mmal_buffer, vb);
488 int ret;
489
490 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
491 "%s: dev:%p buf:%p, idx %u\n",
492 __func__, dev, buf, vb2->vb2_buf.index);
493
494 ret = vchiq_mmal_submit_buffer(dev->instance, dev->capture.port,
495 &buf->mmal);
496 if (ret < 0)
497 v4l2_err(&dev->v4l2_dev, "%s: error submitting buffer\n",
498 __func__);
499 }
500
start_streaming(struct vb2_queue * vq,unsigned int count)501 static int start_streaming(struct vb2_queue *vq, unsigned int count)
502 {
503 struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vq);
504 int ret;
505 u32 parameter_size;
506
507 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n",
508 __func__, dev);
509
510 /* ensure a format has actually been set */
511 if (!dev->capture.port)
512 return -EINVAL;
513
514 if (enable_camera(dev) < 0) {
515 v4l2_err(&dev->v4l2_dev, "Failed to enable camera\n");
516 return -EINVAL;
517 }
518
519 /*init_completion(&dev->capture.frame_cmplt); */
520
521 /* enable frame capture */
522 dev->capture.frame_count = 1;
523
524 /* reset sequence number */
525 dev->capture.sequence = 0;
526
527 /* if the preview is not already running, wait for a few frames for AGC
528 * to settle down.
529 */
530 if (!dev->component[COMP_PREVIEW]->enabled)
531 msleep(300);
532
533 /* enable the connection from camera to encoder (if applicable) */
534 if (dev->capture.camera_port != dev->capture.port &&
535 dev->capture.camera_port) {
536 ret = vchiq_mmal_port_enable(dev->instance,
537 dev->capture.camera_port, NULL);
538 if (ret) {
539 v4l2_err(&dev->v4l2_dev,
540 "Failed to enable encode tunnel - error %d\n",
541 ret);
542 return -1;
543 }
544 }
545
546 /* Get VC timestamp at this point in time */
547 parameter_size = sizeof(dev->capture.vc_start_timestamp);
548 if (vchiq_mmal_port_parameter_get(dev->instance,
549 dev->capture.camera_port,
550 MMAL_PARAMETER_SYSTEM_TIME,
551 &dev->capture.vc_start_timestamp,
552 ¶meter_size)) {
553 v4l2_err(&dev->v4l2_dev,
554 "Failed to get VC start time - update your VC f/w\n");
555
556 /* Flag to indicate just to rely on kernel timestamps */
557 dev->capture.vc_start_timestamp = -1;
558 } else {
559 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
560 "Start time %lld size %d\n",
561 dev->capture.vc_start_timestamp, parameter_size);
562 }
563
564 dev->capture.kernel_start_ts = ktime_get();
565
566 /* enable the camera port */
567 dev->capture.port->cb_ctx = dev;
568 ret = vchiq_mmal_port_enable(dev->instance, dev->capture.port,
569 buffer_cb);
570 if (ret) {
571 v4l2_err(&dev->v4l2_dev,
572 "Failed to enable capture port - error %d. Disabling camera port again\n",
573 ret);
574
575 vchiq_mmal_port_disable(dev->instance,
576 dev->capture.camera_port);
577 if (disable_camera(dev) < 0) {
578 v4l2_err(&dev->v4l2_dev, "Failed to disable camera\n");
579 return -EINVAL;
580 }
581 return -1;
582 }
583
584 /* capture the first frame */
585 vchiq_mmal_port_parameter_set(dev->instance,
586 dev->capture.camera_port,
587 MMAL_PARAMETER_CAPTURE,
588 &dev->capture.frame_count,
589 sizeof(dev->capture.frame_count));
590 return 0;
591 }
592
593 /* abort streaming and wait for last buffer */
stop_streaming(struct vb2_queue * vq)594 static void stop_streaming(struct vb2_queue *vq)
595 {
596 int ret;
597 unsigned long timeout;
598 struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vq);
599 struct vchiq_mmal_port *port = dev->capture.port;
600
601 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n",
602 __func__, dev);
603
604 init_completion(&dev->capture.frame_cmplt);
605 dev->capture.frame_count = 0;
606
607 /* ensure a format has actually been set */
608 if (!port) {
609 v4l2_err(&dev->v4l2_dev,
610 "no capture port - stream not started?\n");
611 return;
612 }
613
614 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "stopping capturing\n");
615
616 /* stop capturing frames */
617 vchiq_mmal_port_parameter_set(dev->instance,
618 dev->capture.camera_port,
619 MMAL_PARAMETER_CAPTURE,
620 &dev->capture.frame_count,
621 sizeof(dev->capture.frame_count));
622
623 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
624 "disabling connection\n");
625
626 /* disable the connection from camera to encoder */
627 ret = vchiq_mmal_port_disable(dev->instance, dev->capture.camera_port);
628 if (!ret && dev->capture.camera_port != port) {
629 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
630 "disabling port\n");
631 ret = vchiq_mmal_port_disable(dev->instance, port);
632 } else if (dev->capture.camera_port != port) {
633 v4l2_err(&dev->v4l2_dev, "port_disable failed, error %d\n",
634 ret);
635 }
636
637 /* wait for all buffers to be returned */
638 while (atomic_read(&port->buffers_with_vpu)) {
639 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
640 "%s: Waiting for buffers to be returned - %d outstanding\n",
641 __func__, atomic_read(&port->buffers_with_vpu));
642 timeout = wait_for_completion_timeout(&dev->capture.frame_cmplt,
643 HZ);
644 if (timeout == 0) {
645 v4l2_err(&dev->v4l2_dev, "%s: Timeout waiting for buffers to be returned - %d outstanding\n",
646 __func__,
647 atomic_read(&port->buffers_with_vpu));
648 break;
649 }
650 }
651
652 if (disable_camera(dev) < 0)
653 v4l2_err(&dev->v4l2_dev, "Failed to disable camera\n");
654 }
655
656 static const struct vb2_ops bcm2835_mmal_video_qops = {
657 .queue_setup = queue_setup,
658 .buf_init = buffer_init,
659 .buf_prepare = buffer_prepare,
660 .buf_cleanup = buffer_cleanup,
661 .buf_queue = buffer_queue,
662 .start_streaming = start_streaming,
663 .stop_streaming = stop_streaming,
664 .wait_prepare = vb2_ops_wait_prepare,
665 .wait_finish = vb2_ops_wait_finish,
666 };
667
668 /* ------------------------------------------------------------------
669 * IOCTL operations
670 * ------------------------------------------------------------------
671 */
672
set_overlay_params(struct bcm2835_mmal_dev * dev,struct vchiq_mmal_port * port)673 static int set_overlay_params(struct bcm2835_mmal_dev *dev,
674 struct vchiq_mmal_port *port)
675 {
676 struct mmal_parameter_displayregion prev_config = {
677 .set = MMAL_DISPLAY_SET_LAYER |
678 MMAL_DISPLAY_SET_ALPHA |
679 MMAL_DISPLAY_SET_DEST_RECT |
680 MMAL_DISPLAY_SET_FULLSCREEN,
681 .layer = 2,
682 .alpha = dev->overlay.global_alpha,
683 .fullscreen = 0,
684 .dest_rect = {
685 .x = dev->overlay.w.left,
686 .y = dev->overlay.w.top,
687 .width = dev->overlay.w.width,
688 .height = dev->overlay.w.height,
689 },
690 };
691 return vchiq_mmal_port_parameter_set(dev->instance, port,
692 MMAL_PARAMETER_DISPLAYREGION,
693 &prev_config, sizeof(prev_config));
694 }
695
696 /* overlay ioctl */
vidioc_enum_fmt_vid_overlay(struct file * file,void * priv,struct v4l2_fmtdesc * f)697 static int vidioc_enum_fmt_vid_overlay(struct file *file, void *priv,
698 struct v4l2_fmtdesc *f)
699 {
700 struct mmal_fmt *fmt;
701
702 if (f->index >= ARRAY_SIZE(formats))
703 return -EINVAL;
704
705 fmt = &formats[f->index];
706
707 f->pixelformat = fmt->fourcc;
708
709 return 0;
710 }
711
vidioc_g_fmt_vid_overlay(struct file * file,void * priv,struct v4l2_format * f)712 static int vidioc_g_fmt_vid_overlay(struct file *file, void *priv,
713 struct v4l2_format *f)
714 {
715 struct bcm2835_mmal_dev *dev = video_drvdata(file);
716
717 f->fmt.win = dev->overlay;
718
719 return 0;
720 }
721
vidioc_try_fmt_vid_overlay(struct file * file,void * priv,struct v4l2_format * f)722 static int vidioc_try_fmt_vid_overlay(struct file *file, void *priv,
723 struct v4l2_format *f)
724 {
725 struct bcm2835_mmal_dev *dev = video_drvdata(file);
726
727 f->fmt.win.field = V4L2_FIELD_NONE;
728 f->fmt.win.chromakey = 0;
729 f->fmt.win.clips = NULL;
730 f->fmt.win.clipcount = 0;
731 f->fmt.win.bitmap = NULL;
732
733 v4l_bound_align_image(&f->fmt.win.w.width, MIN_WIDTH, dev->max_width, 1,
734 &f->fmt.win.w.height, MIN_HEIGHT, dev->max_height,
735 1, 0);
736 v4l_bound_align_image(&f->fmt.win.w.left, MIN_WIDTH, dev->max_width, 1,
737 &f->fmt.win.w.top, MIN_HEIGHT, dev->max_height,
738 1, 0);
739
740 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
741 "Overlay: Now w/h %dx%d l/t %dx%d\n",
742 f->fmt.win.w.width, f->fmt.win.w.height,
743 f->fmt.win.w.left, f->fmt.win.w.top);
744
745 v4l2_dump_win_format(1,
746 bcm2835_v4l2_debug,
747 &dev->v4l2_dev,
748 &f->fmt.win,
749 __func__);
750 return 0;
751 }
752
vidioc_s_fmt_vid_overlay(struct file * file,void * priv,struct v4l2_format * f)753 static int vidioc_s_fmt_vid_overlay(struct file *file, void *priv,
754 struct v4l2_format *f)
755 {
756 struct bcm2835_mmal_dev *dev = video_drvdata(file);
757
758 vidioc_try_fmt_vid_overlay(file, priv, f);
759
760 dev->overlay = f->fmt.win;
761 if (dev->component[COMP_PREVIEW]->enabled) {
762 set_overlay_params(dev,
763 &dev->component[COMP_PREVIEW]->input[0]);
764 }
765
766 return 0;
767 }
768
vidioc_overlay(struct file * file,void * f,unsigned int on)769 static int vidioc_overlay(struct file *file, void *f, unsigned int on)
770 {
771 int ret;
772 struct bcm2835_mmal_dev *dev = video_drvdata(file);
773 struct vchiq_mmal_port *src;
774 struct vchiq_mmal_port *dst;
775
776 if ((on && dev->component[COMP_PREVIEW]->enabled) ||
777 (!on && !dev->component[COMP_PREVIEW]->enabled))
778 return 0; /* already in requested state */
779
780 src = &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW];
781
782 if (!on) {
783 /* disconnect preview ports and disable component */
784 ret = vchiq_mmal_port_disable(dev->instance, src);
785 if (!ret)
786 ret = vchiq_mmal_port_connect_tunnel(dev->instance, src,
787 NULL);
788 if (ret >= 0)
789 ret = vchiq_mmal_component_disable(
790 dev->instance,
791 dev->component[COMP_PREVIEW]);
792
793 disable_camera(dev);
794 return ret;
795 }
796
797 /* set preview port format and connect it to output */
798 dst = &dev->component[COMP_PREVIEW]->input[0];
799
800 ret = vchiq_mmal_port_set_format(dev->instance, src);
801 if (ret < 0)
802 return ret;
803
804 ret = set_overlay_params(dev, dst);
805 if (ret < 0)
806 return ret;
807
808 if (enable_camera(dev) < 0)
809 return -EINVAL;
810
811 ret = vchiq_mmal_component_enable(dev->instance,
812 dev->component[COMP_PREVIEW]);
813 if (ret < 0)
814 return ret;
815
816 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "connecting %p to %p\n",
817 src, dst);
818 ret = vchiq_mmal_port_connect_tunnel(dev->instance, src, dst);
819 if (ret)
820 return ret;
821
822 return vchiq_mmal_port_enable(dev->instance, src, NULL);
823 }
824
vidioc_g_fbuf(struct file * file,void * fh,struct v4l2_framebuffer * a)825 static int vidioc_g_fbuf(struct file *file, void *fh,
826 struct v4l2_framebuffer *a)
827 {
828 /* The video overlay must stay within the framebuffer and can't be
829 * positioned independently.
830 */
831 struct bcm2835_mmal_dev *dev = video_drvdata(file);
832 struct vchiq_mmal_port *preview_port =
833 &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW];
834
835 a->capability = V4L2_FBUF_CAP_EXTERNOVERLAY |
836 V4L2_FBUF_CAP_GLOBAL_ALPHA;
837 a->flags = V4L2_FBUF_FLAG_OVERLAY;
838 a->fmt.width = preview_port->es.video.width;
839 a->fmt.height = preview_port->es.video.height;
840 a->fmt.pixelformat = V4L2_PIX_FMT_YUV420;
841 a->fmt.bytesperline = preview_port->es.video.width;
842 a->fmt.sizeimage = (preview_port->es.video.width *
843 preview_port->es.video.height * 3) >> 1;
844 a->fmt.colorspace = V4L2_COLORSPACE_SMPTE170M;
845
846 return 0;
847 }
848
849 /* input ioctls */
vidioc_enum_input(struct file * file,void * priv,struct v4l2_input * inp)850 static int vidioc_enum_input(struct file *file, void *priv,
851 struct v4l2_input *inp)
852 {
853 /* only a single camera input */
854 if (inp->index)
855 return -EINVAL;
856
857 inp->type = V4L2_INPUT_TYPE_CAMERA;
858 snprintf((char *)inp->name, sizeof(inp->name), "Camera %u", inp->index);
859 return 0;
860 }
861
vidioc_g_input(struct file * file,void * priv,unsigned int * i)862 static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
863 {
864 *i = 0;
865 return 0;
866 }
867
vidioc_s_input(struct file * file,void * priv,unsigned int i)868 static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
869 {
870 if (i)
871 return -EINVAL;
872
873 return 0;
874 }
875
876 /* capture ioctls */
vidioc_querycap(struct file * file,void * priv,struct v4l2_capability * cap)877 static int vidioc_querycap(struct file *file, void *priv,
878 struct v4l2_capability *cap)
879 {
880 struct bcm2835_mmal_dev *dev = video_drvdata(file);
881 u32 major;
882 u32 minor;
883
884 vchiq_mmal_version(dev->instance, &major, &minor);
885
886 strscpy(cap->driver, "bcm2835 mmal", sizeof(cap->driver));
887 snprintf((char *)cap->card, sizeof(cap->card), "mmal service %d.%d", major, minor);
888
889 snprintf((char *)cap->bus_info, sizeof(cap->bus_info), "platform:%s", dev->v4l2_dev.name);
890 return 0;
891 }
892
vidioc_enum_fmt_vid_cap(struct file * file,void * priv,struct v4l2_fmtdesc * f)893 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
894 struct v4l2_fmtdesc *f)
895 {
896 struct mmal_fmt *fmt;
897
898 if (f->index >= ARRAY_SIZE(formats))
899 return -EINVAL;
900
901 fmt = &formats[f->index];
902
903 f->pixelformat = fmt->fourcc;
904
905 return 0;
906 }
907
vidioc_g_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)908 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
909 struct v4l2_format *f)
910 {
911 struct bcm2835_mmal_dev *dev = video_drvdata(file);
912
913 f->fmt.pix.width = dev->capture.width;
914 f->fmt.pix.height = dev->capture.height;
915 f->fmt.pix.field = V4L2_FIELD_NONE;
916 f->fmt.pix.pixelformat = dev->capture.fmt->fourcc;
917 f->fmt.pix.bytesperline = dev->capture.stride;
918 f->fmt.pix.sizeimage = dev->capture.buffersize;
919
920 if (dev->capture.fmt->fourcc == V4L2_PIX_FMT_RGB24)
921 f->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB;
922 else if (dev->capture.fmt->fourcc == V4L2_PIX_FMT_JPEG)
923 f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
924 else
925 f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
926 f->fmt.pix.priv = 0;
927
928 v4l2_dump_pix_format(1, bcm2835_v4l2_debug, &dev->v4l2_dev, &f->fmt.pix,
929 __func__);
930 return 0;
931 }
932
vidioc_try_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)933 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
934 struct v4l2_format *f)
935 {
936 struct bcm2835_mmal_dev *dev = video_drvdata(file);
937 struct mmal_fmt *mfmt;
938
939 mfmt = get_format(f);
940 if (!mfmt) {
941 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
942 "Fourcc format (0x%08x) unknown.\n",
943 f->fmt.pix.pixelformat);
944 f->fmt.pix.pixelformat = formats[0].fourcc;
945 mfmt = get_format(f);
946 }
947
948 f->fmt.pix.field = V4L2_FIELD_NONE;
949
950 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
951 "Clipping/aligning %dx%d format %08X\n",
952 f->fmt.pix.width, f->fmt.pix.height, f->fmt.pix.pixelformat);
953
954 v4l_bound_align_image(&f->fmt.pix.width, MIN_WIDTH, dev->max_width, 1,
955 &f->fmt.pix.height, MIN_HEIGHT, dev->max_height,
956 1, 0);
957 f->fmt.pix.bytesperline = f->fmt.pix.width * mfmt->ybbp;
958 if (!mfmt->remove_padding) {
959 if (mfmt->depth == 24) {
960 /*
961 * 24bpp is a pain as we can't use simple masking.
962 * Min stride is width aligned to 16, times 24bpp.
963 */
964 f->fmt.pix.bytesperline =
965 ((f->fmt.pix.width + 15) & ~15) * 3;
966 } else {
967 /*
968 * GPU isn't removing padding, so stride is aligned to
969 * 32
970 */
971 int align_mask = ((32 * mfmt->depth) >> 3) - 1;
972
973 f->fmt.pix.bytesperline =
974 (f->fmt.pix.bytesperline + align_mask) &
975 ~align_mask;
976 }
977 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
978 "Not removing padding, so bytes/line = %d\n",
979 f->fmt.pix.bytesperline);
980 }
981
982 /* Image buffer has to be padded to allow for alignment, even though
983 * we sometimes then remove that padding before delivering the buffer.
984 */
985 f->fmt.pix.sizeimage = ((f->fmt.pix.height + 15) & ~15) *
986 (((f->fmt.pix.width + 31) & ~31) * mfmt->depth) >> 3;
987
988 if ((mfmt->flags & V4L2_FMT_FLAG_COMPRESSED) &&
989 f->fmt.pix.sizeimage < MIN_BUFFER_SIZE)
990 f->fmt.pix.sizeimage = MIN_BUFFER_SIZE;
991
992 if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_RGB24)
993 f->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB;
994 else if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_JPEG)
995 f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
996 else
997 f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
998 f->fmt.pix.priv = 0;
999
1000 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
1001 "Now %dx%d format %08X\n",
1002 f->fmt.pix.width, f->fmt.pix.height, f->fmt.pix.pixelformat);
1003
1004 v4l2_dump_pix_format(1, bcm2835_v4l2_debug, &dev->v4l2_dev, &f->fmt.pix,
1005 __func__);
1006 return 0;
1007 }
1008
1009
mmal_setup_video_component(struct bcm2835_mmal_dev * dev,struct v4l2_format * f)1010 static int mmal_setup_video_component(struct bcm2835_mmal_dev *dev,
1011 struct v4l2_format *f)
1012 {
1013 bool overlay_enabled = !!dev->component[COMP_PREVIEW]->enabled;
1014 struct vchiq_mmal_port *preview_port;
1015 int ret;
1016
1017 preview_port = &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW];
1018
1019 /* Preview and encode ports need to match on resolution */
1020 if (overlay_enabled) {
1021 /* Need to disable the overlay before we can update
1022 * the resolution
1023 */
1024 ret = vchiq_mmal_port_disable(dev->instance, preview_port);
1025 if (!ret) {
1026 ret = vchiq_mmal_port_connect_tunnel(dev->instance,
1027 preview_port,
1028 NULL);
1029 }
1030 }
1031 preview_port->es.video.width = f->fmt.pix.width;
1032 preview_port->es.video.height = f->fmt.pix.height;
1033 preview_port->es.video.crop.x = 0;
1034 preview_port->es.video.crop.y = 0;
1035 preview_port->es.video.crop.width = f->fmt.pix.width;
1036 preview_port->es.video.crop.height = f->fmt.pix.height;
1037 preview_port->es.video.frame_rate.numerator =
1038 dev->capture.timeperframe.denominator;
1039 preview_port->es.video.frame_rate.denominator =
1040 dev->capture.timeperframe.numerator;
1041 ret = vchiq_mmal_port_set_format(dev->instance, preview_port);
1042
1043 if (overlay_enabled) {
1044 ret = vchiq_mmal_port_connect_tunnel(dev->instance,
1045 preview_port,
1046 &dev->component[COMP_PREVIEW]->input[0]);
1047 if (ret)
1048 return ret;
1049
1050 ret = vchiq_mmal_port_enable(dev->instance, preview_port, NULL);
1051 }
1052
1053 return ret;
1054 }
1055
mmal_setup_encode_component(struct bcm2835_mmal_dev * dev,struct v4l2_format * f,struct vchiq_mmal_port * port,struct vchiq_mmal_port * camera_port,struct vchiq_mmal_component * component)1056 static int mmal_setup_encode_component(struct bcm2835_mmal_dev *dev,
1057 struct v4l2_format *f,
1058 struct vchiq_mmal_port *port,
1059 struct vchiq_mmal_port *camera_port,
1060 struct vchiq_mmal_component *component)
1061 {
1062 struct mmal_fmt *mfmt = get_format(f);
1063 int ret;
1064
1065 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
1066 "vid_cap - set up encode comp\n");
1067
1068 /* configure buffering */
1069 camera_port->current_buffer.size = camera_port->recommended_buffer.size;
1070 camera_port->current_buffer.num = camera_port->recommended_buffer.num;
1071
1072 ret = vchiq_mmal_port_connect_tunnel(dev->instance, camera_port,
1073 &component->input[0]);
1074 if (ret) {
1075 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
1076 "%s failed to create connection\n", __func__);
1077 /* ensure capture is not going to be tried */
1078 dev->capture.port = NULL;
1079 return ret;
1080 }
1081
1082 port->es.video.width = f->fmt.pix.width;
1083 port->es.video.height = f->fmt.pix.height;
1084 port->es.video.crop.x = 0;
1085 port->es.video.crop.y = 0;
1086 port->es.video.crop.width = f->fmt.pix.width;
1087 port->es.video.crop.height = f->fmt.pix.height;
1088 port->es.video.frame_rate.numerator =
1089 dev->capture.timeperframe.denominator;
1090 port->es.video.frame_rate.denominator =
1091 dev->capture.timeperframe.numerator;
1092
1093 port->format.encoding = mfmt->mmal;
1094 port->format.encoding_variant = 0;
1095 /* Set any encoding specific parameters */
1096 switch (mfmt->mmal_component) {
1097 case COMP_VIDEO_ENCODE:
1098 port->format.bitrate = dev->capture.encode_bitrate;
1099 break;
1100 case COMP_IMAGE_ENCODE:
1101 /* Could set EXIF parameters here */
1102 break;
1103 default:
1104 break;
1105 }
1106
1107 ret = vchiq_mmal_port_set_format(dev->instance, port);
1108 if (ret) {
1109 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
1110 "%s failed to set format %dx%d fmt %08X\n",
1111 __func__,
1112 f->fmt.pix.width,
1113 f->fmt.pix.height,
1114 f->fmt.pix.pixelformat);
1115 return ret;
1116 }
1117
1118 ret = vchiq_mmal_component_enable(dev->instance, component);
1119 if (ret) {
1120 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
1121 "%s Failed to enable encode components\n", __func__);
1122 return ret;
1123 }
1124
1125 /* configure buffering */
1126 port->current_buffer.num = 1;
1127 port->current_buffer.size = f->fmt.pix.sizeimage;
1128 if (port->format.encoding == MMAL_ENCODING_JPEG) {
1129 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
1130 "JPG - buf size now %d was %d\n",
1131 f->fmt.pix.sizeimage,
1132 port->current_buffer.size);
1133 port->current_buffer.size =
1134 (f->fmt.pix.sizeimage < (100 << 10)) ?
1135 (100 << 10) : f->fmt.pix.sizeimage;
1136 }
1137 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
1138 "vid_cap - cur_buf.size set to %d\n", f->fmt.pix.sizeimage);
1139 port->current_buffer.alignment = 0;
1140
1141 return 0;
1142 }
1143
mmal_setup_components(struct bcm2835_mmal_dev * dev,struct v4l2_format * f)1144 static int mmal_setup_components(struct bcm2835_mmal_dev *dev,
1145 struct v4l2_format *f)
1146 {
1147 int ret;
1148 struct vchiq_mmal_port *port = NULL, *camera_port = NULL;
1149 struct vchiq_mmal_component *encode_component = NULL;
1150 struct mmal_fmt *mfmt = get_format(f);
1151 bool remove_padding;
1152
1153 if (!mfmt)
1154 return -EINVAL;
1155
1156 if (dev->capture.encode_component) {
1157 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
1158 "vid_cap - disconnect previous tunnel\n");
1159
1160 /* Disconnect any previous connection */
1161 vchiq_mmal_port_connect_tunnel(dev->instance,
1162 dev->capture.camera_port, NULL);
1163 dev->capture.camera_port = NULL;
1164 ret = vchiq_mmal_component_disable(dev->instance,
1165 dev->capture.encode_component);
1166 if (ret)
1167 v4l2_err(&dev->v4l2_dev,
1168 "Failed to disable encode component %d\n",
1169 ret);
1170
1171 dev->capture.encode_component = NULL;
1172 }
1173 /* format dependent port setup */
1174 switch (mfmt->mmal_component) {
1175 case COMP_CAMERA:
1176 /* Make a further decision on port based on resolution */
1177 if (f->fmt.pix.width <= max_video_width &&
1178 f->fmt.pix.height <= max_video_height)
1179 camera_port =
1180 &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO];
1181 else
1182 camera_port =
1183 &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE];
1184 port = camera_port;
1185 break;
1186 case COMP_IMAGE_ENCODE:
1187 encode_component = dev->component[COMP_IMAGE_ENCODE];
1188 port = &dev->component[COMP_IMAGE_ENCODE]->output[0];
1189 camera_port =
1190 &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE];
1191 break;
1192 case COMP_VIDEO_ENCODE:
1193 encode_component = dev->component[COMP_VIDEO_ENCODE];
1194 port = &dev->component[COMP_VIDEO_ENCODE]->output[0];
1195 camera_port =
1196 &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO];
1197 break;
1198 default:
1199 break;
1200 }
1201
1202 if (!port)
1203 return -EINVAL;
1204
1205 if (encode_component)
1206 camera_port->format.encoding = MMAL_ENCODING_OPAQUE;
1207 else
1208 camera_port->format.encoding = mfmt->mmal;
1209
1210 if (dev->rgb_bgr_swapped) {
1211 if (camera_port->format.encoding == MMAL_ENCODING_RGB24)
1212 camera_port->format.encoding = MMAL_ENCODING_BGR24;
1213 else if (camera_port->format.encoding == MMAL_ENCODING_BGR24)
1214 camera_port->format.encoding = MMAL_ENCODING_RGB24;
1215 }
1216
1217 remove_padding = mfmt->remove_padding;
1218 vchiq_mmal_port_parameter_set(dev->instance, camera_port,
1219 MMAL_PARAMETER_NO_IMAGE_PADDING,
1220 &remove_padding, sizeof(remove_padding));
1221
1222 camera_port->format.encoding_variant = 0;
1223 camera_port->es.video.width = f->fmt.pix.width;
1224 camera_port->es.video.height = f->fmt.pix.height;
1225 camera_port->es.video.crop.x = 0;
1226 camera_port->es.video.crop.y = 0;
1227 camera_port->es.video.crop.width = f->fmt.pix.width;
1228 camera_port->es.video.crop.height = f->fmt.pix.height;
1229 camera_port->es.video.frame_rate.numerator = 0;
1230 camera_port->es.video.frame_rate.denominator = 1;
1231 camera_port->es.video.color_space = MMAL_COLOR_SPACE_JPEG_JFIF;
1232
1233 ret = vchiq_mmal_port_set_format(dev->instance, camera_port);
1234
1235 if (!ret &&
1236 camera_port ==
1237 &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO]) {
1238 ret = mmal_setup_video_component(dev, f);
1239 }
1240
1241 if (ret) {
1242 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
1243 "%s failed to set format %dx%d %08X\n", __func__,
1244 f->fmt.pix.width, f->fmt.pix.height,
1245 f->fmt.pix.pixelformat);
1246 /* ensure capture is not going to be tried */
1247 dev->capture.port = NULL;
1248 return ret;
1249 }
1250
1251 if (encode_component) {
1252 ret = mmal_setup_encode_component(dev, f, port,
1253 camera_port,
1254 encode_component);
1255
1256 if (ret)
1257 return ret;
1258 } else {
1259 /* configure buffering */
1260 camera_port->current_buffer.num = 1;
1261 camera_port->current_buffer.size = f->fmt.pix.sizeimage;
1262 camera_port->current_buffer.alignment = 0;
1263 }
1264
1265 dev->capture.fmt = mfmt;
1266 dev->capture.stride = f->fmt.pix.bytesperline;
1267 dev->capture.width = camera_port->es.video.crop.width;
1268 dev->capture.height = camera_port->es.video.crop.height;
1269 dev->capture.buffersize = port->current_buffer.size;
1270
1271 /* select port for capture */
1272 dev->capture.port = port;
1273 dev->capture.camera_port = camera_port;
1274 dev->capture.encode_component = encode_component;
1275 v4l2_dbg(1, bcm2835_v4l2_debug,
1276 &dev->v4l2_dev,
1277 "Set dev->capture.fmt %08X, %dx%d, stride %d, size %d",
1278 port->format.encoding,
1279 dev->capture.width, dev->capture.height,
1280 dev->capture.stride, dev->capture.buffersize);
1281
1282 /* todo: Need to convert the vchiq/mmal error into a v4l2 error. */
1283 return ret;
1284 }
1285
vidioc_s_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)1286 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
1287 struct v4l2_format *f)
1288 {
1289 int ret;
1290 struct bcm2835_mmal_dev *dev = video_drvdata(file);
1291 struct mmal_fmt *mfmt;
1292
1293 /* try the format to set valid parameters */
1294 ret = vidioc_try_fmt_vid_cap(file, priv, f);
1295 if (ret) {
1296 v4l2_err(&dev->v4l2_dev,
1297 "vid_cap - vidioc_try_fmt_vid_cap failed\n");
1298 return ret;
1299 }
1300
1301 /* if a capture is running refuse to set format */
1302 if (vb2_is_busy(&dev->capture.vb_vidq)) {
1303 v4l2_info(&dev->v4l2_dev, "%s device busy\n", __func__);
1304 return -EBUSY;
1305 }
1306
1307 /* If the format is unsupported v4l2 says we should switch to
1308 * a supported one and not return an error.
1309 */
1310 mfmt = get_format(f);
1311 if (!mfmt) {
1312 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
1313 "Fourcc format (0x%08x) unknown.\n",
1314 f->fmt.pix.pixelformat);
1315 f->fmt.pix.pixelformat = formats[0].fourcc;
1316 mfmt = get_format(f);
1317 }
1318
1319 ret = mmal_setup_components(dev, f);
1320 if (ret) {
1321 v4l2_err(&dev->v4l2_dev,
1322 "%s: failed to setup mmal components: %d\n",
1323 __func__, ret);
1324 ret = -EINVAL;
1325 }
1326
1327 return ret;
1328 }
1329
vidioc_enum_framesizes(struct file * file,void * fh,struct v4l2_frmsizeenum * fsize)1330 static int vidioc_enum_framesizes(struct file *file, void *fh,
1331 struct v4l2_frmsizeenum *fsize)
1332 {
1333 struct bcm2835_mmal_dev *dev = video_drvdata(file);
1334 static const struct v4l2_frmsize_stepwise sizes = {
1335 MIN_WIDTH, 0, 2,
1336 MIN_HEIGHT, 0, 2
1337 };
1338 int i;
1339
1340 if (fsize->index)
1341 return -EINVAL;
1342 for (i = 0; i < ARRAY_SIZE(formats); i++)
1343 if (formats[i].fourcc == fsize->pixel_format)
1344 break;
1345 if (i == ARRAY_SIZE(formats))
1346 return -EINVAL;
1347 fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
1348 fsize->stepwise = sizes;
1349 fsize->stepwise.max_width = dev->max_width;
1350 fsize->stepwise.max_height = dev->max_height;
1351 return 0;
1352 }
1353
1354 /* timeperframe is arbitrary and continuous */
vidioc_enum_frameintervals(struct file * file,void * priv,struct v4l2_frmivalenum * fival)1355 static int vidioc_enum_frameintervals(struct file *file, void *priv,
1356 struct v4l2_frmivalenum *fival)
1357 {
1358 struct bcm2835_mmal_dev *dev = video_drvdata(file);
1359 int i;
1360
1361 if (fival->index)
1362 return -EINVAL;
1363
1364 for (i = 0; i < ARRAY_SIZE(formats); i++)
1365 if (formats[i].fourcc == fival->pixel_format)
1366 break;
1367 if (i == ARRAY_SIZE(formats))
1368 return -EINVAL;
1369
1370 /* regarding width & height - we support any within range */
1371 if (fival->width < MIN_WIDTH || fival->width > dev->max_width ||
1372 fival->height < MIN_HEIGHT || fival->height > dev->max_height)
1373 return -EINVAL;
1374
1375 fival->type = V4L2_FRMIVAL_TYPE_CONTINUOUS;
1376
1377 /* fill in stepwise (step=1.0 is required by V4L2 spec) */
1378 fival->stepwise.min = tpf_min;
1379 fival->stepwise.max = tpf_max;
1380 fival->stepwise.step = (struct v4l2_fract) {1, 1};
1381
1382 return 0;
1383 }
1384
vidioc_g_parm(struct file * file,void * priv,struct v4l2_streamparm * parm)1385 static int vidioc_g_parm(struct file *file, void *priv,
1386 struct v4l2_streamparm *parm)
1387 {
1388 struct bcm2835_mmal_dev *dev = video_drvdata(file);
1389
1390 if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
1391 return -EINVAL;
1392
1393 parm->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
1394 parm->parm.capture.timeperframe = dev->capture.timeperframe;
1395 parm->parm.capture.readbuffers = 1;
1396 return 0;
1397 }
1398
vidioc_s_parm(struct file * file,void * priv,struct v4l2_streamparm * parm)1399 static int vidioc_s_parm(struct file *file, void *priv,
1400 struct v4l2_streamparm *parm)
1401 {
1402 struct bcm2835_mmal_dev *dev = video_drvdata(file);
1403 struct v4l2_fract tpf;
1404
1405 if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
1406 return -EINVAL;
1407
1408 tpf = parm->parm.capture.timeperframe;
1409
1410 /* tpf: {*, 0} resets timing; clip to [min, max]*/
1411 tpf = tpf.denominator ? tpf : tpf_default;
1412 tpf = V4L2_FRACT_COMPARE(tpf, <, tpf_min) ? tpf_min : tpf;
1413 tpf = V4L2_FRACT_COMPARE(tpf, >, tpf_max) ? tpf_max : tpf;
1414
1415 dev->capture.timeperframe = tpf;
1416 parm->parm.capture.timeperframe = tpf;
1417 parm->parm.capture.readbuffers = 1;
1418 parm->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
1419
1420 set_framerate_params(dev);
1421
1422 return 0;
1423 }
1424
1425 static const struct v4l2_ioctl_ops camera0_ioctl_ops = {
1426 /* overlay */
1427 .vidioc_enum_fmt_vid_overlay = vidioc_enum_fmt_vid_overlay,
1428 .vidioc_g_fmt_vid_overlay = vidioc_g_fmt_vid_overlay,
1429 .vidioc_try_fmt_vid_overlay = vidioc_try_fmt_vid_overlay,
1430 .vidioc_s_fmt_vid_overlay = vidioc_s_fmt_vid_overlay,
1431 .vidioc_overlay = vidioc_overlay,
1432 .vidioc_g_fbuf = vidioc_g_fbuf,
1433
1434 /* inputs */
1435 .vidioc_enum_input = vidioc_enum_input,
1436 .vidioc_g_input = vidioc_g_input,
1437 .vidioc_s_input = vidioc_s_input,
1438
1439 /* capture */
1440 .vidioc_querycap = vidioc_querycap,
1441 .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
1442 .vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap,
1443 .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
1444 .vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap,
1445
1446 /* buffer management */
1447 .vidioc_reqbufs = vb2_ioctl_reqbufs,
1448 .vidioc_create_bufs = vb2_ioctl_create_bufs,
1449 .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
1450 .vidioc_querybuf = vb2_ioctl_querybuf,
1451 .vidioc_qbuf = vb2_ioctl_qbuf,
1452 .vidioc_dqbuf = vb2_ioctl_dqbuf,
1453 .vidioc_enum_framesizes = vidioc_enum_framesizes,
1454 .vidioc_enum_frameintervals = vidioc_enum_frameintervals,
1455 .vidioc_g_parm = vidioc_g_parm,
1456 .vidioc_s_parm = vidioc_s_parm,
1457 .vidioc_streamon = vb2_ioctl_streamon,
1458 .vidioc_streamoff = vb2_ioctl_streamoff,
1459
1460 .vidioc_log_status = v4l2_ctrl_log_status,
1461 .vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
1462 .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
1463 };
1464
1465 /* ------------------------------------------------------------------
1466 * Driver init/finalise
1467 * ------------------------------------------------------------------
1468 */
1469
1470 static const struct v4l2_file_operations camera0_fops = {
1471 .owner = THIS_MODULE,
1472 .open = v4l2_fh_open,
1473 .release = vb2_fop_release,
1474 .read = vb2_fop_read,
1475 .poll = vb2_fop_poll,
1476 .unlocked_ioctl = video_ioctl2, /* V4L2 ioctl handler */
1477 .mmap = vb2_fop_mmap,
1478 };
1479
1480 static const struct video_device vdev_template = {
1481 .name = "camera0",
1482 .fops = &camera0_fops,
1483 .ioctl_ops = &camera0_ioctl_ops,
1484 .release = video_device_release_empty,
1485 .device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OVERLAY |
1486 V4L2_CAP_STREAMING | V4L2_CAP_READWRITE,
1487 };
1488
1489 /* Returns the number of cameras, and also the max resolution supported
1490 * by those cameras.
1491 */
get_num_cameras(struct vchiq_mmal_instance * instance,unsigned int resolutions[][2],int num_resolutions)1492 static int get_num_cameras(struct vchiq_mmal_instance *instance,
1493 unsigned int resolutions[][2], int num_resolutions)
1494 {
1495 int ret;
1496 struct vchiq_mmal_component *cam_info_component;
1497 struct mmal_parameter_camera_info cam_info = {0};
1498 u32 param_size = sizeof(cam_info);
1499 int i;
1500
1501 /* create a camera_info component */
1502 ret = vchiq_mmal_component_init(instance, "camera_info",
1503 &cam_info_component);
1504 if (ret < 0)
1505 /* Unusual failure - let's guess one camera. */
1506 return 1;
1507
1508 if (vchiq_mmal_port_parameter_get(instance,
1509 &cam_info_component->control,
1510 MMAL_PARAMETER_CAMERA_INFO,
1511 &cam_info,
1512 ¶m_size)) {
1513 pr_info("Failed to get camera info\n");
1514 }
1515 for (i = 0;
1516 i < min_t(unsigned int, cam_info.num_cameras, num_resolutions);
1517 i++) {
1518 resolutions[i][0] = cam_info.cameras[i].max_width;
1519 resolutions[i][1] = cam_info.cameras[i].max_height;
1520 }
1521
1522 vchiq_mmal_component_finalise(instance,
1523 cam_info_component);
1524
1525 return cam_info.num_cameras;
1526 }
1527
set_camera_parameters(struct vchiq_mmal_instance * instance,struct vchiq_mmal_component * camera,struct bcm2835_mmal_dev * dev)1528 static int set_camera_parameters(struct vchiq_mmal_instance *instance,
1529 struct vchiq_mmal_component *camera,
1530 struct bcm2835_mmal_dev *dev)
1531 {
1532 struct mmal_parameter_camera_config cam_config = {
1533 .max_stills_w = dev->max_width,
1534 .max_stills_h = dev->max_height,
1535 .stills_yuv422 = 1,
1536 .one_shot_stills = 1,
1537 .max_preview_video_w = (max_video_width > 1920) ?
1538 max_video_width : 1920,
1539 .max_preview_video_h = (max_video_height > 1088) ?
1540 max_video_height : 1088,
1541 .num_preview_video_frames = 3,
1542 .stills_capture_circular_buffer_height = 0,
1543 .fast_preview_resume = 0,
1544 .use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RAW_STC
1545 };
1546
1547 return vchiq_mmal_port_parameter_set(instance, &camera->control,
1548 MMAL_PARAMETER_CAMERA_CONFIG,
1549 &cam_config, sizeof(cam_config));
1550 }
1551
1552 #define MAX_SUPPORTED_ENCODINGS 20
1553
1554 /* MMAL instance and component init */
mmal_init(struct bcm2835_mmal_dev * dev)1555 static int mmal_init(struct bcm2835_mmal_dev *dev)
1556 {
1557 int ret;
1558 struct mmal_es_format_local *format;
1559 u32 supported_encodings[MAX_SUPPORTED_ENCODINGS];
1560 u32 param_size;
1561 struct vchiq_mmal_component *camera;
1562
1563 ret = vchiq_mmal_init(&dev->instance);
1564 if (ret < 0) {
1565 v4l2_err(&dev->v4l2_dev, "%s: vchiq mmal init failed %d\n",
1566 __func__, ret);
1567 return ret;
1568 }
1569
1570 /* get the camera component ready */
1571 ret = vchiq_mmal_component_init(dev->instance, "ril.camera",
1572 &dev->component[COMP_CAMERA]);
1573 if (ret < 0)
1574 goto unreg_mmal;
1575
1576 camera = dev->component[COMP_CAMERA];
1577 if (camera->outputs < CAM_PORT_COUNT) {
1578 v4l2_err(&dev->v4l2_dev, "%s: too few camera outputs %d needed %d\n",
1579 __func__, camera->outputs, CAM_PORT_COUNT);
1580 ret = -EINVAL;
1581 goto unreg_camera;
1582 }
1583
1584 ret = set_camera_parameters(dev->instance,
1585 camera,
1586 dev);
1587 if (ret < 0) {
1588 v4l2_err(&dev->v4l2_dev, "%s: unable to set camera parameters: %d\n",
1589 __func__, ret);
1590 goto unreg_camera;
1591 }
1592
1593 /* There was an error in the firmware that meant the camera component
1594 * produced BGR instead of RGB.
1595 * This is now fixed, but in order to support the old firmwares, we
1596 * have to check.
1597 */
1598 dev->rgb_bgr_swapped = true;
1599 param_size = sizeof(supported_encodings);
1600 ret = vchiq_mmal_port_parameter_get(dev->instance,
1601 &camera->output[CAM_PORT_CAPTURE],
1602 MMAL_PARAMETER_SUPPORTED_ENCODINGS,
1603 &supported_encodings,
1604 ¶m_size);
1605 if (ret == 0) {
1606 int i;
1607
1608 for (i = 0; i < param_size / sizeof(u32); i++) {
1609 if (supported_encodings[i] == MMAL_ENCODING_BGR24) {
1610 /* Found BGR24 first - old firmware. */
1611 break;
1612 }
1613 if (supported_encodings[i] == MMAL_ENCODING_RGB24) {
1614 /* Found RGB24 first
1615 * new firmware, so use RGB24.
1616 */
1617 dev->rgb_bgr_swapped = false;
1618 break;
1619 }
1620 }
1621 }
1622 format = &camera->output[CAM_PORT_PREVIEW].format;
1623
1624 format->encoding = MMAL_ENCODING_OPAQUE;
1625 format->encoding_variant = MMAL_ENCODING_I420;
1626
1627 format->es->video.width = 1024;
1628 format->es->video.height = 768;
1629 format->es->video.crop.x = 0;
1630 format->es->video.crop.y = 0;
1631 format->es->video.crop.width = 1024;
1632 format->es->video.crop.height = 768;
1633 format->es->video.frame_rate.numerator = 0; /* Rely on fps_range */
1634 format->es->video.frame_rate.denominator = 1;
1635
1636 format = &camera->output[CAM_PORT_VIDEO].format;
1637
1638 format->encoding = MMAL_ENCODING_OPAQUE;
1639 format->encoding_variant = MMAL_ENCODING_I420;
1640
1641 format->es->video.width = 1024;
1642 format->es->video.height = 768;
1643 format->es->video.crop.x = 0;
1644 format->es->video.crop.y = 0;
1645 format->es->video.crop.width = 1024;
1646 format->es->video.crop.height = 768;
1647 format->es->video.frame_rate.numerator = 0; /* Rely on fps_range */
1648 format->es->video.frame_rate.denominator = 1;
1649
1650 format = &camera->output[CAM_PORT_CAPTURE].format;
1651
1652 format->encoding = MMAL_ENCODING_OPAQUE;
1653
1654 format->es->video.width = 2592;
1655 format->es->video.height = 1944;
1656 format->es->video.crop.x = 0;
1657 format->es->video.crop.y = 0;
1658 format->es->video.crop.width = 2592;
1659 format->es->video.crop.height = 1944;
1660 format->es->video.frame_rate.numerator = 0; /* Rely on fps_range */
1661 format->es->video.frame_rate.denominator = 1;
1662
1663 dev->capture.width = format->es->video.width;
1664 dev->capture.height = format->es->video.height;
1665 dev->capture.fmt = &formats[0];
1666 dev->capture.encode_component = NULL;
1667 dev->capture.timeperframe = tpf_default;
1668 dev->capture.enc_profile = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH;
1669 dev->capture.enc_level = V4L2_MPEG_VIDEO_H264_LEVEL_4_0;
1670
1671 /* get the preview component ready */
1672 ret = vchiq_mmal_component_init(dev->instance, "ril.video_render",
1673 &dev->component[COMP_PREVIEW]);
1674 if (ret < 0)
1675 goto unreg_camera;
1676
1677 if (dev->component[COMP_PREVIEW]->inputs < 1) {
1678 ret = -EINVAL;
1679 v4l2_err(&dev->v4l2_dev, "%s: too few input ports %d needed %d\n",
1680 __func__, dev->component[COMP_PREVIEW]->inputs, 1);
1681 goto unreg_preview;
1682 }
1683
1684 /* get the image encoder component ready */
1685 ret = vchiq_mmal_component_init(dev->instance, "ril.image_encode",
1686 &dev->component[COMP_IMAGE_ENCODE]);
1687 if (ret < 0)
1688 goto unreg_preview;
1689
1690 if (dev->component[COMP_IMAGE_ENCODE]->inputs < 1) {
1691 ret = -EINVAL;
1692 v4l2_err(&dev->v4l2_dev, "%s: too few input ports %d needed %d\n",
1693 __func__, dev->component[COMP_IMAGE_ENCODE]->inputs,
1694 1);
1695 goto unreg_image_encoder;
1696 }
1697
1698 /* get the video encoder component ready */
1699 ret = vchiq_mmal_component_init(dev->instance, "ril.video_encode",
1700 &dev->component[COMP_VIDEO_ENCODE]);
1701 if (ret < 0)
1702 goto unreg_image_encoder;
1703
1704 if (dev->component[COMP_VIDEO_ENCODE]->inputs < 1) {
1705 ret = -EINVAL;
1706 v4l2_err(&dev->v4l2_dev, "%s: too few input ports %d needed %d\n",
1707 __func__, dev->component[COMP_VIDEO_ENCODE]->inputs,
1708 1);
1709 goto unreg_vid_encoder;
1710 }
1711
1712 {
1713 struct vchiq_mmal_port *encoder_port =
1714 &dev->component[COMP_VIDEO_ENCODE]->output[0];
1715 encoder_port->format.encoding = MMAL_ENCODING_H264;
1716 ret = vchiq_mmal_port_set_format(dev->instance,
1717 encoder_port);
1718 }
1719
1720 {
1721 unsigned int enable = 1;
1722
1723 vchiq_mmal_port_parameter_set(
1724 dev->instance,
1725 &dev->component[COMP_VIDEO_ENCODE]->control,
1726 MMAL_PARAMETER_VIDEO_IMMUTABLE_INPUT,
1727 &enable, sizeof(enable));
1728
1729 vchiq_mmal_port_parameter_set(dev->instance,
1730 &dev->component[COMP_VIDEO_ENCODE]->control,
1731 MMAL_PARAMETER_MINIMISE_FRAGMENTATION,
1732 &enable,
1733 sizeof(enable));
1734 }
1735 ret = bcm2835_mmal_set_all_camera_controls(dev);
1736 if (ret < 0) {
1737 v4l2_err(&dev->v4l2_dev, "%s: failed to set all camera controls: %d\n",
1738 __func__, ret);
1739 goto unreg_vid_encoder;
1740 }
1741
1742 return 0;
1743
1744 unreg_vid_encoder:
1745 pr_err("Cleanup: Destroy video encoder\n");
1746 vchiq_mmal_component_finalise(dev->instance,
1747 dev->component[COMP_VIDEO_ENCODE]);
1748
1749 unreg_image_encoder:
1750 pr_err("Cleanup: Destroy image encoder\n");
1751 vchiq_mmal_component_finalise(dev->instance,
1752 dev->component[COMP_IMAGE_ENCODE]);
1753
1754 unreg_preview:
1755 pr_err("Cleanup: Destroy video render\n");
1756 vchiq_mmal_component_finalise(dev->instance,
1757 dev->component[COMP_PREVIEW]);
1758
1759 unreg_camera:
1760 pr_err("Cleanup: Destroy camera\n");
1761 vchiq_mmal_component_finalise(dev->instance,
1762 dev->component[COMP_CAMERA]);
1763
1764 unreg_mmal:
1765 vchiq_mmal_finalise(dev->instance);
1766 return ret;
1767 }
1768
bcm2835_mmal_init_device(struct bcm2835_mmal_dev * dev,struct video_device * vfd)1769 static int bcm2835_mmal_init_device(struct bcm2835_mmal_dev *dev, struct video_device *vfd)
1770 {
1771 int ret;
1772
1773 *vfd = vdev_template;
1774
1775 vfd->v4l2_dev = &dev->v4l2_dev;
1776
1777 vfd->lock = &dev->mutex;
1778
1779 vfd->queue = &dev->capture.vb_vidq;
1780
1781 /* video device needs to be able to access instance data */
1782 video_set_drvdata(vfd, dev);
1783
1784 ret = video_register_device(vfd, VFL_TYPE_VIDEO,
1785 video_nr[dev->camera_num]);
1786 if (ret < 0)
1787 return ret;
1788
1789 v4l2_info(vfd->v4l2_dev,
1790 "V4L2 device registered as %s - stills mode > %dx%d\n",
1791 video_device_node_name(vfd),
1792 max_video_width, max_video_height);
1793
1794 return 0;
1795 }
1796
bcm2835_cleanup_instance(struct bcm2835_mmal_dev * dev)1797 static void bcm2835_cleanup_instance(struct bcm2835_mmal_dev *dev)
1798 {
1799 if (!dev)
1800 return;
1801
1802 v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
1803 video_device_node_name(&dev->vdev));
1804
1805 video_unregister_device(&dev->vdev);
1806
1807 if (dev->capture.encode_component) {
1808 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
1809 "mmal_exit - disconnect tunnel\n");
1810 vchiq_mmal_port_connect_tunnel(dev->instance,
1811 dev->capture.camera_port, NULL);
1812 vchiq_mmal_component_disable(dev->instance,
1813 dev->capture.encode_component);
1814 }
1815 vchiq_mmal_component_disable(dev->instance,
1816 dev->component[COMP_CAMERA]);
1817
1818 vchiq_mmal_component_finalise(dev->instance,
1819 dev->component[COMP_VIDEO_ENCODE]);
1820
1821 vchiq_mmal_component_finalise(dev->instance,
1822 dev->component[COMP_IMAGE_ENCODE]);
1823
1824 vchiq_mmal_component_finalise(dev->instance,
1825 dev->component[COMP_PREVIEW]);
1826
1827 vchiq_mmal_component_finalise(dev->instance,
1828 dev->component[COMP_CAMERA]);
1829
1830 v4l2_ctrl_handler_free(&dev->ctrl_handler);
1831
1832 v4l2_device_unregister(&dev->v4l2_dev);
1833
1834 kfree(dev);
1835 }
1836
1837 static struct v4l2_format default_v4l2_format = {
1838 .fmt.pix.pixelformat = V4L2_PIX_FMT_JPEG,
1839 .fmt.pix.width = 1024,
1840 .fmt.pix.bytesperline = 0,
1841 .fmt.pix.height = 768,
1842 .fmt.pix.sizeimage = 1024 * 768,
1843 };
1844
bcm2835_mmal_probe(struct vchiq_device * device)1845 static int bcm2835_mmal_probe(struct vchiq_device *device)
1846 {
1847 int ret;
1848 struct bcm2835_mmal_dev *dev;
1849 struct vb2_queue *q;
1850 int camera;
1851 unsigned int num_cameras;
1852 struct vchiq_mmal_instance *instance;
1853 unsigned int resolutions[MAX_BCM2835_CAMERAS][2];
1854 int i;
1855
1856 ret = dma_set_mask_and_coherent(&device->dev, DMA_BIT_MASK(32));
1857 if (ret) {
1858 dev_err(&device->dev, "dma_set_mask_and_coherent failed: %d\n", ret);
1859 return ret;
1860 }
1861
1862 ret = vchiq_mmal_init(&instance);
1863 if (ret < 0)
1864 return ret;
1865
1866 num_cameras = get_num_cameras(instance,
1867 resolutions,
1868 MAX_BCM2835_CAMERAS);
1869
1870 if (num_cameras < 1) {
1871 ret = -ENODEV;
1872 goto cleanup_mmal;
1873 }
1874
1875 if (num_cameras > MAX_BCM2835_CAMERAS)
1876 num_cameras = MAX_BCM2835_CAMERAS;
1877
1878 for (camera = 0; camera < num_cameras; camera++) {
1879 dev = kzalloc(sizeof(*dev), GFP_KERNEL);
1880 if (!dev) {
1881 ret = -ENOMEM;
1882 goto cleanup_gdev;
1883 }
1884
1885 /* v4l2 core mutex used to protect all fops and v4l2 ioctls. */
1886 mutex_init(&dev->mutex);
1887 dev->max_width = resolutions[camera][0];
1888 dev->max_height = resolutions[camera][1];
1889
1890 /* setup device defaults */
1891 dev->overlay.w.left = 150;
1892 dev->overlay.w.top = 50;
1893 dev->overlay.w.width = 1024;
1894 dev->overlay.w.height = 768;
1895 dev->overlay.clipcount = 0;
1896 dev->overlay.field = V4L2_FIELD_NONE;
1897 dev->overlay.global_alpha = 255;
1898
1899 dev->capture.fmt = &formats[3]; /* JPEG */
1900
1901 /* v4l device registration */
1902 dev->camera_num = v4l2_device_set_name(&dev->v4l2_dev, KBUILD_MODNAME,
1903 &camera_instance);
1904 ret = v4l2_device_register(NULL, &dev->v4l2_dev);
1905 if (ret) {
1906 dev_err(&device->dev, "%s: could not register V4L2 device: %d\n",
1907 __func__, ret);
1908 goto free_dev;
1909 }
1910
1911 /* setup v4l controls */
1912 ret = bcm2835_mmal_init_controls(dev, &dev->ctrl_handler);
1913 if (ret < 0) {
1914 v4l2_err(&dev->v4l2_dev, "%s: could not init controls: %d\n",
1915 __func__, ret);
1916 goto unreg_dev;
1917 }
1918 dev->v4l2_dev.ctrl_handler = &dev->ctrl_handler;
1919
1920 /* mmal init */
1921 dev->instance = instance;
1922 ret = mmal_init(dev);
1923 if (ret < 0) {
1924 v4l2_err(&dev->v4l2_dev, "%s: mmal init failed: %d\n",
1925 __func__, ret);
1926 goto unreg_dev;
1927 }
1928 /* initialize queue */
1929 q = &dev->capture.vb_vidq;
1930 memset(q, 0, sizeof(*q));
1931 q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1932 q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ;
1933 q->drv_priv = dev;
1934 q->buf_struct_size = sizeof(struct vb2_mmal_buffer);
1935 q->ops = &bcm2835_mmal_video_qops;
1936 q->mem_ops = &vb2_vmalloc_memops;
1937 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1938 q->lock = &dev->mutex;
1939 ret = vb2_queue_init(q);
1940 if (ret < 0)
1941 goto unreg_dev;
1942
1943 /* initialise video devices */
1944 ret = bcm2835_mmal_init_device(dev, &dev->vdev);
1945 if (ret < 0) {
1946 v4l2_err(&dev->v4l2_dev, "%s: could not init device: %d\n",
1947 __func__, ret);
1948 goto unreg_dev;
1949 }
1950
1951 /* Really want to call vidioc_s_fmt_vid_cap with the default
1952 * format, but currently the APIs don't join up.
1953 */
1954 ret = mmal_setup_components(dev, &default_v4l2_format);
1955 if (ret < 0) {
1956 v4l2_err(&dev->v4l2_dev, "%s: could not setup components: %d\n",
1957 __func__, ret);
1958 goto unreg_dev;
1959 }
1960
1961 v4l2_info(&dev->v4l2_dev, "Broadcom 2835 MMAL video capture loaded.\n");
1962
1963 gdev[camera] = dev;
1964 }
1965 return 0;
1966
1967 unreg_dev:
1968 v4l2_ctrl_handler_free(&dev->ctrl_handler);
1969 v4l2_device_unregister(&dev->v4l2_dev);
1970
1971 free_dev:
1972 kfree(dev);
1973
1974 cleanup_gdev:
1975 for (i = 0; i < camera; i++) {
1976 bcm2835_cleanup_instance(gdev[i]);
1977 gdev[i] = NULL;
1978 }
1979
1980 cleanup_mmal:
1981 vchiq_mmal_finalise(instance);
1982
1983 return ret;
1984 }
1985
bcm2835_mmal_remove(struct vchiq_device * device)1986 static void bcm2835_mmal_remove(struct vchiq_device *device)
1987 {
1988 int camera;
1989 struct vchiq_mmal_instance *instance = gdev[0]->instance;
1990
1991 for (camera = 0; camera < MAX_BCM2835_CAMERAS; camera++) {
1992 bcm2835_cleanup_instance(gdev[camera]);
1993 gdev[camera] = NULL;
1994 }
1995 vchiq_mmal_finalise(instance);
1996 }
1997
1998 static const struct vchiq_device_id device_id_table[] = {
1999 { .name = "bcm2835-camera" },
2000 {}
2001 };
2002 MODULE_DEVICE_TABLE(vchiq, device_id_table);
2003
2004 static struct vchiq_driver bcm2835_camera_driver = {
2005 .probe = bcm2835_mmal_probe,
2006 .remove = bcm2835_mmal_remove,
2007 .id_table = device_id_table,
2008 .driver = {
2009 .name = "bcm2835-camera",
2010 },
2011 };
2012
2013 module_vchiq_driver(bcm2835_camera_driver)
2014
2015 MODULE_DESCRIPTION("Broadcom 2835 MMAL video capture");
2016 MODULE_AUTHOR("Vincent Sanders");
2017 MODULE_LICENSE("GPL");
2018