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