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