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
20 static 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) */
c3_isp_cap_wrmifx3_buff(struct c3_isp_capture * cap)120 static 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
c3_isp_cap_wrmifx3_format(struct c3_isp_capture * cap)142 static 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
c3_isp_cap_dummy_buff_create(struct c3_isp_capture * cap)216 static 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
c3_isp_cap_dummy_buff_destroy(struct c3_isp_capture * cap)238 static 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
c3_isp_cap_cfg_buff(struct c3_isp_capture * cap)245 static 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
c3_isp_cap_start(struct c3_isp_capture * cap)256 static 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
c3_isp_cap_stop(struct c3_isp_capture * cap)280 static 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
c3_isp_cap_done(struct c3_isp_capture * cap)299 static 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
c3_cap_find_fmt(u32 fourcc)317 static 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
c3_cap_try_fmt(struct v4l2_pix_format_mplane * pix_mp)327 static 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
c3_isp_cap_return_buffers(struct c3_isp_capture * cap,enum vb2_buffer_state state)366 static 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
c3_isp_cap_querycap(struct file * file,void * fh,struct v4l2_capability * cap)386 static 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
c3_isp_cap_enum_fmt(struct file * file,void * fh,struct v4l2_fmtdesc * f)395 static 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
c3_isp_cap_g_fmt_mplane(struct file * file,void * fh,struct v4l2_format * f)425 static 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
c3_isp_cap_s_fmt_mplane(struct file * file,void * fh,struct v4l2_format * f)435 static 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
c3_isp_cap_try_fmt_mplane(struct file * file,void * fh,struct v4l2_format * f)448 static 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
c3_isp_cap_enum_frmsize(struct file * file,void * fh,struct v4l2_frmsizeenum * fsize)456 static 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
479 static 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
499 static 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
c3_isp_cap_link_validate(struct media_link * link)507 static 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
541 static const struct media_entity_operations isp_cap_entity_ops = {
542 .link_validate = c3_isp_cap_link_validate,
543 };
544
c3_isp_vb2_queue_setup(struct vb2_queue * q,unsigned int * num_buffers,unsigned int * num_planes,unsigned int sizes[],struct device * alloc_devs[])545 static 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
c3_isp_vb2_buf_queue(struct vb2_buffer * vb)573 static 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
c3_isp_vb2_buf_prepare(struct vb2_buffer * vb)585 static 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
c3_isp_vb2_buf_init(struct vb2_buffer * vb)605 static 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
c3_isp_vb2_start_streaming(struct vb2_queue * q,unsigned int count)618 static 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
648 err_pm_put:
649 pm_runtime_put(cap->isp->dev);
650 err_dummy_destroy:
651 c3_isp_cap_dummy_buff_destroy(cap);
652 err_pipeline_stop:
653 video_device_pipeline_stop(&cap->vdev);
654 err_return_buffers:
655 c3_isp_cap_return_buffers(cap, VB2_BUF_STATE_QUEUED);
656 return ret;
657 }
658
c3_isp_vb2_stop_streaming(struct vb2_queue * q)659 static 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
677 static 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
c3_isp_register_capture(struct c3_isp_capture * cap)686 static 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
734 err_entity_cleanup:
735 media_entity_cleanup(&vdev->entity);
736 err_queue_release:
737 vb2_queue_release(vb2_q);
738 err_destroy:
739 mutex_destroy(&cap->lock);
740 return ret;
741 }
742
c3_isp_captures_register(struct c3_isp_device * isp)743 int 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
c3_isp_captures_unregister(struct c3_isp_device * isp)782 void 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
c3_isp_captures_isr(struct c3_isp_device * isp)799 void 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 }
805