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