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