[linux-block.git] / drivers / media / platform / sunxi / sun4i-csi / sun4i_v4l2.c

// SPDX-License-Identifier: GPL-2.0+
/*
 * Copyright (C) 2016 NextThing Co
 * Copyright (C) 2016-2019 Bootlin
 *
 * Author: Maxime Ripard <maxime.ripard@bootlin.com>
 */

#include <linux/device.h>
#include <linux/pm_runtime.h>

#include <media/v4l2-ioctl.h>
#include <media/v4l2-mc.h>
#include <media/videobuf2-v4l2.h>

#include "sun4i_csi.h"

#define CSI_DEFAULT_WIDTH	640
#define CSI_DEFAULT_HEIGHT	480

static const struct sun4i_csi_format sun4i_csi_formats[] = {
	/* YUV422 inputs */
	{
		.mbus		= MEDIA_BUS_FMT_YUYV8_2X8,
		.fourcc		= V4L2_PIX_FMT_YUV420M,
		.input		= CSI_INPUT_YUV,
		.output		= CSI_OUTPUT_YUV_420_PLANAR,
		.num_planes	= 3,
		.bpp		= { 8, 8, 8 },
		.hsub		= 2,
		.vsub		= 2,
	},
};

const struct sun4i_csi_format *sun4i_csi_find_format(const u32 *fourcc,
						     const u32 *mbus)
{
	unsigned int i;

	for (i = 0; i < ARRAY_SIZE(sun4i_csi_formats); i++) {
		if (fourcc && *fourcc != sun4i_csi_formats[i].fourcc)
			continue;

		if (mbus && *mbus != sun4i_csi_formats[i].mbus)
			continue;

		return &sun4i_csi_formats[i];
	}

	return NULL;
}

static int sun4i_csi_querycap(struct file *file, void *priv,
			      struct v4l2_capability *cap)
{
	struct sun4i_csi *csi = video_drvdata(file);

	strscpy(cap->driver, KBUILD_MODNAME, sizeof(cap->driver));
	strscpy(cap->card, "sun4i-csi", sizeof(cap->card));
	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
		 dev_name(csi->dev));

	return 0;
}

static int sun4i_csi_enum_input(struct file *file, void *priv,
				struct v4l2_input *inp)
{
	if (inp->index != 0)
		return -EINVAL;

	inp->type = V4L2_INPUT_TYPE_CAMERA;
	strscpy(inp->name, "Camera", sizeof(inp->name));

	return 0;
}

static int sun4i_csi_g_input(struct file *file, void *fh,
			     unsigned int *i)
{
	*i = 0;

	return 0;
}

static int sun4i_csi_s_input(struct file *file, void *fh,
			     unsigned int i)
{
	if (i != 0)
		return -EINVAL;

	return 0;
}

static void _sun4i_csi_try_fmt(struct sun4i_csi *csi,
			       struct v4l2_pix_format_mplane *pix)
{
	const struct sun4i_csi_format *_fmt;
	unsigned int height, width;
	unsigned int i;

	_fmt = sun4i_csi_find_format(&pix->pixelformat, NULL);
	if (!_fmt)
		_fmt = &sun4i_csi_formats[0];

	pix->field = V4L2_FIELD_NONE;
	pix->colorspace = V4L2_COLORSPACE_SRGB;
	pix->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(pix->colorspace);
	pix->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(pix->colorspace);
	pix->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true, pix->colorspace,
							  pix->ycbcr_enc);

	pix->num_planes = _fmt->num_planes;
	pix->pixelformat = _fmt->fourcc;

	memset(pix->reserved, 0, sizeof(pix->reserved));

	/* Align the width and height on the subsampling */
	width = ALIGN(pix->width, _fmt->hsub);
	height = ALIGN(pix->height, _fmt->vsub);

	/* Clamp the width and height to our capabilities */
	pix->width = clamp(width, _fmt->hsub, CSI_MAX_WIDTH);
	pix->height = clamp(height, _fmt->vsub, CSI_MAX_HEIGHT);

	for (i = 0; i < _fmt->num_planes; i++) {
		unsigned int hsub = i > 0 ? _fmt->hsub : 1;
		unsigned int vsub = i > 0 ? _fmt->vsub : 1;
		unsigned int bpl;

		bpl = pix->width / hsub * _fmt->bpp[i] / 8;
		pix->plane_fmt[i].bytesperline = bpl;
		pix->plane_fmt[i].sizeimage = bpl * pix->height / vsub;
		memset(pix->plane_fmt[i].reserved, 0,
		       sizeof(pix->plane_fmt[i].reserved));
	}
}

