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