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