static int sun4i_csi_try_fmt_vid_cap(struct file *file, void *priv,
				     struct v4l2_format *f)
{
	struct sun4i_csi *csi = video_drvdata(file);

	_sun4i_csi_try_fmt(csi, &f->fmt.pix_mp);

	return 0;
}

static int sun4i_csi_s_fmt_vid_cap(struct file *file, void *priv,
				   struct v4l2_format *f)
{
	struct sun4i_csi *csi = video_drvdata(file);

	_sun4i_csi_try_fmt(csi, &f->fmt.pix_mp);
	csi->fmt = f->fmt.pix_mp;

	return 0;
}

static int sun4i_csi_g_fmt_vid_cap(struct file *file, void *priv,
				   struct v4l2_format *f)
{
	struct sun4i_csi *csi = video_drvdata(file);

	f->fmt.pix_mp = csi->fmt;

	return 0;
}

static int sun4i_csi_enum_fmt_vid_cap(struct file *file, void *priv,
				      struct v4l2_fmtdesc *f)
{
	if (f->index >= ARRAY_SIZE(sun4i_csi_formats))
		return -EINVAL;

	f->pixelformat = sun4i_csi_formats[f->index].fourcc;

	return 0;
}

static const struct v4l2_ioctl_ops sun4i_csi_ioctl_ops = {
	.vidioc_querycap		= sun4i_csi_querycap,

	.vidioc_enum_fmt_vid_cap	= sun4i_csi_enum_fmt_vid_cap,
	.vidioc_g_fmt_vid_cap_mplane	= sun4i_csi_g_fmt_vid_cap,
	.vidioc_s_fmt_vid_cap_mplane	= sun4i_csi_s_fmt_vid_cap,
	.vidioc_try_fmt_vid_cap_mplane	= sun4i_csi_try_fmt_vid_cap,

	.vidioc_enum_input		= sun4i_csi_enum_input,
	.vidioc_g_input			= sun4i_csi_g_input,
	.vidioc_s_input			= sun4i_csi_s_input,

	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
	.vidioc_querybuf		= vb2_ioctl_querybuf,
	.vidioc_qbuf			= vb2_ioctl_qbuf,
	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
	.vidioc_expbuf			= vb2_ioctl_expbuf,
	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
	.vidioc_streamon		= vb2_ioctl_streamon,
	.vidioc_streamoff		= vb2_ioctl_streamoff,
};

static int sun4i_csi_open(struct file *file)
{
	struct sun4i_csi *csi = video_drvdata(file);
	int ret;

	ret = mutex_lock_interruptible(&csi->lock);
	if (ret)
		return ret;

	ret = pm_runtime_get_sync(csi->dev);
	if (ret < 0)
		goto err_pm_put;

	ret = v4l2_pipeline_pm_get(&csi->vdev.entity);
	if (ret)
		goto err_pm_put;

	ret = v4l2_fh_open(file);
	if (ret)
		goto err_pipeline_pm_put;

	mutex_unlock(&csi->lock);

	return 0;

err_pipeline_pm_put:
	v4l2_pipeline_pm_put(&csi->vdev.entity);

err_pm_put:
	pm_runtime_put(csi->dev);
	mutex_unlock(&csi->lock);

	return ret;
}

static int sun4i_csi_release(struct file *file)
{
	struct sun4i_csi *csi = video_drvdata(file);

	mutex_lock(&csi->lock);

	v4l2_fh_release(file);
	v4l2_pipeline_pm_put(&csi->vdev.entity);
	pm_runtime_put(csi->dev);

	mutex_unlock(&csi->lock);

	return 0;
}

static const struct v4l2_file_operations sun4i_csi_fops = {
	.owner		= THIS_MODULE,
	.open		= sun4i_csi_open,
	.release	= sun4i_csi_release,
	.unlocked_ioctl	= video_ioctl2,
	.read		= vb2_fop_read,
	.write		= vb2_fop_write,
	.poll		= vb2_fop_poll,
	.mmap		= vb2_fop_mmap,
};

static const struct v4l2_mbus_framefmt sun4i_csi_pad_fmt_default = {
	.width = CSI_DEFAULT_WIDTH,
	.height = CSI_DEFAULT_HEIGHT,
	.code = MEDIA_BUS_FMT_YUYV8_2X8,
	.field = V4L2_FIELD_NONE,
	.colorspace = V4L2_COLORSPACE_RAW,
	.ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT,
	.quantization = V4L2_QUANTIZATION_DEFAULT,
	.xfer_func = V4L2_XFER_FUNC_DEFAULT,
};

