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