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