1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3  * Copyright (C) 2016 NextThing Co
4  * Copyright (C) 2016-2019 Bootlin
5  *
6  * Author: Maxime Ripard <maxime.ripard@bootlin.com>
7  */
8 
9 #include <linux/device.h>
10 #include <linux/pm_runtime.h>
11 
12 #include <media/v4l2-ioctl.h>
13 #include <media/v4l2-mc.h>
14 #include <media/videobuf2-v4l2.h>
15 
16 #include "sun4i_csi.h"
17 
18 #define CSI_DEFAULT_WIDTH	640
19 #define CSI_DEFAULT_HEIGHT	480
20 
21 static const struct sun4i_csi_format sun4i_csi_formats[] = {
22 	/* YUV422 inputs */
23 	{
24 		.mbus		= MEDIA_BUS_FMT_YUYV8_2X8,
25 		.fourcc		= V4L2_PIX_FMT_YUV420M,
26 		.input		= CSI_INPUT_YUV,
27 		.output		= CSI_OUTPUT_YUV_420_PLANAR,
28 		.num_planes	= 3,
29 		.bpp		= { 8, 8, 8 },
30 		.hsub		= 2,
31 		.vsub		= 2,
32 	},
33 };
34 
sun4i_csi_find_format(const u32 * fourcc,const u32 * mbus)35 const struct sun4i_csi_format *sun4i_csi_find_format(const u32 *fourcc,
36 						     const u32 *mbus)
37 {
38 	unsigned int i;
39 
40 	for (i = 0; i < ARRAY_SIZE(sun4i_csi_formats); i++) {
41 		if (fourcc && *fourcc != sun4i_csi_formats[i].fourcc)
42 			continue;
43 
44 		if (mbus && *mbus != sun4i_csi_formats[i].mbus)
45 			continue;
46 
47 		return &sun4i_csi_formats[i];
48 	}
49 
50 	return NULL;
51 }
52 
sun4i_csi_querycap(struct file * file,void * priv,struct v4l2_capability * cap)53 static int sun4i_csi_querycap(struct file *file, void *priv,
54 			      struct v4l2_capability *cap)
55 {
56 	strscpy(cap->driver, KBUILD_MODNAME, sizeof(cap->driver));
57 	strscpy(cap->card, "sun4i-csi", sizeof(cap->card));
58 
59 	return 0;
60 }
61 
sun4i_csi_enum_input(struct file * file,void * priv,struct v4l2_input * inp)62 static int sun4i_csi_enum_input(struct file *file, void *priv,
63 				struct v4l2_input *inp)
64 {
65 	if (inp->index != 0)
66 		return -EINVAL;
67 
68 	inp->type = V4L2_INPUT_TYPE_CAMERA;
69 	strscpy(inp->name, "Camera", sizeof(inp->name));
70 
71 	return 0;
72 }
73 
sun4i_csi_g_input(struct file * file,void * fh,unsigned int * i)74 static int sun4i_csi_g_input(struct file *file, void *fh,
75 			     unsigned int *i)
76 {
77 	*i = 0;
78 
79 	return 0;
80 }
81 
sun4i_csi_s_input(struct file * file,void * fh,unsigned int i)82 static int sun4i_csi_s_input(struct file *file, void *fh,
83 			     unsigned int i)
84 {
85 	if (i != 0)
86 		return -EINVAL;
87 
88 	return 0;
89 }
90 
_sun4i_csi_try_fmt(struct sun4i_csi * csi,struct v4l2_pix_format_mplane * pix)91 static void _sun4i_csi_try_fmt(struct sun4i_csi *csi,
92 			       struct v4l2_pix_format_mplane *pix)
93 {
94 	const struct sun4i_csi_format *_fmt;
95 	unsigned int height, width;
96 	unsigned int i;
97 
98 	_fmt = sun4i_csi_find_format(&pix->pixelformat, NULL);
99 	if (!_fmt)
100 		_fmt = &sun4i_csi_formats[0];
101 
102 	pix->field = V4L2_FIELD_NONE;
103 	pix->colorspace = V4L2_COLORSPACE_SRGB;
104 	pix->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(pix->colorspace);
105 	pix->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(pix->colorspace);
106 	pix->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true, pix->colorspace,
107 							  pix->ycbcr_enc);
108 
109 	pix->num_planes = _fmt->num_planes;
110 	pix->pixelformat = _fmt->fourcc;
111 
112 	/* Align the width and height on the subsampling */
113 	width = ALIGN(pix->width, _fmt->hsub);
114 	height = ALIGN(pix->height, _fmt->vsub);
115 
116 	/* Clamp the width and height to our capabilities */
117 	pix->width = clamp(width, _fmt->hsub, CSI_MAX_WIDTH);
118 	pix->height = clamp(height, _fmt->vsub, CSI_MAX_HEIGHT);
119 
120 	for (i = 0; i < _fmt->num_planes; i++) {
121 		unsigned int hsub = i > 0 ? _fmt->hsub : 1;
122 		unsigned int vsub = i > 0 ? _fmt->vsub : 1;
123 		unsigned int bpl;
124 
125 		bpl = pix->width / hsub * _fmt->bpp[i] / 8;
126 		pix->plane_fmt[i].bytesperline = bpl;
127 		pix->plane_fmt[i].sizeimage = bpl * pix->height / vsub;
128 	}
129 }
130 
sun4i_csi_try_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)131 static int sun4i_csi_try_fmt_vid_cap(struct file *file, void *priv,
132 				     struct v4l2_format *f)
133 {
134 	struct sun4i_csi *csi = video_drvdata(file);
135 
136 	_sun4i_csi_try_fmt(csi, &f->fmt.pix_mp);
137 
138 	return 0;
139 }
140 
sun4i_csi_s_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)141 static int sun4i_csi_s_fmt_vid_cap(struct file *file, void *priv,
142 				   struct v4l2_format *f)
143 {
144 	struct sun4i_csi *csi = video_drvdata(file);
145 
146 	_sun4i_csi_try_fmt(csi, &f->fmt.pix_mp);
147 	csi->fmt = f->fmt.pix_mp;
148 
149 	return 0;
150 }
151 
sun4i_csi_g_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)152 static int sun4i_csi_g_fmt_vid_cap(struct file *file, void *priv,
153 				   struct v4l2_format *f)
154 {
155 	struct sun4i_csi *csi = video_drvdata(file);
156 
157 	f->fmt.pix_mp = csi->fmt;
158 
159 	return 0;
160 }
161 
sun4i_csi_enum_fmt_vid_cap(struct file * file,void * priv,struct v4l2_fmtdesc * f)162 static int sun4i_csi_enum_fmt_vid_cap(struct file *file, void *priv,
163 				      struct v4l2_fmtdesc *f)
164 {
165 	if (f->index >= ARRAY_SIZE(sun4i_csi_formats))
166 		return -EINVAL;
167 
168 	f->pixelformat = sun4i_csi_formats[f->index].fourcc;
169 
170 	return 0;
171 }
172 
173 static const struct v4l2_ioctl_ops sun4i_csi_ioctl_ops = {
174 	.vidioc_querycap		= sun4i_csi_querycap,
175 
176 	.vidioc_enum_fmt_vid_cap	= sun4i_csi_enum_fmt_vid_cap,
177 	.vidioc_g_fmt_vid_cap_mplane	= sun4i_csi_g_fmt_vid_cap,
178 	.vidioc_s_fmt_vid_cap_mplane	= sun4i_csi_s_fmt_vid_cap,
179 	.vidioc_try_fmt_vid_cap_mplane	= sun4i_csi_try_fmt_vid_cap,
180 
181 	.vidioc_enum_input		= sun4i_csi_enum_input,
182 	.vidioc_g_input			= sun4i_csi_g_input,
183 	.vidioc_s_input			= sun4i_csi_s_input,
184 
185 	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
186 	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
187 	.vidioc_querybuf		= vb2_ioctl_querybuf,
188 	.vidioc_qbuf			= vb2_ioctl_qbuf,
189 	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
190 	.vidioc_expbuf			= vb2_ioctl_expbuf,
191 	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
192 	.vidioc_streamon		= vb2_ioctl_streamon,
193 	.vidioc_streamoff		= vb2_ioctl_streamoff,
194 };
195 
sun4i_csi_open(struct file * file)196 static int sun4i_csi_open(struct file *file)
197 {
198 	struct sun4i_csi *csi = video_drvdata(file);
199 	int ret;
200 
201 	ret = mutex_lock_interruptible(&csi->lock);
202 	if (ret)
203 		return ret;
204 
205 	ret = pm_runtime_resume_and_get(csi->dev);
206 	if (ret < 0)
207 		goto err_unlock;
208 
209 	ret = v4l2_pipeline_pm_get(&csi->vdev.entity);
210 	if (ret)
211 		goto err_pm_put;
212 
213 	ret = v4l2_fh_open(file);
214 	if (ret)
215 		goto err_pipeline_pm_put;
216 
217 	mutex_unlock(&csi->lock);
218 
219 	return 0;
220 
221 err_pipeline_pm_put:
222 	v4l2_pipeline_pm_put(&csi->vdev.entity);
223 
224 err_pm_put:
225 	pm_runtime_put(csi->dev);
226 
227 err_unlock:
228 	mutex_unlock(&csi->lock);
229 
230 	return ret;
231 }
232 
sun4i_csi_release(struct file * file)233 static int sun4i_csi_release(struct file *file)
234 {
235 	struct sun4i_csi *csi = video_drvdata(file);
236 
237 	mutex_lock(&csi->lock);
238 
239 	_vb2_fop_release(file, NULL);
240 
241 	v4l2_pipeline_pm_put(&csi->vdev.entity);
242 	pm_runtime_put(csi->dev);
243 
244 	mutex_unlock(&csi->lock);
245 
246 	return 0;
247 }
248 
249 static const struct v4l2_file_operations sun4i_csi_fops = {
250 	.owner		= THIS_MODULE,
251 	.open		= sun4i_csi_open,
252 	.release	= sun4i_csi_release,
253 	.unlocked_ioctl	= video_ioctl2,
254 	.poll		= vb2_fop_poll,
255 	.mmap		= vb2_fop_mmap,
256 };
257 
258 static const struct v4l2_mbus_framefmt sun4i_csi_pad_fmt_default = {
259 	.width = CSI_DEFAULT_WIDTH,
260 	.height = CSI_DEFAULT_HEIGHT,
261 	.code = MEDIA_BUS_FMT_YUYV8_2X8,
262 	.field = V4L2_FIELD_NONE,
263 	.colorspace = V4L2_COLORSPACE_RAW,
264 	.ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT,
265 	.quantization = V4L2_QUANTIZATION_DEFAULT,
266 	.xfer_func = V4L2_XFER_FUNC_DEFAULT,
267 };
268 
sun4i_csi_subdev_init_cfg(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state)269 static int sun4i_csi_subdev_init_cfg(struct v4l2_subdev *subdev,
270 				     struct v4l2_subdev_state *sd_state)
271 {
272 	struct v4l2_mbus_framefmt *fmt;
273 
274 	fmt = v4l2_subdev_get_try_format(subdev, sd_state, CSI_SUBDEV_SINK);
275 	*fmt = sun4i_csi_pad_fmt_default;
276 
277 	return 0;
278 }
279 
sun4i_csi_subdev_get_fmt(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * fmt)280 static int sun4i_csi_subdev_get_fmt(struct v4l2_subdev *subdev,
281 				    struct v4l2_subdev_state *sd_state,
282 				    struct v4l2_subdev_format *fmt)
283 {
284 	struct sun4i_csi *csi = container_of(subdev, struct sun4i_csi, subdev);
285 	struct v4l2_mbus_framefmt *subdev_fmt;
286 
287 	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
288 		subdev_fmt = v4l2_subdev_get_try_format(subdev, sd_state,
289 							fmt->pad);
290 	else
291 		subdev_fmt = &csi->subdev_fmt;
292 
293 	fmt->format = *subdev_fmt;
294 
295 	return 0;
296 }
297 
sun4i_csi_subdev_set_fmt(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * fmt)298 static int sun4i_csi_subdev_set_fmt(struct v4l2_subdev *subdev,
299 				    struct v4l2_subdev_state *sd_state,
300 				    struct v4l2_subdev_format *fmt)
301 {
302 	struct sun4i_csi *csi = container_of(subdev, struct sun4i_csi, subdev);
303 	struct v4l2_mbus_framefmt *subdev_fmt;
304 
305 	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
306 		subdev_fmt = v4l2_subdev_get_try_format(subdev, sd_state,
307 							fmt->pad);
308 	else
309 		subdev_fmt = &csi->subdev_fmt;
310 
311 	/* We can only set the format on the sink pad */
312 	if (fmt->pad == CSI_SUBDEV_SINK) {
313 		/* It's the sink, only allow changing the frame size */
314 		subdev_fmt->width = fmt->format.width;
315 		subdev_fmt->height = fmt->format.height;
316 		subdev_fmt->code = fmt->format.code;
317 	}
318 
319 	fmt->format = *subdev_fmt;
320 
321 	return 0;
322 }
323 
324 static int
sun4i_csi_subdev_enum_mbus_code(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_mbus_code_enum * mbus)325 sun4i_csi_subdev_enum_mbus_code(struct v4l2_subdev *subdev,
326 				struct v4l2_subdev_state *sd_state,
327 				struct v4l2_subdev_mbus_code_enum *mbus)
328 {
329 	if (mbus->index >= ARRAY_SIZE(sun4i_csi_formats))
330 		return -EINVAL;
331 
332 	mbus->code = sun4i_csi_formats[mbus->index].mbus;
333 
334 	return 0;
335 }
336 
337 static const struct v4l2_subdev_pad_ops sun4i_csi_subdev_pad_ops = {
338 	.link_validate	= v4l2_subdev_link_validate_default,
339 	.init_cfg	= sun4i_csi_subdev_init_cfg,
340 	.get_fmt	= sun4i_csi_subdev_get_fmt,
341 	.set_fmt	= sun4i_csi_subdev_set_fmt,
342 	.enum_mbus_code	= sun4i_csi_subdev_enum_mbus_code,
343 };
344 
345 const struct v4l2_subdev_ops sun4i_csi_subdev_ops = {
346 	.pad = &sun4i_csi_subdev_pad_ops,
347 };
348 
sun4i_csi_v4l2_register(struct sun4i_csi * csi)349 int sun4i_csi_v4l2_register(struct sun4i_csi *csi)
350 {
351 	struct video_device *vdev = &csi->vdev;
352 	int ret;
353 
354 	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_STREAMING;
355 	vdev->v4l2_dev = &csi->v4l;
356 	vdev->queue = &csi->queue;
357 	strscpy(vdev->name, KBUILD_MODNAME, sizeof(vdev->name));
358 	vdev->release = video_device_release_empty;
359 	vdev->lock = &csi->lock;
360 
361 	/* Set a default format */
362 	csi->fmt.pixelformat = sun4i_csi_formats[0].fourcc;
363 	csi->fmt.width = CSI_DEFAULT_WIDTH;
364 	csi->fmt.height = CSI_DEFAULT_HEIGHT;
365 	_sun4i_csi_try_fmt(csi, &csi->fmt);
366 	csi->subdev_fmt = sun4i_csi_pad_fmt_default;
367 
368 	vdev->fops = &sun4i_csi_fops;
369 	vdev->ioctl_ops = &sun4i_csi_ioctl_ops;
370 	video_set_drvdata(vdev, csi);
371 
372 	ret = video_register_device(&csi->vdev, VFL_TYPE_VIDEO, -1);
373 	if (ret)
374 		return ret;
375 
376 	dev_info(csi->dev, "Device registered as %s\n",
377 		 video_device_node_name(vdev));
378 
379 	return 0;
380 }
381