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