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