forked from ~ljy/RK356X_SDK_RELEASE
blame | history | patch | commit | commitdiff
hc
2024-01-03 2f7c68cb55ecb7331f2381deb497c27155f32faf 
 kernel/drivers/media/platform/am437x/am437x-vpfe.c
....@@ -1,3 +1,4 @@
1
+// SPDX-License-Identifier: GPL-2.0-only
12 /*
23  * TI VPFE capture Driver
34  *
....@@ -5,19 +6,6 @@
56  *
67  * Benoit Parrot <bparrot@ti.com>
78  * Lad, Prabhakar <prabhakar.csengg@gmail.com>
8
- *
9
- * This program is free software; you may redistribute it and/or modify
10
- * it under the terms of the GNU General Public License as published by
11
- * the Free Software Foundation; version 2 of the License.
12
- *
13
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
14
- * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
15
- * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
16
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
17
- * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
18
- * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
19
- * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
20
- * SOFTWARE.
219  */
2210 
2311 #include <linux/delay.h>
....@@ -38,6 +26,7 @@
3826 #include <media/v4l2-ctrls.h>
3927 #include <media/v4l2-event.h>
4028 #include <media/v4l2-fwnode.h>
29
+#include <media/v4l2-rect.h>
4130 
4231 #include "am437x-vpfe.h"
4332 
....@@ -69,137 +58,64 @@
6958 	{V4L2_STD_625_50, 720, 576, {54, 59}, 1},
7059 };
7160 
72
-struct bus_format {
73
-	unsigned int width;
74
-	unsigned int bpp;
75
-};
76
-
77
-/*
78
- * struct vpfe_fmt - VPFE media bus format information
79
- * @name: V4L2 format description
80
- * @code: V4L2 media bus format code
81
- * @shifted: V4L2 media bus format code for the same pixel layout but
82
- *	shifted to be 8 bits per pixel. =0 if format is not shiftable.
83
- * @pixelformat: V4L2 pixel format FCC identifier
84
- * @width: Bits per pixel (when transferred over a bus)
85
- * @bpp: Bytes per pixel (when stored in memory)
86
- * @supported: Indicates format supported by subdev
87
- */
88
-struct vpfe_fmt {
89
-	const char *name;
90
-	u32 fourcc;
91
-	u32 code;
92
-	struct bus_format l;
93
-	struct bus_format s;
94
-	bool supported;
95
-	u32 index;
96
-};
97
-
98
-static struct vpfe_fmt formats[] = {
61
+static struct vpfe_fmt formats[VPFE_NUM_FORMATS] = {
9962 	{
100
-		.name		= "YUV 4:2:2 packed, YCbYCr",
10163 		.fourcc		= V4L2_PIX_FMT_YUYV,
10264 		.code		= MEDIA_BUS_FMT_YUYV8_2X8,
103
-		.l.width	= 10,
104
-		.l.bpp		= 4,
105
-		.s.width	= 8,
106
-		.s.bpp		= 2,
107
-		.supported	= false,
65
+		.bitsperpixel	= 16,
10866 	}, {
109
-		.name		= "YUV 4:2:2 packed, CbYCrY",
11067 		.fourcc		= V4L2_PIX_FMT_UYVY,
11168 		.code		= MEDIA_BUS_FMT_UYVY8_2X8,
112
-		.l.width	= 10,
113
-		.l.bpp		= 4,
114
-		.s.width	= 8,
115
-		.s.bpp		= 2,
116
-		.supported	= false,
69
+		.bitsperpixel	= 16,
11770 	}, {
118
-		.name		= "YUV 4:2:2 packed, YCrYCb",
11971 		.fourcc		= V4L2_PIX_FMT_YVYU,
12072 		.code		= MEDIA_BUS_FMT_YVYU8_2X8,
121
-		.l.width	= 10,
122
-		.l.bpp		= 4,
123
-		.s.width	= 8,
124
-		.s.bpp		= 2,
125
-		.supported	= false,
73
+		.bitsperpixel	= 16,
12674 	}, {
127
-		.name		= "YUV 4:2:2 packed, CrYCbY",
12875 		.fourcc		= V4L2_PIX_FMT_VYUY,
12976 		.code		= MEDIA_BUS_FMT_VYUY8_2X8,
130
-		.l.width	= 10,
131
-		.l.bpp		= 4,
132
-		.s.width	= 8,
133
-		.s.bpp		= 2,
134
-		.supported	= false,
77
+		.bitsperpixel	= 16,
13578 	}, {
136
-		.name		= "RAW8 BGGR",
13779 		.fourcc		= V4L2_PIX_FMT_SBGGR8,
13880 		.code		= MEDIA_BUS_FMT_SBGGR8_1X8,
139
-		.l.width	= 10,
140
-		.l.bpp		= 2,
141
-		.s.width	= 8,
142
-		.s.bpp		= 1,
143
-		.supported	= false,
81
+		.bitsperpixel	= 8,
14482 	}, {
145
-		.name		= "RAW8 GBRG",
14683 		.fourcc		= V4L2_PIX_FMT_SGBRG8,
14784 		.code		= MEDIA_BUS_FMT_SGBRG8_1X8,
148
-		.l.width	= 10,
149
-		.l.bpp		= 2,
150
-		.s.width	= 8,
151
-		.s.bpp		= 1,
152
-		.supported	= false,
85
+		.bitsperpixel	= 8,
15386 	}, {
154
-		.name		= "RAW8 GRBG",
15587 		.fourcc		= V4L2_PIX_FMT_SGRBG8,
15688 		.code		= MEDIA_BUS_FMT_SGRBG8_1X8,
157
-		.l.width	= 10,
158
-		.l.bpp		= 2,
159
-		.s.width	= 8,
160
-		.s.bpp		= 1,
161
-		.supported	= false,
89
+		.bitsperpixel	= 8,
16290 	}, {
163
-		.name		= "RAW8 RGGB",
16491 		.fourcc		= V4L2_PIX_FMT_SRGGB8,
16592 		.code		= MEDIA_BUS_FMT_SRGGB8_1X8,
166
-		.l.width	= 10,
167
-		.l.bpp		= 2,
168
-		.s.width	= 8,
169
-		.s.bpp		= 1,
170
-		.supported	= false,
93
+		.bitsperpixel	= 8,
17194 	}, {
172
-		.name		= "RGB565 (LE)",
17395 		.fourcc		= V4L2_PIX_FMT_RGB565,
17496 		.code		= MEDIA_BUS_FMT_RGB565_2X8_LE,
175
-		.l.width	= 10,
176
-		.l.bpp		= 4,
177
-		.s.width	= 8,
178
-		.s.bpp		= 2,
179
-		.supported	= false,
97
+		.bitsperpixel	= 16,
18098 	}, {
181
-		.name		= "RGB565 (BE)",
18299 		.fourcc		= V4L2_PIX_FMT_RGB565X,
183100 		.code		= MEDIA_BUS_FMT_RGB565_2X8_BE,
184
-		.l.width	= 10,
185
-		.l.bpp		= 4,
186
-		.s.width	= 8,
187
-		.s.bpp		= 2,
188
-		.supported	= false,
101
+		.bitsperpixel	= 16,
189102 	},
190103 };
191104 
192
-static int
193
-__vpfe_get_format(struct vpfe_device *vpfe,
194
-		  struct v4l2_format *format, unsigned int *bpp);
105
+static int __subdev_get_format(struct vpfe_device *vpfe,
106
+			       struct v4l2_mbus_framefmt *fmt);
107
+static int vpfe_calc_format_size(struct vpfe_device *vpfe,
108
+				 const struct vpfe_fmt *fmt,
109
+				 struct v4l2_format *f);
195110 
196
-static struct vpfe_fmt *find_format_by_code(unsigned int code)
111
+static struct vpfe_fmt *find_format_by_code(struct vpfe_device *vpfe,
112
+					    unsigned int code)
197113 {
198114 	struct vpfe_fmt *fmt;
199115 	unsigned int k;
200116 
201
-	for (k = 0; k < ARRAY_SIZE(formats); k++) {
202
-		fmt = &formats[k];
117
+	for (k = 0; k < vpfe->num_active_fmt; k++) {
118
+		fmt = vpfe->active_fmt[k];
203119 		if (fmt->code == code)
204120 			return fmt;
205121 	}
....@@ -207,13 +123,14 @@
207123 	return NULL;
208124 }
209125 
210
-static struct vpfe_fmt *find_format_by_pix(unsigned int pixelformat)
126
+static struct vpfe_fmt *find_format_by_pix(struct vpfe_device *vpfe,
127
+					   unsigned int pixelformat)
211128 {
212129 	struct vpfe_fmt *fmt;
213130 	unsigned int k;
214131 
215
-	for (k = 0; k < ARRAY_SIZE(formats); k++) {
216
-		fmt = &formats[k];
132
+	for (k = 0; k < vpfe->num_active_fmt; k++) {
133
+		fmt = vpfe->active_fmt[k];
217134 		if (fmt->fourcc == pixelformat)
218135 			return fmt;
219136 	}
....@@ -221,48 +138,18 @@
221138 	return NULL;
222139 }
223140 
224
-static void
225
-mbus_to_pix(struct vpfe_device *vpfe,
226
-	    const struct v4l2_mbus_framefmt *mbus,
227
-	    struct v4l2_pix_format *pix, unsigned int *bpp)
141
+static unsigned int __get_bytesperpixel(struct vpfe_device *vpfe,
142
+					const struct vpfe_fmt *fmt)
228143 {
229144 	struct vpfe_subdev_info *sdinfo = vpfe->current_subdev;
230145 	unsigned int bus_width = sdinfo->vpfe_param.bus_width;
231
-	struct vpfe_fmt *fmt;
146
+	u32 bpp, bus_width_bytes, clocksperpixel;
232147 
233
-	fmt = find_format_by_code(mbus->code);
234
-	if (WARN_ON(fmt == NULL)) {
235
-		pr_err("Invalid mbus code set\n");
236
-		*bpp = 1;
237
-		return;
238
-	}
148
+	bus_width_bytes = ALIGN(bus_width, 8) >> 3;
149
+	clocksperpixel = DIV_ROUND_UP(fmt->bitsperpixel, bus_width);
150
+	bpp = clocksperpixel * bus_width_bytes;
239151 
240
-	memset(pix, 0, sizeof(*pix));
241
-	v4l2_fill_pix_format(pix, mbus);
242
-	pix->pixelformat = fmt->fourcc;
243
-	*bpp = (bus_width == 10) ?  fmt->l.bpp : fmt->s.bpp;
244
-
245
-	/* pitch should be 32 bytes aligned */
246
-	pix->bytesperline = ALIGN(pix->width * *bpp, 32);
247
-	pix->sizeimage = pix->bytesperline * pix->height;
248
-}
249
-
250
-static void pix_to_mbus(struct vpfe_device *vpfe,
251
-			struct v4l2_pix_format *pix_fmt,
252
-			struct v4l2_mbus_framefmt *mbus_fmt)
253
-{
254
-	struct vpfe_fmt *fmt;
255
-
256
-	fmt = find_format_by_pix(pix_fmt->pixelformat);
257
-	if (!fmt) {
258
-		/* default to first entry */
259
-		vpfe_dbg(3, vpfe, "Invalid pixel code: %x, default used instead\n",
260
-			pix_fmt->pixelformat);
261
-		fmt = &formats[0];
262
-	}
263
-
264
-	memset(mbus_fmt, 0, sizeof(*mbus_fmt));
265
-	v4l2_fill_mbus_format(mbus_fmt, pix_fmt, fmt->code);
152
+	return bpp;
266153 }
267154 
268155 /*  Print Four-character-code (FOURCC) */
....@@ -277,20 +164,6 @@
277164 	code[4] = '\0';
278165 
279166 	return code;
280
-}
281
-
282
-static int
283
-cmp_v4l2_format(const struct v4l2_format *lhs, const struct v4l2_format *rhs)
284
-{
285
-	return lhs->type == rhs->type &&
286
-		lhs->fmt.pix.width == rhs->fmt.pix.width &&
287
-		lhs->fmt.pix.height == rhs->fmt.pix.height &&
288
-		lhs->fmt.pix.pixelformat == rhs->fmt.pix.pixelformat &&
289
-		lhs->fmt.pix.field == rhs->fmt.pix.field &&
290
-		lhs->fmt.pix.colorspace == rhs->fmt.pix.colorspace &&
291
-		lhs->fmt.pix.ycbcr_enc == rhs->fmt.pix.ycbcr_enc &&
292
-		lhs->fmt.pix.quantization == rhs->fmt.pix.quantization &&
293
-		lhs->fmt.pix.xfer_func == rhs->fmt.pix.xfer_func;
294167 }
295168 
296169 static inline u32 vpfe_reg_read(struct vpfe_ccdc *ccdc, u32 offset)
....@@ -357,13 +230,9 @@
357230 	if (frm_fmt == CCDC_FRMFMT_INTERLACED) {
358231 		vert_nr_lines = (image_win->height >> 1) - 1;
359232 		vert_start >>= 1;
360
-		/* Since first line doesn't have any data */
361
-		vert_start += 1;
362233 		/* configure VDINT0 */
363234 		val = (vert_start << VPFE_VDINT_VDINT0_SHIFT);
364235 	} else {
365
-		/* Since first line doesn't have any data */
366
-		vert_start += 1;
367236 		vert_nr_lines = image_win->height - 1;
368237 		/*
369238 		 * configure VDINT0 and VDINT1. VDINT1 will be at half
....@@ -417,7 +286,7 @@
417286 	max_data = ccdc_data_size_max_bit(ccdcparam->data_sz);
418287 
419288 	if (ccdcparam->alaw.gamma_wd > VPFE_CCDC_GAMMA_BITS_09_0 ||
420
-	    ccdcparam->alaw.gamma_wd < VPFE_CCDC_GAMMA_BITS_15_6 ||
289
+	    ccdcparam->data_sz > VPFE_CCDC_DATA_8BITS ||
421290 	    max_gamma > max_data) {
422291 		vpfe_dbg(1, vpfe, "Invalid data line select\n");
423292 		return -EINVAL;
....@@ -457,46 +326,31 @@
457326 
458327 static int vpfe_ccdc_close(struct vpfe_ccdc *ccdc, struct device *dev)
459328 {
460
-	int dma_cntl, i, pcr;
329
+	struct vpfe_device *vpfe = to_vpfe(ccdc);
330
+	u32 dma_cntl, pcr;
461331 
462
-	/* If the CCDC module is still busy wait for it to be done */
463
-	for (i = 0; i < 10; i++) {
464
-		usleep_range(5000, 6000);
465
-		pcr = vpfe_reg_read(ccdc, VPFE_PCR);
466
-		if (!pcr)
467
-			break;
332
+	pcr = vpfe_reg_read(ccdc, VPFE_PCR);
333
+	if (pcr)
334
+		vpfe_dbg(1, vpfe, "VPFE_PCR is still set (%x)", pcr);
468335 
469
-		/* make sure it it is disabled */
470
-		vpfe_pcr_enable(ccdc, 0);
471
-	}
336
+	dma_cntl = vpfe_reg_read(ccdc, VPFE_DMA_CNTL);
337
+	if ((dma_cntl & VPFE_DMA_CNTL_OVERFLOW))
338
+		vpfe_dbg(1, vpfe, "VPFE_DMA_CNTL_OVERFLOW is still set (%x)",
339
+			 dma_cntl);
472340 
473341 	/* Disable CCDC by resetting all register to default POR values */
474342 	vpfe_ccdc_restore_defaults(ccdc);
475
-
476
-	/* if DMA_CNTL overflow bit is set. Clear it
477
-	 *  It appears to take a while for this to become quiescent ~20ms
478
-	 */
479
-	for (i = 0; i < 10; i++) {
480
-		dma_cntl = vpfe_reg_read(ccdc, VPFE_DMA_CNTL);
481
-		if (!(dma_cntl & VPFE_DMA_CNTL_OVERFLOW))
482
-			break;
483
-
484
-		/* Clear the overflow bit */
485
-		vpfe_reg_write(ccdc, dma_cntl, VPFE_DMA_CNTL);
486
-		usleep_range(5000, 6000);
487
-	}
488343 
489344 	/* Disabled the module at the CONFIG level */
490345 	vpfe_config_enable(ccdc, 0);
491346 
492347 	pm_runtime_put_sync(dev);
493
-
494348 	return 0;
495349 }
496350 
497351 static int vpfe_ccdc_set_params(struct vpfe_ccdc *ccdc, void __user *params)
498352 {
499
-	struct vpfe_device *vpfe = container_of(ccdc, struct vpfe_device, ccdc);
353
+	struct vpfe_device *vpfe = to_vpfe(ccdc);
500354 	struct vpfe_ccdc_config_params_raw raw_params;
501355 	int x;
502356 
....@@ -506,8 +360,8 @@
506360 	x = copy_from_user(&raw_params, params, sizeof(raw_params));
507361 	if (x) {
508362 		vpfe_dbg(1, vpfe,
509
-			"vpfe_ccdc_set_params: error in copying ccdc params, %d\n",
510
-			x);
363
+			 "%s: error in copying ccdc params, %d\n",
364
+			 __func__, x);
511365 		return -EFAULT;
512366 	}
513367 
....@@ -525,11 +379,9 @@
525379  */
526380 static void vpfe_ccdc_config_ycbcr(struct vpfe_ccdc *ccdc)
527381 {
528
-	struct vpfe_device *vpfe = container_of(ccdc, struct vpfe_device, ccdc);
529382 	struct ccdc_params_ycbcr *params = &ccdc->ccdc_cfg.ycbcr;
530383 	u32 syn_mode;
531384 
532
-	vpfe_dbg(3, vpfe, "vpfe_ccdc_config_ycbcr:\n");
533385 	/*
534386 	 * first restore the CCDC registers to default values
535387 	 * This is important since we assume default values to be set in
....@@ -654,14 +506,12 @@
654506  */
655507 static void vpfe_ccdc_config_raw(struct vpfe_ccdc *ccdc)
656508 {
657
-	struct vpfe_device *vpfe = container_of(ccdc, struct vpfe_device, ccdc);
509
+	struct vpfe_device *vpfe = to_vpfe(ccdc);
658510 	struct vpfe_ccdc_config_params_raw *config_params =
659511 				&ccdc->ccdc_cfg.bayer.config_params;
660512 	struct ccdc_params_raw *params = &ccdc->ccdc_cfg.bayer;
661513 	unsigned int syn_mode;
662514 	unsigned int val;
663
-
664
-	vpfe_dbg(3, vpfe, "vpfe_ccdc_config_raw:\n");
665515 
666516 	/* Reset CCDC */
667517 	vpfe_ccdc_restore_defaults(ccdc);
....@@ -761,10 +611,10 @@
761611 
762612 static int vpfe_ccdc_set_pixel_format(struct vpfe_ccdc *ccdc, u32 pixfmt)
763613 {
764
-	struct vpfe_device *vpfe = container_of(ccdc, struct vpfe_device, ccdc);
614
+	struct vpfe_device *vpfe = to_vpfe(ccdc);
765615 
766
-	vpfe_dbg(1, vpfe, "vpfe_ccdc_set_pixel_format: if_type: %d, pixfmt:%s\n",
767
-		 ccdc->ccdc_cfg.if_type, print_fourcc(pixfmt));
616
+	vpfe_dbg(1, vpfe, "%s: if_type: %d, pixfmt:%s\n",
617
+		 __func__, ccdc->ccdc_cfg.if_type, print_fourcc(pixfmt));
768618 
769619 	if (ccdc->ccdc_cfg.if_type == VPFE_RAW_BAYER) {
770620 		ccdc->ccdc_cfg.bayer.pix_fmt = CCDC_PIXFMT_RAW;
....@@ -893,7 +743,7 @@
893743 static int vpfe_ccdc_set_hw_if_params(struct vpfe_ccdc *ccdc,
894744 				      struct vpfe_hw_if_param *params)
895745 {
896
-	struct vpfe_device *vpfe = container_of(ccdc, struct vpfe_device, ccdc);
746
+	struct vpfe_device *vpfe = to_vpfe(ccdc);
897747 
898748 	ccdc->ccdc_cfg.if_type = params->if_type;
899749 
....@@ -1048,9 +898,8 @@
1048898 static int vpfe_config_ccdc_image_format(struct vpfe_device *vpfe)
1049899 {
1050900 	enum ccdc_frmfmt frm_fmt = CCDC_FRMFMT_INTERLACED;
901
+	u32 bpp;
1051902 	int ret = 0;
1052
-
1053
-	vpfe_dbg(2, vpfe, "vpfe_config_ccdc_image_format\n");
1054903 
1055904 	vpfe_dbg(1, vpfe, "pixelformat: %s\n",
1056905 		print_fourcc(vpfe->fmt.fmt.pix.pixelformat));
....@@ -1062,7 +911,8 @@
1062911 	}
1063912 
1064913 	/* configure the image window */
1065
-	vpfe_ccdc_set_image_window(&vpfe->ccdc, &vpfe->crop, vpfe->bpp);
914
+	bpp = __get_bytesperpixel(vpfe, vpfe->current_vpfe_fmt);
915
+	vpfe_ccdc_set_image_window(&vpfe->ccdc, &vpfe->crop, bpp);
1066916 
1067917 	switch (vpfe->fmt.fmt.pix.field) {
1068918 	case V4L2_FIELD_INTERLACED:
....@@ -1106,7 +956,8 @@
1106956 static int vpfe_config_image_format(struct vpfe_device *vpfe,
1107957 				    v4l2_std_id std_id)
1108958 {
1109
-	struct v4l2_pix_format *pix = &vpfe->fmt.fmt.pix;
959
+	struct vpfe_fmt *fmt;
960
+	struct v4l2_mbus_framefmt mbus_fmt;
1110961 	int i, ret;
1111962 
1112963 	for (i = 0; i < ARRAY_SIZE(vpfe_standards); i++) {
....@@ -1128,26 +979,29 @@
1128979 		return -EINVAL;
1129980 	}
1130981 
1131
-	vpfe->crop.top = vpfe->crop.left = 0;
1132
-	vpfe->crop.width = vpfe->std_info.active_pixels;
1133
-	vpfe->crop.height = vpfe->std_info.active_lines;
1134
-	pix->width = vpfe->crop.width;
1135
-	pix->height = vpfe->crop.height;
1136
-	pix->pixelformat = V4L2_PIX_FMT_YUYV;
1137
-
1138
-	/* first field and frame format based on standard frame format */
1139
-	if (vpfe->std_info.frame_format)
1140
-		pix->field = V4L2_FIELD_INTERLACED;
1141
-	else
1142
-		pix->field = V4L2_FIELD_NONE;
1143
-
1144
-	ret = __vpfe_get_format(vpfe, &vpfe->fmt, &vpfe->bpp);
982
+	ret = __subdev_get_format(vpfe, &mbus_fmt);
1145983 	if (ret)
1146984 		return ret;
1147985 
986
+	fmt = find_format_by_code(vpfe, mbus_fmt.code);
987
+	if (!fmt) {
988
+		vpfe_dbg(3, vpfe, "mbus code format (0x%08x) not found.\n",
989
+			 mbus_fmt.code);
990
+		return -EINVAL;
991
+	}
992
+
993
+	/* Save current subdev format */
994
+	v4l2_fill_pix_format(&vpfe->fmt.fmt.pix, &mbus_fmt);
995
+	vpfe->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
996
+	vpfe->fmt.fmt.pix.pixelformat = fmt->fourcc;
997
+	vpfe_calc_format_size(vpfe, fmt, &vpfe->fmt);
998
+	vpfe->current_vpfe_fmt = fmt;
999
+
11481000 	/* Update the crop window based on found values */
1149
-	vpfe->crop.width = pix->width;
1150
-	vpfe->crop.height = pix->height;
1001
+	vpfe->crop.top = 0;
1002
+	vpfe->crop.left = 0;
1003
+	vpfe->crop.width = mbus_fmt.width;
1004
+	vpfe->crop.height = mbus_fmt.height;
11511005 
11521006 	return vpfe_config_ccdc_image_format(vpfe);
11531007 }
....@@ -1167,7 +1021,9 @@
11671021 	if (ret)
11681022 		return ret;
11691023 
1170
-	pm_runtime_get_sync(vpfe->pdev);
1024
+	ret = pm_runtime_resume_and_get(vpfe->pdev);
1025
+	if (ret < 0)
1026
+		return ret;
11711027 
11721028 	vpfe_config_enable(&vpfe->ccdc, 1);
11731029 
....@@ -1249,22 +1105,29 @@
12491105  * This function will get next buffer from the dma queue and
12501106  * set the buffer address in the vpfe register for capture.
12511107  * the buffer is marked active
1252
- *
1253
- * Assumes caller is holding vpfe->dma_queue_lock already
12541108  */
1255
-static inline void vpfe_schedule_next_buffer(struct vpfe_device *vpfe)
1109
+static void vpfe_schedule_next_buffer(struct vpfe_device *vpfe)
12561110 {
1111
+	dma_addr_t addr;
1112
+
1113
+	spin_lock(&vpfe->dma_queue_lock);
1114
+	if (list_empty(&vpfe->dma_queue)) {
1115
+		spin_unlock(&vpfe->dma_queue_lock);
1116
+		return;
1117
+	}
1118
+
12571119 	vpfe->next_frm = list_entry(vpfe->dma_queue.next,
12581120 				    struct vpfe_cap_buffer, list);
12591121 	list_del(&vpfe->next_frm->list);
1122
+	spin_unlock(&vpfe->dma_queue_lock);
12601123 
1261
-	vpfe_set_sdr_addr(&vpfe->ccdc,
1262
-	       vb2_dma_contig_plane_dma_addr(&vpfe->next_frm->vb.vb2_buf, 0));
1124
+	addr = vb2_dma_contig_plane_dma_addr(&vpfe->next_frm->vb.vb2_buf, 0);
1125
+	vpfe_set_sdr_addr(&vpfe->ccdc, addr);
12631126 }
12641127 
12651128 static inline void vpfe_schedule_bottom_field(struct vpfe_device *vpfe)
12661129 {
1267
-	unsigned long addr;
1130
+	dma_addr_t addr;
12681131 
12691132 	addr = vb2_dma_contig_plane_dma_addr(&vpfe->next_frm->vb.vb2_buf, 0) +
12701133 					vpfe->field_off;
....@@ -1289,6 +1152,58 @@
12891152 	vpfe->cur_frm = vpfe->next_frm;
12901153 }
12911154 
1155
+static void vpfe_handle_interlaced_irq(struct vpfe_device *vpfe,
1156
+				       enum v4l2_field field)
1157
+{
1158
+	int fid;
1159
+
1160
+	/* interlaced or TB capture check which field
1161
+	 * we are in hardware
1162
+	 */
1163
+	fid = vpfe_ccdc_getfid(&vpfe->ccdc);
1164
+
1165
+	/* switch the software maintained field id */
1166
+	vpfe->field ^= 1;
1167
+	if (fid == vpfe->field) {
1168
+		/* we are in-sync here,continue */
1169
+		if (fid == 0) {
1170
+			/*
1171
+			 * One frame is just being captured. If the
1172
+			 * next frame is available, release the
1173
+			 * current frame and move on
1174
+			 */
1175
+			if (vpfe->cur_frm != vpfe->next_frm)
1176
+				vpfe_process_buffer_complete(vpfe);
1177
+
1178
+			if (vpfe->stopping)
1179
+				return;
1180
+
1181
+			/*
1182
+			 * based on whether the two fields are stored
1183
+			 * interleave or separately in memory,
1184
+			 * reconfigure the CCDC memory address
1185
+			 */
1186
+			if (field == V4L2_FIELD_SEQ_TB)
1187
+				vpfe_schedule_bottom_field(vpfe);
1188
+		} else {
1189
+			/*
1190
+			 * if one field is just being captured configure
1191
+			 * the next frame get the next frame from the empty
1192
+			 * queue if no frame is available hold on to the
1193
+			 * current buffer
1194
+			 */
1195
+			if (vpfe->cur_frm == vpfe->next_frm)
1196
+				vpfe_schedule_next_buffer(vpfe);
1197
+		}
1198
+	} else if (fid == 0) {
1199
+		/*
1200
+		 * out of sync. Recover from any hardware out-of-sync.
1201
+		 * May loose one frame
1202
+		 */
1203
+		vpfe->field = fid;
1204
+	}
1205
+}
1206
+
12921207 /*
12931208  * vpfe_isr : ISR handler for vpfe capture (VINT0)
12941209  * @irq: irq number
....@@ -1300,76 +1215,28 @@
13001215 static irqreturn_t vpfe_isr(int irq, void *dev)
13011216 {
13021217 	struct vpfe_device *vpfe = (struct vpfe_device *)dev;
1303
-	enum v4l2_field field;
1304
-	int intr_status;
1305
-	int fid;
1218
+	enum v4l2_field field = vpfe->fmt.fmt.pix.field;
1219
+	int intr_status, stopping = vpfe->stopping;
13061220 
13071221 	intr_status = vpfe_reg_read(&vpfe->ccdc, VPFE_IRQ_STS);
13081222 
13091223 	if (intr_status & VPFE_VDINT0) {
1310
-		field = vpfe->fmt.fmt.pix.field;
1311
-
13121224 		if (field == V4L2_FIELD_NONE) {
1313
-			/* handle progressive frame capture */
13141225 			if (vpfe->cur_frm != vpfe->next_frm)
13151226 				vpfe_process_buffer_complete(vpfe);
1316
-			goto next_intr;
1227
+		} else {
1228
+			vpfe_handle_interlaced_irq(vpfe, field);
13171229 		}
1318
-
1319
-		/* interlaced or TB capture check which field
1320
-		   we are in hardware */
1321
-		fid = vpfe_ccdc_getfid(&vpfe->ccdc);
1322
-
1323
-		/* switch the software maintained field id */
1324
-		vpfe->field ^= 1;
1325
-		if (fid == vpfe->field) {
1326
-			/* we are in-sync here,continue */
1327
-			if (fid == 0) {
1328
-				/*
1329
-				 * One frame is just being captured. If the
1330
-				 * next frame is available, release the
1331
-				 * current frame and move on
1332
-				 */
1333
-				if (vpfe->cur_frm != vpfe->next_frm)
1334
-					vpfe_process_buffer_complete(vpfe);
1335
-				/*
1336
-				 * based on whether the two fields are stored
1337
-				 * interleave or separately in memory,
1338
-				 * reconfigure the CCDC memory address
1339
-				 */
1340
-				if (field == V4L2_FIELD_SEQ_TB)
1341
-					vpfe_schedule_bottom_field(vpfe);
1342
-
1343
-				goto next_intr;
1344
-			}
1345
-			/*
1346
-			 * if one field is just being captured configure
1347
-			 * the next frame get the next frame from the empty
1348
-			 * queue if no frame is available hold on to the
1349
-			 * current buffer
1350
-			 */
1351
-			spin_lock(&vpfe->dma_queue_lock);
1352
-			if (!list_empty(&vpfe->dma_queue) &&
1353
-			    vpfe->cur_frm == vpfe->next_frm)
1354
-				vpfe_schedule_next_buffer(vpfe);
1355
-			spin_unlock(&vpfe->dma_queue_lock);
1356
-		} else if (fid == 0) {
1357
-			/*
1358
-			 * out of sync. Recover from any hardware out-of-sync.
1359
-			 * May loose one frame
1360
-			 */
1361
-			vpfe->field = fid;
1230
+		if (stopping) {
1231
+			vpfe->stopping = false;
1232
+			complete(&vpfe->capture_stop);
13621233 		}
13631234 	}
13641235 
1365
-next_intr:
1366
-	if (intr_status & VPFE_VDINT1) {
1367
-		spin_lock(&vpfe->dma_queue_lock);
1368
-		if (vpfe->fmt.fmt.pix.field == V4L2_FIELD_NONE &&
1369
-		    !list_empty(&vpfe->dma_queue) &&
1236
+	if (intr_status & VPFE_VDINT1 && !stopping) {
1237
+		if (field == V4L2_FIELD_NONE &&
13701238 		    vpfe->cur_frm == vpfe->next_frm)
13711239 			vpfe_schedule_next_buffer(vpfe);
1372
-		spin_unlock(&vpfe->dma_queue_lock);
13731240 	}
13741241 
13751242 	vpfe_clear_intr(&vpfe->ccdc, intr_status);
....@@ -1406,97 +1273,82 @@
14061273 {
14071274 	struct vpfe_device *vpfe = video_drvdata(file);
14081275 
1409
-	vpfe_dbg(2, vpfe, "vpfe_querycap\n");
1410
-
1411
-	strlcpy(cap->driver, VPFE_MODULE_NAME, sizeof(cap->driver));
1412
-	strlcpy(cap->card, "TI AM437x VPFE", sizeof(cap->card));
1276
+	strscpy(cap->driver, VPFE_MODULE_NAME, sizeof(cap->driver));
1277
+	strscpy(cap->card, "TI AM437x VPFE", sizeof(cap->card));
14131278 	snprintf(cap->bus_info, sizeof(cap->bus_info),
14141279 			"platform:%s", vpfe->v4l2_dev.name);
1415
-	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
1416
-			    V4L2_CAP_READWRITE;
1417
-	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
1418
-
14191280 	return 0;
14201281 }
14211282 
14221283 /* get the format set at output pad of the adjacent subdev */
1423
-static int __vpfe_get_format(struct vpfe_device *vpfe,
1424
-			     struct v4l2_format *format, unsigned int *bpp)
1284
+static int __subdev_get_format(struct vpfe_device *vpfe,
1285
+			       struct v4l2_mbus_framefmt *fmt)
14251286 {
1426
-	struct v4l2_mbus_framefmt mbus_fmt;
1427
-	struct vpfe_subdev_info *sdinfo;
1428
-	struct v4l2_subdev_format fmt;
1287
+	struct v4l2_subdev *sd = vpfe->current_subdev->sd;
1288
+	struct v4l2_subdev_format sd_fmt;
1289
+	struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format;
14291290 	int ret;
14301291 
1431
-	sdinfo = vpfe->current_subdev;
1432
-	if (!sdinfo->sd)
1433
-		return -EINVAL;
1292
+	sd_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
1293
+	sd_fmt.pad = 0;
14341294 
1435
-	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
1436
-	fmt.pad = 0;
1437
-
1438
-	ret = v4l2_subdev_call(sdinfo->sd, pad, get_fmt, NULL, &fmt);
1439
-	if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV)
1295
+	ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &sd_fmt);
1296
+	if (ret)
14401297 		return ret;
14411298 
1442
-	if (!ret) {
1443
-		v4l2_fill_pix_format(&format->fmt.pix, &fmt.format);
1444
-		mbus_to_pix(vpfe, &fmt.format, &format->fmt.pix, bpp);
1445
-	} else {
1446
-		ret = v4l2_device_call_until_err(&vpfe->v4l2_dev,
1447
-						 sdinfo->grp_id,
1448
-						 pad, get_fmt,
1449
-						 NULL, &fmt);
1450
-		if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV)
1451
-			return ret;
1452
-		v4l2_fill_pix_format(&format->fmt.pix, &mbus_fmt);
1453
-		mbus_to_pix(vpfe, &mbus_fmt, &format->fmt.pix, bpp);
1454
-	}
1299
+	*fmt = *mbus_fmt;
14551300 
1456
-	format->type = vpfe->fmt.type;
1457
-
1458
-	vpfe_dbg(1, vpfe,
1459
-		 "%s size %dx%d (%s) bytesperline = %d, size = %d, bpp = %d\n",
1460
-		 __func__, format->fmt.pix.width, format->fmt.pix.height,
1461
-		 print_fourcc(format->fmt.pix.pixelformat),
1462
-		 format->fmt.pix.bytesperline, format->fmt.pix.sizeimage, *bpp);
1301
+	vpfe_dbg(1, vpfe, "%s: %dx%d code:%04X\n", __func__,
1302
+		 fmt->width, fmt->height, fmt->code);
14631303 
14641304 	return 0;
14651305 }
14661306 
14671307 /* set the format at output pad of the adjacent subdev */
1468
-static int __vpfe_set_format(struct vpfe_device *vpfe,
1469
-			     struct v4l2_format *format, unsigned int *bpp)
1308
+static int __subdev_set_format(struct vpfe_device *vpfe,
1309
+			       struct v4l2_mbus_framefmt *fmt)
14701310 {
1471
-	struct vpfe_subdev_info *sdinfo;
1472
-	struct v4l2_subdev_format fmt;
1311
+	struct v4l2_subdev *sd = vpfe->current_subdev->sd;
1312
+	struct v4l2_subdev_format sd_fmt;
1313
+	struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format;
14731314 	int ret;
14741315 
1475
-	vpfe_dbg(2, vpfe, "__vpfe_set_format\n");
1316
+	sd_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
1317
+	sd_fmt.pad = 0;
1318
+	*mbus_fmt = *fmt;
14761319 
1477
-	sdinfo = vpfe->current_subdev;
1478
-	if (!sdinfo->sd)
1479
-		return -EINVAL;
1480
-
1481
-	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
1482
-	fmt.pad = 0;
1483
-
1484
-	pix_to_mbus(vpfe, &format->fmt.pix, &fmt.format);
1485
-
1486
-	ret = v4l2_subdev_call(sdinfo->sd, pad, set_fmt, NULL, &fmt);
1320
+	ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &sd_fmt);
14871321 	if (ret)
14881322 		return ret;
14891323 
1490
-	v4l2_fill_pix_format(&format->fmt.pix, &fmt.format);
1491
-	mbus_to_pix(vpfe, &fmt.format, &format->fmt.pix, bpp);
1324
+	vpfe_dbg(1, vpfe, "%s %dx%d code:%04X\n", __func__,
1325
+		 fmt->width, fmt->height, fmt->code);
14921326 
1493
-	format->type = vpfe->fmt.type;
1327
+	return 0;
1328
+}
14941329 
1495
-	vpfe_dbg(1, vpfe,
1496
-		 "%s size %dx%d (%s) bytesperline = %d, size = %d, bpp = %d\n",
1497
-		 __func__,  format->fmt.pix.width, format->fmt.pix.height,
1498
-		 print_fourcc(format->fmt.pix.pixelformat),
1499
-		 format->fmt.pix.bytesperline, format->fmt.pix.sizeimage, *bpp);
1330
+static int vpfe_calc_format_size(struct vpfe_device *vpfe,
1331
+				 const struct vpfe_fmt *fmt,
1332
+				 struct v4l2_format *f)
1333
+{
1334
+	u32 bpp;
1335
+
1336
+	if (!fmt) {
1337
+		vpfe_dbg(3, vpfe, "No vpfe_fmt provided!\n");
1338
+		return -EINVAL;
1339
+	}
1340
+
1341
+	bpp = __get_bytesperpixel(vpfe, fmt);
1342
+
1343
+	/* pitch should be 32 bytes aligned */
1344
+	f->fmt.pix.bytesperline = ALIGN(f->fmt.pix.width * bpp, 32);
1345
+	f->fmt.pix.sizeimage = f->fmt.pix.bytesperline *
1346
+			       f->fmt.pix.height;
1347
+
1348
+	vpfe_dbg(3, vpfe, "%s: fourcc: %s size: %dx%d bpl:%d img_size:%d\n",
1349
+		 __func__, print_fourcc(f->fmt.pix.pixelformat),
1350
+		 f->fmt.pix.width, f->fmt.pix.height,
1351
+		 f->fmt.pix.bytesperline, f->fmt.pix.sizeimage);
15001352 
15011353 	return 0;
15021354 }
....@@ -1505,8 +1357,6 @@
15051357 		      struct v4l2_format *fmt)
15061358 {
15071359 	struct vpfe_device *vpfe = video_drvdata(file);
1508
-
1509
-	vpfe_dbg(2, vpfe, "vpfe_g_fmt\n");
15101360 
15111361 	*fmt = vpfe->fmt;
15121362 
....@@ -1518,58 +1368,91 @@
15181368 {
15191369 	struct vpfe_device *vpfe = video_drvdata(file);
15201370 	struct vpfe_subdev_info *sdinfo;
1521
-	struct vpfe_fmt *fmt = NULL;
1522
-	unsigned int k;
1523
-
1524
-	vpfe_dbg(2, vpfe, "vpfe_enum_format index:%d\n",
1525
-		f->index);
1371
+	struct vpfe_fmt *fmt;
15261372 
15271373 	sdinfo = vpfe->current_subdev;
15281374 	if (!sdinfo->sd)
15291375 		return -EINVAL;
15301376 
1531
-	if (f->index > ARRAY_SIZE(formats))
1377
+	if (f->index >= vpfe->num_active_fmt)
15321378 		return -EINVAL;
15331379 
1534
-	for (k = 0; k < ARRAY_SIZE(formats); k++) {
1535
-		if (formats[k].index == f->index) {
1536
-			fmt = &formats[k];
1537
-			break;
1538
-		}
1539
-	}
1540
-	if (!fmt)
1541
-		return -EINVAL;
1380
+	fmt = vpfe->active_fmt[f->index];
15421381 
1543
-	strncpy(f->description, fmt->name, sizeof(f->description) - 1);
15441382 	f->pixelformat = fmt->fourcc;
1545
-	f->type = vpfe->fmt.type;
15461383 
1547
-	vpfe_dbg(1, vpfe, "vpfe_enum_format: mbus index: %d code: %x pixelformat: %s [%s]\n",
1548
-		f->index, fmt->code, print_fourcc(fmt->fourcc), fmt->name);
1384
+	vpfe_dbg(1, vpfe, "%s: mbus index: %d code: %x pixelformat: %s\n",
1385
+		 __func__, f->index, fmt->code, print_fourcc(fmt->fourcc));
15491386 
15501387 	return 0;
15511388 }
15521389 
15531390 static int vpfe_try_fmt(struct file *file, void *priv,
1554
-			struct v4l2_format *fmt)
1391
+			struct v4l2_format *f)
15551392 {
15561393 	struct vpfe_device *vpfe = video_drvdata(file);
1557
-	unsigned int bpp;
1394
+	struct v4l2_subdev *sd = vpfe->current_subdev->sd;
1395
+	const struct vpfe_fmt *fmt;
1396
+	struct v4l2_subdev_frame_size_enum fse;
1397
+	int ret, found;
15581398 
1559
-	vpfe_dbg(2, vpfe, "vpfe_try_fmt\n");
1399
+	fmt = find_format_by_pix(vpfe, f->fmt.pix.pixelformat);
1400
+	if (!fmt) {
1401
+		/* default to first entry */
1402
+		vpfe_dbg(3, vpfe, "Invalid pixel code: %x, default used instead\n",
1403
+			 f->fmt.pix.pixelformat);
1404
+		fmt = vpfe->active_fmt[0];
1405
+		f->fmt.pix.pixelformat = fmt->fourcc;
1406
+	}
15601407 
1561
-	return __vpfe_get_format(vpfe, fmt, &bpp);
1408
+	f->fmt.pix.field = vpfe->fmt.fmt.pix.field;
1409
+
1410
+	/* check for/find a valid width/height */
1411
+	ret = 0;
1412
+	found = false;
1413
+	fse.pad = 0;
1414
+	fse.code = fmt->code;
1415
+	fse.which = V4L2_SUBDEV_FORMAT_ACTIVE;
1416
+	for (fse.index = 0; ; fse.index++) {
1417
+		ret = v4l2_subdev_call(sd, pad, enum_frame_size,
1418
+				       NULL, &fse);
1419
+		if (ret)
1420
+			break;
1421
+
1422
+		if (f->fmt.pix.width == fse.max_width &&
1423
+		    f->fmt.pix.height == fse.max_height) {
1424
+			found = true;
1425
+			break;
1426
+		} else if (f->fmt.pix.width >= fse.min_width &&
1427
+			   f->fmt.pix.width <= fse.max_width &&
1428
+			   f->fmt.pix.height >= fse.min_height &&
1429
+			   f->fmt.pix.height <= fse.max_height) {
1430
+			found = true;
1431
+			break;
1432
+		}
1433
+	}
1434
+
1435
+	if (!found) {
1436
+		/* use existing values as default */
1437
+		f->fmt.pix.width = vpfe->fmt.fmt.pix.width;
1438
+		f->fmt.pix.height =  vpfe->fmt.fmt.pix.height;
1439
+	}
1440
+
1441
+	/*
1442
+	 * Use current colorspace for now, it will get
1443
+	 * updated properly during s_fmt
1444
+	 */
1445
+	f->fmt.pix.colorspace = vpfe->fmt.fmt.pix.colorspace;
1446
+	return vpfe_calc_format_size(vpfe, fmt, f);
15621447 }
15631448 
15641449 static int vpfe_s_fmt(struct file *file, void *priv,
15651450 		      struct v4l2_format *fmt)
15661451 {
15671452 	struct vpfe_device *vpfe = video_drvdata(file);
1568
-	struct v4l2_format format;
1569
-	unsigned int bpp;
1453
+	struct vpfe_fmt *f;
1454
+	struct v4l2_mbus_framefmt mbus_fmt;
15701455 	int ret;
1571
-
1572
-	vpfe_dbg(2, vpfe, "vpfe_s_fmt\n");
15731456 
15741457 	/* If streaming is started, return error */
15751458 	if (vb2_is_busy(&vpfe->buffer_queue)) {
....@@ -1577,25 +1460,32 @@
15771460 		return -EBUSY;
15781461 	}
15791462 
1580
-	ret = __vpfe_get_format(vpfe, &format, &bpp);
1463
+	ret = vpfe_try_fmt(file, priv, fmt);
1464
+	if (ret < 0)
1465
+		return ret;
1466
+
1467
+	f = find_format_by_pix(vpfe, fmt->fmt.pix.pixelformat);
1468
+
1469
+	v4l2_fill_mbus_format(&mbus_fmt, &fmt->fmt.pix, f->code);
1470
+
1471
+	ret = __subdev_set_format(vpfe, &mbus_fmt);
15811472 	if (ret)
15821473 		return ret;
15831474 
1475
+	/* Just double check nothing has gone wrong */
1476
+	if (mbus_fmt.code != f->code) {
1477
+		vpfe_dbg(3, vpfe,
1478
+			 "%s subdev changed format on us, this should not happen\n",
1479
+			 __func__);
1480
+		return -EINVAL;
1481
+	}
15841482 
1585
-	if (!cmp_v4l2_format(fmt, &format)) {
1586
-		/* Sensor format is different from the requested format
1587
-		 * so we need to change it
1588
-		 */
1589
-		ret = __vpfe_set_format(vpfe, fmt, &bpp);
1590
-		if (ret)
1591
-			return ret;
1592
-	} else /* Just make sure all of the fields are consistent */
1593
-		*fmt = format;
1594
-
1595
-	/* First detach any IRQ if currently attached */
1596
-	vpfe_detach_irq(vpfe);
1597
-	vpfe->fmt = *fmt;
1598
-	vpfe->bpp = bpp;
1483
+	v4l2_fill_pix_format(&vpfe->fmt.fmt.pix, &mbus_fmt);
1484
+	vpfe->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1485
+	vpfe->fmt.fmt.pix.pixelformat  = f->fourcc;
1486
+	vpfe_calc_format_size(vpfe, f, &vpfe->fmt);
1487
+	*fmt = vpfe->fmt;
1488
+	vpfe->current_vpfe_fmt = f;
15991489 
16001490 	/* Update the crop window based on found values */
16011491 	vpfe->crop.width = fmt->fmt.pix.width;
....@@ -1610,57 +1500,40 @@
16101500 {
16111501 	struct vpfe_device *vpfe = video_drvdata(file);
16121502 	struct v4l2_subdev_frame_size_enum fse;
1613
-	struct vpfe_subdev_info *sdinfo;
1614
-	struct v4l2_mbus_framefmt mbus;
1615
-	struct v4l2_pix_format pix;
1503
+	struct v4l2_subdev *sd = vpfe->current_subdev->sd;
16161504 	struct vpfe_fmt *fmt;
16171505 	int ret;
16181506 
1619
-	vpfe_dbg(2, vpfe, "vpfe_enum_size\n");
1620
-
16211507 	/* check for valid format */
1622
-	fmt = find_format_by_pix(fsize->pixel_format);
1508
+	fmt = find_format_by_pix(vpfe, fsize->pixel_format);
16231509 	if (!fmt) {
1624
-		vpfe_dbg(3, vpfe, "Invalid pixel code: %x, default used instead\n",
1625
-			fsize->pixel_format);
1510
+		vpfe_dbg(3, vpfe, "Invalid pixel code: %x\n",
1511
+			 fsize->pixel_format);
16261512 		return -EINVAL;
16271513 	}
16281514 
16291515 	memset(fsize->reserved, 0x0, sizeof(fsize->reserved));
16301516 
1631
-	sdinfo = vpfe->current_subdev;
1632
-	if (!sdinfo->sd)
1633
-		return -EINVAL;
1634
-
1635
-	memset(&pix, 0x0, sizeof(pix));
1636
-	/* Construct pix from parameter and use default for the rest */
1637
-	pix.pixelformat = fsize->pixel_format;
1638
-	pix.width = 640;
1639
-	pix.height = 480;
1640
-	pix.colorspace = V4L2_COLORSPACE_SRGB;
1641
-	pix.field = V4L2_FIELD_NONE;
1642
-	pix_to_mbus(vpfe, &pix, &mbus);
1643
-
16441517 	memset(&fse, 0x0, sizeof(fse));
16451518 	fse.index = fsize->index;
16461519 	fse.pad = 0;
1647
-	fse.code = mbus.code;
1520
+	fse.code = fmt->code;
16481521 	fse.which = V4L2_SUBDEV_FORMAT_ACTIVE;
1649
-	ret = v4l2_subdev_call(sdinfo->sd, pad, enum_frame_size, NULL, &fse);
1522
+	ret = v4l2_subdev_call(sd, pad, enum_frame_size, NULL, &fse);
16501523 	if (ret)
1651
-		return -EINVAL;
1524
+		return ret;
16521525 
1653
-	vpfe_dbg(1, vpfe, "vpfe_enum_size: index: %d code: %x W:[%d,%d] H:[%d,%d]\n",
1654
-		fse.index, fse.code, fse.min_width, fse.max_width,
1655
-		fse.min_height, fse.max_height);
1526
+	vpfe_dbg(1, vpfe, "%s: index: %d code: %x W:[%d,%d] H:[%d,%d]\n",
1527
+		 __func__, fse.index, fse.code, fse.min_width, fse.max_width,
1528
+		 fse.min_height, fse.max_height);
16561529 
16571530 	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
16581531 	fsize->discrete.width = fse.max_width;
16591532 	fsize->discrete.height = fse.max_height;
16601533 
1661
-	vpfe_dbg(1, vpfe, "vpfe_enum_size: index: %d pixformat: %s size: %dx%d\n",
1662
-		fsize->index, print_fourcc(fsize->pixel_format),
1663
-		fsize->discrete.width, fsize->discrete.height);
1534
+	vpfe_dbg(1, vpfe, "%s: index: %d pixformat: %s size: %dx%d\n",
1535
+		 __func__, fsize->index, print_fourcc(fsize->pixel_format),
1536
+		 fsize->discrete.width, fsize->discrete.height);
16641537 
16651538 	return 0;
16661539 }
....@@ -1725,8 +1598,6 @@
17251598 	struct vpfe_subdev_info *sdinfo;
17261599 	int subdev, index;
17271600 
1728
-	vpfe_dbg(2, vpfe, "vpfe_enum_input\n");
1729
-
17301601 	if (vpfe_get_subdev_input_index(vpfe, &subdev, &index,
17311602 					inp->index) < 0) {
17321603 		vpfe_dbg(1, vpfe,
....@@ -1743,8 +1614,6 @@
17431614 {
17441615 	struct vpfe_device *vpfe = video_drvdata(file);
17451616 
1746
-	vpfe_dbg(2, vpfe, "vpfe_g_input\n");
1747
-
17481617 	return vpfe_get_app_input_index(vpfe, index);
17491618 }
17501619 
....@@ -1756,8 +1625,6 @@
17561625 	struct vpfe_route *route;
17571626 	u32 input, output;
17581627 	int ret;
1759
-
1760
-	vpfe_dbg(2, vpfe, "vpfe_set_input: index: %d\n", index);
17611628 
17621629 	/* If streaming is started, return error */
17631630 	if (vb2_is_busy(&vpfe->buffer_queue)) {
....@@ -1814,9 +1681,6 @@
18141681 {
18151682 	struct vpfe_device *vpfe = video_drvdata(file);
18161683 
1817
-	vpfe_dbg(2, vpfe,
1818
-		"vpfe_s_input: index: %d\n", index);
1819
-
18201684 	return vpfe_set_input(vpfe, index);
18211685 }
18221686 
....@@ -1824,8 +1688,6 @@
18241688 {
18251689 	struct vpfe_device *vpfe = video_drvdata(file);
18261690 	struct vpfe_subdev_info *sdinfo;
1827
-
1828
-	vpfe_dbg(2, vpfe, "vpfe_querystd\n");
18291691 
18301692 	sdinfo = vpfe->current_subdev;
18311693 	if (!(sdinfo->inputs[0].capabilities & V4L2_IN_CAP_STD))
....@@ -1841,8 +1703,6 @@
18411703 	struct vpfe_device *vpfe = video_drvdata(file);
18421704 	struct vpfe_subdev_info *sdinfo;
18431705 	int ret;
1844
-
1845
-	vpfe_dbg(2, vpfe, "vpfe_s_std\n");
18461706 
18471707 	sdinfo = vpfe->current_subdev;
18481708 	if (!(sdinfo->inputs[0].capabilities & V4L2_IN_CAP_STD))
....@@ -1875,8 +1735,6 @@
18751735 	struct vpfe_device *vpfe = video_drvdata(file);
18761736 	struct vpfe_subdev_info *sdinfo;
18771737 
1878
-	vpfe_dbg(2, vpfe, "vpfe_g_std\n");
1879
-
18801738 	sdinfo = vpfe->current_subdev;
18811739 	if (sdinfo->inputs[0].capabilities != V4L2_IN_CAP_STD)
18821740 		return -ENODATA;
....@@ -1893,8 +1751,6 @@
18931751 static void vpfe_calculate_offsets(struct vpfe_device *vpfe)
18941752 {
18951753 	struct v4l2_rect image_win;
1896
-
1897
-	vpfe_dbg(2, vpfe, "vpfe_calculate_offsets\n");
18981754 
18991755 	vpfe_ccdc_get_image_window(&vpfe->ccdc, &image_win);
19001756 	vpfe->field_off = image_win.height * image_win.width;
....@@ -1979,6 +1835,29 @@
19791835 	spin_unlock_irqrestore(&vpfe->dma_queue_lock, flags);
19801836 }
19811837 
1838
+static void vpfe_return_all_buffers(struct vpfe_device *vpfe,
1839
+				    enum vb2_buffer_state state)
1840
+{
1841
+	struct vpfe_cap_buffer *buf, *node;
1842
+	unsigned long flags;
1843
+
1844
+	spin_lock_irqsave(&vpfe->dma_queue_lock, flags);
1845
+	list_for_each_entry_safe(buf, node, &vpfe->dma_queue, list) {
1846
+		vb2_buffer_done(&buf->vb.vb2_buf, state);
1847
+		list_del(&buf->list);
1848
+	}
1849
+
1850
+	if (vpfe->cur_frm)
1851
+		vb2_buffer_done(&vpfe->cur_frm->vb.vb2_buf, state);
1852
+
1853
+	if (vpfe->next_frm && vpfe->next_frm != vpfe->cur_frm)
1854
+		vb2_buffer_done(&vpfe->next_frm->vb.vb2_buf, state);
1855
+
1856
+	vpfe->cur_frm = NULL;
1857
+	vpfe->next_frm = NULL;
1858
+	spin_unlock_irqrestore(&vpfe->dma_queue_lock, flags);
1859
+}
1860
+
19821861 /*
19831862  * vpfe_start_streaming : Starts the DMA engine for streaming
19841863  * @vb: ptr to vb2_buffer
....@@ -1987,7 +1866,6 @@
19871866 static int vpfe_start_streaming(struct vb2_queue *vq, unsigned int count)
19881867 {
19891868 	struct vpfe_device *vpfe = vb2_get_drv_priv(vq);
1990
-	struct vpfe_cap_buffer *buf, *tmp;
19911869 	struct vpfe_subdev_info *sdinfo;
19921870 	unsigned long flags;
19931871 	unsigned long addr;
....@@ -2001,6 +1879,9 @@
20011879 	sdinfo = vpfe->current_subdev;
20021880 
20031881 	vpfe_attach_irq(vpfe);
1882
+
1883
+	vpfe->stopping = false;
1884
+	init_completion(&vpfe->capture_stop);
20041885 
20051886 	if (vpfe->ccdc.ccdc_cfg.if_type == VPFE_RAW_BAYER)
20061887 		vpfe_ccdc_config_raw(&vpfe->ccdc);
....@@ -2030,11 +1911,8 @@
20301911 	return 0;
20311912 
20321913 err:
2033
-	list_for_each_entry_safe(buf, tmp, &vpfe->dma_queue, list) {
2034
-		list_del(&buf->list);
2035
-		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
2036
-	}
2037
-
1914
+	vpfe_return_all_buffers(vpfe, VB2_BUF_STATE_QUEUED);
1915
+	vpfe_pcr_enable(&vpfe->ccdc, 0);
20381916 	return ret;
20391917 }
20401918 
....@@ -2049,10 +1927,14 @@
20491927 {
20501928 	struct vpfe_device *vpfe = vb2_get_drv_priv(vq);
20511929 	struct vpfe_subdev_info *sdinfo;
2052
-	unsigned long flags;
20531930 	int ret;
20541931 
20551932 	vpfe_pcr_enable(&vpfe->ccdc, 0);
1933
+
1934
+	/* Wait for the last frame to be captured */
1935
+	vpfe->stopping = true;
1936
+	wait_for_completion_timeout(&vpfe->capture_stop,
1937
+				    msecs_to_jiffies(250));
20561938 
20571939 	vpfe_detach_irq(vpfe);
20581940 
....@@ -2062,47 +1944,19 @@
20621944 		vpfe_dbg(1, vpfe, "stream off failed in subdev\n");
20631945 
20641946 	/* release all active buffers */
2065
-	spin_lock_irqsave(&vpfe->dma_queue_lock, flags);
2066
-	if (vpfe->cur_frm == vpfe->next_frm) {
2067
-		vb2_buffer_done(&vpfe->cur_frm->vb.vb2_buf,
2068
-				VB2_BUF_STATE_ERROR);
2069
-	} else {
2070
-		if (vpfe->cur_frm != NULL)
2071
-			vb2_buffer_done(&vpfe->cur_frm->vb.vb2_buf,
2072
-					VB2_BUF_STATE_ERROR);
2073
-		if (vpfe->next_frm != NULL)
2074
-			vb2_buffer_done(&vpfe->next_frm->vb.vb2_buf,
2075
-					VB2_BUF_STATE_ERROR);
2076
-	}
2077
-
2078
-	while (!list_empty(&vpfe->dma_queue)) {
2079
-		vpfe->next_frm = list_entry(vpfe->dma_queue.next,
2080
-						struct vpfe_cap_buffer, list);
2081
-		list_del(&vpfe->next_frm->list);
2082
-		vb2_buffer_done(&vpfe->next_frm->vb.vb2_buf,
2083
-				VB2_BUF_STATE_ERROR);
2084
-	}
2085
-	spin_unlock_irqrestore(&vpfe->dma_queue_lock, flags);
1947
+	vpfe_return_all_buffers(vpfe, VB2_BUF_STATE_ERROR);
20861948 }
20871949 
2088
-static int vpfe_cropcap(struct file *file, void *priv,
2089
-			struct v4l2_cropcap *crop)
1950
+static int vpfe_g_pixelaspect(struct file *file, void *priv,
1951
+			      int type, struct v4l2_fract *f)
20901952 {
20911953 	struct vpfe_device *vpfe = video_drvdata(file);
20921954 
2093
-	vpfe_dbg(2, vpfe, "vpfe_cropcap\n");
2094
-
2095
-	if (vpfe->std_index >= ARRAY_SIZE(vpfe_standards))
1955
+	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
1956
+	    vpfe->std_index >= ARRAY_SIZE(vpfe_standards))
20961957 		return -EINVAL;
20971958 
2098
-	memset(crop, 0, sizeof(struct v4l2_cropcap));
2099
-
2100
-	crop->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
2101
-	crop->defrect.width = vpfe_standards[vpfe->std_index].width;
2102
-	crop->bounds.width = crop->defrect.width;
2103
-	crop->defrect.height = vpfe_standards[vpfe->std_index].height;
2104
-	crop->bounds.height = crop->defrect.height;
2105
-	crop->pixelaspect = vpfe_standards[vpfe->std_index].pixelaspect;
1959
+	*f = vpfe_standards[vpfe->std_index].pixelaspect;
21061960 
21071961 	return 0;
21081962 }
....@@ -2112,12 +1966,17 @@
21121966 {
21131967 	struct vpfe_device *vpfe = video_drvdata(file);
21141968 
1969
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
1970
+	    vpfe->std_index >= ARRAY_SIZE(vpfe_standards))
1971
+		return -EINVAL;
1972
+
21151973 	switch (s->target) {
21161974 	case V4L2_SEL_TGT_CROP_BOUNDS:
21171975 	case V4L2_SEL_TGT_CROP_DEFAULT:
2118
-		s->r.left = s->r.top = 0;
2119
-		s->r.width = vpfe->crop.width;
2120
-		s->r.height = vpfe->crop.height;
1976
+		s->r.left = 0;
1977
+		s->r.top = 0;
1978
+		s->r.width = vpfe_standards[vpfe->std_index].width;
1979
+		s->r.height = vpfe_standards[vpfe->std_index].height;
21211980 		break;
21221981 
21231982 	case V4L2_SEL_TGT_CROP:
....@@ -2131,26 +1990,13 @@
21311990 	return 0;
21321991 }
21331992 
2134
-static int enclosed_rectangle(struct v4l2_rect *a, struct v4l2_rect *b)
2135
-{
2136
-	if (a->left < b->left || a->top < b->top)
2137
-		return 0;
2138
-
2139
-	if (a->left + a->width > b->left + b->width)
2140
-		return 0;
2141
-
2142
-	if (a->top + a->height > b->top + b->height)
2143
-		return 0;
2144
-
2145
-	return 1;
2146
-}
2147
-
21481993 static int
21491994 vpfe_s_selection(struct file *file, void *fh, struct v4l2_selection *s)
21501995 {
21511996 	struct vpfe_device *vpfe = video_drvdata(file);
21521997 	struct v4l2_rect cr = vpfe->crop;
21531998 	struct v4l2_rect r = s->r;
1999
+	u32 bpp;
21542000 
21552001 	/* If streaming is started, return error */
21562002 	if (vb2_is_busy(&vpfe->buffer_queue)) {
....@@ -2168,18 +2014,20 @@
21682014 	r.left = clamp_t(unsigned int, r.left, 0, cr.width - r.width);
21692015 	r.top  = clamp_t(unsigned int, r.top, 0, cr.height - r.height);
21702016 
2171
-	if (s->flags & V4L2_SEL_FLAG_LE && !enclosed_rectangle(&r, &s->r))
2017
+	if (s->flags & V4L2_SEL_FLAG_LE && !v4l2_rect_enclosed(&r, &s->r))
21722018 		return -ERANGE;
21732019 
2174
-	if (s->flags & V4L2_SEL_FLAG_GE && !enclosed_rectangle(&s->r, &r))
2020
+	if (s->flags & V4L2_SEL_FLAG_GE && !v4l2_rect_enclosed(&s->r, &r))
21752021 		return -ERANGE;
21762022 
21772023 	s->r = vpfe->crop = r;
21782024 
2179
-	vpfe_ccdc_set_image_window(&vpfe->ccdc, &r, vpfe->bpp);
2025
+	bpp = __get_bytesperpixel(vpfe, vpfe->current_vpfe_fmt);
2026
+	vpfe_ccdc_set_image_window(&vpfe->ccdc, &r, bpp);
21802027 	vpfe->fmt.fmt.pix.width = r.width;
21812028 	vpfe->fmt.fmt.pix.height = r.height;
2182
-	vpfe->fmt.fmt.pix.bytesperline = vpfe_ccdc_get_line_length(&vpfe->ccdc);
2029
+	vpfe->fmt.fmt.pix.bytesperline =
2030
+		vpfe_ccdc_get_line_length(&vpfe->ccdc);
21832031 	vpfe->fmt.fmt.pix.sizeimage = vpfe->fmt.fmt.pix.bytesperline *
21842032 						vpfe->fmt.fmt.pix.height;
21852033 
....@@ -2194,8 +2042,6 @@
21942042 {
21952043 	struct vpfe_device *vpfe = video_drvdata(file);
21962044 	int ret;
2197
-
2198
-	vpfe_dbg(2, vpfe, "vpfe_ioctl_default\n");
21992045 
22002046 	if (!valid_prio) {
22012047 		vpfe_err(vpfe, "%s device busy\n", __func__);
....@@ -2286,7 +2132,7 @@
22862132 	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
22872133 	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
22882134 
2289
-	.vidioc_cropcap			= vpfe_cropcap,
2135
+	.vidioc_g_pixelaspect		= vpfe_g_pixelaspect,
22902136 	.vidioc_g_selection		= vpfe_g_selection,
22912137 	.vidioc_s_selection		= vpfe_s_selection,
22922138 
....@@ -2302,10 +2148,10 @@
23022148 					       struct vpfe_device, v4l2_dev);
23032149 	struct v4l2_subdev_mbus_code_enum mbus_code;
23042150 	struct vpfe_subdev_info *sdinfo;
2151
+	struct vpfe_fmt *fmt;
2152
+	int ret = 0;
23052153 	bool found = false;
2306
-	int i, j;
2307
-
2308
-	vpfe_dbg(1, vpfe, "vpfe_async_bound\n");
2154
+	int i, j, k;
23092155 
23102156 	for (i = 0; i < ARRAY_SIZE(vpfe->cfg->asd); i++) {
23112157 		if (vpfe->cfg->asd[i]->match.fwnode ==
....@@ -2325,27 +2171,37 @@
23252171 
23262172 	vpfe->video_dev.tvnorms |= sdinfo->inputs[0].std;
23272173 
2328
-	/* setup the supported formats & indexes */
2329
-	for (j = 0, i = 0; ; ++j) {
2330
-		struct vpfe_fmt *fmt;
2331
-		int ret;
2332
-
2174
+	vpfe->num_active_fmt = 0;
2175
+	for (j = 0, i = 0; (ret != -EINVAL); ++j) {
23332176 		memset(&mbus_code, 0, sizeof(mbus_code));
23342177 		mbus_code.index = j;
23352178 		mbus_code.which = V4L2_SUBDEV_FORMAT_ACTIVE;
23362179 		ret = v4l2_subdev_call(subdev, pad, enum_mbus_code,
2337
-			       NULL, &mbus_code);
2180
+				       NULL, &mbus_code);
23382181 		if (ret)
2339
-			break;
2340
-
2341
-		fmt = find_format_by_code(mbus_code.code);
2342
-		if (!fmt)
23432182 			continue;
23442183 
2345
-		fmt->supported = true;
2346
-		fmt->index = i++;
2184
+		vpfe_dbg(3, vpfe,
2185
+			 "subdev %s: code: %04x idx: %d\n",
2186
+			 subdev->name, mbus_code.code, j);
2187
+
2188
+		for (k = 0; k < ARRAY_SIZE(formats); k++) {
2189
+			fmt = &formats[k];
2190
+			if (mbus_code.code != fmt->code)
2191
+				continue;
2192
+			vpfe->active_fmt[i] = fmt;
2193
+			vpfe_dbg(3, vpfe,
2194
+				 "matched fourcc: %s code: %04x idx: %d\n",
2195
+				 print_fourcc(fmt->fourcc), mbus_code.code, i);
2196
+			vpfe->num_active_fmt = ++i;
2197
+		}
23472198 	}
23482199 
2200
+	if (!i) {
2201
+		vpfe_err(vpfe, "No suitable format reported by subdev %s\n",
2202
+			 subdev->name);
2203
+		return -EINVAL;
2204
+	}
23492205 	return 0;
23502206 }
23512207 
....@@ -2390,7 +2246,7 @@
23902246 	INIT_LIST_HEAD(&vpfe->dma_queue);
23912247 
23922248 	vdev = &vpfe->video_dev;
2393
-	strlcpy(vdev->name, VPFE_MODULE_NAME, sizeof(vdev->name));
2249
+	strscpy(vdev->name, VPFE_MODULE_NAME, sizeof(vdev->name));
23942250 	vdev->release = video_device_release_empty;
23952251 	vdev->fops = &vpfe_fops;
23962252 	vdev->ioctl_ops = &vpfe_ioctl_ops;
....@@ -2398,8 +2254,10 @@
23982254 	vdev->vfl_dir = VFL_DIR_RX;
23992255 	vdev->queue = q;
24002256 	vdev->lock = &vpfe->lock;
2257
+	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
2258
+			    V4L2_CAP_READWRITE;
24012259 	video_set_drvdata(vdev, vpfe);
2402
-	err = video_register_device(&vpfe->video_dev, VFL_TYPE_GRABBER, -1);
2260
+	err = video_register_device(&vpfe->video_dev, VFL_TYPE_VIDEO, -1);
24032261 	if (err) {
24042262 		vpfe_err(vpfe,
24052263 			"Unable to register video device.\n");
....@@ -2427,30 +2285,32 @@
24272285 };
24282286 
24292287 static struct vpfe_config *
2430
-vpfe_get_pdata(struct platform_device *pdev)
2288
+vpfe_get_pdata(struct vpfe_device *vpfe)
24312289 {
24322290 	struct device_node *endpoint = NULL;
2433
-	struct v4l2_fwnode_endpoint bus_cfg;
2291
+	struct device *dev = vpfe->pdev;
24342292 	struct vpfe_subdev_info *sdinfo;
24352293 	struct vpfe_config *pdata;
24362294 	unsigned int flags;
24372295 	unsigned int i;
24382296 	int err;
24392297 
2440
-	dev_dbg(&pdev->dev, "vpfe_get_pdata\n");
2298
+	dev_dbg(dev, "vpfe_get_pdata\n");
24412299 
2442
-	if (!IS_ENABLED(CONFIG_OF) || !pdev->dev.of_node)
2443
-		return pdev->dev.platform_data;
2300
+	v4l2_async_notifier_init(&vpfe->notifier);
24442301 
2445
-	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
2302
+	if (!IS_ENABLED(CONFIG_OF) || !dev->of_node)
2303
+		return dev->platform_data;
2304
+
2305
+	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
24462306 	if (!pdata)
24472307 		return NULL;
24482308 
24492309 	for (i = 0; ; i++) {
2310
+		struct v4l2_fwnode_endpoint bus_cfg = { .bus_type = 0 };
24502311 		struct device_node *rem;
24512312 
2452
-		endpoint = of_graph_get_next_endpoint(pdev->dev.of_node,
2453
-						      endpoint);
2313
+		endpoint = of_graph_get_next_endpoint(dev->of_node, endpoint);
24542314 		if (!endpoint)
24552315 			break;
24562316 
....@@ -2459,7 +2319,8 @@
24592319 
24602320 		/* we only support camera */
24612321 		sdinfo->inputs[0].index = i;
2462
-		strcpy(sdinfo->inputs[0].name, "Camera");
2322
+		strscpy(sdinfo->inputs[0].name, "Camera",
2323
+			sizeof(sdinfo->inputs[0].name));
24632324 		sdinfo->inputs[0].type = V4L2_INPUT_TYPE_CAMERA;
24642325 		sdinfo->inputs[0].std = V4L2_STD_ALL;
24652326 		sdinfo->inputs[0].capabilities = V4L2_IN_CAP_STD;
....@@ -2477,16 +2338,16 @@
24772338 		err = v4l2_fwnode_endpoint_parse(of_fwnode_handle(endpoint),
24782339 						 &bus_cfg);
24792340 		if (err) {
2480
-			dev_err(&pdev->dev, "Could not parse the endpoint\n");
2481
-			goto done;
2341
+			dev_err(dev, "Could not parse the endpoint\n");
2342
+			goto cleanup;
24822343 		}
24832344 
24842345 		sdinfo->vpfe_param.bus_width = bus_cfg.bus.parallel.bus_width;
24852346 
24862347 		if (sdinfo->vpfe_param.bus_width < 8 ||
24872348 			sdinfo->vpfe_param.bus_width > 16) {
2488
-			dev_err(&pdev->dev, "Invalid bus width.\n");
2489
-			goto done;
2349
+			dev_err(dev, "Invalid bus width.\n");
2350
+			goto cleanup;
24902351 		}
24912352 
24922353 		flags = bus_cfg.bus.parallel.flags;
....@@ -2499,29 +2360,24 @@
24992360 
25002361 		rem = of_graph_get_remote_port_parent(endpoint);
25012362 		if (!rem) {
2502
-			dev_err(&pdev->dev, "Remote device at %pOF not found\n",
2363
+			dev_err(dev, "Remote device at %pOF not found\n",
25032364 				endpoint);
2504
-			goto done;
2365
+			goto cleanup;
25052366 		}
25062367 
2507
-		pdata->asd[i] = devm_kzalloc(&pdev->dev,
2508
-					     sizeof(struct v4l2_async_subdev),
2509
-					     GFP_KERNEL);
2510
-		if (!pdata->asd[i]) {
2511
-			of_node_put(rem);
2512
-			pdata = NULL;
2513
-			goto done;
2514
-		}
2515
-
2516
-		pdata->asd[i]->match_type = V4L2_ASYNC_MATCH_FWNODE;
2517
-		pdata->asd[i]->match.fwnode = of_fwnode_handle(rem);
2368
+		pdata->asd[i] = v4l2_async_notifier_add_fwnode_subdev(
2369
+			&vpfe->notifier, of_fwnode_handle(rem),
2370
+			sizeof(struct v4l2_async_subdev));
25182371 		of_node_put(rem);
2372
+		if (IS_ERR(pdata->asd[i]))
2373
+			goto cleanup;
25192374 	}
25202375 
25212376 	of_node_put(endpoint);
25222377 	return pdata;
25232378 
2524
-done:
2379
+cleanup:
2380
+	v4l2_async_notifier_cleanup(&vpfe->notifier);
25252381 	of_node_put(endpoint);
25262382 	return NULL;
25272383 }
....@@ -2533,34 +2389,38 @@
25332389  */
25342390 static int vpfe_probe(struct platform_device *pdev)
25352391 {
2536
-	struct vpfe_config *vpfe_cfg = vpfe_get_pdata(pdev);
2392
+	struct vpfe_config *vpfe_cfg;
25372393 	struct vpfe_device *vpfe;
25382394 	struct vpfe_ccdc *ccdc;
25392395 	struct resource	*res;
25402396 	int ret;
2541
-
2542
-	if (!vpfe_cfg) {
2543
-		dev_err(&pdev->dev, "No platform data\n");
2544
-		return -EINVAL;
2545
-	}
25462397 
25472398 	vpfe = devm_kzalloc(&pdev->dev, sizeof(*vpfe), GFP_KERNEL);
25482399 	if (!vpfe)
25492400 		return -ENOMEM;
25502401 
25512402 	vpfe->pdev = &pdev->dev;
2403
+
2404
+	vpfe_cfg = vpfe_get_pdata(vpfe);
2405
+	if (!vpfe_cfg) {
2406
+		dev_err(&pdev->dev, "No platform data\n");
2407
+		return -EINVAL;
2408
+	}
2409
+
25522410 	vpfe->cfg = vpfe_cfg;
25532411 	ccdc = &vpfe->ccdc;
25542412 
25552413 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
25562414 	ccdc->ccdc_cfg.base_addr = devm_ioremap_resource(&pdev->dev, res);
2557
-	if (IS_ERR(ccdc->ccdc_cfg.base_addr))
2558
-		return PTR_ERR(ccdc->ccdc_cfg.base_addr);
2415
+	if (IS_ERR(ccdc->ccdc_cfg.base_addr)) {
2416
+		ret = PTR_ERR(ccdc->ccdc_cfg.base_addr);
2417
+		goto probe_out_cleanup;
2418
+	}
25592419 
25602420 	ret = platform_get_irq(pdev, 0);
25612421 	if (ret <= 0) {
2562
-		dev_err(&pdev->dev, "No IRQ resource\n");
2563
-		return -ENODEV;
2422
+		ret = -ENODEV;
2423
+		goto probe_out_cleanup;
25642424 	}
25652425 	vpfe->irq = ret;
25662426 
....@@ -2568,14 +2428,15 @@
25682428 			       "vpfe_capture0", vpfe);
25692429 	if (ret) {
25702430 		dev_err(&pdev->dev, "Unable to request interrupt\n");
2571
-		return -EINVAL;
2431
+		ret = -EINVAL;
2432
+		goto probe_out_cleanup;
25722433 	}
25732434 
25742435 	ret = v4l2_device_register(&pdev->dev, &vpfe->v4l2_dev);
25752436 	if (ret) {
25762437 		vpfe_err(vpfe,
25772438 			"Unable to register v4l2 device.\n");
2578
-		return ret;
2439
+		goto probe_out_cleanup;
25792440 	}
25802441 
25812442 	/* set the driver data in platform device */
....@@ -2584,7 +2445,11 @@
25842445 	pm_runtime_enable(&pdev->dev);
25852446 
25862447 	/* for now just enable it here instead of waiting for the open */
2587
-	pm_runtime_get_sync(&pdev->dev);
2448
+	ret = pm_runtime_resume_and_get(&pdev->dev);
2449
+	if (ret < 0) {
2450
+		vpfe_err(vpfe, "Unable to resume device.\n");
2451
+		goto probe_out_v4l2_unregister;
2452
+	}
25882453 
25892454 	vpfe_ccdc_config_defaults(ccdc);
25902455 
....@@ -2599,11 +2464,8 @@
25992464 		goto probe_out_v4l2_unregister;
26002465 	}
26012466 
2602
-	vpfe->notifier.subdevs = vpfe->cfg->asd;
2603
-	vpfe->notifier.num_subdevs = ARRAY_SIZE(vpfe->cfg->asd);
26042467 	vpfe->notifier.ops = &vpfe_async_ops;
2605
-	ret = v4l2_async_notifier_register(&vpfe->v4l2_dev,
2606
-						&vpfe->notifier);
2468
+	ret = v4l2_async_notifier_register(&vpfe->v4l2_dev, &vpfe->notifier);
26072469 	if (ret) {
26082470 		vpfe_err(vpfe, "Error registering async notifier\n");
26092471 		ret = -EINVAL;
....@@ -2614,6 +2476,8 @@
26142476 
26152477 probe_out_v4l2_unregister:
26162478 	v4l2_device_unregister(&vpfe->v4l2_dev);
2479
+probe_out_cleanup:
2480
+	v4l2_async_notifier_cleanup(&vpfe->notifier);
26172481 	return ret;
26182482 }
26192483 
....@@ -2624,11 +2488,10 @@
26242488 {
26252489 	struct vpfe_device *vpfe = platform_get_drvdata(pdev);
26262490 
2627
-	vpfe_dbg(2, vpfe, "vpfe_remove\n");
2628
-
26292491 	pm_runtime_disable(&pdev->dev);
26302492 
26312493 	v4l2_async_notifier_unregister(&vpfe->notifier);
2494
+	v4l2_async_notifier_cleanup(&vpfe->notifier);
26322495 	v4l2_device_unregister(&vpfe->v4l2_dev);
26332496 	video_unregister_device(&vpfe->video_dev);
26342497 
....@@ -2671,22 +2534,26 @@
26712534 	struct vpfe_device *vpfe = dev_get_drvdata(dev);
26722535 	struct vpfe_ccdc *ccdc = &vpfe->ccdc;
26732536 
2674
-	/* if streaming has not started we don't care */
2675
-	if (!vb2_start_streaming_called(&vpfe->buffer_queue))
2676
-		return 0;
2537
+	/* only do full suspend if streaming has started */
2538
+	if (vb2_start_streaming_called(&vpfe->buffer_queue)) {
2539
+		/*
2540
+		 * ignore RPM resume errors here, as it is already too late.
2541
+		 * A check like that should happen earlier, either at
2542
+		 * open() or just before start streaming.
2543
+		 */
2544
+		pm_runtime_get_sync(dev);
2545
+		vpfe_config_enable(ccdc, 1);
26772546 
2678
-	pm_runtime_get_sync(dev);
2679
-	vpfe_config_enable(ccdc, 1);
2547
+		/* Save VPFE context */
2548
+		vpfe_save_context(ccdc);
26802549 
2681
-	/* Save VPFE context */
2682
-	vpfe_save_context(ccdc);
2550
+		/* Disable CCDC */
2551
+		vpfe_pcr_enable(ccdc, 0);
2552
+		vpfe_config_enable(ccdc, 0);
26832553 
2684
-	/* Disable CCDC */
2685
-	vpfe_pcr_enable(ccdc, 0);
2686
-	vpfe_config_enable(ccdc, 0);
2687
-
2688
-	/* Disable both master and slave clock */
2689
-	pm_runtime_put_sync(dev);
2554
+		/* Disable both master and slave clock */
2555
+		pm_runtime_put_sync(dev);
2556
+	}
26902557 
26912558 	/* Select sleep pin state */
26922559 	pinctrl_pm_select_sleep_state(dev);
....@@ -2728,19 +2595,18 @@
27282595 	struct vpfe_device *vpfe = dev_get_drvdata(dev);
27292596 	struct vpfe_ccdc *ccdc = &vpfe->ccdc;
27302597 
2731
-	/* if streaming has not started we don't care */
2732
-	if (!vb2_start_streaming_called(&vpfe->buffer_queue))
2733
-		return 0;
2598
+	/* only do full resume if streaming has started */
2599
+	if (vb2_start_streaming_called(&vpfe->buffer_queue)) {
2600
+		/* Enable both master and slave clock */
2601
+		pm_runtime_get_sync(dev);
2602
+		vpfe_config_enable(ccdc, 1);
27342603 
2735
-	/* Enable both master and slave clock */
2736
-	pm_runtime_get_sync(dev);
2737
-	vpfe_config_enable(ccdc, 1);
2604
+		/* Restore VPFE context */
2605
+		vpfe_restore_context(ccdc);
27382606 
2739
-	/* Restore VPFE context */
2740
-	vpfe_restore_context(ccdc);
2741
-
2742
-	vpfe_config_enable(ccdc, 0);
2743
-	pm_runtime_put_sync(dev);
2607
+		vpfe_config_enable(ccdc, 0);
2608
+		pm_runtime_put_sync(dev);
2609
+	}
27442610 
27452611 	/* Select default pin state */
27462612 	pinctrl_pm_select_default_state(dev);