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