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