xref: /linux/drivers/media/platform/rockchip/rkisp1/rkisp1-capture.c (revision f6154d8babbb8a98f0d3ea325aafae2e33bfd8be)
1 // SPDX-License-Identifier: (GPL-2.0+ OR MIT)
2 /*
3  * Rockchip ISP1 Driver - V4l capture device
4  *
5  * Copyright (C) 2019 Collabora, Ltd.
6  *
7  * Based on Rockchip ISP1 driver by Rockchip Electronics Co., Ltd.
8  * Copyright (C) 2017 Rockchip Electronics Co., Ltd.
9  */
10 
11 #include <linux/delay.h>
12 #include <linux/pm_runtime.h>
13 #include <media/v4l2-common.h>
14 #include <media/v4l2-event.h>
15 #include <media/v4l2-fh.h>
16 #include <media/v4l2-ioctl.h>
17 #include <media/v4l2-mc.h>
18 #include <media/v4l2-subdev.h>
19 #include <media/videobuf2-dma-contig.h>
20 
21 #include "rkisp1-common.h"
22 
23 /*
24  * NOTE: There are two capture video devices in rkisp1, selfpath and mainpath.
25  *
26  * differences between selfpath and mainpath
27  * available mp sink input: isp
28  * available sp sink input : isp, dma(TODO)
29  * available mp sink pad fmts: yuv422, raw
30  * available sp sink pad fmts: yuv422, yuv420......
31  * available mp source fmts: yuv, raw, jpeg(TODO)
32  * available sp source fmts: yuv, rgb
33  */
34 
35 #define RKISP1_SP_DEV_NAME	RKISP1_DRIVER_NAME "_selfpath"
36 #define RKISP1_MP_DEV_NAME	RKISP1_DRIVER_NAME "_mainpath"
37 
38 #define RKISP1_MIN_BUFFERS_NEEDED 3
39 
40 enum rkisp1_plane {
41 	RKISP1_PLANE_Y	= 0,
42 	RKISP1_PLANE_CB	= 1,
43 	RKISP1_PLANE_CR	= 2
44 };
45 
46 /*
47  * @fourcc: pixel format
48  * @fmt_type: helper filed for pixel format
49  * @uv_swap: if cb cr swapped, for yuv
50  * @write_format: defines how YCbCr self picture data is written to memory
51  * @output_format: defines sp output format
52  * @mbus: the mbus code on the src resizer pad that matches the pixel format
53  */
54 struct rkisp1_capture_fmt_cfg {
55 	u32 fourcc;
56 	u8 uv_swap;
57 	u32 write_format;
58 	u32 output_format;
59 	u32 mbus;
60 };
61 
62 struct rkisp1_capture_ops {
63 	void (*config)(struct rkisp1_capture *cap);
64 	void (*stop)(struct rkisp1_capture *cap);
65 	void (*enable)(struct rkisp1_capture *cap);
66 	void (*disable)(struct rkisp1_capture *cap);
67 	void (*set_data_path)(struct rkisp1_capture *cap);
68 	bool (*is_stopped)(struct rkisp1_capture *cap);
69 };
70 
71 struct rkisp1_capture_config {
72 	const struct rkisp1_capture_fmt_cfg *fmts;
73 	int fmt_size;
74 	struct {
75 		u32 y_size_init;
76 		u32 cb_size_init;
77 		u32 cr_size_init;
78 		u32 y_base_ad_init;
79 		u32 cb_base_ad_init;
80 		u32 cr_base_ad_init;
81 		u32 y_offs_cnt_init;
82 		u32 cb_offs_cnt_init;
83 		u32 cr_offs_cnt_init;
84 	} mi;
85 };
86 
87 /*
88  * The supported pixel formats for mainpath. NOTE, pixel formats with identical 'mbus'
89  * are grouped together. This is assumed and used by the function rkisp1_cap_enum_mbus_codes
90  */
91 static const struct rkisp1_capture_fmt_cfg rkisp1_mp_fmts[] = {
92 	/* yuv422 */
93 	{
94 		.fourcc = V4L2_PIX_FMT_YUYV,
95 		.uv_swap = 0,
96 		.write_format = RKISP1_MI_CTRL_MP_WRITE_YUVINT,
97 		.mbus = MEDIA_BUS_FMT_YUYV8_2X8,
98 	}, {
99 		.fourcc = V4L2_PIX_FMT_YUV422P,
100 		.uv_swap = 0,
101 		.write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_PLA_OR_RAW8,
102 		.mbus = MEDIA_BUS_FMT_YUYV8_2X8,
103 	}, {
104 		.fourcc = V4L2_PIX_FMT_NV16,
105 		.uv_swap = 0,
106 		.write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_SPLA,
107 		.mbus = MEDIA_BUS_FMT_YUYV8_2X8,
108 	}, {
109 		.fourcc = V4L2_PIX_FMT_NV61,
110 		.uv_swap = 1,
111 		.write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_SPLA,
112 		.mbus = MEDIA_BUS_FMT_YUYV8_2X8,
113 	}, {
114 		.fourcc = V4L2_PIX_FMT_NV16M,
115 		.uv_swap = 0,
116 		.write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_SPLA,
117 		.mbus = MEDIA_BUS_FMT_YUYV8_2X8,
118 	}, {
119 		.fourcc = V4L2_PIX_FMT_NV61M,
120 		.uv_swap = 1,
121 		.write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_SPLA,
122 		.mbus = MEDIA_BUS_FMT_YUYV8_2X8,
123 	}, {
124 		.fourcc = V4L2_PIX_FMT_YVU422M,
125 		.uv_swap = 1,
126 		.write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_PLA_OR_RAW8,
127 		.mbus = MEDIA_BUS_FMT_YUYV8_2X8,
128 	},
129 	/* yuv400 */
130 	{
131 		.fourcc = V4L2_PIX_FMT_GREY,
132 		.uv_swap = 0,
133 		.write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_PLA_OR_RAW8,
134 		.mbus = MEDIA_BUS_FMT_YUYV8_2X8,
135 	},
136 	/* yuv420 */
137 	{
138 		.fourcc = V4L2_PIX_FMT_NV21,
139 		.uv_swap = 1,
140 		.write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_SPLA,
141 		.mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
142 	}, {
143 		.fourcc = V4L2_PIX_FMT_NV12,
144 		.uv_swap = 0,
145 		.write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_SPLA,
146 		.mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
147 	}, {
148 		.fourcc = V4L2_PIX_FMT_NV21M,
149 		.uv_swap = 1,
150 		.write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_SPLA,
151 		.mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
152 	}, {
153 		.fourcc = V4L2_PIX_FMT_NV12M,
154 		.uv_swap = 0,
155 		.write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_SPLA,
156 		.mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
157 	}, {
158 		.fourcc = V4L2_PIX_FMT_YUV420,
159 		.uv_swap = 0,
160 		.write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_PLA_OR_RAW8,
161 		.mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
162 	}, {
163 		.fourcc = V4L2_PIX_FMT_YVU420,
164 		.uv_swap = 1,
165 		.write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_PLA_OR_RAW8,
166 		.mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
167 	},
168 	/* raw */
169 	{
170 		.fourcc = V4L2_PIX_FMT_SRGGB8,
171 		.write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_PLA_OR_RAW8,
172 		.mbus = MEDIA_BUS_FMT_SRGGB8_1X8,
173 	}, {
174 		.fourcc = V4L2_PIX_FMT_SGRBG8,
175 		.write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_PLA_OR_RAW8,
176 		.mbus = MEDIA_BUS_FMT_SGRBG8_1X8,
177 	}, {
178 		.fourcc = V4L2_PIX_FMT_SGBRG8,
179 		.write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_PLA_OR_RAW8,
180 		.mbus = MEDIA_BUS_FMT_SGBRG8_1X8,
181 	}, {
182 		.fourcc = V4L2_PIX_FMT_SBGGR8,
183 		.write_format = RKISP1_MI_CTRL_MP_WRITE_YUV_PLA_OR_RAW8,
184 		.mbus = MEDIA_BUS_FMT_SBGGR8_1X8,
185 	}, {
186 		.fourcc = V4L2_PIX_FMT_SRGGB10,
187 		.write_format = RKISP1_MI_CTRL_MP_WRITE_RAW12,
188 		.mbus = MEDIA_BUS_FMT_SRGGB10_1X10,
189 	}, {
190 		.fourcc = V4L2_PIX_FMT_SGRBG10,
191 		.write_format = RKISP1_MI_CTRL_MP_WRITE_RAW12,
192 		.mbus = MEDIA_BUS_FMT_SGRBG10_1X10,
193 	}, {
194 		.fourcc = V4L2_PIX_FMT_SGBRG10,
195 		.write_format = RKISP1_MI_CTRL_MP_WRITE_RAW12,
196 		.mbus = MEDIA_BUS_FMT_SGBRG10_1X10,
197 	}, {
198 		.fourcc = V4L2_PIX_FMT_SBGGR10,
199 		.write_format = RKISP1_MI_CTRL_MP_WRITE_RAW12,
200 		.mbus = MEDIA_BUS_FMT_SBGGR10_1X10,
201 	}, {
202 		.fourcc = V4L2_PIX_FMT_SRGGB12,
203 		.write_format = RKISP1_MI_CTRL_MP_WRITE_RAW12,
204 		.mbus = MEDIA_BUS_FMT_SRGGB12_1X12,
205 	}, {
206 		.fourcc = V4L2_PIX_FMT_SGRBG12,
207 		.write_format = RKISP1_MI_CTRL_MP_WRITE_RAW12,
208 		.mbus = MEDIA_BUS_FMT_SGRBG12_1X12,
209 	}, {
210 		.fourcc = V4L2_PIX_FMT_SGBRG12,
211 		.write_format = RKISP1_MI_CTRL_MP_WRITE_RAW12,
212 		.mbus = MEDIA_BUS_FMT_SGBRG12_1X12,
213 	}, {
214 		.fourcc = V4L2_PIX_FMT_SBGGR12,
215 		.write_format = RKISP1_MI_CTRL_MP_WRITE_RAW12,
216 		.mbus = MEDIA_BUS_FMT_SBGGR12_1X12,
217 	},
218 };
219 
220 /*
221  * The supported pixel formats for selfpath. NOTE, pixel formats with identical 'mbus'
222  * are grouped together. This is assumed and used by the function rkisp1_cap_enum_mbus_codes
223  */
224 static const struct rkisp1_capture_fmt_cfg rkisp1_sp_fmts[] = {
225 	/* yuv422 */
226 	{
227 		.fourcc = V4L2_PIX_FMT_YUYV,
228 		.uv_swap = 0,
229 		.write_format = RKISP1_MI_CTRL_SP_WRITE_INT,
230 		.output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV422,
231 		.mbus = MEDIA_BUS_FMT_YUYV8_2X8,
232 	}, {
233 		.fourcc = V4L2_PIX_FMT_YUV422P,
234 		.uv_swap = 0,
235 		.write_format = RKISP1_MI_CTRL_SP_WRITE_PLA,
236 		.output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV422,
237 		.mbus = MEDIA_BUS_FMT_YUYV8_2X8,
238 	}, {
239 		.fourcc = V4L2_PIX_FMT_NV16,
240 		.uv_swap = 0,
241 		.write_format = RKISP1_MI_CTRL_SP_WRITE_SPLA,
242 		.output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV422,
243 		.mbus = MEDIA_BUS_FMT_YUYV8_2X8,
244 	}, {
245 		.fourcc = V4L2_PIX_FMT_NV61,
246 		.uv_swap = 1,
247 		.write_format = RKISP1_MI_CTRL_SP_WRITE_SPLA,
248 		.output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV422,
249 		.mbus = MEDIA_BUS_FMT_YUYV8_2X8,
250 	}, {
251 		.fourcc = V4L2_PIX_FMT_NV16M,
252 		.uv_swap = 0,
253 		.write_format = RKISP1_MI_CTRL_SP_WRITE_SPLA,
254 		.output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV422,
255 		.mbus = MEDIA_BUS_FMT_YUYV8_2X8,
256 	}, {
257 		.fourcc = V4L2_PIX_FMT_NV61M,
258 		.uv_swap = 1,
259 		.write_format = RKISP1_MI_CTRL_SP_WRITE_SPLA,
260 		.output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV422,
261 		.mbus = MEDIA_BUS_FMT_YUYV8_2X8,
262 	}, {
263 		.fourcc = V4L2_PIX_FMT_YVU422M,
264 		.uv_swap = 1,
265 		.write_format = RKISP1_MI_CTRL_SP_WRITE_PLA,
266 		.output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV422,
267 		.mbus = MEDIA_BUS_FMT_YUYV8_2X8,
268 	},
269 	/* yuv400 */
270 	{
271 		.fourcc = V4L2_PIX_FMT_GREY,
272 		.uv_swap = 0,
273 		.write_format = RKISP1_MI_CTRL_SP_WRITE_PLA,
274 		.output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV422,
275 		.mbus = MEDIA_BUS_FMT_YUYV8_2X8,
276 	},
277 	/* rgb */
278 	{
279 		.fourcc = V4L2_PIX_FMT_XBGR32,
280 		.write_format = RKISP1_MI_CTRL_SP_WRITE_PLA,
281 		.output_format = RKISP1_MI_CTRL_SP_OUTPUT_RGB888,
282 		.mbus = MEDIA_BUS_FMT_YUYV8_2X8,
283 	}, {
284 		.fourcc = V4L2_PIX_FMT_RGB565,
285 		.write_format = RKISP1_MI_CTRL_SP_WRITE_PLA,
286 		.output_format = RKISP1_MI_CTRL_SP_OUTPUT_RGB565,
287 		.mbus = MEDIA_BUS_FMT_YUYV8_2X8,
288 	},
289 	/* yuv420 */
290 	{
291 		.fourcc = V4L2_PIX_FMT_NV21,
292 		.uv_swap = 1,
293 		.write_format = RKISP1_MI_CTRL_SP_WRITE_SPLA,
294 		.output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV420,
295 		.mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
296 	}, {
297 		.fourcc = V4L2_PIX_FMT_NV12,
298 		.uv_swap = 0,
299 		.write_format = RKISP1_MI_CTRL_SP_WRITE_SPLA,
300 		.output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV420,
301 		.mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
302 	}, {
303 		.fourcc = V4L2_PIX_FMT_NV21M,
304 		.uv_swap = 1,
305 		.write_format = RKISP1_MI_CTRL_SP_WRITE_SPLA,
306 		.output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV420,
307 		.mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
308 	}, {
309 		.fourcc = V4L2_PIX_FMT_NV12M,
310 		.uv_swap = 0,
311 		.write_format = RKISP1_MI_CTRL_SP_WRITE_SPLA,
312 		.output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV420,
313 		.mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
314 	}, {
315 		.fourcc = V4L2_PIX_FMT_YUV420,
316 		.uv_swap = 0,
317 		.write_format = RKISP1_MI_CTRL_SP_WRITE_PLA,
318 		.output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV420,
319 		.mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
320 	}, {
321 		.fourcc = V4L2_PIX_FMT_YVU420,
322 		.uv_swap = 1,
323 		.write_format = RKISP1_MI_CTRL_SP_WRITE_PLA,
324 		.output_format = RKISP1_MI_CTRL_SP_OUTPUT_YUV420,
325 		.mbus = MEDIA_BUS_FMT_YUYV8_1_5X8,
326 	},
327 };
328 
329 static const struct rkisp1_capture_config rkisp1_capture_config_mp = {
330 	.fmts = rkisp1_mp_fmts,
331 	.fmt_size = ARRAY_SIZE(rkisp1_mp_fmts),
332 	.mi = {
333 		.y_size_init =		RKISP1_CIF_MI_MP_Y_SIZE_INIT,
334 		.cb_size_init =		RKISP1_CIF_MI_MP_CB_SIZE_INIT,
335 		.cr_size_init =		RKISP1_CIF_MI_MP_CR_SIZE_INIT,
336 		.y_base_ad_init =	RKISP1_CIF_MI_MP_Y_BASE_AD_INIT,
337 		.cb_base_ad_init =	RKISP1_CIF_MI_MP_CB_BASE_AD_INIT,
338 		.cr_base_ad_init =	RKISP1_CIF_MI_MP_CR_BASE_AD_INIT,
339 		.y_offs_cnt_init =	RKISP1_CIF_MI_MP_Y_OFFS_CNT_INIT,
340 		.cb_offs_cnt_init =	RKISP1_CIF_MI_MP_CB_OFFS_CNT_INIT,
341 		.cr_offs_cnt_init =	RKISP1_CIF_MI_MP_CR_OFFS_CNT_INIT,
342 	},
343 };
344 
345 static const struct rkisp1_capture_config rkisp1_capture_config_sp = {
346 	.fmts = rkisp1_sp_fmts,
347 	.fmt_size = ARRAY_SIZE(rkisp1_sp_fmts),
348 	.mi = {
349 		.y_size_init =		RKISP1_CIF_MI_SP_Y_SIZE_INIT,
350 		.cb_size_init =		RKISP1_CIF_MI_SP_CB_SIZE_INIT,
351 		.cr_size_init =		RKISP1_CIF_MI_SP_CR_SIZE_INIT,
352 		.y_base_ad_init =	RKISP1_CIF_MI_SP_Y_BASE_AD_INIT,
353 		.cb_base_ad_init =	RKISP1_CIF_MI_SP_CB_BASE_AD_INIT,
354 		.cr_base_ad_init =	RKISP1_CIF_MI_SP_CR_BASE_AD_INIT,
355 		.y_offs_cnt_init =	RKISP1_CIF_MI_SP_Y_OFFS_CNT_INIT,
356 		.cb_offs_cnt_init =	RKISP1_CIF_MI_SP_CB_OFFS_CNT_INIT,
357 		.cr_offs_cnt_init =	RKISP1_CIF_MI_SP_CR_OFFS_CNT_INIT,
358 	},
359 };
360 
361 static inline struct rkisp1_vdev_node *
362 rkisp1_vdev_to_node(struct video_device *vdev)
363 {
364 	return container_of(vdev, struct rkisp1_vdev_node, vdev);
365 }
366 
367 int rkisp1_cap_enum_mbus_codes(struct rkisp1_capture *cap,
368 			       struct v4l2_subdev_mbus_code_enum *code)
369 {
370 	const struct rkisp1_capture_fmt_cfg *fmts = cap->config->fmts;
371 	/*
372 	 * initialize curr_mbus to non existing mbus code 0 to ensure it is
373 	 * different from fmts[0].mbus
374 	 */
375 	u32 curr_mbus = 0;
376 	int i, n = 0;
377 
378 	for (i = 0; i < cap->config->fmt_size; i++) {
379 		if (fmts[i].mbus == curr_mbus)
380 			continue;
381 
382 		curr_mbus = fmts[i].mbus;
383 		if (n++ == code->index) {
384 			code->code = curr_mbus;
385 			return 0;
386 		}
387 	}
388 	return -EINVAL;
389 }
390 
391 /* ----------------------------------------------------------------------------
392  * Stream operations for self-picture path (sp) and main-picture path (mp)
393  */
394 
395 static void rkisp1_mi_config_ctrl(struct rkisp1_capture *cap)
396 {
397 	u32 mi_ctrl = rkisp1_read(cap->rkisp1, RKISP1_CIF_MI_CTRL);
398 
399 	mi_ctrl &= ~GENMASK(17, 16);
400 	mi_ctrl |= RKISP1_CIF_MI_CTRL_BURST_LEN_LUM_64;
401 
402 	mi_ctrl &= ~GENMASK(19, 18);
403 	mi_ctrl |= RKISP1_CIF_MI_CTRL_BURST_LEN_CHROM_64;
404 
405 	mi_ctrl |= RKISP1_CIF_MI_CTRL_INIT_BASE_EN |
406 		   RKISP1_CIF_MI_CTRL_INIT_OFFSET_EN;
407 
408 	rkisp1_write(cap->rkisp1, RKISP1_CIF_MI_CTRL, mi_ctrl);
409 }
410 
411 static u32 rkisp1_pixfmt_comp_size(const struct v4l2_pix_format_mplane *pixm,
412 				   unsigned int component)
413 {
414 	/*
415 	 * If packed format, then plane_fmt[0].sizeimage is the sum of all
416 	 * components, so we need to calculate just the size of Y component.
417 	 * See rkisp1_fill_pixfmt().
418 	 */
419 	if (!component && pixm->num_planes == 1)
420 		return pixm->plane_fmt[0].bytesperline * pixm->height;
421 	return pixm->plane_fmt[component].sizeimage;
422 }
423 
424 static void rkisp1_irq_frame_end_enable(struct rkisp1_capture *cap)
425 {
426 	u32 mi_imsc = rkisp1_read(cap->rkisp1, RKISP1_CIF_MI_IMSC);
427 
428 	mi_imsc |= RKISP1_CIF_MI_FRAME(cap);
429 	rkisp1_write(cap->rkisp1, RKISP1_CIF_MI_IMSC, mi_imsc);
430 }
431 
432 static void rkisp1_mp_config(struct rkisp1_capture *cap)
433 {
434 	const struct v4l2_pix_format_mplane *pixm = &cap->pix.fmt;
435 	struct rkisp1_device *rkisp1 = cap->rkisp1;
436 	u32 reg;
437 
438 	rkisp1_write(rkisp1, cap->config->mi.y_size_init,
439 		     rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_Y));
440 	rkisp1_write(rkisp1, cap->config->mi.cb_size_init,
441 		     rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_CB));
442 	rkisp1_write(rkisp1, cap->config->mi.cr_size_init,
443 		     rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_CR));
444 
445 	rkisp1_irq_frame_end_enable(cap);
446 
447 	/* set uv swapping for semiplanar formats */
448 	if (cap->pix.info->comp_planes == 2) {
449 		reg = rkisp1_read(rkisp1, RKISP1_CIF_MI_XTD_FORMAT_CTRL);
450 		if (cap->pix.cfg->uv_swap)
451 			reg |= RKISP1_CIF_MI_XTD_FMT_CTRL_MP_CB_CR_SWAP;
452 		else
453 			reg &= ~RKISP1_CIF_MI_XTD_FMT_CTRL_MP_CB_CR_SWAP;
454 		rkisp1_write(rkisp1, RKISP1_CIF_MI_XTD_FORMAT_CTRL, reg);
455 	}
456 
457 	rkisp1_mi_config_ctrl(cap);
458 
459 	reg = rkisp1_read(rkisp1, RKISP1_CIF_MI_CTRL);
460 	reg &= ~RKISP1_MI_CTRL_MP_FMT_MASK;
461 	reg |= cap->pix.cfg->write_format;
462 	rkisp1_write(rkisp1, RKISP1_CIF_MI_CTRL, reg);
463 
464 	reg = rkisp1_read(rkisp1, RKISP1_CIF_MI_CTRL);
465 	reg |= RKISP1_CIF_MI_MP_AUTOUPDATE_ENABLE;
466 	rkisp1_write(rkisp1, RKISP1_CIF_MI_CTRL, reg);
467 }
468 
469 static void rkisp1_sp_config(struct rkisp1_capture *cap)
470 {
471 	const struct v4l2_pix_format_mplane *pixm = &cap->pix.fmt;
472 	struct rkisp1_device *rkisp1 = cap->rkisp1;
473 	u32 mi_ctrl, reg;
474 
475 	rkisp1_write(rkisp1, cap->config->mi.y_size_init,
476 		     rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_Y));
477 	rkisp1_write(rkisp1, cap->config->mi.cb_size_init,
478 		     rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_CB));
479 	rkisp1_write(rkisp1, cap->config->mi.cr_size_init,
480 		     rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_CR));
481 
482 	rkisp1_write(rkisp1, RKISP1_CIF_MI_SP_Y_LLENGTH, cap->sp_y_stride);
483 	rkisp1_write(rkisp1, RKISP1_CIF_MI_SP_Y_PIC_WIDTH, pixm->width);
484 	rkisp1_write(rkisp1, RKISP1_CIF_MI_SP_Y_PIC_HEIGHT, pixm->height);
485 	rkisp1_write(rkisp1, RKISP1_CIF_MI_SP_Y_PIC_SIZE,
486 		     cap->sp_y_stride * pixm->height);
487 
488 	rkisp1_irq_frame_end_enable(cap);
489 
490 	/* set uv swapping for semiplanar formats */
491 	if (cap->pix.info->comp_planes == 2) {
492 		reg = rkisp1_read(rkisp1, RKISP1_CIF_MI_XTD_FORMAT_CTRL);
493 		if (cap->pix.cfg->uv_swap)
494 			reg |= RKISP1_CIF_MI_XTD_FMT_CTRL_SP_CB_CR_SWAP;
495 		else
496 			reg &= ~RKISP1_CIF_MI_XTD_FMT_CTRL_SP_CB_CR_SWAP;
497 		rkisp1_write(rkisp1, RKISP1_CIF_MI_XTD_FORMAT_CTRL, reg);
498 	}
499 
500 	rkisp1_mi_config_ctrl(cap);
501 
502 	mi_ctrl = rkisp1_read(rkisp1, RKISP1_CIF_MI_CTRL);
503 	mi_ctrl &= ~RKISP1_MI_CTRL_SP_FMT_MASK;
504 	mi_ctrl |= cap->pix.cfg->write_format |
505 		   RKISP1_MI_CTRL_SP_INPUT_YUV422 |
506 		   cap->pix.cfg->output_format |
507 		   RKISP1_CIF_MI_SP_AUTOUPDATE_ENABLE;
508 	rkisp1_write(rkisp1, RKISP1_CIF_MI_CTRL, mi_ctrl);
509 }
510 
511 static void rkisp1_mp_disable(struct rkisp1_capture *cap)
512 {
513 	u32 mi_ctrl = rkisp1_read(cap->rkisp1, RKISP1_CIF_MI_CTRL);
514 
515 	mi_ctrl &= ~(RKISP1_CIF_MI_CTRL_MP_ENABLE |
516 		     RKISP1_CIF_MI_CTRL_RAW_ENABLE);
517 	rkisp1_write(cap->rkisp1, RKISP1_CIF_MI_CTRL, mi_ctrl);
518 }
519 
520 static void rkisp1_sp_disable(struct rkisp1_capture *cap)
521 {
522 	u32 mi_ctrl = rkisp1_read(cap->rkisp1, RKISP1_CIF_MI_CTRL);
523 
524 	mi_ctrl &= ~RKISP1_CIF_MI_CTRL_SP_ENABLE;
525 	rkisp1_write(cap->rkisp1, RKISP1_CIF_MI_CTRL, mi_ctrl);
526 }
527 
528 static void rkisp1_mp_enable(struct rkisp1_capture *cap)
529 {
530 	u32 mi_ctrl;
531 
532 	rkisp1_mp_disable(cap);
533 
534 	mi_ctrl = rkisp1_read(cap->rkisp1, RKISP1_CIF_MI_CTRL);
535 	if (v4l2_is_format_bayer(cap->pix.info))
536 		mi_ctrl |= RKISP1_CIF_MI_CTRL_RAW_ENABLE;
537 	/* YUV */
538 	else
539 		mi_ctrl |= RKISP1_CIF_MI_CTRL_MP_ENABLE;
540 
541 	rkisp1_write(cap->rkisp1, RKISP1_CIF_MI_CTRL, mi_ctrl);
542 }
543 
544 static void rkisp1_sp_enable(struct rkisp1_capture *cap)
545 {
546 	u32 mi_ctrl = rkisp1_read(cap->rkisp1, RKISP1_CIF_MI_CTRL);
547 
548 	mi_ctrl |= RKISP1_CIF_MI_CTRL_SP_ENABLE;
549 	rkisp1_write(cap->rkisp1, RKISP1_CIF_MI_CTRL, mi_ctrl);
550 }
551 
552 static void rkisp1_mp_sp_stop(struct rkisp1_capture *cap)
553 {
554 	if (!cap->is_streaming)
555 		return;
556 	rkisp1_write(cap->rkisp1, RKISP1_CIF_MI_ICR, RKISP1_CIF_MI_FRAME(cap));
557 	cap->ops->disable(cap);
558 }
559 
560 static bool rkisp1_mp_is_stopped(struct rkisp1_capture *cap)
561 {
562 	u32 en = RKISP1_CIF_MI_CTRL_SHD_MP_IN_ENABLED |
563 		 RKISP1_CIF_MI_CTRL_SHD_RAW_OUT_ENABLED;
564 
565 	return !(rkisp1_read(cap->rkisp1, RKISP1_CIF_MI_CTRL_SHD) & en);
566 }
567 
568 static bool rkisp1_sp_is_stopped(struct rkisp1_capture *cap)
569 {
570 	return !(rkisp1_read(cap->rkisp1, RKISP1_CIF_MI_CTRL_SHD) &
571 		 RKISP1_CIF_MI_CTRL_SHD_SP_IN_ENABLED);
572 }
573 
574 static void rkisp1_mp_set_data_path(struct rkisp1_capture *cap)
575 {
576 	u32 dpcl = rkisp1_read(cap->rkisp1, RKISP1_CIF_VI_DPCL);
577 
578 	dpcl = dpcl | RKISP1_CIF_VI_DPCL_CHAN_MODE_MP |
579 	       RKISP1_CIF_VI_DPCL_MP_MUX_MRSZ_MI;
580 	rkisp1_write(cap->rkisp1, RKISP1_CIF_VI_DPCL, dpcl);
581 }
582 
583 static void rkisp1_sp_set_data_path(struct rkisp1_capture *cap)
584 {
585 	u32 dpcl = rkisp1_read(cap->rkisp1, RKISP1_CIF_VI_DPCL);
586 
587 	dpcl |= RKISP1_CIF_VI_DPCL_CHAN_MODE_SP;
588 	rkisp1_write(cap->rkisp1, RKISP1_CIF_VI_DPCL, dpcl);
589 }
590 
591 static const struct rkisp1_capture_ops rkisp1_capture_ops_mp = {
592 	.config = rkisp1_mp_config,
593 	.enable = rkisp1_mp_enable,
594 	.disable = rkisp1_mp_disable,
595 	.stop = rkisp1_mp_sp_stop,
596 	.set_data_path = rkisp1_mp_set_data_path,
597 	.is_stopped = rkisp1_mp_is_stopped,
598 };
599 
600 static const struct rkisp1_capture_ops rkisp1_capture_ops_sp = {
601 	.config = rkisp1_sp_config,
602 	.enable = rkisp1_sp_enable,
603 	.disable = rkisp1_sp_disable,
604 	.stop = rkisp1_mp_sp_stop,
605 	.set_data_path = rkisp1_sp_set_data_path,
606 	.is_stopped = rkisp1_sp_is_stopped,
607 };
608 
609 /* ----------------------------------------------------------------------------
610  * Frame buffer operations
611  */
612 
613 static int rkisp1_dummy_buf_create(struct rkisp1_capture *cap)
614 {
615 	const struct v4l2_pix_format_mplane *pixm = &cap->pix.fmt;
616 	struct rkisp1_dummy_buffer *dummy_buf = &cap->buf.dummy;
617 
618 	dummy_buf->size = max3(rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_Y),
619 			       rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_CB),
620 			       rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_CR));
621 
622 	/* The driver never access vaddr, no mapping is required */
623 	dummy_buf->vaddr = dma_alloc_attrs(cap->rkisp1->dev,
624 					   dummy_buf->size,
625 					   &dummy_buf->dma_addr,
626 					   GFP_KERNEL,
627 					   DMA_ATTR_NO_KERNEL_MAPPING);
628 	if (!dummy_buf->vaddr)
629 		return -ENOMEM;
630 
631 	return 0;
632 }
633 
634 static void rkisp1_dummy_buf_destroy(struct rkisp1_capture *cap)
635 {
636 	dma_free_attrs(cap->rkisp1->dev,
637 		       cap->buf.dummy.size, cap->buf.dummy.vaddr,
638 		       cap->buf.dummy.dma_addr, DMA_ATTR_NO_KERNEL_MAPPING);
639 }
640 
641 static void rkisp1_set_next_buf(struct rkisp1_capture *cap)
642 {
643 	cap->buf.curr = cap->buf.next;
644 	cap->buf.next = NULL;
645 
646 	if (!list_empty(&cap->buf.queue)) {
647 		u32 *buff_addr;
648 
649 		cap->buf.next = list_first_entry(&cap->buf.queue, struct rkisp1_buffer, queue);
650 		list_del(&cap->buf.next->queue);
651 
652 		buff_addr = cap->buf.next->buff_addr;
653 
654 		rkisp1_write(cap->rkisp1, cap->config->mi.y_base_ad_init,
655 			     buff_addr[RKISP1_PLANE_Y]);
656 		/*
657 		 * In order to support grey format we capture
658 		 * YUV422 planar format from the camera and
659 		 * set the U and V planes to the dummy buffer
660 		 */
661 		if (cap->pix.cfg->fourcc == V4L2_PIX_FMT_GREY) {
662 			rkisp1_write(cap->rkisp1,
663 				     cap->config->mi.cb_base_ad_init,
664 				     cap->buf.dummy.dma_addr);
665 			rkisp1_write(cap->rkisp1,
666 				     cap->config->mi.cr_base_ad_init,
667 				     cap->buf.dummy.dma_addr);
668 		} else {
669 			rkisp1_write(cap->rkisp1,
670 				     cap->config->mi.cb_base_ad_init,
671 				     buff_addr[RKISP1_PLANE_CB]);
672 			rkisp1_write(cap->rkisp1,
673 				     cap->config->mi.cr_base_ad_init,
674 				     buff_addr[RKISP1_PLANE_CR]);
675 		}
676 	} else {
677 		/*
678 		 * Use the dummy space allocated by dma_alloc_coherent to
679 		 * throw data if there is no available buffer.
680 		 */
681 		rkisp1_write(cap->rkisp1, cap->config->mi.y_base_ad_init,
682 			     cap->buf.dummy.dma_addr);
683 		rkisp1_write(cap->rkisp1, cap->config->mi.cb_base_ad_init,
684 			     cap->buf.dummy.dma_addr);
685 		rkisp1_write(cap->rkisp1, cap->config->mi.cr_base_ad_init,
686 			     cap->buf.dummy.dma_addr);
687 	}
688 
689 	/* Set plane offsets */
690 	rkisp1_write(cap->rkisp1, cap->config->mi.y_offs_cnt_init, 0);
691 	rkisp1_write(cap->rkisp1, cap->config->mi.cb_offs_cnt_init, 0);
692 	rkisp1_write(cap->rkisp1, cap->config->mi.cr_offs_cnt_init, 0);
693 }
694 
695 /*
696  * This function is called when a frame end comes. The next frame
697  * is processing and we should set up buffer for next-next frame,
698  * otherwise it will overflow.
699  */
700 static void rkisp1_handle_buffer(struct rkisp1_capture *cap)
701 {
702 	struct rkisp1_isp *isp = &cap->rkisp1->isp;
703 	struct rkisp1_buffer *curr_buf;
704 
705 	spin_lock(&cap->buf.lock);
706 	curr_buf = cap->buf.curr;
707 
708 	if (curr_buf) {
709 		curr_buf->vb.sequence = isp->frame_sequence;
710 		curr_buf->vb.vb2_buf.timestamp = ktime_get_boottime_ns();
711 		curr_buf->vb.field = V4L2_FIELD_NONE;
712 		vb2_buffer_done(&curr_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
713 	} else {
714 		cap->rkisp1->debug.frame_drop[cap->id]++;
715 	}
716 
717 	rkisp1_set_next_buf(cap);
718 	spin_unlock(&cap->buf.lock);
719 }
720 
721 irqreturn_t rkisp1_capture_isr(int irq, void *ctx)
722 {
723 	struct device *dev = ctx;
724 	struct rkisp1_device *rkisp1 = dev_get_drvdata(dev);
725 	unsigned int i;
726 	u32 status;
727 
728 	status = rkisp1_read(rkisp1, RKISP1_CIF_MI_MIS);
729 	if (!status)
730 		return IRQ_NONE;
731 
732 	rkisp1_write(rkisp1, RKISP1_CIF_MI_ICR, status);
733 
734 	for (i = 0; i < ARRAY_SIZE(rkisp1->capture_devs); ++i) {
735 		struct rkisp1_capture *cap = &rkisp1->capture_devs[i];
736 
737 		if (!(status & RKISP1_CIF_MI_FRAME(cap)))
738 			continue;
739 		if (!cap->is_stopping) {
740 			rkisp1_handle_buffer(cap);
741 			continue;
742 		}
743 		/*
744 		 * Make sure stream is actually stopped, whose state
745 		 * can be read from the shadow register, before
746 		 * wake_up() thread which would immediately free all
747 		 * frame buffers. stop() takes effect at the next
748 		 * frame end that sync the configurations to shadow
749 		 * regs.
750 		 */
751 		if (!cap->ops->is_stopped(cap)) {
752 			cap->ops->stop(cap);
753 			continue;
754 		}
755 		cap->is_stopping = false;
756 		cap->is_streaming = false;
757 		wake_up(&cap->done);
758 	}
759 
760 	return IRQ_HANDLED;
761 }
762 
763 /* ----------------------------------------------------------------------------
764  * Vb2 operations
765  */
766 
767 static int rkisp1_vb2_queue_setup(struct vb2_queue *queue,
768 				  unsigned int *num_buffers,
769 				  unsigned int *num_planes,
770 				  unsigned int sizes[],
771 				  struct device *alloc_devs[])
772 {
773 	struct rkisp1_capture *cap = queue->drv_priv;
774 	const struct v4l2_pix_format_mplane *pixm = &cap->pix.fmt;
775 	unsigned int i;
776 
777 	if (*num_planes) {
778 		if (*num_planes != pixm->num_planes)
779 			return -EINVAL;
780 
781 		for (i = 0; i < pixm->num_planes; i++)
782 			if (sizes[i] < pixm->plane_fmt[i].sizeimage)
783 				return -EINVAL;
784 	} else {
785 		*num_planes = pixm->num_planes;
786 		for (i = 0; i < pixm->num_planes; i++)
787 			sizes[i] = pixm->plane_fmt[i].sizeimage;
788 	}
789 
790 	return 0;
791 }
792 
793 static int rkisp1_vb2_buf_init(struct vb2_buffer *vb)
794 {
795 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
796 	struct rkisp1_buffer *ispbuf =
797 		container_of(vbuf, struct rkisp1_buffer, vb);
798 	struct rkisp1_capture *cap = vb->vb2_queue->drv_priv;
799 	const struct v4l2_pix_format_mplane *pixm = &cap->pix.fmt;
800 	unsigned int i;
801 
802 	memset(ispbuf->buff_addr, 0, sizeof(ispbuf->buff_addr));
803 	for (i = 0; i < pixm->num_planes; i++)
804 		ispbuf->buff_addr[i] = vb2_dma_contig_plane_dma_addr(vb, i);
805 
806 	/* Convert to non-MPLANE */
807 	if (pixm->num_planes == 1) {
808 		ispbuf->buff_addr[RKISP1_PLANE_CB] =
809 			ispbuf->buff_addr[RKISP1_PLANE_Y] +
810 			rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_Y);
811 		ispbuf->buff_addr[RKISP1_PLANE_CR] =
812 			ispbuf->buff_addr[RKISP1_PLANE_CB] +
813 			rkisp1_pixfmt_comp_size(pixm, RKISP1_PLANE_CB);
814 	}
815 
816 	/*
817 	 * uv swap can be supported for planar formats by switching
818 	 * the address of cb and cr
819 	 */
820 	if (cap->pix.info->comp_planes == 3 && cap->pix.cfg->uv_swap)
821 		swap(ispbuf->buff_addr[RKISP1_PLANE_CR],
822 		     ispbuf->buff_addr[RKISP1_PLANE_CB]);
823 	return 0;
824 }
825 
826 static void rkisp1_vb2_buf_queue(struct vb2_buffer *vb)
827 {
828 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
829 	struct rkisp1_buffer *ispbuf =
830 		container_of(vbuf, struct rkisp1_buffer, vb);
831 	struct rkisp1_capture *cap = vb->vb2_queue->drv_priv;
832 
833 	spin_lock_irq(&cap->buf.lock);
834 	list_add_tail(&ispbuf->queue, &cap->buf.queue);
835 	spin_unlock_irq(&cap->buf.lock);
836 }
837 
838 static int rkisp1_vb2_buf_prepare(struct vb2_buffer *vb)
839 {
840 	struct rkisp1_capture *cap = vb->vb2_queue->drv_priv;
841 	unsigned int i;
842 
843 	for (i = 0; i < cap->pix.fmt.num_planes; i++) {
844 		unsigned long size = cap->pix.fmt.plane_fmt[i].sizeimage;
845 
846 		if (vb2_plane_size(vb, i) < size) {
847 			dev_err(cap->rkisp1->dev,
848 				"User buffer too small (%ld < %ld)\n",
849 				vb2_plane_size(vb, i), size);
850 			return -EINVAL;
851 		}
852 		vb2_set_plane_payload(vb, i, size);
853 	}
854 
855 	return 0;
856 }
857 
858 static void rkisp1_return_all_buffers(struct rkisp1_capture *cap,
859 				      enum vb2_buffer_state state)
860 {
861 	struct rkisp1_buffer *buf;
862 
863 	spin_lock_irq(&cap->buf.lock);
864 	if (cap->buf.curr) {
865 		vb2_buffer_done(&cap->buf.curr->vb.vb2_buf, state);
866 		cap->buf.curr = NULL;
867 	}
868 	if (cap->buf.next) {
869 		vb2_buffer_done(&cap->buf.next->vb.vb2_buf, state);
870 		cap->buf.next = NULL;
871 	}
872 	while (!list_empty(&cap->buf.queue)) {
873 		buf = list_first_entry(&cap->buf.queue,
874 				       struct rkisp1_buffer, queue);
875 		list_del(&buf->queue);
876 		vb2_buffer_done(&buf->vb.vb2_buf, state);
877 	}
878 	spin_unlock_irq(&cap->buf.lock);
879 }
880 
881 /*
882  * Most registers inside the rockchip ISP1 have shadow register since
883  * they must not be changed while processing a frame.
884  * Usually, each sub-module updates its shadow register after
885  * processing the last pixel of a frame.
886  */
887 static void rkisp1_cap_stream_enable(struct rkisp1_capture *cap)
888 {
889 	struct rkisp1_device *rkisp1 = cap->rkisp1;
890 	struct rkisp1_capture *other = &rkisp1->capture_devs[cap->id ^ 1];
891 
892 	cap->ops->set_data_path(cap);
893 	cap->ops->config(cap);
894 
895 	/* Setup a buffer for the next frame */
896 	spin_lock_irq(&cap->buf.lock);
897 	rkisp1_set_next_buf(cap);
898 	cap->ops->enable(cap);
899 	/* It's safe to configure ACTIVE and SHADOW registers for the
900 	 * first stream. While when the second is starting, do NOT
901 	 * force update because it also updates the first one.
902 	 *
903 	 * The latter case would drop one more buffer(that is 2) since
904 	 * there's no buffer in a shadow register when the second FE received.
905 	 * This's also required because the second FE maybe corrupt
906 	 * especially when run at 120fps.
907 	 */
908 	if (!other->is_streaming) {
909 		/* force cfg update */
910 		rkisp1_write(rkisp1, RKISP1_CIF_MI_INIT,
911 			     RKISP1_CIF_MI_INIT_SOFT_UPD);
912 		rkisp1_set_next_buf(cap);
913 	}
914 	spin_unlock_irq(&cap->buf.lock);
915 	cap->is_streaming = true;
916 }
917 
918 static void rkisp1_cap_stream_disable(struct rkisp1_capture *cap)
919 {
920 	int ret;
921 
922 	/* Stream should stop in interrupt. If it doesn't, stop it by force. */
923 	cap->is_stopping = true;
924 	ret = wait_event_timeout(cap->done,
925 				 !cap->is_streaming,
926 				 msecs_to_jiffies(1000));
927 	if (!ret) {
928 		cap->rkisp1->debug.stop_timeout[cap->id]++;
929 		cap->ops->stop(cap);
930 		cap->is_stopping = false;
931 		cap->is_streaming = false;
932 	}
933 }
934 
935 /*
936  * rkisp1_pipeline_stream_disable - disable nodes in the pipeline
937  *
938  * Call s_stream(false) in the reverse order from
939  * rkisp1_pipeline_stream_enable() and disable the DMA engine.
940  * Should be called before video_device_pipeline_stop()
941  */
942 static void rkisp1_pipeline_stream_disable(struct rkisp1_capture *cap)
943 	__must_hold(&cap->rkisp1->stream_lock)
944 {
945 	struct rkisp1_device *rkisp1 = cap->rkisp1;
946 
947 	rkisp1_cap_stream_disable(cap);
948 
949 	/*
950 	 * If the other capture is streaming, isp and sensor nodes shouldn't
951 	 * be disabled, skip them.
952 	 */
953 	if (rkisp1->pipe.start_count < 2)
954 		v4l2_subdev_call(&rkisp1->isp.sd, video, s_stream, false);
955 
956 	v4l2_subdev_call(&rkisp1->resizer_devs[cap->id].sd, video, s_stream,
957 			 false);
958 }
959 
960 /*
961  * rkisp1_pipeline_stream_enable - enable nodes in the pipeline
962  *
963  * Enable the DMA Engine and call s_stream(true) through the pipeline.
964  * Should be called after video_device_pipeline_start()
965  */
966 static int rkisp1_pipeline_stream_enable(struct rkisp1_capture *cap)
967 	__must_hold(&cap->rkisp1->stream_lock)
968 {
969 	struct rkisp1_device *rkisp1 = cap->rkisp1;
970 	int ret;
971 
972 	rkisp1_cap_stream_enable(cap);
973 
974 	ret = v4l2_subdev_call(&rkisp1->resizer_devs[cap->id].sd, video,
975 			       s_stream, true);
976 	if (ret)
977 		goto err_disable_cap;
978 
979 	/*
980 	 * If the other capture is streaming, isp and sensor nodes are already
981 	 * enabled, skip them.
982 	 */
983 	if (rkisp1->pipe.start_count > 1)
984 		return 0;
985 
986 	ret = v4l2_subdev_call(&rkisp1->isp.sd, video, s_stream, true);
987 	if (ret)
988 		goto err_disable_rsz;
989 
990 	return 0;
991 
992 err_disable_rsz:
993 	v4l2_subdev_call(&rkisp1->resizer_devs[cap->id].sd, video, s_stream,
994 			 false);
995 err_disable_cap:
996 	rkisp1_cap_stream_disable(cap);
997 
998 	return ret;
999 }
1000 
1001 static void rkisp1_vb2_stop_streaming(struct vb2_queue *queue)
1002 {
1003 	struct rkisp1_capture *cap = queue->drv_priv;
1004 	struct rkisp1_vdev_node *node = &cap->vnode;
1005 	struct rkisp1_device *rkisp1 = cap->rkisp1;
1006 	int ret;
1007 
1008 	mutex_lock(&cap->rkisp1->stream_lock);
1009 
1010 	rkisp1_pipeline_stream_disable(cap);
1011 
1012 	rkisp1_return_all_buffers(cap, VB2_BUF_STATE_ERROR);
1013 
1014 	v4l2_pipeline_pm_put(&node->vdev.entity);
1015 	ret = pm_runtime_put(rkisp1->dev);
1016 	if (ret < 0)
1017 		dev_err(rkisp1->dev, "power down failed error:%d\n", ret);
1018 
1019 	rkisp1_dummy_buf_destroy(cap);
1020 
1021 	video_device_pipeline_stop(&node->vdev);
1022 
1023 	mutex_unlock(&cap->rkisp1->stream_lock);
1024 }
1025 
1026 static int
1027 rkisp1_vb2_start_streaming(struct vb2_queue *queue, unsigned int count)
1028 {
1029 	struct rkisp1_capture *cap = queue->drv_priv;
1030 	struct media_entity *entity = &cap->vnode.vdev.entity;
1031 	int ret;
1032 
1033 	mutex_lock(&cap->rkisp1->stream_lock);
1034 
1035 	ret = video_device_pipeline_start(&cap->vnode.vdev, &cap->rkisp1->pipe);
1036 	if (ret) {
1037 		dev_err(cap->rkisp1->dev, "start pipeline failed %d\n", ret);
1038 		goto err_ret_buffers;
1039 	}
1040 
1041 	ret = rkisp1_dummy_buf_create(cap);
1042 	if (ret)
1043 		goto err_pipeline_stop;
1044 
1045 	ret = pm_runtime_resume_and_get(cap->rkisp1->dev);
1046 	if (ret < 0) {
1047 		dev_err(cap->rkisp1->dev, "power up failed %d\n", ret);
1048 		goto err_destroy_dummy;
1049 	}
1050 	ret = v4l2_pipeline_pm_get(entity);
1051 	if (ret) {
1052 		dev_err(cap->rkisp1->dev, "open cif pipeline failed %d\n", ret);
1053 		goto err_pipe_pm_put;
1054 	}
1055 
1056 	ret = rkisp1_pipeline_stream_enable(cap);
1057 	if (ret)
1058 		goto err_v4l2_pm_put;
1059 
1060 	mutex_unlock(&cap->rkisp1->stream_lock);
1061 
1062 	return 0;
1063 
1064 err_v4l2_pm_put:
1065 	v4l2_pipeline_pm_put(entity);
1066 err_pipe_pm_put:
1067 	pm_runtime_put(cap->rkisp1->dev);
1068 err_destroy_dummy:
1069 	rkisp1_dummy_buf_destroy(cap);
1070 err_pipeline_stop:
1071 	video_device_pipeline_stop(&cap->vnode.vdev);
1072 err_ret_buffers:
1073 	rkisp1_return_all_buffers(cap, VB2_BUF_STATE_QUEUED);
1074 	mutex_unlock(&cap->rkisp1->stream_lock);
1075 
1076 	return ret;
1077 }
1078 
1079 static const struct vb2_ops rkisp1_vb2_ops = {
1080 	.queue_setup = rkisp1_vb2_queue_setup,
1081 	.buf_init = rkisp1_vb2_buf_init,
1082 	.buf_queue = rkisp1_vb2_buf_queue,
1083 	.buf_prepare = rkisp1_vb2_buf_prepare,
1084 	.wait_prepare = vb2_ops_wait_prepare,
1085 	.wait_finish = vb2_ops_wait_finish,
1086 	.stop_streaming = rkisp1_vb2_stop_streaming,
1087 	.start_streaming = rkisp1_vb2_start_streaming,
1088 };
1089 
1090 /* ----------------------------------------------------------------------------
1091  * IOCTLs operations
1092  */
1093 
1094 static const struct v4l2_format_info *
1095 rkisp1_fill_pixfmt(struct v4l2_pix_format_mplane *pixm,
1096 		   enum rkisp1_stream_id id)
1097 {
1098 	struct v4l2_plane_pix_format *plane_y = &pixm->plane_fmt[0];
1099 	const struct v4l2_format_info *info;
1100 	unsigned int i;
1101 	u32 stride;
1102 
1103 	memset(pixm->plane_fmt, 0, sizeof(pixm->plane_fmt));
1104 	info = v4l2_format_info(pixm->pixelformat);
1105 	pixm->num_planes = info->mem_planes;
1106 
1107 	/*
1108 	 * The SP supports custom strides, expressed as a number of pixels for
1109 	 * the Y plane. Clamp the stride to a reasonable value to avoid integer
1110 	 * overflows when calculating the bytesperline and sizeimage values.
1111 	 */
1112 	if (id == RKISP1_SELFPATH)
1113 		stride = clamp(DIV_ROUND_UP(plane_y->bytesperline, info->bpp[0]),
1114 			       pixm->width, 65536U);
1115 	else
1116 		stride = pixm->width;
1117 
1118 	plane_y->bytesperline = stride * info->bpp[0];
1119 	plane_y->sizeimage = plane_y->bytesperline * pixm->height;
1120 
1121 	for (i = 1; i < info->comp_planes; i++) {
1122 		struct v4l2_plane_pix_format *plane = &pixm->plane_fmt[i];
1123 
1124 		/* bytesperline for other components derive from Y component */
1125 		plane->bytesperline = DIV_ROUND_UP(stride, info->hdiv) *
1126 				      info->bpp[i];
1127 		plane->sizeimage = plane->bytesperline *
1128 				   DIV_ROUND_UP(pixm->height, info->vdiv);
1129 	}
1130 
1131 	/*
1132 	 * If pixfmt is packed, then plane_fmt[0] should contain the total size
1133 	 * considering all components. plane_fmt[i] for i > 0 should be ignored
1134 	 * by userspace as mem_planes == 1, but we are keeping information there
1135 	 * for convenience.
1136 	 */
1137 	if (info->mem_planes == 1)
1138 		for (i = 1; i < info->comp_planes; i++)
1139 			plane_y->sizeimage += pixm->plane_fmt[i].sizeimage;
1140 
1141 	return info;
1142 }
1143 
1144 static const struct rkisp1_capture_fmt_cfg *
1145 rkisp1_find_fmt_cfg(const struct rkisp1_capture *cap, const u32 pixelfmt)
1146 {
1147 	unsigned int i;
1148 
1149 	for (i = 0; i < cap->config->fmt_size; i++) {
1150 		if (cap->config->fmts[i].fourcc == pixelfmt)
1151 			return &cap->config->fmts[i];
1152 	}
1153 	return NULL;
1154 }
1155 
1156 static void rkisp1_try_fmt(const struct rkisp1_capture *cap,
1157 			   struct v4l2_pix_format_mplane *pixm,
1158 			   const struct rkisp1_capture_fmt_cfg **fmt_cfg,
1159 			   const struct v4l2_format_info **fmt_info)
1160 {
1161 	const struct rkisp1_capture_config *config = cap->config;
1162 	const struct rkisp1_capture_fmt_cfg *fmt;
1163 	const struct v4l2_format_info *info;
1164 	static const unsigned int max_widths[] = {
1165 		RKISP1_RSZ_MP_SRC_MAX_WIDTH, RKISP1_RSZ_SP_SRC_MAX_WIDTH
1166 	};
1167 	static const unsigned int max_heights[] = {
1168 		RKISP1_RSZ_MP_SRC_MAX_HEIGHT, RKISP1_RSZ_SP_SRC_MAX_HEIGHT
1169 	};
1170 
1171 	fmt = rkisp1_find_fmt_cfg(cap, pixm->pixelformat);
1172 	if (!fmt) {
1173 		fmt = config->fmts;
1174 		pixm->pixelformat = fmt->fourcc;
1175 	}
1176 
1177 	pixm->width = clamp_t(u32, pixm->width,
1178 			      RKISP1_RSZ_SRC_MIN_WIDTH, max_widths[cap->id]);
1179 	pixm->height = clamp_t(u32, pixm->height,
1180 			       RKISP1_RSZ_SRC_MIN_HEIGHT, max_heights[cap->id]);
1181 
1182 	pixm->field = V4L2_FIELD_NONE;
1183 	pixm->colorspace = V4L2_COLORSPACE_DEFAULT;
1184 	pixm->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
1185 	pixm->quantization = V4L2_QUANTIZATION_DEFAULT;
1186 
1187 	info = rkisp1_fill_pixfmt(pixm, cap->id);
1188 
1189 	if (fmt_cfg)
1190 		*fmt_cfg = fmt;
1191 	if (fmt_info)
1192 		*fmt_info = info;
1193 }
1194 
1195 static void rkisp1_set_fmt(struct rkisp1_capture *cap,
1196 			   struct v4l2_pix_format_mplane *pixm)
1197 {
1198 	rkisp1_try_fmt(cap, pixm, &cap->pix.cfg, &cap->pix.info);
1199 	cap->pix.fmt = *pixm;
1200 
1201 	/* SP supports custom stride in number of pixels of the Y plane */
1202 	if (cap->id == RKISP1_SELFPATH)
1203 		cap->sp_y_stride = pixm->plane_fmt[0].bytesperline /
1204 				   cap->pix.info->bpp[0];
1205 }
1206 
1207 static int rkisp1_try_fmt_vid_cap_mplane(struct file *file, void *fh,
1208 					 struct v4l2_format *f)
1209 {
1210 	struct rkisp1_capture *cap = video_drvdata(file);
1211 
1212 	rkisp1_try_fmt(cap, &f->fmt.pix_mp, NULL, NULL);
1213 
1214 	return 0;
1215 }
1216 
1217 static int rkisp1_enum_fmt_vid_cap_mplane(struct file *file, void *priv,
1218 					  struct v4l2_fmtdesc *f)
1219 {
1220 	struct rkisp1_capture *cap = video_drvdata(file);
1221 	const struct rkisp1_capture_fmt_cfg *fmt = NULL;
1222 	unsigned int i, n = 0;
1223 
1224 	if (!f->mbus_code) {
1225 		if (f->index >= cap->config->fmt_size)
1226 			return -EINVAL;
1227 
1228 		fmt = &cap->config->fmts[f->index];
1229 		f->pixelformat = fmt->fourcc;
1230 		return 0;
1231 	}
1232 
1233 	for (i = 0; i < cap->config->fmt_size; i++) {
1234 		if (cap->config->fmts[i].mbus != f->mbus_code)
1235 			continue;
1236 
1237 		if (n++ == f->index) {
1238 			f->pixelformat = cap->config->fmts[i].fourcc;
1239 			return 0;
1240 		}
1241 	}
1242 	return -EINVAL;
1243 }
1244 
1245 static int rkisp1_enum_framesizes(struct file *file, void *fh,
1246 				  struct v4l2_frmsizeenum *fsize)
1247 {
1248 	static const unsigned int max_widths[] = {
1249 		RKISP1_RSZ_MP_SRC_MAX_WIDTH,
1250 		RKISP1_RSZ_SP_SRC_MAX_WIDTH,
1251 	};
1252 	static const unsigned int max_heights[] = {
1253 		RKISP1_RSZ_MP_SRC_MAX_HEIGHT,
1254 		RKISP1_RSZ_SP_SRC_MAX_HEIGHT,
1255 	};
1256 	struct rkisp1_capture *cap = video_drvdata(file);
1257 
1258 	if (fsize->index != 0)
1259 		return -EINVAL;
1260 
1261 	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
1262 
1263 	fsize->stepwise.min_width = RKISP1_RSZ_SRC_MIN_WIDTH;
1264 	fsize->stepwise.max_width = max_widths[cap->id];
1265 	fsize->stepwise.step_width = 2;
1266 
1267 	fsize->stepwise.min_height = RKISP1_RSZ_SRC_MIN_HEIGHT;
1268 	fsize->stepwise.max_height = max_heights[cap->id];
1269 	fsize->stepwise.step_height = 2;
1270 
1271 	return 0;
1272 }
1273 
1274 static int rkisp1_s_fmt_vid_cap_mplane(struct file *file,
1275 				       void *priv, struct v4l2_format *f)
1276 {
1277 	struct rkisp1_capture *cap = video_drvdata(file);
1278 	struct rkisp1_vdev_node *node =
1279 				rkisp1_vdev_to_node(&cap->vnode.vdev);
1280 
1281 	if (vb2_is_busy(&node->buf_queue))
1282 		return -EBUSY;
1283 
1284 	rkisp1_set_fmt(cap, &f->fmt.pix_mp);
1285 
1286 	return 0;
1287 }
1288 
1289 static int rkisp1_g_fmt_vid_cap_mplane(struct file *file, void *fh,
1290 				       struct v4l2_format *f)
1291 {
1292 	struct rkisp1_capture *cap = video_drvdata(file);
1293 
1294 	f->fmt.pix_mp = cap->pix.fmt;
1295 
1296 	return 0;
1297 }
1298 
1299 static int
1300 rkisp1_querycap(struct file *file, void *priv, struct v4l2_capability *cap)
1301 {
1302 	strscpy(cap->driver, RKISP1_DRIVER_NAME, sizeof(cap->driver));
1303 	strscpy(cap->card, RKISP1_DRIVER_NAME, sizeof(cap->card));
1304 	strscpy(cap->bus_info, RKISP1_BUS_INFO, sizeof(cap->bus_info));
1305 
1306 	return 0;
1307 }
1308 
1309 static const struct v4l2_ioctl_ops rkisp1_v4l2_ioctl_ops = {
1310 	.vidioc_reqbufs = vb2_ioctl_reqbufs,
1311 	.vidioc_querybuf = vb2_ioctl_querybuf,
1312 	.vidioc_create_bufs = vb2_ioctl_create_bufs,
1313 	.vidioc_qbuf = vb2_ioctl_qbuf,
1314 	.vidioc_expbuf = vb2_ioctl_expbuf,
1315 	.vidioc_dqbuf = vb2_ioctl_dqbuf,
1316 	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
1317 	.vidioc_streamon = vb2_ioctl_streamon,
1318 	.vidioc_streamoff = vb2_ioctl_streamoff,
1319 	.vidioc_try_fmt_vid_cap_mplane = rkisp1_try_fmt_vid_cap_mplane,
1320 	.vidioc_s_fmt_vid_cap_mplane = rkisp1_s_fmt_vid_cap_mplane,
1321 	.vidioc_g_fmt_vid_cap_mplane = rkisp1_g_fmt_vid_cap_mplane,
1322 	.vidioc_enum_fmt_vid_cap = rkisp1_enum_fmt_vid_cap_mplane,
1323 	.vidioc_enum_framesizes = rkisp1_enum_framesizes,
1324 	.vidioc_querycap = rkisp1_querycap,
1325 	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
1326 	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
1327 };
1328 
1329 static int rkisp1_capture_link_validate(struct media_link *link)
1330 {
1331 	struct video_device *vdev =
1332 		media_entity_to_video_device(link->sink->entity);
1333 	struct v4l2_subdev *sd =
1334 		media_entity_to_v4l2_subdev(link->source->entity);
1335 	struct rkisp1_capture *cap = video_get_drvdata(vdev);
1336 	const struct rkisp1_capture_fmt_cfg *fmt =
1337 		rkisp1_find_fmt_cfg(cap, cap->pix.fmt.pixelformat);
1338 	struct v4l2_subdev_format sd_fmt = {
1339 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
1340 		.pad = link->source->index,
1341 	};
1342 	int ret;
1343 
1344 	ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &sd_fmt);
1345 	if (ret)
1346 		return ret;
1347 
1348 	if (sd_fmt.format.height != cap->pix.fmt.height ||
1349 	    sd_fmt.format.width != cap->pix.fmt.width ||
1350 	    sd_fmt.format.code != fmt->mbus) {
1351 		dev_dbg(cap->rkisp1->dev,
1352 			"link '%s':%u -> '%s':%u not valid: 0x%04x/%ux%u != 0x%04x/%ux%u\n",
1353 			link->source->entity->name, link->source->index,
1354 			link->sink->entity->name, link->sink->index,
1355 			sd_fmt.format.code, sd_fmt.format.width,
1356 			sd_fmt.format.height, fmt->mbus, cap->pix.fmt.width,
1357 			cap->pix.fmt.height);
1358 		return -EPIPE;
1359 	}
1360 
1361 	return 0;
1362 }
1363 
1364 /* ----------------------------------------------------------------------------
1365  * core functions
1366  */
1367 
1368 static const struct media_entity_operations rkisp1_media_ops = {
1369 	.link_validate = rkisp1_capture_link_validate,
1370 };
1371 
1372 static const struct v4l2_file_operations rkisp1_fops = {
1373 	.open = v4l2_fh_open,
1374 	.release = vb2_fop_release,
1375 	.unlocked_ioctl = video_ioctl2,
1376 	.poll = vb2_fop_poll,
1377 	.mmap = vb2_fop_mmap,
1378 };
1379 
1380 static void rkisp1_unregister_capture(struct rkisp1_capture *cap)
1381 {
1382 	if (!video_is_registered(&cap->vnode.vdev))
1383 		return;
1384 
1385 	media_entity_cleanup(&cap->vnode.vdev.entity);
1386 	vb2_video_unregister_device(&cap->vnode.vdev);
1387 	mutex_destroy(&cap->vnode.vlock);
1388 }
1389 
1390 void rkisp1_capture_devs_unregister(struct rkisp1_device *rkisp1)
1391 {
1392 	struct rkisp1_capture *mp = &rkisp1->capture_devs[RKISP1_MAINPATH];
1393 	struct rkisp1_capture *sp = &rkisp1->capture_devs[RKISP1_SELFPATH];
1394 
1395 	rkisp1_unregister_capture(mp);
1396 	rkisp1_unregister_capture(sp);
1397 }
1398 
1399 static int rkisp1_register_capture(struct rkisp1_capture *cap)
1400 {
1401 	static const char * const dev_names[] = {
1402 		RKISP1_MP_DEV_NAME, RKISP1_SP_DEV_NAME
1403 	};
1404 	struct v4l2_device *v4l2_dev = &cap->rkisp1->v4l2_dev;
1405 	struct video_device *vdev = &cap->vnode.vdev;
1406 	struct rkisp1_vdev_node *node;
1407 	struct vb2_queue *q;
1408 	int ret;
1409 
1410 	strscpy(vdev->name, dev_names[cap->id], sizeof(vdev->name));
1411 	node = rkisp1_vdev_to_node(vdev);
1412 	mutex_init(&node->vlock);
1413 
1414 	vdev->ioctl_ops = &rkisp1_v4l2_ioctl_ops;
1415 	vdev->release = video_device_release_empty;
1416 	vdev->fops = &rkisp1_fops;
1417 	vdev->minor = -1;
1418 	vdev->v4l2_dev = v4l2_dev;
1419 	vdev->lock = &node->vlock;
1420 	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE |
1421 			    V4L2_CAP_STREAMING | V4L2_CAP_IO_MC;
1422 	vdev->entity.ops = &rkisp1_media_ops;
1423 	video_set_drvdata(vdev, cap);
1424 	vdev->vfl_dir = VFL_DIR_RX;
1425 	node->pad.flags = MEDIA_PAD_FL_SINK;
1426 
1427 	q = &node->buf_queue;
1428 	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
1429 	q->io_modes = VB2_MMAP | VB2_DMABUF;
1430 	q->drv_priv = cap;
1431 	q->ops = &rkisp1_vb2_ops;
1432 	q->mem_ops = &vb2_dma_contig_memops;
1433 	q->buf_struct_size = sizeof(struct rkisp1_buffer);
1434 	q->min_buffers_needed = RKISP1_MIN_BUFFERS_NEEDED;
1435 	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1436 	q->lock = &node->vlock;
1437 	q->dev = cap->rkisp1->dev;
1438 	ret = vb2_queue_init(q);
1439 	if (ret) {
1440 		dev_err(cap->rkisp1->dev,
1441 			"vb2 queue init failed (err=%d)\n", ret);
1442 		goto error;
1443 	}
1444 
1445 	vdev->queue = q;
1446 
1447 	ret = media_entity_pads_init(&vdev->entity, 1, &node->pad);
1448 	if (ret)
1449 		goto error;
1450 
1451 	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
1452 	if (ret) {
1453 		dev_err(cap->rkisp1->dev,
1454 			"failed to register %s, ret=%d\n", vdev->name, ret);
1455 		goto error;
1456 	}
1457 
1458 	v4l2_info(v4l2_dev, "registered %s as /dev/video%d\n", vdev->name,
1459 		  vdev->num);
1460 
1461 	return 0;
1462 
1463 error:
1464 	media_entity_cleanup(&vdev->entity);
1465 	mutex_destroy(&node->vlock);
1466 	return ret;
1467 }
1468 
1469 static void
1470 rkisp1_capture_init(struct rkisp1_device *rkisp1, enum rkisp1_stream_id id)
1471 {
1472 	struct rkisp1_capture *cap = &rkisp1->capture_devs[id];
1473 	struct v4l2_pix_format_mplane pixm;
1474 
1475 	memset(cap, 0, sizeof(*cap));
1476 	cap->id = id;
1477 	cap->rkisp1 = rkisp1;
1478 
1479 	INIT_LIST_HEAD(&cap->buf.queue);
1480 	init_waitqueue_head(&cap->done);
1481 	spin_lock_init(&cap->buf.lock);
1482 	if (cap->id == RKISP1_SELFPATH) {
1483 		cap->ops = &rkisp1_capture_ops_sp;
1484 		cap->config = &rkisp1_capture_config_sp;
1485 	} else {
1486 		cap->ops = &rkisp1_capture_ops_mp;
1487 		cap->config = &rkisp1_capture_config_mp;
1488 	}
1489 
1490 	cap->is_streaming = false;
1491 
1492 	memset(&pixm, 0, sizeof(pixm));
1493 	pixm.pixelformat = V4L2_PIX_FMT_YUYV;
1494 	pixm.width = RKISP1_DEFAULT_WIDTH;
1495 	pixm.height = RKISP1_DEFAULT_HEIGHT;
1496 	rkisp1_set_fmt(cap, &pixm);
1497 }
1498 
1499 int rkisp1_capture_devs_register(struct rkisp1_device *rkisp1)
1500 {
1501 	unsigned int i;
1502 	int ret;
1503 
1504 	for (i = 0; i < ARRAY_SIZE(rkisp1->capture_devs); i++) {
1505 		struct rkisp1_capture *cap = &rkisp1->capture_devs[i];
1506 
1507 		rkisp1_capture_init(rkisp1, i);
1508 
1509 		ret = rkisp1_register_capture(cap);
1510 		if (ret) {
1511 			rkisp1_capture_devs_unregister(rkisp1);
1512 			return ret;
1513 		}
1514 	}
1515 
1516 	return 0;
1517 
1518 }
1519