static int sun4i_csi_subdev_init_cfg(struct v4l2_subdev *subdev,
				     struct v4l2_subdev_pad_config *cfg)
{
	struct v4l2_mbus_framefmt *fmt;

	fmt = v4l2_subdev_get_try_format(subdev, cfg, CSI_SUBDEV_SINK);
	*fmt = sun4i_csi_pad_fmt_default;

	return 0;
}

static int sun4i_csi_subdev_get_fmt(struct v4l2_subdev *subdev,
				    struct v4l2_subdev_pad_config *cfg,
				    struct v4l2_subdev_format *fmt)
{
	struct sun4i_csi *csi = container_of(subdev, struct sun4i_csi, subdev);
	struct v4l2_mbus_framefmt *subdev_fmt;

	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
		subdev_fmt = v4l2_subdev_get_try_format(subdev, cfg, fmt->pad);
	else
		subdev_fmt = &csi->subdev_fmt;

	fmt->format = *subdev_fmt;

	return 0;
}

static int sun4i_csi_subdev_set_fmt(struct v4l2_subdev *subdev,
				    struct v4l2_subdev_pad_config *cfg,
				    struct v4l2_subdev_format *fmt)
{
	struct sun4i_csi *csi = container_of(subdev, struct sun4i_csi, subdev);
	struct v4l2_mbus_framefmt *subdev_fmt;

	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
		subdev_fmt = v4l2_subdev_get_try_format(subdev, cfg, fmt->pad);
	else
		subdev_fmt = &csi->subdev_fmt;

	/* We can only set the format on the sink pad */
	if (fmt->pad == CSI_SUBDEV_SINK) {
		/* It's the sink, only allow changing the frame size */
		subdev_fmt->width = fmt->format.width;
		subdev_fmt->height = fmt->format.height;
		subdev_fmt->code = fmt->format.code;
	}

	fmt->format = *subdev_fmt;

	return 0;
}

static int
sun4i_csi_subdev_enum_mbus_code(struct v4l2_subdev *subdev,
				struct v4l2_subdev_pad_config *cfg,
				struct v4l2_subdev_mbus_code_enum *mbus)
{
	if (mbus->index >= ARRAY_SIZE(sun4i_csi_formats))
		return -EINVAL;

	mbus->code = sun4i_csi_formats[mbus->index].mbus;

	return 0;
}

static const struct v4l2_subdev_pad_ops sun4i_csi_subdev_pad_ops = {
	.link_validate	= v4l2_subdev_link_validate_default,
	.init_cfg	= sun4i_csi_subdev_init_cfg,
	.get_fmt	= sun4i_csi_subdev_get_fmt,
	.set_fmt	= sun4i_csi_subdev_set_fmt,
	.enum_mbus_code	= sun4i_csi_subdev_enum_mbus_code,
};

const struct v4l2_subdev_ops sun4i_csi_subdev_ops = {
	.pad = &sun4i_csi_subdev_pad_ops,
};

int sun4i_csi_v4l2_register(struct sun4i_csi *csi)
{
	struct video_device *vdev = &csi->vdev;
	int ret;

	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_STREAMING;
	vdev->v4l2_dev = &csi->v4l;
	vdev->queue = &csi->queue;
	strscpy(vdev->name, KBUILD_MODNAME, sizeof(vdev->name));
	vdev->release = video_device_release_empty;
	vdev->lock = &csi->lock;

	/* Set a default format */
	csi->fmt.pixelformat = sun4i_csi_formats[0].fourcc,
	csi->fmt.width = CSI_DEFAULT_WIDTH;
	csi->fmt.height = CSI_DEFAULT_HEIGHT;
	_sun4i_csi_try_fmt(csi, &csi->fmt);
	csi->subdev_fmt = sun4i_csi_pad_fmt_default;

	vdev->fops = &sun4i_csi_fops;
	vdev->ioctl_ops = &sun4i_csi_ioctl_ops;
	video_set_drvdata(vdev, csi);

	ret = video_register_device(&csi->vdev, VFL_TYPE_VIDEO, -1);
	if (ret)
		return ret;

	dev_info(csi->dev, "Device registered as %s\n",
		 video_device_node_name(vdev));

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