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