c3-isp-capture.c

  1// SPDX-License-Identifier: (GPL-2.0-only OR MIT)
  2/*
  3 * Copyright (C) 2024 Amlogic, Inc. All rights reserved
  4 */
  5
  6#include <linux/cleanup.h>
  7#include <linux/pm_runtime.h>
  8
  9#include <media/v4l2-ctrls.h>
 10#include <media/v4l2-event.h>
 11#include <media/v4l2-ioctl.h>
 12#include <media/v4l2-mc.h>
 13#include <media/videobuf2-dma-contig.h>
 14
 15#include "c3-isp-common.h"
 16#include "c3-isp-regs.h"
 17
 18#define C3_ISP_WRMIFX3_REG(addr, id)	((addr) + (id) * 0x100)
 19
 20static const struct c3_isp_cap_format_info cap_formats[] = {
 21	/* YUV formats */
 22	{
 23		.mbus_code = MEDIA_BUS_FMT_YUV10_1X30,
 24		.fourcc = V4L2_PIX_FMT_GREY,
 25		.format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_Y_ONLY,
 26		.planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X1,
 27		.ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_8BITS,
 28		.uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_VU,
 29		.in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_8BIT,
 30		.hdiv = 1,
 31		.vdiv = 1,
 32	}, {
 33		.mbus_code = MEDIA_BUS_FMT_YUV10_1X30,
 34		.fourcc = V4L2_PIX_FMT_NV12M,
 35		.format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_YUV420,
 36		.planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X2,
 37		.ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_8BITS,
 38		.uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_UV,
 39		.in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_8BIT,
 40		.hdiv = 2,
 41		.vdiv = 2,
 42	}, {
 43		.mbus_code = MEDIA_BUS_FMT_YUV10_1X30,
 44		.fourcc = V4L2_PIX_FMT_NV21M,
 45		.format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_YUV420,
 46		.planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X2,
 47		.ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_8BITS,
 48		.uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_VU,
 49		.in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_8BIT,
 50		.hdiv = 2,
 51		.vdiv = 2,
 52	}, {
 53		.mbus_code = MEDIA_BUS_FMT_YUV10_1X30,
 54		.fourcc = V4L2_PIX_FMT_NV16M,
 55		.format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_YUV422,
 56		.planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X2,
 57		.ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_8BITS,
 58		.uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_UV,
 59		.in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_8BIT,
 60		.hdiv = 1,
 61		.vdiv = 2
 62	}, {
 63		.mbus_code = MEDIA_BUS_FMT_YUV10_1X30,
 64		.fourcc = V4L2_PIX_FMT_NV61M,
 65		.format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_YUV422,
 66		.planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X2,
 67		.ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_8BITS,
 68		.uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_VU,
 69		.in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_8BIT,
 70		.hdiv = 1,
 71		.vdiv = 2,
 72	},
 73	/* RAW formats */
 74	{
 75		.mbus_code = MEDIA_BUS_FMT_SRGGB16_1X16,
 76		.fourcc = V4L2_PIX_FMT_SRGGB12,
 77		.format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_RAW,
 78		.planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X1,
 79		.ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_16BITS,
 80		.uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_VU,
 81		.in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_16BIT,
 82		.hdiv = 1,
 83		.vdiv = 1,
 84	}, {
 85		.mbus_code = MEDIA_BUS_FMT_SBGGR16_1X16,
 86		.fourcc = V4L2_PIX_FMT_SBGGR12,
 87		.format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_RAW,
 88		.planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X1,
 89		.ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_16BITS,
 90		.uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_VU,
 91		.in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_16BIT,
 92		.hdiv = 1,
 93		.vdiv = 1,
 94	}, {
 95		.mbus_code = MEDIA_BUS_FMT_SGRBG16_1X16,
 96		.fourcc = V4L2_PIX_FMT_SGRBG12,
 97		.format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_RAW,
 98		.planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X1,
 99		.ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_16BITS,
100		.uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_VU,
101		.in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_16BIT,
102		.hdiv = 1,
103		.vdiv = 1,
104	}, {
105		.mbus_code = MEDIA_BUS_FMT_SGBRG16_1X16,
106		.fourcc = V4L2_PIX_FMT_SGBRG12,
107		.format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_RAW,
108		.planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X1,
109		.ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_16BITS,
110		.uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_VU,
111		.in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_16BIT,
112		.hdiv = 1,
113		.vdiv = 1,
114	},
115};
116
117/* Hardware configuration */
118
119/* Set the address of wrmifx3(write memory interface) */
120static void c3_isp_cap_wrmifx3_buff(struct c3_isp_capture *cap)
121{
122	dma_addr_t y_dma_addr;
123	dma_addr_t uv_dma_addr;
124
125	if (cap->buff) {
126		y_dma_addr = cap->buff->dma_addr[C3_ISP_PLANE_Y];
127		uv_dma_addr = cap->buff->dma_addr[C3_ISP_PLANE_UV];
128	} else {
129		y_dma_addr = cap->dummy_buff.dma_addr;
130		uv_dma_addr = cap->dummy_buff.dma_addr;
131	}
132
133	c3_isp_write(cap->isp,
134		     C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_CH0_BADDR, cap->id),
135		     ISP_WRMIFX3_0_CH0_BASE_ADDR(y_dma_addr));
136
137	c3_isp_write(cap->isp,
138		     C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_CH1_BADDR, cap->id),
139		     ISP_WRMIFX3_0_CH1_BASE_ADDR(uv_dma_addr));
140}
141
142static void c3_isp_cap_wrmifx3_format(struct c3_isp_capture *cap)
143{
144	struct v4l2_pix_format_mplane *pix_mp = &cap->format.pix_mp;
145	const struct c3_isp_cap_format_info *info = cap->format.info;
146	u32 stride;
147	u32 chrom_h;
148	u32 chrom_v;
149
150	c3_isp_write(cap->isp,
151		     C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_FMT_SIZE, cap->id),
152		     ISP_WRMIFX3_0_FMT_SIZE_HSIZE(pix_mp->width) |
153		     ISP_WRMIFX3_0_FMT_SIZE_VSIZE(pix_mp->height));
154
155	c3_isp_update_bits(cap->isp,
156			   C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_FMT_CTRL, cap->id),
157			   ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_MASK, info->format);
158
159	c3_isp_update_bits(cap->isp,
160			   C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_FMT_CTRL, cap->id),
161			   ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_MASK,
162			   info->in_bits);
163
164	c3_isp_update_bits(cap->isp,
165			   C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_FMT_CTRL, cap->id),
166			   ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_MASK, info->planes);
167
168	c3_isp_update_bits(cap->isp,
169			   C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_FMT_CTRL, cap->id),
170			   ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_MASK,
171			   info->uv_swap);
172
173	stride = DIV_ROUND_UP(pix_mp->plane_fmt[C3_ISP_PLANE_Y].bytesperline,
174			      C3_ISP_DMA_SIZE_ALIGN_BYTES);
175	c3_isp_update_bits(cap->isp,
176			   C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_CH0_CTRL0, cap->id),
177			   ISP_WRMIFX3_0_CH0_CTRL0_STRIDE_MASK,
178			   ISP_WRMIFX3_0_CH0_CTRL0_STRIDE(stride));
179
180	c3_isp_update_bits(cap->isp,
181			   C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_CH0_CTRL1, cap->id),
182			   ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_MODE_MASK,
183			   info->ch0_pix_bits);
184
185	c3_isp_write(cap->isp,
186		     C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_WIN_LUMA_H, cap->id),
187		     ISP_WRMIFX3_0_WIN_LUMA_H_LUMA_HEND(pix_mp->width));
188
189	c3_isp_write(cap->isp,
190		     C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_WIN_LUMA_V, cap->id),
191		     ISP_WRMIFX3_0_WIN_LUMA_V_LUMA_VEND(pix_mp->height));
192
193	stride = DIV_ROUND_UP(pix_mp->plane_fmt[C3_ISP_PLANE_UV].bytesperline,
194			      C3_ISP_DMA_SIZE_ALIGN_BYTES);
195	c3_isp_update_bits(cap->isp,
196			   C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_CH1_CTRL0, cap->id),
197			   ISP_WRMIFX3_0_CH1_CTRL0_STRIDE_MASK,
198			   ISP_WRMIFX3_0_CH1_CTRL0_STRIDE(stride));
199
200	c3_isp_update_bits(cap->isp,
201			   C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_CH1_CTRL1, cap->id),
202			   ISP_WRMIFX3_0_CH1_CTRL1_PIX_BITS_MODE_MASK,
203			   ISP_WRMIFX3_0_CH1_CTRL1_PIX_BITS_16BITS);
204
205	chrom_h = DIV_ROUND_UP(pix_mp->width, info->hdiv);
206	c3_isp_write(cap->isp,
207		     C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_WIN_CHROM_H, cap->id),
208		     ISP_WRMIFX3_0_WIN_CHROM_H_CHROM_HEND(chrom_h));
209
210	chrom_v = DIV_ROUND_UP(pix_mp->height, info->vdiv);
211	c3_isp_write(cap->isp,
212		     C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_WIN_CHROM_V, cap->id),
213		     ISP_WRMIFX3_0_WIN_CHROM_V_CHROM_VEND(chrom_v));
214}
215
216static int c3_isp_cap_dummy_buff_create(struct c3_isp_capture *cap)
217{
218	struct c3_isp_dummy_buffer *dummy_buff = &cap->dummy_buff;
219	struct v4l2_pix_format_mplane *pix_mp = &cap->format.pix_mp;
220
221	if (pix_mp->num_planes == 1)
222		dummy_buff->size = pix_mp->plane_fmt[C3_ISP_PLANE_Y].sizeimage;
223	else
224		dummy_buff->size =
225			max(pix_mp->plane_fmt[C3_ISP_PLANE_Y].sizeimage,
226			    pix_mp->plane_fmt[C3_ISP_PLANE_UV].sizeimage);
227
228	/* The driver never access vaddr, no mapping is required */
229	dummy_buff->vaddr = dma_alloc_attrs(cap->isp->dev, dummy_buff->size,
230					    &dummy_buff->dma_addr, GFP_KERNEL,
231					    DMA_ATTR_NO_KERNEL_MAPPING);
232	if (!dummy_buff->vaddr)
233		return -ENOMEM;
234
235	return 0;
236}
237
238static void c3_isp_cap_dummy_buff_destroy(struct c3_isp_capture *cap)
239{
240	dma_free_attrs(cap->isp->dev, cap->dummy_buff.size,
241		       cap->dummy_buff.vaddr, cap->dummy_buff.dma_addr,
242		       DMA_ATTR_NO_KERNEL_MAPPING);
243}
244
245static void c3_isp_cap_cfg_buff(struct c3_isp_capture *cap)
246{
247	cap->buff = list_first_entry_or_null(&cap->pending,
248					     struct c3_isp_cap_buffer, list);
249
250	c3_isp_cap_wrmifx3_buff(cap);
251
252	if (cap->buff)
253		list_del(&cap->buff->list);
254}
255
256static void c3_isp_cap_start(struct c3_isp_capture *cap)
257{
258	u32 mask;
259	u32 val;
260
261	scoped_guard(spinlock_irqsave, &cap->buff_lock)
262		c3_isp_cap_cfg_buff(cap);
263
264	c3_isp_cap_wrmifx3_format(cap);
265
266	if (cap->id == C3_ISP_CAP_DEV_0) {
267		mask = ISP_TOP_PATH_EN_WRMIF0_EN_MASK;
268		val = ISP_TOP_PATH_EN_WRMIF0_EN;
269	} else if (cap->id == C3_ISP_CAP_DEV_1) {
270		mask = ISP_TOP_PATH_EN_WRMIF1_EN_MASK;
271		val = ISP_TOP_PATH_EN_WRMIF1_EN;
272	} else {
273		mask = ISP_TOP_PATH_EN_WRMIF2_EN_MASK;
274		val = ISP_TOP_PATH_EN_WRMIF2_EN;
275	}
276
277	c3_isp_update_bits(cap->isp, ISP_TOP_PATH_EN, mask, val);
278}
279
280static void c3_isp_cap_stop(struct c3_isp_capture *cap)
281{
282	u32 mask;
283	u32 val;
284
285	if (cap->id == C3_ISP_CAP_DEV_0) {
286		mask = ISP_TOP_PATH_EN_WRMIF0_EN_MASK;
287		val = ISP_TOP_PATH_EN_WRMIF0_DIS;
288	} else if (cap->id == C3_ISP_CAP_DEV_1) {
289		mask = ISP_TOP_PATH_EN_WRMIF1_EN_MASK;
290		val = ISP_TOP_PATH_EN_WRMIF1_DIS;
291	} else {
292		mask = ISP_TOP_PATH_EN_WRMIF2_EN_MASK;
293		val = ISP_TOP_PATH_EN_WRMIF2_DIS;
294	}
295
296	c3_isp_update_bits(cap->isp, ISP_TOP_PATH_EN, mask, val);
297}
298
299static void c3_isp_cap_done(struct c3_isp_capture *cap)
300{
301	struct c3_isp_cap_buffer *buff = cap->buff;
302
303	guard(spinlock_irqsave)(&cap->buff_lock);
304
305	if (buff) {
306		buff->vb.sequence = cap->isp->frm_sequence;
307		buff->vb.vb2_buf.timestamp = ktime_get();
308		buff->vb.field = V4L2_FIELD_NONE;
309		vb2_buffer_done(&buff->vb.vb2_buf, VB2_BUF_STATE_DONE);
310	}
311
312	c3_isp_cap_cfg_buff(cap);
313}
314
315/* V4L2 video operations */
316
317static const struct c3_isp_cap_format_info *c3_cap_find_fmt(u32 fourcc)
318{
319	for (unsigned int i = 0; i < ARRAY_SIZE(cap_formats); i++) {
320		if (cap_formats[i].fourcc == fourcc)
321			return &cap_formats[i];
322	}
323
324	return NULL;
325}
326
327static void c3_cap_try_fmt(struct v4l2_pix_format_mplane *pix_mp)
328{
329	const struct c3_isp_cap_format_info *fmt;
330	const struct v4l2_format_info *info;
331	struct v4l2_plane_pix_format *plane;
332
333	fmt = c3_cap_find_fmt(pix_mp->pixelformat);
334	if (!fmt)
335		fmt = &cap_formats[0];
336
337	pix_mp->width = clamp(pix_mp->width, C3_ISP_MIN_WIDTH,
338			      C3_ISP_MAX_WIDTH);
339	pix_mp->height = clamp(pix_mp->height, C3_ISP_MIN_HEIGHT,
340			       C3_ISP_MAX_HEIGHT);
341	pix_mp->pixelformat = fmt->fourcc;
342	pix_mp->field = V4L2_FIELD_NONE;
343	pix_mp->colorspace = V4L2_COLORSPACE_SRGB;
344	pix_mp->ycbcr_enc = V4L2_YCBCR_ENC_601;
345	pix_mp->quantization = V4L2_QUANTIZATION_LIM_RANGE;
346
347	info = v4l2_format_info(fmt->fourcc);
348	pix_mp->num_planes = info->mem_planes;
349	memset(pix_mp->plane_fmt, 0, sizeof(pix_mp->plane_fmt));
350
351	for (unsigned int i = 0; i < info->comp_planes; i++) {
352		unsigned int hdiv = (i == 0) ? 1 : info->hdiv;
353		unsigned int vdiv = (i == 0) ? 1 : info->vdiv;
354
355		plane = &pix_mp->plane_fmt[i];
356
357		plane->bytesperline = DIV_ROUND_UP(pix_mp->width, hdiv) *
358				      info->bpp[i] / info->bpp_div[i];
359		plane->bytesperline = ALIGN(plane->bytesperline,
360					    C3_ISP_DMA_SIZE_ALIGN_BYTES);
361		plane->sizeimage = plane->bytesperline *
362				   DIV_ROUND_UP(pix_mp->height, vdiv);
363	}
364}
365
366static void c3_isp_cap_return_buffers(struct c3_isp_capture *cap,
367				      enum vb2_buffer_state state)
368{
369	struct c3_isp_cap_buffer *buff;
370
371	guard(spinlock_irqsave)(&cap->buff_lock);
372
373	if (cap->buff) {
374		vb2_buffer_done(&cap->buff->vb.vb2_buf, state);
375		cap->buff = NULL;
376	}
377
378	while (!list_empty(&cap->pending)) {
379		buff = list_first_entry(&cap->pending,
380					struct c3_isp_cap_buffer, list);
381		list_del(&buff->list);
382		vb2_buffer_done(&buff->vb.vb2_buf, state);
383	}
384}
385
386static int c3_isp_cap_querycap(struct file *file, void *fh,
387			       struct v4l2_capability *cap)
388{
389	strscpy(cap->driver, C3_ISP_DRIVER_NAME, sizeof(cap->driver));
390	strscpy(cap->card, "AML C3 ISP", sizeof(cap->card));
391
392	return 0;
393}
394
395static int c3_isp_cap_enum_fmt(struct file *file, void *fh,
396			       struct v4l2_fmtdesc *f)
397{
398	const struct c3_isp_cap_format_info *fmt;
399	unsigned int index = 0;
400	unsigned int i;
401
402	if (!f->mbus_code) {
403		if (f->index >= ARRAY_SIZE(cap_formats))
404			return -EINVAL;
405
406		fmt = &cap_formats[f->index];
407		f->pixelformat = fmt->fourcc;
408		return 0;
409	}
410
411	for (i = 0; i < ARRAY_SIZE(cap_formats); i++) {
412		fmt = &cap_formats[i];
413		if (f->mbus_code != fmt->mbus_code)
414			continue;
415
416		if (index++ == f->index) {
417			f->pixelformat = cap_formats[i].fourcc;
418			return 0;
419		}
420	}
421
422	return -EINVAL;
423}
424
425static int c3_isp_cap_g_fmt_mplane(struct file *file, void *fh,
426				   struct v4l2_format *f)
427{
428	struct c3_isp_capture *cap = video_drvdata(file);
429
430	f->fmt.pix_mp = cap->format.pix_mp;
431
432	return 0;
433}
434
435static int c3_isp_cap_s_fmt_mplane(struct file *file, void *fh,
436				   struct v4l2_format *f)
437{
438	struct c3_isp_capture *cap = video_drvdata(file);
439
440	c3_cap_try_fmt(&f->fmt.pix_mp);
441
442	cap->format.pix_mp = f->fmt.pix_mp;
443	cap->format.info = c3_cap_find_fmt(f->fmt.pix_mp.pixelformat);
444
445	return 0;
446}
447
448static int c3_isp_cap_try_fmt_mplane(struct file *file, void *fh,
449				     struct v4l2_format *f)
450{
451	c3_cap_try_fmt(&f->fmt.pix_mp);
452
453	return 0;
454}
455
456static int c3_isp_cap_enum_frmsize(struct file *file, void *fh,
457				   struct v4l2_frmsizeenum *fsize)
458{
459	const struct c3_isp_cap_format_info *fmt;
460
461	if (fsize->index)
462		return -EINVAL;
463
464	fmt = c3_cap_find_fmt(fsize->pixel_format);
465	if (!fmt)
466		return -EINVAL;
467
468	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
469	fsize->stepwise.min_width = C3_ISP_MIN_WIDTH;
470	fsize->stepwise.min_height = C3_ISP_MIN_HEIGHT;
471	fsize->stepwise.max_width = C3_ISP_MAX_WIDTH;
472	fsize->stepwise.max_height = C3_ISP_MAX_HEIGHT;
473	fsize->stepwise.step_width = 2;
474	fsize->stepwise.step_height = 2;
475
476	return 0;
477}
478
479static const struct v4l2_ioctl_ops isp_cap_v4l2_ioctl_ops = {
480	.vidioc_querycap		= c3_isp_cap_querycap,
481	.vidioc_enum_fmt_vid_cap	= c3_isp_cap_enum_fmt,
482	.vidioc_g_fmt_vid_cap_mplane	= c3_isp_cap_g_fmt_mplane,
483	.vidioc_s_fmt_vid_cap_mplane	= c3_isp_cap_s_fmt_mplane,
484	.vidioc_try_fmt_vid_cap_mplane	= c3_isp_cap_try_fmt_mplane,
485	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
486	.vidioc_querybuf		= vb2_ioctl_querybuf,
487	.vidioc_qbuf			= vb2_ioctl_qbuf,
488	.vidioc_expbuf			= vb2_ioctl_expbuf,
489	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
490	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
491	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
492	.vidioc_streamon		= vb2_ioctl_streamon,
493	.vidioc_streamoff		= vb2_ioctl_streamoff,
494	.vidioc_enum_framesizes         = c3_isp_cap_enum_frmsize,
495	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
496	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
497};
498
499static const struct v4l2_file_operations isp_cap_v4l2_fops = {
500	.open = v4l2_fh_open,
501	.release = vb2_fop_release,
502	.poll = vb2_fop_poll,
503	.unlocked_ioctl = video_ioctl2,
504	.mmap = vb2_fop_mmap,
505};
506
507static int c3_isp_cap_link_validate(struct media_link *link)
508{
509	struct video_device *vdev =
510		media_entity_to_video_device(link->sink->entity);
511	struct v4l2_subdev *sd =
512		media_entity_to_v4l2_subdev(link->source->entity);
513	struct c3_isp_capture *cap = video_get_drvdata(vdev);
514	struct v4l2_subdev_format src_fmt = {
515		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
516		.pad = link->source->index,
517	};
518	int ret;
519
520	ret = v4l2_subdev_call_state_active(sd, pad, get_fmt, &src_fmt);
521	if (ret)
522		return ret;
523
524	if (src_fmt.format.width != cap->format.pix_mp.width ||
525	    src_fmt.format.height != cap->format.pix_mp.height ||
526	    src_fmt.format.code != cap->format.info->mbus_code) {
527		dev_err(cap->isp->dev,
528			"link %s: %u -> %s: %u not valid: 0x%04x/%ux%u not match 0x%04x/%ux%u\n",
529			link->source->entity->name, link->source->index,
530			link->sink->entity->name, link->sink->index,
531			src_fmt.format.code, src_fmt.format.width,
532			src_fmt.format.height, cap->format.info->mbus_code,
533			cap->format.pix_mp.width, cap->format.pix_mp.height);
534
535		return -EPIPE;
536	}
537
538	return 0;
539}
540
541static const struct media_entity_operations isp_cap_entity_ops = {
542	.link_validate = c3_isp_cap_link_validate,
543};
544
545static int c3_isp_vb2_queue_setup(struct vb2_queue *q,
546				  unsigned int *num_buffers,
547				  unsigned int *num_planes,
548				  unsigned int sizes[],
549				  struct device *alloc_devs[])
550{
551	struct c3_isp_capture *cap = vb2_get_drv_priv(q);
552	const struct v4l2_pix_format_mplane *pix_mp = &cap->format.pix_mp;
553	unsigned int i;
554
555	if (*num_planes) {
556		if (*num_planes != pix_mp->num_planes)
557			return -EINVAL;
558
559		for (i = 0; i < pix_mp->num_planes; i++)
560			if (sizes[i] < pix_mp->plane_fmt[i].sizeimage)
561				return -EINVAL;
562
563		return 0;
564	}
565
566	*num_planes = pix_mp->num_planes;
567	for (i = 0; i < pix_mp->num_planes; i++)
568		sizes[i] = pix_mp->plane_fmt[i].sizeimage;
569
570	return 0;
571}
572
573static void c3_isp_vb2_buf_queue(struct vb2_buffer *vb)
574{
575	struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb);
576	struct c3_isp_cap_buffer *buf =
577			container_of(v4l2_buf, struct c3_isp_cap_buffer, vb);
578	struct c3_isp_capture *cap = vb2_get_drv_priv(vb->vb2_queue);
579
580	guard(spinlock_irqsave)(&cap->buff_lock);
581
582	list_add_tail(&buf->list, &cap->pending);
583}
584
585static int c3_isp_vb2_buf_prepare(struct vb2_buffer *vb)
586{
587	struct c3_isp_capture *cap = vb2_get_drv_priv(vb->vb2_queue);
588	unsigned long size;
589
590	for (unsigned int i = 0; i < cap->format.pix_mp.num_planes; i++) {
591		size = cap->format.pix_mp.plane_fmt[i].sizeimage;
592		if (vb2_plane_size(vb, i) < size) {
593			dev_err(cap->isp->dev,
594				"User buffer too small (%ld < %lu)\n",
595				vb2_plane_size(vb, i), size);
596			return -EINVAL;
597		}
598
599		vb2_set_plane_payload(vb, i, size);
600	}
601
602	return 0;
603}
604
605static int c3_isp_vb2_buf_init(struct vb2_buffer *vb)
606{
607	struct c3_isp_capture *cap = vb2_get_drv_priv(vb->vb2_queue);
608	struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb);
609	struct c3_isp_cap_buffer *buf =
610		container_of(v4l2_buf, struct c3_isp_cap_buffer, vb);
611
612	for (unsigned int i = 0; i < cap->format.pix_mp.num_planes; i++)
613		buf->dma_addr[i] = vb2_dma_contig_plane_dma_addr(vb, i);
614
615	return 0;
616}
617
618static int c3_isp_vb2_start_streaming(struct vb2_queue *q,
619				      unsigned int count)
620{
621	struct c3_isp_capture *cap = vb2_get_drv_priv(q);
622	int ret;
623
624	ret = video_device_pipeline_start(&cap->vdev, &cap->isp->pipe);
625	if (ret) {
626		dev_err(cap->isp->dev,
627			"Failed to start cap%u pipeline: %d\n", cap->id, ret);
628		goto err_return_buffers;
629	}
630
631	ret = c3_isp_cap_dummy_buff_create(cap);
632	if (ret)
633		goto err_pipeline_stop;
634
635	ret = pm_runtime_resume_and_get(cap->isp->dev);
636	if (ret)
637		goto err_dummy_destroy;
638
639	c3_isp_cap_start(cap);
640
641	ret = v4l2_subdev_enable_streams(&cap->rsz->sd, C3_ISP_RSZ_PAD_SOURCE,
642					 BIT(0));
643	if (ret)
644		goto err_pm_put;
645
646	return 0;
647
648err_pm_put:
649	pm_runtime_put(cap->isp->dev);
650err_dummy_destroy:
651	c3_isp_cap_dummy_buff_destroy(cap);
652err_pipeline_stop:
653	video_device_pipeline_stop(&cap->vdev);
654err_return_buffers:
655	c3_isp_cap_return_buffers(cap, VB2_BUF_STATE_QUEUED);
656	return ret;
657}
658
659static void c3_isp_vb2_stop_streaming(struct vb2_queue *q)
660{
661	struct c3_isp_capture *cap = vb2_get_drv_priv(q);
662
663	c3_isp_cap_stop(cap);
664
665	c3_isp_cap_return_buffers(cap, VB2_BUF_STATE_ERROR);
666
667	v4l2_subdev_disable_streams(&cap->rsz->sd, C3_ISP_RSZ_PAD_SOURCE,
668				    BIT(0));
669
670	pm_runtime_put(cap->isp->dev);
671
672	c3_isp_cap_dummy_buff_destroy(cap);
673
674	video_device_pipeline_stop(&cap->vdev);
675}
676
677static const struct vb2_ops isp_video_vb2_ops = {
678	.queue_setup = c3_isp_vb2_queue_setup,
679	.buf_queue = c3_isp_vb2_buf_queue,
680	.buf_prepare = c3_isp_vb2_buf_prepare,
681	.buf_init = c3_isp_vb2_buf_init,
682	.start_streaming = c3_isp_vb2_start_streaming,
683	.stop_streaming = c3_isp_vb2_stop_streaming,
684};
685
686static int c3_isp_register_capture(struct c3_isp_capture *cap)
687{
688	struct video_device *vdev = &cap->vdev;
689	struct vb2_queue *vb2_q = &cap->vb2_q;
690	int ret;
691
692	snprintf(vdev->name, sizeof(vdev->name), "c3-isp-cap%u", cap->id);
693	vdev->fops = &isp_cap_v4l2_fops;
694	vdev->ioctl_ops = &isp_cap_v4l2_ioctl_ops;
695	vdev->v4l2_dev = &cap->isp->v4l2_dev;
696	vdev->entity.ops = &isp_cap_entity_ops;
697	vdev->lock = &cap->lock;
698	vdev->minor = -1;
699	vdev->queue = vb2_q;
700	vdev->release = video_device_release_empty;
701	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE |
702			    V4L2_CAP_STREAMING;
703	vdev->vfl_dir = VFL_DIR_RX;
704	video_set_drvdata(vdev, cap);
705
706	vb2_q->drv_priv = cap;
707	vb2_q->mem_ops = &vb2_dma_contig_memops;
708	vb2_q->ops = &isp_video_vb2_ops;
709	vb2_q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
710	vb2_q->io_modes = VB2_DMABUF | VB2_MMAP;
711	vb2_q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
712	vb2_q->buf_struct_size = sizeof(struct c3_isp_cap_buffer);
713	vb2_q->dev = cap->isp->dev;
714	vb2_q->lock = &cap->lock;
715
716	ret = vb2_queue_init(vb2_q);
717	if (ret)
718		goto err_destroy;
719
720	cap->pad.flags = MEDIA_PAD_FL_SINK;
721	ret = media_entity_pads_init(&vdev->entity, 1, &cap->pad);
722	if (ret)
723		goto err_queue_release;
724
725	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
726	if (ret) {
727		dev_err(cap->isp->dev,
728			"Failed to register %s: %d\n", vdev->name, ret);
729		goto err_entity_cleanup;
730	}
731
732	return 0;
733
734err_entity_cleanup:
735	media_entity_cleanup(&vdev->entity);
736err_queue_release:
737	vb2_queue_release(vb2_q);
738err_destroy:
739	mutex_destroy(&cap->lock);
740	return ret;
741}
742
743int c3_isp_captures_register(struct c3_isp_device *isp)
744{
745	int ret;
746	unsigned int i;
747	struct c3_isp_capture *cap;
748
749	for (i = C3_ISP_CAP_DEV_0; i < C3_ISP_NUM_CAP_DEVS; i++) {
750		cap = &isp->caps[i];
751		memset(cap, 0, sizeof(*cap));
752
753		cap->format.pix_mp.width = C3_ISP_DEFAULT_WIDTH;
754		cap->format.pix_mp.height = C3_ISP_DEFAULT_HEIGHT;
755		cap->format.pix_mp.field = V4L2_FIELD_NONE;
756		cap->format.pix_mp.pixelformat = V4L2_PIX_FMT_NV12M;
757		cap->format.pix_mp.colorspace = V4L2_COLORSPACE_SRGB;
758		cap->format.info =
759			c3_cap_find_fmt(cap->format.pix_mp.pixelformat);
760
761		c3_cap_try_fmt(&cap->format.pix_mp);
762
763		cap->id = i;
764		cap->rsz = &isp->resizers[i];
765		cap->isp = isp;
766		INIT_LIST_HEAD(&cap->pending);
767		spin_lock_init(&cap->buff_lock);
768		mutex_init(&cap->lock);
769
770		ret = c3_isp_register_capture(cap);
771		if (ret) {
772			cap->isp = NULL;
773			mutex_destroy(&cap->lock);
774			c3_isp_captures_unregister(isp);
775			return ret;
776		}
777	}
778
779	return 0;
780}
781
782void c3_isp_captures_unregister(struct c3_isp_device *isp)
783{
784	unsigned int i;
785	struct c3_isp_capture *cap;
786
787	for (i = C3_ISP_CAP_DEV_0; i < C3_ISP_NUM_CAP_DEVS; i++) {
788		cap = &isp->caps[i];
789
790		if (!cap->isp)
791			continue;
792		vb2_queue_release(&cap->vb2_q);
793		media_entity_cleanup(&cap->vdev.entity);
794		video_unregister_device(&cap->vdev);
795		mutex_destroy(&cap->lock);
796	}
797}
798
799void c3_isp_captures_isr(struct c3_isp_device *isp)
800{
801	c3_isp_cap_done(&isp->caps[C3_ISP_CAP_DEV_0]);
802	c3_isp_cap_done(&isp->caps[C3_ISP_CAP_DEV_1]);
803	c3_isp_cap_done(&isp->caps[C3_ISP_CAP_DEV_2]);
804}