~hc/RK356X_SDK_RELEASE.git

..	..	@@ -18,12 +18,15 @@
18	18	#include <media/videobuf2-dma-contig.h>
19	19	#include <media/videobuf2-dma-sg.h>
20	20	#include <soc/rockchip/rockchip-system-status.h>
21		-#include <dt-bindings/soc/rockchip-system-status.h>
	21	+#include <soc/rockchip/rockchip_iommu.h>
	22	+#include <linux/rk-isp32-config.h>
22	23
23	24	#include "dev.h"
24	25	#include "mipi-csi2.h"
	26	+#include "common.h"
	27	+#include "rkcif-externel.h"
25	28
26		-#define CIF_REQ_BUFS_MIN 3
	29	+#define CIF_REQ_BUFS_MIN 1
27	30	#define CIF_MIN_WIDTH 64
28	31	#define CIF_MIN_HEIGHT 64
29	32	#define CIF_MAX_WIDTH 8192
..	..	@@ -33,6 +36,7 @@
33	36
34	37	#define RKCIF_PLANE_Y 0
35	38	#define RKCIF_PLANE_CBCR 1
	39	+#define RKCIF_MAX_PLANE 3
36	40
37	41	#define STREAM_PAD_SINK 0
38	42	#define STREAM_PAD_SOURCE 1
..	..	@@ -146,10 +150,18 @@
146	150	.csi_fmt_val = CSI_WRDDR_TYPE_RGB888,
147	151	.fmt_type = CIF_FMT_TYPE_RAW,
148	152	}, {
	153	+ .fourcc = V4L2_PIX_FMT_BGR24,
	154	+ .cplanes = 1,
	155	+ .mplanes = 1,
	156	+ .bpp = { 24 },
	157	+ .csi_fmt_val = CSI_WRDDR_TYPE_RGB888,
	158	+ .fmt_type = CIF_FMT_TYPE_RAW,
	159	+ }, {
149	160	.fourcc = V4L2_PIX_FMT_RGB565,
150	161	.cplanes = 1,
151	162	.mplanes = 1,
152	163	.bpp = { 16 },
	164	+ .csi_fmt_val = CSI_WRDDR_TYPE_RGB565,
153	165	.fmt_type = CIF_FMT_TYPE_RAW,
154	166	}, {
155	167	.fourcc = V4L2_PIX_FMT_BGR666,
..	..	@@ -262,6 +274,30 @@
262	274	.csi_fmt_val = CSI_WRDDR_TYPE_RAW8,
263	275	.fmt_type = CIF_FMT_TYPE_RAW,
264	276	}, {
	277	+ .fourcc = V4L2_PIX_FMT_SGBRG16,
	278	+ .cplanes = 1,
	279	+ .mplanes = 1,
	280	+ .bpp = { 16 },
	281	+ .raw_bpp = 16,
	282	+ .csi_fmt_val = CSI_WRDDR_TYPE_RAW8,
	283	+ .fmt_type = CIF_FMT_TYPE_RAW,
	284	+ }, {
	285	+ .fourcc = V4L2_PIX_FMT_SGRBG16,
	286	+ .cplanes = 1,
	287	+ .mplanes = 1,
	288	+ .bpp = { 16 },
	289	+ .raw_bpp = 16,
	290	+ .csi_fmt_val = CSI_WRDDR_TYPE_RAW8,
	291	+ .fmt_type = CIF_FMT_TYPE_RAW,
	292	+ }, {
	293	+ .fourcc = V4L2_PIX_FMT_SRGGB16,
	294	+ .cplanes = 1,
	295	+ .mplanes = 1,
	296	+ .bpp = { 16 },
	297	+ .raw_bpp = 16,
	298	+ .csi_fmt_val = CSI_WRDDR_TYPE_RAW8,
	299	+ .fmt_type = CIF_FMT_TYPE_RAW,
	300	+ }, {
265	301	.fourcc = V4L2_PIX_FMT_Y16,
266	302	.cplanes = 1,
267	303	.mplanes = 1,
..	..	@@ -293,8 +329,51 @@
293	329	.raw_bpp = 16,
294	330	.csi_fmt_val = CSI_WRDDR_TYPE_RAW8,
295	331	.fmt_type = CIF_FMT_TYPE_RAW,
	332	+ }, {
	333	+ .fourcc = V4L2_PIX_FMT_Y12,
	334	+ .cplanes = 1,
	335	+ .mplanes = 1,
	336	+ .bpp = { 16 },
	337	+ .raw_bpp = 12,
	338	+ .csi_fmt_val = CSI_WRDDR_TYPE_RAW12,
	339	+ .fmt_type = CIF_FMT_TYPE_RAW,
	340	+ }, {
	341	+ .fourcc = V4L2_PIX_FMT_Y10,
	342	+ .cplanes = 1,
	343	+ .mplanes = 1,
	344	+ .bpp = { 16 },
	345	+ .raw_bpp = 10,
	346	+ .csi_fmt_val = CSI_WRDDR_TYPE_RAW10,
	347	+ .fmt_type = CIF_FMT_TYPE_RAW,
	348	+ }, {
	349	+ .fourcc = V4L2_PIX_FMT_SRGGB16,
	350	+ .cplanes = 1,
	351	+ .mplanes = 1,
	352	+ .bpp = { 16 },
	353	+ .raw_bpp = 16,
	354	+ .fmt_type = CIF_FMT_TYPE_RAW,
	355	+ }, {
	356	+ .fourcc = V4L2_PIX_FMT_SGRBG16,
	357	+ .cplanes = 1,
	358	+ .mplanes = 1,
	359	+ .bpp = { 16 },
	360	+ .raw_bpp = 16,
	361	+ .fmt_type = CIF_FMT_TYPE_RAW,
	362	+ }, {
	363	+ .fourcc = V4L2_PIX_FMT_SGBRG16,
	364	+ .cplanes = 1,
	365	+ .mplanes = 1,
	366	+ .bpp = { 16 },
	367	+ .raw_bpp = 16,
	368	+ .fmt_type = CIF_FMT_TYPE_RAW,
	369	+ }, {
	370	+ .fourcc = V4L2_PIX_FMT_SBGGR16,
	371	+ .cplanes = 1,
	372	+ .mplanes = 1,
	373	+ .bpp = { 16 },
	374	+ .raw_bpp = 16,
	375	+ .fmt_type = CIF_FMT_TYPE_RAW,
296	376	}
297		-
298	377	/* TODO: We can support NV12M/NV21M/NV16M/NV61M too */
299	378	};
300	379
..	..	@@ -432,6 +511,18 @@
432	511	.csi_fmt_val = CSI_WRDDR_TYPE_RGB888,
433	512	.field = V4L2_FIELD_NONE,
434	513	}, {
	514	+ .mbus_code = MEDIA_BUS_FMT_BGR888_1X24,
	515	+ .csi_fmt_val = CSI_WRDDR_TYPE_RGB888,
	516	+ .field = V4L2_FIELD_NONE,
	517	+ }, {
	518	+ .mbus_code = MEDIA_BUS_FMT_GBR888_1X24,
	519	+ .csi_fmt_val = CSI_WRDDR_TYPE_RGB888,
	520	+ .field = V4L2_FIELD_NONE,
	521	+ }, {
	522	+ .mbus_code = MEDIA_BUS_FMT_RGB565_1X16,
	523	+ .csi_fmt_val = CSI_WRDDR_TYPE_RGB565,
	524	+ .field = V4L2_FIELD_NONE,
	525	+ }, {
435	526	.mbus_code = MEDIA_BUS_FMT_Y8_1X8,
436	527	.dvp_fmt_val = INPUT_MODE_RAW \| RAW_DATA_WIDTH_8,
437	528	.csi_fmt_val = CSI_WRDDR_TYPE_RAW8,
..	..	@@ -463,6 +554,144 @@
463	554	.field = V4L2_FIELD_NONE,
464	555	}
465	556	};
	557	+
	558	+static int rkcif_output_fmt_check(struct rkcif_stream *stream,
	559	+ const struct cif_output_fmt *output_fmt)
	560	+{
	561	+ const struct cif_input_fmt *input_fmt = stream->cif_fmt_in;
	562	+ struct csi_channel_info *channel = &stream->cifdev->channels[stream->id];
	563	+ int ret = -EINVAL;
	564	+
	565	+ switch (input_fmt->mbus_code) {
	566	+ case MEDIA_BUS_FMT_YUYV8_2X8:
	567	+ case MEDIA_BUS_FMT_YVYU8_2X8:
	568	+ case MEDIA_BUS_FMT_UYVY8_2X8:
	569	+ case MEDIA_BUS_FMT_VYUY8_2X8:
	570	+ if (output_fmt->fourcc == V4L2_PIX_FMT_NV16 \|\|
	571	+ output_fmt->fourcc == V4L2_PIX_FMT_NV61 \|\|
	572	+ output_fmt->fourcc == V4L2_PIX_FMT_NV12 \|\|
	573	+ output_fmt->fourcc == V4L2_PIX_FMT_NV21 \|\|
	574	+ output_fmt->fourcc == V4L2_PIX_FMT_YUYV \|\|
	575	+ output_fmt->fourcc == V4L2_PIX_FMT_YVYU \|\|
	576	+ output_fmt->fourcc == V4L2_PIX_FMT_UYVY \|\|
	577	+ output_fmt->fourcc == V4L2_PIX_FMT_VYUY)
	578	+ ret = 0;
	579	+ break;
	580	+ case MEDIA_BUS_FMT_SBGGR8_1X8:
	581	+ case MEDIA_BUS_FMT_SGBRG8_1X8:
	582	+ case MEDIA_BUS_FMT_SGRBG8_1X8:
	583	+ case MEDIA_BUS_FMT_SRGGB8_1X8:
	584	+ case MEDIA_BUS_FMT_Y8_1X8:
	585	+ if (output_fmt->fourcc == V4L2_PIX_FMT_SRGGB8 \|\|
	586	+ output_fmt->fourcc == V4L2_PIX_FMT_SGRBG8 \|\|
	587	+ output_fmt->fourcc == V4L2_PIX_FMT_SGBRG8 \|\|
	588	+ output_fmt->fourcc == V4L2_PIX_FMT_SBGGR8 \|\|
	589	+ output_fmt->fourcc == V4L2_PIX_FMT_GREY)
	590	+ ret = 0;
	591	+ break;
	592	+ case MEDIA_BUS_FMT_SBGGR10_1X10:
	593	+ case MEDIA_BUS_FMT_SGBRG10_1X10:
	594	+ case MEDIA_BUS_FMT_SGRBG10_1X10:
	595	+ case MEDIA_BUS_FMT_SRGGB10_1X10:
	596	+ case MEDIA_BUS_FMT_Y10_1X10:
	597	+ if (output_fmt->fourcc == V4L2_PIX_FMT_SRGGB10 \|\|
	598	+ output_fmt->fourcc == V4L2_PIX_FMT_SGRBG10 \|\|
	599	+ output_fmt->fourcc == V4L2_PIX_FMT_SGBRG10 \|\|
	600	+ output_fmt->fourcc == V4L2_PIX_FMT_SBGGR10 \|\|
	601	+ output_fmt->fourcc == V4L2_PIX_FMT_Y10)
	602	+ ret = 0;
	603	+ break;
	604	+ case MEDIA_BUS_FMT_SBGGR12_1X12:
	605	+ case MEDIA_BUS_FMT_SGBRG12_1X12:
	606	+ case MEDIA_BUS_FMT_SGRBG12_1X12:
	607	+ case MEDIA_BUS_FMT_SRGGB12_1X12:
	608	+ case MEDIA_BUS_FMT_Y12_1X12:
	609	+ if (output_fmt->fourcc == V4L2_PIX_FMT_SRGGB12 \|\|
	610	+ output_fmt->fourcc == V4L2_PIX_FMT_SGRBG12 \|\|
	611	+ output_fmt->fourcc == V4L2_PIX_FMT_SGBRG12 \|\|
	612	+ output_fmt->fourcc == V4L2_PIX_FMT_SBGGR12 \|\|
	613	+ output_fmt->fourcc == V4L2_PIX_FMT_Y12)
	614	+ ret = 0;
	615	+ break;
	616	+ case MEDIA_BUS_FMT_RGB888_1X24:
	617	+ case MEDIA_BUS_FMT_BGR888_1X24:
	618	+ case MEDIA_BUS_FMT_GBR888_1X24:
	619	+ if (output_fmt->fourcc == V4L2_PIX_FMT_RGB24 \|\|
	620	+ output_fmt->fourcc == V4L2_PIX_FMT_BGR24)
	621	+ ret = 0;
	622	+ break;
	623	+ case MEDIA_BUS_FMT_RGB565_1X16:
	624	+ if (output_fmt->fourcc == V4L2_PIX_FMT_RGB565)
	625	+ ret = 0;
	626	+ break;
	627	+ case MEDIA_BUS_FMT_EBD_1X8:
	628	+ if (output_fmt->fourcc == V4l2_PIX_FMT_EBD8 \|\|
	629	+ (channel->data_bit == 8 &&
	630	+ (output_fmt->fourcc == V4L2_PIX_FMT_SRGGB8 \|\|
	631	+ output_fmt->fourcc == V4L2_PIX_FMT_SGRBG8 \|\|
	632	+ output_fmt->fourcc == V4L2_PIX_FMT_SGBRG8 \|\|
	633	+ output_fmt->fourcc == V4L2_PIX_FMT_SBGGR8)) \|\|
	634	+ (channel->data_bit == 10 &&
	635	+ (output_fmt->fourcc == V4L2_PIX_FMT_SRGGB10 \|\|
	636	+ output_fmt->fourcc == V4L2_PIX_FMT_SGRBG10 \|\|
	637	+ output_fmt->fourcc == V4L2_PIX_FMT_SGBRG10 \|\|
	638	+ output_fmt->fourcc == V4L2_PIX_FMT_SBGGR10)) \|\|
	639	+ (channel->data_bit == 12 &&
	640	+ (output_fmt->fourcc == V4L2_PIX_FMT_SRGGB12 \|\|
	641	+ output_fmt->fourcc == V4L2_PIX_FMT_SGRBG12 \|\|
	642	+ output_fmt->fourcc == V4L2_PIX_FMT_SGBRG12 \|\|
	643	+ output_fmt->fourcc == V4L2_PIX_FMT_SBGGR12)) \|\|
	644	+ (channel->data_bit == 16 &&
	645	+ (output_fmt->fourcc == V4L2_PIX_FMT_SRGGB16 \|\|
	646	+ output_fmt->fourcc == V4L2_PIX_FMT_SGRBG16 \|\|
	647	+ output_fmt->fourcc == V4L2_PIX_FMT_SGBRG16 \|\|
	648	+ output_fmt->fourcc == V4L2_PIX_FMT_SBGGR16)))
	649	+ ret = 0;
	650	+ break;
	651	+ case MEDIA_BUS_FMT_SPD_2X8:
	652	+ if (output_fmt->fourcc == V4l2_PIX_FMT_SPD16 \|\|
	653	+ (channel->data_bit == 8 &&
	654	+ (output_fmt->fourcc == V4L2_PIX_FMT_SRGGB8 \|\|
	655	+ output_fmt->fourcc == V4L2_PIX_FMT_SGRBG8 \|\|
	656	+ output_fmt->fourcc == V4L2_PIX_FMT_SGBRG8 \|\|
	657	+ output_fmt->fourcc == V4L2_PIX_FMT_SBGGR8)) \|\|
	658	+ (channel->data_bit == 10 &&
	659	+ (output_fmt->fourcc == V4L2_PIX_FMT_SRGGB10 \|\|
	660	+ output_fmt->fourcc == V4L2_PIX_FMT_SGRBG10 \|\|
	661	+ output_fmt->fourcc == V4L2_PIX_FMT_SGBRG10 \|\|
	662	+ output_fmt->fourcc == V4L2_PIX_FMT_SBGGR10)) \|\|
	663	+ (channel->data_bit == 12 &&
	664	+ (output_fmt->fourcc == V4L2_PIX_FMT_SRGGB12 \|\|
	665	+ output_fmt->fourcc == V4L2_PIX_FMT_SGRBG12 \|\|
	666	+ output_fmt->fourcc == V4L2_PIX_FMT_SGBRG12 \|\|
	667	+ output_fmt->fourcc == V4L2_PIX_FMT_SBGGR12)) \|\|
	668	+ (channel->data_bit == 16 &&
	669	+ (output_fmt->fourcc == V4L2_PIX_FMT_SRGGB16 \|\|
	670	+ output_fmt->fourcc == V4L2_PIX_FMT_SGRBG16 \|\|
	671	+ output_fmt->fourcc == V4L2_PIX_FMT_SGBRG16 \|\|
	672	+ output_fmt->fourcc == V4L2_PIX_FMT_SBGGR16)))
	673	+ ret = 0;
	674	+ break;
	675	+ default:
	676	+ break;
	677	+ }
	678	+ if (ret)
	679	+ v4l2_dbg(4, rkcif_debug, &stream->cifdev->v4l2_dev,
	680	+ "input mbus_code 0x%x, can't transform to %c%c%c%c\n",
	681	+ input_fmt->mbus_code,
	682	+ output_fmt->fourcc & 0xff,
	683	+ (output_fmt->fourcc >> 8) & 0xff,
	684	+ (output_fmt->fourcc >> 16) & 0xff,
	685	+ (output_fmt->fourcc >> 24) & 0xff);
	686	+ return ret;
	687	+}
	688	+
	689	+static int rkcif_stop_dma_capture(struct rkcif_stream *stream);
	690	+
	691	+struct rkcif_rx_buffer to_cif_rx_buf(struct rkisp_rx_buf dbufs)
	692	+{
	693	+ return container_of(dbufs, struct rkcif_rx_buffer, dbufs);
	694	+}
466	695
467	696	static struct v4l2_subdev get_remote_sensor(struct rkcif_stream stream, u16 *index)
468	697	{
..	..	@@ -575,7 +804,9 @@
575	804	case MEDIA_BUS_FMT_YUYV8_2X8:
576	805	case MEDIA_BUS_FMT_YVYU8_2X8:
577	806	return 0x1e;
578		- case MEDIA_BUS_FMT_RGB888_1X24: {
	807	+ case MEDIA_BUS_FMT_RGB888_1X24:
	808	+ case MEDIA_BUS_FMT_BGR888_1X24:
	809	+ case MEDIA_BUS_FMT_GBR888_1X24:
579	810	if (dsi_input) {
580	811	if (cmd_mode_en) /* dsi command mode*/
581	812	return 0x39;
..	..	@@ -584,7 +815,15 @@
584	815	} else {
585	816	return 0x24;
586	817	}
587		- }
	818	+ case MEDIA_BUS_FMT_RGB565_1X16:
	819	+ if (dsi_input) {
	820	+ if (cmd_mode_en) /* dsi command mode*/
	821	+ return 0x39;
	822	+ else /* dsi video mode */
	823	+ return 0x0e;
	824	+ } else {
	825	+ return 0x22;
	826	+ }
588	827	case MEDIA_BUS_FMT_EBD_1X8:
589	828	return 0x12;
590	829	case MEDIA_BUS_FMT_SPD_2X8:
..	..	@@ -600,6 +839,8 @@
600	839	switch (fmt_in->csi_fmt_val) {
601	840	case CSI_WRDDR_TYPE_RGB888:
602	841	return 24;
	842	+ case CSI_WRDDR_TYPE_RGB565:
	843	+ return 16;
603	844	case CSI_WRDDR_TYPE_RAW10:
604	845	case CSI_WRDDR_TYPE_RAW12:
605	846	return 4;
..	..	@@ -611,51 +852,61 @@
611	852	}
612	853	}
613	854
614		-static const struct
615		-cif_input_fmt get_input_fmt(struct v4l2_subdev sd, struct v4l2_rect *rect,
616		- u32 pad, int *vc)
	855	+const struct
	856	+cif_input_fmt rkcif_get_input_fmt(struct rkcif_device dev, struct v4l2_rect *rect,
	857	+ u32 pad_id, struct csi_channel_info *csi_info)
617	858	{
618	859	struct v4l2_subdev_format fmt;
	860	+ struct v4l2_subdev *sd = dev->terminal_sensor.sd;
	861	+ struct rkmodule_channel_info ch_info = {0};
	862	+ struct rkmodule_capture_info capture_info;
619	863	int ret;
620	864	u32 i;
621		- struct rkmodule_vc_fmt_info vc_info = {0};
622	865
623		- ret = v4l2_subdev_call(sd,
624		- core, ioctl,
625		- RKMODULE_GET_VC_FMT_INFO,
626		- &vc_info);
627		- if (ret < 0)
628		- v4l2_warn(sd->v4l2_dev,
629		- "get sensor vc info failed, maybe not support\n");
630		-
631		- fmt.pad = pad;
	866	+ fmt.pad = 0;
632	867	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
633	868	fmt.reserved[0] = 0;
	869	+ fmt.format.field = V4L2_FIELD_NONE;
634	870	ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
635	871	if (ret < 0) {
636	872	v4l2_warn(sd->v4l2_dev,
637	873	"sensor fmt invalid, set to default size\n");
638	874	goto set_default;
639	875	}
640		-
641		- /* v4l2_subdev_format reserved[0]
642		- * using as mipi virtual channel
643		- */
644		- switch (fmt.reserved[0]) {
645		- case V4L2_MBUS_CSI2_CHANNEL_3:
646		- *vc = 3;
647		- break;
648		- case V4L2_MBUS_CSI2_CHANNEL_2:
649		- *vc = 2;
650		- break;
651		- case V4L2_MBUS_CSI2_CHANNEL_1:
652		- *vc = 1;
653		- break;
654		- case V4L2_MBUS_CSI2_CHANNEL_0:
655		- *vc = 0;
656		- break;
657		- default:
658		- *vc = -1;
	876	+ ch_info.index = pad_id;
	877	+ ret = v4l2_subdev_call(sd,
	878	+ core, ioctl,
	879	+ RKMODULE_GET_CHANNEL_INFO,
	880	+ &ch_info);
	881	+ if (!ret) {
	882	+ fmt.format.width = ch_info.width;
	883	+ fmt.format.height = ch_info.height;
	884	+ fmt.format.code = ch_info.bus_fmt;
	885	+ switch (ch_info.vc) {
	886	+ case V4L2_MBUS_CSI2_CHANNEL_3:
	887	+ csi_info->vc = 3;
	888	+ break;
	889	+ case V4L2_MBUS_CSI2_CHANNEL_2:
	890	+ csi_info->vc = 2;
	891	+ break;
	892	+ case V4L2_MBUS_CSI2_CHANNEL_1:
	893	+ csi_info->vc = 1;
	894	+ break;
	895	+ case V4L2_MBUS_CSI2_CHANNEL_0:
	896	+ csi_info->vc = 0;
	897	+ break;
	898	+ default:
	899	+ csi_info->vc = 0xff;
	900	+ }
	901	+ if (ch_info.bus_fmt == MEDIA_BUS_FMT_SPD_2X8 \|\|
	902	+ ch_info.bus_fmt == MEDIA_BUS_FMT_EBD_1X8) {
	903	+ if (ch_info.data_type > 0)
	904	+ csi_info->data_type = ch_info.data_type;
	905	+ if (ch_info.data_bit > 0)
	906	+ csi_info->data_bit = ch_info.data_bit;
	907	+ }
	908	+ } else {
	909	+ csi_info->vc = 0xff;
659	910	}
660	911
661	912	v4l2_dbg(1, rkcif_debug, sd->v4l2_dev,
..	..	@@ -666,7 +917,26 @@
666	917	rect->top = 0;
667	918	rect->width = fmt.format.width;
668	919	rect->height = fmt.format.height;
669		-
	920	+ ret = v4l2_subdev_call(sd,
	921	+ core, ioctl,
	922	+ RKMODULE_GET_CAPTURE_MODE,
	923	+ &capture_info);
	924	+ if (!ret) {
	925	+ if (capture_info.mode == RKMODULE_MULTI_DEV_COMBINE_ONE &&
	926	+ dev->hw_dev->is_rk3588s2) {
	927	+ for (i = 0; i < capture_info.multi_dev.dev_num; i++) {
	928	+ if (capture_info.multi_dev.dev_idx[i] == 0)
	929	+ capture_info.multi_dev.dev_idx[i] = 2;
	930	+ else if (capture_info.multi_dev.dev_idx[i] == 2)
	931	+ capture_info.multi_dev.dev_idx[i] = 4;
	932	+ else if (capture_info.multi_dev.dev_idx[i] == 3)
	933	+ capture_info.multi_dev.dev_idx[i] = 5;
	934	+ }
	935	+ }
	936	+ csi_info->capture_info = capture_info;
	937	+ } else {
	938	+ csi_info->capture_info.mode = RKMODULE_CAPTURE_MODE_NONE;
	939	+ }
670	940	for (i = 0; i < ARRAY_SIZE(in_fmts); i++)
671	941	if (fmt.format.code == in_fmts[i].mbus_code &&
672	942	fmt.format.field == in_fmts[i].field)
..	..	@@ -683,8 +953,8 @@
683	953	return NULL;
684	954	}
685	955
686		-static const struct
687		-cif_output_fmt find_output_fmt(struct rkcif_stream stream, u32 pixelfmt)
	956	+const struct
	957	+cif_output_fmt rkcif_find_output_fmt(struct rkcif_stream stream, u32 pixelfmt)
688	958	{
689	959	const struct cif_output_fmt *fmt;
690	960	u32 i;
..	..	@@ -717,6 +987,31 @@
717	987	break;
718	988	default:
719	989	index = CIF_REG_MIPI_LVDS_ID0_CTRL0;
	990	+ break;
	991	+ }
	992	+
	993	+ return index;
	994	+}
	995	+
	996	+static enum cif_reg_index get_reg_index_of_lvds_id_ctrl0(int channel_id)
	997	+{
	998	+ enum cif_reg_index index;
	999	+
	1000	+ switch (channel_id) {
	1001	+ case 0:
	1002	+ index = CIF_REG_LVDS_ID0_CTRL0;
	1003	+ break;
	1004	+ case 1:
	1005	+ index = CIF_REG_LVDS_ID1_CTRL0;
	1006	+ break;
	1007	+ case 2:
	1008	+ index = CIF_REG_LVDS_ID2_CTRL0;
	1009	+ break;
	1010	+ case 3:
	1011	+ index = CIF_REG_LVDS_ID3_CTRL0;
	1012	+ break;
	1013	+ default:
	1014	+ index = CIF_REG_LVDS_ID0_CTRL0;
720	1015	break;
721	1016	}
722	1017
..	..	@@ -1173,12 +1468,50 @@
1173	1468	return index;
1174	1469	}
1175	1470
	1471	+int rkcif_get_linetime(struct rkcif_stream *stream)
	1472	+{
	1473	+ struct rkcif_device *cif_dev = stream->cifdev;
	1474	+ struct rkcif_sensor_info *sensor = &cif_dev->terminal_sensor;
	1475	+ u32 numerator, denominator;
	1476	+ u32 def_fps = 0;
	1477	+ int line_time = 0;
	1478	+ int vblank_def = 0;
	1479	+ int vblank_curr = 0;
	1480	+
	1481	+ numerator = sensor->fi.interval.numerator;
	1482	+ denominator = sensor->fi.interval.denominator;
	1483	+ if (!numerator \|\| !denominator) {
	1484	+ v4l2_err(&cif_dev->v4l2_dev,
	1485	+ "get frame interval fail, numerator %d, denominator %d\n",
	1486	+ numerator, denominator);
	1487	+ return -EINVAL;
	1488	+ }
	1489	+ def_fps = denominator / numerator;
	1490	+ if (!def_fps) {
	1491	+ v4l2_err(&cif_dev->v4l2_dev,
	1492	+ "get fps fail, numerator %d, denominator %d\n",
	1493	+ numerator, denominator);
	1494	+ return -EINVAL;
	1495	+ }
	1496	+ vblank_def = rkcif_get_sensor_vblank_def(cif_dev);
	1497	+ vblank_curr = rkcif_get_sensor_vblank(cif_dev);
	1498	+ if (!vblank_def \|\| !vblank_curr) {
	1499	+ v4l2_err(&cif_dev->v4l2_dev,
	1500	+ "get vblank fail, vblank_def %d, vblank_curr %d\n",
	1501	+ vblank_def, vblank_curr);
	1502	+ return -EINVAL;
	1503	+ }
	1504	+ line_time = div_u64(1000000000, def_fps);
	1505	+ line_time = div_u64(line_time, vblank_def + sensor->raw_rect.height);
	1506	+ return line_time;
	1507	+}
	1508	+
1176	1509	/*************************** stream operations ****************************/
1177	1510	static int rkcif_assign_new_buffer_oneframe(struct rkcif_stream *stream,
1178		- enum rkcif_yuvaddr_state stat)
	1511	+ enum rkcif_yuvaddr_state stat)
1179	1512	{
1180	1513	struct rkcif_device *dev = stream->cifdev;
1181		- struct rkcif_dummy_buffer *dummy_buf = &dev->dummy_buf;
	1514	+ struct rkcif_dummy_buffer *dummy_buf = &dev->hw_dev->dummy_buf;
1182	1515	struct rkcif_buffer *buffer = NULL;
1183	1516	u32 frm_addr_y = CIF_REG_DVP_FRM0_ADDR_Y;
1184	1517	u32 frm_addr_uv = CIF_REG_DVP_FRM0_ADDR_UV;
..	..	@@ -1273,12 +1606,738 @@
1273	1606	} else {
1274	1607	ret = -EINVAL;
1275	1608	}
1276		- v4l2_dbg(1, rkcif_debug, &dev->v4l2_dev,
1277		- "not active buffer, frame Drop\n");
	1609	+ dev->err_state \|= (RKCIF_ERR_ID0_NOT_BUF << stream->id);
	1610	+ dev->irq_stats.not_active_buf_cnt[stream->id]++;
1278	1611	}
1279	1612	}
1280	1613	spin_unlock_irqrestore(&stream->vbq_lock, flags);
1281	1614	return ret;
	1615	+}
	1616	+
	1617	+static struct v4l2_subdev get_rkisp_sd(struct sditf_priv priv)
	1618	+{
	1619	+ struct media_pad *pad = NULL;
	1620	+
	1621	+ if (priv && priv->pads[0].entity->num_links) {
	1622	+ if (priv->is_combine_mode)
	1623	+ pad = media_entity_remote_pad(&priv->pads[1]);
	1624	+ else
	1625	+ pad = media_entity_remote_pad(&priv->pads[0]);
	1626	+ if (pad)
	1627	+ return media_entity_to_v4l2_subdev(pad->entity);
	1628	+ }
	1629	+ return NULL;
	1630	+}
	1631	+
	1632	+static void rkcif_rx_buffer_free(struct rkcif_stream *stream)
	1633	+{
	1634	+ struct v4l2_subdev *sd;
	1635	+ struct rkisp_rx_buf *dbufs;
	1636	+ struct rkcif_device *dev = stream->cifdev;
	1637	+
	1638	+ sd = get_rkisp_sd(dev->sditf[0]);
	1639	+ if (!sd)
	1640	+ return;
	1641	+
	1642	+ while (!list_empty(&stream->rx_buf_head_vicap)) {
	1643	+ dbufs = list_first_entry(&stream->rx_buf_head_vicap, struct rkisp_rx_buf, list);
	1644	+ if (dbufs->is_init)
	1645	+ v4l2_subdev_call(sd, core, ioctl,
	1646	+ RKISP_VICAP_CMD_RX_BUFFER_FREE, dbufs);
	1647	+ dma_buf_put(dbufs->dbuf);
	1648	+ list_del(&dbufs->list);
	1649	+ kfree(dbufs);
	1650	+ }
	1651	+}
	1652	+
	1653	+static void rkcif_s_rx_buffer(struct rkcif_stream stream, struct rkisp_rx_buf dbufs)
	1654	+{
	1655	+ struct rkcif_device *dev = stream->cifdev;
	1656	+ struct v4l2_subdev *sd;
	1657	+ struct rkcif_rx_buffer *rx_buf = NULL;
	1658	+
	1659	+ sd = get_rkisp_sd(dev->sditf[0]);
	1660	+ if (!sd)
	1661	+ return;
	1662	+ if ((dev->rdbk_debug &&
	1663	+ dbufs->sequence < 15) \|\|
	1664	+ rkcif_debug == 3) {
	1665	+ rx_buf = to_cif_rx_buf(dbufs);
	1666	+ v4l2_info(&dev->v4l2_dev,
	1667	+ "s_buf seq %d type %d, dma addr %x, %lld\n",
	1668	+ dbufs->sequence, dbufs->type, (u32)rx_buf->dummy.dma_addr,
	1669	+ rkcif_time_get_ns(dev));
	1670	+ }
	1671	+ v4l2_subdev_call(sd, video, s_rx_buffer, dbufs, NULL);
	1672	+}
	1673	+
	1674	+static void rkcif_enable_skip_frame(struct rkcif_stream *stream, int cap_m, int skip_n)
	1675	+{
	1676	+ struct rkcif_device *dev = stream->cifdev;
	1677	+ u32 val = 0;
	1678	+
	1679	+ val = rkcif_read_register(dev, CIF_REG_MIPI_LVDS_CTRL);
	1680	+ val &= 0xc00fffff;
	1681	+ val \|= cap_m << RKCIF_CAP_SHIFT \| skip_n << RKCIF_SKIP_SHIFT \| RKCIF_SKIP_EN(stream->id);
	1682	+ rkcif_write_register(dev, CIF_REG_MIPI_LVDS_CTRL, val);
	1683	+ stream->skip_info.skip_en = true;
	1684	+}
	1685	+
	1686	+static void rkcif_disable_skip_frame(struct rkcif_stream *stream)
	1687	+{ struct rkcif_device *dev = stream->cifdev;
	1688	+ u32 val = 0;
	1689	+
	1690	+ val = rkcif_read_register(dev, CIF_REG_MIPI_LVDS_CTRL);
	1691	+ val &= ~(RKCIF_SKIP_EN(stream->id));
	1692	+ rkcif_write_register(dev, CIF_REG_MIPI_LVDS_CTRL, val);
	1693	+ stream->skip_info.skip_en = false;
	1694	+}
	1695	+
	1696	+static void rkcif_rdbk_with_tools(struct rkcif_stream *stream,
	1697	+ struct rkcif_rx_buffer *active_buf)
	1698	+{
	1699	+ unsigned long flags;
	1700	+
	1701	+ spin_lock_irqsave(&stream->tools_vdev->vbq_lock, flags);
	1702	+ if (stream->tools_vdev->state == RKCIF_STATE_STREAMING) {
	1703	+ list_add_tail(&active_buf->list, &stream->tools_vdev->buf_done_head);
	1704	+ if (!work_busy(&stream->tools_vdev->work))
	1705	+ schedule_work(&stream->tools_vdev->work);
	1706	+ }
	1707	+ spin_unlock_irqrestore(&stream->tools_vdev->vbq_lock, flags);
	1708	+}
	1709	+
	1710	+static void rkcif_rdbk_frame_end_toisp(struct rkcif_stream *stream,
	1711	+ struct rkcif_rx_buffer *buffer)
	1712	+{
	1713	+ struct rkcif_device *dev = stream->cifdev;
	1714	+ struct rkcif_sensor_info *sensor = &stream->cifdev->terminal_sensor;
	1715	+ u32 denominator, numerator;
	1716	+ u64 l_ts, m_ts, s_ts, time = 30000000LL;
	1717	+ int ret, fps = -1;
	1718	+ unsigned long flags;
	1719	+
	1720	+ spin_lock_irqsave(&dev->hdr_lock, flags);
	1721	+ if (dev->rdbk_rx_buf[stream->id]) {
	1722	+ list_add_tail(&dev->rdbk_rx_buf[stream->id]->list, &stream->rx_buf_head);
	1723	+ dev->rdbk_rx_buf[stream->id] = buffer;
	1724	+ } else {
	1725	+ dev->rdbk_rx_buf[stream->id] = buffer;
	1726	+ }
	1727	+
	1728	+ numerator = sensor->fi.interval.numerator;
	1729	+ denominator = sensor->fi.interval.denominator;
	1730	+ if (denominator && numerator)
	1731	+ time = numerator * 1000 / denominator * 1000 * 1000;
	1732	+
	1733	+ if (dev->hdr.hdr_mode == HDR_X3 &&
	1734	+ dev->rdbk_rx_buf[RDBK_L] &&
	1735	+ dev->rdbk_rx_buf[RDBK_M] &&
	1736	+ dev->rdbk_rx_buf[RDBK_S]) {
	1737	+ l_ts = dev->rdbk_rx_buf[RDBK_L]->fe_timestamp;
	1738	+ m_ts = dev->rdbk_rx_buf[RDBK_M]->fe_timestamp;
	1739	+ s_ts = dev->rdbk_rx_buf[RDBK_S]->fe_timestamp;
	1740	+
	1741	+ if (m_ts < l_ts \|\| s_ts < m_ts) {
	1742	+ v4l2_err(&dev->v4l2_dev,
	1743	+ "s/m/l frame err, timestamp s:%lld m:%lld l:%lld\n",
	1744	+ s_ts, m_ts, l_ts);
	1745	+ goto RDBK_TOISP_UNMATCH;
	1746	+ }
	1747	+
	1748	+ if ((m_ts - l_ts) > time \|\| (s_ts - m_ts) > time) {
	1749	+ ret = v4l2_subdev_call(sensor->sd,
	1750	+ video,
	1751	+ g_frame_interval,
	1752	+ &sensor->fi);
	1753	+ if (!ret) {
	1754	+ denominator = sensor->fi.interval.denominator;
	1755	+ numerator = sensor->fi.interval.numerator;
	1756	+ if (denominator && numerator) {
	1757	+ time = numerator * 1000 / denominator * 1000 * 1000;
	1758	+ fps = denominator / numerator;
	1759	+ }
	1760	+ }
	1761	+
	1762	+ if ((m_ts - l_ts) > time \|\| (s_ts - m_ts) > time) {
	1763	+ v4l2_err(&dev->v4l2_dev,
	1764	+ "timestamp no match, s:%lld m:%lld l:%lld, fps:%d\n",
	1765	+ s_ts, m_ts, l_ts, fps);
	1766	+ goto RDBK_TOISP_UNMATCH;
	1767	+ }
	1768	+ }
	1769	+ dev->rdbk_rx_buf[RDBK_M]->dbufs.sequence = dev->rdbk_rx_buf[RDBK_L]->dbufs.sequence;
	1770	+ dev->rdbk_rx_buf[RDBK_S]->dbufs.sequence = dev->rdbk_rx_buf[RDBK_L]->dbufs.sequence;
	1771	+ rkcif_s_rx_buffer(&dev->stream[RDBK_L], &dev->rdbk_rx_buf[RDBK_L]->dbufs);
	1772	+ rkcif_s_rx_buffer(&dev->stream[RDBK_M], &dev->rdbk_rx_buf[RDBK_M]->dbufs);
	1773	+ rkcif_s_rx_buffer(&dev->stream[RDBK_S], &dev->rdbk_rx_buf[RDBK_S]->dbufs);
	1774	+ rkcif_rdbk_with_tools(&dev->stream[RDBK_L], dev->rdbk_rx_buf[RDBK_L]);
	1775	+ rkcif_rdbk_with_tools(&dev->stream[RDBK_M], dev->rdbk_rx_buf[RDBK_M]);
	1776	+ rkcif_rdbk_with_tools(&dev->stream[RDBK_S], dev->rdbk_rx_buf[RDBK_S]);
	1777	+ atomic_dec(&dev->stream[RDBK_L].buf_cnt);
	1778	+ atomic_dec(&dev->stream[RDBK_M].buf_cnt);
	1779	+ atomic_dec(&dev->stream[RDBK_S].buf_cnt);
	1780	+ dev->rdbk_rx_buf[RDBK_L] = NULL;
	1781	+ dev->rdbk_rx_buf[RDBK_M] = NULL;
	1782	+ dev->rdbk_rx_buf[RDBK_S] = NULL;
	1783	+ } else if (dev->hdr.hdr_mode == HDR_X2 &&
	1784	+ dev->rdbk_rx_buf[RDBK_L] && dev->rdbk_rx_buf[RDBK_M]) {
	1785	+ l_ts = dev->rdbk_rx_buf[RDBK_L]->fe_timestamp;
	1786	+ s_ts = dev->rdbk_rx_buf[RDBK_M]->fe_timestamp;
	1787	+
	1788	+ if (s_ts < l_ts) {
	1789	+ v4l2_err(&dev->v4l2_dev,
	1790	+ "s/l frame err, timestamp s:%lld l:%lld\n",
	1791	+ s_ts, l_ts);
	1792	+ goto RDBK_TOISP_UNMATCH;
	1793	+ }
	1794	+
	1795	+ if ((s_ts - l_ts) > time) {
	1796	+ ret = v4l2_subdev_call(sensor->sd,
	1797	+ video,
	1798	+ g_frame_interval,
	1799	+ &sensor->fi);
	1800	+ if (!ret) {
	1801	+ denominator = sensor->fi.interval.denominator;
	1802	+ numerator = sensor->fi.interval.numerator;
	1803	+ if (denominator && numerator) {
	1804	+ time = numerator * 1000 / denominator * 1000 * 1000;
	1805	+ fps = denominator / numerator;
	1806	+ }
	1807	+ }
	1808	+ if ((s_ts - l_ts) > time) {
	1809	+ v4l2_err(&dev->v4l2_dev,
	1810	+ "timestamp no match, s:%lld l:%lld, fps:%d\n",
	1811	+ s_ts, l_ts, fps);
	1812	+ goto RDBK_TOISP_UNMATCH;
	1813	+ }
	1814	+ }
	1815	+ dev->rdbk_rx_buf[RDBK_M]->dbufs.sequence = dev->rdbk_rx_buf[RDBK_L]->dbufs.sequence;
	1816	+ rkcif_s_rx_buffer(&dev->stream[RDBK_L], &dev->rdbk_rx_buf[RDBK_L]->dbufs);
	1817	+ rkcif_s_rx_buffer(&dev->stream[RDBK_M], &dev->rdbk_rx_buf[RDBK_M]->dbufs);
	1818	+ rkcif_rdbk_with_tools(&dev->stream[RDBK_L], dev->rdbk_rx_buf[RDBK_L]);
	1819	+ rkcif_rdbk_with_tools(&dev->stream[RDBK_M], dev->rdbk_rx_buf[RDBK_M]);
	1820	+ atomic_dec(&dev->stream[RDBK_L].buf_cnt);
	1821	+ atomic_dec(&dev->stream[RDBK_M].buf_cnt);
	1822	+ dev->rdbk_rx_buf[RDBK_L] = NULL;
	1823	+ dev->rdbk_rx_buf[RDBK_M] = NULL;
	1824	+ }
	1825	+
	1826	+ spin_unlock_irqrestore(&dev->hdr_lock, flags);
	1827	+ return;
	1828	+
	1829	+RDBK_TOISP_UNMATCH:
	1830	+ spin_unlock_irqrestore(&dev->hdr_lock, flags);
	1831	+}
	1832	+
	1833	+static void rkcif_write_buff_addr_multi_dev_combine(struct rkcif_stream *stream,
	1834	+ u32 frm_addr_y, u32 frm_addr_uv,
	1835	+ u32 buff_addr_y, u32 buff_addr_cbcr,
	1836	+ bool is_dummy_buf)
	1837	+{
	1838	+ struct rkcif_device *dev = stream->cifdev;
	1839	+ struct rkmodule_capture_info *capture_info = &dev->channels[stream->id].capture_info;
	1840	+ u32 addr_y, addr_cbcr;
	1841	+ int addr_offset = 0;
	1842	+ int i = 0;
	1843	+ int tmp_host_index = dev->csi_host_idx;
	1844	+
	1845	+ for (i = 0; i < capture_info->multi_dev.dev_num; i++) {
	1846	+ if (is_dummy_buf) {
	1847	+ addr_y = buff_addr_y;
	1848	+ } else {
	1849	+ addr_offset = dev->channels[stream->id].left_virtual_width;
	1850	+ addr_y = buff_addr_y + addr_offset * i;
	1851	+ }
	1852	+ dev->csi_host_idx = capture_info->multi_dev.dev_idx[i];
	1853	+ rkcif_write_register(dev, frm_addr_y, addr_y);
	1854	+ if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW &&
	1855	+ frm_addr_uv && buff_addr_cbcr) {
	1856	+ if (is_dummy_buf) {
	1857	+ addr_cbcr = buff_addr_cbcr;
	1858	+ } else {
	1859	+ addr_offset = dev->channels[stream->id].left_virtual_width;
	1860	+ addr_cbcr = buff_addr_cbcr + addr_offset * i;
	1861	+ }
	1862	+ rkcif_write_register(dev, frm_addr_uv, addr_cbcr);
	1863	+ }
	1864	+ }
	1865	+ dev->csi_host_idx = tmp_host_index;
	1866	+}
	1867	+
	1868	+static void rkcif_assign_new_buffer_init_toisp(struct rkcif_stream *stream,
	1869	+ int channel_id)
	1870	+{
	1871	+ struct rkcif_device *dev = stream->cifdev;
	1872	+ struct rkcif_rx_buffer *rx_buf;
	1873	+ struct v4l2_mbus_config *mbus_cfg = &dev->active_sensor->mbus;
	1874	+ struct rkmodule_capture_info *capture_info = &dev->channels[channel_id].capture_info;
	1875	+ u32 frm0_addr_y;
	1876	+ u32 frm1_addr_y;
	1877	+ u32 buff_addr_y;
	1878	+ unsigned long flags;
	1879	+
	1880	+ if (mbus_cfg->type == V4L2_MBUS_CSI2_DPHY \|\|
	1881	+ mbus_cfg->type == V4L2_MBUS_CSI2_CPHY \|\|
	1882	+ mbus_cfg->type == V4L2_MBUS_CCP2) {
	1883	+ frm0_addr_y = get_reg_index_of_frm0_y_addr(channel_id);
	1884	+ frm1_addr_y = get_reg_index_of_frm1_y_addr(channel_id);
	1885	+ } else {
	1886	+ frm0_addr_y = get_dvp_reg_index_of_frm0_y_addr(channel_id);
	1887	+ frm1_addr_y = get_dvp_reg_index_of_frm1_y_addr(channel_id);
	1888	+ }
	1889	+
	1890	+ spin_lock_irqsave(&stream->vbq_lock, flags);
	1891	+
	1892	+ if (!stream->curr_buf_toisp) {
	1893	+ if (!list_empty(&stream->rx_buf_head)) {
	1894	+ rx_buf = list_first_entry(&stream->rx_buf_head,
	1895	+ struct rkcif_rx_buffer,
	1896	+ list);
	1897	+ if (rx_buf) {
	1898	+ list_del(&rx_buf->list);
	1899	+ stream->curr_buf_toisp = rx_buf;
	1900	+ }
	1901	+ }
	1902	+ }
	1903	+
	1904	+ if (stream->curr_buf_toisp) {
	1905	+ buff_addr_y = stream->curr_buf_toisp->dummy.dma_addr;
	1906	+ if (capture_info->mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	1907	+ rkcif_write_buff_addr_multi_dev_combine(stream, frm0_addr_y, 0,
	1908	+ buff_addr_y, 0, false);
	1909	+ } else {
	1910	+ rkcif_write_register(dev, frm0_addr_y, buff_addr_y);
	1911	+ }
	1912	+ } else {
	1913	+ if (stream->lack_buf_cnt < 2)
	1914	+ stream->lack_buf_cnt++;
	1915	+ }
	1916	+
	1917	+ if (!stream->next_buf_toisp) {
	1918	+ if (!list_empty(&stream->rx_buf_head)) {
	1919	+ rx_buf = list_first_entry(&stream->rx_buf_head,
	1920	+ struct rkcif_rx_buffer, list);
	1921	+ if (rx_buf) {
	1922	+ list_del(&rx_buf->list);
	1923	+ stream->next_buf_toisp = rx_buf;
	1924	+ } else {
	1925	+ stream->next_buf_toisp = stream->curr_buf_toisp;
	1926	+ }
	1927	+ } else {
	1928	+ stream->next_buf_toisp = stream->curr_buf_toisp;
	1929	+ }
	1930	+ }
	1931	+
	1932	+ if (stream->next_buf_toisp) {
	1933	+ buff_addr_y = stream->next_buf_toisp->dummy.dma_addr;
	1934	+ if (capture_info->mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	1935	+ rkcif_write_buff_addr_multi_dev_combine(stream, frm1_addr_y, 0,
	1936	+ buff_addr_y, 0, false);
	1937	+ } else {
	1938	+ rkcif_write_register(dev, frm1_addr_y, buff_addr_y);
	1939	+ }
	1940	+ } else {
	1941	+ if (stream->lack_buf_cnt < 2)
	1942	+ stream->lack_buf_cnt++;
	1943	+ }
	1944	+
	1945	+ spin_unlock_irqrestore(&stream->vbq_lock, flags);
	1946	+ stream->buf_owner = RKCIF_DMAEN_BY_ISP;
	1947	+}
	1948	+
	1949	+static void rkcif_dphy_quick_stream(struct rkcif_device *dev, int on)
	1950	+{
	1951	+ struct rkcif_pipeline *p = NULL;
	1952	+ int j = 0;
	1953	+
	1954	+ if (dev->active_sensor->mbus.type == V4L2_MBUS_CSI2_DPHY \|\|
	1955	+ dev->active_sensor->mbus.type == V4L2_MBUS_CSI2_CPHY \|\|
	1956	+ dev->active_sensor->mbus.type == V4L2_MBUS_CCP2) {
	1957	+ p = &dev->pipe;
	1958	+ for (j = 0; j < p->num_subdevs; j++) {
	1959	+ if (p->subdevs[j] != dev->terminal_sensor.sd &&
	1960	+ p->subdevs[j] != dev->active_sensor->sd) {
	1961	+ v4l2_subdev_call(p->subdevs[j], core, ioctl,
	1962	+ RKMODULE_SET_QUICK_STREAM, &on);
	1963	+ break;
	1964	+ }
	1965	+ }
	1966	+ }
	1967	+}
	1968	+
	1969	+static int rkcif_assign_new_buffer_update_toisp(struct rkcif_stream *stream,
	1970	+ int channel_id)
	1971	+{
	1972	+ struct rkcif_device *dev = stream->cifdev;
	1973	+ struct v4l2_mbus_config *mbus_cfg = &dev->active_sensor->mbus;
	1974	+ struct rkmodule_capture_info *capture_info = &dev->channels[channel_id].capture_info;
	1975	+ struct rkcif_rx_buffer *buffer = NULL;
	1976	+ struct rkcif_rx_buffer *active_buf = NULL;
	1977	+ struct sditf_priv *priv = dev->sditf[0];
	1978	+ u32 frm_addr_y, buff_addr_y;
	1979	+ unsigned long flags;
	1980	+
	1981	+
	1982	+ if (mbus_cfg->type == V4L2_MBUS_CSI2_DPHY \|\|
	1983	+ mbus_cfg->type == V4L2_MBUS_CSI2_CPHY \|\|
	1984	+ mbus_cfg->type == V4L2_MBUS_CCP2) {
	1985	+ frm_addr_y = stream->frame_phase & CIF_CSI_FRAME0_READY ?
	1986	+ get_reg_index_of_frm0_y_addr(channel_id) :
	1987	+ get_reg_index_of_frm1_y_addr(channel_id);
	1988	+ } else {
	1989	+ frm_addr_y = stream->frame_phase & CIF_CSI_FRAME0_READY ?
	1990	+ get_dvp_reg_index_of_frm0_y_addr(channel_id) :
	1991	+ get_dvp_reg_index_of_frm1_y_addr(channel_id);
	1992	+ }
	1993	+
	1994	+ spin_lock_irqsave(&stream->vbq_lock, flags);
	1995	+ if (stream->cur_skip_frame)
	1996	+ goto out_get_buf;
	1997	+ memset(&stream->toisp_buf_state, 0, sizeof(stream->toisp_buf_state));
	1998	+ if (!list_empty(&stream->rx_buf_head)) {
	1999	+ if (stream->curr_buf_toisp && stream->next_buf_toisp &&
	2000	+ stream->curr_buf_toisp != stream->next_buf_toisp)
	2001	+ stream->toisp_buf_state.state = RKCIF_TOISP_BUF_ROTATE;
	2002	+ else
	2003	+ stream->toisp_buf_state.state = RKCIF_TOISP_BUF_LOSS;
	2004	+ if (stream->frame_phase == CIF_CSI_FRAME0_READY) {
	2005	+ active_buf = stream->curr_buf_toisp;
	2006	+
	2007	+ buffer = list_first_entry(&stream->rx_buf_head,
	2008	+ struct rkcif_rx_buffer, list);
	2009	+ if (buffer) {
	2010	+ list_del(&buffer->list);
	2011	+ stream->curr_buf_toisp = buffer;
	2012	+ }
	2013	+ if (priv && priv->mode.rdbk_mode == RKISP_VICAP_RDBK_AUTO) {
	2014	+ if (!active_buf)
	2015	+ goto out_get_buf;
	2016	+ if (stream->frame_idx == 1)
	2017	+ active_buf->dbufs.is_first = true;
	2018	+ active_buf->dbufs.sequence = stream->frame_idx - 1;
	2019	+ active_buf->dbufs.timestamp = stream->readout.fs_timestamp;
	2020	+ active_buf->fe_timestamp = rkcif_time_get_ns(dev);
	2021	+ stream->last_frame_idx = stream->frame_idx;
	2022	+ if (dev->hdr.hdr_mode == NO_HDR) {
	2023	+ rkcif_s_rx_buffer(stream, &active_buf->dbufs);
	2024	+ if (dev->is_support_tools && stream->tools_vdev)
	2025	+ rkcif_rdbk_with_tools(stream, active_buf);
	2026	+ atomic_dec(&stream->buf_cnt);
	2027	+ } else {
	2028	+ rkcif_rdbk_frame_end_toisp(stream, active_buf);
	2029	+ }
	2030	+ } else {
	2031	+ if (active_buf)
	2032	+ rkcif_s_rx_buffer(stream, &active_buf->dbufs);
	2033	+ if (dev->is_support_tools && stream->tools_vdev)
	2034	+ rkcif_rdbk_with_tools(stream, active_buf);
	2035	+ }
	2036	+ } else if (stream->frame_phase == CIF_CSI_FRAME1_READY) {
	2037	+ active_buf = stream->next_buf_toisp;
	2038	+ buffer = list_first_entry(&stream->rx_buf_head,
	2039	+ struct rkcif_rx_buffer, list);
	2040	+ if (buffer) {
	2041	+ list_del(&buffer->list);
	2042	+ stream->next_buf_toisp = buffer;
	2043	+ }
	2044	+ if (priv && priv->mode.rdbk_mode == RKISP_VICAP_RDBK_AUTO) {
	2045	+ if (!active_buf)
	2046	+ goto out_get_buf;
	2047	+ if (stream->frame_idx == 1)
	2048	+ active_buf->dbufs.is_first = true;
	2049	+ active_buf->dbufs.sequence = stream->frame_idx - 1;
	2050	+ active_buf->dbufs.timestamp = stream->readout.fs_timestamp;
	2051	+ active_buf->fe_timestamp = rkcif_time_get_ns(dev);
	2052	+ stream->last_frame_idx = stream->frame_idx;
	2053	+ if (dev->hdr.hdr_mode == NO_HDR) {
	2054	+ rkcif_s_rx_buffer(stream, &active_buf->dbufs);
	2055	+ if (dev->is_support_tools && stream->tools_vdev)
	2056	+ rkcif_rdbk_with_tools(stream, active_buf);
	2057	+ atomic_dec(&stream->buf_cnt);
	2058	+ } else {
	2059	+ rkcif_rdbk_frame_end_toisp(stream, active_buf);
	2060	+ }
	2061	+ } else {
	2062	+ if (active_buf)
	2063	+ rkcif_s_rx_buffer(stream, &active_buf->dbufs);
	2064	+ if (dev->is_support_tools && stream->tools_vdev)
	2065	+ rkcif_rdbk_with_tools(stream, active_buf);
	2066	+ }
	2067	+ }
	2068	+ if (stream->lack_buf_cnt)
	2069	+ stream->lack_buf_cnt--;
	2070	+ } else {
	2071	+ if (priv->mode.rdbk_mode == RKISP_VICAP_ONLINE)
	2072	+ goto out_get_buf;
	2073	+ if (stream->lack_buf_cnt < 2)
	2074	+ stream->lack_buf_cnt++;
	2075	+ if (dev->hw_dev->dummy_buf.vaddr) {
	2076	+ if (stream->frame_phase == CIF_CSI_FRAME0_READY) {
	2077	+ active_buf = stream->curr_buf_toisp;
	2078	+ } else {
	2079	+ active_buf = stream->next_buf_toisp;
	2080	+ }
	2081	+ } else if (stream->curr_buf_toisp && stream->next_buf_toisp &&
	2082	+ stream->curr_buf_toisp != stream->next_buf_toisp) {
	2083	+ if (stream->frame_phase == CIF_CSI_FRAME0_READY) {
	2084	+ active_buf = stream->curr_buf_toisp;
	2085	+ stream->curr_buf_toisp = stream->next_buf_toisp;
	2086	+ buffer = stream->next_buf_toisp;
	2087	+ } else if (stream->frame_phase == CIF_CSI_FRAME1_READY) {
	2088	+ active_buf = stream->next_buf_toisp;
	2089	+ stream->next_buf_toisp = stream->curr_buf_toisp;
	2090	+ buffer = stream->curr_buf_toisp;
	2091	+ }
	2092	+ stream->toisp_buf_state.state = RKCIF_TOISP_BUF_THESAME;
	2093	+ if (stream->cifdev->rdbk_debug)
	2094	+ v4l2_info(&stream->cifdev->v4l2_dev,
	2095	+ "stream[%d] hold buf %x\n",
	2096	+ stream->id,
	2097	+ (u32)stream->next_buf_toisp->dummy.dma_addr);
	2098	+ } else {
	2099	+ stream->toisp_buf_state.state = RKCIF_TOISP_BUF_LOSS;
	2100	+ if (stream->is_single_cap) {
	2101	+ active_buf = stream->curr_buf_toisp;
	2102	+ stream->curr_buf_toisp = NULL;
	2103	+ stream->next_buf_toisp = NULL;
	2104	+ }
	2105	+ }
	2106	+
	2107	+ if (active_buf) {
	2108	+ if (stream->frame_idx == 1)
	2109	+ active_buf->dbufs.is_first = true;
	2110	+ active_buf->dbufs.sequence = stream->frame_idx - 1;
	2111	+ active_buf->dbufs.timestamp = stream->readout.fs_timestamp;
	2112	+ active_buf->fe_timestamp = rkcif_time_get_ns(dev);
	2113	+ stream->last_frame_idx = stream->frame_idx;
	2114	+ if (dev->hdr.hdr_mode == NO_HDR) {
	2115	+ rkcif_s_rx_buffer(stream, &active_buf->dbufs);
	2116	+ atomic_dec(&stream->buf_cnt);
	2117	+ } else {
	2118	+ rkcif_rdbk_frame_end_toisp(stream, active_buf);
	2119	+ }
	2120	+ } else {
	2121	+ if (stream->cifdev->rdbk_debug && dev->hw_dev->dummy_buf.vaddr)
	2122	+ v4l2_info(&stream->cifdev->v4l2_dev,
	2123	+ "stream[%d] loss frame %d\n",
	2124	+ stream->id,
	2125	+ stream->frame_idx - 1);
	2126	+ }
	2127	+ if (dev->is_support_tools && stream->tools_vdev && active_buf)
	2128	+ rkcif_rdbk_with_tools(stream, active_buf);
	2129	+ }
	2130	+out_get_buf:
	2131	+ stream->frame_phase_cache = stream->frame_phase;
	2132	+ if (buffer) {
	2133	+ buff_addr_y = buffer->dummy.dma_addr;
	2134	+ if (capture_info->mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	2135	+ rkcif_write_buff_addr_multi_dev_combine(stream, frm_addr_y, 0,
	2136	+ buff_addr_y, 0, false);
	2137	+ } else {
	2138	+ rkcif_write_register(dev, frm_addr_y, buff_addr_y);
	2139	+ }
	2140	+ if (dev->rdbk_debug > 1 &&
	2141	+ stream->frame_idx < 15)
	2142	+ v4l2_info(&dev->v4l2_dev,
	2143	+ "stream[%d] update, seq %d, reg %x, buf %x\n",
	2144	+ stream->id,
	2145	+ stream->frame_idx - 1,
	2146	+ frm_addr_y, (u32)buffer->dummy.dma_addr);
	2147	+ } else if (dev->hw_dev->dummy_buf.vaddr && priv &&
	2148	+ priv->mode.rdbk_mode == RKISP_VICAP_RDBK_AUTO) {
	2149	+ buff_addr_y = dev->hw_dev->dummy_buf.dma_addr;
	2150	+ if (capture_info->mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	2151	+ rkcif_write_buff_addr_multi_dev_combine(stream, frm_addr_y, 0,
	2152	+ buff_addr_y, 0, true);
	2153	+ } else {
	2154	+ rkcif_write_register(dev, frm_addr_y, buff_addr_y);
	2155	+ }
	2156	+ }
	2157	+ spin_unlock_irqrestore(&stream->vbq_lock, flags);
	2158	+ return 0;
	2159	+}
	2160	+
	2161	+static int rkcif_assign_new_buffer_pingpong_toisp(struct rkcif_stream *stream,
	2162	+ int init, int channel_id)
	2163	+{
	2164	+ int ret = 0;
	2165	+
	2166	+ if (init)
	2167	+ rkcif_assign_new_buffer_init_toisp(stream, channel_id);
	2168	+ else
	2169	+ ret = rkcif_assign_new_buffer_update_toisp(stream, channel_id);
	2170	+ return ret;
	2171	+}
	2172	+
	2173	+void rkcif_assign_check_buffer_update_toisp(struct rkcif_stream *stream)
	2174	+{
	2175	+ struct rkcif_device *dev = stream->cifdev;
	2176	+ struct v4l2_mbus_config *mbus_cfg = &dev->active_sensor->mbus;
	2177	+ struct rkcif_rx_buffer *buffer = NULL;
	2178	+ struct rkmodule_capture_info *capture_info = &dev->channels[stream->id].capture_info;
	2179	+ struct rkcif_rx_buffer *active_buf = NULL;
	2180	+ u32 frm_addr_y, buff_addr_y;
	2181	+ u32 vblank = 0;
	2182	+ u32 vblank_ns = 0;
	2183	+ u64 cur_time = 0;
	2184	+ int frame_phase = 0;
	2185	+ int frame_phase_next = 0;
	2186	+
	2187	+ if (stream->toisp_buf_state.state == RKCIF_TOISP_BUF_ROTATE \|\|
	2188	+ (stream->toisp_buf_state.state == RKCIF_TOISP_BUF_THESAME &&
	2189	+ stream->toisp_buf_state.check_cnt >= 1) \|\|
	2190	+ (stream->toisp_buf_state.state == RKCIF_TOISP_BUF_LOSS &&
	2191	+ stream->toisp_buf_state.check_cnt >= 2)) {
	2192	+ if (dev->rdbk_debug > 2 &&
	2193	+ stream->frame_idx < 15)
	2194	+ v4l2_info(&dev->v4l2_dev,
	2195	+ "stream[%d] addr check not equal 0x%x 0x%x\n",
	2196	+ stream->id,
	2197	+ (u32)stream->curr_buf_toisp->dummy.dma_addr,
	2198	+ (u32)stream->next_buf_toisp->dummy.dma_addr);
	2199	+ return;
	2200	+ }
	2201	+ if (!dev->sensor_linetime)
	2202	+ dev->sensor_linetime = rkcif_get_linetime(stream);
	2203	+ vblank = rkcif_get_sensor_vblank(dev);
	2204	+ vblank_ns = vblank * dev->sensor_linetime;
	2205	+ cur_time = rkcif_time_get_ns(dev);
	2206	+
	2207	+ if (stream->toisp_buf_state.state == RKCIF_TOISP_BUF_THESAME) {
	2208	+ frame_phase = stream->frame_phase;
	2209	+ } else {
	2210	+ if (stream->toisp_buf_state.state == RKCIF_TOISP_BUF_LOSS &&
	2211	+ stream->toisp_buf_state.check_cnt == 0 &&
	2212	+ cur_time - stream->readout.fe_timestamp < (vblank_ns - 500000)) {
	2213	+ stream->toisp_buf_state.is_early_update = true;
	2214	+ frame_phase = stream->frame_phase & CIF_CSI_FRAME0_READY ?
	2215	+ CIF_CSI_FRAME1_READY : CIF_CSI_FRAME0_READY;
	2216	+ frame_phase_next = stream->frame_phase & CIF_CSI_FRAME0_READY ?
	2217	+ CIF_CSI_FRAME0_READY : CIF_CSI_FRAME1_READY;
	2218	+ } else {
	2219	+ if (stream->toisp_buf_state.check_cnt == 1 &&
	2220	+ (!stream->toisp_buf_state.is_early_update))
	2221	+ return;
	2222	+ frame_phase = stream->frame_phase;
	2223	+ stream->toisp_buf_state.is_early_update = false;
	2224	+ }
	2225	+ }
	2226	+ if (dev->rdbk_debug > 2 &&
	2227	+ stream->frame_idx < 15)
	2228	+ v4l2_info(&dev->v4l2_dev,
	2229	+ "stream[%d] check update, cur %lld, fe %lld, vb %u lack_buf %d\n",
	2230	+ stream->id,
	2231	+ cur_time, stream->readout.fe_timestamp,
	2232	+ vblank_ns, stream->lack_buf_cnt);
	2233	+ if (mbus_cfg->type == V4L2_MBUS_CSI2_DPHY \|\|
	2234	+ mbus_cfg->type == V4L2_MBUS_CSI2_CPHY \|\|
	2235	+ mbus_cfg->type == V4L2_MBUS_CCP2) {
	2236	+ frm_addr_y = frame_phase & CIF_CSI_FRAME0_READY ?
	2237	+ get_reg_index_of_frm0_y_addr(stream->id) :
	2238	+ get_reg_index_of_frm1_y_addr(stream->id);
	2239	+ } else {
	2240	+ frm_addr_y = frame_phase & CIF_CSI_FRAME0_READY ?
	2241	+ get_dvp_reg_index_of_frm0_y_addr(stream->id) :
	2242	+ get_dvp_reg_index_of_frm1_y_addr(stream->id);
	2243	+ }
	2244	+ if (!list_empty(&stream->rx_buf_head)) {
	2245	+ if (frame_phase == CIF_CSI_FRAME0_READY) {
	2246	+ active_buf = stream->curr_buf_toisp;
	2247	+ buffer = list_first_entry(&stream->rx_buf_head,
	2248	+ struct rkcif_rx_buffer, list);
	2249	+ if (buffer) {
	2250	+ list_del(&buffer->list);
	2251	+ stream->curr_buf_toisp = buffer;
	2252	+ buff_addr_y = stream->curr_buf_toisp->dummy.dma_addr;
	2253	+ if (capture_info->mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	2254	+ rkcif_write_buff_addr_multi_dev_combine(stream,
	2255	+ frm_addr_y, 0,
	2256	+ buff_addr_y, 0,
	2257	+ false);
	2258	+ } else {
	2259	+ rkcif_write_register(dev, frm_addr_y, buff_addr_y);
	2260	+ }
	2261	+ if (dev->rdbk_debug > 1 &&
	2262	+ stream->frame_idx < 15)
	2263	+ v4l2_info(&dev->v4l2_dev,
	2264	+ "stream[%d] check update, seq %d, addr 0x%x, buf 0x%x\n",
	2265	+ stream->id,
	2266	+ stream->frame_idx - 1, frm_addr_y,
	2267	+ (u32)stream->curr_buf_toisp->dummy.dma_addr);
	2268	+ }
	2269	+ } else if (frame_phase == CIF_CSI_FRAME1_READY) {
	2270	+ active_buf = stream->next_buf_toisp;
	2271	+ buffer = list_first_entry(&stream->rx_buf_head,
	2272	+ struct rkcif_rx_buffer, list);
	2273	+ if (buffer) {
	2274	+ list_del(&buffer->list);
	2275	+ stream->next_buf_toisp = buffer;
	2276	+ buff_addr_y = stream->next_buf_toisp->dummy.dma_addr;
	2277	+ if (capture_info->mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	2278	+ rkcif_write_buff_addr_multi_dev_combine(stream,
	2279	+ frm_addr_y, 0,
	2280	+ buff_addr_y, 0,
	2281	+ false);
	2282	+ } else {
	2283	+ rkcif_write_register(dev, frm_addr_y, buff_addr_y);
	2284	+ }
	2285	+ if (dev->rdbk_debug > 1 &&
	2286	+ stream->frame_idx < 15)
	2287	+ v4l2_info(&dev->v4l2_dev,
	2288	+ "stream[%d] check update, seq %d, addr 0x%x, buf 0x%x\n",
	2289	+ stream->id,
	2290	+ stream->frame_idx - 1, frm_addr_y,
	2291	+ (u32)stream->next_buf_toisp->dummy.dma_addr);
	2292	+ }
	2293	+ }
	2294	+ if (stream->lack_buf_cnt)
	2295	+ stream->lack_buf_cnt--;
	2296	+ }
	2297	+ if (stream->toisp_buf_state.is_early_update) {
	2298	+ if (dev->rdbk_debug > 1 &&
	2299	+ stream->frame_idx < 15)
	2300	+ v4l2_info(&dev->v4l2_dev,
	2301	+ "stream[%d] early update, seq %d\n",
	2302	+ stream->id,
	2303	+ stream->frame_idx - 1);
	2304	+ if (mbus_cfg->type == V4L2_MBUS_CSI2_DPHY \|\|
	2305	+ mbus_cfg->type == V4L2_MBUS_CSI2_CPHY)
	2306	+ rkcif_write_register_or(dev, CIF_REG_MIPI_LVDS_CTRL, 0x00010000);
	2307	+ else
	2308	+ rkcif_write_register_or(dev, CIF_REG_DVP_CTRL, 0x00010000);
	2309	+ if (active_buf) {
	2310	+ active_buf->dbufs.sequence = stream->frame_idx - 1;
	2311	+ active_buf->dbufs.timestamp = stream->readout.fs_timestamp;
	2312	+ stream->last_frame_idx = stream->frame_idx;
	2313	+ rkcif_s_rx_buffer(stream, &active_buf->dbufs);
	2314	+ }
	2315	+ if (dev->hw_dev->dummy_buf.vaddr)
	2316	+ return;
	2317	+ if (mbus_cfg->type == V4L2_MBUS_CSI2_DPHY \|\|
	2318	+ mbus_cfg->type == V4L2_MBUS_CSI2_CPHY \|\|
	2319	+ mbus_cfg->type == V4L2_MBUS_CCP2) {
	2320	+ frm_addr_y = frame_phase_next & CIF_CSI_FRAME0_READY ?
	2321	+ get_reg_index_of_frm0_y_addr(stream->id) :
	2322	+ get_reg_index_of_frm1_y_addr(stream->id);
	2323	+ } else {
	2324	+ frm_addr_y = frame_phase_next & CIF_CSI_FRAME0_READY ?
	2325	+ get_dvp_reg_index_of_frm0_y_addr(stream->id) :
	2326	+ get_dvp_reg_index_of_frm1_y_addr(stream->id);
	2327	+ }
	2328	+ if (frame_phase == CIF_CSI_FRAME0_READY)
	2329	+ stream->next_buf_toisp = stream->curr_buf_toisp;
	2330	+ else
	2331	+ stream->curr_buf_toisp = stream->next_buf_toisp;
	2332	+ buff_addr_y = stream->curr_buf_toisp->dummy.dma_addr;
	2333	+ if (capture_info->mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	2334	+ rkcif_write_buff_addr_multi_dev_combine(stream, frm_addr_y, 0,
	2335	+ buff_addr_y, 0, false);
	2336	+ } else {
	2337	+ rkcif_write_register(dev, frm_addr_y, buff_addr_y);
	2338	+ }
	2339	+ }
	2340	+ stream->toisp_buf_state.check_cnt++;
1282	2341	}
1283	2342
1284	2343	static void rkcif_assign_new_buffer_init(struct rkcif_stream *stream,
..	..	@@ -1288,11 +2347,14 @@
1288	2347	struct v4l2_mbus_config *mbus_cfg = &dev->active_sensor->mbus;
1289	2348	u32 frm0_addr_y, frm0_addr_uv;
1290	2349	u32 frm1_addr_y, frm1_addr_uv;
	2350	+ u32 buff_addr_y, buff_addr_cbcr;
1291	2351	unsigned long flags;
1292		- struct rkcif_dummy_buffer *dummy_buf = &dev->dummy_buf;
	2352	+ struct rkcif_dummy_buffer *dummy_buf = &dev->hw_dev->dummy_buf;
1293	2353	struct csi_channel_info *channel = &dev->channels[channel_id];
1294	2354
1295		- if (mbus_cfg->type == V4L2_MBUS_CSI2 \|\|
	2355	+ stream->lack_buf_cnt = 0;
	2356	+ if (mbus_cfg->type == V4L2_MBUS_CSI2_DPHY \|\|
	2357	+ mbus_cfg->type == V4L2_MBUS_CSI2_CPHY \|\|
1296	2358	mbus_cfg->type == V4L2_MBUS_CCP2) {
1297	2359	frm0_addr_y = get_reg_index_of_frm0_y_addr(channel_id);
1298	2360	frm0_addr_uv = get_reg_index_of_frm0_uv_addr(channel_id);
..	..	@@ -1312,53 +2374,119 @@
1312	2374	stream->curr_buf = list_first_entry(&stream->buf_head,
1313	2375	struct rkcif_buffer,
1314	2376	queue);
	2377	+ v4l2_dbg(4, rkcif_debug, &dev->v4l2_dev, "%s %d, stream[%d] buf idx %d\n",
	2378	+ __func__, __LINE__, stream->id, stream->curr_buf->vb.vb2_buf.index);
1315	2379	list_del(&stream->curr_buf->queue);
1316	2380	}
1317	2381	}
1318	2382
1319	2383	if (stream->curr_buf) {
1320		- rkcif_write_register(dev, frm0_addr_y,
1321		- stream->curr_buf->buff_addr[RKCIF_PLANE_Y]);
1322		- if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
1323		- rkcif_write_register(dev, frm0_addr_uv,
1324		- stream->curr_buf->buff_addr[RKCIF_PLANE_CBCR]);
	2384	+ buff_addr_y = stream->curr_buf->buff_addr[RKCIF_PLANE_Y];
	2385	+ buff_addr_cbcr = stream->curr_buf->buff_addr[RKCIF_PLANE_CBCR];
	2386	+ if (channel->capture_info.mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	2387	+ rkcif_write_buff_addr_multi_dev_combine(stream,
	2388	+ frm0_addr_y,
	2389	+ frm0_addr_uv,
	2390	+ buff_addr_y,
	2391	+ buff_addr_cbcr,
	2392	+ false);
	2393	+ } else {
	2394	+ rkcif_write_register(dev, frm0_addr_y,
	2395	+ stream->curr_buf->buff_addr[RKCIF_PLANE_Y]);
	2396	+ if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
	2397	+ rkcif_write_register(dev, frm0_addr_uv,
	2398	+ stream->curr_buf->buff_addr[RKCIF_PLANE_CBCR]);
	2399	+ }
1325	2400	} else {
1326	2401	if (dummy_buf->vaddr) {
1327		- rkcif_write_register(dev, frm0_addr_y, dummy_buf->dma_addr);
1328		- if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
1329		- rkcif_write_register(dev, frm0_addr_uv, dummy_buf->dma_addr);
	2402	+ buff_addr_y = dummy_buf->dma_addr;
	2403	+ buff_addr_cbcr = dummy_buf->dma_addr;
	2404	+ if (channel->capture_info.mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	2405	+ rkcif_write_buff_addr_multi_dev_combine(stream,
	2406	+ frm0_addr_y,
	2407	+ frm0_addr_uv,
	2408	+ buff_addr_y,
	2409	+ buff_addr_cbcr,
	2410	+ true);
	2411	+ } else {
	2412	+ rkcif_write_register(dev, frm0_addr_y, buff_addr_y);
	2413	+ if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
	2414	+ rkcif_write_register(dev, frm0_addr_uv, buff_addr_cbcr);
	2415	+ }
	2416	+ } else {
	2417	+ if (stream->lack_buf_cnt < 2)
	2418	+ stream->lack_buf_cnt++;
1330	2419	}
1331	2420	}
1332	2421
1333	2422	if (stream->cif_fmt_in->field == V4L2_FIELD_INTERLACED) {
1334	2423	stream->next_buf = stream->curr_buf;
1335	2424	if (stream->next_buf) {
1336		- rkcif_write_register(dev, frm1_addr_y,
1337		- stream->next_buf->buff_addr[RKCIF_PLANE_Y] + (channel->virtual_width / 2));
1338		- if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
1339		- rkcif_write_register(dev, frm1_addr_uv,
1340		- stream->next_buf->buff_addr[RKCIF_PLANE_CBCR] + (channel->virtual_width / 2));
	2425	+ buff_addr_y = stream->next_buf->buff_addr[RKCIF_PLANE_Y];
	2426	+ buff_addr_cbcr = stream->next_buf->buff_addr[RKCIF_PLANE_CBCR];
	2427	+ if (channel->capture_info.mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	2428	+ rkcif_write_buff_addr_multi_dev_combine(stream,
	2429	+ frm1_addr_y,
	2430	+ frm1_addr_uv,
	2431	+ buff_addr_y,
	2432	+ buff_addr_cbcr,
	2433	+ false);
	2434	+ } else {
	2435	+ rkcif_write_register(dev, frm1_addr_y,
	2436	+ buff_addr_y + (channel->virtual_width / 2));
	2437	+ if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
	2438	+ rkcif_write_register(dev, frm1_addr_uv,
	2439	+ buff_addr_cbcr + (channel->virtual_width / 2));
	2440	+ }
1341	2441	}
1342	2442	} else {
1343	2443	if (!stream->next_buf) {
1344	2444	if (!list_empty(&stream->buf_head)) {
1345	2445	stream->next_buf = list_first_entry(&stream->buf_head,
1346	2446	struct rkcif_buffer, queue);
	2447	+ v4l2_dbg(4, rkcif_debug, &dev->v4l2_dev, "%s %d, stream[%d] buf idx %d\n",
	2448	+ __func__, __LINE__, stream->id, stream->next_buf->vb.vb2_buf.index);
1347	2449	list_del(&stream->next_buf->queue);
	2450	+ } else if (stream->curr_buf) {
	2451	+ stream->next_buf = stream->curr_buf;
1348	2452	}
1349	2453	}
1350	2454
1351		- if (stream->next_buf) {
1352		- rkcif_write_register(dev, frm1_addr_y,
1353		- stream->next_buf->buff_addr[RKCIF_PLANE_Y]);
1354		- if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
1355		- rkcif_write_register(dev, frm1_addr_uv,
1356		- stream->next_buf->buff_addr[RKCIF_PLANE_CBCR]);
1357		- } else {
1358		- if (dummy_buf->vaddr) {
	2455	+ if (!stream->next_buf && dummy_buf->vaddr) {
	2456	+ buff_addr_y = dummy_buf->dma_addr;
	2457	+ buff_addr_cbcr = dummy_buf->dma_addr;
	2458	+ if (channel->capture_info.mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	2459	+ rkcif_write_buff_addr_multi_dev_combine(stream,
	2460	+ frm1_addr_y,
	2461	+ frm1_addr_uv,
	2462	+ buff_addr_y,
	2463	+ buff_addr_cbcr,
	2464	+ true);
	2465	+ } else {
1359	2466	rkcif_write_register(dev, frm1_addr_y, dummy_buf->dma_addr);
1360	2467	if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
1361	2468	rkcif_write_register(dev, frm1_addr_uv, dummy_buf->dma_addr);
	2469	+ }
	2470	+
	2471	+ } else if (!stream->next_buf && stream->curr_buf) {
	2472	+ stream->next_buf = stream->curr_buf;
	2473	+ if (stream->lack_buf_cnt < 2)
	2474	+ stream->lack_buf_cnt++;
	2475	+ }
	2476	+ if (stream->next_buf) {
	2477	+ buff_addr_y = stream->next_buf->buff_addr[RKCIF_PLANE_Y];
	2478	+ buff_addr_cbcr = stream->next_buf->buff_addr[RKCIF_PLANE_CBCR];
	2479	+ if (channel->capture_info.mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	2480	+ rkcif_write_buff_addr_multi_dev_combine(stream,
	2481	+ frm1_addr_y,
	2482	+ frm1_addr_uv,
	2483	+ buff_addr_y,
	2484	+ buff_addr_cbcr,
	2485	+ false);
	2486	+ } else {
	2487	+ rkcif_write_register(dev, frm1_addr_y, buff_addr_y);
	2488	+ if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
	2489	+ rkcif_write_register(dev, frm1_addr_uv, buff_addr_cbcr);
1362	2490	}
1363	2491	}
1364	2492	}
..	..	@@ -1391,22 +2519,26 @@
1391	2519	}
1392	2520	}
1393	2521	}
1394		-
	2522	+ stream->buf_owner = RKCIF_DMAEN_BY_VICAP;
1395	2523	}
1396	2524
1397	2525	static int rkcif_assign_new_buffer_update(struct rkcif_stream *stream,
1398		- int channel_id)
	2526	+ int channel_id)
1399	2527	{
1400	2528	struct rkcif_device *dev = stream->cifdev;
1401		- struct rkcif_dummy_buffer *dummy_buf = &dev->dummy_buf;
	2529	+ struct rkcif_dummy_buffer *dummy_buf = &dev->hw_dev->dummy_buf;
1402	2530	struct v4l2_mbus_config *mbus_cfg = &dev->active_sensor->mbus;
1403	2531	struct rkcif_buffer *buffer = NULL;
1404	2532	u32 frm_addr_y, frm_addr_uv;
1405	2533	struct csi_channel_info *channel = &dev->channels[channel_id];
	2534	+ struct rkisp_rx_buf *dbufs = NULL;
	2535	+ struct dma_buf *dbuf = NULL;
1406	2536	int ret = 0;
	2537	+ u32 buff_addr_y, buff_addr_cbcr;
1407	2538	unsigned long flags;
1408	2539
1409		- if (mbus_cfg->type == V4L2_MBUS_CSI2 \|\|
	2540	+ if (mbus_cfg->type == V4L2_MBUS_CSI2_DPHY \|\|
	2541	+ mbus_cfg->type == V4L2_MBUS_CSI2_CPHY \|\|
1410	2542	mbus_cfg->type == V4L2_MBUS_CCP2) {
1411	2543	frm_addr_y = stream->frame_phase & CIF_CSI_FRAME0_READY ?
1412	2544	get_reg_index_of_frm0_y_addr(channel_id) :
..	..	@@ -1423,20 +2555,34 @@
1423	2555	get_dvp_reg_index_of_frm1_uv_addr(channel_id);
1424	2556	}
1425	2557
	2558	+ if (stream->to_stop_dma && (stream->dma_en & RKCIF_DMAEN_BY_ISP)) {
	2559	+ v4l2_dbg(3, rkcif_debug, &dev->v4l2_dev, "%s %d\n", __func__, __LINE__);
	2560	+ goto stop_dma;
	2561	+ }
	2562	+
1426	2563	spin_lock_irqsave(&stream->vbq_lock, flags);
1427	2564	if (!list_empty(&stream->buf_head)) {
	2565	+
1428	2566	if (!dummy_buf->vaddr &&
1429	2567	stream->curr_buf == stream->next_buf &&
1430	2568	stream->cif_fmt_in->field != V4L2_FIELD_INTERLACED)
1431	2569	ret = -EINVAL;
	2570	+
1432	2571	if (stream->frame_phase == CIF_CSI_FRAME0_READY) {
	2572	+ if (!stream->curr_buf)
	2573	+ ret = -EINVAL;
1433	2574	stream->curr_buf = list_first_entry(&stream->buf_head,
1434	2575	struct rkcif_buffer, queue);
1435	2576	if (stream->curr_buf) {
1436	2577	list_del(&stream->curr_buf->queue);
1437	2578	buffer = stream->curr_buf;
	2579	+ v4l2_dbg(3, rkcif_debug, &dev->v4l2_dev,
	2580	+ "stream[%d] update curr_buf 0x%x, buf idx %d\n",
	2581	+ stream->id, buffer->buff_addr[0], stream->curr_buf->vb.vb2_buf.index);
1438	2582	}
1439	2583	} else if (stream->frame_phase == CIF_CSI_FRAME1_READY) {
	2584	+ if (!stream->next_buf)
	2585	+ ret = -EINVAL;
1440	2586	if (stream->cif_fmt_in->field == V4L2_FIELD_INTERLACED) {
1441	2587	if (stream->next_buf != stream->curr_buf) {
1442	2588	stream->next_buf = stream->curr_buf;
..	..	@@ -1451,14 +2597,17 @@
1451	2597	if (stream->next_buf) {
1452	2598	list_del(&stream->next_buf->queue);
1453	2599	buffer = stream->next_buf;
	2600	+ v4l2_dbg(3, rkcif_debug, &dev->v4l2_dev,
	2601	+ "stream[%d] update next_buf 0x%x, buf idx %d\n",
	2602	+ stream->id, buffer->buff_addr[0], stream->next_buf->vb.vb2_buf.index);
1454	2603	}
1455	2604	}
1456	2605	}
1457	2606	} else {
1458	2607	buffer = NULL;
1459		- if (dummy_buf->vaddr) {
	2608	+ if (!(stream->cur_stream_mode & RKCIF_STREAM_MODE_TOISP) && dummy_buf->vaddr) {
1460	2609	if (stream->frame_phase == CIF_CSI_FRAME0_READY) {
1461		- stream->curr_buf = NULL;
	2610	+ stream->curr_buf = NULL;
1462	2611	} else if (stream->frame_phase == CIF_CSI_FRAME1_READY) {
1463	2612	if (stream->cif_fmt_in->field == V4L2_FIELD_INTERLACED) {
1464	2613	stream->next_buf = stream->curr_buf;
..	..	@@ -1468,56 +2617,123 @@
1468	2617	}
1469	2618	}
1470	2619	} else if (stream->curr_buf != stream->next_buf) {
1471		- if (stream->frame_phase == CIF_CSI_FRAME0_READY) {
	2620	+ if (stream->frame_phase == CIF_CSI_FRAME0_READY && stream->next_buf) {
1472	2621	stream->curr_buf = stream->next_buf;
1473	2622	buffer = stream->next_buf;
1474		- } else if (stream->frame_phase == CIF_CSI_FRAME1_READY) {
	2623	+ } else if (stream->frame_phase == CIF_CSI_FRAME1_READY && stream->curr_buf) {
1475	2624	stream->next_buf = stream->curr_buf;
1476	2625	buffer = stream->curr_buf;
1477	2626	}
1478		-
	2627	+ if (stream->lack_buf_cnt < 2)
	2628	+ stream->lack_buf_cnt++;
	2629	+ } else {
	2630	+ stream->curr_buf = NULL;
	2631	+ stream->next_buf = NULL;
	2632	+ if (stream->lack_buf_cnt < 2)
	2633	+ stream->lack_buf_cnt++;
1479	2634	}
1480		-
1481	2635	}
1482	2636	stream->frame_phase_cache = stream->frame_phase;
1483		- spin_unlock_irqrestore(&stream->vbq_lock, flags);
1484	2637
1485	2638	if (buffer) {
	2639	+ buff_addr_y = buffer->buff_addr[RKCIF_PLANE_Y];
	2640	+ buff_addr_cbcr = buffer->buff_addr[RKCIF_PLANE_CBCR];
1486	2641	if (stream->cif_fmt_in->field == V4L2_FIELD_INTERLACED &&
1487	2642	stream->frame_phase == CIF_CSI_FRAME1_READY) {
1488		- rkcif_write_register(dev, frm_addr_y,
1489		- buffer->buff_addr[RKCIF_PLANE_Y] + (channel->virtual_width / 2));
1490		- if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
1491		- rkcif_write_register(dev, frm_addr_uv,
1492		- buffer->buff_addr[RKCIF_PLANE_CBCR] + (channel->virtual_width / 2));
	2643	+ if (channel->capture_info.mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	2644	+ rkcif_write_buff_addr_multi_dev_combine(stream,
	2645	+ frm_addr_y,
	2646	+ frm_addr_uv,
	2647	+ buff_addr_y,
	2648	+ buff_addr_cbcr,
	2649	+ false);
	2650	+ } else {
	2651	+ rkcif_write_register(dev, frm_addr_y,
	2652	+ buff_addr_y + (channel->virtual_width / 2));
	2653	+ if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
	2654	+ rkcif_write_register(dev, frm_addr_uv,
	2655	+ buff_addr_cbcr + (channel->virtual_width / 2));
	2656	+ }
1493	2657	} else {
1494		- rkcif_write_register(dev, frm_addr_y,
1495		- buffer->buff_addr[RKCIF_PLANE_Y]);
1496		- if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
1497		- rkcif_write_register(dev, frm_addr_uv,
1498		- buffer->buff_addr[RKCIF_PLANE_CBCR]);
	2658	+ if (channel->capture_info.mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	2659	+ rkcif_write_buff_addr_multi_dev_combine(stream,
	2660	+ frm_addr_y,
	2661	+ frm_addr_uv,
	2662	+ buff_addr_y,
	2663	+ buff_addr_cbcr,
	2664	+ false);
	2665	+ } else {
	2666	+ rkcif_write_register(dev, frm_addr_y, buff_addr_y);
	2667	+ if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
	2668	+ rkcif_write_register(dev, frm_addr_uv, buff_addr_cbcr);
	2669	+ }
1499	2670	}
1500		- } else {
1501		- if (dummy_buf->vaddr) {
1502		- rkcif_write_register(dev, frm_addr_y, dummy_buf->dma_addr);
1503		- if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
1504		- rkcif_write_register(dev, frm_addr_uv, dummy_buf->dma_addr);
1505		- } else {
1506		- ret = -EINVAL;
	2671	+ if (stream->dma_en & RKCIF_DMAEN_BY_ISP) {
	2672	+ if (stream->buf_replace_cnt < 2)
	2673	+ stream->buf_replace_cnt++;
	2674	+ if (stream->frame_phase == CIF_CSI_FRAME0_READY &&
	2675	+ stream->next_buf)
	2676	+ dbuf = stream->next_buf->dbuf;
	2677	+ else if (stream->frame_phase == CIF_CSI_FRAME1_READY &&
	2678	+ stream->curr_buf)
	2679	+ dbuf = stream->curr_buf->dbuf;
	2680	+
	2681	+ if (dbuf) {
	2682	+ list_for_each_entry(dbufs, &stream->rx_buf_head_vicap, list) {
	2683	+ if (dbufs->dbuf == dbuf)
	2684	+ break;
	2685	+ }
	2686	+ }
	2687	+ if (dbufs)
	2688	+ rkcif_s_rx_buffer(stream, dbufs);
1507	2689	}
1508		- v4l2_info(&dev->v4l2_dev,
1509		- "not active buffer, skip current frame, %s stream[%d]\n",
1510		- (mbus_cfg->type == V4L2_MBUS_CSI2 \|\|
1511		- mbus_cfg->type == V4L2_MBUS_CCP2) ? "mipi/lvds" : "dvp",
1512		- stream->id);
1513	2690	}
	2691	+ spin_unlock_irqrestore(&stream->vbq_lock, flags);
1514	2692	return ret;
	2693	+stop_dma:
	2694	+ if (stream->buf_replace_cnt) {
	2695	+ spin_lock_irqsave(&stream->vbq_lock, flags);
	2696	+ buff_addr_y = stream->curr_buf_toisp->dummy.dma_addr;
	2697	+ if (channel->capture_info.mode == RKMODULE_MULTI_DEV_COMBINE_ONE)
	2698	+ rkcif_write_buff_addr_multi_dev_combine(stream,
	2699	+ frm_addr_y, 0,
	2700	+ buff_addr_y, 0, false);
	2701	+ else
	2702	+ rkcif_write_register(dev, frm_addr_y, buff_addr_y);
	2703	+ if (stream->frame_phase == CIF_CSI_FRAME0_READY &&
	2704	+ stream->next_buf)
	2705	+ dbuf = stream->next_buf->dbuf;
	2706	+ else if (stream->frame_phase == CIF_CSI_FRAME1_READY &&
	2707	+ stream->curr_buf)
	2708	+ dbuf = stream->curr_buf->dbuf;
	2709	+
	2710	+ if (dbuf) {
	2711	+ list_for_each_entry(dbufs, &stream->rx_buf_head_vicap, list)
	2712	+ if (dbufs->dbuf == dbuf)
	2713	+ break;
	2714	+ } else {
	2715	+ dbufs = &stream->curr_buf_toisp->dbufs;
	2716	+ }
	2717	+ if (dbufs)
	2718	+ rkcif_s_rx_buffer(stream, dbufs);
	2719	+
	2720	+ if (stream->frame_phase == CIF_CSI_FRAME0_READY &&
	2721	+ stream->curr_buf) {
	2722	+ stream->curr_buf = NULL;
	2723	+ } else if (stream->frame_phase == CIF_CSI_FRAME1_READY &&
	2724	+ stream->next_buf) {
	2725	+ stream->next_buf = NULL;
	2726	+ }
	2727	+ stream->buf_replace_cnt--;
	2728	+ spin_unlock_irqrestore(&stream->vbq_lock, flags);
	2729	+ }
	2730	+ return -EINVAL;
1515	2731	}
1516	2732
1517	2733	static int rkcif_get_new_buffer_wake_up_mode(struct rkcif_stream *stream)
1518	2734	{
1519	2735	struct rkcif_device *dev = stream->cifdev;
1520		- struct rkcif_dummy_buffer *dummy_buf = &dev->dummy_buf;
	2736	+ struct rkcif_dummy_buffer *dummy_buf = &dev->hw_dev->dummy_buf;
1521	2737	int ret = 0;
1522	2738	unsigned long flags;
1523	2739
..	..	@@ -1538,6 +2754,8 @@
1538	2754	list_del(&stream->next_buf->queue);
1539	2755	}
1540	2756	stream->is_buf_active = true;
	2757	+ if (stream->lack_buf_cnt)
	2758	+ stream->lack_buf_cnt--;
1541	2759	} else {
1542	2760	stream->is_buf_active = false;
1543	2761	if (dummy_buf->vaddr) {
..	..	@@ -1554,8 +2772,15 @@
1554	2772	stream->frame_phase_cache = CIF_CSI_FRAME1_READY;
1555	2773	}
1556	2774	stream->is_buf_active = true;
	2775	+ if (stream->lack_buf_cnt < 2)
	2776	+ stream->lack_buf_cnt++;
1557	2777	} else {
1558		- ret = -EINVAL;
	2778	+ if (dev->chip_id < CHIP_RK3588_CIF)
	2779	+ ret = -EINVAL;
	2780	+ else
	2781	+ ret = 0;
	2782	+ if (stream->lack_buf_cnt < 2)
	2783	+ stream->lack_buf_cnt++;
1559	2784	}
1560	2785	}
1561	2786	spin_unlock_irqrestore(&stream->vbq_lock, flags);
..	..	@@ -1566,15 +2791,18 @@
1566	2791	static int rkcif_update_new_buffer_wake_up_mode(struct rkcif_stream *stream)
1567	2792	{
1568	2793	struct rkcif_device *dev = stream->cifdev;
1569		- struct rkcif_dummy_buffer *dummy_buf = &dev->dummy_buf;
	2794	+ struct rkcif_dummy_buffer *dummy_buf = &dev->hw_dev->dummy_buf;
1570	2795	struct v4l2_mbus_config *mbus_cfg = &dev->active_sensor->mbus;
	2796	+ struct rkmodule_capture_info *capture_info = &dev->channels[stream->id].capture_info;
1571	2797	struct rkcif_buffer *buffer = NULL;
1572	2798	u32 frm_addr_y, frm_addr_uv;
	2799	+ u32 buff_addr_y, buff_addr_cbcr;
1573	2800	int channel_id = stream->id;
1574	2801	int ret = 0;
1575	2802	unsigned long flags;
1576	2803
1577		- if (mbus_cfg->type == V4L2_MBUS_CSI2 \|\|
	2804	+ if (mbus_cfg->type == V4L2_MBUS_CSI2_DPHY \|\|
	2805	+ mbus_cfg->type == V4L2_MBUS_CSI2_CPHY \|\|
1578	2806	mbus_cfg->type == V4L2_MBUS_CCP2) {
1579	2807	frm_addr_y = stream->frame_phase & CIF_CSI_FRAME0_READY ?
1580	2808	get_reg_index_of_frm0_y_addr(channel_id) :
..	..	@@ -1599,25 +2827,130 @@
1599	2827	}
1600	2828	spin_unlock_irqrestore(&stream->vbq_lock, flags);
1601	2829	if (buffer) {
1602		- rkcif_write_register(dev, frm_addr_y,
1603		- buffer->buff_addr[RKCIF_PLANE_Y]);
1604		- if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
1605		- rkcif_write_register(dev, frm_addr_uv,
1606		- buffer->buff_addr[RKCIF_PLANE_CBCR]);
	2830	+ buff_addr_y = buffer->buff_addr[RKCIF_PLANE_Y];
	2831	+ buff_addr_cbcr = buffer->buff_addr[RKCIF_PLANE_CBCR];
	2832	+ if (capture_info->mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	2833	+ rkcif_write_buff_addr_multi_dev_combine(stream, frm_addr_y,
	2834	+ frm_addr_uv,
	2835	+ buff_addr_y,
	2836	+ buff_addr_cbcr,
	2837	+ false);
	2838	+ } else {
	2839	+ rkcif_write_register(dev, frm_addr_y, buff_addr_y);
	2840	+ if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
	2841	+ rkcif_write_register(dev, frm_addr_uv, buff_addr_cbcr);
	2842	+ }
1607	2843	} else {
1608	2844	if (dummy_buf->vaddr) {
1609		- rkcif_write_register(dev, frm_addr_y, dummy_buf->dma_addr);
1610		- if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
1611		- rkcif_write_register(dev, frm_addr_uv, dummy_buf->dma_addr);
	2845	+ buff_addr_y = dummy_buf->dma_addr;
	2846	+ buff_addr_cbcr = dummy_buf->dma_addr;
	2847	+ if (capture_info->mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	2848	+ rkcif_write_buff_addr_multi_dev_combine(stream,
	2849	+ frm_addr_y,
	2850	+ frm_addr_uv,
	2851	+ buff_addr_y,
	2852	+ buff_addr_cbcr,
	2853	+ true);
	2854	+ } else {
	2855	+ rkcif_write_register(dev, frm_addr_y, buff_addr_y);
	2856	+ if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
	2857	+ rkcif_write_register(dev, frm_addr_uv, buff_addr_cbcr);
	2858	+ }
	2859	+ } else {
	2860	+ if (dev->chip_id < CHIP_RK3588_CIF)
	2861	+ ret = -EINVAL;
	2862	+ else
	2863	+ ret = 0;
	2864	+ }
	2865	+ dev->err_state \|= (RKCIF_ERR_ID0_NOT_BUF << stream->id);
	2866	+ dev->irq_stats.not_active_buf_cnt[stream->id]++;
	2867	+ }
	2868	+
	2869	+ return ret;
	2870	+}
	2871	+
	2872	+static int rkcif_get_new_buffer_wake_up_mode_rdbk(struct rkcif_stream *stream)
	2873	+{
	2874	+ struct rkcif_rx_buffer *buffer = NULL;
	2875	+ struct rkcif_device *dev = stream->cifdev;
	2876	+ struct v4l2_mbus_config *mbus_cfg = &dev->active_sensor->mbus;
	2877	+ int ret = 0;
	2878	+ unsigned long flags;
	2879	+ u32 frm_addr_y;
	2880	+ int frame_phase = 0;
	2881	+
	2882	+ spin_lock_irqsave(&stream->vbq_lock, flags);
	2883	+ memset(&stream->toisp_buf_state, 0, sizeof(stream->toisp_buf_state));
	2884	+ if (!list_empty(&stream->rx_buf_head)) {
	2885	+ if (stream->curr_buf_toisp && stream->next_buf_toisp &&
	2886	+ stream->curr_buf_toisp != stream->next_buf_toisp)
	2887	+ stream->toisp_buf_state.state = RKCIF_TOISP_BUF_ROTATE;
	2888	+ else
	2889	+ stream->toisp_buf_state.state = RKCIF_TOISP_BUF_LOSS;
	2890	+ if (stream->line_int_cnt % 2) {
	2891	+ buffer = list_first_entry(&stream->rx_buf_head,
	2892	+ struct rkcif_rx_buffer, list);
	2893	+ if (buffer) {
	2894	+ list_del(&buffer->list);
	2895	+ stream->curr_buf_toisp = buffer;
	2896	+ }
	2897	+ frame_phase = CIF_CSI_FRAME0_READY;
	2898	+ } else {
	2899	+ buffer = list_first_entry(&stream->rx_buf_head,
	2900	+ struct rkcif_rx_buffer, list);
	2901	+ if (buffer) {
	2902	+ list_del(&buffer->list);
	2903	+ stream->next_buf_toisp = buffer;
	2904	+ }
	2905	+ frame_phase = CIF_CSI_FRAME1_READY;
	2906	+ }
	2907	+ if (stream->lack_buf_cnt)
	2908	+ stream->lack_buf_cnt--;
	2909	+ } else {
	2910	+ if (stream->lack_buf_cnt < 2)
	2911	+ stream->lack_buf_cnt++;
	2912	+ if (stream->curr_buf_toisp && stream->next_buf_toisp &&
	2913	+ stream->curr_buf_toisp != stream->next_buf_toisp) {
	2914	+ if (stream->line_int_cnt % 2)
	2915	+ stream->curr_buf_toisp = stream->next_buf_toisp;
	2916	+ else
	2917	+ stream->next_buf_toisp = stream->curr_buf_toisp;
	2918	+ buffer = stream->curr_buf_toisp;
	2919	+ ret = 0;
	2920	+ if (stream->cifdev->rdbk_debug)
	2921	+ v4l2_info(&stream->cifdev->v4l2_dev,
	2922	+ "stream[%d] hold buf %x\n",
	2923	+ stream->id,
	2924	+ (u32)stream->next_buf_toisp->dummy.dma_addr);
	2925	+ stream->toisp_buf_state.state = RKCIF_TOISP_BUF_THESAME;
1612	2926	} else {
1613	2927	ret = -EINVAL;
	2928	+ stream->toisp_buf_state.state = RKCIF_TOISP_BUF_LOSS;
1614	2929	}
1615		- v4l2_info(&dev->v4l2_dev,
1616		- "not active buffer, skip current frame, %s stream[%d]\n",
1617		- (mbus_cfg->type == V4L2_MBUS_CSI2 \|\|
1618		- mbus_cfg->type == V4L2_MBUS_CCP2) ? "mipi/lvds" : "dvp",
1619		- stream->id);
1620	2930	}
	2931	+ if (buffer) {
	2932	+ if (mbus_cfg->type == V4L2_MBUS_CSI2_DPHY \|\|
	2933	+ mbus_cfg->type == V4L2_MBUS_CSI2_CPHY \|\|
	2934	+ mbus_cfg->type == V4L2_MBUS_CCP2) {
	2935	+ frm_addr_y = frame_phase & CIF_CSI_FRAME0_READY ?
	2936	+ get_reg_index_of_frm0_y_addr(stream->id) :
	2937	+ get_reg_index_of_frm1_y_addr(stream->id);
	2938	+ } else {
	2939	+ frm_addr_y = frame_phase & CIF_CSI_FRAME0_READY ?
	2940	+ get_dvp_reg_index_of_frm0_y_addr(stream->id) :
	2941	+ get_dvp_reg_index_of_frm1_y_addr(stream->id);
	2942	+ }
	2943	+ rkcif_write_register(dev, frm_addr_y,
	2944	+ buffer->dummy.dma_addr);
	2945	+ if (dev->rdbk_debug > 1 &&
	2946	+ stream->frame_idx < 15)
	2947	+ v4l2_info(&dev->v4l2_dev,
	2948	+ "stream[%d] rdbk update, seq %d, reg %x, buf %x\n",
	2949	+ stream->id,
	2950	+ stream->frame_idx - 1,
	2951	+ frm_addr_y, (u32)buffer->dummy.dma_addr);
	2952	+ }
	2953	+ spin_unlock_irqrestore(&stream->vbq_lock, flags);
1621	2954
1622	2955	return ret;
1623	2956	}
..	..	@@ -1626,7 +2959,7 @@
1626	2959	{
1627	2960	struct rkcif_device *dev = stream->cifdev;
1628	2961	struct v4l2_mbus_config *mbus_cfg = &dev->active_sensor->mbus;
1629		- struct rkcif_dummy_buffer *dummy_buf = &dev->dummy_buf;
	2962	+ struct rkcif_dummy_buffer *dummy_buf = &dev->hw_dev->dummy_buf;
1630	2963	unsigned long flags;
1631	2964
1632	2965	spin_lock_irqsave(&stream->vbq_lock, flags);
..	..	@@ -1661,6 +2994,276 @@
1661	2994	rkcif_assign_new_buffer_init(stream, channel_id);
1662	2995	else
1663	2996	ret = rkcif_assign_new_buffer_update(stream, channel_id);
	2997	+ return ret;
	2998	+}
	2999	+
	3000	+static void rkcif_assign_new_buffer_init_rockit(struct rkcif_stream *stream,
	3001	+ int channel_id)
	3002	+{
	3003	+ struct rkcif_device *dev = stream->cifdev;
	3004	+ struct v4l2_mbus_config *mbus_cfg = &dev->active_sensor->mbus;
	3005	+ u32 frm0_addr_y, frm0_addr_uv;
	3006	+ u32 frm1_addr_y, frm1_addr_uv;
	3007	+ unsigned long flags;
	3008	+ struct rkcif_dummy_buffer *dummy_buf = &dev->hw_dev->dummy_buf;
	3009	+ struct csi_channel_info *channel = &dev->channels[channel_id];
	3010	+
	3011	+ if (mbus_cfg->type == V4L2_MBUS_CSI2_DPHY \|\|
	3012	+ mbus_cfg->type == V4L2_MBUS_CSI2_CPHY \|\|
	3013	+ mbus_cfg->type == V4L2_MBUS_CCP2) {
	3014	+ frm0_addr_y = get_reg_index_of_frm0_y_addr(channel_id);
	3015	+ frm0_addr_uv = get_reg_index_of_frm0_uv_addr(channel_id);
	3016	+ frm1_addr_y = get_reg_index_of_frm1_y_addr(channel_id);
	3017	+ frm1_addr_uv = get_reg_index_of_frm1_uv_addr(channel_id);
	3018	+ } else {
	3019	+ frm0_addr_y = get_dvp_reg_index_of_frm0_y_addr(channel_id);
	3020	+ frm0_addr_uv = get_dvp_reg_index_of_frm0_uv_addr(channel_id);
	3021	+ frm1_addr_y = get_dvp_reg_index_of_frm1_y_addr(channel_id);
	3022	+ frm1_addr_uv = get_dvp_reg_index_of_frm1_uv_addr(channel_id);
	3023	+ }
	3024	+
	3025	+ spin_lock_irqsave(&stream->vbq_lock, flags);
	3026	+
	3027	+ if (!stream->curr_buf_rockit) {
	3028	+ if (!list_empty(&stream->rockit_buf_head)) {
	3029	+ stream->curr_buf_rockit = list_first_entry(&stream->rockit_buf_head,
	3030	+ struct rkcif_buffer,
	3031	+ queue);
	3032	+ list_del(&stream->curr_buf_rockit->queue);
	3033	+ }
	3034	+ }
	3035	+
	3036	+ if (stream->curr_buf_rockit) {
	3037	+ rkcif_write_register(dev, frm0_addr_y,
	3038	+ stream->curr_buf_rockit->buff_addr[RKCIF_PLANE_Y]);
	3039	+ if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
	3040	+ rkcif_write_register(dev, frm0_addr_uv,
	3041	+ stream->curr_buf_rockit->buff_addr[RKCIF_PLANE_CBCR]);
	3042	+ } else {
	3043	+ if (dummy_buf->vaddr) {
	3044	+ rkcif_write_register(dev, frm0_addr_y, dummy_buf->dma_addr);
	3045	+ if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
	3046	+ rkcif_write_register(dev, frm0_addr_uv, dummy_buf->dma_addr);
	3047	+ } else {
	3048	+ if (stream->lack_buf_cnt < 2)
	3049	+ stream->lack_buf_cnt++;
	3050	+ }
	3051	+ }
	3052	+
	3053	+ if (stream->cif_fmt_in->field == V4L2_FIELD_INTERLACED) {
	3054	+ stream->next_buf_rockit = stream->curr_buf_rockit;
	3055	+ if (stream->next_buf_rockit) {
	3056	+ rkcif_write_register(dev, frm1_addr_y,
	3057	+ stream->next_buf_rockit->buff_addr[RKCIF_PLANE_Y] + (channel->virtual_width / 2));
	3058	+ if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
	3059	+ rkcif_write_register(dev, frm1_addr_uv,
	3060	+ stream->next_buf_rockit->buff_addr[RKCIF_PLANE_CBCR] + (channel->virtual_width / 2));
	3061	+ }
	3062	+ } else {
	3063	+ if (!stream->next_buf_rockit) {
	3064	+ if (!list_empty(&stream->rockit_buf_head)) {
	3065	+ stream->next_buf_rockit = list_first_entry(&stream->rockit_buf_head,
	3066	+ struct rkcif_buffer, queue);
	3067	+ list_del(&stream->next_buf_rockit->queue);
	3068	+ }
	3069	+ }
	3070	+
	3071	+ if (stream->next_buf_rockit) {
	3072	+ rkcif_write_register(dev, frm1_addr_y,
	3073	+ stream->next_buf_rockit->buff_addr[RKCIF_PLANE_Y]);
	3074	+ if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
	3075	+ rkcif_write_register(dev, frm1_addr_uv,
	3076	+ stream->next_buf_rockit->buff_addr[RKCIF_PLANE_CBCR]);
	3077	+ } else {
	3078	+ if (dummy_buf->vaddr) {
	3079	+ rkcif_write_register(dev, frm1_addr_y, dummy_buf->dma_addr);
	3080	+ if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
	3081	+ rkcif_write_register(dev, frm1_addr_uv, dummy_buf->dma_addr);
	3082	+ } else {
	3083	+ if (stream->curr_buf_rockit) {
	3084	+ stream->next_buf_rockit = stream->curr_buf_rockit;
	3085	+ rkcif_write_register(dev, frm1_addr_y,
	3086	+ stream->next_buf_rockit->buff_addr[RKCIF_PLANE_Y]);
	3087	+ if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
	3088	+ rkcif_write_register(dev, frm1_addr_uv,
	3089	+ stream->next_buf_rockit->buff_addr[RKCIF_PLANE_CBCR]);
	3090	+ }
	3091	+ if (stream->lack_buf_cnt < 2)
	3092	+ stream->lack_buf_cnt++;
	3093	+ }
	3094	+ }
	3095	+ }
	3096	+ spin_unlock_irqrestore(&stream->vbq_lock, flags);
	3097	+
	3098	+ stream->is_dvp_yuv_addr_init = true;
	3099	+
	3100	+ /* for BT.656/BT.1120 multi channels function,
	3101	+ * yuv addr of unused channel must be set
	3102	+ */
	3103	+ if (mbus_cfg->type == V4L2_MBUS_BT656) {
	3104	+ int ch_id;
	3105	+
	3106	+ for (ch_id = 0; ch_id < RKCIF_MAX_STREAM_DVP; ch_id++) {
	3107	+ if (dev->stream[ch_id].is_dvp_yuv_addr_init)
	3108	+ continue;
	3109	+ if (dummy_buf->dma_addr) {
	3110	+ rkcif_write_register(dev,
	3111	+ get_dvp_reg_index_of_frm0_y_addr(ch_id),
	3112	+ dummy_buf->dma_addr);
	3113	+ rkcif_write_register(dev,
	3114	+ get_dvp_reg_index_of_frm0_uv_addr(ch_id),
	3115	+ dummy_buf->dma_addr);
	3116	+ rkcif_write_register(dev,
	3117	+ get_dvp_reg_index_of_frm1_y_addr(ch_id),
	3118	+ dummy_buf->dma_addr);
	3119	+ rkcif_write_register(dev,
	3120	+ get_dvp_reg_index_of_frm1_uv_addr(ch_id),
	3121	+ dummy_buf->dma_addr);
	3122	+ }
	3123	+ }
	3124	+ }
	3125	+ stream->buf_owner = RKCIF_DMAEN_BY_ROCKIT;
	3126	+}
	3127	+
	3128	+static int rkcif_assign_new_buffer_update_rockit(struct rkcif_stream *stream,
	3129	+ int channel_id)
	3130	+{
	3131	+ struct rkcif_device *dev = stream->cifdev;
	3132	+ struct rkcif_dummy_buffer *dummy_buf = &dev->hw_dev->dummy_buf;
	3133	+ struct v4l2_mbus_config *mbus_cfg = &dev->active_sensor->mbus;
	3134	+ struct rkcif_buffer *buffer = NULL;
	3135	+ u32 frm_addr_y, frm_addr_uv;
	3136	+ struct csi_channel_info *channel = &dev->channels[channel_id];
	3137	+ int ret = 0;
	3138	+ unsigned long flags;
	3139	+
	3140	+ if (mbus_cfg->type == V4L2_MBUS_CSI2_DPHY \|\|
	3141	+ mbus_cfg->type == V4L2_MBUS_CSI2_CPHY \|\|
	3142	+ mbus_cfg->type == V4L2_MBUS_CCP2) {
	3143	+ frm_addr_y = stream->frame_phase & CIF_CSI_FRAME0_READY ?
	3144	+ get_reg_index_of_frm0_y_addr(channel_id) :
	3145	+ get_reg_index_of_frm1_y_addr(channel_id);
	3146	+ frm_addr_uv = stream->frame_phase & CIF_CSI_FRAME0_READY ?
	3147	+ get_reg_index_of_frm0_uv_addr(channel_id) :
	3148	+ get_reg_index_of_frm1_uv_addr(channel_id);
	3149	+ } else {
	3150	+ frm_addr_y = stream->frame_phase & CIF_CSI_FRAME0_READY ?
	3151	+ get_dvp_reg_index_of_frm0_y_addr(channel_id) :
	3152	+ get_dvp_reg_index_of_frm1_y_addr(channel_id);
	3153	+ frm_addr_uv = stream->frame_phase & CIF_CSI_FRAME0_READY ?
	3154	+ get_dvp_reg_index_of_frm0_uv_addr(channel_id) :
	3155	+ get_dvp_reg_index_of_frm1_uv_addr(channel_id);
	3156	+ }
	3157	+
	3158	+ spin_lock_irqsave(&stream->vbq_lock, flags);
	3159	+ if (!list_empty(&stream->rockit_buf_head)) {
	3160	+
	3161	+ if (!dummy_buf->vaddr &&
	3162	+ stream->curr_buf_rockit == stream->next_buf_rockit &&
	3163	+ stream->cif_fmt_in->field != V4L2_FIELD_INTERLACED)
	3164	+ ret = -EINVAL;
	3165	+
	3166	+ if (stream->frame_phase == CIF_CSI_FRAME0_READY) {
	3167	+ if (!stream->curr_buf_rockit)
	3168	+ ret = -EINVAL;
	3169	+ stream->curr_buf_rockit = list_first_entry(&stream->rockit_buf_head,
	3170	+ struct rkcif_buffer, queue);
	3171	+ if (stream->curr_buf_rockit) {
	3172	+ list_del(&stream->curr_buf_rockit->queue);
	3173	+ buffer = stream->curr_buf_rockit;
	3174	+ }
	3175	+ } else if (stream->frame_phase == CIF_CSI_FRAME1_READY) {
	3176	+ if (!stream->next_buf_rockit)
	3177	+ ret = -EINVAL;
	3178	+ if (stream->cif_fmt_in->field == V4L2_FIELD_INTERLACED) {
	3179	+ if (stream->next_buf_rockit != stream->curr_buf_rockit) {
	3180	+ stream->next_buf_rockit = stream->curr_buf_rockit;
	3181	+ buffer = stream->next_buf_rockit;
	3182	+ } else {
	3183	+ buffer = NULL;
	3184	+ }
	3185	+
	3186	+ } else {
	3187	+ stream->next_buf_rockit = list_first_entry(&stream->rockit_buf_head,
	3188	+ struct rkcif_buffer, queue);
	3189	+ if (stream->next_buf_rockit) {
	3190	+ list_del(&stream->next_buf_rockit->queue);
	3191	+ buffer = stream->next_buf_rockit;
	3192	+ }
	3193	+ }
	3194	+ }
	3195	+ } else {
	3196	+ buffer = NULL;
	3197	+ if (dummy_buf->vaddr) {
	3198	+ if (stream->frame_phase == CIF_CSI_FRAME0_READY) {
	3199	+ stream->curr_buf_rockit = NULL;
	3200	+ } else if (stream->frame_phase == CIF_CSI_FRAME1_READY) {
	3201	+ if (stream->cif_fmt_in->field == V4L2_FIELD_INTERLACED) {
	3202	+ stream->next_buf_rockit = stream->curr_buf_rockit;
	3203	+ buffer = stream->next_buf_rockit;
	3204	+ } else {
	3205	+ stream->next_buf_rockit = NULL;
	3206	+ }
	3207	+ }
	3208	+ } else if (stream->curr_buf_rockit && stream->next_buf_rockit &&
	3209	+ stream->curr_buf_rockit != stream->next_buf_rockit) {
	3210	+ if (stream->frame_phase == CIF_CSI_FRAME0_READY) {
	3211	+ stream->curr_buf_rockit = stream->next_buf_rockit;
	3212	+ buffer = stream->next_buf_rockit;
	3213	+ } else if (stream->frame_phase == CIF_CSI_FRAME1_READY) {
	3214	+ stream->next_buf_rockit = stream->curr_buf_rockit;
	3215	+ buffer = stream->curr_buf_rockit;
	3216	+ }
	3217	+ if (stream->lack_buf_cnt < 2)
	3218	+ stream->lack_buf_cnt++;
	3219	+ } else {
	3220	+ if (stream->lack_buf_cnt < 2)
	3221	+ stream->lack_buf_cnt++;
	3222	+ }
	3223	+ }
	3224	+ stream->frame_phase_cache = stream->frame_phase;
	3225	+
	3226	+ if (buffer) {
	3227	+ if (stream->cif_fmt_in->field == V4L2_FIELD_INTERLACED &&
	3228	+ stream->frame_phase == CIF_CSI_FRAME1_READY) {
	3229	+ rkcif_write_register(dev, frm_addr_y,
	3230	+ buffer->buff_addr[RKCIF_PLANE_Y] + (channel->virtual_width / 2));
	3231	+ if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
	3232	+ rkcif_write_register(dev, frm_addr_uv,
	3233	+ buffer->buff_addr[RKCIF_PLANE_CBCR] + (channel->virtual_width / 2));
	3234	+ } else {
	3235	+ rkcif_write_register(dev, frm_addr_y,
	3236	+ buffer->buff_addr[RKCIF_PLANE_Y]);
	3237	+ if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
	3238	+ rkcif_write_register(dev, frm_addr_uv,
	3239	+ buffer->buff_addr[RKCIF_PLANE_CBCR]);
	3240	+ }
	3241	+ } else {
	3242	+ if (dummy_buf->vaddr) {
	3243	+ rkcif_write_register(dev, frm_addr_y, dummy_buf->dma_addr);
	3244	+ if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
	3245	+ rkcif_write_register(dev, frm_addr_uv, dummy_buf->dma_addr);
	3246	+ dev->err_state \|= (RKCIF_ERR_ID0_NOT_BUF << stream->id);
	3247	+ dev->irq_stats.not_active_buf_cnt[stream->id]++;
	3248	+ } else {
	3249	+ ret = -EINVAL;
	3250	+ dev->err_state \|= (RKCIF_ERR_ID0_NOT_BUF << stream->id);
	3251	+ dev->irq_stats.not_active_buf_cnt[stream->id]++;
	3252	+ }
	3253	+ }
	3254	+ spin_unlock_irqrestore(&stream->vbq_lock, flags);
	3255	+ return ret;
	3256	+}
	3257	+
	3258	+static int rkcif_assign_new_buffer_pingpong_rockit(struct rkcif_stream *stream,
	3259	+ int init, int channel_id)
	3260	+{
	3261	+ int ret = 0;
	3262	+
	3263	+ if (init)
	3264	+ rkcif_assign_new_buffer_init_rockit(stream, channel_id);
	3265	+ else
	3266	+ ret = rkcif_assign_new_buffer_update_rockit(stream, channel_id);
1664	3267	return ret;
1665	3268	}
1666	3269
..	..	@@ -1712,7 +3315,7 @@
1712	3315	struct rkmodule_lvds_frm_sync_code *odd_sync_code = NULL;
1713	3316	struct rkmodule_lvds_frm_sync_code *even_sync_code = NULL;
1714	3317
1715		- if (dev->hdr.mode == NO_HDR) {
	3318	+ if (dev->hdr.hdr_mode == NO_HDR \|\| dev->hdr.hdr_mode == HDR_COMPR) {
1716	3319	frm_sync_code = &lvds_cfg->frm_sync_code[LVDS_CODE_GRP_LINEAR];
1717	3320	odd_sync_code = &frm_sync_code->odd_sync_code;
1718	3321	even_sync_code = odd_sync_code;
..	..	@@ -1720,12 +3323,12 @@
1720	3323	if (channel->id == RKCIF_STREAM_MIPI_ID0)
1721	3324	frm_sync_code = &lvds_cfg->frm_sync_code[LVDS_CODE_GRP_LONG];
1722	3325
1723		- if (dev->hdr.mode == HDR_X2) {
	3326	+ if (dev->hdr.hdr_mode == HDR_X2) {
1724	3327	if (channel->id == RKCIF_STREAM_MIPI_ID1)
1725	3328	frm_sync_code = &lvds_cfg->frm_sync_code[LVDS_CODE_GRP_SHORT];
1726	3329	else
1727	3330	frm_sync_code = &lvds_cfg->frm_sync_code[LVDS_CODE_GRP_LONG];
1728		- } else if (dev->hdr.mode == HDR_X3) {
	3331	+ } else if (dev->hdr.hdr_mode == HDR_X3) {
1729	3332	if (channel->id == RKCIF_STREAM_MIPI_ID1)
1730	3333	frm_sync_code = &lvds_cfg->frm_sync_code[LVDS_CODE_GRP_MEDIUM];
1731	3334	else if (channel->id == RKCIF_STREAM_MIPI_ID2)
..	..	@@ -1761,12 +3364,44 @@
1761	3364	}
1762	3365	}
1763	3366
	3367	+static unsigned char get_csi_fmt_val(const struct cif_input_fmt *cif_fmt_in,
	3368	+ struct csi_channel_info *csi_info)
	3369	+{
	3370	+ unsigned char csi_fmt_val = 0;
	3371	+
	3372	+ if (cif_fmt_in->mbus_code == MEDIA_BUS_FMT_SPD_2X8 \|\|
	3373	+ cif_fmt_in->mbus_code == MEDIA_BUS_FMT_EBD_1X8) {
	3374	+ switch (csi_info->data_bit) {
	3375	+ case 8:
	3376	+ csi_fmt_val = CSI_WRDDR_TYPE_RAW8;
	3377	+ break;
	3378	+ case 10:
	3379	+ csi_fmt_val = CSI_WRDDR_TYPE_RAW10;
	3380	+ break;
	3381	+ case 12:
	3382	+ csi_fmt_val = CSI_WRDDR_TYPE_RAW12;
	3383	+ break;
	3384	+ default:
	3385	+ csi_fmt_val = CSI_WRDDR_TYPE_RAW12;
	3386	+ break;
	3387	+ }
	3388	+ } else if (cif_fmt_in->csi_fmt_val == CSI_WRDDR_TYPE_RGB888 \|\|
	3389	+ cif_fmt_in->csi_fmt_val == CSI_WRDDR_TYPE_RGB565) {
	3390	+ csi_fmt_val = CSI_WRDDR_TYPE_RAW8;
	3391	+ } else {
	3392	+ csi_fmt_val = cif_fmt_in->csi_fmt_val;
	3393	+ }
	3394	+ return csi_fmt_val;
	3395	+}
	3396	+
1764	3397	static int rkcif_csi_channel_init(struct rkcif_stream *stream,
1765	3398	struct csi_channel_info *channel)
1766	3399	{
1767	3400	struct rkcif_device *dev = stream->cifdev;
	3401	+ struct sditf_priv *priv = dev->sditf[0];
1768	3402	const struct cif_output_fmt *fmt;
1769	3403	u32 fourcc;
	3404	+ int vc = dev->channels[stream->id].vc;
1770	3405
1771	3406	channel->enable = 1;
1772	3407	channel->width = stream->pixm.width;
..	..	@@ -1782,10 +3417,14 @@
1782	3417
1783	3418	if (channel->fmt_val == CSI_WRDDR_TYPE_RGB888)
1784	3419	channel->crop_st_x = 3 * stream->crop[CROP_SRC_ACT].left;
	3420	+ else if (channel->fmt_val == CSI_WRDDR_TYPE_RGB565)
	3421	+ channel->crop_st_x = 2 * stream->crop[CROP_SRC_ACT].left;
1785	3422	else
1786	3423	channel->crop_st_x = stream->crop[CROP_SRC_ACT].left;
1787	3424
1788	3425	channel->crop_st_y = stream->crop[CROP_SRC_ACT].top;
	3426	+ if (priv && priv->is_combine_mode && dev->sditf_cnt <= RKCIF_MAX_SDITF)
	3427	+ channel->crop_st_y *= dev->sditf_cnt;
1789	3428	channel->width = stream->crop[CROP_SRC_ACT].width;
1790	3429	channel->height = stream->crop[CROP_SRC_ACT].height;
1791	3430	} else {
..	..	@@ -1794,13 +3433,18 @@
1794	3433	channel->crop_en = 0;
1795	3434	}
1796	3435
1797		- fmt = find_output_fmt(stream, stream->pixm.pixelformat);
	3436	+ if (priv && priv->is_combine_mode && dev->sditf_cnt <= RKCIF_MAX_SDITF)
	3437	+ channel->height *= dev->sditf_cnt;
	3438	+
	3439	+ fmt = rkcif_find_output_fmt(stream, stream->pixm.pixelformat);
1798	3440	if (!fmt) {
1799	3441	v4l2_err(&dev->v4l2_dev, "can not find output format: 0x%x",
1800	3442	stream->pixm.pixelformat);
1801	3443	return -EINVAL;
1802	3444	}
1803	3445
	3446	+ if (channel->capture_info.mode == RKMODULE_MULTI_DEV_COMBINE_ONE)
	3447	+ channel->width /= channel->capture_info.multi_dev.dev_num;
1804	3448	/*
1805	3449	* for mipi or lvds, when enable compact, the virtual width of raw10/raw12
1806	3450	* needs aligned with :ALIGN(bits_per_pixel * width / 8, 8), if enable 16bit mode
..	..	@@ -1808,13 +3452,24 @@
1808	3452	* writing of ddr, aliged with 256
1809	3453	*/
1810	3454	if (fmt->fmt_type == CIF_FMT_TYPE_RAW && stream->is_compact &&
1811		- fmt->csi_fmt_val != CSI_WRDDR_TYPE_RGB888) {
1812		- channel->virtual_width = ALIGN(channel->width * fmt->raw_bpp / 8, 256);
	3455	+ fmt->csi_fmt_val != CSI_WRDDR_TYPE_RGB888 &&
	3456	+ fmt->csi_fmt_val != CSI_WRDDR_TYPE_RGB565) {
	3457	+ if (channel->capture_info.mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	3458	+ channel->virtual_width = ALIGN(channel->width * 2 * fmt->raw_bpp / 8, 256);
	3459	+ channel->left_virtual_width = channel->width * fmt->raw_bpp / 8;
	3460	+ } else {
	3461	+ channel->virtual_width = ALIGN(channel->width * fmt->raw_bpp / 8, 256);
	3462	+ }
1813	3463	} else {
1814		- channel->virtual_width = ALIGN(channel->width * fmt->bpp[0] / 8, 8);
	3464	+ if (channel->capture_info.mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	3465	+ channel->virtual_width = ALIGN(channel->width * 2 * fmt->bpp[0] / 8, 8);
	3466	+ channel->left_virtual_width = ALIGN(channel->width * fmt->bpp[0] / 8, 8);
	3467	+ } else {
	3468	+ channel->virtual_width = ALIGN(channel->width * fmt->bpp[0] / 8, 8);
	3469	+ }
1815	3470	}
1816	3471
1817		- if (channel->fmt_val == CSI_WRDDR_TYPE_RGB888)
	3472	+ if (channel->fmt_val == CSI_WRDDR_TYPE_RGB888 \|\| channel->fmt_val == CSI_WRDDR_TYPE_RGB565)
1818	3473	channel->width = channel->width * fmt->bpp[0] / 8;
1819	3474	/*
1820	3475	* rk cif don't support output yuyv fmt data
..	..	@@ -1822,36 +3477,53 @@
1822	3477	* and the width is double Because the real input fmt is
1823	3478	* yuyv
1824	3479	*/
1825		- fourcc = stream->cif_fmt_out->fourcc;
	3480	+ fourcc = stream->cif_fmt_out->fourcc;
1826	3481	if (fourcc == V4L2_PIX_FMT_YUYV \|\| fourcc == V4L2_PIX_FMT_YVYU \|\|
1827	3482	fourcc == V4L2_PIX_FMT_UYVY \|\| fourcc == V4L2_PIX_FMT_VYUY) {
1828		- channel->fmt_val = CSI_WRDDR_TYPE_RAW8;
1829		- channel->width *= 2;
	3483	+ if (dev->chip_id < CHIP_RK3588_CIF) {
	3484	+ channel->fmt_val = CSI_WRDDR_TYPE_RAW8;
	3485	+ channel->width *= 2;
	3486	+ }
1830	3487	channel->virtual_width *= 2;
	3488	+ if (channel->capture_info.mode == RKMODULE_MULTI_DEV_COMBINE_ONE)
	3489	+ channel->left_virtual_width *= 2;
1831	3490	}
1832	3491	if (stream->cif_fmt_in->field == V4L2_FIELD_INTERLACED) {
1833	3492	channel->virtual_width *= 2;
1834	3493	channel->height /= 2;
1835	3494	}
	3495	+ if (stream->cif_fmt_in->mbus_code == MEDIA_BUS_FMT_EBD_1X8 \|\|
	3496	+ stream->cif_fmt_in->mbus_code == MEDIA_BUS_FMT_SPD_2X8) {
	3497	+ if (dev->channels[stream->id].data_type)
	3498	+ channel->data_type = dev->channels[stream->id].data_type;
	3499	+ else
	3500	+ channel->data_type = get_data_type(stream->cif_fmt_in->mbus_code,
	3501	+ channel->cmd_mode_en,
	3502	+ channel->dsi_input);
	3503	+ } else {
	3504	+ channel->data_type = get_data_type(stream->cif_fmt_in->mbus_code,
	3505	+ channel->cmd_mode_en,
	3506	+ channel->dsi_input);
	3507	+ }
	3508	+ channel->csi_fmt_val = get_csi_fmt_val(stream->cif_fmt_in,
	3509	+ &dev->channels[stream->id]);
1836	3510
1837		- channel->data_type = get_data_type(stream->cif_fmt_in->mbus_code,
1838		- channel->cmd_mode_en, channel->dsi_input);
1839		-
1840		- if ((dev->hdr.mode == NO_HDR && stream->vc >= 0) \|\|
1841		- (dev->hdr.mode == HDR_X2 && stream->vc > 1) \|\|
1842		- (dev->hdr.mode == HDR_X3 && stream->vc > 2))
1843		- channel->vc = stream->vc;
	3511	+ if (dev->hdr.hdr_mode == NO_HDR \|\|
	3512	+ dev->hdr.hdr_mode == HDR_COMPR \|\|
	3513	+ (dev->hdr.hdr_mode == HDR_X2 && stream->id > 1) \|\|
	3514	+ (dev->hdr.hdr_mode == HDR_X3 && stream->id > 2))
	3515	+ channel->vc = vc < 4 ? vc : channel->id;
1844	3516	else
1845	3517	channel->vc = channel->id;
1846		- v4l2_dbg(3, rkcif_debug, &dev->v4l2_dev,
	3518	+ v4l2_dbg(1, rkcif_debug, &dev->v4l2_dev,
1847	3519	"%s: channel width %d, height %d, virtual_width %d, vc %d\n", __func__,
1848	3520	channel->width, channel->height, channel->virtual_width, channel->vc);
1849	3521	return 0;
1850	3522	}
1851	3523
1852	3524	static int rkcif_csi_channel_set(struct rkcif_stream *stream,
1853		- struct csi_channel_info *channel,
1854		- enum v4l2_mbus_type mbus_type)
	3525	+ struct csi_channel_info *channel,
	3526	+ enum v4l2_mbus_type mbus_type)
1855	3527	{
1856	3528	unsigned int val = 0x0;
1857	3529	struct rkcif_device *dev = stream->cifdev;
..	..	@@ -1872,10 +3544,9 @@
1872	3544	CSI_DMA_END_INTSTAT(channel->id) \|
1873	3545	CSI_LINE_INTSTAT(channel->id)));
1874	3546
1875		- /* enable id0 frame start int for sof(long frame, for hdr) */
1876		- if (channel->id == RKCIF_STREAM_MIPI_ID0)
1877		- rkcif_write_register_or(dev, CIF_REG_MIPI_LVDS_INTEN,
1878		- CSI_START_INTEN(channel->id));
	3547	+ rkcif_write_register_or(dev, CIF_REG_MIPI_LVDS_INTEN,
	3548	+ CSI_START_INTEN(channel->id));
	3549	+
1879	3550	if (detect_stream->is_line_wake_up) {
1880	3551	rkcif_write_register_or(dev, CIF_REG_MIPI_LVDS_INTEN,
1881	3552	CSI_LINE_INTEN(channel->id));
..	..	@@ -1894,15 +3565,15 @@
1894	3565	CIF_MIPI_LVDS_SW_HURRY_VALUE_RK1808(0x3) \|
1895	3566	CIF_MIPI_LVDS_SW_HURRY_ENABLE_RK1808);
1896	3567
1897		- val = CIF_MIPI_LVDS_SW_PRESS_VALUE(0x7) \|
	3568	+ val = CIF_MIPI_LVDS_SW_PRESS_VALUE(0x3) \|
1898	3569	CIF_MIPI_LVDS_SW_PRESS_ENABLE \|
1899		- CIF_MIPI_LVDS_SW_HURRY_VALUE(0x7) \|
	3570	+ CIF_MIPI_LVDS_SW_HURRY_VALUE(0x3) \|
1900	3571	CIF_MIPI_LVDS_SW_HURRY_ENABLE \|
1901	3572	CIF_MIPI_LVDS_SW_WATER_LINE_25 \|
1902	3573	CIF_MIPI_LVDS_SW_WATER_LINE_ENABLE;
1903		- if (mbus_type == V4L2_MBUS_CSI2) {
	3574	+ if (mbus_type == V4L2_MBUS_CSI2_DPHY) {
1904	3575	val &= ~CIF_MIPI_LVDS_SW_SEL_LVDS;
1905		- } else if (mbus_type == V4L2_MBUS_CCP2) {
	3576	+ } else if (mbus_type == V4L2_MBUS_CCP2) {
1906	3577	if (channel->fmt_val == CSI_WRDDR_TYPE_RAW12)
1907	3578	val \|= CIF_MIPI_LVDS_SW_LVDS_WIDTH_12BITS;
1908	3579	else if (channel->fmt_val == CSI_WRDDR_TYPE_RAW10)
..	..	@@ -1937,7 +3608,7 @@
1937	3608	RKCIF_YUV_ADDR_STATE_INIT,
1938	3609	channel->id);
1939	3610
1940		- if (mbus_type == V4L2_MBUS_CSI2) {
	3611	+ if (mbus_type == V4L2_MBUS_CSI2_DPHY) {
1941	3612	//need always enable crop
1942	3613	val = CSI_ENABLE_CAPTURE \| channel->fmt_val \|
1943	3614	channel->cmd_mode_en << 4 \| CSI_ENABLE_CROP \|
..	..	@@ -1949,7 +3620,7 @@
1949	3620
1950	3621	if (stream->cifdev->chip_id >= CHIP_RK3568_CIF)
1951	3622	val \|= stream->cif_fmt_in->csi_yuv_order;
1952		- } else if (mbus_type == V4L2_MBUS_CCP2) {
	3623	+ } else if (mbus_type == V4L2_MBUS_CCP2) {
1953	3624	rkcif_csi_set_lvds_sav_eav(stream, channel);
1954	3625	val = LVDS_ENABLE_CAPTURE \| LVDS_MODE(channel->lvds_cfg.mode) \|
1955	3626	LVDS_MAIN_LANE(0) \| LVDS_FID(0) \|
..	..	@@ -1961,15 +3632,405 @@
1961	3632	val &= ~LVDS_COMPACT;
1962	3633	}
1963	3634	if (stream->is_high_align)
1964		- val \|= CSI_ENABLE_MIPI_HIGH_ALIGN;
	3635	+ val \|= CSI_HIGH_ALIGN;
1965	3636	else
1966		- val &= ~CSI_ENABLE_MIPI_HIGH_ALIGN;
	3637	+ val &= ~CSI_HIGH_ALIGN;
1967	3638	rkcif_write_register(dev, get_reg_index_of_id_ctrl0(channel->id), val);
1968	3639
	3640	+ dev->intr_mask = rkcif_read_register(dev, CIF_REG_MIPI_LVDS_INTEN);
1969	3641	return 0;
1970	3642	}
1971	3643
1972		-static int rkcif_csi_stream_start(struct rkcif_stream *stream)
	3644	+static int rkcif_dvp_get_input_yuv_order(struct rkcif_stream *stream)
	3645	+{
	3646	+ unsigned int mask;
	3647	+ const struct cif_input_fmt *fmt = stream->cif_fmt_in;
	3648	+
	3649	+ switch (fmt->mbus_code) {
	3650	+ case MEDIA_BUS_FMT_UYVY8_2X8:
	3651	+ mask = CSI_YUV_INPUT_ORDER_UYVY >> 11;
	3652	+ break;
	3653	+ case MEDIA_BUS_FMT_VYUY8_2X8:
	3654	+ mask = CSI_YUV_INPUT_ORDER_VYUY >> 11;
	3655	+ break;
	3656	+ case MEDIA_BUS_FMT_YUYV8_2X8:
	3657	+ mask = CSI_YUV_INPUT_ORDER_YUYV >> 11;
	3658	+ break;
	3659	+ case MEDIA_BUS_FMT_YVYU8_2X8:
	3660	+ mask = CSI_YUV_INPUT_ORDER_YVYU >> 11;
	3661	+ break;
	3662	+ default:
	3663	+ mask = CSI_YUV_INPUT_ORDER_UYVY >> 11;
	3664	+ break;
	3665	+ }
	3666	+ return mask;
	3667	+}
	3668	+
	3669	+static int rkcif_csi_get_output_type_mask(struct rkcif_stream *stream)
	3670	+{
	3671	+ unsigned int mask;
	3672	+ const struct cif_output_fmt *fmt = stream->cif_fmt_out;
	3673	+
	3674	+ switch (fmt->fourcc) {
	3675	+ case V4L2_PIX_FMT_NV16:
	3676	+ mask = CSI_WRDDR_TYPE_YUV422SP_RK3588 \| CSI_YUV_OUTPUT_ORDER_UYVY;
	3677	+ break;
	3678	+ case V4L2_PIX_FMT_NV61:
	3679	+ mask = CSI_WRDDR_TYPE_YUV422SP_RK3588 \| CSI_YUV_OUTPUT_ORDER_VYUY;
	3680	+ break;
	3681	+ case V4L2_PIX_FMT_NV12:
	3682	+ mask = CSI_WRDDR_TYPE_YUV420SP_RK3588 \| CSI_YUV_OUTPUT_ORDER_UYVY;
	3683	+ break;
	3684	+ case V4L2_PIX_FMT_NV21:
	3685	+ mask = CSI_WRDDR_TYPE_YUV420SP_RK3588 \| CSI_YUV_OUTPUT_ORDER_VYUY;
	3686	+ break;
	3687	+ case V4L2_PIX_FMT_YUYV:
	3688	+ mask = CSI_WRDDR_TYPE_YUV_PACKET \| CSI_YUV_OUTPUT_ORDER_YUYV;
	3689	+ break;
	3690	+ case V4L2_PIX_FMT_YVYU:
	3691	+ mask = CSI_WRDDR_TYPE_YUV_PACKET \| CSI_YUV_OUTPUT_ORDER_YVYU;
	3692	+ break;
	3693	+ case V4L2_PIX_FMT_UYVY:
	3694	+ mask = CSI_WRDDR_TYPE_YUV_PACKET \| CSI_YUV_OUTPUT_ORDER_UYVY;
	3695	+ break;
	3696	+ case V4L2_PIX_FMT_VYUY:
	3697	+ mask = CSI_WRDDR_TYPE_YUV_PACKET \| CSI_YUV_OUTPUT_ORDER_VYUY;
	3698	+ break;
	3699	+ case V4L2_PIX_FMT_RGB24:
	3700	+ case V4L2_PIX_FMT_BGR24:
	3701	+ case V4L2_PIX_FMT_RGB565:
	3702	+ case V4L2_PIX_FMT_BGR666:
	3703	+ mask = CSI_WRDDR_TYPE_RAW_COMPACT;
	3704	+ break;
	3705	+ case V4L2_PIX_FMT_SRGGB8:
	3706	+ case V4L2_PIX_FMT_SGRBG8:
	3707	+ case V4L2_PIX_FMT_SGBRG8:
	3708	+ case V4L2_PIX_FMT_SBGGR8:
	3709	+ case V4L2_PIX_FMT_SRGGB10:
	3710	+ case V4L2_PIX_FMT_SGRBG10:
	3711	+ case V4L2_PIX_FMT_SGBRG10:
	3712	+ case V4L2_PIX_FMT_SBGGR10:
	3713	+ case V4L2_PIX_FMT_SRGGB12:
	3714	+ case V4L2_PIX_FMT_SGRBG12:
	3715	+ case V4L2_PIX_FMT_SGBRG12:
	3716	+ case V4L2_PIX_FMT_SBGGR12:
	3717	+ case V4L2_PIX_FMT_GREY:
	3718	+ case V4L2_PIX_FMT_Y10:
	3719	+ case V4L2_PIX_FMT_Y12:
	3720	+ if (stream->is_compact)
	3721	+ mask = CSI_WRDDR_TYPE_RAW_COMPACT;
	3722	+ else
	3723	+ mask = CSI_WRDDR_TYPE_RAW_UNCOMPACT;
	3724	+ break;
	3725	+ case V4L2_PIX_FMT_SBGGR16:
	3726	+ case V4L2_PIX_FMT_SGBRG16:
	3727	+ case V4L2_PIX_FMT_SGRBG16:
	3728	+ case V4L2_PIX_FMT_SRGGB16:
	3729	+ case V4L2_PIX_FMT_Y16:
	3730	+ mask = CSI_WRDDR_TYPE_RAW_UNCOMPACT;
	3731	+ break;
	3732	+ default:
	3733	+ mask = CSI_WRDDR_TYPE_RAW_COMPACT;
	3734	+ break;
	3735	+ }
	3736	+ return mask;
	3737	+}
	3738	+
	3739	+static int rkcif_lvds_get_output_type_mask(struct rkcif_stream *stream)
	3740	+{
	3741	+ unsigned int mask;
	3742	+ const struct cif_output_fmt *fmt = stream->cif_fmt_out;
	3743	+ int wr_type_offset = 0;
	3744	+ int yuvout_offset = 0;
	3745	+
	3746	+ if (stream->cifdev->chip_id == CHIP_RV1106_CIF) {
	3747	+ wr_type_offset = 17;
	3748	+ yuvout_offset = 9;
	3749	+ }
	3750	+
	3751	+ switch (fmt->fourcc) {
	3752	+ case V4L2_PIX_FMT_NV16:
	3753	+ mask = (CSI_WRDDR_TYPE_YUV422SP_RK3588 << wr_type_offset) \|
	3754	+ (CSI_YUV_OUTPUT_ORDER_UYVY << yuvout_offset);
	3755	+ break;
	3756	+ case V4L2_PIX_FMT_NV61:
	3757	+ mask = (CSI_WRDDR_TYPE_YUV422SP_RK3588 << wr_type_offset) \|
	3758	+ (CSI_YUV_OUTPUT_ORDER_VYUY << yuvout_offset);
	3759	+ break;
	3760	+ case V4L2_PIX_FMT_NV12:
	3761	+ mask = (CSI_WRDDR_TYPE_YUV420SP_RK3588 << wr_type_offset) \|
	3762	+ (CSI_YUV_OUTPUT_ORDER_UYVY << yuvout_offset);
	3763	+ break;
	3764	+ case V4L2_PIX_FMT_NV21:
	3765	+ mask = (CSI_WRDDR_TYPE_YUV420SP_RK3588 << wr_type_offset) \|
	3766	+ (CSI_YUV_OUTPUT_ORDER_VYUY << yuvout_offset);
	3767	+ break;
	3768	+ case V4L2_PIX_FMT_YUYV:
	3769	+ mask = (CSI_WRDDR_TYPE_YUV_PACKET << wr_type_offset) \|
	3770	+ (CSI_YUV_OUTPUT_ORDER_YUYV << yuvout_offset);
	3771	+ break;
	3772	+ case V4L2_PIX_FMT_YVYU:
	3773	+ mask = (CSI_WRDDR_TYPE_YUV_PACKET << wr_type_offset) \|
	3774	+ (CSI_YUV_OUTPUT_ORDER_YVYU << yuvout_offset);
	3775	+ break;
	3776	+ case V4L2_PIX_FMT_UYVY:
	3777	+ mask = (CSI_WRDDR_TYPE_YUV_PACKET << wr_type_offset) \|
	3778	+ (CSI_YUV_OUTPUT_ORDER_UYVY << yuvout_offset);
	3779	+ break;
	3780	+ case V4L2_PIX_FMT_VYUY:
	3781	+ mask = (CSI_WRDDR_TYPE_YUV_PACKET << wr_type_offset) \|
	3782	+ (CSI_YUV_OUTPUT_ORDER_VYUY << yuvout_offset);
	3783	+ break;
	3784	+ case V4L2_PIX_FMT_RGB24:
	3785	+ case V4L2_PIX_FMT_BGR24:
	3786	+ case V4L2_PIX_FMT_RGB565:
	3787	+ case V4L2_PIX_FMT_BGR666:
	3788	+ mask = CSI_WRDDR_TYPE_RAW_COMPACT << wr_type_offset;
	3789	+ break;
	3790	+ case V4L2_PIX_FMT_SRGGB8:
	3791	+ case V4L2_PIX_FMT_SGRBG8:
	3792	+ case V4L2_PIX_FMT_SGBRG8:
	3793	+ case V4L2_PIX_FMT_SBGGR8:
	3794	+ case V4L2_PIX_FMT_SRGGB10:
	3795	+ case V4L2_PIX_FMT_SGRBG10:
	3796	+ case V4L2_PIX_FMT_SGBRG10:
	3797	+ case V4L2_PIX_FMT_SBGGR10:
	3798	+ case V4L2_PIX_FMT_SRGGB12:
	3799	+ case V4L2_PIX_FMT_SGRBG12:
	3800	+ case V4L2_PIX_FMT_SGBRG12:
	3801	+ case V4L2_PIX_FMT_SBGGR12:
	3802	+ case V4L2_PIX_FMT_GREY:
	3803	+ case V4L2_PIX_FMT_Y10:
	3804	+ case V4L2_PIX_FMT_Y12:
	3805	+ if (stream->is_compact)
	3806	+ mask = CSI_WRDDR_TYPE_RAW_COMPACT << wr_type_offset;
	3807	+ else
	3808	+ mask = CSI_WRDDR_TYPE_RAW_UNCOMPACT << wr_type_offset;
	3809	+ break;
	3810	+ case V4L2_PIX_FMT_SBGGR16:
	3811	+ case V4L2_PIX_FMT_SGBRG16:
	3812	+ case V4L2_PIX_FMT_SGRBG16:
	3813	+ case V4L2_PIX_FMT_SRGGB16:
	3814	+ case V4L2_PIX_FMT_Y16:
	3815	+ mask = CSI_WRDDR_TYPE_RAW_UNCOMPACT << wr_type_offset;
	3816	+ break;
	3817	+ default:
	3818	+ mask = CSI_WRDDR_TYPE_RAW_COMPACT << wr_type_offset;
	3819	+ break;
	3820	+ }
	3821	+ return mask;
	3822	+}
	3823	+
	3824	+static void rkcif_modify_frame_skip_config(struct rkcif_stream *stream)
	3825	+{
	3826	+ if (stream->skip_info.skip_to_en) {
	3827	+ rkcif_disable_skip_frame(stream);
	3828	+ rkcif_enable_skip_frame(stream,
	3829	+ stream->skip_info.cap_m,
	3830	+ stream->skip_info.skip_n);
	3831	+ stream->skip_info.skip_to_en = false;
	3832	+ } else if (stream->skip_info.skip_to_dis) {
	3833	+ rkcif_disable_skip_frame(stream);
	3834	+ }
	3835	+}
	3836	+
	3837	+/config reg for rk3588/
	3838	+static int rkcif_csi_channel_set_v1(struct rkcif_stream *stream,
	3839	+ struct csi_channel_info *channel,
	3840	+ enum v4l2_mbus_type mbus_type, unsigned int mode,
	3841	+ int index)
	3842	+{
	3843	+ unsigned int val = 0x0;
	3844	+ struct rkcif_device *dev = stream->cifdev;
	3845	+ struct rkcif_stream *detect_stream = &dev->stream[0];
	3846	+ struct sditf_priv *priv = dev->sditf[0];
	3847	+ struct rkmodule_capture_info *capture_info = &channel->capture_info;
	3848	+ unsigned int wait_line = 0x3fff;
	3849	+ unsigned int dma_en = 0;
	3850	+ int offset = 0;
	3851	+
	3852	+ if (channel->id >= 4)
	3853	+ return -EINVAL;
	3854	+
	3855	+ if (!channel->enable) {
	3856	+ rkcif_write_register(dev, get_reg_index_of_id_ctrl0(channel->id),
	3857	+ CSI_DISABLE_CAPTURE);
	3858	+ return 0;
	3859	+ }
	3860	+
	3861	+ rkcif_write_register_and(dev, CIF_REG_MIPI_LVDS_INTSTAT,
	3862	+ ~(CSI_START_INTSTAT(channel->id) \|
	3863	+ CSI_DMA_END_INTSTAT(channel->id) \|
	3864	+ CSI_LINE_INTSTAT_V1(channel->id)));
	3865	+
	3866	+ if (!(capture_info->mode == RKMODULE_MULTI_DEV_COMBINE_ONE &&
	3867	+ index < capture_info->multi_dev.dev_num - 1)) {
	3868	+
	3869	+ rkcif_write_register_or(dev, CIF_REG_MIPI_LVDS_INTEN,
	3870	+ CSI_START_INTEN(channel->id));
	3871	+
	3872	+ if (priv && priv->mode.rdbk_mode && detect_stream->is_line_wake_up) {
	3873	+ rkcif_write_register_or(dev, CIF_REG_MIPI_LVDS_INTEN,
	3874	+ CSI_LINE_INTEN_RK3588(channel->id));
	3875	+ wait_line = dev->wait_line;
	3876	+ }
	3877	+ rkcif_write_register(dev, CIF_REG_MIPI_LVDS_LINE_INT_NUM_ID0_1,
	3878	+ wait_line << 16 \| wait_line);
	3879	+ rkcif_write_register(dev, CIF_REG_MIPI_LVDS_LINE_INT_NUM_ID2_3,
	3880	+ wait_line << 16 \| wait_line);
	3881	+
	3882	+ rkcif_write_register_or(dev, CIF_REG_MIPI_LVDS_INTEN,
	3883	+ CSI_DMA_END_INTEN(channel->id));
	3884	+
	3885	+ rkcif_write_register_or(dev, CIF_REG_MIPI_LVDS_INTEN,
	3886	+ CSI_ALL_ERROR_INTEN_V1);
	3887	+ }
	3888	+ if (stream->cifdev->id_use_cnt == 0) {
	3889	+ val = CIF_MIPI_LVDS_SW_PRESS_VALUE_RK3588(0x3) \|
	3890	+ CIF_MIPI_LVDS_SW_PRESS_ENABLE \|
	3891	+ CIF_MIPI_LVDS_SW_HURRY_VALUE_RK3588(0x3) \|
	3892	+ CIF_MIPI_LVDS_SW_HURRY_ENABLE \|
	3893	+ CIF_MIPI_LVDS_SW_WATER_LINE_25 \|
	3894	+ CIF_MIPI_LVDS_SW_WATER_LINE_ENABLE;
	3895	+ if (mbus_type == V4L2_MBUS_CSI2_DPHY \|\|
	3896	+ mbus_type == V4L2_MBUS_CSI2_CPHY)
	3897	+ val &= ~CIF_MIPI_LVDS_SW_SEL_LVDS_RV1106;
	3898	+ else
	3899	+ val \|= CIF_MIPI_LVDS_SW_SEL_LVDS_RV1106;
	3900	+ rkcif_write_register(dev, CIF_REG_MIPI_LVDS_CTRL, val);
	3901	+ }
	3902	+#if IS_ENABLED(CONFIG_CPU_RV1106)
	3903	+ if (channel->id == 1)
	3904	+ rv1106_sdmmc_get_lock();
	3905	+#endif
	3906	+ if (capture_info->mode == RKMODULE_MULTI_DEV_COMBINE_ONE &&
	3907	+ priv && priv->mode.rdbk_mode == RKISP_VICAP_ONLINE &&
	3908	+ (dev->hdr.hdr_mode == NO_HDR \|\|
	3909	+ (dev->hdr.hdr_mode == HDR_X2 && stream->id == 1) \|\|
	3910	+ (dev->hdr.hdr_mode == HDR_X3 && stream->id == 2)))
	3911	+ offset = channel->capture_info.multi_dev.pixel_offset;
	3912	+
	3913	+ rkcif_write_register(dev, get_reg_index_of_id_ctrl1(channel->id),
	3914	+ (channel->width + offset) \| (channel->height << 16));
	3915	+
	3916	+#if IS_ENABLED(CONFIG_CPU_RV1106)
	3917	+ if (channel->id == 1)
	3918	+ rv1106_sdmmc_put_lock();
	3919	+#endif
	3920	+
	3921	+ if (channel->crop_en)
	3922	+ rkcif_write_register(dev, get_reg_index_of_id_crop_start(channel->id),
	3923	+ channel->crop_st_y << 16 \| channel->crop_st_x);
	3924	+
	3925	+ if (mode == RKCIF_STREAM_MODE_CAPTURE)
	3926	+ rkcif_assign_new_buffer_pingpong(stream,
	3927	+ RKCIF_YUV_ADDR_STATE_INIT,
	3928	+ channel->id);
	3929	+ else if (mode == RKCIF_STREAM_MODE_TOISP \|\|
	3930	+ mode == RKCIF_STREAM_MODE_TOISP_RDBK)
	3931	+ rkcif_assign_new_buffer_pingpong_toisp(stream,
	3932	+ RKCIF_YUV_ADDR_STATE_INIT,
	3933	+ channel->id);
	3934	+ else if (mode == RKCIF_STREAM_MODE_ROCKIT)
	3935	+ rkcif_assign_new_buffer_pingpong_rockit(stream,
	3936	+ RKCIF_YUV_ADDR_STATE_INIT,
	3937	+ channel->id);
	3938	+
	3939	+ if (capture_info->mode == RKMODULE_MULTI_DEV_COMBINE_ONE &&
	3940	+ index == (capture_info->multi_dev.dev_num - 1) &&
	3941	+ priv && priv->mode.rdbk_mode != RKISP_VICAP_ONLINE)
	3942	+ rkcif_write_register(dev, get_reg_index_of_id_crop_start(channel->id),
	3943	+ channel->crop_st_y << 16 \|
	3944	+ (channel->crop_st_x + capture_info->multi_dev.pixel_offset));
	3945	+
	3946	+ rkcif_write_register(dev, get_reg_index_of_frm0_y_vlw(channel->id),
	3947	+ channel->virtual_width);
	3948	+
	3949	+ if (stream->lack_buf_cnt == 2)
	3950	+ stream->dma_en = 0;
	3951	+
	3952	+ if (stream->dma_en) {
	3953	+ if (mbus_type == V4L2_MBUS_CSI2_DPHY \|\|
	3954	+ mbus_type == V4L2_MBUS_CSI2_CPHY)
	3955	+ dma_en = CSI_DMA_ENABLE;
	3956	+ else
	3957	+ dma_en = LVDS_DMAEN_RV1106;
	3958	+ }
	3959	+ if (mbus_type == V4L2_MBUS_CSI2_DPHY \|\|
	3960	+ mbus_type == V4L2_MBUS_CSI2_CPHY) {
	3961	+
	3962	+ if (stream->cifdev->hdr.esp.mode == HDR_LINE_CNT \|\|
	3963	+ stream->cifdev->hdr.esp.mode == HDR_ID_CODE)
	3964	+ channel->vc = 0;
	3965	+
	3966	+ val = CSI_ENABLE_CAPTURE \| dma_en \|
	3967	+ channel->cmd_mode_en << 26 \| CSI_ENABLE_CROP_V1 \|
	3968	+ channel->vc << 8 \| channel->data_type << 10 \|
	3969	+ channel->csi_fmt_val;
	3970	+
	3971	+ val \|= stream->cif_fmt_in->csi_yuv_order;
	3972	+ val \|= rkcif_csi_get_output_type_mask(stream);
	3973	+ if (stream->cifdev->hdr.hdr_mode == NO_HDR \|\|
	3974	+ stream->cifdev->hdr.hdr_mode == HDR_COMPR)
	3975	+ val \|= CSI_NO_HDR;
	3976	+ else if (stream->cifdev->hdr.hdr_mode == HDR_X2)
	3977	+ val \|= CSI_HDR2;
	3978	+ else if (stream->cifdev->hdr.hdr_mode == HDR_X3)
	3979	+ val \|= CSI_HDR3;
	3980	+ if (stream->cifdev->hdr.esp.mode == HDR_NORMAL_VC)
	3981	+ val \|= CSI_HDR_MODE_VC;
	3982	+ else if (stream->cifdev->hdr.esp.mode == HDR_LINE_CNT)
	3983	+ val \|= CSI_HDR_MODE_LINE_CNT;
	3984	+ else if (stream->cifdev->hdr.esp.mode == HDR_ID_CODE)
	3985	+ val \|= CSI_HDR_MODE_LINE_INFO;
	3986	+ if (stream->cifdev->hdr.hdr_mode != NO_HDR &&
	3987	+ stream->cifdev->hdr.esp.mode == HDR_NORMAL_VC)
	3988	+ val \|= CSI_HDR_VC_MODE_PROTECT;
	3989	+ if (stream->is_high_align)
	3990	+ val \|= CSI_HIGH_ALIGN_RK3588;
	3991	+ else
	3992	+ val &= ~CSI_HIGH_ALIGN_RK3588;
	3993	+ rkcif_write_register(dev, get_reg_index_of_id_ctrl0(channel->id), val);
	3994	+ rkcif_write_register(dev, CIF_REG_MIPI_EFFECT_CODE_ID0, 0x02410251);
	3995	+ rkcif_write_register(dev, CIF_REG_MIPI_EFFECT_CODE_ID1, 0x02420252);
	3996	+ } else if (mbus_type == V4L2_MBUS_CCP2) {
	3997	+ rkcif_csi_set_lvds_sav_eav(stream, channel);
	3998	+ val = LVDS_ENABLE_CAPTURE_RV1106 \| LVDS_MODE_RV1106(channel->lvds_cfg.mode) \|
	3999	+ LVDS_MAIN_LANE_RV1106(0) \| LVDS_FID_RV1106(0) \|
	4000	+ LVDS_LANES_ENABLED_RV1106(dev->active_sensor->lanes) \|
	4001	+ (channel->csi_fmt_val << 18) \|
	4002	+ rkcif_lvds_get_output_type_mask(stream) \|
	4003	+ (stream->cif_fmt_in->csi_yuv_order << 9) \|
	4004	+ dma_en;
	4005	+ if (stream->cifdev->hdr.hdr_mode == HDR_X3)
	4006	+ val \|= BIT(12);
	4007	+ rkcif_write_register(dev, get_reg_index_of_lvds_id_ctrl0(channel->id), val);
	4008	+ }
	4009	+ if (dev->chip_id >= CHIP_RV1106_CIF)
	4010	+ rkcif_modify_frame_skip_config(stream);
	4011	+ if (capture_info->mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	4012	+ if (index == (capture_info->multi_dev.dev_num - 1))
	4013	+ stream->cifdev->id_use_cnt++;
	4014	+ } else {
	4015	+ stream->cifdev->id_use_cnt++;
	4016	+ }
	4017	+ if (!(capture_info->mode == RKMODULE_MULTI_DEV_COMBINE_ONE &&
	4018	+ index < capture_info->multi_dev.dev_num - 1)) {
	4019	+ if (mode == RKCIF_STREAM_MODE_CAPTURE)
	4020	+ rkcif_assign_new_buffer_pingpong(stream,
	4021	+ RKCIF_YUV_ADDR_STATE_INIT,
	4022	+ channel->id);
	4023	+ else if (mode == RKCIF_STREAM_MODE_TOISP \|\|
	4024	+ mode == RKCIF_STREAM_MODE_TOISP_RDBK)
	4025	+ rkcif_assign_new_buffer_pingpong_toisp(stream,
	4026	+ RKCIF_YUV_ADDR_STATE_INIT,
	4027	+ channel->id);
	4028	+ }
	4029	+ dev->intr_mask = rkcif_read_register(dev, CIF_REG_MIPI_LVDS_INTEN);
	4030	+ return 0;
	4031	+}
	4032	+
	4033	+static int rkcif_csi_stream_start(struct rkcif_stream *stream, unsigned int mode)
1973	4034	{
1974	4035	struct rkcif_device *dev = stream->cifdev;
1975	4036	struct rkcif_sensor_info *active_sensor = dev->active_sensor;
..	..	@@ -1977,23 +4038,22 @@
1977	4038	enum v4l2_mbus_type mbus_type = active_sensor->mbus.type;
1978	4039	struct csi_channel_info *channel;
1979	4040	u32 ret = 0;
	4041	+ int i;
1980	4042
1981		- if (stream->state != RKCIF_STATE_RESET_IN_STREAMING)
	4043	+ if (stream->state < RKCIF_STATE_STREAMING) {
1982	4044	stream->frame_idx = 0;
	4045	+ stream->buf_wake_up_cnt = 0;
	4046	+ stream->frame_phase = 0;
	4047	+ stream->lack_buf_cnt = 0;
	4048	+ stream->is_in_vblank = false;
	4049	+ stream->is_change_toisp = false;
	4050	+ }
1983	4051
1984		- if (mbus_type == V4L2_MBUS_CSI2) {
1985		- rkcif_csi_get_vc_num(dev, flags);
	4052	+ rkcif_csi_get_vc_num(dev, flags);
1986	4053
1987		- channel = &dev->channels[stream->id];
1988		- channel->id = stream->id;
1989		- rkcif_csi_channel_init(stream, channel);
1990		- rkcif_csi_channel_set(stream, channel, V4L2_MBUS_CSI2);
1991		- } else if (mbus_type == V4L2_MBUS_CCP2) {
1992		- rkcif_csi_get_vc_num(dev, flags);
1993		-
1994		- channel = &dev->channels[stream->id];
1995		- channel->id = stream->id;
1996		-
	4054	+ channel = &dev->channels[stream->id];
	4055	+ channel->id = stream->id;
	4056	+ if (mbus_type == V4L2_MBUS_CCP2) {
1997	4057	ret = v4l2_subdev_call(dev->terminal_sensor.sd, core,
1998	4058	ioctl, RKMODULE_GET_LVDS_CFG,
1999	4059	&channel->lvds_cfg);
..	..	@@ -2001,18 +4061,63 @@
2001	4061	v4l2_err(&dev->v4l2_dev, "Err: get lvds config failed!!\n");
2002	4062	return ret;
2003	4063	}
2004		-
2005		- rkcif_csi_channel_init(stream, channel);
2006		- rkcif_csi_channel_set(stream, channel, V4L2_MBUS_CCP2);
2007	4064	}
2008		-
2009		- stream->line_int_cnt = 0;
2010		- if (stream->is_line_wake_up)
2011		- stream->is_can_stop = false;
2012		- else
2013		- stream->is_can_stop = true;
2014		- stream->state = RKCIF_STATE_STREAMING;
2015		- dev->workmode = RKCIF_WORKMODE_PINGPONG;
	4065	+ rkcif_csi_channel_init(stream, channel);
	4066	+ if (stream->state != RKCIF_STATE_STREAMING) {
	4067	+ if (mode == RKCIF_STREAM_MODE_CAPTURE) {
	4068	+ stream->dma_en \|= RKCIF_DMAEN_BY_VICAP;
	4069	+ } else if (mode == RKCIF_STREAM_MODE_TOISP_RDBK) {
	4070	+ stream->dma_en \|= RKCIF_DMAEN_BY_ISP;
	4071	+ } else if (mode == RKCIF_STREAM_MODE_TOISP) {
	4072	+ if (dev->hdr.hdr_mode == HDR_X2 &&
	4073	+ stream->id == 0)
	4074	+ stream->dma_en \|= RKCIF_DMAEN_BY_ISP;
	4075	+ else if (dev->hdr.hdr_mode == HDR_X3 && (stream->id == 0 \|\| stream->id == 1))
	4076	+ stream->dma_en \|= RKCIF_DMAEN_BY_ISP;
	4077	+ } else if (mode == RKCIF_STREAM_MODE_ROCKIT) {
	4078	+ stream->dma_en \|= RKCIF_DMAEN_BY_ROCKIT;
	4079	+ }
	4080	+ if (stream->cifdev->chip_id < CHIP_RK3588_CIF) {
	4081	+ rkcif_csi_channel_set(stream, channel, mbus_type);
	4082	+ } else {
	4083	+ if (channel->capture_info.mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	4084	+ for (i = 0; i < channel->capture_info.multi_dev.dev_num; i++) {
	4085	+ dev->csi_host_idx = channel->capture_info.multi_dev.dev_idx[i];
	4086	+ rkcif_csi_channel_set_v1(stream, channel, mbus_type, mode, i);
	4087	+ }
	4088	+ } else {
	4089	+ rkcif_csi_channel_set_v1(stream, channel, mbus_type, mode, 0);
	4090	+ }
	4091	+ }
	4092	+ } else {
	4093	+ if (stream->cifdev->chip_id >= CHIP_RK3588_CIF) {
	4094	+ if (mode == RKCIF_STREAM_MODE_CAPTURE) {
	4095	+ stream->to_en_dma = RKCIF_DMAEN_BY_VICAP;
	4096	+ } else if (mode == RKCIF_STREAM_MODE_TOISP_RDBK) {
	4097	+ stream->to_en_dma = RKCIF_DMAEN_BY_ISP;
	4098	+ } else if (mode == RKCIF_STREAM_MODE_TOISP) {
	4099	+ if (dev->hdr.hdr_mode == HDR_X2 &&
	4100	+ stream->id == 0 &&
	4101	+ (!stream->dma_en))
	4102	+ stream->to_en_dma = RKCIF_DMAEN_BY_ISP;
	4103	+ else if (dev->hdr.hdr_mode == HDR_X3 &&
	4104	+ (stream->id == 0 \|\| stream->id == 1) &&
	4105	+ (!stream->dma_en))
	4106	+ stream->to_en_dma = RKCIF_DMAEN_BY_ISP;
	4107	+ } else if (mode == RKCIF_STREAM_MODE_ROCKIT) {
	4108	+ stream->to_en_dma = RKCIF_DMAEN_BY_ROCKIT;
	4109	+ }
	4110	+ }
	4111	+ }
	4112	+ if (stream->state != RKCIF_STATE_STREAMING) {
	4113	+ stream->line_int_cnt = 0;
	4114	+ if (stream->is_line_wake_up)
	4115	+ stream->is_can_stop = false;
	4116	+ else
	4117	+ stream->is_can_stop = true;
	4118	+ stream->state = RKCIF_STATE_STREAMING;
	4119	+ dev->workmode = RKCIF_WORKMODE_PINGPONG;
	4120	+ }
2016	4121
2017	4122	return 0;
2018	4123	}
..	..	@@ -2023,17 +4128,28 @@
2023	4128	struct v4l2_mbus_config *mbus_cfg = &cif_dev->active_sensor->mbus;
2024	4129	u32 val;
2025	4130	int id;
	4131	+ int i = 0;
2026	4132
2027		- if (mbus_cfg->type == V4L2_MBUS_CSI2 \|\|
	4133	+ stream->cifdev->id_use_cnt--;
	4134	+ if (mbus_cfg->type == V4L2_MBUS_CSI2_DPHY \|\|
	4135	+ mbus_cfg->type == V4L2_MBUS_CSI2_CPHY \|\|
2028	4136	mbus_cfg->type == V4L2_MBUS_CCP2) {
2029	4137	id = stream->id;
2030	4138	val = rkcif_read_register(cif_dev, get_reg_index_of_id_ctrl0(id));
2031		- if (mbus_cfg->type == V4L2_MBUS_CSI2)
2032		- val &= ~CSI_ENABLE_CAPTURE;
	4139	+ if (mbus_cfg->type == V4L2_MBUS_CSI2_DPHY \|\|
	4140	+ mbus_cfg->type == V4L2_MBUS_CSI2_CPHY)
	4141	+ val &= ~(CSI_ENABLE_CAPTURE \| CSI_DMA_ENABLE);
2033	4142	else
2034	4143	val &= ~LVDS_ENABLE_CAPTURE;
2035	4144
2036		- rkcif_write_register(cif_dev, get_reg_index_of_id_ctrl0(id), val);
	4145	+ if (cif_dev->channels[id].capture_info.mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	4146	+ for (i = 0; i < cif_dev->channels[id].capture_info.multi_dev.dev_num; i++) {
	4147	+ cif_dev->csi_host_idx = cif_dev->channels[id].capture_info.multi_dev.dev_idx[i];
	4148	+ rkcif_write_register(cif_dev, get_reg_index_of_id_ctrl0(id), val);
	4149	+ }
	4150	+ } else {
	4151	+ rkcif_write_register(cif_dev, get_reg_index_of_id_ctrl0(id), val);
	4152	+ }
2037	4153
2038	4154	rkcif_write_register_or(cif_dev, CIF_REG_MIPI_LVDS_INTSTAT,
2039	4155	CSI_START_INTSTAT(id) \|
..	..	@@ -2045,8 +4161,25 @@
2045	4161	CSI_DMA_END_INTEN(id) \|
2046	4162	CSI_LINE_INTEN(id)));
2047	4163
2048		- rkcif_write_register_and(cif_dev, CIF_REG_MIPI_LVDS_INTEN,
2049		- ~CSI_ALL_ERROR_INTEN);
	4164	+ if (stream->cifdev->chip_id < CHIP_RK3588_CIF) {
	4165	+ rkcif_write_register_and(cif_dev, CIF_REG_MIPI_LVDS_INTEN,
	4166	+ ~CSI_ALL_ERROR_INTEN);
	4167	+ } else {
	4168	+ if (stream->cifdev->id_use_cnt == 0) {
	4169	+ rkcif_write_register_and(cif_dev, CIF_REG_MIPI_LVDS_INTEN,
	4170	+ ~CSI_ALL_ERROR_INTEN_V1);
	4171	+ if (cif_dev->channels[id].capture_info.mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	4172	+ for (i = 0; i < cif_dev->channels[id].capture_info.multi_dev.dev_num; i++) {
	4173	+ cif_dev->csi_host_idx = cif_dev->channels[id].capture_info.multi_dev.dev_idx[i];
	4174	+ rkcif_write_register_and(cif_dev, CIF_REG_MIPI_LVDS_CTRL,
	4175	+ ~CSI_ENABLE_CAPTURE);
	4176	+ }
	4177	+ } else {
	4178	+ rkcif_write_register_and(cif_dev, CIF_REG_MIPI_LVDS_CTRL,
	4179	+ ~CSI_ENABLE_CAPTURE);
	4180	+ }
	4181	+ }
	4182	+ }
2050	4183
2051	4184	} else {
2052	4185	if (atomic_read(&cif_dev->pipe.stream_cnt) == 1) {
..	..	@@ -2056,16 +4189,17 @@
2056	4189	rkcif_write_register(cif_dev, CIF_REG_DVP_INTEN, 0x0);
2057	4190	rkcif_write_register(cif_dev, CIF_REG_DVP_INTSTAT, 0x3ff);
2058	4191	rkcif_write_register(cif_dev, CIF_REG_DVP_FRAME_STATUS, 0x0);
	4192	+ if (IS_ENABLED(CONFIG_CPU_RV1106))
	4193	+ rkcif_config_dvp_pin(cif_dev, false);
2059	4194	}
2060		- stream->is_dvp_yuv_addr_init = false;
2061	4195	}
2062		-
2063	4196	stream->state = RKCIF_STATE_READY;
	4197	+ stream->dma_en = 0;
2064	4198	}
2065	4199
2066	4200	static bool rkcif_is_extending_line_for_height(struct rkcif_device *dev,
2067		- struct rkcif_stream *stream,
2068		- const struct cif_input_fmt *fmt)
	4201	+ struct rkcif_stream *stream,
	4202	+ const struct cif_input_fmt *fmt)
2069	4203	{
2070	4204	bool is_extended = false;
2071	4205	struct rkmodule_hdr_cfg hdr_cfg;
..	..	@@ -2079,17 +4213,17 @@
2079	4213	RKMODULE_GET_HDR_CFG,
2080	4214	&hdr_cfg);
2081	4215	if (!ret)
2082		- dev->hdr.mode = hdr_cfg.hdr_mode;
	4216	+ dev->hdr = hdr_cfg;
2083	4217	else
2084		- dev->hdr.mode = NO_HDR;
	4218	+ dev->hdr.hdr_mode = NO_HDR;
2085	4219	}
2086	4220
2087	4221	if (fmt && fmt->fmt_type == CIF_FMT_TYPE_RAW) {
2088		- if ((dev->hdr.mode == HDR_X2 &&
	4222	+ if ((dev->hdr.hdr_mode == HDR_X2 &&
2089	4223	stream->id == RKCIF_STREAM_MIPI_ID1) \|\|
2090		- (dev->hdr.mode == HDR_X3 &&
	4224	+ (dev->hdr.hdr_mode == HDR_X3 &&
2091	4225	stream->id == RKCIF_STREAM_MIPI_ID2) \|\|
2092		- (dev->hdr.mode == NO_HDR)) {
	4226	+ (dev->hdr.hdr_mode == NO_HDR)) {
2093	4227	is_extended = true;
2094	4228	}
2095	4229	}
..	..	@@ -2137,7 +4271,7 @@
2137	4271	plane_fmt = &pixm->plane_fmt[i];
2138	4272	sizes[i] = plane_fmt->sizeimage / height * h;
2139	4273	}
2140		-
	4274	+ stream->total_buf_num = *num_buffers;
2141	4275	v4l2_dbg(1, rkcif_debug, &dev->v4l2_dev, "%s count %d, size %d, extended(%d, %d)\n",
2142	4276	v4l2_type_names[queue->type], *num_buffers, sizes[0],
2143	4277	is_extended, extend_line->is_extended);
..	..	@@ -2146,91 +4280,192 @@
2146	4280	}
2147	4281
2148	4282	static void rkcif_check_buffer_update_pingpong(struct rkcif_stream *stream,
2149		- int channel_id)
	4283	+ int channel_id)
2150	4284	{
2151	4285	struct rkcif_device *dev = stream->cifdev;
2152	4286	struct v4l2_mbus_config *mbus_cfg = &dev->active_sensor->mbus;
2153	4287	struct rkcif_buffer *buffer = NULL;
2154		- struct rkcif_dummy_buffer *dummy_buf = &dev->dummy_buf;
2155		- u32 frm_addr_y, frm_addr_uv;
	4288	+ struct rkcif_dummy_buffer *dummy_buf = &dev->hw_dev->dummy_buf;
	4289	+ u32 frm_addr_y = 0, frm_addr_uv = 0;
	4290	+ u32 frm0_addr_y = 0, frm0_addr_uv = 0;
	4291	+ u32 frm1_addr_y = 0, frm1_addr_uv = 0;
	4292	+ u32 buff_addr_y = 0, buff_addr_cbcr = 0;
	4293	+ struct rkmodule_capture_info *capture_info = &dev->channels[channel_id].capture_info;
2156	4294	unsigned long flags;
	4295	+ int frame_phase = 0;
	4296	+ bool is_dual_update_buf = false;
2157	4297
	4298	+ spin_lock_irqsave(&stream->vbq_lock, flags);
2158	4299	if (stream->state == RKCIF_STATE_STREAMING &&
2159		- stream->curr_buf == stream->next_buf &&
2160		- stream->cif_fmt_in->field != V4L2_FIELD_INTERLACED &&
2161		- (!dummy_buf->vaddr)) {
2162		- if (!stream->is_line_wake_up) {
2163		- if (mbus_cfg->type == V4L2_MBUS_CSI2 \|\|
	4300	+ ((stream->curr_buf == stream->next_buf &&
	4301	+ stream->cif_fmt_in->field != V4L2_FIELD_INTERLACED &&
	4302	+ (!dummy_buf->vaddr)) \|\|
	4303	+ stream->curr_buf == NULL \|\|
	4304	+ stream->next_buf == NULL)) {
	4305	+ frame_phase = stream->frame_phase_cache;
	4306	+ if (!stream->is_line_wake_up \|\|
	4307	+ (stream->is_line_wake_up && stream->frame_idx < 2)) {
	4308	+ if (mbus_cfg->type == V4L2_MBUS_CSI2_DPHY \|\|
	4309	+ mbus_cfg->type == V4L2_MBUS_CSI2_CPHY \|\|
2164	4310	mbus_cfg->type == V4L2_MBUS_CCP2) {
2165		- frm_addr_y = stream->frame_phase_cache & CIF_CSI_FRAME0_READY ?
2166		- get_reg_index_of_frm0_y_addr(channel_id) :
2167		- get_reg_index_of_frm1_y_addr(channel_id);
2168		- frm_addr_uv = stream->frame_phase_cache & CIF_CSI_FRAME0_READY ?
2169		- get_reg_index_of_frm0_uv_addr(channel_id) :
2170		- get_reg_index_of_frm1_uv_addr(channel_id);
	4311	+ frm0_addr_y = get_reg_index_of_frm0_y_addr(channel_id);
	4312	+ frm1_addr_y = get_reg_index_of_frm1_y_addr(channel_id);
	4313	+ frm0_addr_uv = get_reg_index_of_frm0_uv_addr(channel_id);
	4314	+ frm1_addr_uv = get_reg_index_of_frm1_uv_addr(channel_id);
2171	4315	} else {
2172		- frm_addr_y = stream->frame_phase_cache & CIF_CSI_FRAME0_READY ?
2173		- get_dvp_reg_index_of_frm0_y_addr(channel_id) :
2174		- get_dvp_reg_index_of_frm1_y_addr(channel_id);
2175		- frm_addr_uv = stream->frame_phase_cache & CIF_CSI_FRAME0_READY ?
2176		- get_dvp_reg_index_of_frm0_uv_addr(channel_id) :
2177		- get_dvp_reg_index_of_frm1_uv_addr(channel_id);
	4316	+ frm0_addr_y = get_dvp_reg_index_of_frm0_y_addr(channel_id);
	4317	+ frm1_addr_y = get_dvp_reg_index_of_frm1_y_addr(channel_id);
	4318	+ frm0_addr_uv = get_dvp_reg_index_of_frm0_uv_addr(channel_id);
	4319	+ frm1_addr_uv = get_dvp_reg_index_of_frm1_uv_addr(channel_id);
2178	4320	}
2179		- v4l2_dbg(1, rkcif_debug, &stream->cifdev->v4l2_dev,
2180		- "stream[%d] update buf in %s, stream->frame_phase_cache %d\n",
2181		- stream->id, __func__, stream->frame_phase_cache);
2182		- spin_lock_irqsave(&stream->vbq_lock, flags);
	4321	+ if (frame_phase & CIF_CSI_FRAME0_READY) {
	4322	+ frm_addr_y = frm0_addr_y;
	4323	+ frm_addr_uv = frm0_addr_uv;
	4324	+ } else {
	4325	+ frm_addr_y = frm1_addr_y;
	4326	+ frm_addr_uv = frm1_addr_uv;
	4327	+ }
	4328	+ if (!stream->dma_en && stream->curr_buf == NULL && stream->next_buf == NULL)
	4329	+ is_dual_update_buf = true;
2183	4330	if (!list_empty(&stream->buf_head)) {
2184		- if (stream->frame_phase_cache == CIF_CSI_FRAME0_READY) {
	4331	+ if (frame_phase == CIF_CSI_FRAME0_READY) {
2185	4332	stream->curr_buf = list_first_entry(&stream->buf_head,
2186	4333	struct rkcif_buffer, queue);
2187	4334	if (stream->curr_buf) {
2188	4335	list_del(&stream->curr_buf->queue);
2189	4336	buffer = stream->curr_buf;
2190	4337	}
2191		- } else if (stream->frame_phase_cache == CIF_CSI_FRAME1_READY) {
2192		- stream->next_buf = list_first_entry(&stream->buf_head,
	4338	+ if (buffer && is_dual_update_buf)
	4339	+ stream->next_buf = buffer;
	4340	+ } else if (frame_phase == CIF_CSI_FRAME1_READY) {
	4341	+ if (stream->next_buf == NULL &&
	4342	+ stream->cif_fmt_in->field == V4L2_FIELD_INTERLACED) {
	4343	+ stream->next_buf = stream->curr_buf;
	4344	+ } else {
	4345	+ stream->next_buf = list_first_entry(&stream->buf_head,
2193	4346	struct rkcif_buffer, queue);
2194		- if (stream->next_buf) {
2195		- list_del(&stream->next_buf->queue);
2196		- buffer = stream->next_buf;
	4347	+ if (stream->next_buf) {
	4348	+ list_del(&stream->next_buf->queue);
	4349	+ buffer = stream->next_buf;
	4350	+ }
	4351	+ if (buffer && is_dual_update_buf)
	4352	+ stream->curr_buf = buffer;
	4353	+ }
	4354	+ }
	4355	+ } else {
	4356	+ v4l2_info(&dev->v4l2_dev, "%s %d\n", __func__, __LINE__);
	4357	+ }
	4358	+ if (buffer) {
	4359	+ if (is_dual_update_buf) {
	4360	+ buff_addr_y = buffer->buff_addr[RKCIF_PLANE_Y];
	4361	+ buff_addr_cbcr = buffer->buff_addr[RKCIF_PLANE_CBCR];
	4362	+ if (capture_info->mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	4363	+ rkcif_write_buff_addr_multi_dev_combine(stream,
	4364	+ frm0_addr_y,
	4365	+ frm0_addr_uv,
	4366	+ buff_addr_y,
	4367	+ buff_addr_cbcr,
	4368	+ false);
	4369	+ rkcif_write_buff_addr_multi_dev_combine(stream,
	4370	+ frm1_addr_y,
	4371	+ frm1_addr_uv,
	4372	+ buff_addr_y,
	4373	+ buff_addr_cbcr,
	4374	+ false);
	4375	+ } else {
	4376	+ rkcif_write_register(dev, frm0_addr_y, buff_addr_y);
	4377	+ if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
	4378	+ rkcif_write_register(dev,
	4379	+ frm0_addr_uv,
	4380	+ buff_addr_cbcr);
	4381	+ rkcif_write_register(dev, frm1_addr_y, buff_addr_y);
	4382	+ if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
	4383	+ rkcif_write_register(dev,
	4384	+ frm1_addr_uv,
	4385	+ buff_addr_cbcr);
	4386	+ }
	4387	+ } else {
	4388	+
	4389	+ buff_addr_y = buffer->buff_addr[RKCIF_PLANE_Y];
	4390	+ buff_addr_cbcr = buffer->buff_addr[RKCIF_PLANE_CBCR];
	4391	+ if (capture_info->mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	4392	+ rkcif_write_buff_addr_multi_dev_combine(stream,
	4393	+ frm_addr_y,
	4394	+ frm_addr_uv,
	4395	+ buff_addr_y,
	4396	+ buff_addr_cbcr,
	4397	+ false);
	4398	+ } else {
	4399	+ rkcif_write_register(dev, frm_addr_y, buff_addr_y);
	4400	+ if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
	4401	+ rkcif_write_register(dev,
	4402	+ frm_addr_uv,
	4403	+ buff_addr_cbcr);
2197	4404	}
2198	4405	}
2199	4406	}
2200		- spin_unlock_irqrestore(&stream->vbq_lock, flags);
2201		- if (buffer) {
2202		- rkcif_write_register(dev, frm_addr_y,
2203		- buffer->buff_addr[RKCIF_PLANE_Y]);
2204		- if (stream->cif_fmt_out->fmt_type != CIF_FMT_TYPE_RAW)
2205		- rkcif_write_register(dev, frm_addr_uv,
2206		- buffer->buff_addr[RKCIF_PLANE_CBCR]);
2207		- }
2208	4407	} else {
2209		- spin_lock_irqsave(&stream->vbq_lock, flags);
	4408	+ v4l2_dbg(3, rkcif_debug, &stream->cifdev->v4l2_dev,
	4409	+ "%s %d, is_wake_up %d, frame_idx %d\n",
	4410	+ __func__, __LINE__, stream->is_line_wake_up, stream->frame_idx);
2210	4411	if (stream->curr_buf == stream->next_buf) {
2211	4412	if (stream->frame_phase_cache == CIF_CSI_FRAME0_READY) {
2212	4413	stream->curr_buf = list_first_entry(&stream->buf_head,
2213	4414	struct rkcif_buffer, queue);
	4415	+ v4l2_dbg(3, rkcif_debug, &dev->v4l2_dev,
	4416	+ "%s %d, stream[%d] buf idx %d\n",
	4417	+ __func__, __LINE__, stream->id, stream->curr_buf->vb.vb2_buf.index);
2214	4418	if (stream->curr_buf)
2215	4419	list_del(&stream->curr_buf->queue);
2216	4420	} else if (stream->frame_phase_cache == CIF_CSI_FRAME1_READY) {
2217	4421	stream->next_buf = list_first_entry(&stream->buf_head,
2218	4422	struct rkcif_buffer, queue);
	4423	+ v4l2_dbg(4, rkcif_debug, &dev->v4l2_dev,
	4424	+ "%s %d, stream[%d] buf idx %d\n",
	4425	+ __func__, __LINE__, stream->id, stream->next_buf->vb.vb2_buf.index);
2219	4426	if (stream->next_buf)
2220	4427	list_del(&stream->next_buf->queue);
2221	4428	}
2222	4429	stream->is_buf_active = true;
2223	4430	}
2224		- spin_unlock_irqrestore(&stream->vbq_lock, flags);
2225	4431	}
	4432	+ v4l2_dbg(3, rkcif_debug, &stream->cifdev->v4l2_dev,
	4433	+ "%s, stream[%d] update buffer, frame_phase %d, is_stop %s, lack_buf_cnt %d\n",
	4434	+ __func__, stream->id, frame_phase,
	4435	+ (stream->dma_en ? "false" : "true"),
	4436	+ stream->lack_buf_cnt);
	4437	+ if (!stream->dma_en) {
	4438	+ if (stream->to_stop_dma) {
	4439	+ stream->to_stop_dma = 0;
	4440	+ wake_up(&stream->wq_stopped);
	4441	+ } else {
	4442	+ stream->to_en_dma = RKCIF_DMAEN_BY_VICAP;
	4443	+ v4l2_dbg(3, rkcif_debug, &stream->cifdev->v4l2_dev,
	4444	+ "%s stream[%d] start dma capture, frame cnt %d\n",
	4445	+ __func__, stream->id, stream->frame_idx);
	4446	+ }
	4447	+ } else {
	4448	+ v4l2_dbg(3, rkcif_debug, &stream->cifdev->v4l2_dev,
	4449	+ "%s %d, dma_en 0x%x, frame cnt %d\n",
	4450	+ __func__, __LINE__, stream->dma_en, stream->frame_idx);
	4451	+ }
	4452	+ if (stream->lack_buf_cnt)
	4453	+ stream->lack_buf_cnt--;
	4454	+
	4455	+ } else {
	4456	+ v4l2_info(&dev->v4l2_dev, "%s %d, state %d, curr_buf %p, next_buf %p\n",
	4457	+ __func__, __LINE__, stream->state, stream->curr_buf, stream->next_buf);
2226	4458	}
	4459	+ spin_unlock_irqrestore(&stream->vbq_lock, flags);
	4460	+ if (stream->to_en_dma)
	4461	+ rkcif_enable_dma_capture(stream, true);
2227	4462	}
2228	4463
2229	4464	/*
2230	4465	* The vb2_buffer are stored in rkcif_buffer, in order to unify
2231	4466	* mplane buffer and none-mplane buffer.
2232	4467	*/
2233		-static void rkcif_buf_queue(struct vb2_buffer *vb)
	4468	+void rkcif_buf_queue(struct vb2_buffer *vb)
2234	4469	{
2235	4470	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
2236	4471	struct rkcif_buffer *cifbuf = to_rkcif_buffer(vbuf);
..	..	@@ -2239,8 +4474,32 @@
2239	4474	struct v4l2_pix_format_mplane *pixm = &stream->pixm;
2240	4475	const struct cif_output_fmt *fmt = stream->cif_fmt_out;
2241	4476	struct rkcif_hw *hw_dev = stream->cifdev->hw_dev;
	4477	+ struct rkcif_tools_buffer *tools_buf;
	4478	+ struct rkcif_tools_vdev *tools_vdev = stream->tools_vdev;
2242	4479	unsigned long flags;
2243	4480	int i;
	4481	+ bool is_find_tools_buf = false;
	4482	+
	4483	+ if (tools_vdev) {
	4484	+ spin_lock_irqsave(&stream->tools_vdev->vbq_lock, flags);
	4485	+ if (!list_empty(&tools_vdev->src_buf_head)) {
	4486	+ list_for_each_entry(tools_buf, &tools_vdev->src_buf_head, list) {
	4487	+ if (tools_buf->vb == vbuf) {
	4488	+ is_find_tools_buf = true;
	4489	+ break;
	4490	+ }
	4491	+ }
	4492	+ if (is_find_tools_buf) {
	4493	+ if (tools_buf->use_cnt)
	4494	+ tools_buf->use_cnt--;
	4495	+ if (tools_buf->use_cnt) {
	4496	+ spin_unlock_irqrestore(&stream->tools_vdev->vbq_lock, flags);
	4497	+ return;
	4498	+ }
	4499	+ }
	4500	+ }
	4501	+ spin_unlock_irqrestore(&stream->tools_vdev->vbq_lock, flags);
	4502	+ }
2244	4503
2245	4504	memset(cifbuf->buff_addr, 0, sizeof(cifbuf->buff_addr));
2246	4505	/* If mplanes > 1, every c-plane has its own m-plane,
..	..	@@ -2249,7 +4508,7 @@
2249	4508	for (i = 0; i < fmt->mplanes; i++) {
2250	4509	void *addr = vb2_plane_vaddr(vb, i);
2251	4510
2252		- if (hw_dev->iommu_en) {
	4511	+ if (hw_dev->is_dma_sg_ops) {
2253	4512	struct sg_table *sgt = vb2_dma_sg_plane_desc(vb, i);
2254	4513
2255	4514	cifbuf->buff_addr[i] = sg_dma_address(sgt->sgl);
..	..	@@ -2258,7 +4517,7 @@
2258	4517	}
2259	4518	if (rkcif_debug && addr && !hw_dev->iommu_en) {
2260	4519	memset(addr, 0, pixm->plane_fmt[i].sizeimage);
2261		- v4l2_dbg(1, rkcif_debug, &stream->cifdev->v4l2_dev,
	4520	+ v4l2_dbg(3, rkcif_debug, &stream->cifdev->v4l2_dev,
2262	4521	"Clear buffer, size: 0x%08x\n",
2263	4522	pixm->plane_fmt[i].sizeimage);
2264	4523	}
..	..	@@ -2272,40 +4531,297 @@
2272	4531	spin_lock_irqsave(&stream->vbq_lock, flags);
2273	4532	list_add_tail(&cifbuf->queue, &stream->buf_head);
2274	4533	spin_unlock_irqrestore(&stream->vbq_lock, flags);
2275		- if (stream->cifdev->workmode == RKCIF_WORKMODE_PINGPONG)
	4534	+ if (stream->dma_en & RKCIF_DMAEN_BY_ISP && (!cifbuf->dbuf)) {
	4535	+ struct rkisp_rx_buf *dbufs = NULL;
	4536	+
	4537	+ dbufs = kzalloc(sizeof(struct rkisp_rx_buf), GFP_KERNEL);
	4538	+
	4539	+ memset(dbufs, 0, sizeof(struct rkisp_rx_buf));
	4540	+ if (stream->cifdev->hdr.hdr_mode == HDR_X2 && stream->id == 0)
	4541	+ dbufs->type = BUF_MIDDLE;
	4542	+ else if (stream->cifdev->hdr.hdr_mode == HDR_X3 && stream->id == 0)
	4543	+ dbufs->type = BUF_LONG;
	4544	+ else if (stream->cifdev->hdr.hdr_mode == HDR_X3 && stream->id == 1)
	4545	+ dbufs->type = BUF_MIDDLE;
	4546	+ cifbuf->dbuf = hw_dev->mem_ops->get_dmabuf(vb->planes[0].mem_priv, O_RDWR);
	4547	+ if (cifbuf->dbuf)
	4548	+ dbufs->dbuf = cifbuf->dbuf;
	4549	+ list_add_tail(&dbufs->list, &stream->rx_buf_head_vicap);
	4550	+ }
	4551	+ if (stream->cifdev->workmode == RKCIF_WORKMODE_PINGPONG &&
	4552	+ stream->lack_buf_cnt &&
	4553	+ stream->cur_stream_mode & RKCIF_STREAM_MODE_CAPTURE)
2276	4554	rkcif_check_buffer_update_pingpong(stream, stream->id);
	4555	+ v4l2_dbg(3, rkcif_debug, &stream->cifdev->v4l2_dev,
	4556	+ "stream[%d] buf queue, index: %d, dma_addr 0x%x\n",
	4557	+ stream->id, vb->index, cifbuf->buff_addr[0]);
	4558	+ atomic_inc(&stream->buf_cnt);
	4559	+}
	4560	+
	4561	+void rkcif_free_rx_buf(struct rkcif_stream *stream, int buf_num)
	4562	+{
	4563	+ struct rkcif_rx_buffer *buf;
	4564	+ struct rkcif_device *dev = stream->cifdev;
	4565	+ struct sditf_priv *priv = dev->sditf[0];
	4566	+ struct v4l2_subdev *sd;
	4567	+ int i = 0;
	4568	+ unsigned long flags;
	4569	+ phys_addr_t resmem_free_start;
	4570	+ phys_addr_t resmem_free_end;
	4571	+ u32 share_head_size = 0;
	4572	+
	4573	+ if (!priv)
	4574	+ return;
	4575	+
	4576	+ sd = get_rkisp_sd(dev->sditf[0]);
	4577	+ if (!sd)
	4578	+ return;
	4579	+
	4580	+ if (dev->is_rtt_suspend && dev->is_thunderboot) {
	4581	+ stream->curr_buf_toisp = NULL;
	4582	+ stream->next_buf_toisp = NULL;
	4583	+ INIT_LIST_HEAD(&stream->rx_buf_head);
	4584	+
	4585	+ for (i = 0; i < buf_num; i++) {
	4586	+ buf = &stream->rx_buf[i];
	4587	+ if (buf->dbufs.is_init)
	4588	+ v4l2_subdev_call(sd, core, ioctl,
	4589	+ RKISP_VICAP_CMD_RX_BUFFER_FREE, &buf->dbufs);
	4590	+ buf->dummy.is_free = true;
	4591	+ }
	4592	+
	4593	+ if (IS_ENABLED(CONFIG_VIDEO_ROCKCHIP_THUNDER_BOOT_ISP)) {
	4594	+ share_head_size = dev->thunderboot_sensor_num * sizeof(struct rkisp32_thunderboot_resmem_head);
	4595	+ if (share_head_size != dev->share_mem_size)
	4596	+ v4l2_info(&stream->cifdev->v4l2_dev,
	4597	+ "share mem head error, rtt head size %d, arm head size %d\n",
	4598	+ dev->share_mem_size, share_head_size);
	4599	+ resmem_free_start = dev->resmem_pa + share_head_size + dev->nr_buf_size;
	4600	+ resmem_free_end = dev->resmem_pa + dev->resmem_size;
	4601	+ v4l2_info(&stream->cifdev->v4l2_dev,
	4602	+ "free reserved mem start 0x%x, end 0x%x, share_head_size 0x%x, nr_buf_size 0x%x\n",
	4603	+ (u32)resmem_free_start, (u32)resmem_free_end, share_head_size, dev->nr_buf_size);
	4604	+ free_reserved_area(phys_to_virt(resmem_free_start),
	4605	+ phys_to_virt(resmem_free_end),
	4606	+ -1, "rkisp_thunderboot");
	4607	+ }
	4608	+ atomic_set(&stream->buf_cnt, 0);
	4609	+ stream->total_buf_num = 0;
	4610	+ stream->rx_buf_num = 0;
	4611	+
	4612	+ return;
	4613	+ }
	4614	+
	4615	+ spin_lock_irqsave(&stream->vbq_lock, flags);
	4616	+ stream->curr_buf_toisp = NULL;
	4617	+ stream->next_buf_toisp = NULL;
	4618	+ INIT_LIST_HEAD(&stream->rx_buf_head);
	4619	+ spin_unlock_irqrestore(&stream->vbq_lock, flags);
	4620	+
	4621	+ if (dev->is_thunderboot)
	4622	+ spin_lock_irqsave(&dev->buffree_lock, flags);
	4623	+ for (i = 0; i < buf_num; i++) {
	4624	+ buf = &stream->rx_buf[i];
	4625	+ if (buf->dummy.is_free)
	4626	+ continue;
	4627	+ if (buf->dbufs.is_init)
	4628	+ v4l2_subdev_call(sd, core, ioctl,
	4629	+ RKISP_VICAP_CMD_RX_BUFFER_FREE, &buf->dbufs);
	4630	+ if (!dev->is_thunderboot)
	4631	+ rkcif_free_buffer(dev, &buf->dummy);
	4632	+ else
	4633	+ list_add_tail(&buf->list_free, &priv->buf_free_list);
	4634	+ atomic_dec(&stream->buf_cnt);
	4635	+ stream->total_buf_num--;
	4636	+ }
	4637	+ stream->rx_buf_num = 0;
	4638	+
	4639	+ if (dev->is_thunderboot) {
	4640	+ spin_unlock_irqrestore(&dev->buffree_lock, flags);
	4641	+ schedule_work(&priv->buffree_work.work);
	4642	+ }
	4643	+ stream->dma_en &= ~RKCIF_DMAEN_BY_ISP;
2277	4644	v4l2_dbg(1, rkcif_debug, &stream->cifdev->v4l2_dev,
2278		- "stream[%d] buf queue, index: %d\n",
2279		- stream->id, vb->index);
	4645	+ "free rx_buf, buf_num %d\n", buf_num);
	4646	+}
	4647	+
	4648	+static void rkcif_get_resmem_head(struct rkcif_device *cif_dev);
	4649	+int rkcif_init_rx_buf(struct rkcif_stream *stream, int buf_num)
	4650	+{
	4651	+ struct rkcif_device *dev = stream->cifdev;
	4652	+ struct v4l2_pix_format_mplane *pixm = &stream->pixm;
	4653	+ struct rkcif_dummy_buffer *dummy;
	4654	+ struct rkcif_rx_buffer *buf;
	4655	+ struct sditf_priv *priv = dev->sditf[0];
	4656	+ int frm_type = 0;
	4657	+ int i = 0;
	4658	+ int ret = 0;
	4659	+
	4660	+ if (!priv)
	4661	+ return -EINVAL;
	4662	+
	4663	+ if (buf_num > RKISP_VICAP_BUF_CNT_MAX)
	4664	+ return -EINVAL;
	4665	+
	4666	+ if (dev->hdr.hdr_mode == NO_HDR) {
	4667	+ if (stream->id == 0)
	4668	+ frm_type = BUF_SHORT;
	4669	+ else
	4670	+ return -EINVAL;
	4671	+ } else if (dev->hdr.hdr_mode == HDR_X2) {
	4672	+ if (stream->id == 0)
	4673	+ frm_type = BUF_MIDDLE;
	4674	+ else if (stream->id == 1)
	4675	+ frm_type = BUF_SHORT;
	4676	+ else
	4677	+ return -EINVAL;
	4678	+ } else if (dev->hdr.hdr_mode == HDR_X3) {
	4679	+ if (stream->id == 0)
	4680	+ frm_type = BUF_LONG;
	4681	+ else if (stream->id == 1)
	4682	+ frm_type = BUF_MIDDLE;
	4683	+ else if (stream->id == 2)
	4684	+ frm_type = BUF_SHORT;
	4685	+ else
	4686	+ return -EINVAL;
	4687	+ }
	4688	+ while (true) {
	4689	+ buf = &stream->rx_buf[i];
	4690	+ memset(buf, 0, sizeof(*buf));
	4691	+ dummy = &buf->dummy;
	4692	+ dummy->size = pixm->plane_fmt[0].sizeimage;
	4693	+ dummy->is_need_vaddr = true;
	4694	+ dummy->is_need_dbuf = true;
	4695	+ if (dev->is_thunderboot) {
	4696	+ if (i == 0)
	4697	+ rkcif_get_resmem_head(dev);
	4698	+ buf->buf_idx = i;
	4699	+ ret = rkcif_alloc_reserved_mem_buf(dev, buf);
	4700	+ if (ret) {
	4701	+ stream->rx_buf_num = i;
	4702	+ v4l2_info(&dev->v4l2_dev,
	4703	+ "reserved mem support alloc buf num %d, require buf num %d\n",
	4704	+ i, buf_num);
	4705	+ break;
	4706	+ }
	4707	+ if (dev->rdbk_debug)
	4708	+ v4l2_info(&dev->v4l2_dev,
	4709	+ "stream[%d] buf addr 0x%llx\n",
	4710	+ stream->id, (u64)dummy->dma_addr);
	4711	+ } else {
	4712	+ ret = rkcif_alloc_buffer(dev, dummy);
	4713	+ if (ret) {
	4714	+ stream->rx_buf_num = i;
	4715	+ v4l2_info(&dev->v4l2_dev,
	4716	+ "alloc buf num %d, require buf num %d\n",
	4717	+ i, buf_num);
	4718	+ break;
	4719	+ }
	4720	+ buf->dbufs.dbuf = dummy->dbuf;
	4721	+ }
	4722	+ buf->dbufs.is_init = false;
	4723	+ buf->dbufs.type = frm_type;
	4724	+ list_add_tail(&buf->list, &stream->rx_buf_head);
	4725	+ dummy->is_free = false;
	4726	+ if (stream->is_compact)
	4727	+ buf->dbufs.is_uncompact = false;
	4728	+ else
	4729	+ buf->dbufs.is_uncompact = true;
	4730	+ if (priv && i == 0) {
	4731	+ buf->dbufs.is_first = true;
	4732	+ if (priv->mode.rdbk_mode == RKISP_VICAP_ONLINE)
	4733	+ rkcif_s_rx_buffer(stream, &buf->dbufs);
	4734	+ }
	4735	+ i++;
	4736	+ if (!dev->is_thunderboot && i >= buf_num) {
	4737	+ stream->rx_buf_num = buf_num;
	4738	+ break;
	4739	+ } else if (i >= RKISP_VICAP_BUF_CNT_MAX) {
	4740	+ stream->rx_buf_num = i;
	4741	+ v4l2_info(&dev->v4l2_dev,
	4742	+ "reserved mem alloc buf num %d\n", i);
	4743	+ break;
	4744	+ }
	4745	+ v4l2_dbg(1, rkcif_debug, &dev->v4l2_dev,
	4746	+ "init rx_buf,dma_addr 0x%llx size: 0x%x\n",
	4747	+ (u64)dummy->dma_addr, pixm->plane_fmt[0].sizeimage);
	4748	+ }
	4749	+ if (stream->rx_buf_num) {
	4750	+ stream->total_buf_num = stream->rx_buf_num;
	4751	+ atomic_set(&stream->buf_cnt, stream->rx_buf_num);
	4752	+ return 0;
	4753	+ } else {
	4754	+ return -EINVAL;
	4755	+ }
2280	4756	}
2281	4757
2282	4758	static int rkcif_create_dummy_buf(struct rkcif_stream *stream)
2283	4759	{
2284		- u32 fourcc;
2285	4760	struct rkcif_device *dev = stream->cifdev;
2286		- struct rkcif_dummy_buffer *dummy_buf = &dev->dummy_buf;
2287		- struct rkcif_hw *hw_dev = dev->hw_dev;
	4761	+ struct rkcif_hw *hw = dev->hw_dev;
	4762	+ struct rkcif_dummy_buffer *dummy_buf = &hw->dummy_buf;
	4763	+ struct rkcif_device *tmp_dev = NULL;
	4764	+ struct v4l2_subdev_frame_interval_enum fie;
	4765	+ struct v4l2_subdev_format fmt;
	4766	+ u32 max_size = 0;
	4767	+ u32 size = 0;
	4768	+ int ret = 0;
	4769	+ int i, j;
2288	4770
2289		- /* get a maximum plane size */
2290		- dummy_buf->size = max3(stream->pixm.plane_fmt[0].bytesperline *
2291		- stream->pixm.height,
2292		- stream->pixm.plane_fmt[1].sizeimage,
2293		- stream->pixm.plane_fmt[2].sizeimage);
2294		- /*
2295		- * rk cif don't support output yuyv fmt data
2296		- * if user request yuyv fmt, the input mode must be RAW8
2297		- * and the width is double Because the real input fmt is
2298		- * yuyv
2299		- */
2300		- fourcc = stream->cif_fmt_out->fourcc;
2301		- if (fourcc == V4L2_PIX_FMT_YUYV \|\| fourcc == V4L2_PIX_FMT_YVYU \|\|
2302		- fourcc == V4L2_PIX_FMT_UYVY \|\| fourcc == V4L2_PIX_FMT_VYUY)
2303		- dummy_buf->size *= 2;
	4771	+ for (i = 0; i < hw->dev_num; i++) {
	4772	+ tmp_dev = hw->cif_dev[i];
	4773	+ if (tmp_dev->terminal_sensor.sd) {
	4774	+ for (j = 0; j < 32; j++) {
	4775	+ memset(&fie, 0, sizeof(fie));
	4776	+ fie.index = j;
	4777	+ fie.pad = 0;
	4778	+ fie.which = V4L2_SUBDEV_FORMAT_ACTIVE;
	4779	+ ret = v4l2_subdev_call(tmp_dev->terminal_sensor.sd,
	4780	+ pad, enum_frame_interval,
	4781	+ NULL, &fie);
	4782	+ if (!ret) {
	4783	+ if (fie.code == MEDIA_BUS_FMT_RGB888_1X24 \|\|
	4784	+ fie.code == MEDIA_BUS_FMT_BGR888_1X24 \|\|
	4785	+ fie.code == MEDIA_BUS_FMT_GBR888_1X24)
	4786	+ size = fie.width * fie.height * 3;
	4787	+ else
	4788	+ size = fie.width * fie.height * 2;
	4789	+ v4l2_dbg(1, rkcif_debug, &dev->v4l2_dev,
	4790	+ "%s enum fmt, width %d, height %d\n",
	4791	+ __func__, fie.width, fie.height);
	4792	+ } else {
	4793	+ break;
	4794	+ }
	4795	+ if (size > max_size)
	4796	+ max_size = size;
	4797	+ }
	4798	+ } else {
	4799	+ continue;
	4800	+ }
	4801	+ }
2304	4802
2305		- dummy_buf->vaddr = dma_alloc_coherent(hw_dev->dev, dummy_buf->size,
2306		- &dummy_buf->dma_addr,
2307		- GFP_KERNEL);
2308		- if (!dummy_buf->vaddr) {
	4803	+ if (max_size == 0 && dev->terminal_sensor.sd) {
	4804	+ fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
	4805	+ ret = v4l2_subdev_call(dev->terminal_sensor.sd,
	4806	+ pad, get_fmt, NULL, &fmt);
	4807	+ if (!ret) {
	4808	+ if (fmt.format.code == MEDIA_BUS_FMT_RGB888_1X24 \|\|
	4809	+ fmt.format.code == MEDIA_BUS_FMT_BGR888_1X24 \|\|
	4810	+ fmt.format.code == MEDIA_BUS_FMT_GBR888_1X24)
	4811	+ size = fmt.format.width * fmt.format.height * 3;
	4812	+ else
	4813	+ size = fmt.format.width * fmt.format.height * 2;
	4814	+ if (size > max_size)
	4815	+ max_size = size;
	4816	+ }
	4817	+ }
	4818	+
	4819	+ dummy_buf->size = max_size;
	4820	+
	4821	+ dummy_buf->is_need_vaddr = true;
	4822	+ dummy_buf->is_need_dbuf = true;
	4823	+ ret = rkcif_alloc_buffer(dev, dummy_buf);
	4824	+ if (ret) {
2309	4825	v4l2_err(&dev->v4l2_dev,
2310	4826	"Failed to allocate the memory for dummy buffer\n");
2311	4827	return -ENOMEM;
..	..	@@ -2314,18 +4830,16 @@
2314	4830	v4l2_info(&dev->v4l2_dev, "Allocate dummy buffer, size: 0x%08x\n",
2315	4831	dummy_buf->size);
2316	4832
2317		- return 0;
	4833	+ return ret;
2318	4834	}
2319	4835
2320	4836	static void rkcif_destroy_dummy_buf(struct rkcif_stream *stream)
2321	4837	{
2322	4838	struct rkcif_device *dev = stream->cifdev;
2323		- struct rkcif_dummy_buffer *dummy_buf = &dev->dummy_buf;
2324		- struct rkcif_hw *hw_dev = dev->hw_dev;
	4839	+ struct rkcif_dummy_buffer *dummy_buf = &dev->hw_dev->dummy_buf;
2325	4840
2326	4841	if (dummy_buf->vaddr)
2327		- dma_free_coherent(hw_dev->dev, dummy_buf->size,
2328		- dummy_buf->vaddr, dummy_buf->dma_addr);
	4842	+ rkcif_free_buffer(dev, dummy_buf);
2329	4843	dummy_buf->dma_addr = 0;
2330	4844	dummy_buf->vaddr = NULL;
2331	4845	}
..	..	@@ -2339,7 +4853,7 @@
2339	4853	if (dev->luma_vdev.enable)
2340	4854	rkcif_stop_luma(&dev->luma_vdev);
2341	4855
2342		- if (dev->hdr.mode != NO_HDR) {
	4856	+ if (dev->hdr.hdr_mode != NO_HDR) {
2343	4857	if (dev->chip_id == CHIP_RK1808_CIF) {
2344	4858	val = rkcif_read_register(dev, CIF_REG_MIPI_WATER_LINE);
2345	4859	val \|= CIF_MIPI_LVDS_SW_DMA_IDLE_RK1808;
..	..	@@ -2363,14 +4877,22 @@
2363	4877	reset_control_deassert(cif_hw->cif_rst[i]);
2364	4878
2365	4879	if (cif_hw->iommu_en) {
2366		- struct iommu_domain *domain;
2367		-
2368		- domain = iommu_get_domain_for_dev(cif_hw->dev);
2369		- if (domain) {
2370		- iommu_detach_device(domain, cif_hw->dev);
2371		- iommu_attach_device(domain, cif_hw->dev);
2372		- }
	4880	+ rockchip_iommu_disable(cif_hw->dev);
	4881	+ rockchip_iommu_enable(cif_hw->dev);
2373	4882	}
	4883	+}
	4884	+
	4885	+void rkcif_do_soft_reset(struct rkcif_device *dev)
	4886	+{
	4887	+ if (dev->active_sensor->mbus.type == V4L2_MBUS_CSI2_DPHY \|\|
	4888	+ dev->active_sensor->mbus.type == V4L2_MBUS_CSI2_CPHY \|\|
	4889	+ dev->active_sensor->mbus.type == V4L2_MBUS_CCP2)
	4890	+ rkcif_write_register_or(dev, CIF_REG_MIPI_LVDS_CTRL, 0x000A0000);
	4891	+ else
	4892	+ rkcif_write_register_or(dev, CIF_REG_DVP_CTRL, 0x000A0000);
	4893	+ usleep_range(10, 20);
	4894	+ v4l2_dbg(1, rkcif_debug, &dev->v4l2_dev,
	4895	+ "vicap do soft reset 0x%x\n", 0x000A0000);
2374	4896	}
2375	4897
2376	4898	static void rkcif_release_rdbk_buf(struct rkcif_stream *stream)
..	..	@@ -2415,9 +4937,52 @@
2415	4937
2416	4938	}
2417	4939
2418		-static void rkcif_stop_streaming(struct vb2_queue *queue)
	4940	+static void rkcif_detach_sync_mode(struct rkcif_device *cif_dev)
2419	4941	{
2420		- struct rkcif_stream *stream = queue->drv_priv;
	4942	+ int i = 0;
	4943	+ struct rkcif_hw *hw = cif_dev->hw_dev;
	4944	+ struct rkcif_device *tmp_dev;
	4945	+ struct rkcif_multi_sync_config *sync_config;
	4946	+
	4947	+ if ((!cif_dev->sync_cfg.type) \|\|
	4948	+ (atomic_read(&cif_dev->pipe.stream_cnt) != 0))
	4949	+ return;
	4950	+ mutex_lock(&hw->dev_lock);
	4951	+ memset(&cif_dev->sync_cfg, 0, sizeof(cif_dev->sync_cfg));
	4952	+ sync_config = &hw->sync_config[cif_dev->sync_cfg.group];
	4953	+ sync_config->streaming_cnt--;
	4954	+ if (cif_dev->sync_cfg.type == EXTERNAL_MASTER_MODE) {
	4955	+ for (i = 0; i < sync_config->ext_master.count; i++) {
	4956	+ tmp_dev = sync_config->ext_master.cif_dev[i];
	4957	+ if (tmp_dev == cif_dev) {
	4958	+ sync_config->ext_master.is_streaming[i] = false;
	4959	+ break;
	4960	+ }
	4961	+ }
	4962	+ }
	4963	+ if (cif_dev->sync_cfg.type == INTERNAL_MASTER_MODE)
	4964	+ sync_config->int_master.is_streaming[0] = false;
	4965	+ if (cif_dev->sync_cfg.type == SLAVE_MODE) {
	4966	+ for (i = 0; i < sync_config->slave.count; i++) {
	4967	+ tmp_dev = sync_config->slave.cif_dev[i];
	4968	+ if (tmp_dev == cif_dev) {
	4969	+ sync_config->slave.is_streaming[i] = false;
	4970	+ break;
	4971	+ }
	4972	+ }
	4973	+ }
	4974	+
	4975	+ if (!sync_config->streaming_cnt && sync_config->is_attach) {
	4976	+ sync_config->is_attach = false;
	4977	+ sync_config->mode = RKCIF_NOSYNC_MODE;
	4978	+ sync_config->dev_cnt = 0;
	4979	+ }
	4980	+ mutex_unlock(&hw->dev_lock);
	4981	+}
	4982	+
	4983	+void rkcif_do_stop_stream(struct rkcif_stream *stream,
	4984	+ unsigned int mode)
	4985	+{
2421	4986	struct rkcif_vdev_node *node = &stream->vnode;
2422	4987	struct rkcif_device *dev = stream->cifdev;
2423	4988	struct v4l2_device *v4l2_dev = &dev->v4l2_dev;
..	..	@@ -2427,93 +4992,172 @@
2427	4992	bool can_reset = true;
2428	4993	int i;
2429	4994	unsigned long flags;
	4995	+ u32 vblank = 0;
	4996	+ u32 frame_time_ns = 0;
	4997	+ u64 cur_time = 0;
	4998	+ u64 fs_time = 0;
2430	4999
2431		- mutex_lock(&dev->hw_dev->dev_lock);
	5000	+ mutex_lock(&dev->stream_lock);
2432	5001
2433		- v4l2_info(&dev->v4l2_dev, "stream[%d] start stopping\n", stream->id);
	5002	+ v4l2_info(&dev->v4l2_dev, "stream[%d] start stopping, total mode 0x%x, cur 0x%x\n",
	5003	+ stream->id, stream->cur_stream_mode, mode);
2434	5004
2435		- stream->stopping = true;
	5005	+ if (mode == stream->cur_stream_mode) {
	5006	+ if (stream->dma_en) {
	5007	+ if (!dev->sensor_linetime)
	5008	+ dev->sensor_linetime = rkcif_get_linetime(stream);
	5009	+ vblank = rkcif_get_sensor_vblank(dev);
	5010	+ if (vblank) {
	5011	+ frame_time_ns = (vblank + dev->terminal_sensor.raw_rect.height) *
	5012	+ dev->sensor_linetime;
	5013	+ spin_lock_irqsave(&stream->fps_lock, flags);
	5014	+ fs_time = stream->readout.fs_timestamp;
	5015	+ spin_unlock_irqrestore(&stream->fps_lock, flags);
	5016	+ cur_time = rkcif_time_get_ns(dev);
	5017	+ if (cur_time > fs_time &&
	5018	+ cur_time - fs_time < (frame_time_ns - 10000000)) {
	5019	+ spin_lock_irqsave(&stream->vbq_lock, flags);
	5020	+ if (stream->dma_en & RKCIF_DMAEN_BY_VICAP)
	5021	+ stream->to_stop_dma = RKCIF_DMAEN_BY_VICAP;
	5022	+ else if (stream->dma_en & RKCIF_DMAEN_BY_ISP)
	5023	+ stream->to_stop_dma = RKCIF_DMAEN_BY_ISP;
	5024	+ stream->is_stop_capture = true;
	5025	+ rkcif_stop_dma_capture(stream);
	5026	+ spin_unlock_irqrestore(&stream->vbq_lock, flags);
	5027	+ }
	5028	+ }
	5029	+ }
	5030	+ stream->stopping = true;
	5031	+ ret = wait_event_timeout(stream->wq_stopped,
	5032	+ stream->state != RKCIF_STATE_STREAMING,
	5033	+ msecs_to_jiffies(500));
	5034	+ if (!ret) {
	5035	+ rkcif_stream_stop(stream);
	5036	+ stream->stopping = false;
	5037	+ }
2436	5038
2437		- ret = wait_event_timeout(stream->wq_stopped,
2438		- stream->state != RKCIF_STATE_STREAMING,
2439		- msecs_to_jiffies(1000));
2440		- if (!ret) {
2441		- rkcif_stream_stop(stream);
2442		- stream->stopping = false;
	5039	+ media_pipeline_stop(&node->vdev.entity);
	5040	+ ret = dev->pipe.set_stream(&dev->pipe, false);
	5041	+ if (ret < 0)
	5042	+ v4l2_err(v4l2_dev, "pipeline stream-off failed error:%d\n",
	5043	+ ret);
	5044	+
	5045	+ dev->is_start_hdr = false;
	5046	+ stream->is_dvp_yuv_addr_init = false;
	5047	+ if (stream->skip_info.skip_en) {
	5048	+ stream->skip_info.skip_en = false;
	5049	+ stream->skip_info.skip_to_en = true;
	5050	+ }
	5051	+ } else if (mode == RKCIF_STREAM_MODE_CAPTURE && stream->dma_en & RKCIF_DMAEN_BY_VICAP) {
	5052	+ //only stop dma
	5053	+ stream->to_stop_dma = RKCIF_DMAEN_BY_VICAP;
	5054	+ stream->is_wait_dma_stop = true;
	5055	+ wait_event_timeout(stream->wq_stopped,
	5056	+ !stream->is_wait_dma_stop,
	5057	+ msecs_to_jiffies(1000));
	5058	+ } else if (mode == RKCIF_STREAM_MODE_TOISP && stream->dma_en & RKCIF_DMAEN_BY_VICAP) {
	5059	+ //only stop dma
	5060	+ stream->to_stop_dma = RKCIF_DMAEN_BY_ISP;
	5061	+ stream->is_wait_dma_stop = true;
	5062	+ wait_event_timeout(stream->wq_stopped,
	5063	+ !stream->is_wait_dma_stop,
	5064	+ msecs_to_jiffies(1000));
	5065	+ }
	5066	+ if ((mode & RKCIF_STREAM_MODE_CAPTURE) == RKCIF_STREAM_MODE_CAPTURE) {
	5067	+ /* release buffers */
	5068	+ spin_lock_irqsave(&stream->vbq_lock, flags);
	5069	+ if (stream->curr_buf)
	5070	+ list_add_tail(&stream->curr_buf->queue, &stream->buf_head);
	5071	+ if (stream->next_buf &&
	5072	+ stream->next_buf != stream->curr_buf)
	5073	+ list_add_tail(&stream->next_buf->queue, &stream->buf_head);
	5074	+ spin_unlock_irqrestore(&stream->vbq_lock, flags);
	5075	+
	5076	+ if (dev->hdr.hdr_mode == HDR_X2 \|\|
	5077	+ dev->hdr.hdr_mode == HDR_X3)
	5078	+ rkcif_release_rdbk_buf(stream);
	5079	+
	5080	+ stream->curr_buf = NULL;
	5081	+ stream->next_buf = NULL;
	5082	+
	5083	+ rkcif_rx_buffer_free(stream);
	5084	+ list_for_each_entry(buf, &stream->buf_head, queue) {
	5085	+ v4l2_dbg(3, rkcif_debug, &dev->v4l2_dev,
	5086	+ "stream[%d] buf return addr 0x%x\n",
	5087	+ stream->id, buf->buff_addr[0]);
	5088	+ vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
	5089	+ }
	5090	+ INIT_LIST_HEAD(&stream->buf_head);
	5091	+ while (!list_empty(&stream->vb_done_list)) {
	5092	+ buf = list_first_entry(&stream->vb_done_list,
	5093	+ struct rkcif_buffer, queue);
	5094	+ if (buf) {
	5095	+ list_del(&buf->queue);
	5096	+ vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
	5097	+ }
	5098	+ }
	5099	+ stream->total_buf_num = 0;
	5100	+ atomic_set(&stream->buf_cnt, 0);
	5101	+ stream->lack_buf_cnt = 0;
	5102	+ stream->dma_en &= ~RKCIF_DMAEN_BY_VICAP;
2443	5103	}
2444	5104
2445		- media_pipeline_stop(&node->vdev.entity);
2446		- ret = dev->pipe.set_stream(&dev->pipe, false);
2447		- if (ret < 0)
2448		- v4l2_err(v4l2_dev, "pipeline stream-off failed error:%d\n",
2449		- ret);
2450		-
2451		- dev->is_start_hdr = false;
2452		- stream->is_dvp_yuv_addr_init = false;
2453		-
2454		- /* release buffers */
2455		- if (stream->curr_buf)
2456		- list_add_tail(&stream->curr_buf->queue, &stream->buf_head);
2457		-
2458		- if (stream->next_buf &&
2459		- stream->next_buf != stream->curr_buf)
2460		- list_add_tail(&stream->next_buf->queue, &stream->buf_head);
2461		-
2462		- if (dev->hdr.mode != NO_HDR)
2463		- rkcif_release_rdbk_buf(stream);
2464		-
2465		- stream->curr_buf = NULL;
2466		- stream->next_buf = NULL;
2467		-
2468		- while (!list_empty(&stream->buf_head)) {
2469		- buf = list_first_entry(&stream->buf_head,
2470		- struct rkcif_buffer, queue);
2471		- list_del(&buf->queue);
2472		- vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
2473		- }
2474		-
2475		- ret = dev->pipe.close(&dev->pipe);
2476		- if (ret < 0)
2477		- v4l2_err(v4l2_dev, "pipeline close failed error:%d\n", ret);
2478		-
2479		- if (dev->hdr.mode == HDR_X2) {
2480		- if (dev->stream[RKCIF_STREAM_MIPI_ID0].state == RKCIF_STATE_READY &&
2481		- dev->stream[RKCIF_STREAM_MIPI_ID1].state == RKCIF_STATE_READY) {
	5105	+ if (mode == stream->cur_stream_mode) {
	5106	+ ret = dev->pipe.close(&dev->pipe);
	5107	+ if (ret < 0)
	5108	+ v4l2_err(v4l2_dev, "pipeline close failed error:%d\n", ret);
	5109	+ if (dev->hdr.hdr_mode == HDR_X2) {
	5110	+ if (dev->stream[RKCIF_STREAM_MIPI_ID0].state == RKCIF_STATE_READY &&
	5111	+ dev->stream[RKCIF_STREAM_MIPI_ID1].state == RKCIF_STATE_READY) {
	5112	+ dev->can_be_reset = true;
	5113	+ }
	5114	+ } else if (dev->hdr.hdr_mode == HDR_X3) {
	5115	+ if (dev->stream[RKCIF_STREAM_MIPI_ID0].state == RKCIF_STATE_READY &&
	5116	+ dev->stream[RKCIF_STREAM_MIPI_ID1].state == RKCIF_STATE_READY &&
	5117	+ dev->stream[RKCIF_STREAM_MIPI_ID2].state == RKCIF_STATE_READY) {
	5118	+ dev->can_be_reset = true;
	5119	+ }
	5120	+ } else {
2482	5121	dev->can_be_reset = true;
2483	5122	}
2484		- } else if (dev->hdr.mode == HDR_X3) {
2485		- if (dev->stream[RKCIF_STREAM_MIPI_ID0].state == RKCIF_STATE_READY &&
2486		- dev->stream[RKCIF_STREAM_MIPI_ID1].state == RKCIF_STATE_READY &&
2487		- dev->stream[RKCIF_STREAM_MIPI_ID2].state == RKCIF_STATE_READY) {
2488		- dev->can_be_reset = true;
	5123	+ mutex_lock(&hw_dev->dev_lock);
	5124	+ for (i = 0; i < hw_dev->dev_num; i++) {
	5125	+ if (atomic_read(&hw_dev->cif_dev[i]->pipe.stream_cnt) != 0) {
	5126	+ can_reset = false;
	5127	+ break;
	5128	+ }
2489	5129	}
2490		- } else {
2491		- dev->can_be_reset = true;
2492		- }
2493		-
2494		- spin_lock_irqsave(&hw_dev->hw_timer.timer_lock, flags);
2495		- for (i = 0; i < hw_dev->dev_num; i++) {
2496		- if (atomic_read(&hw_dev->cif_dev[i]->pipe.stream_cnt) != 0) {
2497		- can_reset = false;
2498		- break;
	5130	+ mutex_unlock(&hw_dev->dev_lock);
	5131	+ if (dev->can_be_reset && dev->chip_id >= CHIP_RK3588_CIF) {
	5132	+ rkcif_do_soft_reset(dev);
	5133	+ atomic_set(&dev->streamoff_cnt, 0);
2499	5134	}
	5135	+ if (dev->can_be_reset && can_reset) {
	5136	+ dev->can_be_reset = false;
	5137	+ dev->reset_work_cancel = true;
	5138	+ dev->early_line = 0;
	5139	+ dev->sensor_linetime = 0;
	5140	+ dev->wait_line = 0;
	5141	+ stream->is_line_wake_up = false;
	5142	+ }
	5143	+ if (can_reset && hw_dev->dummy_buf.vaddr)
	5144	+ rkcif_destroy_dummy_buf(stream);
2500	5145	}
	5146	+ if (mode == RKCIF_STREAM_MODE_CAPTURE)
	5147	+ tasklet_disable(&stream->vb_done_tasklet);
2501	5148
2502		- if (dev->can_be_reset && can_reset) {
2503		- rkcif_do_cru_reset(dev);
2504		- dev->can_be_reset = false;
2505		- hw_dev->reset_work_cancel = true;
2506		- hw_dev->hw_timer.is_running = false;
2507		- hw_dev->reset_info.is_need_reset = 0;
2508		- }
2509		- spin_unlock_irqrestore(&hw_dev->hw_timer.timer_lock, flags);
	5149	+ stream->cur_stream_mode &= ~mode;
	5150	+ INIT_LIST_HEAD(&stream->vb_done_list);
	5151	+ v4l2_info(&dev->v4l2_dev, "stream[%d] stopping finished, dma_en 0x%x\n", stream->id, stream->dma_en);
	5152	+ mutex_unlock(&dev->stream_lock);
	5153	+ rkcif_detach_sync_mode(dev);
	5154	+}
2510	5155
2511		- if (!atomic_read(&dev->pipe.stream_cnt) && dev->dummy_buf.vaddr)
2512		- rkcif_destroy_dummy_buf(stream);
	5156	+static void rkcif_stop_streaming(struct vb2_queue *queue)
	5157	+{
	5158	+ struct rkcif_stream *stream = queue->drv_priv;
2513	5159
2514		- v4l2_info(&dev->v4l2_dev, "stream[%d] stopping finished\n", stream->id);
2515		-
2516		- mutex_unlock(&dev->hw_dev->dev_lock);
	5160	+ rkcif_do_stop_stream(stream, RKCIF_STREAM_MODE_CAPTURE);
2517	5161	}
2518	5162
2519	5163	/*
..	..	@@ -2589,6 +5233,70 @@
2589	5233	return mode;
2590	5234	}
2591	5235
	5236	+static u32 rkcif_determine_input_mode_rk3588(struct rkcif_stream *stream)
	5237	+{
	5238	+ struct rkcif_device *dev = stream->cifdev;
	5239	+ struct rkcif_sensor_info *sensor_info = dev->active_sensor;
	5240	+ struct rkcif_sensor_info *terminal_sensor = &dev->terminal_sensor;
	5241	+ __u32 intf = BT656_STD_RAW;
	5242	+ u32 mode = INPUT_MODE_YUV;
	5243	+ v4l2_std_id std;
	5244	+ int ret;
	5245	+
	5246	+ ret = v4l2_subdev_call(sensor_info->sd, video, querystd, &std);
	5247	+ if (ret == 0) {
	5248	+ /* retrieve std from sensor if exist */
	5249	+ switch (std) {
	5250	+ case V4L2_STD_NTSC:
	5251	+ case V4L2_STD_PAL:
	5252	+ mode = INPUT_BT656_YUV422;
	5253	+ break;
	5254	+ case V4L2_STD_ATSC:
	5255	+ mode = INPUT_BT1120_YUV422;
	5256	+ break;
	5257	+ default:
	5258	+ v4l2_err(&dev->v4l2_dev,
	5259	+ "std: %lld is not supported", std);
	5260	+ }
	5261	+ mode \|= CSI_WRDDR_TYPE_RAW8 << 6;
	5262	+ } else {
	5263	+ /* determine input mode by mbus_code (fmt_type) */
	5264	+ switch (stream->cif_fmt_in->fmt_type) {
	5265	+ case CIF_FMT_TYPE_YUV:
	5266	+ if (sensor_info->mbus.type == V4L2_MBUS_BT656) {
	5267	+ if ((sensor_info->mbus.flags & CIF_DVP_PCLK_DUAL_EDGE) == CIF_DVP_PCLK_DUAL_EDGE)
	5268	+ mode = INPUT_BT1120_YUV422;
	5269	+ else
	5270	+ mode = INPUT_BT656_YUV422;
	5271	+ } else {
	5272	+ mode = INPUT_BT601_YUV422;
	5273	+ }
	5274	+ mode \|= CSI_WRDDR_TYPE_RAW8 << 6;
	5275	+ break;
	5276	+ case CIF_FMT_TYPE_RAW:
	5277	+ ret = v4l2_subdev_call(terminal_sensor->sd,
	5278	+ core, ioctl,
	5279	+ RKMODULE_GET_BT656_INTF_TYPE,
	5280	+ &intf);
	5281	+ if (!ret) {
	5282	+ if (intf == BT656_SONY_RAW)
	5283	+ mode = INPUT_SONY_RAW;
	5284	+ else
	5285	+ mode = INPUT_BT601_RAW;
	5286	+ } else {
	5287	+ mode = INPUT_BT601_RAW;
	5288	+ }
	5289	+ mode \|= stream->cif_fmt_in->csi_fmt_val << 6;
	5290	+ break;
	5291	+ }
	5292	+ }
	5293	+ if (stream->cif_fmt_in->field == V4L2_FIELD_NONE)
	5294	+ mode \|= TRANSMIT_PROGRESS_RK3588;
	5295	+ else
	5296	+ mode \|= TRANSMIT_INTERFACE_RK3588;
	5297	+ return mode;
	5298	+}
	5299	+
2592	5300	static inline u32 rkcif_scl_ctl(struct rkcif_stream *stream)
2593	5301	{
2594	5302	u32 fmt_type = stream->cif_fmt_in->fmt_type;
..	..	@@ -2613,15 +5321,21 @@
2613	5321	case V4L2_PIX_FMT_NV61:
2614	5322	case V4L2_PIX_FMT_NV12:
2615	5323	case V4L2_PIX_FMT_NV21:
2616		- case V4L2_PIX_FMT_YUYV:
2617		- case V4L2_PIX_FMT_YVYU:
2618		- case V4L2_PIX_FMT_UYVY:
2619		- case V4L2_PIX_FMT_VYUY:
2620	5324	case V4L2_PIX_FMT_GREY:
2621	5325	case V4L2_PIX_FMT_Y16:
2622	5326	bpp = fmt->bpp[plane_index];
2623	5327	break;
	5328	+ case V4L2_PIX_FMT_YUYV:
	5329	+ case V4L2_PIX_FMT_YVYU:
	5330	+ case V4L2_PIX_FMT_UYVY:
	5331	+ case V4L2_PIX_FMT_VYUY:
	5332	+ if (stream->cifdev->chip_id < CHIP_RK3588_CIF)
	5333	+ bpp = fmt->bpp[plane_index];
	5334	+ else
	5335	+ bpp = fmt->bpp[plane_index + 1];
	5336	+ break;
2624	5337	case V4L2_PIX_FMT_RGB24:
	5338	+ case V4L2_PIX_FMT_BGR24:
2625	5339	case V4L2_PIX_FMT_RGB565:
2626	5340	case V4L2_PIX_FMT_BGR666:
2627	5341	case V4L2_PIX_FMT_SRGGB8:
..	..	@@ -2637,8 +5351,13 @@
2637	5351	case V4L2_PIX_FMT_SGBRG12:
2638	5352	case V4L2_PIX_FMT_SBGGR12:
2639	5353	case V4L2_PIX_FMT_SBGGR16:
	5354	+ case V4L2_PIX_FMT_SGBRG16:
	5355	+ case V4L2_PIX_FMT_SGRBG16:
	5356	+ case V4L2_PIX_FMT_SRGGB16:
2640	5357	case V4l2_PIX_FMT_SPD16:
2641	5358	case V4l2_PIX_FMT_EBD8:
	5359	+ case V4L2_PIX_FMT_Y10:
	5360	+ case V4L2_PIX_FMT_Y12:
2642	5361	if (stream->cifdev->chip_id < CHIP_RV1126_CIF) {
2643	5362	bpp = max(fmt->bpp[plane_index], (u8)CIF_RAW_STORED_BIT_WIDTH);
2644	5363	cal = CIF_RAW_STORED_BIT_WIDTH;
..	..	@@ -2654,7 +5373,8 @@
2654	5373	}
2655	5374	break;
2656	5375	default:
2657		- pr_err("fourcc: %d is not supported!\n", fmt->fourcc);
	5376	+ v4l2_err(&stream->cifdev->v4l2_dev, "fourcc: %d is not supported!\n",
	5377	+ fmt->fourcc);
2658	5378	break;
2659	5379	}
2660	5380	}
..	..	@@ -2688,7 +5408,8 @@
2688	5408
2689	5409	if (dev->terminal_sensor.sd) {
2690	5410	input_sel.target = V4L2_SEL_TGT_CROP_BOUNDS;
2691		-
	5411	+ input_sel.which = V4L2_SUBDEV_FORMAT_ACTIVE;
	5412	+ input_sel.pad = 0;
2692	5413	ret = v4l2_subdev_call(dev->terminal_sensor.sd,
2693	5414	pad, get_selection, NULL,
2694	5415	&input_sel);
..	..	@@ -2698,6 +5419,7 @@
2698	5419	stream->crop_mask \|= CROP_SRC_SENSOR_MASK;
2699	5420	dev->terminal_sensor.selection = input_sel;
2700	5421	} else {
	5422	+ stream->crop_mask &= ~CROP_SRC_SENSOR_MASK;
2701	5423	dev->terminal_sensor.selection.r = dev->terminal_sensor.raw_rect;
2702	5424	}
2703	5425	}
..	..	@@ -2718,8 +5440,10 @@
2718	5440	stream->crop[CROP_SRC_ACT].top = stream->crop[CROP_SRC_USR].top +
2719	5441	stream->crop[CROP_SRC_SENSOR].top;
2720	5442	}
2721		- } else {
	5443	+ } else if (stream->crop_mask & CROP_SRC_SENSOR_MASK) {
2722	5444	stream->crop[CROP_SRC_ACT] = stream->crop[CROP_SRC_SENSOR];
	5445	+ } else {
	5446	+ stream->crop[CROP_SRC_ACT] = dev->terminal_sensor.raw_rect;
2723	5447	}
2724	5448	}
2725	5449
..	..	@@ -2733,11 +5457,11 @@
2733	5457	struct rkcif_device *dev = stream->cifdev;
2734	5458	struct v4l2_device *v4l2_dev = &dev->v4l2_dev;
2735	5459	struct v4l2_rect input, *crop;
2736		- int vc;
2737	5460
2738	5461	if (dev->terminal_sensor.sd) {
2739		- stream->cif_fmt_in = get_input_fmt(dev->terminal_sensor.sd,
2740		- &input, stream->id, &vc);
	5462	+ stream->cif_fmt_in = rkcif_get_input_fmt(dev,
	5463	+ &input, stream->id,
	5464	+ &dev->channels[stream->id]);
2741	5465	if (!stream->cif_fmt_in) {
2742	5466	v4l2_err(v4l2_dev, "Input fmt is invalid\n");
2743	5467	return -EINVAL;
..	..	@@ -2747,7 +5471,6 @@
2747	5471	return -EINVAL;
2748	5472	}
2749	5473
2750		- stream->vc = vc;
2751	5474	if (stream->cif_fmt_in->mbus_code == MEDIA_BUS_FMT_EBD_1X8 \|\|
2752	5475	stream->cif_fmt_in->mbus_code == MEDIA_BUS_FMT_SPD_2X8) {
2753	5476	stream->crop_enable = false;
..	..	@@ -2765,7 +5488,9 @@
2765	5488	return -EINVAL;
2766	5489	}
2767	5490
2768		- if (dev->active_sensor->mbus.type == V4L2_MBUS_CSI2) {
	5491	+ if (dev->active_sensor &&
	5492	+ (dev->active_sensor->mbus.type == V4L2_MBUS_CSI2_DPHY \|\|
	5493	+ dev->active_sensor->mbus.type == V4L2_MBUS_CSI2_CPHY)) {
2769	5494	if (crop->left > 0) {
2770	5495	int align_x = get_csi_crop_align(stream->cif_fmt_in);
2771	5496
..	..	@@ -2776,7 +5501,7 @@
2776	5501	return -EINVAL;
2777	5502	}
2778	5503	}
2779		- } else if (dev->active_sensor->mbus.type == V4L2_MBUS_CCP2) {
	5504	+ } else if (dev->active_sensor && dev->active_sensor->mbus.type == V4L2_MBUS_CCP2) {
2780	5505	if (crop->left % 4 != 0 && crop->width % 4 != 0) {
2781	5506	v4l2_err(v4l2_dev,
2782	5507	"ERROR: lvds crop left and width must align %d\n", 4);
..	..	@@ -2808,8 +5533,8 @@
2808	5533	__func__, stream->id);
2809	5534	return -ENODEV;
2810	5535	}
2811		- ret = v4l2_subdev_call(sensor->sd, video, g_mbus_config,
2812		- &sensor->mbus);
	5536	+ ret = v4l2_subdev_call(sensor->sd, pad, get_mbus_config,
	5537	+ 0, &sensor->mbus);
2813	5538	if (ret && ret != -ENOIOCTLCMD) {
2814	5539	v4l2_err(&stream->cifdev->v4l2_dev,
2815	5540	"%s: get remote %s mbus failed!\n", __func__, sensor->sd->name);
..	..	@@ -2821,24 +5546,38 @@
2821	5546	terminal_sensor = &stream->cifdev->terminal_sensor;
2822	5547	get_remote_terminal_sensor(stream, &terminal_sensor->sd);
2823	5548	if (terminal_sensor->sd) {
2824		- ret = v4l2_subdev_call(terminal_sensor->sd, video, g_mbus_config,
2825		- &terminal_sensor->mbus);
	5549	+ ret = v4l2_subdev_call(terminal_sensor->sd, pad, get_mbus_config,
	5550	+ 0, &terminal_sensor->mbus);
2826	5551	if (ret && ret != -ENOIOCTLCMD) {
2827	5552	v4l2_err(&stream->cifdev->v4l2_dev,
2828	5553	"%s: get terminal %s mbus failed!\n",
2829	5554	__func__, terminal_sensor->sd->name);
2830	5555	return ret;
2831	5556	}
2832		- if (v4l2_subdev_call(terminal_sensor->sd, core, ioctl, RKMODULE_GET_CSI_DSI_INFO,
2833		- &terminal_sensor->dsi_input_en)) {
2834		- v4l2_dbg(1, rkcif_debug, &stream->cifdev->v4l2_dev,
2835		- "%s: get terminal %s CSI/DSI sel failed, default csi input!\n",
	5557	+ ret = v4l2_subdev_call(terminal_sensor->sd, video,
	5558	+ g_frame_interval, &terminal_sensor->fi);
	5559	+ if (ret) {
	5560	+ v4l2_err(&stream->cifdev->v4l2_dev,
	5561	+ "%s: get terminal %s g_frame_interval failed!\n",
2836	5562	__func__, terminal_sensor->sd->name);
	5563	+ return ret;
	5564	+ }
	5565	+ if (v4l2_subdev_call(terminal_sensor->sd, core, ioctl, RKMODULE_GET_CSI_DSI_INFO,
	5566	+ &terminal_sensor->dsi_input_en)) {
	5567	+ v4l2_dbg(1, rkcif_debug, &stream->cifdev->v4l2_dev,
	5568	+ "%s: get terminal %s CSI/DSI sel failed, default csi input!\n",
	5569	+ __func__, terminal_sensor->sd->name);
2837	5570	terminal_sensor->dsi_input_en = 0;
2838	5571	}
	5572	+ } else {
	5573	+ v4l2_err(&stream->cifdev->v4l2_dev,
	5574	+ "%s: stream[%d] get remote terminal sensor failed!\n",
	5575	+ __func__, stream->id);
	5576	+ return -ENODEV;
2839	5577	}
2840	5578
2841		- if (terminal_sensor->mbus.type == V4L2_MBUS_CSI2 \|\|
	5579	+ if (terminal_sensor->mbus.type == V4L2_MBUS_CSI2_DPHY \|\|
	5580	+ terminal_sensor->mbus.type == V4L2_MBUS_CSI2_CPHY \|\|
2842	5581	terminal_sensor->mbus.type == V4L2_MBUS_CCP2) {
2843	5582	switch (terminal_sensor->mbus.flags & V4L2_MBUS_CSI2_LANES) {
2844	5583	case V4L2_MBUS_CSI2_1_LANE:
..	..	@@ -2865,7 +5604,85 @@
2865	5604	return ret;
2866	5605	}
2867	5606
2868		-static int rkcif_stream_start(struct rkcif_stream *stream)
	5607	+static int rkcif_dvp_get_output_type_mask(struct rkcif_stream *stream)
	5608	+{
	5609	+ unsigned int mask;
	5610	+ const struct cif_output_fmt *fmt = stream->cif_fmt_out;
	5611	+
	5612	+ switch (fmt->fourcc) {
	5613	+ case V4L2_PIX_FMT_NV16:
	5614	+ mask = (CSI_WRDDR_TYPE_YUV422SP_RK3588 << 11) \|
	5615	+ (CSI_YUV_OUTPUT_ORDER_UYVY << 1);
	5616	+ break;
	5617	+ case V4L2_PIX_FMT_NV61:
	5618	+ mask = (CSI_WRDDR_TYPE_YUV422SP_RK3588 << 11) \|
	5619	+ (CSI_YUV_OUTPUT_ORDER_VYUY << 1);
	5620	+ break;
	5621	+ case V4L2_PIX_FMT_NV12:
	5622	+ mask = (CSI_WRDDR_TYPE_YUV420SP_RK3588 << 11) \|
	5623	+ (CSI_YUV_OUTPUT_ORDER_UYVY << 1);
	5624	+ break;
	5625	+ case V4L2_PIX_FMT_NV21:
	5626	+ mask = (CSI_WRDDR_TYPE_YUV420SP_RK3588 << 11) \|
	5627	+ (CSI_YUV_OUTPUT_ORDER_VYUY << 1);
	5628	+ break;
	5629	+ case V4L2_PIX_FMT_YUYV:
	5630	+ mask = (CSI_WRDDR_TYPE_YUV_PACKET << 11) \|
	5631	+ (CSI_YUV_OUTPUT_ORDER_YUYV << 1);
	5632	+ break;
	5633	+ case V4L2_PIX_FMT_YVYU:
	5634	+ mask = (CSI_WRDDR_TYPE_YUV_PACKET << 11) \|
	5635	+ (CSI_YUV_OUTPUT_ORDER_YVYU << 1);
	5636	+ break;
	5637	+ case V4L2_PIX_FMT_UYVY:
	5638	+ mask = (CSI_WRDDR_TYPE_YUV_PACKET << 11) \|
	5639	+ (CSI_YUV_OUTPUT_ORDER_UYVY << 1);
	5640	+ break;
	5641	+ case V4L2_PIX_FMT_VYUY:
	5642	+ mask = (CSI_WRDDR_TYPE_YUV_PACKET << 11) \|
	5643	+ (CSI_YUV_OUTPUT_ORDER_VYUY << 1);
	5644	+ break;
	5645	+ case V4L2_PIX_FMT_RGB24:
	5646	+ case V4L2_PIX_FMT_BGR24:
	5647	+ case V4L2_PIX_FMT_RGB565:
	5648	+ case V4L2_PIX_FMT_BGR666:
	5649	+ mask = CSI_WRDDR_TYPE_RAW_COMPACT << 11;
	5650	+ break;
	5651	+ case V4L2_PIX_FMT_SRGGB8:
	5652	+ case V4L2_PIX_FMT_SGRBG8:
	5653	+ case V4L2_PIX_FMT_SGBRG8:
	5654	+ case V4L2_PIX_FMT_SBGGR8:
	5655	+ case V4L2_PIX_FMT_SRGGB10:
	5656	+ case V4L2_PIX_FMT_SGRBG10:
	5657	+ case V4L2_PIX_FMT_SGBRG10:
	5658	+ case V4L2_PIX_FMT_SBGGR10:
	5659	+ case V4L2_PIX_FMT_SRGGB12:
	5660	+ case V4L2_PIX_FMT_SGRBG12:
	5661	+ case V4L2_PIX_FMT_SGBRG12:
	5662	+ case V4L2_PIX_FMT_SBGGR12:
	5663	+ case V4L2_PIX_FMT_GREY:
	5664	+ case V4L2_PIX_FMT_Y10:
	5665	+ case V4L2_PIX_FMT_Y12:
	5666	+ if (stream->is_compact)
	5667	+ mask = CSI_WRDDR_TYPE_RAW_COMPACT << 11;
	5668	+ else
	5669	+ mask = CSI_WRDDR_TYPE_RAW_UNCOMPACT << 11;
	5670	+ break;
	5671	+ case V4L2_PIX_FMT_SBGGR16:
	5672	+ case V4L2_PIX_FMT_SGBRG16:
	5673	+ case V4L2_PIX_FMT_SGRBG16:
	5674	+ case V4L2_PIX_FMT_SRGGB16:
	5675	+ case V4L2_PIX_FMT_Y16:
	5676	+ mask = CSI_WRDDR_TYPE_RAW_UNCOMPACT << 11;
	5677	+ break;
	5678	+ default:
	5679	+ mask = CSI_WRDDR_TYPE_RAW_COMPACT << 11;
	5680	+ break;
	5681	+ }
	5682	+ return mask;
	5683	+}
	5684	+
	5685	+static int rkcif_stream_start(struct rkcif_stream *stream, unsigned int mode)
2869	5686	{
2870	5687	u32 val, mbus_flags, href_pol, vsync_pol,
2871	5688	xfer_mode = 0, yc_swap = 0, inputmode = 0,
..	..	@@ -2874,20 +5691,66 @@
2874	5691	multi_id_mode = BT656_1120_MULTI_ID_MODE_1,
2875	5692	multi_id_sel = BT656_1120_MULTI_ID_SEL_LSB,
2876	5693	bt1120_edge_mode = BT1120_CLOCK_SINGLE_EDGES,
2877		- bt1120_flags = 0;
	5694	+ bt1120_flags = 0,
	5695	+ out_fmt_mask = 0,
	5696	+ in_fmt_yuv_order = 0;
2878	5697	struct rkmodule_bt656_mbus_info bt1120_info;
2879	5698	struct rkcif_device *dev = stream->cifdev;
2880	5699	struct rkcif_sensor_info *sensor_info;
2881	5700	struct v4l2_mbus_config *mbus;
2882	5701	struct rkcif_dvp_sof_subdev *sof_sd = &dev->dvp_sof_subdev;
2883	5702	const struct cif_output_fmt *fmt;
	5703	+ unsigned int dma_en = 0;
	5704	+ unsigned int dma_state = 0;
	5705	+ int i = 0;
	5706	+ u32 sav_detect = BT656_DETECT_SAV;
	5707	+ u32 reserved = 0;
2884	5708
2885		- if (stream->state != RKCIF_STATE_RESET_IN_STREAMING)
	5709	+ if (stream->state < RKCIF_STATE_STREAMING) {
2886	5710	stream->frame_idx = 0;
	5711	+ stream->buf_wake_up_cnt = 0;
	5712	+ stream->lack_buf_cnt = 0;
	5713	+ stream->frame_phase = 0;
	5714	+ stream->is_in_vblank = false;
	5715	+ stream->is_change_toisp = false;
	5716	+ }
2887	5717
2888	5718	sensor_info = dev->active_sensor;
2889	5719	mbus = &sensor_info->mbus;
2890	5720
	5721	+ dma_state = stream->dma_en;
	5722	+ if ((mode & RKCIF_STREAM_MODE_CAPTURE) == RKCIF_STREAM_MODE_CAPTURE)
	5723	+ stream->dma_en \|= RKCIF_DMAEN_BY_VICAP;
	5724	+ else if ((mode & RKCIF_STREAM_MODE_TOISP_RDBK) == RKCIF_STREAM_MODE_TOISP_RDBK)
	5725	+ stream->dma_en \|= RKCIF_DMAEN_BY_ISP;
	5726	+ else if ((mode & RKCIF_STREAM_MODE_ROCKIT) == RKCIF_STREAM_MODE_ROCKIT)
	5727	+ stream->dma_en \|= RKCIF_DMAEN_BY_ROCKIT;
	5728	+
	5729	+ if (dma_state)
	5730	+ return 0;
	5731	+
	5732	+ mbus_flags = mbus->flags;
	5733	+ if ((mbus_flags & CIF_DVP_PCLK_DUAL_EDGE) == CIF_DVP_PCLK_DUAL_EDGE) {
	5734	+ bt1120_edge_mode = (dev->chip_id < CHIP_RK3588_CIF ?
	5735	+ BT1120_CLOCK_DOUBLE_EDGES : BT1120_CLOCK_DOUBLE_EDGES_RK3588);
	5736	+ rkcif_enable_dvp_clk_dual_edge(dev, true);
	5737	+ } else {
	5738	+ bt1120_edge_mode = dev->chip_id < CHIP_RK3588_CIF ?
	5739	+ BT1120_CLOCK_SINGLE_EDGES : BT1120_CLOCK_SINGLE_EDGES_RK3588;
	5740	+ rkcif_enable_dvp_clk_dual_edge(dev, false);
	5741	+ }
	5742	+
	5743	+ if (IS_ENABLED(CONFIG_CPU_RV1106))
	5744	+ rkcif_config_dvp_pin(dev, true);
	5745	+
	5746	+ if (mbus_flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
	5747	+ rkcif_config_dvp_clk_sampling_edge(dev, RKCIF_CLK_RISING);
	5748	+ else
	5749	+ rkcif_config_dvp_clk_sampling_edge(dev, RKCIF_CLK_FALLING);
	5750	+
	5751	+#if IS_ENABLED(CONFIG_CPU_RV1106)
	5752	+ rv1106_sdmmc_get_lock();
	5753	+#endif
2891	5754	if (sensor_info->sd && mbus->type == V4L2_MBUS_BT656) {
2892	5755	int ret;
2893	5756
..	..	@@ -2911,32 +5774,21 @@
2911	5774	multi_id_mode = BT656_1120_MULTI_ID_MODE_4;
2912	5775	else if (((bt1120_flags & RKMODULE_CAMERA_BT656_CHANNELS) >> 2) > 1)
2913	5776	multi_id_mode = BT656_1120_MULTI_ID_MODE_2;
2914		-
2915		- multi_id = DVP_SW_MULTI_ID(stream->id, stream->id, bt1120_info.id_en_bits);
	5777	+ for (i = 0; i < 4; i++)
	5778	+ multi_id \|= DVP_SW_MULTI_ID(i, i, bt1120_info.id_en_bits);
2916	5779	rkcif_write_register_or(dev, CIF_REG_DVP_MULTI_ID, multi_id);
2917	5780	}
2918	5781	}
2919		-
2920		- mbus_flags = mbus->flags;
2921		- if ((mbus_flags & CIF_DVP_PCLK_DUAL_EDGE) == CIF_DVP_PCLK_DUAL_EDGE) {
2922		- bt1120_edge_mode = BT1120_CLOCK_DOUBLE_EDGES;
2923		- rkcif_enable_dvp_clk_dual_edge(dev, true);
2924		- } else {
2925		- bt1120_edge_mode = BT1120_CLOCK_SINGLE_EDGES;
2926		- rkcif_enable_dvp_clk_dual_edge(dev, false);
2927		- }
2928		-
2929		- if (mbus_flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
2930		- rkcif_config_dvp_clk_sampling_edge(dev, RKCIF_CLK_RISING);
2931		- else
2932		- rkcif_config_dvp_clk_sampling_edge(dev, RKCIF_CLK_FALLING);
2933	5782
2934	5783	href_pol = (mbus_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) ?
2935	5784	HSY_HIGH_ACTIVE : HSY_LOW_ACTIVE;
2936	5785	vsync_pol = (mbus_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH) ?
2937	5786	VSY_HIGH_ACTIVE : VSY_LOW_ACTIVE;
2938	5787
2939		- inputmode = rkcif_determine_input_mode(stream);
	5788	+ if (dev->chip_id < CHIP_RK3588_CIF)
	5789	+ inputmode = rkcif_determine_input_mode(stream);
	5790	+ else
	5791	+ inputmode = rkcif_determine_input_mode_rk3588(stream);
2940	5792	if (dev->chip_id <= CHIP_RK1808_CIF) {
2941	5793	if (inputmode == INPUT_MODE_BT1120) {
2942	5794	if (stream->cif_fmt_in->field == V4L2_FIELD_NONE)
..	..	@@ -2946,7 +5798,7 @@
2946	5798	if (CIF_FETCH_IS_Y_FIRST(stream->cif_fmt_in->dvp_fmt_val))
2947	5799	yc_swap = BT1120_YC_SWAP;
2948	5800	}
2949		- } else {
	5801	+ } else if (dev->chip_id < CHIP_RK3588_CIF) {
2950	5802	if (sensor_info->mbus.type == V4L2_MBUS_BT656) {
2951	5803	if (stream->cif_fmt_in->field == V4L2_FIELD_NONE)
2952	5804	xfer_mode = BT1120_TRANSMIT_PROGRESS;
..	..	@@ -2958,9 +5810,14 @@
2958	5810	if (CIF_FETCH_IS_Y_FIRST(stream->cif_fmt_in->dvp_fmt_val))
2959	5811	yc_swap = BT1120_YC_SWAP;
2960	5812	}
	5813	+ } else {
	5814	+ if ((inputmode & INPUT_BT1120_YUV422) == INPUT_BT1120_YUV422)
	5815	+ if (CIF_FETCH_IS_Y_FIRST(stream->cif_fmt_in->dvp_fmt_val))
	5816	+ yc_swap = BT1120_YC_SWAP_RK3588;
2961	5817	}
2962	5818
2963		- if (dev->active_sensor->mbus.type == V4L2_MBUS_CSI2) {
	5819	+ if (dev->active_sensor->mbus.type == V4L2_MBUS_CSI2_DPHY \|\|
	5820	+ dev->active_sensor->mbus.type == V4L2_MBUS_CSI2_CPHY) {
2964	5821	inputmode = INPUT_MODE_MIPI;
2965	5822
2966	5823	/* if cif is linked with mipi,
..	..	@@ -2978,33 +5835,43 @@
2978	5835	mipimode = MIPI_MODE_32BITS_BYPASS;
2979	5836	}
2980	5837
2981		- val = vsync_pol \| href_pol \| inputmode \| mipimode
2982		- \| stream->cif_fmt_out->fmt_val
2983		- \| stream->cif_fmt_in->dvp_fmt_val
2984		- \| xfer_mode \| yc_swap \| multi_id_en
2985		- \| multi_id_sel \| multi_id_mode \| bt1120_edge_mode;
2986		-
2987		- if (stream->is_high_align)
2988		- val \|= CIF_HIGH_ALIGN;
2989		- else
2990		- val &= ~CIF_HIGH_ALIGN;
	5838	+ if (dev->chip_id < CHIP_RK3588_CIF) {
	5839	+ val = vsync_pol \| href_pol \| inputmode \| mipimode
	5840	+ \| stream->cif_fmt_out->fmt_val
	5841	+ \| stream->cif_fmt_in->dvp_fmt_val
	5842	+ \| xfer_mode \| yc_swap \| multi_id_en
	5843	+ \| multi_id_sel \| multi_id_mode \| bt1120_edge_mode;
	5844	+ if (stream->is_high_align)
	5845	+ val \|= CIF_HIGH_ALIGN;
	5846	+ else
	5847	+ val &= ~CIF_HIGH_ALIGN;
	5848	+ } else {
	5849	+ out_fmt_mask = rkcif_dvp_get_output_type_mask(stream);
	5850	+ in_fmt_yuv_order = rkcif_dvp_get_input_yuv_order(stream);
	5851	+ val = vsync_pol \| href_pol \| inputmode
	5852	+ \| yc_swap
	5853	+ \| out_fmt_mask
	5854	+ \| in_fmt_yuv_order
	5855	+ \| multi_id_en
	5856	+ \| sav_detect
	5857	+ \| multi_id_sel \| multi_id_mode \| bt1120_edge_mode;
	5858	+ if (stream->is_high_align)
	5859	+ val \|= CIF_HIGH_ALIGN_RK3588;
	5860	+ else
	5861	+ val &= ~CIF_HIGH_ALIGN_RK3588;
	5862	+ }
2991	5863	rkcif_write_register(dev, CIF_REG_DVP_FOR, val);
2992	5864
2993		- val = stream->pixm.width;
2994		- if (stream->cif_fmt_in->fmt_type == CIF_FMT_TYPE_RAW) {
2995		- fmt = find_output_fmt(stream, stream->pixm.pixelformat);
2996		- if (fmt->fourcc == V4L2_PIX_FMT_GREY \|\|
2997		- fmt->fourcc == V4L2_PIX_FMT_SRGGB8 \|\|
2998		- fmt->fourcc == V4L2_PIX_FMT_SGRBG8 \|\|
2999		- fmt->fourcc == V4L2_PIX_FMT_SGBRG8 \|\|
3000		- fmt->fourcc == V4L2_PIX_FMT_SBGGR8)
	5865	+ if (dev->chip_id >= CHIP_RK3588_CIF) {
	5866	+ val = stream->pixm.plane_fmt[0].bytesperline;
	5867	+ } else {
	5868	+ fmt = rkcif_find_output_fmt(stream, stream->pixm.pixelformat);
	5869	+ if (fmt->fmt_type == CIF_FMT_TYPE_RAW &&
	5870	+ fmt->csi_fmt_val == CSI_WRDDR_TYPE_RAW8)
3001	5871	val = ALIGN(stream->pixm.width * fmt->raw_bpp / 8, 256);
3002	5872	else
3003	5873	val = stream->pixm.width * rkcif_cal_raw_vir_line_ratio(stream, fmt);
3004	5874	}
3005		- rkcif_write_register(dev, CIF_REG_DVP_VIR_LINE_WIDTH, val);
3006		- rkcif_write_register(dev, CIF_REG_DVP_SET_SIZE,
3007		- stream->pixm.width \| (stream->pixm.height << 16));
3008	5875
3009	5876	if (stream->crop_enable) {
3010	5877	dev->channels[stream->id].crop_en = 1;
..	..	@@ -3020,6 +5887,10 @@
3020	5887	dev->channels[stream->id].crop_en = 0;
3021	5888	}
3022	5889
	5890	+ rkcif_write_register(dev, CIF_REG_DVP_VIR_LINE_WIDTH, val);
	5891	+ rkcif_write_register(dev, CIF_REG_DVP_SET_SIZE,
	5892	+ dev->channels[stream->id].width \|
	5893	+ (dev->channels[stream->id].height << 16));
3023	5894	rkcif_write_register(dev, CIF_REG_DVP_CROP,
3024	5895	dev->channels[stream->id].crop_st_y << CIF_CROP_Y_SHIFT \|
3025	5896	dev->channels[stream->id].crop_st_x);
..	..	@@ -3027,17 +5898,44 @@
3027	5898	if (atomic_read(&dev->pipe.stream_cnt) <= 1)
3028	5899	rkcif_write_register(dev, CIF_REG_DVP_FRAME_STATUS, FRAME_STAT_CLS);
3029	5900
3030		- rkcif_write_register(dev, CIF_REG_DVP_INTSTAT, INTSTAT_CLS);
3031		- rkcif_write_register(dev, CIF_REG_DVP_SCL_CTRL, rkcif_scl_ctl(stream));
	5901	+ if (dev->chip_id < CHIP_RK3588_CIF) {
	5902	+ rkcif_write_register(dev, CIF_REG_DVP_INTSTAT, INTSTAT_CLS);
	5903	+ rkcif_write_register(dev, CIF_REG_DVP_SCL_CTRL, rkcif_scl_ctl(stream));
	5904	+ rkcif_write_register_or(dev, CIF_REG_DVP_INTEN,
	5905	+ DVP_DMA_END_INTSTAT(stream->id) \|
	5906	+ INTSTAT_ERR \| PST_INF_FRAME_END);
	5907	+ /* enable line int for sof */
	5908	+ rkcif_write_register(dev, CIF_REG_DVP_LINE_INT_NUM, 0x1);
	5909	+ rkcif_write_register_or(dev, CIF_REG_DVP_INTEN, LINE_INT_EN);
	5910	+ } else {
	5911	+ if (dev->chip_id == CHIP_RV1106_CIF)
	5912	+ reserved = 0xfc3c0000;
	5913	+ else
	5914	+ reserved = 0;
	5915	+ rkcif_write_register(dev, CIF_REG_DVP_INTSTAT, 0x3c3ffff \| reserved);
	5916	+ rkcif_write_register_or(dev, CIF_REG_DVP_INTEN, 0x033ffff);//0x3c3ffff
	5917	+ }
3032	5918
3033		- if (dev->chip_id < CHIP_RK1808_CIF)
3034		- rkcif_assign_new_buffer_oneframe(stream,
3035		- RKCIF_YUV_ADDR_STATE_INIT);
3036		- else
3037		- rkcif_assign_new_buffer_pingpong(stream,
	5919	+ if (stream->dma_en) {
	5920	+ if (dev->chip_id < CHIP_RK1808_CIF) {
	5921	+ rkcif_assign_new_buffer_oneframe(stream,
	5922	+ RKCIF_YUV_ADDR_STATE_INIT);
	5923	+ } else {
	5924	+ if (mode == RKCIF_STREAM_MODE_CAPTURE)
	5925	+ rkcif_assign_new_buffer_pingpong(stream,
3038	5926	RKCIF_YUV_ADDR_STATE_INIT,
3039	5927	stream->id);
3040		-
	5928	+ else if (mode == RKCIF_STREAM_MODE_TOISP \|\|
	5929	+ mode == RKCIF_STREAM_MODE_TOISP_RDBK)
	5930	+ rkcif_assign_new_buffer_pingpong_toisp(stream,
	5931	+ RKCIF_YUV_ADDR_STATE_INIT,
	5932	+ stream->id);
	5933	+ else if (mode == RKCIF_STREAM_MODE_ROCKIT)
	5934	+ rkcif_assign_new_buffer_pingpong_rockit(stream,
	5935	+ RKCIF_YUV_ADDR_STATE_INIT,
	5936	+ stream->id);
	5937	+ }
	5938	+ }
3041	5939	rkcif_write_register_or(dev, CIF_REG_DVP_INTEN,
3042	5940	DVP_DMA_END_INTSTAT(stream->id) \|
3043	5941	INTSTAT_ERR \| PST_INF_FRAME_END);
..	..	@@ -3053,14 +5951,48 @@
3053	5951	else
3054	5952	workmode = MODE_LINELOOP;
3055	5953
3056		- if (inputmode == INPUT_MODE_BT1120) {
	5954	+ if ((inputmode & INPUT_MODE_BT1120) == INPUT_MODE_BT1120) {
3057	5955	workmode = MODE_PINGPONG;
3058	5956	dev->workmode = RKCIF_WORKMODE_PINGPONG;
3059	5957	}
3060	5958
3061		- rkcif_write_register(dev, CIF_REG_DVP_CTRL,
3062		- AXI_BURST_16 \| workmode \| ENABLE_CAPTURE);
3063		-
	5959	+ if (dev->chip_id == CHIP_RK3588_CIF) {
	5960	+ if (stream->dma_en)
	5961	+ dma_en = DVP_DMA_EN;
	5962	+ if (stream->lack_buf_cnt == 2)
	5963	+ dma_en = 0;
	5964	+ rkcif_write_register(dev, CIF_REG_DVP_CTRL,
	5965	+ DVP_SW_WATER_LINE_25
	5966	+ \| dma_en
	5967	+ \| DVP_PRESS_EN
	5968	+ \| DVP_HURRY_EN
	5969	+ \| DVP_SW_WATER_LINE_25
	5970	+ \| DVP_SW_PRESS_VALUE(3)
	5971	+ \| DVP_SW_HURRY_VALUE(3)
	5972	+ \| ENABLE_CAPTURE);
	5973	+ } else if (dev->chip_id == CHIP_RV1106_CIF) {
	5974	+ if (stream->dma_en)
	5975	+ dma_en = DVP_SW_DMA_EN(stream->id);
	5976	+ if (stream->lack_buf_cnt == 2)
	5977	+ dma_en = 0;
	5978	+ rkcif_write_register(dev, CIF_REG_DVP_CTRL,
	5979	+ DVP_SW_WATER_LINE_25
	5980	+ \| DVP_PRESS_EN
	5981	+ \| DVP_HURRY_EN
	5982	+ \| DVP_SW_WATER_LINE_25
	5983	+ \| DVP_SW_PRESS_VALUE(3)
	5984	+ \| DVP_SW_HURRY_VALUE(3)
	5985	+ \| DVP_SW_CAP_EN(stream->id)
	5986	+ \| dma_en
	5987	+ \| ENABLE_CAPTURE);
	5988	+ } else {
	5989	+ rkcif_write_register(dev, CIF_REG_DVP_CTRL,
	5990	+ AXI_BURST_16 \| workmode \| ENABLE_CAPTURE);
	5991	+ }
	5992	+ dev->intr_mask = rkcif_read_register(dev, CIF_REG_DVP_INTSTAT);
	5993	+#if IS_ENABLED(CONFIG_CPU_RV1106)
	5994	+ rv1106_sdmmc_put_lock();
	5995	+#endif
3064	5996	atomic_set(&sof_sd->frm_sync_seq, 0);
3065	5997	stream->state = RKCIF_STATE_STREAMING;
3066	5998	stream->cifdev->dvp_sof_in_oneframe = 0;
..	..	@@ -3068,34 +6000,199 @@
3068	6000	return 0;
3069	6001	}
3070	6002
3071		-static int rkcif_start_streaming(struct vb2_queue *queue, unsigned int count)
	6003	+static void rkcif_attach_sync_mode(struct rkcif_device *cifdev)
3072	6004	{
3073		- struct rkcif_stream *stream = queue->drv_priv;
	6005	+ struct rkcif_hw *hw = cifdev->hw_dev;
	6006	+ struct rkcif_device *dev;
	6007	+ struct sditf_priv *priv;
	6008	+ int i = 0, j = 0;
	6009	+ int ret = 0;
	6010	+ int count = 0;
	6011	+ int sync_type = NO_SYNC_MODE;
	6012	+ int sync_group = 0;
	6013	+ struct rkcif_sync_cfg sync_cfg;
	6014	+ struct rkcif_multi_sync_config *sync_config;
	6015	+
	6016	+ mutex_lock(&hw->dev_lock);
	6017	+ if (cifdev->sditf_cnt <= 1) {
	6018	+ ret = v4l2_subdev_call(cifdev->terminal_sensor.sd,
	6019	+ core, ioctl,
	6020	+ RKMODULE_GET_SYNC_MODE,
	6021	+ &sync_type);
	6022	+ if (!ret)
	6023	+ sync_cfg.type = sync_type;
	6024	+ else
	6025	+ sync_cfg.type = NO_SYNC_MODE;
	6026	+ ret = v4l2_subdev_call(cifdev->terminal_sensor.sd,
	6027	+ core, ioctl,
	6028	+ RKMODULE_GET_GROUP_ID,
	6029	+ &sync_group);
	6030	+ if (!ret && sync_group < RKCIF_MAX_GROUP)
	6031	+ sync_cfg.group = sync_group;
	6032	+ else
	6033	+ sync_cfg.group = 0;
	6034	+ } else {
	6035	+ for (j = 0; j < cifdev->sditf_cnt; j++) {
	6036	+ ret \|= v4l2_subdev_call(cifdev->sditf[j]->sensor_sd,
	6037	+ core, ioctl,
	6038	+ RKMODULE_GET_SYNC_MODE,
	6039	+ &sync_type);
	6040	+ if (!ret && sync_type)
	6041	+ break;
	6042	+ }
	6043	+ if (!ret)
	6044	+ sync_cfg.type = sync_type;
	6045	+ else
	6046	+ sync_cfg.type = NO_SYNC_MODE;
	6047	+ ret = v4l2_subdev_call(cifdev->sditf[j]->sensor_sd,
	6048	+ core, ioctl,
	6049	+ RKMODULE_GET_GROUP_ID,
	6050	+ &sync_group);
	6051	+ if (!ret && sync_group < RKCIF_MAX_GROUP)
	6052	+ sync_cfg.group = sync_group;
	6053	+ else
	6054	+ sync_cfg.group = 0;
	6055	+ }
	6056	+ cifdev->sync_cfg = sync_cfg;
	6057	+ if (sync_cfg.type == NO_SYNC_MODE \|\|
	6058	+ hw->sync_config[sync_cfg.group].is_attach) {
	6059	+ mutex_unlock(&hw->dev_lock);
	6060	+ return;
	6061	+ }
	6062	+
	6063	+ sync_config = &hw->sync_config[sync_cfg.group];
	6064	+ memset(sync_config, 0, sizeof(struct rkcif_multi_sync_config));
	6065	+ for (i = 0; i < hw->dev_num; i++) {
	6066	+ dev = hw->cif_dev[i];
	6067	+ if (dev->sditf_cnt <= 1) {
	6068	+ ret = v4l2_subdev_call(dev->terminal_sensor.sd,
	6069	+ core, ioctl,
	6070	+ RKMODULE_GET_SYNC_MODE,
	6071	+ &sync_type);
	6072	+ if (!ret)
	6073	+ sync_cfg.type = sync_type;
	6074	+ else
	6075	+ sync_cfg.type = NO_SYNC_MODE;
	6076	+ ret = v4l2_subdev_call(dev->terminal_sensor.sd,
	6077	+ core, ioctl,
	6078	+ RKMODULE_GET_GROUP_ID,
	6079	+ &sync_group);
	6080	+ if (!ret && sync_group < RKCIF_MAX_GROUP)
	6081	+ sync_cfg.group = sync_group;
	6082	+ else
	6083	+ sync_cfg.group = 0;
	6084	+ } else {
	6085	+ priv = dev->sditf[0];
	6086	+ if (priv && priv->is_combine_mode && dev->sditf_cnt <= RKCIF_MAX_SDITF) {
	6087	+ for (j = 0; j < dev->sditf_cnt; j++) {
	6088	+ ret \|= v4l2_subdev_call(dev->sditf[j]->sensor_sd,
	6089	+ core, ioctl,
	6090	+ RKMODULE_GET_SYNC_MODE,
	6091	+ &sync_type);
	6092	+ if (!ret && sync_type) {
	6093	+ priv = dev->sditf[j];
	6094	+ break;
	6095	+ }
	6096	+ }
	6097	+ if (!ret)
	6098	+ sync_cfg.type = sync_type;
	6099	+ else
	6100	+ sync_cfg.type = NO_SYNC_MODE;
	6101	+ ret = v4l2_subdev_call(priv->sensor_sd,
	6102	+ core, ioctl,
	6103	+ RKMODULE_GET_GROUP_ID,
	6104	+ &sync_group);
	6105	+ if (!ret && sync_group < RKCIF_MAX_GROUP)
	6106	+ sync_cfg.group = sync_group;
	6107	+ else
	6108	+ sync_cfg.group = 0;
	6109	+ }
	6110	+ }
	6111	+ if (sync_cfg.group == cifdev->sync_cfg.group) {
	6112	+ if (sync_cfg.type == EXTERNAL_MASTER_MODE) {
	6113	+ count = sync_config->ext_master.count;
	6114	+ sync_config->ext_master.cif_dev[count] = dev;
	6115	+ sync_config->ext_master.count++;
	6116	+ sync_config->dev_cnt++;
	6117	+ sync_config->sync_mask \|= BIT(dev->csi_host_idx);
	6118	+ } else if (sync_cfg.type == INTERNAL_MASTER_MODE) {
	6119	+ count = sync_config->int_master.count;
	6120	+ sync_config->int_master.cif_dev[count] = dev;
	6121	+ sync_config->int_master.count++;
	6122	+ sync_config->dev_cnt++;
	6123	+ sync_config->sync_mask \|= BIT(dev->csi_host_idx);
	6124	+ } else if (sync_cfg.type == SLAVE_MODE) {
	6125	+ count = sync_config->slave.count;
	6126	+ sync_config->slave.cif_dev[count] = dev;
	6127	+ sync_config->slave.count++;
	6128	+ sync_config->dev_cnt++;
	6129	+ sync_config->sync_mask \|= BIT(dev->csi_host_idx);
	6130	+ }
	6131	+ dev->sync_cfg = sync_cfg;
	6132	+ } else {
	6133	+ ret = v4l2_subdev_call(dev->terminal_sensor.sd,
	6134	+ core, ioctl,
	6135	+ RKMODULE_GET_SYNC_MODE,
	6136	+ &sync_type);
	6137	+ }
	6138	+ }
	6139	+ if (sync_config->int_master.count == 1) {
	6140	+ if (sync_config->ext_master.count) {
	6141	+ sync_config->mode = RKCIF_MASTER_MASTER;
	6142	+ sync_config->is_attach = true;
	6143	+ } else if (sync_config->slave.count) {
	6144	+ sync_config->mode = RKCIF_MASTER_SLAVE;
	6145	+ sync_config->is_attach = true;
	6146	+ } else {
	6147	+ dev_info(hw->dev,
	6148	+ "Missing slave device, do not use sync mode\n");
	6149	+ }
	6150	+ if (sync_config->is_attach)
	6151	+ dev_info(hw->dev,
	6152	+ "group %d, int_master %d, ext_master %d, slave %d\n",
	6153	+ i,
	6154	+ sync_config->int_master.count,
	6155	+ sync_config->ext_master.count,
	6156	+ sync_config->slave.count);
	6157	+ }
	6158	+ mutex_unlock(&hw->dev_lock);
	6159	+}
	6160	+
	6161	+static void rkcif_monitor_reset_event(struct rkcif_device *dev);
	6162	+
	6163	+int rkcif_do_start_stream(struct rkcif_stream *stream, unsigned int mode)
	6164	+{
3074	6165	struct rkcif_vdev_node *node = &stream->vnode;
3075	6166	struct rkcif_device *dev = stream->cifdev;
	6167	+ struct rkcif_hw *hw_dev = dev->hw_dev;
3076	6168	struct v4l2_device *v4l2_dev = &dev->v4l2_dev;
3077	6169	struct rkcif_sensor_info *sensor_info = dev->active_sensor;
3078		- struct rkcif_sensor_info *terminal_sensor = &dev->terminal_sensor;
	6170	+ struct rkcif_sensor_info *terminal_sensor = NULL;
3079	6171	struct rkmodule_hdr_cfg hdr_cfg;
	6172	+ struct rkcif_csi_info csi_info = {0};
3080	6173	int rkmodule_stream_seq = RKMODULE_START_STREAM_DEFAULT;
3081	6174	int ret;
	6175	+ int i = 0;
	6176	+ u32 skip_frame = 0;
3082	6177
3083	6178	v4l2_info(&dev->v4l2_dev, "stream[%d] start streaming\n", stream->id);
3084	6179
3085		- mutex_lock(&dev->hw_dev->dev_lock);
	6180	+ rkcif_attach_sync_mode(dev);
	6181	+ mutex_lock(&dev->stream_lock);
3086	6182
3087		- if (WARN_ON(stream->state != RKCIF_STATE_READY)) {
	6183	+ if ((stream->cur_stream_mode & RKCIF_STREAM_MODE_CAPTURE) == mode) {
3088	6184	ret = -EBUSY;
3089	6185	v4l2_err(v4l2_dev, "stream in busy state\n");
3090	6186	goto destroy_buf;
3091	6187	}
	6188	+ if (stream->dma_en == 0)
	6189	+ stream->fs_cnt_in_single_frame = 0;
3092	6190	if (stream->is_line_wake_up)
3093	6191	stream->is_line_inten = true;
3094	6192	else
3095	6193	stream->is_line_inten = false;
3096		- stream->fs_cnt_in_single_frame = 0;
3097	6194
3098		- if (dev->active_sensor) {
	6195	+ if (!dev->active_sensor) {
3099	6196	ret = rkcif_update_sensor_info(stream);
3100	6197	if (ret < 0) {
3101	6198	v4l2_err(v4l2_dev,
..	..	@@ -3104,16 +6201,16 @@
3104	6201	goto out;
3105	6202	}
3106	6203	}
3107		-
	6204	+ terminal_sensor = &dev->terminal_sensor;
3108	6205	if (terminal_sensor->sd) {
3109	6206	ret = v4l2_subdev_call(terminal_sensor->sd,
3110	6207	core, ioctl,
3111	6208	RKMODULE_GET_HDR_CFG,
3112	6209	&hdr_cfg);
3113	6210	if (!ret)
3114		- dev->hdr.mode = hdr_cfg.hdr_mode;
	6211	+ dev->hdr = hdr_cfg;
3115	6212	else
3116		- dev->hdr.mode = NO_HDR;
	6213	+ dev->hdr.hdr_mode = NO_HDR;
3117	6214
3118	6215	ret = v4l2_subdev_call(terminal_sensor->sd,
3119	6216	video, g_frame_interval, &terminal_sensor->fi);
..	..	@@ -3134,102 +6231,158 @@
3134	6231	if (ret < 0)
3135	6232	goto destroy_buf;
3136	6233
	6234	+ mutex_lock(&hw_dev->dev_lock);
	6235	+ if (atomic_read(&dev->pipe.stream_cnt) == 0 &&
	6236	+ dev->active_sensor &&
	6237	+ (dev->active_sensor->mbus.type == V4L2_MBUS_CSI2_DPHY \|\|
	6238	+ dev->active_sensor->mbus.type == V4L2_MBUS_CSI2_CPHY \|\|
	6239	+ dev->active_sensor->mbus.type == V4L2_MBUS_CCP2)) {
	6240	+ if (dev->channels[0].capture_info.mode == RKMODULE_MULTI_DEV_COMBINE_ONE) {
	6241	+ csi_info.csi_num = dev->channels[0].capture_info.multi_dev.dev_num;
	6242	+ if (csi_info.csi_num > RKCIF_MAX_CSI_NUM) {
	6243	+ v4l2_err(v4l2_dev,
	6244	+ "csi num %d, max %d\n",
	6245	+ csi_info.csi_num, RKCIF_MAX_CSI_NUM);
	6246	+ goto out;
	6247	+ }
	6248	+ for (i = 0; i < csi_info.csi_num; i++) {
	6249	+ csi_info.csi_idx[i] = dev->channels[0].capture_info.multi_dev.dev_idx[i];
	6250	+ if (dev->hw_dev->is_rk3588s2)
	6251	+ v4l2_info(v4l2_dev, "rk3588s2 combine mode attach to mipi%d\n",
	6252	+ csi_info.csi_idx[i]);
	6253	+ }
	6254	+ } else {
	6255	+ csi_info.csi_num = 1;
	6256	+ dev->csi_host_idx = dev->csi_host_idx_def;
	6257	+ csi_info.csi_idx[0] = dev->csi_host_idx;
	6258	+ }
	6259	+ ret = v4l2_subdev_call(dev->active_sensor->sd,
	6260	+ core, ioctl,
	6261	+ RKCIF_CMD_SET_CSI_IDX,
	6262	+ &csi_info);
	6263	+ if (ret)
	6264	+ v4l2_err(&dev->v4l2_dev, "set csi idx %d fail\n", dev->csi_host_idx);
	6265	+
	6266	+ }
	6267	+
3137	6268	if (((dev->active_sensor && dev->active_sensor->mbus.type == V4L2_MBUS_BT656) \|\|
3138	6269	dev->is_use_dummybuf) &&
3139		- (!dev->dummy_buf.vaddr)) {
	6270	+ (!dev->hw_dev->dummy_buf.vaddr) &&
	6271	+ mode == RKCIF_STREAM_MODE_CAPTURE) {
3140	6272	ret = rkcif_create_dummy_buf(stream);
3141	6273	if (ret < 0) {
	6274	+ mutex_unlock(&hw_dev->dev_lock);
3142	6275	v4l2_err(v4l2_dev, "Failed to create dummy_buf, %d\n", ret);
3143	6276	goto destroy_buf;
3144	6277	}
3145	6278	}
	6279	+ mutex_unlock(&hw_dev->dev_lock);
3146	6280
3147		- ret = dev->pipe.open(&dev->pipe, &node->vdev.entity, true);
3148		- if (ret < 0) {
3149		- v4l2_err(v4l2_dev, "open cif pipeline failed %d\n",
3150		- ret);
3151		- goto destroy_buf;
3152		- }
	6281	+ if (mode == RKCIF_STREAM_MODE_CAPTURE)
	6282	+ tasklet_enable(&stream->vb_done_tasklet);
3153	6283
3154		- /*
3155		- * start sub-devices
3156		- * When use bt601, the sampling edge of cif is random,
3157		- * can be rising or fallling after powering on cif.
3158		- * To keep the coherence of edge, open sensor in advance.
3159		- */
3160		- if (sensor_info->mbus.type == V4L2_MBUS_PARALLEL \|\|
3161		- rkmodule_stream_seq == RKMODULE_START_STREAM_FRONT) {
3162		- ret = dev->pipe.set_stream(&dev->pipe, true);
3163		- if (ret < 0)
3164		- goto runtime_put;
3165		- }
	6284	+ if (stream->cur_stream_mode == RKCIF_STREAM_MODE_NONE) {
	6285	+ ret = dev->pipe.open(&dev->pipe, &node->vdev.entity, true);
	6286	+ if (ret < 0) {
	6287	+ v4l2_err(v4l2_dev, "open cif pipeline failed %d\n",
	6288	+ ret);
	6289	+ goto destroy_buf;
	6290	+ }
3166	6291
3167		- if (dev->chip_id >= CHIP_RK1808_CIF) {
3168		- if (dev->active_sensor->mbus.type == V4L2_MBUS_CSI2 \|\|
3169		- dev->active_sensor->mbus.type == V4L2_MBUS_CCP2)
3170		- ret = rkcif_csi_stream_start(stream);
	6292	+ /*
	6293	+ * start sub-devices
	6294	+ * When use bt601, the sampling edge of cif is random,
	6295	+ * can be rising or fallling after powering on cif.
	6296	+ * To keep the coherence of edge, open sensor in advance.
	6297	+ */
	6298	+ if (sensor_info->mbus.type == V4L2_MBUS_PARALLEL \|\|
	6299	+ rkmodule_stream_seq == RKMODULE_START_STREAM_FRONT) {
	6300	+ ret = dev->pipe.set_stream(&dev->pipe, true);
	6301	+ if (ret < 0)
	6302	+ goto destroy_buf;
	6303	+ }
	6304	+ ret = v4l2_subdev_call(terminal_sensor->sd,
	6305	+ core, ioctl,
	6306	+ RKMODULE_GET_SKIP_FRAME,
	6307	+ &skip_frame);
	6308	+ if (!ret && skip_frame < RKCIF_SKIP_FRAME_MAX)
	6309	+ stream->skip_frame = skip_frame;
3171	6310	else
3172		- ret = rkcif_stream_start(stream);
	6311	+ stream->skip_frame = 0;
	6312	+ stream->cur_skip_frame = stream->skip_frame;
	6313	+ }
	6314	+ if (dev->chip_id >= CHIP_RK1808_CIF) {
	6315	+ if (dev->active_sensor &&
	6316	+ (dev->active_sensor->mbus.type == V4L2_MBUS_CSI2_DPHY \|\|
	6317	+ dev->active_sensor->mbus.type == V4L2_MBUS_CSI2_CPHY \|\|
	6318	+ dev->active_sensor->mbus.type == V4L2_MBUS_CCP2))
	6319	+ ret = rkcif_csi_stream_start(stream, mode);
	6320	+ else
	6321	+ ret = rkcif_stream_start(stream, mode);
3173	6322	} else {
3174		- ret = rkcif_stream_start(stream);
	6323	+ ret = rkcif_stream_start(stream, mode);
3175	6324	}
3176	6325
3177	6326	if (ret < 0)
3178		- goto runtime_put;
	6327	+ goto destroy_buf;
3179	6328
3180		- ret = media_pipeline_start(&node->vdev.entity, &dev->pipe.pipe);
3181		- if (ret < 0) {
3182		- v4l2_err(&dev->v4l2_dev, "start pipeline failed %d\n",
3183		- ret);
3184		- goto pipe_stream_off;
	6329	+ if (stream->cur_stream_mode == RKCIF_STREAM_MODE_NONE) {
	6330	+ ret = media_pipeline_start(&node->vdev.entity, &dev->pipe.pipe);
	6331	+ if (ret < 0) {
	6332	+ v4l2_err(&dev->v4l2_dev, "start pipeline failed %d\n",
	6333	+ ret);
	6334	+ goto pipe_stream_off;
	6335	+ }
	6336	+
	6337	+ if (sensor_info->mbus.type != V4L2_MBUS_PARALLEL &&
	6338	+ rkmodule_stream_seq != RKMODULE_START_STREAM_FRONT) {
	6339	+ ret = dev->pipe.set_stream(&dev->pipe, true);
	6340	+ if (ret < 0)
	6341	+ goto stop_stream;
	6342	+ }
3185	6343	}
3186		-
3187		- if (sensor_info->mbus.type != V4L2_MBUS_PARALLEL &&
3188		- rkmodule_stream_seq != RKMODULE_START_STREAM_FRONT) {
3189		- ret = dev->pipe.set_stream(&dev->pipe, true);
3190		- if (ret < 0)
3191		- goto stop_stream;
3192		- }
3193		-
3194		- if (dev->hdr.mode == NO_HDR) {
3195		- if (dev->stream[RKCIF_STREAM_MIPI_ID0].state == RKCIF_STATE_STREAMING) {
3196		- rkcif_start_luma(&dev->luma_vdev,
	6344	+ if (dev->chip_id == CHIP_RV1126_CIF \|\|
	6345	+ dev->chip_id == CHIP_RV1126_CIF_LITE \|\|
	6346	+ dev->chip_id == CHIP_RK3568_CIF) {
	6347	+ if (dev->hdr.hdr_mode == NO_HDR) {
	6348	+ if (dev->stream[RKCIF_STREAM_MIPI_ID0].state == RKCIF_STATE_STREAMING)
	6349	+ rkcif_start_luma(&dev->luma_vdev,
	6350	+ dev->stream[RKCIF_STREAM_MIPI_ID0].cif_fmt_in);
	6351	+ } else if (dev->hdr.hdr_mode == HDR_X2) {
	6352	+ if (dev->stream[RKCIF_STREAM_MIPI_ID0].state == RKCIF_STATE_STREAMING &&
	6353	+ dev->stream[RKCIF_STREAM_MIPI_ID1].state == RKCIF_STATE_STREAMING)
	6354	+ rkcif_start_luma(&dev->luma_vdev,
	6355	+ dev->stream[RKCIF_STREAM_MIPI_ID0].cif_fmt_in);
	6356	+ } else if (dev->hdr.hdr_mode == HDR_X3) {
	6357	+ if (dev->stream[RKCIF_STREAM_MIPI_ID0].state == RKCIF_STATE_STREAMING &&
	6358	+ dev->stream[RKCIF_STREAM_MIPI_ID1].state == RKCIF_STATE_STREAMING &&
	6359	+ dev->stream[RKCIF_STREAM_MIPI_ID2].state == RKCIF_STATE_STREAMING)
	6360	+ rkcif_start_luma(&dev->luma_vdev,
3197	6361	dev->stream[RKCIF_STREAM_MIPI_ID0].cif_fmt_in);
3198	6362	}
3199		- } else if (dev->hdr.mode == HDR_X2) {
3200		- if (dev->stream[RKCIF_STREAM_MIPI_ID0].state == RKCIF_STATE_STREAMING &&
3201		- dev->stream[RKCIF_STREAM_MIPI_ID1].state == RKCIF_STATE_STREAMING) {
3202		- rkcif_start_luma(&dev->luma_vdev,
3203		- dev->stream[RKCIF_STREAM_MIPI_ID0].cif_fmt_in);
3204		- }
3205		- } else if (dev->hdr.mode == HDR_X3) {
3206		- if (dev->stream[RKCIF_STREAM_MIPI_ID0].state == RKCIF_STATE_STREAMING &&
3207		- dev->stream[RKCIF_STREAM_MIPI_ID1].state == RKCIF_STATE_STREAMING &&
3208		- dev->stream[RKCIF_STREAM_MIPI_ID2].state == RKCIF_STATE_STREAMING) {
3209		- rkcif_start_luma(&dev->luma_vdev,
3210		- dev->stream[RKCIF_STREAM_MIPI_ID0].cif_fmt_in);
3211		- }
3212	6363	}
3213		-
3214		- if (dev->hw_dev->reset_work_cancel)
3215		- dev->hw_dev->reset_work_cancel = false;
3216		- if (dev->hdr.mode == NO_HDR)
3217		- stream->streamon_timestamp = ktime_get_ns();
	6364	+ dev->reset_work_cancel = false;
	6365	+ stream->cur_stream_mode \|= mode;
	6366	+ rkcif_monitor_reset_event(dev);
3218	6367	goto out;
3219	6368
3220	6369	stop_stream:
3221	6370	rkcif_stream_stop(stream);
3222	6371	pipe_stream_off:
3223	6372	dev->pipe.set_stream(&dev->pipe, false);
3224		-runtime_put:
3225		- pm_runtime_put_sync(dev->dev);
	6373	+
3226	6374	destroy_buf:
3227		- if (stream->next_buf)
3228		- vb2_buffer_done(&stream->next_buf->vb.vb2_buf,
3229		- VB2_BUF_STATE_QUEUED);
	6375	+ if (mode == RKCIF_STREAM_MODE_CAPTURE)
	6376	+ tasklet_disable(&stream->vb_done_tasklet);
3230	6377	if (stream->curr_buf)
3231		- vb2_buffer_done(&stream->curr_buf->vb.vb2_buf,
3232		- VB2_BUF_STATE_QUEUED);
	6378	+ list_add_tail(&stream->curr_buf->queue, &stream->buf_head);
	6379	+ if (stream->next_buf &&
	6380	+ stream->next_buf != stream->curr_buf)
	6381	+ list_add_tail(&stream->next_buf->queue, &stream->buf_head);
	6382	+
	6383	+ stream->curr_buf = NULL;
	6384	+ stream->next_buf = NULL;
	6385	+ atomic_set(&stream->buf_cnt, 0);
3233	6386	while (!list_empty(&stream->buf_head)) {
3234	6387	struct rkcif_buffer *buf;
3235	6388
..	..	@@ -3240,7 +6393,16 @@
3240	6393	}
3241	6394
3242	6395	out:
3243		- mutex_unlock(&dev->hw_dev->dev_lock);
	6396	+ mutex_unlock(&dev->stream_lock);
	6397	+ return ret;
	6398	+}
	6399	+
	6400	+static int rkcif_start_streaming(struct vb2_queue *queue, unsigned int count)
	6401	+{
	6402	+ struct rkcif_stream *stream = queue->drv_priv;
	6403	+ int ret = 0;
	6404	+
	6405	+ ret = rkcif_do_start_stream(stream, RKCIF_STREAM_MODE_CAPTURE);
3244	6406	return ret;
3245	6407	}
3246	6408
..	..	@@ -3263,10 +6425,7 @@
3263	6425	q->io_modes = VB2_MMAP \| VB2_DMABUF;
3264	6426	q->drv_priv = stream;
3265	6427	q->ops = &rkcif_vb2_ops;
3266		- if (hw_dev->iommu_en)
3267		- q->mem_ops = &vb2_dma_sg_memops;
3268		- else
3269		- q->mem_ops = &vb2_dma_contig_memops;
	6428	+ q->mem_ops = hw_dev->mem_ops;
3270	6429	q->buf_struct_size = sizeof(struct rkcif_buffer);
3271	6430	if (stream->cifdev->is_use_dummybuf)
3272	6431	q->min_buffers_needed = 1;
..	..	@@ -3278,14 +6437,17 @@
3278	6437	q->allow_cache_hints = 1;
3279	6438	q->bidirectional = 1;
3280	6439	q->gfp_flags = GFP_DMA32;
	6440	+ if (hw_dev->is_dma_contig)
	6441	+ q->dma_attrs = DMA_ATTR_FORCE_CONTIGUOUS;
3281	6442	return vb2_queue_init(q);
3282	6443	}
3283	6444
3284		-static void rkcif_set_fmt(struct rkcif_stream *stream,
3285		- struct v4l2_pix_format_mplane *pixm,
3286		- bool try)
	6445	+int rkcif_set_fmt(struct rkcif_stream *stream,
	6446	+ struct v4l2_pix_format_mplane *pixm,
	6447	+ bool try)
3287	6448	{
3288	6449	struct rkcif_device *dev = stream->cifdev;
	6450	+ struct sditf_priv *priv = dev->sditf[0];
3289	6451	const struct cif_output_fmt *fmt;
3290	6452	const struct cif_input_fmt *cif_fmt_in = NULL;
3291	6453	struct v4l2_rect input_rect;
..	..	@@ -3293,9 +6455,13 @@
3293	6455	u32 xsubs = 1, ysubs = 1, i;
3294	6456	struct rkmodule_hdr_cfg hdr_cfg;
3295	6457	struct rkcif_extend_info *extend_line = &stream->extend_line;
3296		- int ret, vc;
	6458	+ struct csi_channel_info *channel_info = &dev->channels[stream->id];
	6459	+ int ret;
3297	6460
3298		- fmt = find_output_fmt(stream, pixm->pixelformat);
	6461	+ for (i = 0; i < RKCIF_MAX_PLANE; i++)
	6462	+ memset(&pixm->plane_fmt[i], 0, sizeof(struct v4l2_plane_pix_format));
	6463	+
	6464	+ fmt = rkcif_find_output_fmt(stream, pixm->pixelformat);
3299	6465	if (!fmt)
3300	6466	fmt = &out_fmts[0];
3301	6467
..	..	@@ -3303,10 +6469,19 @@
3303	6469	input_rect.height = RKCIF_DEFAULT_HEIGHT;
3304	6470
3305	6471	if (dev->terminal_sensor.sd) {
3306		- cif_fmt_in = get_input_fmt(dev->terminal_sensor.sd,
3307		- &input_rect, stream->id, &vc);
	6472	+ cif_fmt_in = rkcif_get_input_fmt(dev,
	6473	+ &input_rect, stream->id,
	6474	+ channel_info);
3308	6475	stream->cif_fmt_in = cif_fmt_in;
	6476	+ } else {
	6477	+ v4l2_err(&stream->cifdev->v4l2_dev,
	6478	+ "terminal subdev does not exist\n");
	6479	+ return -EINVAL;
3309	6480	}
	6481	+
	6482	+ ret = rkcif_output_fmt_check(stream, fmt);
	6483	+ if (ret)
	6484	+ return -EINVAL;
3310	6485
3311	6486	if (dev->terminal_sensor.sd) {
3312	6487	ret = v4l2_subdev_call(dev->terminal_sensor.sd,
..	..	@@ -3314,9 +6489,9 @@
3314	6489	RKMODULE_GET_HDR_CFG,
3315	6490	&hdr_cfg);
3316	6491	if (!ret)
3317		- dev->hdr.mode = hdr_cfg.hdr_mode;
	6492	+ dev->hdr = hdr_cfg;
3318	6493	else
3319		- dev->hdr.mode = NO_HDR;
	6494	+ dev->hdr.hdr_mode = NO_HDR;
3320	6495
3321	6496	dev->terminal_sensor.raw_rect = input_rect;
3322	6497	}
..	..	@@ -3337,6 +6512,11 @@
3337	6512	fcc_xysubs(fmt->fourcc, &xsubs, &ysubs);
3338	6513
3339	6514	planes = fmt->cplanes ? fmt->cplanes : fmt->mplanes;
	6515	+
	6516	+ if (cif_fmt_in &&
	6517	+ (cif_fmt_in->mbus_code == MEDIA_BUS_FMT_SPD_2X8 \|\|
	6518	+ cif_fmt_in->mbus_code == MEDIA_BUS_FMT_EBD_1X8))
	6519	+ stream->crop_enable = false;
3340	6520
3341	6521	for (i = 0; i < planes; i++) {
3342	6522	struct v4l2_plane_pix_format *plane_fmt;
..	..	@@ -3360,6 +6540,9 @@
3360	6540	}
3361	6541	}
3362	6542
	6543	+ if (priv && priv->is_combine_mode && dev->sditf_cnt <= RKCIF_MAX_SDITF)
	6544	+ height *= dev->sditf_cnt;
	6545	+
3363	6546	extend_line->pixm.height = height + RKMODULE_EXTEND_LINE;
3364	6547
3365	6548	/* compact mode need bytesperline 4bytes align,
..	..	@@ -3367,25 +6550,29 @@
3367	6550	* to optimize reading and writing of ddr, aliged with 256.
3368	6551	*/
3369	6552	if (fmt->fmt_type == CIF_FMT_TYPE_RAW &&
3370		- (stream->cif_fmt_in->mbus_code == MEDIA_BUS_FMT_EBD_1X8 \|\|
3371		- stream->cif_fmt_in->mbus_code == MEDIA_BUS_FMT_SPD_2X8)) {
	6553	+ cif_fmt_in &&
	6554	+ (cif_fmt_in->mbus_code == MEDIA_BUS_FMT_EBD_1X8 \|\|
	6555	+ cif_fmt_in->mbus_code == MEDIA_BUS_FMT_SPD_2X8)) {
3372	6556	stream->is_compact = false;
3373	6557	}
3374	6558
3375	6559	if (fmt->fmt_type == CIF_FMT_TYPE_RAW && stream->is_compact &&
3376		- (dev->active_sensor->mbus.type == V4L2_MBUS_CSI2 \|\|
	6560	+ (dev->active_sensor->mbus.type == V4L2_MBUS_CSI2_DPHY \|\|
	6561	+ dev->active_sensor->mbus.type == V4L2_MBUS_CSI2_CPHY \|\|
3377	6562	dev->active_sensor->mbus.type == V4L2_MBUS_CCP2) &&
3378		- fmt->csi_fmt_val != CSI_WRDDR_TYPE_RGB888) {
3379		- bpl = ALIGN(width * fmt->raw_bpp / 8, 256);
3380		- } else if (fmt->fourcc == V4L2_PIX_FMT_GREY \|\|
3381		- fmt->fourcc == V4L2_PIX_FMT_SRGGB8 \|\|
3382		- fmt->fourcc == V4L2_PIX_FMT_SGRBG8 \|\|
3383		- fmt->fourcc == V4L2_PIX_FMT_SGBRG8 \|\|
3384		- fmt->fourcc == V4L2_PIX_FMT_SBGGR8) {
	6563	+ fmt->csi_fmt_val != CSI_WRDDR_TYPE_RGB888 &&
	6564	+ fmt->csi_fmt_val != CSI_WRDDR_TYPE_RGB565) {
3385	6565	bpl = ALIGN(width * fmt->raw_bpp / 8, 256);
3386	6566	} else {
3387		- bpp = rkcif_align_bits_per_pixel(stream, fmt, i);
3388		- bpl = width * bpp / CIF_YUV_STORED_BIT_WIDTH;
	6567	+ if (fmt->fmt_type == CIF_FMT_TYPE_RAW && stream->is_compact &&
	6568	+ fmt->csi_fmt_val != CSI_WRDDR_TYPE_RGB888 &&
	6569	+ fmt->csi_fmt_val != CSI_WRDDR_TYPE_RGB565 &&
	6570	+ dev->chip_id >= CHIP_RK3588_CIF) {
	6571	+ bpl = ALIGN(width * fmt->raw_bpp / 8, 256);
	6572	+ } else {
	6573	+ bpp = rkcif_align_bits_per_pixel(stream, fmt, i);
	6574	+ bpl = width * bpp / CIF_YUV_STORED_BIT_WIDTH;
	6575	+ }
3389	6576	}
3390	6577	size = bpl * height;
3391	6578	imagesize += size;
..	..	@@ -3425,6 +6612,7 @@
3425	6612	pixm->width, pixm->height,
3426	6613	stream->pixm.width, stream->pixm.height);
3427	6614	}
	6615	+ return 0;
3428	6616	}
3429	6617
3430	6618	void rkcif_stream_init(struct rkcif_device *dev, u32 id)
..	..	@@ -3439,6 +6627,9 @@
3439	6627	stream->cifdev = dev;
3440	6628
3441	6629	INIT_LIST_HEAD(&stream->buf_head);
	6630	+ INIT_LIST_HEAD(&stream->rx_buf_head);
	6631	+ INIT_LIST_HEAD(&stream->rx_buf_head_vicap);
	6632	+ INIT_LIST_HEAD(&stream->rockit_buf_head);
3442	6633	spin_lock_init(&stream->vbq_lock);
3443	6634	spin_lock_init(&stream->fps_lock);
3444	6635	stream->state = RKCIF_STATE_READY;
..	..	@@ -3474,6 +6665,8 @@
3474	6665	}
3475	6666
3476	6667	stream->is_high_align = false;
	6668	+ stream->is_finish_stop_dma = false;
	6669	+ stream->is_wait_dma_stop = false;
3477	6670
3478	6671	if (dev->chip_id == CHIP_RV1126_CIF \|\|
3479	6672	dev->chip_id == CHIP_RV1126_CIF_LITE)
..	..	@@ -3493,7 +6686,19 @@
3493	6686	dev->wait_line_cache = 0;
3494	6687	dev->wait_line_bak = 0;
3495	6688	}
3496		-
	6689	+ stream->cur_stream_mode = 0;
	6690	+ stream->dma_en = 0;
	6691	+ stream->to_en_dma = 0;
	6692	+ stream->to_stop_dma = 0;
	6693	+ stream->to_en_scale = false;
	6694	+ stream->buf_owner = 0;
	6695	+ stream->buf_replace_cnt = 0;
	6696	+ stream->is_stop_capture = false;
	6697	+ stream->is_single_cap = false;
	6698	+ atomic_set(&stream->buf_cnt, 0);
	6699	+ stream->rx_buf_num = 0;
	6700	+ init_completion(&stream->stop_complete);
	6701	+ stream->is_wait_stop_complete = false;
3497	6702	}
3498	6703
3499	6704	static int rkcif_fh_open(struct file *filp)
..	..	@@ -3503,6 +6708,7 @@
3503	6708	struct rkcif_stream *stream = to_rkcif_stream(vnode);
3504	6709	struct rkcif_device *cifdev = stream->cifdev;
3505	6710	int ret;
	6711	+ int i = 0;
3506	6712
3507	6713	ret = rkcif_attach_hw(cifdev);
3508	6714	if (ret)
..	..	@@ -3518,38 +6724,35 @@
3518	6724	return ret;
3519	6725	}
3520	6726
3521		- /* enable clocks/power-domains */
3522		- ret = pm_runtime_get_sync(cifdev->dev);
	6727	+ ret = pm_runtime_resume_and_get(cifdev->dev);
3523	6728	if (ret < 0) {
3524	6729	v4l2_err(vdev, "Failed to get runtime pm, %d\n",
3525	6730	ret);
3526	6731	return ret;
3527	6732	}
3528		- /*
3529		- * Soft reset via CRU.
3530		- * Because CRU would reset iommu too, so there's not chance
3531		- * to reset cif once we hold buffers after buf queued
3532		- */
3533		- if (cifdev->chip_id == CHIP_RK1808_CIF \|\|
3534		- cifdev->chip_id == CHIP_RV1126_CIF \|\|
3535		- cifdev->chip_id == CHIP_RV1126_CIF_LITE \|\|
3536		- cifdev->chip_id == CHIP_RK3568_CIF) {
3537		- mutex_lock(&cifdev->stream_lock);
3538		- if (!atomic_read(&cifdev->fh_cnt))
3539		- rkcif_soft_reset(cifdev, true);
3540		- atomic_inc(&cifdev->fh_cnt);
3541		- mutex_unlock(&cifdev->stream_lock);
3542		- } else {
3543		- rkcif_soft_reset(cifdev, true);
3544		- }
3545	6733
3546	6734	ret = v4l2_fh_open(filp);
3547	6735	if (!ret) {
3548		- ret = v4l2_pipeline_pm_use(&vnode->vdev.entity, 1);
	6736	+ mutex_lock(&cifdev->stream_lock);
	6737	+ ret = v4l2_pipeline_pm_get(&vnode->vdev.entity);
	6738	+ v4l2_info(vdev, "open video, entity use_countt %d\n",
	6739	+ vnode->vdev.entity.use_count);
	6740	+ mutex_unlock(&cifdev->stream_lock);
3549	6741	if (ret < 0)
3550	6742	vb2_fop_release(filp);
3551	6743	}
3552		-
	6744	+ if (cifdev->sditf_cnt > 1) {
	6745	+ for (i = 0; i < cifdev->sditf_cnt; i++) {
	6746	+ if (cifdev->sditf[i]->sensor_sd)
	6747	+ ret \|= v4l2_subdev_call(cifdev->sditf[i]->sensor_sd,
	6748	+ core,
	6749	+ s_power,
	6750	+ 1);
	6751	+ }
	6752	+ if (ret < 0)
	6753	+ v4l2_err(vdev, "set sensor power on fail, ret %d\n",
	6754	+ ret);
	6755	+ }
3553	6756	return ret;
3554	6757	}
3555	6758
..	..	@@ -3560,23 +6763,30 @@
3560	6763	struct rkcif_stream *stream = to_rkcif_stream(vnode);
3561	6764	struct rkcif_device *cifdev = stream->cifdev;
3562	6765	int ret = 0;
	6766	+ int i = 0;
3563	6767
3564	6768	ret = vb2_fop_release(filp);
3565	6769	if (!ret) {
3566		- ret = v4l2_pipeline_pm_use(&vnode->vdev.entity, 0);
3567		- if (ret < 0)
3568		- v4l2_err(&cifdev->v4l2_dev,
3569		- "set pipeline power failed %d\n", ret);
	6770	+ mutex_lock(&cifdev->stream_lock);
	6771	+ v4l2_pipeline_pm_put(&vnode->vdev.entity);
	6772	+ v4l2_info(vdev, "close video, entity use_count %d\n",
	6773	+ vnode->vdev.entity.use_count);
	6774	+ mutex_unlock(&cifdev->stream_lock);
3570	6775	}
3571	6776
3572		- mutex_lock(&cifdev->stream_lock);
3573		- if (!atomic_dec_return(&cifdev->fh_cnt))
3574		- rkcif_soft_reset(cifdev, true);
3575		- else if (atomic_read(&cifdev->fh_cnt) < 0)
3576		- atomic_set(&cifdev->fh_cnt, 0);
3577		- mutex_unlock(&cifdev->stream_lock);
3578	6777	pm_runtime_put_sync(cifdev->dev);
3579		-
	6778	+ if (cifdev->sditf_cnt > 1) {
	6779	+ for (i = 0; i < cifdev->sditf_cnt; i++) {
	6780	+ if (cifdev->sditf[i]->sensor_sd)
	6781	+ ret \|= v4l2_subdev_call(cifdev->sditf[i]->sensor_sd,
	6782	+ core,
	6783	+ s_power,
	6784	+ 0);
	6785	+ }
	6786	+ if (ret < 0)
	6787	+ v4l2_err(vdev, "set sensor power on fail, ret %d\n",
	6788	+ ret);
	6789	+ }
3580	6790	return ret;
3581	6791	}
3582	6792
..	..	@@ -3607,41 +6817,50 @@
3607	6817	struct v4l2_format *f)
3608	6818	{
3609	6819	struct rkcif_stream *stream = video_drvdata(file);
	6820	+ int ret = 0;
3610	6821
3611		- rkcif_set_fmt(stream, &f->fmt.pix_mp, true);
	6822	+ ret = rkcif_set_fmt(stream, &f->fmt.pix_mp, true);
3612	6823
3613		- return 0;
	6824	+ return ret;
3614	6825	}
3615	6826
3616	6827	static int rkcif_enum_framesizes(struct file file, void prov,
3617	6828	struct v4l2_frmsizeenum *fsize)
3618	6829	{
	6830	+ struct v4l2_frmsize_discrete *d = &fsize->discrete;
3619	6831	struct v4l2_frmsize_stepwise *s = &fsize->stepwise;
3620	6832	struct rkcif_stream *stream = video_drvdata(file);
3621	6833	struct rkcif_device *dev = stream->cifdev;
3622	6834	struct v4l2_rect input_rect;
3623		- int vc;
	6835	+ struct csi_channel_info csi_info;
3624	6836
3625	6837	if (fsize->index != 0)
3626	6838	return -EINVAL;
3627	6839
3628		- if (!find_output_fmt(stream, fsize->pixel_format))
	6840	+ if (!rkcif_find_output_fmt(stream, fsize->pixel_format))
3629	6841	return -EINVAL;
3630	6842
3631	6843	input_rect.width = RKCIF_DEFAULT_WIDTH;
3632	6844	input_rect.height = RKCIF_DEFAULT_HEIGHT;
3633	6845
3634	6846	if (dev->terminal_sensor.sd)
3635		- get_input_fmt(dev->terminal_sensor.sd,
3636		- &input_rect, stream->id, &vc);
	6847	+ rkcif_get_input_fmt(dev,
	6848	+ &input_rect, stream->id,
	6849	+ &csi_info);
3637	6850
3638		- fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
3639		- s->min_width = CIF_MIN_WIDTH;
3640		- s->min_height = CIF_MIN_HEIGHT;
3641		- s->max_width = input_rect.width;
3642		- s->max_height = input_rect.height;
3643		- s->step_width = OUTPUT_STEP_WISE;
3644		- s->step_height = OUTPUT_STEP_WISE;
	6851	+ if (dev->hw_dev->adapt_to_usbcamerahal) {
	6852	+ fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
	6853	+ d->width = input_rect.width;
	6854	+ d->height = input_rect.height;
	6855	+ } else {
	6856	+ fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
	6857	+ s->min_width = CIF_MIN_WIDTH;
	6858	+ s->min_height = CIF_MIN_HEIGHT;
	6859	+ s->max_width = input_rect.width;
	6860	+ s->max_height = input_rect.height;
	6861	+ s->step_width = OUTPUT_STEP_WISE;
	6862	+ s->step_height = OUTPUT_STEP_WISE;
	6863	+ }
3645	6864
3646	6865	return 0;
3647	6866	}
..	..	@@ -3673,13 +6892,19 @@
3673	6892	fi.interval.denominator = 30;
3674	6893	}
3675	6894
3676		- fival->type = V4L2_FRMIVAL_TYPE_CONTINUOUS;
3677		- fival->stepwise.step.numerator = 1;
3678		- fival->stepwise.step.denominator = 1;
3679		- fival->stepwise.max.numerator = 1;
3680		- fival->stepwise.max.denominator = 1;
3681		- fival->stepwise.min.numerator = fi.interval.numerator;
3682		- fival->stepwise.min.denominator = fi.interval.denominator;
	6895	+ if (dev->hw_dev->adapt_to_usbcamerahal) {
	6896	+ fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
	6897	+ fival->discrete.numerator = fi.interval.numerator;
	6898	+ fival->discrete.denominator = fi.interval.denominator;
	6899	+ } else {
	6900	+ fival->type = V4L2_FRMIVAL_TYPE_CONTINUOUS;
	6901	+ fival->stepwise.step.numerator = 1;
	6902	+ fival->stepwise.step.denominator = 1;
	6903	+ fival->stepwise.max.numerator = 1;
	6904	+ fival->stepwise.max.denominator = 1;
	6905	+ fival->stepwise.min.numerator = fi.interval.numerator;
	6906	+ fival->stepwise.min.denominator = fi.interval.denominator;
	6907	+ }
3683	6908
3684	6909	return 0;
3685	6910	}
..	..	@@ -3688,13 +6913,57 @@
3688	6913	struct v4l2_fmtdesc *f)
3689	6914	{
3690	6915	const struct cif_output_fmt *fmt = NULL;
	6916	+ struct rkcif_stream *stream = video_drvdata(file);
	6917	+ struct rkcif_device *dev = stream->cifdev;
	6918	+ const struct cif_input_fmt *cif_fmt_in = NULL;
	6919	+ struct v4l2_rect input_rect;
	6920	+ int i = 0;
	6921	+ int ret = 0;
	6922	+ int fource_idx = 0;
3691	6923
3692	6924	if (f->index >= ARRAY_SIZE(out_fmts))
3693	6925	return -EINVAL;
3694	6926
3695		- fmt = &out_fmts[f->index];
3696		- f->pixelformat = fmt->fourcc;
	6927	+ if (dev->terminal_sensor.sd) {
	6928	+ cif_fmt_in = rkcif_get_input_fmt(dev,
	6929	+ &input_rect, stream->id,
	6930	+ &dev->channels[stream->id]);
	6931	+ stream->cif_fmt_in = cif_fmt_in;
	6932	+ } else {
	6933	+ v4l2_err(&stream->cifdev->v4l2_dev,
	6934	+ "terminal subdev does not exist\n");
	6935	+ return -EINVAL;
	6936	+ }
3697	6937
	6938	+ if (f->index != 0)
	6939	+ fource_idx = stream->new_fource_idx;
	6940	+
	6941	+ for (i = fource_idx; i < ARRAY_SIZE(out_fmts); i++) {
	6942	+ fmt = &out_fmts[i];
	6943	+ ret = rkcif_output_fmt_check(stream, fmt);
	6944	+ if (!ret) {
	6945	+ f->pixelformat = fmt->fourcc;
	6946	+ stream->new_fource_idx = i + 1;
	6947	+ break;
	6948	+ }
	6949	+ }
	6950	+ if (i == ARRAY_SIZE(out_fmts))
	6951	+ return -EINVAL;
	6952	+
	6953	+ switch (f->pixelformat) {
	6954	+ case V4l2_PIX_FMT_EBD8:
	6955	+ strscpy(f->description,
	6956	+ "Embedded data 8-bit",
	6957	+ sizeof(f->description));
	6958	+ break;
	6959	+ case V4l2_PIX_FMT_SPD16:
	6960	+ strscpy(f->description,
	6961	+ "Shield pix data 16-bit",
	6962	+ sizeof(f->description));
	6963	+ break;
	6964	+ default:
	6965	+ break;
	6966	+ }
3698	6967	return 0;
3699	6968	}
3700	6969
..	..	@@ -3703,15 +6972,16 @@
3703	6972	{
3704	6973	struct rkcif_stream *stream = video_drvdata(file);
3705	6974	struct rkcif_device *dev = stream->cifdev;
	6975	+ int ret = 0;
3706	6976
3707	6977	if (vb2_is_busy(&stream->vnode.buf_queue)) {
3708	6978	v4l2_err(&dev->v4l2_dev, "%s queue busy\n", __func__);
3709	6979	return -EBUSY;
3710	6980	}
3711	6981
3712		- rkcif_set_fmt(stream, &f->fmt.pix_mp, false);
	6982	+ ret = rkcif_set_fmt(stream, &f->fmt.pix_mp, false);
3713	6983
3714		- return 0;
	6984	+ return ret;
3715	6985	}
3716	6986
3717	6987	static int rkcif_g_fmt_vid_cap_mplane(struct file file, void fh,
..	..	@@ -3738,8 +7008,8 @@
3738	7008	return 0;
3739	7009	}
3740	7010
3741		-static int rkcif_cropcap(struct file file, void fh,
3742		- struct v4l2_cropcap *cap)
	7011	+static __maybe_unused int rkcif_cropcap(struct file file, void fh,
	7012	+ struct v4l2_cropcap *cap)
3743	7013	{
3744	7014	struct rkcif_stream *stream = video_drvdata(file);
3745	7015	struct rkcif_device *dev = stream->cifdev;
..	..	@@ -3765,78 +7035,16 @@
3765	7035	return ret;
3766	7036	}
3767	7037
3768		-static int rkcif_s_crop(struct file file, void fh, const struct v4l2_crop *a)
3769		-{
3770		- struct rkcif_stream *stream = video_drvdata(file);
3771		- struct rkcif_device *dev = stream->cifdev;
3772		- const struct v4l2_rect *rect = &a->c;
3773		- struct v4l2_rect sensor_crop;
3774		- struct v4l2_rect *raw_rect = &dev->terminal_sensor.raw_rect;
3775		- int ret;
3776		-
3777		- ret = rkcif_sanity_check_fmt(stream, rect);
3778		- if (ret) {
3779		- v4l2_err(&dev->v4l2_dev, "set crop failed\n");
3780		- return ret;
3781		- }
3782		-
3783		- if (stream->crop_mask & CROP_SRC_SENSOR) {
3784		- sensor_crop = stream->crop[CROP_SRC_SENSOR];
3785		- if (rect->left + rect->width > sensor_crop.width \|\|
3786		- rect->top + rect->height > sensor_crop.height) {
3787		- v4l2_err(&dev->v4l2_dev,
3788		- "crop size is bigger than sensor input:left:%d, top:%d, width:%d, height:%d\n",
3789		- sensor_crop.left, sensor_crop.top, sensor_crop.width, sensor_crop.height);
3790		- return -EINVAL;
3791		- }
3792		- } else {
3793		- if (rect->left + rect->width > raw_rect->width \|\|
3794		- rect->top + rect->height > raw_rect->height) {
3795		- v4l2_err(&dev->v4l2_dev,
3796		- "crop size is bigger than sensor raw input:left:%d, top:%d, width:%d, height:%d\n",
3797		- raw_rect->left, raw_rect->top, raw_rect->width, raw_rect->height);
3798		- return -EINVAL;
3799		- }
3800		- }
3801		-
3802		- stream->crop[CROP_SRC_USR] = *rect;
3803		- stream->crop_enable = true;
3804		- stream->crop_mask \|= CROP_SRC_USR_MASK;
3805		- stream->crop[CROP_SRC_ACT] = stream->crop[CROP_SRC_USR];
3806		- if (stream->crop_mask & CROP_SRC_SENSOR) {
3807		- stream->crop[CROP_SRC_ACT].left = sensor_crop.left + stream->crop[CROP_SRC_USR].left;
3808		- stream->crop[CROP_SRC_ACT].top = sensor_crop.top + stream->crop[CROP_SRC_USR].top;
3809		- }
3810		-
3811		- if (stream->state == RKCIF_STATE_STREAMING) {
3812		- stream->crop_dyn_en = true;
3813		-
3814		- v4l2_info(&dev->v4l2_dev, "enable dynamic crop, S_CROP(%ux%u@%u:%u) type: %d\n",
3815		- rect->width, rect->height, rect->left, rect->top, a->type);
3816		- } else {
3817		- v4l2_info(&dev->v4l2_dev, "static crop, S_CROP(%ux%u@%u:%u) type: %d\n",
3818		- rect->width, rect->height, rect->left, rect->top, a->type);
3819		- }
3820		-
3821		- return ret;
3822		-}
3823		-
3824		-static int rkcif_g_crop(struct file file, void fh, struct v4l2_crop *a)
3825		-{
3826		- struct rkcif_stream *stream = video_drvdata(file);
3827		-
3828		- a->c = stream->crop[CROP_SRC_ACT];
3829		-
3830		- return 0;
3831		-}
3832		-
3833	7038	static int rkcif_s_selection(struct file file, void fh,
3834		- struct v4l2_selection *s)
	7039	+ struct v4l2_selection *s)
3835	7040	{
3836	7041	struct rkcif_stream *stream = video_drvdata(file);
3837	7042	struct rkcif_device *dev = stream->cifdev;
3838	7043	struct v4l2_subdev *sensor_sd;
3839	7044	struct v4l2_subdev_selection sd_sel;
	7045	+ const struct v4l2_rect *rect = &s->r;
	7046	+ struct v4l2_rect sensor_crop;
	7047	+ struct v4l2_rect *raw_rect = &dev->terminal_sensor.raw_rect;
3840	7048	u16 pad = 0;
3841	7049	int ret = 0;
3842	7050
..	..	@@ -3845,18 +7053,69 @@
3845	7053	goto err;
3846	7054	}
3847	7055
3848		- sensor_sd = get_remote_sensor(stream, &pad);
	7056	+ if (s->target == V4L2_SEL_TGT_CROP_BOUNDS) {
	7057	+ sensor_sd = get_remote_sensor(stream, &pad);
3849	7058
3850		- sd_sel.r = s->r;
3851		- sd_sel.pad = pad;
3852		- sd_sel.target = s->target;
3853		- sd_sel.which = V4L2_SUBDEV_FORMAT_ACTIVE;
	7059	+ sd_sel.r = s->r;
	7060	+ sd_sel.pad = pad;
	7061	+ sd_sel.target = s->target;
	7062	+ sd_sel.which = V4L2_SUBDEV_FORMAT_ACTIVE;
3854	7063
3855		- ret = v4l2_subdev_call(sensor_sd, pad, set_selection, NULL, &sd_sel);
3856		- if (!ret) {
3857		- s->r = sd_sel.r;
3858		- v4l2_dbg(1, rkcif_debug, &dev->v4l2_dev, "%s: pad:%d, which:%d, target:%d\n",
3859		- __func__, pad, sd_sel.which, sd_sel.target);
	7064	+ ret = v4l2_subdev_call(sensor_sd, pad, set_selection, NULL, &sd_sel);
	7065	+ if (!ret) {
	7066	+ s->r = sd_sel.r;
	7067	+ v4l2_dbg(1, rkcif_debug, &dev->v4l2_dev, "%s: pad:%d, which:%d, target:%d\n",
	7068	+ __func__, pad, sd_sel.which, sd_sel.target);
	7069	+ }
	7070	+ } else if (s->target == V4L2_SEL_TGT_CROP) {
	7071	+ ret = rkcif_sanity_check_fmt(stream, rect);
	7072	+ if (ret) {
	7073	+ v4l2_err(&dev->v4l2_dev, "set crop failed\n");
	7074	+ return ret;
	7075	+ }
	7076	+
	7077	+ if (stream->crop_mask & CROP_SRC_SENSOR) {
	7078	+ sensor_crop = stream->crop[CROP_SRC_SENSOR];
	7079	+ if (rect->left + rect->width > sensor_crop.width \|\|
	7080	+ rect->top + rect->height > sensor_crop.height) {
	7081	+ v4l2_err(&dev->v4l2_dev,
	7082	+ "crop size is bigger than sensor input:left:%d, top:%d, width:%d, height:%d\n",
	7083	+ sensor_crop.left, sensor_crop.top,
	7084	+ sensor_crop.width, sensor_crop.height);
	7085	+ return -EINVAL;
	7086	+ }
	7087	+ } else {
	7088	+ if (rect->left + rect->width > raw_rect->width \|\|
	7089	+ rect->top + rect->height > raw_rect->height) {
	7090	+ v4l2_err(&dev->v4l2_dev,
	7091	+ "crop size is bigger than sensor raw input:left:%d, top:%d, width:%d, height:%d\n",
	7092	+ raw_rect->left, raw_rect->top,
	7093	+ raw_rect->width, raw_rect->height);
	7094	+ return -EINVAL;
	7095	+ }
	7096	+ }
	7097	+
	7098	+ stream->crop[CROP_SRC_USR] = *rect;
	7099	+ stream->crop_enable = true;
	7100	+ stream->crop_mask \|= CROP_SRC_USR_MASK;
	7101	+ stream->crop[CROP_SRC_ACT] = stream->crop[CROP_SRC_USR];
	7102	+ if (stream->crop_mask & CROP_SRC_SENSOR) {
	7103	+ sensor_crop = stream->crop[CROP_SRC_SENSOR];
	7104	+ stream->crop[CROP_SRC_ACT].left = sensor_crop.left + stream->crop[CROP_SRC_USR].left;
	7105	+ stream->crop[CROP_SRC_ACT].top = sensor_crop.top + stream->crop[CROP_SRC_USR].top;
	7106	+ }
	7107	+
	7108	+ if (stream->state == RKCIF_STATE_STREAMING) {
	7109	+ stream->crop_dyn_en = true;
	7110	+
	7111	+ v4l2_info(&dev->v4l2_dev, "enable dynamic crop, S_SELECTION(%ux%u@%u:%u) target: %d\n",
	7112	+ rect->width, rect->height, rect->left, rect->top, s->target);
	7113	+ } else {
	7114	+ v4l2_info(&dev->v4l2_dev, "static crop, S_SELECTION(%ux%u@%u:%u) target: %d\n",
	7115	+ rect->width, rect->height, rect->left, rect->top, s->target);
	7116	+ }
	7117	+ } else {
	7118	+ goto err;
3860	7119	}
3861	7120
3862	7121	return ret;
..	..	@@ -3866,7 +7125,7 @@
3866	7125	}
3867	7126
3868	7127	static int rkcif_g_selection(struct file file, void fh,
3869		- struct v4l2_selection *s)
	7128	+ struct v4l2_selection *s)
3870	7129	{
3871	7130	struct rkcif_stream *stream = video_drvdata(file);
3872	7131	struct rkcif_device *dev = stream->cifdev;
..	..	@@ -3917,20 +7176,144 @@
3917	7176	return -EINVAL;
3918	7177	}
3919	7178
	7179	+static int rkcif_get_max_common_div(int a, int b)
	7180	+{
	7181	+ int remainder = a % b;
	7182	+
	7183	+ while (remainder != 0) {
	7184	+ a = b;
	7185	+ b = remainder;
	7186	+ remainder = a % b;
	7187	+ }
	7188	+ return b;
	7189	+}
	7190	+
	7191	+void rkcif_set_fps(struct rkcif_stream stream, struct rkcif_fps fps)
	7192	+{
	7193	+ struct rkcif_sensor_info *sensor = &stream->cifdev->terminal_sensor;
	7194	+ struct rkcif_device *cif_dev = stream->cifdev;
	7195	+ struct rkcif_stream *tmp_stream = NULL;
	7196	+ u32 numerator, denominator;
	7197	+ u32 def_fps = 0;
	7198	+ u32 cur_fps = 0;
	7199	+ int cap_m, skip_n;
	7200	+ int i = 0;
	7201	+ int max_common_div;
	7202	+ bool skip_en = false;
	7203	+ s32 vblank_def = 0;
	7204	+ s32 vblank_curr = 0;
	7205	+ int ret = 0;
	7206	+
	7207	+ if (!stream->cifdev->terminal_sensor.sd) {
	7208	+ ret = rkcif_update_sensor_info(stream);
	7209	+ if (ret) {
	7210	+ v4l2_err(&stream->cifdev->v4l2_dev,
	7211	+ "%s update sensor info fail\n",
	7212	+ __func__);
	7213	+ return;
	7214	+ }
	7215	+
	7216	+ }
	7217	+ if (!stream->cifdev->terminal_sensor.sd)
	7218	+ return;
	7219	+ numerator = sensor->fi.interval.numerator;
	7220	+ denominator = sensor->fi.interval.denominator;
	7221	+ def_fps = denominator / numerator;
	7222	+
	7223	+ vblank_def = rkcif_get_sensor_vblank_def(cif_dev);
	7224	+ vblank_curr = rkcif_get_sensor_vblank(cif_dev);
	7225	+ if (vblank_def)
	7226	+ cur_fps = def_fps * (u32)(vblank_def + sensor->raw_rect.height) /
	7227	+ (u32)(vblank_curr + sensor->raw_rect.height);
	7228	+ else
	7229	+ cur_fps = def_fps;
	7230	+
	7231	+ if (fps->fps == 0 \|\| fps->fps > cur_fps) {
	7232	+ v4l2_err(&stream->cifdev->v4l2_dev,
	7233	+ "set fps %d fps failed, current fps %d fps\n",
	7234	+ fps->fps, cur_fps);
	7235	+ return;
	7236	+ }
	7237	+ cap_m = fps->fps;
	7238	+ skip_n = cur_fps - fps->fps;
	7239	+ max_common_div = rkcif_get_max_common_div(cap_m, skip_n);
	7240	+ cap_m /= max_common_div;
	7241	+ skip_n /= max_common_div;
	7242	+ if (cap_m > 64) {
	7243	+ skip_n = skip_n / (cap_m / 64);
	7244	+ if (skip_n == 0)
	7245	+ skip_n = 1;
	7246	+ cap_m = 64;
	7247	+ }
	7248	+ if (skip_n > 7) {
	7249	+ cap_m = cap_m / (skip_n / 7);
	7250	+ if (cap_m == 0)
	7251	+ cap_m = 1;
	7252	+ skip_n = 7;
	7253	+ }
	7254	+
	7255	+ if (fps->fps == cur_fps)
	7256	+ skip_en = false;
	7257	+ else
	7258	+ skip_en = true;
	7259	+
	7260	+ if (fps->ch_num > 1 && fps->ch_num < 4) {
	7261	+ for (i = 0; i < fps->ch_num; i++) {
	7262	+ tmp_stream = &cif_dev->stream[i];
	7263	+ if (skip_en) {
	7264	+ tmp_stream->skip_info.skip_to_en = true;
	7265	+ tmp_stream->skip_info.cap_m = cap_m;
	7266	+ tmp_stream->skip_info.skip_n = skip_n;
	7267	+ } else {
	7268	+ tmp_stream->skip_info.skip_to_dis = true;
	7269	+ }
	7270	+ }
	7271	+ } else {
	7272	+ if (skip_en) {
	7273	+ stream->skip_info.skip_to_en = true;
	7274	+ stream->skip_info.cap_m = cap_m;
	7275	+ stream->skip_info.skip_n = skip_n;
	7276	+ } else {
	7277	+ stream->skip_info.skip_to_dis = true;
	7278	+ }
	7279	+ }
	7280	+ v4l2_dbg(1, rkcif_debug, &stream->cifdev->v4l2_dev,
	7281	+ "skip_to_en %d, cap_m %d, skip_n %d\n",
	7282	+ stream->skip_info.skip_to_en,
	7283	+ cap_m,
	7284	+ skip_n);
	7285	+}
	7286	+
	7287	+static bool rkcif_check_can_be_online(struct rkcif_device *cif_dev)
	7288	+{
	7289	+ if (cif_dev->sditf[0] == NULL \|\| cif_dev->sditf[0]->mode.name == NULL \|\|
	7290	+ (cif_dev->chip_id == CHIP_RV1106_CIF &&
	7291	+ strstr(cif_dev->sditf[0]->mode.name, "unite")))
	7292	+ return false;
	7293	+ return true;
	7294	+}
	7295	+
3920	7296	static int rkcif_do_reset_work(struct rkcif_device *cif_dev,
3921	7297	enum rkmodule_reset_src reset_src);
	7298	+static bool rkcif_check_single_dev_stream_on(struct rkcif_hw *hw);
3922	7299
3923	7300	static long rkcif_ioctl_default(struct file file, void fh,
3924		- bool valid_prio, unsigned int cmd, void *arg)
	7301	+ bool valid_prio, unsigned int cmd, void *arg)
3925	7302	{
3926	7303	struct rkcif_stream *stream = video_drvdata(file);
3927	7304	struct rkcif_device *dev = stream->cifdev;
3928	7305	const struct cif_input_fmt *in_fmt;
3929	7306	struct v4l2_rect rect;
3930		- int vc = 0;
3931		- struct rkcif_reset_info *reset_info;
3932		- int reset_src = 0;
3933		- unsigned long flags;
	7307	+ struct csi_channel_info csi_info;
	7308	+ struct rkcif_fps fps;
	7309	+ int reset_src;
	7310	+ struct rkcif_quick_stream_param *stream_param;
	7311	+ bool is_single_dev = false;
	7312	+ struct v4l2_subdev *sd;
	7313	+ int ret = -EINVAL;
	7314	+ int i = 0;
	7315	+ int stream_num = 0;
	7316	+ bool is_can_be_online = false;
3934	7317
3935	7318	switch (cmd) {
3936	7319	case RKCIF_CMD_GET_CSI_MEMORY_MODE:
..	..	@@ -3945,7 +7328,8 @@
3945	7328	break;
3946	7329	case RKCIF_CMD_SET_CSI_MEMORY_MODE:
3947	7330	if (dev->terminal_sensor.sd) {
3948		- in_fmt = get_input_fmt(dev->terminal_sensor.sd, &rect, 0, &vc);
	7331	+ in_fmt = rkcif_get_input_fmt(dev,
	7332	+ &rect, 0, &csi_info);
3949	7333	if (in_fmt == NULL) {
3950	7334	v4l2_err(&dev->v4l2_dev, "can't get sensor input format\n");
3951	7335	return -EINVAL;
..	..	@@ -3971,19 +7355,98 @@
3971	7355	stream->is_high_align = false;
3972	7356	}
3973	7357	break;
3974		- case RKCIF_CMD_GET_RESET_INFO:
3975		- reset_info = (struct rkcif_reset_info *)arg;
3976		- spin_lock_irqsave(&dev->hw_dev->spin_lock, flags);
3977		- *reset_info = dev->hw_dev->reset_info;
3978		- spin_unlock_irqrestore(&dev->hw_dev->spin_lock, flags);
	7358	+ case RKCIF_CMD_SET_FPS:
	7359	+ fps = (struct rkcif_fps )arg;
	7360	+ rkcif_set_fps(stream, &fps);
3979	7361	break;
3980	7362	case RKCIF_CMD_SET_RESET:
3981	7363	reset_src = (int )arg;
3982		- spin_lock_irqsave(&dev->hw_dev->spin_lock, flags);
3983		- if (dev->hw_dev->reset_info.is_need_reset)
3984		- dev->hw_dev->reset_info.is_need_reset = 0;
3985		- spin_unlock_irqrestore(&dev->hw_dev->spin_lock, flags);
3986	7364	return rkcif_do_reset_work(dev, reset_src);
	7365	+ case RKCIF_CMD_SET_QUICK_STREAM:
	7366	+ stream_param = (struct rkcif_quick_stream_param *)arg;
	7367	+ if (!dev->sditf[0])
	7368	+ return -EINVAL;
	7369	+ if (dev->hdr.hdr_mode == HDR_X2)
	7370	+ stream_num = 2;
	7371	+ else if (dev->hdr.hdr_mode == HDR_X3)
	7372	+ stream_num = 3;
	7373	+ else
	7374	+ stream_num = 1;
	7375	+ if (stream_param->on) {
	7376	+ for (i = 0; i < stream_num; i++)
	7377	+ dev->stream[i].cur_skip_frame = dev->stream[i].skip_frame;
	7378	+ is_single_dev = rkcif_check_single_dev_stream_on(dev->hw_dev);
	7379	+ is_can_be_online = rkcif_check_can_be_online(dev);
	7380	+ if (is_single_dev && is_can_be_online) {
	7381	+ for (i = 0; i < stream_num - 1; i++) {
	7382	+ dev->stream[i].to_en_dma = RKCIF_DMAEN_BY_ISP;
	7383	+ rkcif_enable_dma_capture(&dev->stream[i], true);
	7384	+ }
	7385	+ dev->sditf[0]->mode.rdbk_mode = RKISP_VICAP_ONLINE;
	7386	+ sditf_change_to_online(dev->sditf[0]);
	7387	+ sd = get_rkisp_sd(dev->sditf[0]);
	7388	+ if (sd)
	7389	+ ret = v4l2_subdev_call(sd, core, ioctl,
	7390	+ RKISP_VICAP_CMD_MODE, &dev->sditf[0]->mode);
	7391	+ if (ret) {
	7392	+ v4l2_err(&dev->v4l2_dev, "set isp work mode online fail\n");
	7393	+ return -EINVAL;
	7394	+ }
	7395	+ } else {
	7396	+ sditf_disable_immediately(dev->sditf[0]);
	7397	+ dev->sditf[0]->mode.rdbk_mode = RKISP_VICAP_RDBK_AUTO;
	7398	+ sd = get_rkisp_sd(dev->sditf[0]);
	7399	+ if (sd)
	7400	+ ret = v4l2_subdev_call(sd, core, ioctl,
	7401	+ RKISP_VICAP_CMD_MODE, &dev->sditf[0]->mode);
	7402	+ for (i = 0; i < stream_num; i++) {
	7403	+ if (dev->sditf[0]->mode.rdbk_mode == RKISP_VICAP_RDBK_AUTO)
	7404	+ dev->stream[i].to_en_dma = RKCIF_DMAEN_BY_ISP;
	7405	+ else
	7406	+ dev->stream[i].to_en_dma = RKCIF_DMAEN_BY_VICAP;
	7407	+ rkcif_enable_dma_capture(&dev->stream[i], true);
	7408	+ }
	7409	+ }
	7410	+ rkcif_dphy_quick_stream(dev, stream_param->on);
	7411	+ v4l2_subdev_call(dev->terminal_sensor.sd, core, ioctl,
	7412	+ RKMODULE_SET_QUICK_STREAM, &stream_param->on);
	7413	+ } else {
	7414	+ if (dev->sditf[0]->mode.rdbk_mode == RKISP_VICAP_ONLINE) {
	7415	+ for (i = 0; i < stream_num - 1; i++) {
	7416	+ reinit_completion(&dev->stream[i].stop_complete);
	7417	+ dev->stream[i].is_wait_stop_complete = true;
	7418	+ dev->stream[i].to_stop_dma = RKCIF_DMAEN_BY_ISP;
	7419	+ wait_for_completion_timeout(&dev->stream[i].stop_complete,
	7420	+ msecs_to_jiffies(RKCIF_STOP_MAX_WAIT_TIME_MS));
	7421	+ }
	7422	+ stream->cifdev->sensor_state = stream_param->on;
	7423	+ stream->cifdev->sensor_state_change = true;
	7424	+ dev->stream[i].is_wait_stop_complete = true;
	7425	+ wait_for_completion_timeout(&dev->stream[i].stop_complete,
	7426	+ msecs_to_jiffies(RKCIF_STOP_MAX_WAIT_TIME_MS));
	7427	+ } else {
	7428	+ for (i = 0; i < stream_num; i++) {
	7429	+ dev->stream[i].is_wait_stop_complete = true;
	7430	+ reinit_completion(&dev->stream[i].stop_complete);
	7431	+ if (dev->sditf[0]->mode.rdbk_mode == RKISP_VICAP_RDBK_AUTO)
	7432	+ dev->stream[i].to_stop_dma = RKCIF_DMAEN_BY_ISP;
	7433	+ else
	7434	+ dev->stream[i].to_stop_dma = RKCIF_DMAEN_BY_VICAP;
	7435	+ wait_for_completion_timeout(&dev->stream[i].stop_complete,
	7436	+ msecs_to_jiffies(RKCIF_STOP_MAX_WAIT_TIME_MS));
	7437	+ }
	7438	+ rkcif_dphy_quick_stream(dev, stream_param->on);
	7439	+ v4l2_subdev_call(dev->terminal_sensor.sd, core, ioctl,
	7440	+ RKMODULE_SET_QUICK_STREAM, &stream_param->on);
	7441	+ }
	7442	+ stream_param->frame_num = dev->stream[0].frame_idx - 1;
	7443	+ if (!dev->is_rtt_suspend) {
	7444	+ dev->resume_mode = stream_param->resume_mode;
	7445	+ v4l2_dbg(3, rkcif_debug, &dev->v4l2_dev,
	7446	+ "set resume mode %d\n", dev->resume_mode);
	7447	+ }
	7448	+ }
	7449	+ break;
3987	7450	default:
3988	7451	return -EINVAL;
3989	7452	}
..	..	@@ -4003,13 +7466,10 @@
4003	7466	.vidioc_streamoff = vb2_ioctl_streamoff,
4004	7467	.vidioc_enum_input = rkcif_enum_input,
4005	7468	.vidioc_try_fmt_vid_cap_mplane = rkcif_try_fmt_vid_cap_mplane,
4006		- .vidioc_enum_fmt_vid_cap_mplane = rkcif_enum_fmt_vid_cap_mplane,
	7469	+ .vidioc_enum_fmt_vid_cap = rkcif_enum_fmt_vid_cap_mplane,
4007	7470	.vidioc_s_fmt_vid_cap_mplane = rkcif_s_fmt_vid_cap_mplane,
4008	7471	.vidioc_g_fmt_vid_cap_mplane = rkcif_g_fmt_vid_cap_mplane,
4009	7472	.vidioc_querycap = rkcif_querycap,
4010		- .vidioc_cropcap = rkcif_cropcap,
4011		- .vidioc_s_crop = rkcif_s_crop,
4012		- .vidioc_g_crop = rkcif_g_crop,
4013	7473	.vidioc_s_selection = rkcif_s_selection,
4014	7474	.vidioc_g_selection = rkcif_g_selection,
4015	7475	.vidioc_enum_frameintervals = rkcif_enum_frameintervals,
..	..	@@ -4017,8 +7477,59 @@
4017	7477	.vidioc_default = rkcif_ioctl_default,
4018	7478	};
4019	7479
	7480	+void rkcif_vb_done_oneframe(struct rkcif_stream *stream,
	7481	+ struct vb2_v4l2_buffer *vb_done)
	7482	+{
	7483	+ const struct cif_output_fmt *fmt = stream->cif_fmt_out;
	7484	+ u32 i;
	7485	+
	7486	+ /* Dequeue a filled buffer */
	7487	+ for (i = 0; i < fmt->mplanes; i++) {
	7488	+ vb2_set_plane_payload(&vb_done->vb2_buf, i,
	7489	+ stream->pixm.plane_fmt[i].sizeimage);
	7490	+ }
	7491	+
	7492	+ vb2_buffer_done(&vb_done->vb2_buf, VB2_BUF_STATE_DONE);
	7493	+ v4l2_dbg(2, rkcif_debug, &stream->cifdev->v4l2_dev,
	7494	+ "stream[%d] vb done, index: %d, sequence %d\n", stream->id,
	7495	+ vb_done->vb2_buf.index, vb_done->sequence);
	7496	+ atomic_dec(&stream->buf_cnt);
	7497	+}
	7498	+
	7499	+static void rkcif_tasklet_handle(unsigned long data)
	7500	+{
	7501	+ struct rkcif_stream stream = (struct rkcif_stream )data;
	7502	+ struct rkcif_buffer *buf = NULL;
	7503	+ unsigned long flags = 0;
	7504	+ LIST_HEAD(local_list);
	7505	+
	7506	+ spin_lock_irqsave(&stream->vbq_lock, flags);
	7507	+ list_replace_init(&stream->vb_done_list, &local_list);
	7508	+ spin_unlock_irqrestore(&stream->vbq_lock, flags);
	7509	+
	7510	+ while (!list_empty(&local_list)) {
	7511	+ buf = list_first_entry(&local_list,
	7512	+ struct rkcif_buffer, queue);
	7513	+ list_del(&buf->queue);
	7514	+ rkcif_vb_done_oneframe(stream, &buf->vb);
	7515	+ }
	7516	+}
	7517	+
	7518	+void rkcif_vb_done_tasklet(struct rkcif_stream stream, struct rkcif_buffer buf)
	7519	+{
	7520	+ unsigned long flags = 0;
	7521	+
	7522	+ if (!stream \|\| !buf)
	7523	+ return;
	7524	+ spin_lock_irqsave(&stream->vbq_lock, flags);
	7525	+ list_add_tail(&buf->queue, &stream->vb_done_list);
	7526	+ spin_unlock_irqrestore(&stream->vbq_lock, flags);
	7527	+ tasklet_schedule(&stream->vb_done_tasklet);
	7528	+}
	7529	+
4020	7530	static void rkcif_unregister_stream_vdev(struct rkcif_stream *stream)
4021	7531	{
	7532	+ tasklet_kill(&stream->vb_done_tasklet);
4022	7533	media_entity_cleanup(&stream->vnode.vdev.entity);
4023	7534	video_unregister_device(&stream->vnode.vdev);
4024	7535	}
..	..	@@ -4052,6 +7563,7 @@
4052	7563	vdev_name = CIF_DVP_VDEV_NAME;
4053	7564	break;
4054	7565	default:
	7566	+ ret = -EINVAL;
4055	7567	v4l2_err(v4l2_dev, "Invalid stream\n");
4056	7568	goto unreg;
4057	7569	}
..	..	@@ -4074,6 +7586,7 @@
4074	7586	vdev_name = CIF_MIPI_ID3_VDEV_NAME;
4075	7587	break;
4076	7588	default:
	7589	+ ret = -EINVAL;
4077	7590	v4l2_err(v4l2_dev, "Invalid stream\n");
4078	7591	goto unreg;
4079	7592	}
..	..	@@ -4092,6 +7605,7 @@
4092	7605	vdev_name = CIF_DVP_ID3_VDEV_NAME;
4093	7606	break;
4094	7607	default:
	7608	+ ret = -EINVAL;
4095	7609	v4l2_err(v4l2_dev, "Invalid stream\n");
4096	7610	goto unreg;
4097	7611	}
..	..	@@ -4118,7 +7632,7 @@
4118	7632	V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
4119	7633	vdev->queue = &node->buf_queue;
4120	7634
4121		- ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
	7635	+ ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
4122	7636	if (ret < 0) {
4123	7637	v4l2_err(v4l2_dev,
4124	7638	"video_register_device failed with error %d\n", ret);
..	..	@@ -4129,6 +7643,11 @@
4129	7643	if (ret < 0)
4130	7644	goto unreg;
4131	7645
	7646	+ INIT_LIST_HEAD(&stream->vb_done_list);
	7647	+ tasklet_init(&stream->vb_done_tasklet,
	7648	+ rkcif_tasklet_handle,
	7649	+ (unsigned long)stream);
	7650	+ tasklet_disable(&stream->vb_done_tasklet);
4132	7651	return 0;
4133	7652	unreg:
4134	7653	video_unregister_device(vdev);
..	..	@@ -4161,7 +7680,7 @@
4161	7680	if (ret < 0)
4162	7681	goto err;
4163	7682	}
4164		-
	7683	+ dev->num_channels = stream_num;
4165	7684	return 0;
4166	7685	err:
4167	7686	for (j = 0; j < i; j++) {
..	..	@@ -4229,8 +7748,8 @@
4229	7748	}
4230	7749
4231	7750	static struct v4l2_rect rkcif_lvds_sd_get_crop(struct rkcif_lvds_subdev subdev,
4232		- struct v4l2_subdev_pad_config *cfg,
4233		- enum v4l2_subdev_format_whence which)
	7751	+ struct v4l2_subdev_pad_config *cfg,
	7752	+ enum v4l2_subdev_format_whence which)
4234	7753	{
4235	7754	if (which == V4L2_SUBDEV_FORMAT_TRY)
4236	7755	return v4l2_subdev_get_try_crop(&subdev->sd, cfg, RKCIF_LVDS_PAD_SINK);
..	..	@@ -4239,8 +7758,8 @@
4239	7758	}
4240	7759
4241	7760	static int rkcif_lvds_sd_set_selection(struct v4l2_subdev *sd,
4242		- struct v4l2_subdev_pad_config *cfg,
4243		- struct v4l2_subdev_selection *sel)
	7761	+ struct v4l2_subdev_pad_config *cfg,
	7762	+ struct v4l2_subdev_selection *sel)
4244	7763	{
4245	7764	struct rkcif_lvds_subdev *subdev = container_of(sd, struct rkcif_lvds_subdev, sd);
4246	7765	struct v4l2_subdev *sensor = get_lvds_remote_sensor(sd);
..	..	@@ -4255,8 +7774,8 @@
4255	7774	}
4256	7775
4257	7776	static int rkcif_lvds_sd_get_selection(struct v4l2_subdev *sd,
4258		- struct v4l2_subdev_pad_config *cfg,
4259		- struct v4l2_subdev_selection *sel)
	7777	+ struct v4l2_subdev_pad_config *cfg,
	7778	+ struct v4l2_subdev_selection *sel)
4260	7779	{
4261	7780	struct rkcif_lvds_subdev *subdev = container_of(sd, struct rkcif_lvds_subdev, sd);
4262	7781	struct v4l2_subdev *sensor = get_lvds_remote_sensor(sd);
..	..	@@ -4312,13 +7831,13 @@
4312	7831	return -EINVAL;
4313	7832	}
4314	7833
4315		-static int rkcif_lvds_g_mbus_config(struct v4l2_subdev *sd,
	7834	+static int rkcif_lvds_g_mbus_config(struct v4l2_subdev *sd, unsigned int pad_id,
4316	7835	struct v4l2_mbus_config *mbus)
4317	7836	{
4318	7837	struct v4l2_subdev *sensor_sd = get_lvds_remote_sensor(sd);
4319	7838	int ret;
4320	7839
4321		- ret = v4l2_subdev_call(sensor_sd, video, g_mbus_config, mbus);
	7840	+ ret = v4l2_subdev_call(sensor_sd, pad, get_mbus_config, 0, mbus);
4322	7841	if (ret)
4323	7842	return ret;
4324	7843
..	..	@@ -4341,12 +7860,13 @@
4341	7860	}
4342	7861
4343	7862	static int rkcif_sof_subscribe_event(struct v4l2_subdev sd, struct v4l2_fh fh,
4344		- struct v4l2_event_subscription *sub)
	7863	+ struct v4l2_event_subscription *sub)
4345	7864	{
4346		- if (sub->type != V4L2_EVENT_FRAME_SYNC)
	7865	+ if (sub->type == V4L2_EVENT_FRAME_SYNC \|\|
	7866	+ sub->type == V4L2_EVENT_RESET_DEV)
	7867	+ return v4l2_event_subscribe(fh, sub, RKCIF_V4L2_EVENT_ELEMS, NULL);
	7868	+ else
4347	7869	return -EINVAL;
4348		-
4349		- return v4l2_event_subscribe(fh, sub, 0, NULL);
4350	7870	}
4351	7871
4352	7872	static const struct media_entity_operations rkcif_lvds_sd_media_ops = {
..	..	@@ -4359,10 +7879,10 @@
4359	7879	.get_fmt = rkcif_lvds_sd_get_fmt,
4360	7880	.set_selection = rkcif_lvds_sd_set_selection,
4361	7881	.get_selection = rkcif_lvds_sd_get_selection,
	7882	+ .get_mbus_config = rkcif_lvds_g_mbus_config,
4362	7883	};
4363	7884
4364	7885	static const struct v4l2_subdev_video_ops rkcif_lvds_sd_video_ops = {
4365		- .g_mbus_config = rkcif_lvds_g_mbus_config,
4366	7886	.s_stream = rkcif_lvds_sd_s_stream,
4367	7887	};
4368	7888
..	..	@@ -4400,10 +7920,12 @@
4400	7920	return 0;
4401	7921	}
4402	7922
4403		-static void rkcif_lvds_set_sof(struct rkcif_device *dev, u32 seq)
	7923	+static u32 rkcif_lvds_set_sof(struct rkcif_device *dev, u32 seq)
4404	7924	{
4405	7925	if (dev)
4406	7926	atomic_set(&dev->lvds_subdev.frm_sync_seq, seq);
	7927	+
	7928	+ return 0;
4407	7929	}
4408	7930
4409	7931	int rkcif_register_lvds_subdev(struct rkcif_device *dev)
..	..	@@ -4412,6 +7934,7 @@
4412	7934	struct rkcif_lvds_subdev *lvds_subdev = &dev->lvds_subdev;
4413	7935	struct v4l2_subdev *sd;
4414	7936	int ret;
	7937	+ int pad_num = 4;
4415	7938
4416	7939	memset(lvds_subdev, 0, sizeof(*lvds_subdev));
4417	7940	lvds_subdev->cifdev = dev;
..	..	@@ -4420,7 +7943,7 @@
4420	7943	v4l2_subdev_init(sd, &rkcif_lvds_sd_ops);
4421	7944	sd->flags \|= V4L2_SUBDEV_FL_HAS_DEVNODE \| V4L2_SUBDEV_FL_HAS_EVENTS;
4422	7945	sd->entity.ops = &rkcif_lvds_sd_media_ops;
4423		- if (dev->chip_id == CHIP_RV1126_CIF)
	7946	+ if (dev->chip_id != CHIP_RV1126_CIF_LITE)
4424	7947	snprintf(sd->name, sizeof(sd->name), "rkcif-lvds-subdev");
4425	7948	else
4426	7949	snprintf(sd->name, sizeof(sd->name), "rkcif-lite-lvds-subdev");
..	..	@@ -4431,6 +7954,13 @@
4431	7954	lvds_subdev->pads[RKCIF_LVDS_PAD_SRC_ID1].flags = MEDIA_PAD_FL_SOURCE;
4432	7955	lvds_subdev->pads[RKCIF_LVDS_PAD_SRC_ID2].flags = MEDIA_PAD_FL_SOURCE;
4433	7956	lvds_subdev->pads[RKCIF_LVDS_PAD_SRC_ID3].flags = MEDIA_PAD_FL_SOURCE;
	7957	+ if (dev->chip_id == CHIP_RV1106_CIF) {
	7958	+ lvds_subdev->pads[RKCIF_LVDS_PAD_SCL_ID0].flags = MEDIA_PAD_FL_SOURCE;
	7959	+ lvds_subdev->pads[RKCIF_LVDS_PAD_SCL_ID1].flags = MEDIA_PAD_FL_SOURCE;
	7960	+ lvds_subdev->pads[RKCIF_LVDS_PAD_SCL_ID2].flags = MEDIA_PAD_FL_SOURCE;
	7961	+ lvds_subdev->pads[RKCIF_LVDS_PAD_SCL_ID3].flags = MEDIA_PAD_FL_SOURCE;
	7962	+ pad_num = RKCIF_LVDS_PAD_MAX;
	7963	+ }
4434	7964
4435	7965	lvds_subdev->in_fmt.width = RKCIF_DEFAULT_WIDTH;
4436	7966	lvds_subdev->in_fmt.height = RKCIF_DEFAULT_HEIGHT;
..	..	@@ -4439,7 +7969,7 @@
4439	7969	lvds_subdev->crop.width = RKCIF_DEFAULT_WIDTH;
4440	7970	lvds_subdev->crop.height = RKCIF_DEFAULT_HEIGHT;
4441	7971
4442		- ret = media_entity_pads_init(&sd->entity, RKCIF_LVDS_PAD_MAX,
	7972	+ ret = media_entity_pads_init(&sd->entity, pad_num,
4443	7973	lvds_subdev->pads);
4444	7974	if (ret < 0)
4445	7975	return ret;
..	..	@@ -4491,10 +8021,12 @@
4491	8021	return 0;
4492	8022	}
4493	8023
4494		-static void rkcif_dvp_set_sof(struct rkcif_device *dev, u32 seq)
	8024	+static u32 rkcif_dvp_set_sof(struct rkcif_device *dev, u32 seq)
4495	8025	{
4496	8026	if (dev)
4497	8027	atomic_set(&dev->dvp_sof_subdev.frm_sync_seq, seq);
	8028	+
	8029	+ return 0;
4498	8030	}
4499	8031
4500	8032	static const struct v4l2_subdev_core_ops rkcif_dvp_sof_sd_core_ops = {
..	..	@@ -4547,28 +8079,6 @@
4547	8079	v4l2_device_unregister_subdev(sd);
4548	8080	}
4549	8081
4550		-
4551		-static void rkcif_vb_done_oneframe(struct rkcif_stream *stream,
4552		- struct vb2_v4l2_buffer *vb_done)
4553		-{
4554		- const struct cif_output_fmt *fmt = stream->cif_fmt_out;
4555		- u32 i;
4556		-
4557		- /* Dequeue a filled buffer */
4558		- for (i = 0; i < fmt->mplanes; i++) {
4559		- vb2_set_plane_payload(&vb_done->vb2_buf, i,
4560		- stream->pixm.plane_fmt[i].sizeimage);
4561		- }
4562		-
4563		- if (stream->cifdev->hdr.mode == NO_HDR)
4564		- vb_done->vb2_buf.timestamp = ktime_get_ns();
4565		-
4566		- vb2_buffer_done(&vb_done->vb2_buf, VB2_BUF_STATE_DONE);
4567		- v4l2_dbg(1, rkcif_debug, &stream->cifdev->v4l2_dev,
4568		- "stream[%d] vb done, index: %d, sequence %d\n", stream->id,
4569		- vb_done->vb2_buf.index, vb_done->sequence);
4570		-}
4571		-
4572	8082	void rkcif_irq_oneframe(struct rkcif_device *cif_dev)
4573	8083	{
4574	8084	/* TODO: xuhf-debug: add stream type */
..	..	@@ -4600,7 +8110,7 @@
4600	8110	}
4601	8111
4602	8112	if ((intstat & FRAME_END)) {
4603		- struct vb2_v4l2_buffer *vb_done = NULL;
	8113	+ struct rkcif_buffer *active_buf = NULL;
4604	8114
4605	8115	rkcif_write_register(cif_dev, CIF_REG_DVP_INTSTAT,
4606	8116	FRAME_END_CLR);
..	..	@@ -4633,11 +8143,11 @@
4633	8143	if (frmid % 2 != 0) {
4634	8144	stream->frame_phase = CIF_CSI_FRAME0_READY;
4635	8145	if (stream->curr_buf)
4636		- vb_done = &stream->curr_buf->vb;
	8146	+ active_buf = stream->curr_buf;
4637	8147	} else {
4638	8148	stream->frame_phase = CIF_CSI_FRAME1_READY;
4639	8149	if (stream->next_buf)
4640		- vb_done = &stream->next_buf->vb;
	8150	+ active_buf = stream->next_buf;
4641	8151	}
4642	8152
4643	8153	/* In one-frame mode:
..	..	@@ -4650,46 +8160,28 @@
4650	8160	ret = rkcif_assign_new_buffer_oneframe(stream,
4651	8161	RKCIF_YUV_ADDR_STATE_UPDATE);
4652	8162
4653		- if (vb_done && (!ret)) {
4654		- vb_done->sequence = stream->frame_idx;
4655		- rkcif_vb_done_oneframe(stream, vb_done);
	8163	+ if (active_buf && (!ret)) {
	8164	+ active_buf->vb.sequence = stream->frame_idx - 1;
	8165	+ rkcif_vb_done_tasklet(stream, active_buf);
4656	8166	}
4657	8167
4658		- stream->frame_idx++;
4659		- cif_dev->irq_stats.all_frm_end_cnt++;
	8168	+ cif_dev->irq_stats.frm_end_cnt[stream->id]++;
4660	8169	}
4661	8170	}
4662	8171
4663	8172	static int rkcif_csi_g_mipi_id(struct v4l2_device *v4l2_dev,
4664		- unsigned int intstat)
	8173	+ unsigned int intstat)
4665	8174	{
4666		- if (intstat & CSI_FRAME_END_ID0) {
4667		- if ((intstat & CSI_FRAME_END_ID0) ==
4668		- CSI_FRAME_END_ID0)
4669		- v4l2_warn(v4l2_dev, "frame0/1 trigger simultaneously in ID0\n");
	8175	+ if (intstat & CSI_FRAME_END_ID0)
4670	8176	return RKCIF_STREAM_MIPI_ID0;
4671		- }
4672	8177
4673		- if (intstat & CSI_FRAME_END_ID1) {
4674		- if ((intstat & CSI_FRAME_END_ID1) ==
4675		- CSI_FRAME_END_ID1)
4676		- v4l2_warn(v4l2_dev, "frame0/1 trigger simultaneously in ID1\n");
	8178	+ if (intstat & CSI_FRAME_END_ID1)
4677	8179	return RKCIF_STREAM_MIPI_ID1;
4678		- }
4679	8180
4680		- if (intstat & CSI_FRAME_END_ID2) {
4681		- if ((intstat & CSI_FRAME_END_ID2) ==
4682		- CSI_FRAME_END_ID2)
4683		- v4l2_warn(v4l2_dev, "frame0/1 trigger simultaneously in ID2\n");
	8181	+ if (intstat & CSI_FRAME_END_ID2)
4684	8182	return RKCIF_STREAM_MIPI_ID2;
4685		- }
4686		-
4687		- if (intstat & CSI_FRAME_END_ID3) {
4688		- if ((intstat & CSI_FRAME_END_ID3) ==
4689		- CSI_FRAME_END_ID3)
4690		- v4l2_warn(v4l2_dev, "frame0/1 trigger simultaneously in ID3\n");
	8183	+ if (intstat & CSI_FRAME_END_ID3)
4691	8184	return RKCIF_STREAM_MIPI_ID3;
4692		- }
4693	8185
4694	8186	return -EINVAL;
4695	8187	}
..	..	@@ -4698,38 +8190,42 @@
4698	8190	u32 *intstat, u32 frm_stat)
4699	8191	{
4700	8192	if (*intstat & DVP_FRAME_END_ID0) {
4701		- if ((frm_stat & DVP_CHANNEL0_FRM_READ) ==
4702		- DVP_CHANNEL0_FRM_READ)
4703		- v4l2_warn(v4l2_dev, "frame0/1 trigger simultaneously in DVP ID0\n");
4704		-
4705	8193	*intstat &= ~DVP_FRAME_END_ID0;
4706	8194	return RKCIF_STREAM_MIPI_ID0;
4707	8195	}
4708	8196
4709	8197	if (*intstat & DVP_FRAME_END_ID1) {
4710		- if ((frm_stat & DVP_CHANNEL1_FRM_READ) ==
4711		- DVP_CHANNEL1_FRM_READ)
4712		- v4l2_warn(v4l2_dev, "frame0/1 trigger simultaneously in DVP ID1\n");
4713		-
4714	8198	*intstat &= ~DVP_FRAME_END_ID1;
4715	8199	return RKCIF_STREAM_MIPI_ID1;
4716	8200	}
4717	8201
4718	8202	if (*intstat & DVP_FRAME_END_ID2) {
4719		- if ((frm_stat & DVP_CHANNEL2_FRM_READ) ==
4720		- DVP_CHANNEL2_FRM_READ)
4721		- v4l2_warn(v4l2_dev, "frame0/1 trigger simultaneously in DVP ID2\n");
4722	8203	*intstat &= ~DVP_FRAME_END_ID2;
4723	8204	return RKCIF_STREAM_MIPI_ID2;
4724	8205	}
4725	8206
4726	8207	if (*intstat & DVP_FRAME_END_ID3) {
4727		- if ((frm_stat & DVP_CHANNEL3_FRM_READ) ==
4728		- DVP_CHANNEL3_FRM_READ)
4729		- v4l2_warn(v4l2_dev, "frame0/1 trigger simultaneously in DVP ID3\n");
4730	8208	*intstat &= ~DVP_FRAME_END_ID3;
4731	8209	return RKCIF_STREAM_MIPI_ID3;
4732	8210	}
	8211	+
	8212	+ return -EINVAL;
	8213	+}
	8214	+
	8215	+static int rkcif_dvp_g_ch_id_by_fe(struct v4l2_device *v4l2_dev,
	8216	+ u32 intstat)
	8217	+{
	8218	+ if (intstat & DVP_ALL_END_ID0)
	8219	+ return RKCIF_STREAM_MIPI_ID0;
	8220	+
	8221	+ if (intstat & DVP_ALL_END_ID1)
	8222	+ return RKCIF_STREAM_MIPI_ID1;
	8223	+
	8224	+ if (intstat & DVP_ALL_END_ID2)
	8225	+ return RKCIF_STREAM_MIPI_ID2;
	8226	+
	8227	+ if (intstat & DVP_ALL_END_ID3)
	8228	+ return RKCIF_STREAM_MIPI_ID3;
4733	8229
4734	8230	return -EINVAL;
4735	8231	}
..	..	@@ -4740,7 +8236,6 @@
4740	8236	struct rkcif_device,
4741	8237	reset_watchdog_timer);
4742	8238	struct rkcif_stream *stream = &dev->stream[RKCIF_STREAM_MIPI_ID0];
4743		- struct rkcif_hw_timer *hw_timer = &dev->hw_dev->hw_timer;
4744	8239	bool is_triggered = false, is_assign_triggered = false, is_first_err = false;
4745	8240	unsigned long flags;
4746	8241	u64 cur_time, diff_time;
..	..	@@ -4751,11 +8246,11 @@
4751	8246	timer->csi2_err_cnt_odd != 0) {
4752	8247	timer->csi2_err_cnt_odd = 0;
4753	8248	timer->csi2_err_cnt_even = 0;
4754		- hw_timer->reset_src = RKCIF_RESET_SRC_ERR_CSI2;
	8249	+ timer->reset_src = RKCIF_RESET_SRC_ERR_CSI2;
4755	8250	timer->csi2_err_triggered_cnt++;
4756	8251	if (timer->csi2_err_triggered_cnt == 1) {
4757	8252	is_first_err = true;
4758		- timer->csi2_first_err_timestamp = ktime_get_ns();
	8253	+ timer->csi2_first_err_timestamp = rkcif_time_get_ns(dev);
4759	8254	}
4760	8255
4761	8256	is_assign_triggered = true;
..	..	@@ -4767,10 +8262,10 @@
4767	8262
4768	8263	if (!is_first_err) {
4769	8264	if (timer->csi2_err_triggered_cnt >= 1) {
4770		- cur_time = ktime_get_ns();
	8265	+ cur_time = rkcif_time_get_ns(dev);
4771	8266	diff_time = cur_time - timer->csi2_first_err_timestamp;
4772	8267	diff_time = div_u64(diff_time, 1000000);
4773		- if (diff_time >= hw_timer->err_time_interval) {
	8268	+ if (diff_time >= timer->err_time_interval) {
4774	8269	is_triggered = true;
4775	8270	v4l2_info(&dev->v4l2_dev, "trigger reset for time out of csi err\n");
4776	8271	goto end_judge;
..	..	@@ -4790,20 +8285,10 @@
4790	8285	* when fs cnt is beyond 2, it indicates that frame end is not coming,
4791	8286	* or fs and fe had been not paired.
4792	8287	*/
4793		- if (hw_timer->monitor_mode != RKCIF_MONITOR_MODE_HOTPLUG &&
4794		- (stream->is_fs_fe_not_paired \|\|
4795		- stream->fs_cnt_in_single_frame > RKCIF_FS_DETECTED_NUM)) {
	8288	+ if (stream->is_fs_fe_not_paired \|\|
	8289	+ stream->fs_cnt_in_single_frame > RKCIF_FS_DETECTED_NUM) {
4796	8290	is_triggered = true;
4797	8291	v4l2_info(&dev->v4l2_dev, "reset for fs & fe not paired\n");
4798		- }
4799		- if (dev->irq_stats.csi_bwidth_lack_cnt) {
4800		- is_triggered = true;
4801		- dev->irq_stats.csi_bwidth_lack_cnt = 0;
4802		- v4l2_info(&dev->v4l2_dev, "reset for bandwidth lack\n");
4803		- }
4804		- if (dev->irq_stats.csi_overflow_cnt) {
4805		- is_triggered = true;
4806		- dev->irq_stats.csi_overflow_cnt = 0;
4807	8292	}
4808	8293	end_judge:
4809	8294	spin_unlock_irqrestore(&timer->csi2_err_lock, flags);
..	..	@@ -4811,7 +8296,30 @@
4811	8296	return is_triggered;
4812	8297	}
4813	8298
4814		-static s32 rkcif_get_sensor_vblank(struct rkcif_device *dev)
	8299	+static bool rkcif_is_triggered_monitoring(struct rkcif_device *dev)
	8300	+{
	8301	+ struct rkcif_timer *timer = &dev->reset_watchdog_timer;
	8302	+ struct rkcif_stream *stream = &dev->stream[RKCIF_STREAM_MIPI_ID0];
	8303	+ bool ret = false;
	8304	+
	8305	+ if (timer->monitor_mode == RKCIF_MONITOR_MODE_IDLE)
	8306	+ ret = false;
	8307	+
	8308	+ if (timer->monitor_mode == RKCIF_MONITOR_MODE_CONTINUE \|\|
	8309	+ timer->monitor_mode == RKCIF_MONITOR_MODE_HOTPLUG) {
	8310	+ if (stream->frame_idx >= timer->triggered_frame_num)
	8311	+ ret = true;
	8312	+ }
	8313	+
	8314	+ if (timer->monitor_mode == RKCIF_MONITOR_MODE_TRIGGER) {
	8315	+ timer->is_csi2_err_occurred = rkcif_is_csi2_err_trigger_reset(timer);
	8316	+ ret = timer->is_csi2_err_occurred;
	8317	+ }
	8318	+
	8319	+ return ret;
	8320	+}
	8321	+
	8322	+s32 rkcif_get_sensor_vblank(struct rkcif_device *dev)
4815	8323	{
4816	8324	struct rkcif_sensor_info *terminal_sensor = &dev->terminal_sensor;
4817	8325	struct v4l2_subdev *sd = terminal_sensor->sd;
..	..	@@ -4828,7 +8336,7 @@
4828	8336	return 0;
4829	8337	}
4830	8338
4831		-static s32 rkcif_get_sensor_vblank_def(struct rkcif_device *dev)
	8339	+s32 rkcif_get_sensor_vblank_def(struct rkcif_device *dev)
4832	8340	{
4833	8341	struct rkcif_sensor_info *terminal_sensor = &dev->terminal_sensor;
4834	8342	struct v4l2_subdev *sd = terminal_sensor->sd;
..	..	@@ -4854,7 +8362,7 @@
4854	8362	const struct cif_output_fmt *fmt;
4855	8363	u32 fourcc;
4856	8364
4857		- fmt = find_output_fmt(stream, stream->pixm.pixelformat);
	8365	+ fmt = rkcif_find_output_fmt(stream, stream->pixm.pixelformat);
4858	8366	if (!fmt) {
4859	8367	v4l2_err(&dev->v4l2_dev, "can not find output format: 0x%x",
4860	8368	stream->pixm.pixelformat);
..	..	@@ -4870,13 +8378,14 @@
4870	8378	* writing of ddr, aliged with 256
4871	8379	*/
4872	8380	if (fmt->fmt_type == CIF_FMT_TYPE_RAW && stream->is_compact &&
4873		- fmt->csi_fmt_val != CSI_WRDDR_TYPE_RGB888) {
	8381	+ fmt->csi_fmt_val != CSI_WRDDR_TYPE_RGB888 &&
	8382	+ fmt->csi_fmt_val != CSI_WRDDR_TYPE_RGB565) {
4874	8383	crop_vwidth = ALIGN(raw_width fmt->raw_bpp / 8, 256);
4875	8384	} else {
4876	8385	crop_vwidth = ALIGN(raw_width fmt->bpp[0] / 8, 8);
4877	8386	}
4878	8387
4879		- if (channel->fmt_val == CSI_WRDDR_TYPE_RGB888)
	8388	+ if (channel->fmt_val == CSI_WRDDR_TYPE_RGB888 \|\| channel->fmt_val == CSI_WRDDR_TYPE_RGB565)
4880	8389	crop_width = raw_width fmt->bpp[0] / 8;
4881	8390	/*
4882	8391	* rk cif don't support output yuyv fmt data
..	..	@@ -4904,12 +8413,15 @@
4904	8413	return;
4905	8414
4906	8415	mbus = &cif_dev->active_sensor->mbus;
4907		- if (mbus->type == V4L2_MBUS_CSI2 \|\|
	8416	+ if (mbus->type == V4L2_MBUS_CSI2_DPHY \|\|
	8417	+ mbus->type == V4L2_MBUS_CSI2_CPHY \|\|
4908	8418	mbus->type == V4L2_MBUS_CCP2) {
4909	8419	struct csi_channel_info *channel = &cif_dev->channels[stream->id];
4910	8420
4911	8421	if (channel->fmt_val == CSI_WRDDR_TYPE_RGB888)
4912	8422	crop_x = 3 * stream->crop[CROP_SRC_ACT].left;
	8423	+ else if (channel->fmt_val == CSI_WRDDR_TYPE_RGB565)
	8424	+ crop_x = 2 * stream->crop[CROP_SRC_ACT].left;
4913	8425	else
4914	8426	crop_x = stream->crop[CROP_SRC_ACT].left;
4915	8427
..	..	@@ -4951,15 +8463,8 @@
4951	8463	crop_y << CIF_CROP_Y_SHIFT \| crop_x);
4952	8464
4953	8465	if (stream->cif_fmt_in->fmt_type == CIF_FMT_TYPE_RAW) {
4954		- fmt = find_output_fmt(stream, stream->pixm.pixelformat);
4955		- if (fmt->fourcc == V4L2_PIX_FMT_GREY \|\|
4956		- fmt->fourcc == V4L2_PIX_FMT_SRGGB8 \|\|
4957		- fmt->fourcc == V4L2_PIX_FMT_SGRBG8 \|\|
4958		- fmt->fourcc == V4L2_PIX_FMT_SGBRG8 \|\|
4959		- fmt->fourcc == V4L2_PIX_FMT_SBGGR8)
4960		- crop_vwidth = ALIGN(raw_width * fmt->raw_bpp / 8, 256);
4961		- else
4962		- crop_vwidth = raw_width * rkcif_cal_raw_vir_line_ratio(stream, fmt);
	8466	+ fmt = rkcif_find_output_fmt(stream, stream->pixm.pixelformat);
	8467	+ crop_vwidth = raw_width * rkcif_cal_raw_vir_line_ratio(stream, fmt);
4963	8468	}
4964	8469	rkcif_write_register(cif_dev, CIF_REG_DVP_VIR_LINE_WIDTH, crop_vwidth);
4965	8470
..	..	@@ -4970,41 +8475,94 @@
4970	8475	stream->crop_dyn_en = false;
4971	8476	}
4972	8477
4973		-void rkcif_monitor_reset_event(struct rkcif_hw *hw)
	8478	+static void rkcif_monitor_reset_event(struct rkcif_device *dev)
4974	8479	{
4975		- struct rkcif_hw_timer *hw_timer = &hw->hw_timer;
4976		- struct rkcif_timer *timer = NULL;
4977		- struct rkcif_device *cif_dev = NULL;
4978	8480	struct rkcif_stream *stream = NULL;
4979		- unsigned long flags;
4980		- int i = 0, j = 0;
	8481	+ struct rkcif_timer *timer = &dev->reset_watchdog_timer;
	8482	+ unsigned int cycle = 0;
	8483	+ u64 fps, timestamp0, timestamp1;
	8484	+ unsigned long flags, fps_flags;
	8485	+ int i = 0;
4981	8486
4982		- if (hw_timer->is_running)
	8487	+ if (timer->is_running)
4983	8488	return;
4984	8489
4985		- if (hw_timer->monitor_mode == RKCIF_MONITOR_MODE_IDLE)
	8490	+ if (timer->monitor_mode == RKCIF_MONITOR_MODE_IDLE)
4986	8491	return;
4987	8492
4988		- spin_lock_irqsave(&hw_timer->timer_lock, flags);
4989		- hw_timer->is_running = true;
4990		- for (j = 0; i < hw->dev_num; j++) {
4991		- cif_dev = hw->cif_dev[i];
4992		- timer = &cif_dev->reset_watchdog_timer;
4993		- for (i = 0; i < cif_dev->num_channels; i++) {
4994		- stream = &cif_dev->stream[i];
4995		- if (stream->state == RKCIF_STATE_STREAMING)
4996		- timer->last_buf_wakeup_cnt[i] = stream->frame_idx;
4997		- }
	8493	+ for (i = 0; i < RKCIF_MAX_STREAM_MIPI; i++) {
	8494	+ stream = &dev->stream[i];
	8495	+ if (stream->state == RKCIF_STATE_STREAMING)
	8496	+ break;
4998	8497	}
4999		- hw_timer->cycle_jif = msecs_to_jiffies(hw_timer->monitor_cycle);
5000		- hw_timer->timer.expires = jiffies + hw_timer->cycle_jif;
5001		- mod_timer(&hw_timer->timer, hw_timer->timer.expires);
5002	8498
5003		- spin_unlock_irqrestore(&hw_timer->timer_lock, flags);
	8499	+ if (i >= RKCIF_MAX_STREAM_MIPI)
	8500	+ return;
5004	8501
5005		- v4l2_dbg(1, rkcif_debug, &cif_dev->v4l2_dev,
5006		- "start monitor timer, monitor cycle %d\n",
5007		- hw_timer->monitor_cycle);
	8502	+ timer->is_triggered = rkcif_is_triggered_monitoring(dev);
	8503	+
	8504	+ if (timer->is_triggered) {
	8505	+ struct v4l2_rect *raw_rect = &dev->terminal_sensor.raw_rect;
	8506	+ enum rkcif_monitor_mode mode;
	8507	+ s32 vblank = 0;
	8508	+ u32 vts = 0;
	8509	+ u64 numerator = 0;
	8510	+ u64 denominator = 0;
	8511	+
	8512	+ if (stream->frame_idx > 2) {
	8513	+ spin_lock_irqsave(&stream->fps_lock, fps_flags);
	8514	+ timestamp0 = stream->fps_stats.frm0_timestamp;
	8515	+ timestamp1 = stream->fps_stats.frm1_timestamp;
	8516	+ spin_unlock_irqrestore(&stream->fps_lock, fps_flags);
	8517	+
	8518	+ fps = timestamp0 > timestamp1 ?
	8519	+ timestamp0 - timestamp1 : timestamp1 - timestamp0;
	8520	+ fps = div_u64(fps, 1000);
	8521	+ } else {
	8522	+ numerator = dev->terminal_sensor.fi.interval.numerator;
	8523	+ denominator = dev->terminal_sensor.fi.interval.denominator;
	8524	+ fps = div_u64(1000000 * numerator, denominator);
	8525	+ }
	8526	+ spin_lock_irqsave(&timer->timer_lock, flags);
	8527	+
	8528	+ timer->frame_end_cycle_us = fps;
	8529	+
	8530	+ vblank = rkcif_get_sensor_vblank(dev);
	8531	+ timer->raw_height = raw_rect->height;
	8532	+ vts = timer->raw_height + vblank;
	8533	+ timer->vts = vts;
	8534	+
	8535	+ timer->line_end_cycle = div_u64(timer->frame_end_cycle_us, timer->vts);
	8536	+ fps = div_u64(timer->frame_end_cycle_us, 1000);
	8537	+ cycle = fps * timer->frm_num_of_monitor_cycle;
	8538	+ timer->cycle = msecs_to_jiffies(cycle);
	8539	+
	8540	+ timer->run_cnt = 0;
	8541	+ timer->is_running = true;
	8542	+ timer->is_buf_stop_update = false;
	8543	+ for (i = 0; i < dev->num_channels; i++) {
	8544	+ stream = &dev->stream[i];
	8545	+ if (stream->state == RKCIF_STATE_STREAMING)
	8546	+ timer->last_buf_wakeup_cnt[i] = stream->buf_wake_up_cnt;
	8547	+ }
	8548	+ /* in trigger mode, monitoring count is fps */
	8549	+ mode = timer->monitor_mode;
	8550	+ if (mode == RKCIF_MONITOR_MODE_CONTINUE \|\|
	8551	+ mode == RKCIF_MONITOR_MODE_HOTPLUG)
	8552	+ timer->max_run_cnt = 0xffffffff - CIF_TIMEOUT_FRAME_NUM;
	8553	+ else
	8554	+ timer->max_run_cnt = div_u64(1000, fps) * 1;
	8555	+
	8556	+ timer->timer.expires = jiffies + timer->cycle;
	8557	+ mod_timer(&timer->timer, timer->timer.expires);
	8558	+
	8559	+ spin_unlock_irqrestore(&timer->timer_lock, flags);
	8560	+
	8561	+ v4l2_dbg(3, rkcif_debug, &dev->v4l2_dev,
	8562	+ "%s:mode:%d, raw height:%d,vblank:%d, cycle:%ld, fps:%llu\n",
	8563	+ __func__, timer->monitor_mode, raw_rect->height,
	8564	+ vblank, timer->cycle, div_u64(1000, fps));
	8565	+ }
5008	8566	}
5009	8567
5010	8568	static void rkcif_rdbk_frame_end(struct rkcif_stream *stream)
..	..	@@ -5014,13 +8572,15 @@
5014	8572	u32 denominator, numerator;
5015	8573	u64 l_ts, m_ts, s_ts, time = 30000000LL;
5016	8574	int ret, fps = -1;
	8575	+ int i = 0;
	8576	+ unsigned long flags;
5017	8577
5018		- if (dev->hdr.mode == HDR_X2) {
	8578	+ if (dev->hdr.hdr_mode == HDR_X2) {
5019	8579	if (stream->id != RKCIF_STREAM_MIPI_ID1 \|\|
5020	8580	dev->stream[RKCIF_STREAM_MIPI_ID0].state != RKCIF_STATE_STREAMING \|\|
5021	8581	dev->stream[RKCIF_STREAM_MIPI_ID1].state != RKCIF_STATE_STREAMING)
5022	8582	return;
5023		- } else if (dev->hdr.mode == HDR_X3) {
	8583	+ } else if (dev->hdr.hdr_mode == HDR_X3) {
5024	8584	if (stream->id != RKCIF_STREAM_MIPI_ID2 \|\|
5025	8585	dev->stream[RKCIF_STREAM_MIPI_ID0].state != RKCIF_STATE_STREAMING \|\|
5026	8586	dev->stream[RKCIF_STREAM_MIPI_ID1].state != RKCIF_STATE_STREAMING \|\|
..	..	@@ -5033,19 +8593,19 @@
5033	8593	if (denominator && numerator)
5034	8594	time = numerator * 1000 / denominator * 1000 * 1000;
5035	8595
5036		- if (dev->hdr.mode == HDR_X3) {
	8596	+ if (dev->hdr.hdr_mode == HDR_X3) {
5037	8597	if (dev->rdbk_buf[RDBK_L] &&
5038	8598	dev->rdbk_buf[RDBK_M] &&
5039	8599	dev->rdbk_buf[RDBK_S]) {
5040		- l_ts = dev->rdbk_buf[RDBK_L]->vb.vb2_buf.timestamp;
5041		- m_ts = dev->rdbk_buf[RDBK_M]->vb.vb2_buf.timestamp;
5042		- s_ts = dev->rdbk_buf[RDBK_S]->vb.vb2_buf.timestamp;
	8600	+ l_ts = dev->rdbk_buf[RDBK_L]->fe_timestamp;
	8601	+ m_ts = dev->rdbk_buf[RDBK_M]->fe_timestamp;
	8602	+ s_ts = dev->rdbk_buf[RDBK_S]->fe_timestamp;
5043	8603
5044	8604	if (m_ts < l_ts \|\| s_ts < m_ts) {
5045	8605	v4l2_err(&dev->v4l2_dev,
5046	8606	"s/m/l frame err, timestamp s:%lld m:%lld l:%lld\n",
5047	8607	s_ts, m_ts, l_ts);
5048		- goto RDBK_FRM_UNMATCH;
	8608	+ return;
5049	8609	}
5050	8610
5051	8611	if ((m_ts - l_ts) > time \|\| (s_ts - m_ts) > time) {
..	..	@@ -5066,17 +8626,28 @@
5066	8626	v4l2_err(&dev->v4l2_dev,
5067	8627	"timestamp no match, s:%lld m:%lld l:%lld, fps:%d\n",
5068	8628	s_ts, m_ts, l_ts, fps);
5069		- goto RDBK_FRM_UNMATCH;
	8629	+ return;
5070	8630	}
5071	8631	}
5072		- dev->rdbk_buf[RDBK_L]->vb.sequence = dev->rdbk_buf[RDBK_S]->vb.sequence;
5073		- dev->rdbk_buf[RDBK_M]->vb.sequence = dev->rdbk_buf[RDBK_S]->vb.sequence;
5074		- rkcif_vb_done_oneframe(&dev->stream[RKCIF_STREAM_MIPI_ID0],
5075		- &dev->rdbk_buf[RDBK_L]->vb);
5076		- rkcif_vb_done_oneframe(&dev->stream[RKCIF_STREAM_MIPI_ID1],
5077		- &dev->rdbk_buf[RDBK_M]->vb);
5078		- rkcif_vb_done_oneframe(&dev->stream[RKCIF_STREAM_MIPI_ID2],
5079		- &dev->rdbk_buf[RDBK_S]->vb);
	8632	+ dev->rdbk_buf[RDBK_M]->vb.sequence = dev->rdbk_buf[RDBK_L]->vb.sequence;
	8633	+ dev->rdbk_buf[RDBK_S]->vb.sequence = dev->rdbk_buf[RDBK_L]->vb.sequence;
	8634	+ if (dev->is_support_tools &&
	8635	+ dev->stream[RKCIF_STREAM_MIPI_ID0].tools_vdev->state == RKCIF_STATE_STREAMING &&
	8636	+ dev->stream[RKCIF_STREAM_MIPI_ID1].tools_vdev->state == RKCIF_STATE_STREAMING &&
	8637	+ dev->stream[RKCIF_STREAM_MIPI_ID2].tools_vdev->state == RKCIF_STATE_STREAMING) {
	8638	+ for (i = 0; i < 3; i++) {
	8639	+ spin_lock_irqsave(&dev->stream[i].tools_vdev->vbq_lock, flags);
	8640	+ list_add_tail(&dev->rdbk_buf[i]->queue,
	8641	+ &dev->stream[i].tools_vdev->buf_done_head);
	8642	+ if (!work_busy(&dev->stream[i].tools_vdev->work))
	8643	+ schedule_work(&dev->stream[i].tools_vdev->work);
	8644	+ spin_unlock_irqrestore(&dev->stream[i].tools_vdev->vbq_lock, flags);
	8645	+ }
	8646	+ } else {
	8647	+ rkcif_vb_done_tasklet(&dev->stream[RKCIF_STREAM_MIPI_ID0], dev->rdbk_buf[RDBK_L]);
	8648	+ rkcif_vb_done_tasklet(&dev->stream[RKCIF_STREAM_MIPI_ID1], dev->rdbk_buf[RDBK_M]);
	8649	+ rkcif_vb_done_tasklet(&dev->stream[RKCIF_STREAM_MIPI_ID2], dev->rdbk_buf[RDBK_S]);
	8650	+ }
5080	8651	} else {
5081	8652	if (!dev->rdbk_buf[RDBK_L])
5082	8653	v4l2_err(&dev->v4l2_dev, "lost long frames\n");
..	..	@@ -5084,18 +8655,18 @@
5084	8655	v4l2_err(&dev->v4l2_dev, "lost medium frames\n");
5085	8656	if (!dev->rdbk_buf[RDBK_S])
5086	8657	v4l2_err(&dev->v4l2_dev, "lost short frames\n");
5087		- goto RDBK_FRM_UNMATCH;
	8658	+ return;
5088	8659	}
5089		- } else if (dev->hdr.mode == HDR_X2) {
	8660	+ } else if (dev->hdr.hdr_mode == HDR_X2) {
5090	8661	if (dev->rdbk_buf[RDBK_L] && dev->rdbk_buf[RDBK_M]) {
5091		- l_ts = dev->rdbk_buf[RDBK_L]->vb.vb2_buf.timestamp;
5092		- s_ts = dev->rdbk_buf[RDBK_M]->vb.vb2_buf.timestamp;
	8662	+ l_ts = dev->rdbk_buf[RDBK_L]->fe_timestamp;
	8663	+ s_ts = dev->rdbk_buf[RDBK_M]->fe_timestamp;
5093	8664
5094	8665	if (s_ts < l_ts) {
5095	8666	v4l2_err(&dev->v4l2_dev,
5096	8667	"s/l frame err, timestamp s:%lld l:%lld\n",
5097	8668	s_ts, l_ts);
5098		- goto RDBK_FRM_UNMATCH;
	8669	+ return;
5099	8670	}
5100	8671
5101	8672	if ((s_ts - l_ts) > time) {
..	..	@@ -5115,42 +8686,32 @@
5115	8686	v4l2_err(&dev->v4l2_dev,
5116	8687	"timestamp no match, s:%lld l:%lld, fps:%d\n",
5117	8688	s_ts, l_ts, fps);
5118		- goto RDBK_FRM_UNMATCH;
	8689	+ return;
5119	8690	}
5120	8691	}
5121		- dev->rdbk_buf[RDBK_L]->vb.sequence = dev->rdbk_buf[RDBK_M]->vb.sequence;
5122		- rkcif_vb_done_oneframe(&dev->stream[RKCIF_STREAM_MIPI_ID0],
5123		- &dev->rdbk_buf[RDBK_L]->vb);
5124		- rkcif_vb_done_oneframe(&dev->stream[RKCIF_STREAM_MIPI_ID1],
5125		- &dev->rdbk_buf[RDBK_M]->vb);
	8692	+ dev->rdbk_buf[RDBK_M]->vb.sequence = dev->rdbk_buf[RDBK_L]->vb.sequence;
	8693	+ if (dev->is_support_tools &&
	8694	+ dev->stream[RKCIF_STREAM_MIPI_ID0].tools_vdev->state == RKCIF_STATE_STREAMING &&
	8695	+ dev->stream[RKCIF_STREAM_MIPI_ID1].tools_vdev->state == RKCIF_STATE_STREAMING) {
	8696	+ for (i = 0; i < 2; i++) {
	8697	+ spin_lock_irqsave(&dev->stream[i].tools_vdev->vbq_lock, flags);
	8698	+ list_add_tail(&dev->rdbk_buf[i]->queue,
	8699	+ &dev->stream[i].tools_vdev->buf_done_head);
	8700	+ if (!work_busy(&dev->stream[i].tools_vdev->work))
	8701	+ schedule_work(&dev->stream[i].tools_vdev->work);
	8702	+ spin_unlock_irqrestore(&dev->stream[i].tools_vdev->vbq_lock, flags);
	8703	+ }
	8704	+ } else {
	8705	+ rkcif_vb_done_tasklet(&dev->stream[RKCIF_STREAM_MIPI_ID0], dev->rdbk_buf[RDBK_L]);
	8706	+ rkcif_vb_done_tasklet(&dev->stream[RKCIF_STREAM_MIPI_ID1], dev->rdbk_buf[RDBK_M]);
	8707	+ }
5126	8708	} else {
5127	8709	if (!dev->rdbk_buf[RDBK_L])
5128	8710	v4l2_err(&dev->v4l2_dev, "lost long frames\n");
5129	8711	if (!dev->rdbk_buf[RDBK_M])
5130	8712	v4l2_err(&dev->v4l2_dev, "lost short frames\n");
5131		- goto RDBK_FRM_UNMATCH;
	8713	+ return;
5132	8714	}
5133		- } else {
5134		- rkcif_vb_done_oneframe(stream, &dev->rdbk_buf[RDBK_S]->vb);
5135		- }
5136		-
5137		- dev->rdbk_buf[RDBK_L] = NULL;
5138		- dev->rdbk_buf[RDBK_M] = NULL;
5139		- dev->rdbk_buf[RDBK_S] = NULL;
5140		- return;
5141		-
5142		-RDBK_FRM_UNMATCH:
5143		- if (dev->rdbk_buf[RDBK_L]) {
5144		- dev->rdbk_buf[RDBK_L]->vb.vb2_buf.state = VB2_BUF_STATE_ACTIVE;
5145		- rkcif_buf_queue(&dev->rdbk_buf[RDBK_L]->vb.vb2_buf);
5146		- }
5147		- if (dev->rdbk_buf[RDBK_M]) {
5148		- dev->rdbk_buf[RDBK_M]->vb.vb2_buf.state = VB2_BUF_STATE_ACTIVE;
5149		- rkcif_buf_queue(&dev->rdbk_buf[RDBK_M]->vb.vb2_buf);
5150		- }
5151		- if (dev->rdbk_buf[RDBK_S]) {
5152		- dev->rdbk_buf[RDBK_S]->vb.vb2_buf.state = VB2_BUF_STATE_ACTIVE;
5153		- rkcif_buf_queue(&dev->rdbk_buf[RDBK_S]->vb.vb2_buf);
5154	8715	}
5155	8716
5156	8717	dev->rdbk_buf[RDBK_L] = NULL;
..	..	@@ -5158,10 +8719,26 @@
5158	8719	dev->rdbk_buf[RDBK_S] = NULL;
5159	8720	}
5160	8721
	8722	+static void rkcif_buf_done_with_tools(struct rkcif_stream *stream,
	8723	+ struct rkcif_buffer *active_buf)
	8724	+{
	8725	+ unsigned long flags;
	8726	+
	8727	+ spin_lock_irqsave(&stream->tools_vdev->vbq_lock, flags);
	8728	+ if (stream->tools_vdev->state == RKCIF_STATE_STREAMING) {
	8729	+ list_add_tail(&active_buf->queue, &stream->tools_vdev->buf_done_head);
	8730	+ if (!work_busy(&stream->tools_vdev->work))
	8731	+ schedule_work(&stream->tools_vdev->work);
	8732	+ } else {
	8733	+ rkcif_vb_done_tasklet(stream, active_buf);
	8734	+ }
	8735	+ spin_unlock_irqrestore(&stream->tools_vdev->vbq_lock, flags);
	8736	+}
	8737	+
5161	8738	static void rkcif_buf_done_prepare(struct rkcif_stream *stream,
5162		- struct rkcif_buffer *active_buf,
5163		- int mipi_id,
5164		- u32 mode)
	8739	+ struct rkcif_buffer *active_buf,
	8740	+ int mipi_id,
	8741	+ u32 mode)
5165	8742	{
5166	8743	unsigned long flags;
5167	8744	struct vb2_v4l2_buffer *vb_done = NULL;
..	..	@@ -5169,8 +8746,13 @@
5169	8746
5170	8747	if (active_buf) {
5171	8748	vb_done = &active_buf->vb;
5172		- vb_done->vb2_buf.timestamp = ktime_get_ns();
5173		- vb_done->sequence = stream->frame_idx;
	8749	+ if (cif_dev->chip_id < CHIP_RK3588_CIF &&
	8750	+ cif_dev->active_sensor->mbus.type == V4L2_MBUS_BT656)
	8751	+ vb_done->vb2_buf.timestamp = stream->readout.fe_timestamp;
	8752	+ else
	8753	+ vb_done->vb2_buf.timestamp = stream->readout.fs_timestamp;
	8754	+ vb_done->sequence = stream->frame_idx - 1;
	8755	+ active_buf->fe_timestamp = rkcif_time_get_ns(cif_dev);
5174	8756	if (stream->is_line_wake_up) {
5175	8757	spin_lock_irqsave(&stream->fps_lock, flags);
5176	8758	if (mode)
..	..	@@ -5182,58 +8764,83 @@
5182	8764	}
5183	8765	if (stream->cif_fmt_in->field == V4L2_FIELD_INTERLACED)
5184	8766	vb_done->sequence /= 2;
	8767	+ if (stream->cur_skip_frame) {
	8768	+ rkcif_buf_queue(&active_buf->vb.vb2_buf);
	8769	+ return;
	8770	+ }
	8771	+ } else if (cif_dev->rdbk_buf[stream->id]) {
	8772	+ vb_done = &cif_dev->rdbk_buf[stream->id]->vb;
	8773	+ if (cif_dev->chip_id < CHIP_RK3588_CIF &&
	8774	+ cif_dev->active_sensor->mbus.type == V4L2_MBUS_BT656)
	8775	+ vb_done->vb2_buf.timestamp = stream->readout.fe_timestamp;
	8776	+ else
	8777	+ vb_done->vb2_buf.timestamp = stream->readout.fs_timestamp;
	8778	+ vb_done->sequence = stream->frame_idx - 1;
	8779	+ cif_dev->rdbk_buf[stream->id]->fe_timestamp = rkcif_time_get_ns(cif_dev);
5185	8780	}
5186	8781
5187		- if (cif_dev->hdr.mode == NO_HDR) {
	8782	+ if (cif_dev->hdr.hdr_mode == NO_HDR \|\| cif_dev->hdr.hdr_mode == HDR_COMPR) {
5188	8783	if (stream->cif_fmt_in->field == V4L2_FIELD_INTERLACED) {
5189		- if (stream->frame_phase == CIF_CSI_FRAME1_READY && active_buf)
5190		- rkcif_vb_done_oneframe(stream, vb_done);
	8784	+ if (stream->frame_phase == CIF_CSI_FRAME1_READY && active_buf) {
	8785	+
	8786	+ if (cif_dev->is_support_tools && stream->tools_vdev)
	8787	+ rkcif_buf_done_with_tools(stream, active_buf);
	8788	+ else
	8789	+ rkcif_vb_done_tasklet(stream, active_buf);
	8790	+ }
5191	8791	} else {
5192		- if (active_buf)
5193		- rkcif_vb_done_oneframe(stream, vb_done);
	8792	+ if (active_buf) {
	8793	+ if (cif_dev->is_support_tools && stream->tools_vdev)
	8794	+ rkcif_buf_done_with_tools(stream, active_buf);
	8795	+ else
	8796	+ rkcif_vb_done_tasklet(stream, active_buf);
	8797	+ }
5194	8798	}
5195	8799	} else {
5196	8800	if (cif_dev->is_start_hdr) {
5197	8801	spin_lock_irqsave(&cif_dev->hdr_lock, flags);
5198	8802	if (mipi_id == RKCIF_STREAM_MIPI_ID0) {
5199		- if (cif_dev->rdbk_buf[RDBK_L]) {
	8803	+ if (cif_dev->rdbk_buf[RDBK_L] && active_buf) {
5200	8804	v4l2_err(&cif_dev->v4l2_dev,
5201	8805	"multiple long data in %s frame,frm_idx:%d,state:0x%x\n",
5202		- cif_dev->hdr.mode == HDR_X2 ? "hdr_x2" : "hdr_x3",
	8806	+ cif_dev->hdr.hdr_mode == HDR_X2 ? "hdr_x2" : "hdr_x3",
5203	8807	stream->frame_idx,
5204	8808	cif_dev->rdbk_buf[RDBK_L]->vb.vb2_buf.state);
5205	8809	cif_dev->rdbk_buf[RDBK_L]->vb.vb2_buf.state = VB2_BUF_STATE_ACTIVE;
5206	8810	rkcif_buf_queue(&cif_dev->rdbk_buf[RDBK_L]->vb.vb2_buf);
	8811	+ cif_dev->rdbk_buf[RDBK_L] = NULL;
5207	8812	}
5208	8813	if (active_buf)
5209	8814	cif_dev->rdbk_buf[RDBK_L] = active_buf;
5210	8815	} else if (mipi_id == RKCIF_STREAM_MIPI_ID1) {
5211		- if (cif_dev->rdbk_buf[RDBK_M]) {
	8816	+ if (cif_dev->rdbk_buf[RDBK_M] && active_buf) {
5212	8817	v4l2_err(&cif_dev->v4l2_dev,
5213	8818	"multiple %s frame,frm_idx:%d,state:0x%x\n",
5214		- cif_dev->hdr.mode == HDR_X2 ? "short data in hdr_x2" : "medium data in hdr_x3",
	8819	+ cif_dev->hdr.hdr_mode == HDR_X2 ? "short data in hdr_x2" : "medium data in hdr_x3",
5215	8820	stream->frame_idx,
5216	8821	cif_dev->rdbk_buf[RDBK_M]->vb.vb2_buf.state);
5217	8822	cif_dev->rdbk_buf[RDBK_M]->vb.vb2_buf.state = VB2_BUF_STATE_ACTIVE;
5218	8823	rkcif_buf_queue(&cif_dev->rdbk_buf[RDBK_M]->vb.vb2_buf);
	8824	+ cif_dev->rdbk_buf[RDBK_M] = NULL;
5219	8825	}
5220	8826	if (active_buf)
5221	8827	cif_dev->rdbk_buf[RDBK_M] = active_buf;
5222		- if (cif_dev->hdr.mode == HDR_X2)
	8828	+ if (cif_dev->hdr.hdr_mode == HDR_X2)
5223	8829	rkcif_rdbk_frame_end(stream);
5224	8830	} else if (mipi_id == RKCIF_STREAM_MIPI_ID2) {
5225		- if (cif_dev->rdbk_buf[RDBK_S]) {
	8831	+ if (cif_dev->rdbk_buf[RDBK_S] && active_buf) {
5226	8832	v4l2_err(&cif_dev->v4l2_dev,
5227	8833	"multiple %s frame, frm_idx:%d,state:0x%x\n",
5228		- cif_dev->hdr.mode == HDR_X2 ? "err short data in hdr_x3" : "short data in hdr_x3",
	8834	+ cif_dev->hdr.hdr_mode == HDR_X2 ? "err short data in hdr_x3" : "short data in hdr_x3",
5229	8835	stream->frame_idx,
5230	8836	cif_dev->rdbk_buf[RDBK_S]->vb.vb2_buf.state);
5231	8837	cif_dev->rdbk_buf[RDBK_S]->vb.vb2_buf.state = VB2_BUF_STATE_ACTIVE;
5232	8838	rkcif_buf_queue(&cif_dev->rdbk_buf[RDBK_S]->vb.vb2_buf);
	8839	+ cif_dev->rdbk_buf[RDBK_S] = NULL;
5233	8840	}
5234	8841	if (active_buf)
5235	8842	cif_dev->rdbk_buf[RDBK_S] = active_buf;
5236		- if (cif_dev->hdr.mode == HDR_X3)
	8843	+ if (cif_dev->hdr.hdr_mode == HDR_X3)
5237	8844	rkcif_rdbk_frame_end(stream);
5238	8845	}
5239	8846	spin_unlock_irqrestore(&cif_dev->hdr_lock, flags);
..	..	@@ -5276,12 +8883,129 @@
5276	8883	}
5277	8884	ret = rkcif_get_new_buffer_wake_up_mode(stream);
5278	8885	if (ret)
5279		- goto end_wake_up;
5280		-
	8886	+ return;
5281	8887	rkcif_buf_done_prepare(stream, active_buf, mipi_id, mode);
5282		-end_wake_up:
5283		- stream->frame_idx++;
	8888	+}
5284	8889
	8890	+static void rkcif_store_last_buf_for_online(struct rkcif_stream *stream,
	8891	+ struct rkcif_rx_buffer *buf)
	8892	+{
	8893	+ struct rkcif_device *dev = stream->cifdev;
	8894	+ struct v4l2_mbus_config *mbus_cfg = &dev->active_sensor->mbus;
	8895	+ u32 frm0_addr_y, frm1_addr_y;
	8896	+
	8897	+ INIT_LIST_HEAD(&stream->rx_buf_head);
	8898	+ stream->curr_buf_toisp = buf;
	8899	+ stream->next_buf_toisp = buf;
	8900	+ if (mbus_cfg->type == V4L2_MBUS_CSI2_DPHY \|\|
	8901	+ mbus_cfg->type == V4L2_MBUS_CSI2_CPHY \|\|
	8902	+ mbus_cfg->type == V4L2_MBUS_CCP2) {
	8903	+ frm0_addr_y = get_reg_index_of_frm0_y_addr(stream->id);
	8904	+ frm1_addr_y = get_reg_index_of_frm1_y_addr(stream->id);
	8905	+ } else {
	8906	+ frm0_addr_y = get_dvp_reg_index_of_frm0_y_addr(stream->id);
	8907	+ frm1_addr_y = get_dvp_reg_index_of_frm1_y_addr(stream->id);
	8908	+ }
	8909	+ rkcif_write_register(dev, frm0_addr_y,
	8910	+ buf->dummy.dma_addr);
	8911	+ rkcif_write_register(dev, frm1_addr_y,
	8912	+ buf->dummy.dma_addr);
	8913	+}
	8914	+
	8915	+static void rkcif_release_unnecessary_buf_for_online(struct rkcif_stream *stream,
	8916	+ struct rkcif_rx_buffer *buf)
	8917	+{
	8918	+ struct rkcif_device *dev = stream->cifdev;
	8919	+ struct sditf_priv *priv = dev->sditf[0];
	8920	+ struct rkcif_rx_buffer *rx_buf = NULL;
	8921	+ unsigned long flags;
	8922	+ int i = 0;
	8923	+
	8924	+ spin_lock_irqsave(&priv->cif_dev->buffree_lock, flags);
	8925	+ for (i = 0; i < priv->buf_num; i++) {
	8926	+ rx_buf = &stream->rx_buf[i];
	8927	+ if (rx_buf && (!rx_buf->dummy.is_free) && rx_buf != buf) {
	8928	+ list_add_tail(&rx_buf->list_free, &priv->buf_free_list);
	8929	+ stream->total_buf_num--;
	8930	+ }
	8931	+ }
	8932	+ spin_unlock_irqrestore(&priv->cif_dev->buffree_lock, flags);
	8933	+ schedule_work(&priv->buffree_work.work);
	8934	+}
	8935	+
	8936	+static void rkcif_line_wake_up_rdbk(struct rkcif_stream *stream, int mipi_id)
	8937	+{
	8938	+ u32 mode;
	8939	+ struct rkcif_rx_buffer *active_buf = NULL;
	8940	+ struct sditf_priv *priv = NULL;
	8941	+ unsigned long flags;
	8942	+ int ret = 0;
	8943	+
	8944	+ mode = stream->line_int_cnt % 2;
	8945	+ if (mode) {
	8946	+ if (stream->curr_buf_toisp)
	8947	+ active_buf = stream->curr_buf_toisp;
	8948	+ stream->frame_phase = CIF_CSI_FRAME0_READY;
	8949	+ } else {
	8950	+ if (stream->next_buf_toisp)
	8951	+ active_buf = stream->next_buf_toisp;
	8952	+ stream->frame_phase = CIF_CSI_FRAME1_READY;
	8953	+ }
	8954	+
	8955	+ if (!active_buf) {
	8956	+ v4l2_err(&stream->cifdev->v4l2_dev,
	8957	+ "err buffer state in %s\n",
	8958	+ __func__);
	8959	+ return;
	8960	+ }
	8961	+
	8962	+ if (stream->stopping) {
	8963	+ stream->is_can_stop = true;
	8964	+ return;
	8965	+ }
	8966	+ ret = rkcif_get_new_buffer_wake_up_mode_rdbk(stream);
	8967	+ v4l2_dbg(3, rkcif_debug, &stream->cifdev->v4l2_dev,
	8968	+ "%d frame_idx %d, last_rx_buf_idx %d cur dma buf %x\n",
	8969	+ __LINE__, stream->frame_idx, stream->last_rx_buf_idx,
	8970	+ (u32)active_buf->dummy.dma_addr);
	8971	+ if (!ret) {
	8972	+ priv = stream->cifdev->sditf[0];
	8973	+ if (stream->cur_stream_mode & RKCIF_STREAM_MODE_TOISP_RDBK) {
	8974	+ spin_lock_irqsave(&stream->vbq_lock, flags);
	8975	+ if (stream->cifdev->is_thunderboot &&
	8976	+ (stream->frame_idx - 1) == stream->last_rx_buf_idx &&
	8977	+ stream->cifdev->is_rdbk_to_online) {
	8978	+ stream->cur_stream_mode &= ~RKCIF_STREAM_MODE_TOISP_RDBK;
	8979	+ stream->cur_stream_mode \|= RKCIF_STREAM_MODE_TOISP;
	8980	+ stream->cifdev->wait_line = 0;
	8981	+ stream->is_line_wake_up = false;
	8982	+ if (stream->cifdev->hdr.hdr_mode == NO_HDR \|\|
	8983	+ (priv->hdr_cfg.hdr_mode == HDR_X2 && stream->id == 1) \|\|
	8984	+ (priv->hdr_cfg.hdr_mode == HDR_X3 && stream->id == 2)) {
	8985	+ stream->to_stop_dma = RKCIF_DMAEN_BY_ISP;
	8986	+ rkcif_stop_dma_capture(stream);
	8987	+ }
	8988	+ active_buf->dbufs.is_switch = true;
	8989	+ if ((priv->hdr_cfg.hdr_mode == HDR_X2 && stream->id != 1) \|\|
	8990	+ (priv->hdr_cfg.hdr_mode == HDR_X3 && stream->id != 2)) {
	8991	+ rkcif_store_last_buf_for_online(stream, active_buf);
	8992	+ stream->is_change_toisp = true;
	8993	+ }
	8994	+ }
	8995	+ spin_unlock_irqrestore(&stream->vbq_lock, flags);
	8996	+ active_buf->dbufs.sequence = stream->frame_idx - 1;
	8997	+ active_buf->dbufs.timestamp = stream->readout.fs_timestamp;
	8998	+ active_buf->fe_timestamp = rkcif_time_get_ns(stream->cifdev);
	8999	+ stream->last_frame_idx = stream->frame_idx;
	9000	+ if (stream->cifdev->hdr.hdr_mode == NO_HDR) {
	9001	+ rkcif_s_rx_buffer(stream, &active_buf->dbufs);
	9002	+ if (stream->cifdev->is_support_tools && stream->tools_vdev)
	9003	+ rkcif_rdbk_with_tools(stream, active_buf);
	9004	+ } else {
	9005	+ rkcif_rdbk_frame_end_toisp(stream, active_buf);
	9006	+ }
	9007	+ }
	9008	+ }
5285	9009	}
5286	9010
5287	9011	static void rkcif_deal_readout_time(struct rkcif_stream *stream)
..	..	@@ -5291,18 +9015,25 @@
5291	9015	unsigned long flags;
5292	9016
5293	9017	spin_lock_irqsave(&stream->fps_lock, flags);
5294		- stream->readout.fe_timestamp = ktime_get_ns();
	9018	+ stream->readout.fe_timestamp = rkcif_time_get_ns(cif_dev);
	9019	+
	9020	+ if (cif_dev->inf_id == RKCIF_DVP) {
	9021	+ spin_unlock_irqrestore(&stream->fps_lock, flags);
	9022	+ return;
	9023	+ }
	9024	+
5295	9025	if (stream->id == RKCIF_STREAM_MIPI_ID0)
5296	9026	detect_stream->readout.readout_time = stream->readout.fe_timestamp - stream->readout.fs_timestamp;
5297	9027
5298		- if ((cif_dev->hdr.mode == NO_HDR) && (stream->id == RKCIF_STREAM_MIPI_ID0)) {
	9028	+ if ((cif_dev->hdr.hdr_mode == NO_HDR \|\| cif_dev->hdr.hdr_mode == HDR_COMPR) &&
	9029	+ (stream->id == RKCIF_STREAM_MIPI_ID0)) {
5299	9030	detect_stream->readout.early_time = stream->readout.fe_timestamp - stream->readout.wk_timestamp;
5300	9031
5301		- } else if ((cif_dev->hdr.mode == HDR_X2) && (stream->id == RKCIF_STREAM_MIPI_ID1)) {
	9032	+ } else if ((cif_dev->hdr.hdr_mode == HDR_X2) && (stream->id == RKCIF_STREAM_MIPI_ID1)) {
5302	9033	detect_stream->readout.early_time = stream->readout.fe_timestamp - stream->readout.wk_timestamp;
5303	9034	detect_stream->readout.total_time = stream->readout.fe_timestamp - detect_stream->readout.fe_timestamp;
5304	9035	detect_stream->readout.total_time += detect_stream->readout.readout_time;
5305		- } else if ((cif_dev->hdr.mode == HDR_X3) && (stream->id == RKCIF_STREAM_MIPI_ID2)) {
	9036	+ } else if ((cif_dev->hdr.hdr_mode == HDR_X3) && (stream->id == RKCIF_STREAM_MIPI_ID2)) {
5306	9037	detect_stream->readout.early_time = stream->readout.fe_timestamp - stream->readout.wk_timestamp;
5307	9038	detect_stream->readout.total_time = stream->readout.fe_timestamp - detect_stream->readout.fe_timestamp;
5308	9039	detect_stream->readout.total_time += detect_stream->readout.readout_time;
..	..	@@ -5321,11 +9052,8 @@
5321	9052	int ret = 0;
5322	9053
5323	9054	if (stream->frame_phase == (CIF_CSI_FRAME0_READY \| CIF_CSI_FRAME1_READY)) {
5324		-
5325		- v4l2_err(&cif_dev->v4l2_dev, "stream[%d], frm0/frm1 end simultaneously,frm id:%d\n",
5326		- stream->id, stream->frame_idx);
5327		-
5328		- stream->frame_idx++;
	9055	+ cif_dev->err_state \|= (RKCIF_ERR_ID0_TRIG_SIMULT << stream->id);
	9056	+ cif_dev->irq_stats.trig_simult_cnt[stream->id]++;
5329	9057	return;
5330	9058	}
5331	9059	if (!stream->is_line_wake_up) {
..	..	@@ -5334,11 +9062,93 @@
5334	9062	if (stream->frame_phase & CIF_CSI_FRAME0_READY) {
5335	9063	if (stream->curr_buf)
5336	9064	active_buf = stream->curr_buf;
5337		- stream->fps_stats.frm0_timestamp = ktime_get_ns();
	9065	+ stream->fps_stats.frm0_timestamp = rkcif_time_get_ns(cif_dev);
5338	9066	} else if (stream->frame_phase & CIF_CSI_FRAME1_READY) {
5339	9067	if (stream->next_buf)
5340	9068	active_buf = stream->next_buf;
5341		- stream->fps_stats.frm1_timestamp = ktime_get_ns();
	9069	+ stream->fps_stats.frm1_timestamp = rkcif_time_get_ns(cif_dev);
	9070	+ }
	9071	+ spin_unlock_irqrestore(&stream->fps_lock, flags);
	9072	+ }
	9073	+
	9074	+ rkcif_deal_readout_time(stream);
	9075	+
	9076	+ if (!stream->is_line_wake_up) {
	9077	+ ret = rkcif_assign_new_buffer_pingpong(stream,
	9078	+ RKCIF_YUV_ADDR_STATE_UPDATE,
	9079	+ mipi_id);
	9080	+ if (ret && cif_dev->chip_id < CHIP_RK3588_CIF)
	9081	+ return;
	9082	+ } else {
	9083	+ ret = rkcif_update_new_buffer_wake_up_mode(stream);
	9084	+ if (ret && cif_dev->chip_id < CHIP_RK3588_CIF)
	9085	+ return;
	9086	+ }
	9087	+ if (cif_dev->chip_id < CHIP_RK3588_CIF &&
	9088	+ cif_dev->active_sensor->mbus.type == V4L2_MBUS_BT656 &&
	9089	+ stream->id != 0)
	9090	+ stream->frame_idx++;
	9091	+ if (!stream->is_line_wake_up)
	9092	+ rkcif_buf_done_prepare(stream, active_buf, mipi_id, 0);
	9093	+
	9094	+ if (cif_dev->chip_id == CHIP_RV1126_CIF \|\|
	9095	+ cif_dev->chip_id == CHIP_RV1126_CIF_LITE \|\|
	9096	+ cif_dev->chip_id == CHIP_RK3568_CIF)
	9097	+ rkcif_luma_isr(&cif_dev->luma_vdev, mipi_id, cif_dev->stream[0].frame_idx - 1);
	9098	+}
	9099	+
	9100	+static void rkcif_update_stream_toisp(struct rkcif_device *cif_dev,
	9101	+ struct rkcif_stream *stream,
	9102	+ int mipi_id)
	9103	+{
	9104	+ if (stream->frame_phase == (CIF_CSI_FRAME0_READY \| CIF_CSI_FRAME1_READY)) {
	9105	+
	9106	+ v4l2_err(&cif_dev->v4l2_dev, "stream[%d], frm0/frm1 end simultaneously,frm id:%d\n",
	9107	+ stream->id, stream->frame_idx);
	9108	+ return;
	9109	+ }
	9110	+
	9111	+ spin_lock(&stream->fps_lock);
	9112	+ if (stream->frame_phase & CIF_CSI_FRAME0_READY)
	9113	+ stream->fps_stats.frm0_timestamp = rkcif_time_get_ns(cif_dev);
	9114	+ else if (stream->frame_phase & CIF_CSI_FRAME1_READY)
	9115	+ stream->fps_stats.frm1_timestamp = rkcif_time_get_ns(cif_dev);
	9116	+ spin_unlock(&stream->fps_lock);
	9117	+
	9118	+ if (cif_dev->inf_id == RKCIF_MIPI_LVDS)
	9119	+ rkcif_deal_readout_time(stream);
	9120	+
	9121	+ if (!stream->is_line_wake_up)
	9122	+ rkcif_assign_new_buffer_pingpong_toisp(stream,
	9123	+ RKCIF_YUV_ADDR_STATE_UPDATE,
	9124	+ mipi_id);
	9125	+}
	9126	+
	9127	+static void rkcif_update_stream_rockit(struct rkcif_device *cif_dev,
	9128	+ struct rkcif_stream *stream,
	9129	+ int mipi_id)
	9130	+{
	9131	+ struct rkcif_buffer *active_buf = NULL;
	9132	+ unsigned long flags;
	9133	+ int ret = 0;
	9134	+
	9135	+ if (stream->frame_phase == (CIF_CSI_FRAME0_READY \| CIF_CSI_FRAME1_READY)) {
	9136	+
	9137	+ v4l2_err(&cif_dev->v4l2_dev, "stream[%d], frm0/frm1 end simultaneously,frm id:%d\n",
	9138	+ stream->id, stream->frame_idx);
	9139	+ return;
	9140	+ }
	9141	+ if (!stream->is_line_wake_up) {
	9142	+
	9143	+ spin_lock_irqsave(&stream->fps_lock, flags);
	9144	+ if (stream->frame_phase & CIF_CSI_FRAME0_READY) {
	9145	+ if (stream->curr_buf_rockit)
	9146	+ active_buf = stream->curr_buf_rockit;
	9147	+ stream->fps_stats.frm0_timestamp = rkcif_time_get_ns(cif_dev);
	9148	+ } else if (stream->frame_phase & CIF_CSI_FRAME1_READY) {
	9149	+ if (stream->next_buf_rockit)
	9150	+ active_buf = stream->next_buf_rockit;
	9151	+ stream->fps_stats.frm1_timestamp = rkcif_time_get_ns(cif_dev);
5342	9152	}
5343	9153	spin_unlock_irqrestore(&stream->fps_lock, flags);
5344	9154	}
..	..	@@ -5346,86 +9156,98 @@
5346	9156	if (cif_dev->inf_id == RKCIF_MIPI_LVDS)
5347	9157	rkcif_deal_readout_time(stream);
5348	9158
5349		- if (cif_dev->chip_id == CHIP_RV1126_CIF \|\|
5350		- cif_dev->chip_id == CHIP_RV1126_CIF_LITE \|\|
5351		- cif_dev->chip_id == CHIP_RK3568_CIF)
5352		- rkcif_luma_isr(&cif_dev->luma_vdev, mipi_id, stream->frame_idx);
	9159	+ ret = rkcif_assign_new_buffer_pingpong_rockit(stream,
	9160	+ RKCIF_YUV_ADDR_STATE_UPDATE,
	9161	+ mipi_id);
	9162	+ if (ret)
	9163	+ return;
5353	9164
5354		- if (!stream->is_line_wake_up) {
5355		- ret = rkcif_assign_new_buffer_pingpong(stream,
5356		- RKCIF_YUV_ADDR_STATE_UPDATE,
5357		- mipi_id);
5358		- if (ret)
5359		- goto end;
5360		- } else {
5361		- ret = rkcif_update_new_buffer_wake_up_mode(stream);
5362		- if (ret)
5363		- return;
	9165	+ if (active_buf) {
	9166	+ active_buf->vb.vb2_buf.timestamp = stream->readout.fs_timestamp;
	9167	+ active_buf->vb.sequence = stream->frame_idx - 1;
	9168	+ rkcif_rockit_buf_done(stream, active_buf);
5364	9169	}
5365		- if (!stream->is_line_wake_up)
5366		- rkcif_buf_done_prepare(stream, active_buf, mipi_id, 0);
5367		-
5368		-end:
5369		- if (!stream->is_line_wake_up)
5370		- stream->frame_idx++;
5371	9170	}
5372	9171
5373		-u32 rkcif_get_sof(struct rkcif_device *cif_dev)
	9172	+static u32 rkcif_get_sof(struct rkcif_device *cif_dev)
5374	9173	{
5375	9174	u32 val = 0x0;
5376	9175	struct rkcif_sensor_info *sensor = cif_dev->active_sensor;
	9176	+ struct csi2_dev *csi;
5377	9177
5378		- if (sensor->mbus.type == V4L2_MBUS_CSI2)
5379		- val = rkcif_csi2_get_sof();
5380		- else if (sensor->mbus.type == V4L2_MBUS_CCP2)
	9178	+ if (sensor->mbus.type == V4L2_MBUS_CSI2_DPHY \|\|
	9179	+ sensor->mbus.type == V4L2_MBUS_CSI2_CPHY) {
	9180	+ csi = container_of(sensor->sd, struct csi2_dev, sd);
	9181	+ val = rkcif_csi2_get_sof(csi);
	9182	+ } else if (sensor->mbus.type == V4L2_MBUS_CCP2) {
5381	9183	val = rkcif_lvds_get_sof(cif_dev);
5382		- else if (sensor->mbus.type == V4L2_MBUS_PARALLEL \|\|
5383		- sensor->mbus.type == V4L2_MBUS_BT656)
	9184	+ } else if (sensor->mbus.type == V4L2_MBUS_PARALLEL \|\|
	9185	+ sensor->mbus.type == V4L2_MBUS_BT656) {
5384	9186	val = rkcif_dvp_get_sof(cif_dev);
5385		-
	9187	+ }
5386	9188	return val;
5387	9189	}
5388	9190
5389	9191	static void rkcif_set_sof(struct rkcif_device *cif_dev, u32 seq)
5390	9192	{
5391	9193	struct rkcif_sensor_info *sensor = cif_dev->active_sensor;
	9194	+ struct csi2_dev *csi;
5392	9195
5393		- if (sensor->mbus.type == V4L2_MBUS_CSI2)
5394		- rkcif_csi2_set_sof(seq);
5395		- else if (sensor->mbus.type == V4L2_MBUS_CCP2)
	9196	+ if (sensor->mbus.type == V4L2_MBUS_CSI2_DPHY \|\|
	9197	+ sensor->mbus.type == V4L2_MBUS_CSI2_CPHY) {
	9198	+ csi = container_of(sensor->sd, struct csi2_dev, sd);
	9199	+ rkcif_csi2_set_sof(csi, seq);
	9200	+ } else if (sensor->mbus.type == V4L2_MBUS_CCP2) {
5396	9201	rkcif_lvds_set_sof(cif_dev, seq);
5397		- else if (sensor->mbus.type == V4L2_MBUS_PARALLEL \|\|
5398		- sensor->mbus.type == V4L2_MBUS_BT656)
	9202	+ } else if (sensor->mbus.type == V4L2_MBUS_PARALLEL \|\|
	9203	+ sensor->mbus.type == V4L2_MBUS_BT656) {
5399	9204	rkcif_dvp_set_sof(cif_dev, seq);
	9205	+ }
5400	9206	}
5401	9207
5402		-static int rkcif_streamoff_in_reset(struct rkcif_device *cif_dev,
5403		- struct rkcif_stream *resume_stream[],
5404		- struct rkcif_resume_info *resume_info,
5405		- enum rkmodule_reset_src reset_src)
	9208	+static void rkcif_toisp_set_stream(struct rkcif_device *dev, int on)
5406	9209	{
5407		- struct rkcif_stream *stream = NULL;
	9210	+ struct v4l2_subdev *sd = get_rkisp_sd(dev->sditf[0]);
	9211	+
	9212	+ if (sd)
	9213	+ v4l2_subdev_call(sd, core, ioctl,
	9214	+ RKISP_VICAP_CMD_SET_STREAM, &on);
	9215	+}
	9216	+
	9217	+static int rkcif_do_reset_work(struct rkcif_device *cif_dev,
	9218	+ enum rkmodule_reset_src reset_src)
	9219	+{
5408	9220	struct rkcif_pipeline *p = &cif_dev->pipe;
	9221	+ struct rkcif_stream *stream = NULL;
	9222	+ struct rkcif_stream *resume_stream[RKCIF_MAX_STREAM_MIPI] = { NULL };
5409	9223	struct rkcif_sensor_info *terminal_sensor = &cif_dev->terminal_sensor;
5410		- u32 on, sof_cnt;
	9224	+ struct rkcif_resume_info *resume_info = &cif_dev->reset_work.resume_info;
	9225	+ struct rkcif_timer *timer = &cif_dev->reset_watchdog_timer;
	9226	+ struct sditf_priv *priv = cif_dev->sditf[0];
5411	9227	int i, j, ret = 0;
5412		- int stream_off_cnt = 0;
	9228	+ u32 on, sof_cnt;
	9229	+ int capture_mode = 0;
	9230	+
	9231	+ mutex_lock(&cif_dev->stream_lock);
	9232	+ if (cif_dev->reset_work_cancel) {
	9233	+ ret = 0;
	9234	+ goto unlock_stream;
	9235	+ }
	9236	+ v4l2_dbg(1, rkcif_debug, &cif_dev->v4l2_dev, "do rkcif reset\n");
5413	9237
5414	9238	for (i = 0, j = 0; i < RKCIF_MAX_STREAM_MIPI; i++) {
5415	9239	stream = &cif_dev->stream[i];
	9240	+
5416	9241	if (stream->state == RKCIF_STATE_STREAMING) {
5417		- stream_off_cnt++;
5418	9242
5419	9243	v4l2_dbg(1, rkcif_debug, &cif_dev->v4l2_dev,
5420	9244	"stream[%d] stopping\n", stream->id);
5421	9245
5422	9246	rkcif_stream_stop(stream);
5423	9247
5424		- atomic_dec(&p->stream_cnt);
5425		-
5426	9248	if (stream->id == RKCIF_STREAM_MIPI_ID0) {
5427	9249	sof_cnt = rkcif_get_sof(cif_dev);
5428		- v4l2_err(&cif_dev->v4l2_dev,
	9250	+ v4l2_dbg(1, rkcif_debug, &cif_dev->v4l2_dev,
5429	9251	"%s: stream[%d] sync frmid & csi_sof, frm_id:%d, csi_sof:%d\n",
5430	9252	__func__,
5431	9253	stream->id,
..	..	@@ -5443,57 +9265,66 @@
5443	9265
5444	9266	v4l2_dbg(1, rkcif_debug, &cif_dev->v4l2_dev,
5445	9267	"%s stop stream[%d] in streaming, frm_id:%d, csi_sof:%d\n",
5446		- __func__, stream->id, stream->frame_idx, rkcif_csi2_get_sof());
	9268	+ __func__, stream->id, stream->frame_idx, rkcif_get_sof(cif_dev));
5447	9269
5448	9270	}
5449	9271	}
5450		- if (!stream_off_cnt)
5451		- return 0;
5452	9272
5453	9273	on = 0;
5454	9274	for (i = 0; i < p->num_subdevs; i++) {
5455	9275
5456	9276	if (p->subdevs[i] == terminal_sensor->sd) {
5457	9277
5458		- if (reset_src == RKCIF_RESET_SRC_ERR_CSI2 \|\|
5459		- reset_src == RKCIF_RESET_SRC_ERR_HOTPLUG \|\|
5460		- reset_src == RKICF_RESET_SRC_ERR_CUTOFF) {
5461		-
	9278	+ if (reset_src != RKCIF_RESET_SRC_ERR_APP) {
5462	9279	ret = v4l2_subdev_call(p->subdevs[i], core, ioctl,
5463	9280	RKMODULE_SET_QUICK_STREAM, &on);
5464	9281	if (ret)
5465		- v4l2_err(&cif_dev->v4l2_dev, "quick stream off subdev:%s failed\n",
	9282	+ v4l2_dbg(1, rkcif_debug, &cif_dev->v4l2_dev,
	9283	+ "quick stream off subdev:%s failed\n",
5466	9284	p->subdevs[i]->name);
5467	9285	}
	9286	+ } else {
	9287	+ ret = v4l2_subdev_call(p->subdevs[i], video, s_stream, on);
5468	9288	}
5469		-
5470	9289	if (ret)
5471		- v4l2_err(&cif_dev->v4l2_dev, "%s:stream %s subdev:%s failed\n",
	9290	+ v4l2_dbg(1, rkcif_debug, &cif_dev->v4l2_dev,
	9291	+ "%s:stream %s subdev:%s failed\n",
5472	9292	__func__, on ? "on" : "off", p->subdevs[i]->name);
5473		-
5474	9293	}
5475		- return ret;
5476		-}
5477	9294
5478		-static int rkcif_streamon_in_reset(struct rkcif_device *cif_dev,
5479		- struct rkcif_stream *resume_stream[],
5480		- struct rkcif_resume_info *resume_info,
5481		- enum rkmodule_reset_src reset_src)
5482		-{
5483		- struct rkcif_timer *timer = &cif_dev->reset_watchdog_timer;
5484		- struct rkcif_stream *stream = NULL;
5485		- struct rkcif_pipeline *p = &cif_dev->pipe;
5486		- struct rkcif_sensor_info *terminal_sensor = &cif_dev->terminal_sensor;
5487		- int i = 0;
5488		- int ret = 0;
5489		- int on = 0;
5490		- int stream_on_cnt = 0;
	9295	+ if (priv && priv->is_combine_mode && cif_dev->sditf_cnt <= RKCIF_MAX_SDITF) {
	9296	+ for (i = 0; i < cif_dev->sditf_cnt; i++) {
	9297	+ if (cif_dev->sditf[i] && cif_dev->sditf[i]->sensor_sd)
	9298	+ ret = v4l2_subdev_call(cif_dev->sditf[i]->sensor_sd, core, ioctl,
	9299	+ RKMODULE_SET_QUICK_STREAM, &on);
	9300	+ }
	9301	+ }
5491	9302
5492		- for (i = 0; i < RKCIF_MAX_STREAM_MIPI; i++) {
	9303	+ rockchip_clear_system_status(SYS_STATUS_CIF0);
	9304	+
	9305	+ if (cif_dev->chip_id >= CHIP_RK3588_CIF) {
	9306	+ rkcif_do_soft_reset(cif_dev);
	9307	+ } else {
	9308	+
	9309	+ rkcif_do_cru_reset(cif_dev);
	9310	+
	9311	+ rkcif_disable_sys_clk(cif_dev->hw_dev);
	9312	+
	9313	+ udelay(5);
	9314	+
	9315	+ ret = rkcif_enable_sys_clk(cif_dev->hw_dev);
	9316	+
	9317	+ if (ret < 0) {
	9318	+ v4l2_err(&cif_dev->v4l2_dev, "%s:resume cif clk failed\n", __func__);
	9319	+ goto unlock_stream;
	9320	+ }
	9321	+ }
	9322	+
	9323	+ if (priv && priv->mode.rdbk_mode == RKISP_VICAP_ONLINE)
	9324	+ rkcif_toisp_set_stream(cif_dev, 1);
	9325	+
	9326	+ for (i = 0; i < j; i++) {
5493	9327	stream = resume_stream[i];
5494		- if (stream == NULL \|\| stream->state != RKCIF_STATE_RESET_IN_STREAMING)
5495		- break;
5496		- stream_on_cnt++;
5497	9328	stream->fs_cnt_in_single_frame = 0;
5498	9329	if (stream->cif_fmt_in->field == V4L2_FIELD_INTERLACED) {
5499	9330	if (stream->curr_buf == stream->next_buf) {
..	..	@@ -5508,25 +9339,35 @@
5508	9339	stream->curr_buf = NULL;
5509	9340	stream->next_buf = NULL;
5510	9341	}
5511		- if (cif_dev->active_sensor->mbus.type == V4L2_MBUS_CSI2 \|\|
5512		- cif_dev->active_sensor->mbus.type == V4L2_MBUS_CCP2)
5513		- ret = rkcif_csi_stream_start(stream);
	9342	+ if (!cif_dev->sditf[0] \|\| cif_dev->sditf[0]->mode.rdbk_mode == RKISP_VICAP_RDBK_AIQ)
	9343	+ capture_mode = RKCIF_STREAM_MODE_CAPTURE;
	9344	+ else {
	9345	+ if (cif_dev->sditf[0]->mode.rdbk_mode == RKISP_VICAP_ONLINE)
	9346	+ capture_mode = RKCIF_STREAM_MODE_TOISP;
	9347	+ else
	9348	+ capture_mode = RKCIF_STREAM_MODE_TOISP_RDBK;
	9349	+ }
	9350	+ if (cif_dev->active_sensor &&
	9351	+ (cif_dev->active_sensor->mbus.type == V4L2_MBUS_CSI2_DPHY \|\|
	9352	+ cif_dev->active_sensor->mbus.type == V4L2_MBUS_CSI2_CPHY \|\|
	9353	+ cif_dev->active_sensor->mbus.type == V4L2_MBUS_CCP2))
	9354	+ ret = rkcif_csi_stream_start(stream, capture_mode);
5514	9355	else
5515		- ret = rkcif_stream_start(stream);
	9356	+ ret = rkcif_stream_start(stream, capture_mode);
5516	9357	if (ret) {
5517	9358	v4l2_err(&cif_dev->v4l2_dev, "%s:resume stream[%d] failed\n",
5518	9359	__func__, stream->id);
5519	9360	goto unlock_stream;
5520	9361	}
5521		- atomic_inc(&p->stream_cnt);
5522		- stream->streamon_timestamp = ktime_get_ns();
	9362	+
5523	9363	v4l2_dbg(1, rkcif_debug, &cif_dev->v4l2_dev,
5524	9364	"resume stream[%d], frm_idx:%d, csi_sof:%d\n",
5525	9365	stream->id, stream->frame_idx,
5526	9366	rkcif_get_sof(cif_dev));
5527	9367	}
5528		- if (!stream_on_cnt)
5529		- return 0;
	9368	+
	9369	+ rockchip_set_system_status(SYS_STATUS_CIF0);
	9370	+
5530	9371	on = 1;
5531	9372	for (i = 0; i < p->num_subdevs; i++) {
5532	9373
..	..	@@ -5534,9 +9375,7 @@
5534	9375
5535	9376	rkcif_set_sof(cif_dev, resume_info->frm_sync_seq);
5536	9377
5537		- if (reset_src == RKCIF_RESET_SRC_ERR_CSI2 \|\|
5538		- reset_src == RKCIF_RESET_SRC_ERR_HOTPLUG \|\|
5539		- reset_src == RKICF_RESET_SRC_ERR_CUTOFF) {
	9378	+ if (reset_src != RKCIF_RESET_SRC_ERR_APP) {
5540	9379	ret = v4l2_subdev_call(p->subdevs[i], core, ioctl,
5541	9380	RKMODULE_SET_QUICK_STREAM, &on);
5542	9381	if (ret)
..	..	@@ -5544,6 +9383,11 @@
5544	9383	"quick stream on subdev:%s failed\n",
5545	9384	p->subdevs[i]->name);
5546	9385	}
	9386	+ } else {
	9387	+ if (p->subdevs[i] == terminal_sensor->sd)
	9388	+ rkcif_set_sof(cif_dev, resume_info->frm_sync_seq);
	9389	+
	9390	+ ret = v4l2_subdev_call(p->subdevs[i], video, s_stream, on);
5547	9391	}
5548	9392
5549	9393	if (ret)
..	..	@@ -5551,68 +9395,28 @@
5551	9395	p->subdevs[i]->name);
5552	9396	}
5553	9397
5554		- rkcif_start_luma(&cif_dev->luma_vdev, resume_stream[0]->cif_fmt_in);
	9398	+ if (priv && priv->is_combine_mode && cif_dev->sditf_cnt <= RKCIF_MAX_SDITF) {
	9399	+ for (i = 0; i < cif_dev->sditf_cnt; i++) {
	9400	+ if (cif_dev->sditf[i] && cif_dev->sditf[i]->sensor_sd)
	9401	+ v4l2_subdev_call(cif_dev->sditf[i]->sensor_sd, core, ioctl,
	9402	+ RKMODULE_SET_QUICK_STREAM, &on);
	9403	+ }
	9404	+ }
	9405	+
	9406	+ if (cif_dev->chip_id < CHIP_RK3588_CIF)
	9407	+ rkcif_start_luma(&cif_dev->luma_vdev,
	9408	+ cif_dev->stream[RKCIF_STREAM_MIPI_ID0].cif_fmt_in);
5555	9409
5556	9410	timer->csi2_err_triggered_cnt = 0;
	9411	+ rkcif_monitor_reset_event(cif_dev);
5557	9412
5558	9413	v4l2_dbg(1, rkcif_debug, &cif_dev->v4l2_dev, "do rkcif reset successfully!\n");
5559		-
	9414	+ mutex_unlock(&cif_dev->stream_lock);
5560	9415	return 0;
5561	9416
5562	9417	unlock_stream:
	9418	+ mutex_unlock(&cif_dev->stream_lock);
5563	9419	return ret;
5564		-}
5565		-
5566		-static int rkcif_do_reset_work(struct rkcif_device *cif_dev,
5567		- enum rkmodule_reset_src reset_src)
5568		-{
5569		- struct rkcif_stream *resume_stream[RKCIF_MAX_DEV][RKCIF_MAX_STREAM_MIPI] = {0};
5570		- struct rkcif_resume_info *resume_info[RKCIF_MAX_DEV];
5571		- struct rkcif_hw *hw = cif_dev->hw_dev;
5572		- struct rkcif_device *cifdev = NULL;
5573		- int i = 0;
5574		- int ret = 0;
5575		-
5576		- v4l2_info(&cif_dev->v4l2_dev, "do rkcif reset\n");
5577		-
5578		- mutex_lock(&cif_dev->hw_dev->dev_lock);
5579		- if (hw->reset_work_cancel) {
5580		- mutex_unlock(&hw->dev_lock);
5581		- return 0;
5582		- }
5583		- for (i = 0; i < hw->dev_num; i++) {
5584		- cifdev = hw->cif_dev[i];
5585		- resume_info[i] = &cifdev->reset_work.resume_info;
5586		- ret \|= rkcif_streamoff_in_reset(cifdev,
5587		- resume_stream[i],
5588		- resume_info[i],
5589		- reset_src);
5590		- }
5591		-
5592		- rockchip_clear_system_status(SYS_STATUS_CIF0);
5593		-
5594		- rkcif_do_cru_reset(cif_dev);
5595		-
5596		- udelay(30);
5597		-
5598		- for (i = 0; i < hw->dev_num; i++) {
5599		- cifdev = hw->cif_dev[i];
5600		- resume_info[i] = &cifdev->reset_work.resume_info;
5601		- ret \|= rkcif_streamon_in_reset(cifdev,
5602		- resume_stream[i],
5603		- resume_info[i],
5604		- reset_src);
5605		- }
5606		-
5607		- rockchip_set_system_status(SYS_STATUS_CIF0);
5608		-
5609		- hw->hw_timer.is_running = false;
5610		- rkcif_monitor_reset_event(cif_dev->hw_dev);
5611		-
5612		- v4l2_info(&cif_dev->v4l2_dev, "do rkcif reset successfully!\n");
5613		- mutex_unlock(&hw->dev_lock);
5614		- return ret;
5615		-
5616	9420	}
5617	9421
5618	9422	void rkcif_reset_work(struct work_struct *work)
..	..	@@ -5630,130 +9434,193 @@
5630	9434	v4l2_info(&dev->v4l2_dev, "do reset work failed!\n");
5631	9435	}
5632	9436
5633		-static bool rkcif_is_stream_stop_output(struct rkcif_stream *stream)
	9437	+static bool rkcif_is_reduced_frame_rate(struct rkcif_device *dev)
5634	9438	{
5635		- struct rkcif_device *dev = stream->cifdev;
5636		- struct rkcif_hw_timer *hw_timer = &dev->hw_dev->hw_timer;
5637		- struct rkcif_stream *tmp_stream = NULL;
	9439	+ struct rkcif_timer *timer = &dev->reset_watchdog_timer;
	9440	+ struct rkcif_stream *stream = &dev->stream[RKCIF_STREAM_MIPI_ID0];
5638	9441	struct v4l2_rect *raw_rect = &dev->terminal_sensor.raw_rect;
5639		- u64 fps, diff_time;
5640		- unsigned int i;
5641		- s64 vblank_def = 0;
5642		- s32 vblank = 0;
5643		- u64 numerator = 0;
5644		- u64 denominator = 0;
5645		- u64 cur_timestamp = 0;
	9442	+ u64 fps, timestamp0, timestamp1, diff_time;
	9443	+ unsigned long fps_flags;
	9444	+ unsigned int deviation = 1;
	9445	+ bool is_reduced = false;
	9446	+ u64 cur_time = 0;
	9447	+ u64 time_distance = 0;
5646	9448
5647		- if (dev->hdr.mode == HDR_X3) {
5648		- for (i = 0; i < 3; i++) {
5649		- tmp_stream = &dev->stream[i];
5650		- if (tmp_stream->state != RKCIF_STATE_STREAMING)
5651		- return false;
5652		- }
5653		- } else if (dev->hdr.mode == HDR_X2) {
5654		- for (i = 0; i < 2; i++) {
5655		- tmp_stream = &dev->stream[i];
5656		- if (tmp_stream->state != RKCIF_STATE_STREAMING)
5657		- return false;
5658		- }
5659		- }
	9449	+ spin_lock_irqsave(&stream->fps_lock, fps_flags);
	9450	+ timestamp0 = stream->fps_stats.frm0_timestamp;
	9451	+ timestamp1 = stream->fps_stats.frm1_timestamp;
	9452	+ spin_unlock_irqrestore(&stream->fps_lock, fps_flags);
5660	9453
5661		- vblank_def = rkcif_get_sensor_vblank_def(dev);
5662		- vblank = rkcif_get_sensor_vblank(dev);
5663		- numerator = dev->terminal_sensor.fi.interval.numerator;
5664		- denominator = dev->terminal_sensor.fi.interval.denominator;
5665		- fps = div_u64(1000 * numerator, denominator);
5666		- fps *= (raw_rect->height + vblank);
5667		- fps = div_u64((raw_rect->height + vblank_def), fps);
	9454	+ fps = timestamp0 > timestamp1 ?
	9455	+ timestamp0 - timestamp1 : timestamp1 - timestamp0;
	9456	+ fps = div_u64(fps, 1000);
	9457	+ diff_time = fps > timer->frame_end_cycle_us ?
	9458	+ fps - timer->frame_end_cycle_us : 0;
	9459	+ deviation = DIV_ROUND_UP(timer->vts, 100);
5668	9460
5669		- if (fps > hw_timer->monitor_cycle) {
5670		- v4l2_dbg(1, rkcif_debug, &dev->v4l2_dev, "camera work in long expsure, not to check stream loss\n");
	9461	+ v4l2_dbg(3, rkcif_debug, &dev->v4l2_dev, "diff_time:%lld,devi_t:%ld,devi_h:%d\n",
	9462	+ diff_time, timer->line_end_cycle * deviation, deviation);
	9463	+
	9464	+ cur_time = rkcif_time_get_ns(dev);
	9465	+ time_distance = timestamp0 > timestamp1 ?
	9466	+ cur_time - timestamp0 : cur_time - timestamp1;
	9467	+ time_distance = div_u64(time_distance, 1000);
	9468	+ if (time_distance > fps * 2)
5671	9469	return false;
	9470	+
	9471	+ if (diff_time > timer->line_end_cycle * deviation) {
	9472	+ s32 vblank = 0;
	9473	+ unsigned int vts;
	9474	+
	9475	+ is_reduced = true;
	9476	+ vblank = rkcif_get_sensor_vblank(dev);
	9477	+ vts = vblank + timer->raw_height;
	9478	+
	9479	+ v4l2_dbg(3, rkcif_debug, &dev->v4l2_dev, "old vts:%d,new vts:%d\n", timer->vts, vts);
	9480	+
	9481	+ v4l2_dbg(3, rkcif_debug, &dev->v4l2_dev,
	9482	+ "reduce frame rate,vblank:%d, height(raw output):%d, fps:%lld, frm_end_t:%ld, line_t:%ld, diff:%lld\n",
	9483	+ rkcif_get_sensor_vblank(dev),
	9484	+ raw_rect->height,
	9485	+ fps,
	9486	+ timer->frame_end_cycle_us,
	9487	+ timer->line_end_cycle,
	9488	+ diff_time);
	9489	+
	9490	+ timer->vts = vts;
	9491	+ timer->frame_end_cycle_us = fps;
	9492	+ timer->line_end_cycle = div_u64(timer->frame_end_cycle_us, timer->vts);
	9493	+ } else {
	9494	+ is_reduced = false;
5672	9495	}
5673	9496
5674		- cur_timestamp = ktime_get_ns();
5675		- diff_time = cur_timestamp - stream->streamon_timestamp;
5676		- diff_time = div_u64(diff_time, 1000000);
5677		- if (diff_time > hw_timer->monitor_cycle + 10)
5678		- return true;
	9497	+ timer->frame_end_cycle_us = fps;
5679	9498
5680		- return false;
	9499	+ fps = div_u64(fps, 1000);
	9500	+ fps = fps * timer->frm_num_of_monitor_cycle;
	9501	+ timer->cycle = msecs_to_jiffies(fps);
	9502	+ timer->timer.expires = jiffies + timer->cycle;
	9503	+
	9504	+ return is_reduced;
	9505	+
5681	9506	}
5682	9507
5683		-static void rkcif_init_reset_work(struct rkcif_hw_timer *hw_timer)
	9508	+static void rkcif_dvp_event_reset_pipe(struct rkcif_device *dev, int reset_src)
5684	9509	{
5685		- struct rkcif_hw *hw = container_of(hw_timer,
5686		- struct rkcif_hw,
5687		- hw_timer);
5688		- struct rkcif_device *dev = NULL;
	9510	+ struct rkcif_dvp_sof_subdev *subdev = &dev->dvp_sof_subdev;
	9511	+
	9512	+ if (subdev) {
	9513	+ struct v4l2_event event = {
	9514	+ .type = V4L2_EVENT_RESET_DEV,
	9515	+ .reserved[0] = reset_src,
	9516	+ };
	9517	+ v4l2_event_queue(subdev->sd.devnode, &event);
	9518	+ }
	9519	+}
	9520	+
	9521	+static void rkcif_lvds_event_reset_pipe(struct rkcif_device *dev, int reset_src)
	9522	+{
	9523	+ struct rkcif_lvds_subdev *subdev = &dev->lvds_subdev;
	9524	+
	9525	+ if (subdev) {
	9526	+ struct v4l2_event event = {
	9527	+ .type = V4L2_EVENT_RESET_DEV,
	9528	+ .reserved[0] = reset_src,
	9529	+ };
	9530	+ v4l2_event_queue(subdev->sd.devnode, &event);
	9531	+ }
	9532	+}
	9533	+
	9534	+static void rkcif_send_reset_event(struct rkcif_device *cif_dev, int reset_src)
	9535	+{
	9536	+ struct v4l2_mbus_config *mbus = &cif_dev->active_sensor->mbus;
	9537	+ struct csi2_dev *csi;
	9538	+
	9539	+ if (mbus->type == V4L2_MBUS_CSI2_DPHY \|\|
	9540	+ mbus->type == V4L2_MBUS_CSI2_CPHY) {
	9541	+ csi = container_of(cif_dev->active_sensor->sd, struct csi2_dev, sd);
	9542	+ rkcif_csi2_event_reset_pipe(csi, reset_src);
	9543	+ } else if (mbus->type == V4L2_MBUS_CCP2) {
	9544	+ rkcif_lvds_event_reset_pipe(cif_dev, reset_src);
	9545	+ } else {
	9546	+ rkcif_dvp_event_reset_pipe(cif_dev, reset_src);
	9547	+ }
	9548	+ v4l2_dbg(3, rkcif_debug, &cif_dev->v4l2_dev,
	9549	+ "send reset event,bus type 0x%x\n",
	9550	+ mbus->type);
	9551	+}
	9552	+
	9553	+static void rkcif_init_reset_work(struct rkcif_timer *timer)
	9554	+{
	9555	+ struct rkcif_device *dev = container_of(timer,
	9556	+ struct rkcif_device,
	9557	+ reset_watchdog_timer);
5689	9558	struct rkcif_stream *stream = NULL;
5690		- struct rkcif_timer *timer;
5691	9559	unsigned long flags;
5692		- int i = 0, j = 0;
	9560	+ int i = 0;
5693	9561
5694	9562	v4l2_info(&dev->v4l2_dev,
5695	9563	"do reset work schedule, run_cnt:%d, reset source:%d\n",
5696		- hw_timer->run_cnt, hw_timer->reset_src);
	9564	+ timer->run_cnt, timer->reset_src);
5697	9565
5698		- for (j = 0; j < hw->dev_num; j++) {
5699		- dev = hw->cif_dev[j];
5700		- timer = &dev->reset_watchdog_timer;
5701		- spin_lock_irqsave(&timer->csi2_err_lock, flags);
5702		- timer->is_triggered = false;
5703		- timer->csi2_err_cnt_odd = 0;
5704		- timer->csi2_err_cnt_even = 0;
5705		- timer->csi2_err_fs_fe_cnt = 0;
5706		- timer->notifer_called_cnt = 0;
5707		- for (i = 0; i < dev->num_channels; i++) {
5708		- stream = &dev->stream[i];
5709		- if (stream->state == RKCIF_STATE_STREAMING)
5710		- timer->last_buf_wakeup_cnt[stream->id] = stream->frame_idx;
5711		- }
5712		- spin_unlock_irqrestore(&timer->csi2_err_lock, flags);
	9566	+ spin_lock_irqsave(&timer->timer_lock, flags);
	9567	+ timer->is_running = false;
	9568	+ timer->is_triggered = false;
	9569	+ timer->csi2_err_cnt_odd = 0;
	9570	+ timer->csi2_err_cnt_even = 0;
	9571	+ timer->csi2_err_fs_fe_cnt = 0;
	9572	+ timer->notifer_called_cnt = 0;
	9573	+ dev->is_toisp_reset = false;
	9574	+ for (i = 0; i < dev->num_channels; i++) {
	9575	+ stream = &dev->stream[i];
	9576	+ if (stream->state == RKCIF_STATE_STREAMING)
	9577	+ timer->last_buf_wakeup_cnt[stream->id] = stream->buf_wake_up_cnt;
5713	9578	}
5714		- if (hw_timer->is_reset_by_user) {
5715		- spin_lock_irqsave(&hw->spin_lock, flags);
5716		- hw->reset_info.is_need_reset = 1;
5717		- hw->reset_info.reset_src = hw_timer->reset_src;
5718		- spin_unlock_irqrestore(&hw->spin_lock, flags);
	9579	+ spin_unlock_irqrestore(&timer->timer_lock, flags);
	9580	+ if (timer->is_ctrl_by_user) {
	9581	+ rkcif_send_reset_event(dev, timer->reset_src);
5719	9582	} else {
5720		- dev->reset_work.reset_src = hw_timer->reset_src;
5721		- if (schedule_work(&dev->reset_work.work)) {
	9583	+ dev->reset_work.reset_src = timer->reset_src;
	9584	+ if (!schedule_work(&dev->reset_work.work))
5722	9585	v4l2_info(&dev->v4l2_dev,
5723		- "schedule reset work successfully\n");
5724		- } else {
5725		- v4l2_info(&dev->v4l2_dev,
5726		- "schedule reset work failed\n");
5727		- }
	9586	+ "schedule reset work failed\n");
5728	9587	}
5729	9588	}
5730	9589
5731	9590	static int rkcif_detect_reset_event(struct rkcif_stream *stream,
5732	9591	struct rkcif_timer *timer,
5733		- int *check_cnt,
	9592	+ int check_cnt,
5734	9593	bool *is_mod_timer)
5735	9594	{
5736	9595	struct rkcif_device *dev = stream->cifdev;
5737	9596	struct rkcif_sensor_info *terminal_sensor = &dev->terminal_sensor;
5738		- struct rkcif_hw_timer *hw_timer = &dev->hw_dev->hw_timer;
	9597	+ unsigned long flags;
5739	9598	int ret, is_reset = 0;
5740	9599	struct rkmodule_vicap_reset_info rst_info;
5741	9600
5742		- v4l2_dbg(3, rkcif_debug, &dev->v4l2_dev,
5743		- "info: frame end still update(%d, %d), detect cnt: %d, mode:%d check_cnt:%d\n",
5744		- timer->last_buf_wakeup_cnt[stream->id], stream->frame_idx,
5745		- hw_timer->run_cnt, hw_timer->monitor_mode, *check_cnt);
	9601	+ if (dev->is_toisp_reset) {
	9602	+ is_reset = 1;
	9603	+ timer->reset_src = RKCIF_RESET_SRC_ERR_ISP;
	9604	+ }
	9605	+ if (is_reset) {
	9606	+ rkcif_init_reset_work(timer);
	9607	+ return is_reset;
	9608	+ }
5746	9609
5747		- if (timer->last_buf_wakeup_cnt[stream->id] < stream->frame_idx) {
5748		- *check_cnt += 1;
	9610	+ if (timer->last_buf_wakeup_cnt[stream->id] < stream->buf_wake_up_cnt &&
	9611	+ check_cnt == 0) {
	9612	+
5749	9613	v4l2_dbg(1, rkcif_debug, &dev->v4l2_dev,
5750		- "info: frame end still update(%d, %d), detect cnt: %d, mode:%d\n",
	9614	+ "info: frame end still update(%d, %d) in detecting cnt:%d, mode:%d\n",
5751	9615	timer->last_buf_wakeup_cnt[stream->id], stream->frame_idx,
5752		- hw_timer->run_cnt, hw_timer->monitor_mode);
	9616	+ timer->run_cnt, timer->monitor_mode);
5753	9617
5754		- timer->last_buf_wakeup_cnt[stream->id] = stream->frame_idx;
	9618	+ timer->last_buf_wakeup_cnt[stream->id] = stream->buf_wake_up_cnt;
5755	9619
5756		- if (hw_timer->monitor_mode == RKCIF_MONITOR_MODE_HOTPLUG) {
	9620	+ if (stream->frame_idx > 2)
	9621	+ rkcif_is_reduced_frame_rate(dev);
	9622	+
	9623	+ if (timer->monitor_mode == RKCIF_MONITOR_MODE_HOTPLUG) {
5757	9624	ret = v4l2_subdev_call(terminal_sensor->sd,
5758	9625	core, ioctl,
5759	9626	RKMODULE_GET_VICAP_RST_INFO,
..	..	@@ -5763,156 +9630,139 @@
5763	9630	else
5764	9631	is_reset = rst_info.is_reset;
5765	9632	rst_info.is_reset = 0;
5766		- hw_timer->reset_src = RKCIF_RESET_SRC_ERR_HOTPLUG;
	9633	+ if (is_reset)
	9634	+ timer->reset_src = RKCIF_RESET_SRC_ERR_HOTPLUG;
5767	9635	v4l2_subdev_call(terminal_sensor->sd, core, ioctl,
5768	9636	RKMODULE_SET_VICAP_RST_INFO, &rst_info);
5769		- if (!is_reset && stream->cifdev->inf_id == RKCIF_MIPI_LVDS) {
5770		- hw_timer->reset_src = RKICF_RESET_SRC_ERR_CUTOFF;
	9637	+ if (!is_reset) {
5771	9638	is_reset = rkcif_is_csi2_err_trigger_reset(timer);
5772		- }
5773		- } else if (hw_timer->monitor_mode == RKCIF_MONITOR_MODE_CONTINUE) {
5774		- if (stream->cifdev->inf_id == RKCIF_MIPI_LVDS)
5775		- is_reset = rkcif_is_csi2_err_trigger_reset(timer);
5776		- } else if (hw_timer->monitor_mode == RKCIF_MONITOR_MODE_TRIGGER) {
5777		- if (stream->cifdev->inf_id == RKCIF_MIPI_LVDS) {
5778		- is_reset = timer->is_csi2_err_occurred;
5779	9639	if (is_reset)
5780		- hw_timer->reset_src = RKCIF_RESET_SRC_ERR_CSI2;
5781		- timer->is_csi2_err_occurred = false;
	9640	+ timer->reset_src = RKCIF_RESET_SRC_ERR_CSI2;
5782	9641	}
	9642	+ } else if (timer->monitor_mode == RKCIF_MONITOR_MODE_CONTINUE) {
	9643	+ is_reset = rkcif_is_csi2_err_trigger_reset(timer);
	9644	+ } else if (timer->monitor_mode == RKCIF_MONITOR_MODE_TRIGGER) {
	9645	+ is_reset = timer->is_csi2_err_occurred;
	9646	+ if (is_reset)
	9647	+ timer->reset_src = RKCIF_RESET_SRC_ERR_CSI2;
	9648	+ timer->is_csi2_err_occurred = false;
5783	9649	}
5784	9650
5785	9651	if (is_reset) {
5786		- rkcif_init_reset_work(hw_timer);
	9652	+ rkcif_init_reset_work(timer);
5787	9653	return is_reset;
5788	9654	}
5789		- *is_mod_timer = true;
5790		- } else if (timer->last_buf_wakeup_cnt[stream->id] == stream->frame_idx) {
5791		- bool is_loss_stream = false;
5792	9655
5793		- *check_cnt += 1;
5794		- if (hw_timer->run_cnt > 4)
5795		- is_loss_stream = rkcif_is_stream_stop_output(stream);
5796		-
5797		- if (is_loss_stream) {
5798		- v4l2_info(&dev->v4l2_dev,
5799		- "do reset work due to frame end is stopped, run_cnt:%d\n",
5800		- hw_timer->run_cnt);
5801		-
5802		- hw_timer->reset_src = RKICF_RESET_SRC_ERR_CUTOFF;
5803		- is_reset = true;
5804		- rkcif_init_reset_work(hw_timer);
	9656	+ if (timer->monitor_mode == RKCIF_MONITOR_MODE_CONTINUE \|\|
	9657	+ timer->monitor_mode == RKCIF_MONITOR_MODE_HOTPLUG) {
	9658	+ if (timer->run_cnt == timer->max_run_cnt)
	9659	+ timer->run_cnt = 0x0;
	9660	+ *is_mod_timer = true;
5805	9661	} else {
	9662	+ if (timer->run_cnt <= timer->max_run_cnt) {
	9663	+ *is_mod_timer = true;
	9664	+ } else {
	9665	+ spin_lock_irqsave(&timer->timer_lock, flags);
	9666	+ timer->is_triggered = false;
	9667	+ timer->is_running = false;
	9668	+ spin_unlock_irqrestore(&timer->timer_lock, flags);
	9669	+ v4l2_info(&dev->v4l2_dev, "stop reset detecting!\n");
	9670	+ }
	9671	+ }
	9672	+ } else if (timer->last_buf_wakeup_cnt[stream->id] == stream->buf_wake_up_cnt) {
	9673	+
	9674	+ bool is_reduced = false;
	9675	+
	9676	+ if (stream->frame_idx > 2)
	9677	+ is_reduced = rkcif_is_reduced_frame_rate(dev);
	9678	+ else if (timer->run_cnt < 20)
	9679	+ is_reduced = true;
	9680	+
	9681	+ if (is_reduced) {
5806	9682	*is_mod_timer = true;
5807	9683	v4l2_dbg(3, rkcif_debug, &dev->v4l2_dev, "%s fps is reduced\n", __func__);
	9684	+ } else {
	9685	+
	9686	+ v4l2_info(&dev->v4l2_dev,
	9687	+ "do reset work due to frame end is stopped, run_cnt:%d\n",
	9688	+ timer->run_cnt);
	9689	+
	9690	+ timer->reset_src = RKICF_RESET_SRC_ERR_CUTOFF;
	9691	+ rkcif_init_reset_work(timer);
	9692	+ is_reset = true;
5808	9693	}
5809	9694	}
5810	9695
5811		- return is_reset;
5812		-
5813		-}
5814		-
5815		-static int rkcif_dvp_err_trigger_reset(struct rkcif_device *dev)
5816		-{
5817		- int is_reset = 0;
5818		-
5819		- if (dev->irq_stats.dvp_overflow_cnt) {
5820		- dev->irq_stats.dvp_overflow_cnt = 0;
5821		- is_reset = 1;
5822		- }
5823		- return is_reset;
5824		-}
5825		-
5826		-static int rkcif_detect_reset_event_all_dev(struct rkcif_device *dev,
5827		- int *check_cnt,
5828		- bool *is_mod_timer)
5829		-{
5830		- struct rkcif_timer *timer = &dev->reset_watchdog_timer;
5831		- struct rkcif_stream *stream = NULL;
5832		- int i = 0;
5833		- int is_reset = 0;
5834		- int check_cnt_local = 0;
5835		-
5836		- for (i = 0; i < dev->num_channels; i++) {
5837		- stream = &dev->stream[i];
5838		- if (stream->state == RKCIF_STATE_STREAMING) {
5839		- is_reset = rkcif_detect_reset_event(stream,
5840		- timer,
5841		- &check_cnt_local,
5842		- is_mod_timer);
5843		- if (is_reset)
5844		- break;
5845		- }
5846		- }
5847		- if (!is_reset && dev->inf_id == RKCIF_DVP)
5848		- is_reset = rkcif_dvp_err_trigger_reset(dev);
5849		-
5850		- if (check_cnt_local)
5851		- *check_cnt += 1;
5852	9696	return is_reset;
5853	9697
5854	9698	}
5855	9699
5856	9700	void rkcif_reset_watchdog_timer_handler(struct timer_list *t)
5857	9701	{
5858		- struct rkcif_hw_timer *hw_timer = container_of(t, struct rkcif_hw_timer, timer);
5859		- struct rkcif_hw *hw = container_of(hw_timer,
5860		- struct rkcif_hw,
5861		- hw_timer);
5862		- struct rkcif_device *dev = NULL;
	9702	+ struct rkcif_timer *timer = container_of(t, struct rkcif_timer, timer);
	9703	+ struct rkcif_device *dev = container_of(timer,
	9704	+ struct rkcif_device,
	9705	+ reset_watchdog_timer);
	9706	+ struct rkcif_stream *stream = NULL;
5863	9707	unsigned long flags;
5864	9708	unsigned int i;
5865	9709	int is_reset = 0;
5866	9710	int check_cnt = 0;
5867	9711	bool is_mod_timer = false;
5868		- struct rkmodule_vicap_reset_info rst_info;
5869	9712
5870		- for (i = 0; i < hw->dev_num; i++) {
5871		- dev = hw->cif_dev[i];
5872		- is_reset = rkcif_detect_reset_event_all_dev(dev,
5873		- &check_cnt,
	9713	+ for (i = 0; i < dev->num_channels; i++) {
	9714	+ stream = &dev->stream[i];
	9715	+ if (stream->state == RKCIF_STATE_STREAMING) {
	9716	+ is_reset = rkcif_detect_reset_event(stream,
	9717	+ timer,
	9718	+ check_cnt,
5874	9719	&is_mod_timer);
5875		- if (is_reset)
5876		- break;
5877		- }
5878		-
5879		- if (hw_timer->monitor_mode == RKCIF_MONITOR_MODE_HOTPLUG && is_reset) {
5880		- for (i = 0; i < hw->dev_num; i++) {
5881		- dev = hw->cif_dev[i];
5882		- rst_info.is_reset = 0;
5883		- if (dev->terminal_sensor.sd)
5884		- v4l2_subdev_call(dev->terminal_sensor.sd, core, ioctl,
5885		- RKMODULE_SET_VICAP_RST_INFO, &rst_info);
	9720	+ check_cnt++;
	9721	+ if (is_reset)
	9722	+ break;
5886	9723	}
5887	9724	}
5888		-
5889	9725	if (!is_reset && is_mod_timer)
5890		- mod_timer(&hw_timer->timer, jiffies + hw_timer->cycle_jif);
	9726	+ mod_timer(&timer->timer, jiffies + timer->cycle);
5891	9727
5892		- hw_timer->run_cnt += 1;
	9728	+ timer->run_cnt += 1;
5893	9729
5894		- if (!check_cnt && !is_reset) {
5895		- spin_lock_irqsave(&hw_timer->timer_lock, flags);
5896		- hw_timer->is_running = false;
5897		- spin_unlock_irqrestore(&hw_timer->timer_lock, flags);
	9730	+ if (!check_cnt) {
	9731	+ spin_lock_irqsave(&timer->timer_lock, flags);
	9732	+ timer->is_triggered = false;
	9733	+ timer->is_running = false;
	9734	+ spin_unlock_irqrestore(&timer->timer_lock, flags);
5898	9735
5899		- dev_info(hw->dev,
5900		- "all stream is stopped, stop reset detect!\n");
	9736	+ v4l2_info(&dev->v4l2_dev,
	9737	+ "all stream is stopped, stop reset detect!\n");
5901	9738	}
5902	9739	}
5903	9740
5904	9741	int rkcif_reset_notifier(struct notifier_block *nb,
5905	9742	unsigned long action, void *data)
5906	9743	{
5907		- struct rkcif_device *dev = container_of(nb, struct rkcif_device, reset_notifier);
5908		- struct rkcif_timer *timer = &dev->reset_watchdog_timer;
5909		- struct rkcif_hw_timer *hw_timer = &dev->hw_dev->hw_timer;
	9744	+ struct rkcif_hw *hw = container_of(nb, struct rkcif_hw, reset_notifier);
	9745	+ struct rkcif_device *dev = NULL;
	9746	+ struct rkcif_timer *timer = NULL;
5910	9747	unsigned long flags, val;
	9748	+ u32 *csi_idx = data;
	9749	+ int i = 0;
	9750	+ bool is_match_dev = false;
5911	9751
5912		- if (hw_timer->is_running) {
	9752	+ for (i = 0; i < hw->dev_num; i++) {
	9753	+ dev = hw->cif_dev[i];
	9754	+ if (*csi_idx == dev->csi_host_idx) {
	9755	+ is_match_dev = true;
	9756	+ break;
	9757	+ }
	9758	+ }
	9759	+ if (!is_match_dev)
	9760	+ return -EINVAL;
	9761	+ timer = &dev->reset_watchdog_timer;
	9762	+ if (timer->is_running) {
5913	9763	val = action & CSI2_ERR_COUNT_ALL_MASK;
5914	9764	spin_lock_irqsave(&timer->csi2_err_lock, flags);
5915		- if ((val % hw_timer->err_ref_cnt) == 0) {
	9765	+ if ((val % timer->csi2_err_ref_cnt) == 0) {
5916	9766	timer->notifer_called_cnt++;
5917	9767	if ((timer->notifer_called_cnt % 2) == 0)
5918	9768	timer->csi2_err_cnt_even = val;
..	..	@@ -5930,7 +9780,12 @@
5930	9780	static void rkcif_modify_line_int(struct rkcif_stream *stream, bool en)
5931	9781	{
5932	9782	struct rkcif_device *cif_dev = stream->cifdev;
	9783	+ u32 line_intr_en = 0;
5933	9784
	9785	+ if (cif_dev->chip_id >= CHIP_RK3588_CIF)
	9786	+ line_intr_en = CSI_LINE_INTEN_RK3588(stream->id);
	9787	+ else
	9788	+ line_intr_en = CSI_LINE_INTEN(stream->id);
5934	9789	if (en) {
5935	9790	if (cif_dev->wait_line_bak != cif_dev->wait_line) {
5936	9791	cif_dev->wait_line_bak = cif_dev->wait_line;
..	..	@@ -5940,44 +9795,73 @@
5940	9795	cif_dev->wait_line << 16 \| cif_dev->wait_line);
5941	9796	}
5942	9797	rkcif_write_register_or(cif_dev, CIF_REG_MIPI_LVDS_INTEN,
5943		- CSI_LINE_INTEN(stream->id));
	9798	+ line_intr_en);
	9799	+ stream->is_line_inten = true;
5944	9800	} else {
5945	9801	rkcif_write_register_and(cif_dev, CIF_REG_MIPI_LVDS_INTEN,
5946		- ~(CSI_LINE_INTEN(stream->id)));
	9802	+ ~line_intr_en);
5947	9803	}
5948	9804	}
5949	9805
5950	9806	static void rkcif_detect_wake_up_mode_change(struct rkcif_stream *stream)
5951	9807	{
5952	9808	struct rkcif_device *cif_dev = stream->cifdev;
	9809	+ struct sditf_priv *priv = cif_dev->sditf[0];
	9810	+ bool is_change = false;
	9811	+ int ch = 0;
	9812	+ int i = 0;
	9813	+
	9814	+ if (!priv \|\| priv->mode.rdbk_mode == RKISP_VICAP_ONLINE)
	9815	+ return;
	9816	+
	9817	+ if ((cif_dev->hdr.hdr_mode == NO_HDR \|\| cif_dev->hdr.hdr_mode == HDR_COMPR) &&
	9818	+ stream->id == RKCIF_STREAM_MIPI_ID0) {
	9819	+ if (cif_dev->wait_line != cif_dev->wait_line_cache)
	9820	+ cif_dev->wait_line = cif_dev->wait_line_cache;
	9821	+ } else if (cif_dev->hdr.hdr_mode == HDR_X2 && stream->id == RKCIF_STREAM_MIPI_ID1) {
	9822	+ if (cif_dev->wait_line != cif_dev->wait_line_cache)
	9823	+ cif_dev->wait_line = cif_dev->wait_line_cache;
	9824	+ } else if (cif_dev->hdr.hdr_mode == HDR_X3 && stream->id == RKCIF_STREAM_MIPI_ID2) {
	9825	+ if (cif_dev->wait_line != cif_dev->wait_line_cache)
	9826	+ cif_dev->wait_line = cif_dev->wait_line_cache;
	9827	+ }
5953	9828
5954	9829	if (cif_dev->wait_line && (!stream->is_line_wake_up)) {
	9830	+ is_change = true;
5955	9831	stream->is_line_wake_up = true;
5956	9832	if (stream->frame_phase == CIF_CSI_FRAME0_READY)
5957	9833	stream->line_int_cnt = 1;
5958	9834	else if (stream->frame_phase == CIF_CSI_FRAME1_READY)
5959	9835	stream->line_int_cnt = 0;
	9836	+ if (cif_dev->hdr.hdr_mode == HDR_X2) {
	9837	+ cif_dev->stream[0].is_line_wake_up = true;
	9838	+ cif_dev->stream[0].line_int_cnt = stream->line_int_cnt;
	9839	+ } else if (cif_dev->hdr.hdr_mode == HDR_X3) {
	9840	+ cif_dev->stream[0].is_line_wake_up = true;
	9841	+ cif_dev->stream[1].is_line_wake_up = true;
	9842	+ cif_dev->stream[0].line_int_cnt = stream->line_int_cnt;
	9843	+ cif_dev->stream[1].line_int_cnt = stream->line_int_cnt;
	9844	+ }
5960	9845	} else if ((cif_dev->wait_line == 0) && stream->is_line_wake_up) {
5961	9846	stream->is_line_wake_up = false;
5962	9847	}
5963	9848	if (stream->is_line_wake_up) {
5964		- rkcif_modify_line_int(stream, true);
5965		- stream->is_line_inten = true;
5966		- }
5967		-
5968		- if (cif_dev->hdr.mode == NO_HDR && stream->id == RKCIF_STREAM_MIPI_ID0) {
5969		- if (cif_dev->wait_line != cif_dev->wait_line_cache)
5970		- cif_dev->wait_line = cif_dev->wait_line_cache;
5971		- } else if (cif_dev->hdr.mode == HDR_X2 && stream->id == RKCIF_STREAM_MIPI_ID1) {
5972		- if (cif_dev->wait_line != cif_dev->wait_line_cache)
5973		- cif_dev->wait_line = cif_dev->wait_line_cache;
5974		- } else if (cif_dev->hdr.mode == HDR_X3 && stream->id == RKCIF_STREAM_MIPI_ID2) {
5975		- if (cif_dev->wait_line != cif_dev->wait_line_cache)
5976		- cif_dev->wait_line = cif_dev->wait_line_cache;
	9849	+ if (is_change) {
	9850	+ if (cif_dev->hdr.hdr_mode == HDR_X2)
	9851	+ ch = 2;
	9852	+ else if (cif_dev->hdr.hdr_mode == HDR_X3)
	9853	+ ch = 3;
	9854	+ else
	9855	+ ch = 1;
	9856	+ for (i = 0; i < ch; i++)
	9857	+ rkcif_modify_line_int(&cif_dev->stream[i], true);
	9858	+ } else {
	9859	+ rkcif_modify_line_int(stream, true);
	9860	+ }
5977	9861	}
5978	9862	}
5979	9863
5980		-static u32 rkisp_mbus_pixelcode_to_v4l2(u32 pixelcode)
	9864	+u32 rkcif_mbus_pixelcode_to_v4l2(u32 pixelcode)
5981	9865	{
5982	9866	u32 pixelformat;
5983	9867
..	..	@@ -6066,13 +9950,14 @@
6066	9950	v4l2_subdev_call(cif_dev->terminal_sensor.sd, pad, get_fmt, NULL, &fmt);
6067	9951
6068	9952	memset(&pixm, 0, sizeof(pixm));
6069		- pixm.pixelformat = rkisp_mbus_pixelcode_to_v4l2(fmt.format.code);
	9953	+ pixm.pixelformat = rkcif_mbus_pixelcode_to_v4l2(fmt.format.code);
6070	9954	pixm.width = fmt.format.width;
6071	9955	pixm.height = fmt.format.height;
6072	9956
6073	9957	memset(&input_sel, 0, sizeof(input_sel));
6074	9958	input_sel.pad = i;
6075	9959	input_sel.target = V4L2_SEL_TGT_CROP_BOUNDS;
	9960	+ input_sel.which = V4L2_SUBDEV_FORMAT_ACTIVE;
6076	9961	ret = v4l2_subdev_call(cif_dev->terminal_sensor.sd,
6077	9962	pad, get_selection, NULL,
6078	9963	&input_sel);
..	..	@@ -6085,13 +9970,1403 @@
6085	9970	}
6086	9971	}
6087	9972
6088		-#define CSI_START_INTSTAT(id) (0x3 << ((id) * 2))
	9973	+void rkcif_enable_dma_capture(struct rkcif_stream *stream, bool is_only_enable)
	9974	+{
	9975	+ struct rkcif_device *cif_dev = stream->cifdev;
	9976	+ struct v4l2_mbus_config *mbus_cfg = &cif_dev->active_sensor->mbus;
	9977	+ struct csi_channel_info *channel = &cif_dev->channels[stream->id];
	9978	+ u32 val = 0;
	9979	+
	9980	+ if (stream->buf_owner == RKCIF_DMAEN_BY_ISP)
	9981	+ stream->buf_owner = RKCIF_DMAEN_BY_ISP_TO_VICAP;
	9982	+
	9983	+ atomic_dec(&cif_dev->streamoff_cnt);
	9984	+ if (stream->dma_en) {
	9985	+ stream->dma_en \|= stream->to_en_dma;
	9986	+ stream->to_en_dma = 0;
	9987	+ return;
	9988	+ }
	9989	+
	9990	+ stream->dma_en \|= stream->to_en_dma;
	9991	+ if (!is_only_enable) {
	9992	+ if (stream->to_en_dma == RKCIF_DMAEN_BY_VICAP) {
	9993	+ rkcif_assign_new_buffer_pingpong(stream,
	9994	+ RKCIF_YUV_ADDR_STATE_INIT,
	9995	+ stream->id);
	9996	+ rkcif_write_register(cif_dev, get_reg_index_of_frm0_y_vlw(stream->id),
	9997	+ channel->virtual_width);
	9998	+ } else if (stream->to_en_dma == RKCIF_DMAEN_BY_ISP) {
	9999	+ rkcif_assign_new_buffer_pingpong_toisp(stream,
	10000	+ RKCIF_YUV_ADDR_STATE_INIT,
	10001	+ stream->id);
	10002	+ }
	10003	+ }
	10004	+ if (mbus_cfg->type == V4L2_MBUS_CSI2_DPHY \|\|
	10005	+ mbus_cfg->type == V4L2_MBUS_CSI2_CPHY)
	10006	+ rkcif_write_register_or(cif_dev, CIF_REG_MIPI_LVDS_CTRL, 0x00010000);
	10007	+ else
	10008	+ rkcif_write_register_or(cif_dev, CIF_REG_DVP_CTRL, 0x00010000);
	10009	+ if (mbus_cfg->type == V4L2_MBUS_CSI2_DPHY \|\|
	10010	+ mbus_cfg->type == V4L2_MBUS_CSI2_CPHY) {
	10011	+ val = rkcif_read_register(cif_dev, get_reg_index_of_id_ctrl0(stream->id));
	10012	+ if (!stream->is_compact)
	10013	+ val \|= CSI_WRDDR_TYPE_RAW_UNCOMPACT;
	10014	+ else
	10015	+ val &= ~CSI_WRDDR_TYPE_RAW_UNCOMPACT;
	10016	+ val \|= CSI_DMA_ENABLE;
	10017	+ rkcif_write_register(cif_dev, get_reg_index_of_id_ctrl0(stream->id), val);
	10018	+ } else if (mbus_cfg->type == V4L2_MBUS_CCP2) {
	10019	+ val = rkcif_read_register(cif_dev, get_reg_index_of_lvds_id_ctrl0(stream->id));
	10020	+ if (!stream->is_compact)
	10021	+ val \|= CSI_WRDDR_TYPE_RAW_UNCOMPACT << 17;
	10022	+ else
	10023	+ val &= ~(CSI_WRDDR_TYPE_RAW_UNCOMPACT << 17);
	10024	+ val \|= LVDS_DMAEN_RV1106;
	10025	+ rkcif_write_register(cif_dev, get_reg_index_of_lvds_id_ctrl0(stream->id), val);
	10026	+ } else {
	10027	+ val = rkcif_read_register(cif_dev, CIF_REG_DVP_FOR);
	10028	+ if (!stream->is_compact)
	10029	+ val \|= CSI_WRDDR_TYPE_RAW_UNCOMPACT << 11;
	10030	+ else
	10031	+ val &= ~(CSI_WRDDR_TYPE_RAW_UNCOMPACT << 11);
	10032	+ rkcif_write_register(cif_dev, CIF_REG_DVP_FOR, val);
	10033	+ val = rkcif_read_register(cif_dev, CIF_REG_DVP_CTRL);
	10034	+ if (cif_dev->chip_id == CHIP_RK3588_CIF)
	10035	+ val \|= DVP_DMA_EN;
	10036	+ else if (cif_dev->chip_id == CHIP_RV1106_CIF)
	10037	+ val \|= DVP_SW_DMA_EN(stream->id);
	10038	+ rkcif_write_register(cif_dev, CIF_REG_DVP_CTRL, val);
	10039	+ }
	10040	+
	10041	+ stream->to_en_dma = 0;
	10042	+}
	10043	+
	10044	+static int rkcif_stop_dma_capture(struct rkcif_stream *stream)
	10045	+{
	10046	+ struct rkcif_device *cif_dev = stream->cifdev;
	10047	+ struct v4l2_mbus_config *mbus_cfg = &cif_dev->active_sensor->mbus;
	10048	+ u32 val = 0;
	10049	+
	10050	+ if (stream->buf_replace_cnt)
	10051	+ return -EINVAL;
	10052	+
	10053	+ stream->dma_en &= ~stream->to_stop_dma;
	10054	+ atomic_inc(&cif_dev->streamoff_cnt);
	10055	+ if (stream->dma_en != 0) {
	10056	+ if (stream->dma_en & RKCIF_DMAEN_BY_ISP)
	10057	+ stream->buf_owner = RKCIF_DMAEN_BY_ISP;
	10058	+ stream->to_stop_dma = 0;
	10059	+ return 0;
	10060	+ }
	10061	+
	10062	+ if (mbus_cfg->type == V4L2_MBUS_CSI2_DPHY \|\|
	10063	+ mbus_cfg->type == V4L2_MBUS_CSI2_CPHY) {
	10064	+ val = rkcif_read_register(cif_dev, get_reg_index_of_id_ctrl0(stream->id));
	10065	+ val &= ~CSI_DMA_ENABLE;
	10066	+ if (stream->is_stop_capture) {
	10067	+ val &= ~CSI_ENABLE_CAPTURE;
	10068	+ stream->is_stop_capture = false;
	10069	+ }
	10070	+ rkcif_write_register(cif_dev, get_reg_index_of_id_ctrl0(stream->id), val);
	10071	+ } else if (mbus_cfg->type == V4L2_MBUS_CCP2) {
	10072	+ val = rkcif_read_register(cif_dev, get_reg_index_of_lvds_id_ctrl0(stream->id));
	10073	+ val &= ~LVDS_DMAEN_RV1106;
	10074	+ if (stream->is_stop_capture) {
	10075	+ val &= ~ENABLE_CAPTURE;
	10076	+ stream->is_stop_capture = false;
	10077	+ }
	10078	+ rkcif_write_register(cif_dev, get_reg_index_of_lvds_id_ctrl0(stream->id), val);
	10079	+ } else {
	10080	+ val = rkcif_read_register(cif_dev, CIF_REG_DVP_CTRL);
	10081	+ if (cif_dev->chip_id == CHIP_RK3588_CIF)
	10082	+ val &= ~DVP_DMA_EN;
	10083	+ else if (cif_dev->chip_id == CHIP_RV1106_CIF)
	10084	+ val &= ~(DVP_SW_DMA_EN(stream->id));
	10085	+ if (stream->is_stop_capture) {
	10086	+ val &= ~ENABLE_CAPTURE;
	10087	+ stream->is_stop_capture = false;
	10088	+ }
	10089	+ rkcif_write_register(cif_dev, CIF_REG_DVP_CTRL, val);
	10090	+ }
	10091	+ stream->to_stop_dma = 0;
	10092	+ v4l2_dbg(4, rkcif_debug, &cif_dev->v4l2_dev,
	10093	+ "stream[%d] replace_cnt %d, y_addr 0x%x, 0x%x\n",
	10094	+ stream->id, stream->buf_replace_cnt,
	10095	+ rkcif_read_register(cif_dev, get_reg_index_of_frm0_y_addr(stream->id)),
	10096	+ rkcif_read_register(cif_dev, get_reg_index_of_frm1_y_addr(stream->id)));
	10097	+ return 0;
	10098	+}
	10099	+
	10100	+static void rkcif_send_sof(struct rkcif_device *cif_dev)
	10101	+{
	10102	+ struct v4l2_mbus_config *mbus = &cif_dev->active_sensor->mbus;
	10103	+ struct csi2_dev *csi;
	10104	+
	10105	+ if (mbus->type == V4L2_MBUS_CSI2_DPHY \|\|
	10106	+ mbus->type == V4L2_MBUS_CSI2_CPHY) {
	10107	+ csi = container_of(cif_dev->active_sensor->sd, struct csi2_dev, sd);
	10108	+ rkcif_csi2_event_inc_sof(csi);
	10109	+ } else if (mbus->type == V4L2_MBUS_CCP2) {
	10110	+ rkcif_lvds_event_inc_sof(cif_dev);
	10111	+ } else {
	10112	+ rkcif_dvp_event_inc_sof(cif_dev);
	10113	+ }
	10114	+}
	10115	+
	10116	+static int rkcif_g_toisp_ch(unsigned int intstat_glb, int index)
	10117	+{
	10118	+ if (intstat_glb & TOISP_END_CH0(index))
	10119	+ return RKCIF_TOISP_CH0;
	10120	+ if (intstat_glb & TOISP_END_CH1(index))
	10121	+ return RKCIF_TOISP_CH1;
	10122	+ if (intstat_glb & TOISP_END_CH2(index))
	10123	+ return RKCIF_TOISP_CH2;
	10124	+
	10125	+ return -EINVAL;
	10126	+}
	10127	+
	10128	+static int rkcif_g_toisp_fs(unsigned int intstat_glb, int index)
	10129	+{
	10130	+ if (intstat_glb & TOISP_FS_CH0(index))
	10131	+ return RKCIF_TOISP_CH0;
	10132	+ if (intstat_glb & TOISP_FS_CH1(index))
	10133	+ return RKCIF_TOISP_CH1;
	10134	+ if (intstat_glb & TOISP_FS_CH2(index))
	10135	+ return RKCIF_TOISP_CH2;
	10136	+
	10137	+ return -EINVAL;
	10138	+}
	10139	+
	10140	+static void rkcif_toisp_check_stop_status(struct sditf_priv *priv,
	10141	+ unsigned int intstat_glb,
	10142	+ int index)
	10143	+{
	10144	+ int ch = 0;
	10145	+ struct rkcif_stream *stream;
	10146	+ int src_id = 0;
	10147	+ int i = 0;
	10148	+ u32 val = 0;
	10149	+ u64 cur_time = 0;
	10150	+ int on = 0;
	10151	+
	10152	+ for (i = 0; i < TOISP_CH_MAX; i++) {
	10153	+ ch = rkcif_g_toisp_ch(intstat_glb, index);
	10154	+ if (ch >= 0) {
	10155	+ src_id = priv->toisp_inf.ch_info[ch].id;
	10156	+ if (src_id == 24)
	10157	+ stream = &priv->cif_dev->stream[0];
	10158	+ else
	10159	+ stream = &priv->cif_dev->stream[src_id % 4];
	10160	+ if (stream->stopping) {
	10161	+ v4l2_dbg(3, rkcif_debug, &priv->cif_dev->v4l2_dev,
	10162	+ "stream[%d] stop\n",
	10163	+ stream->id);
	10164	+ rkcif_stream_stop(stream);
	10165	+ stream->stopping = false;
	10166	+ wake_up(&stream->wq_stopped);
	10167	+ }
	10168	+ if (stream->cifdev->sensor_state_change) {
	10169	+ rkcif_dphy_quick_stream(stream->cifdev, on);
	10170	+ stream->cifdev->sensor_work.on = stream->cifdev->sensor_state;
	10171	+ schedule_work(&stream->cifdev->sensor_work.work);
	10172	+ stream->cifdev->sensor_state_change = false;
	10173	+ if (stream->is_wait_stop_complete) {
	10174	+ stream->is_wait_stop_complete = false;
	10175	+ complete(&stream->stop_complete);
	10176	+ }
	10177	+ }
	10178	+ if (stream->is_single_cap && (!stream->cur_skip_frame) &&
	10179	+ (stream->cifdev->hdr.hdr_mode == NO_HDR \|\|
	10180	+ (stream->cifdev->hdr.hdr_mode == HDR_X2 && stream->id == 1) \|\|
	10181	+ (stream->cifdev->hdr.hdr_mode == HDR_X3 && stream->id == 2))) {
	10182	+ rkcif_dphy_quick_stream(stream->cifdev, on);
	10183	+ stream->cifdev->sensor_work.on = 0;
	10184	+ schedule_work(&stream->cifdev->sensor_work.work);
	10185	+ stream->is_single_cap = false;
	10186	+ }
	10187	+ if (stream->cur_skip_frame &&
	10188	+ (stream->cifdev->hdr.hdr_mode == NO_HDR \|\|
	10189	+ (stream->cifdev->hdr.hdr_mode == HDR_X2 && stream->id == 1) \|\|
	10190	+ (stream->cifdev->hdr.hdr_mode == HDR_X3 && stream->id == 2)))
	10191	+ stream->cur_skip_frame--;
	10192	+ if (stream->cifdev->chip_id >= CHIP_RV1106_CIF)
	10193	+ rkcif_modify_frame_skip_config(stream);
	10194	+ if (stream->cifdev->rdbk_debug &&
	10195	+ stream->frame_idx < 15)
	10196	+ v4l2_info(&priv->cif_dev->v4l2_dev,
	10197	+ "stream[%d] toisp fe %d\n",
	10198	+ stream->id,
	10199	+ stream->frame_idx - 1);
	10200	+
	10201	+ switch (ch) {
	10202	+ case RKCIF_TOISP_CH0:
	10203	+ val = TOISP_END_CH0(index);
	10204	+ intstat_glb = intstat_glb & (~val);
	10205	+ break;
	10206	+ case RKCIF_TOISP_CH1:
	10207	+ val = TOISP_END_CH1(index);
	10208	+ intstat_glb = intstat_glb & (~val);
	10209	+ break;
	10210	+ case RKCIF_TOISP_CH2:
	10211	+ val = TOISP_END_CH2(index);
	10212	+ intstat_glb = intstat_glb & (~val);
	10213	+ break;
	10214	+ default:
	10215	+ break;
	10216	+ }
	10217	+ }
	10218	+ ch = rkcif_g_toisp_fs(intstat_glb, index);
	10219	+ if (ch >= 0) {
	10220	+ src_id = priv->toisp_inf.ch_info[ch].id;
	10221	+ if (src_id == 24)
	10222	+ stream = &priv->cif_dev->stream[0];
	10223	+ else
	10224	+ stream = &priv->cif_dev->stream[src_id % 4];
	10225	+ if (stream->id == 0)
	10226	+ rkcif_send_sof(stream->cifdev);
	10227	+ stream->frame_idx++;
	10228	+ cur_time = rkcif_time_get_ns(stream->cifdev);
	10229	+ stream->readout.readout_time = cur_time - stream->readout.fs_timestamp;
	10230	+ stream->readout.fs_timestamp = cur_time;
	10231	+ stream->buf_wake_up_cnt++;
	10232	+ if (stream->frame_idx % 2)
	10233	+ stream->fps_stats.frm0_timestamp = rkcif_time_get_ns(stream->cifdev);
	10234	+ else
	10235	+ stream->fps_stats.frm1_timestamp = rkcif_time_get_ns(stream->cifdev);
	10236	+ if (stream->cifdev->rdbk_debug &&
	10237	+ stream->frame_idx < 15)
	10238	+ v4l2_info(&priv->cif_dev->v4l2_dev,
	10239	+ "stream[%d] toisp sof seq %d\n",
	10240	+ stream->id,
	10241	+ stream->frame_idx - 1);
	10242	+ if (stream->to_en_dma)
	10243	+ rkcif_enable_dma_capture(stream, false);
	10244	+ if (stream->to_en_scale) {
	10245	+ stream->to_en_scale = false;
	10246	+ rkcif_scale_start(stream->scale_vdev);
	10247	+ }
	10248	+ switch (ch) {
	10249	+ case RKCIF_TOISP_CH0:
	10250	+ val = TOISP_FS_CH0(index);
	10251	+ intstat_glb = intstat_glb & (~val);
	10252	+ break;
	10253	+ case RKCIF_TOISP_CH1:
	10254	+ val = TOISP_FS_CH1(index);
	10255	+ intstat_glb = intstat_glb & (~val);
	10256	+ break;
	10257	+ case RKCIF_TOISP_CH2:
	10258	+ val = TOISP_FS_CH2(index);
	10259	+ intstat_glb = intstat_glb & (~val);
	10260	+ break;
	10261	+ default:
	10262	+ break;
	10263	+ }
	10264	+ }
	10265	+ }
	10266	+}
	10267	+
	10268	+void rkcif_irq_handle_toisp(struct rkcif_device *cif_dev, unsigned int intstat_glb)
	10269	+{
	10270	+ int i = 0;
	10271	+ bool to_check = false;
	10272	+ struct sditf_priv *priv = cif_dev->sditf[0];
	10273	+
	10274	+ if (!priv \|\| priv->mode.rdbk_mode != RKISP_VICAP_ONLINE)
	10275	+ return;
	10276	+
	10277	+ for (i = 0; i < 2; i++) {
	10278	+ if (priv->toisp_inf.link_mode == TOISP0 &&
	10279	+ i == 0) {
	10280	+ to_check = true;
	10281	+ } else if (priv->toisp_inf.link_mode == TOISP1 &&
	10282	+ i == 1) {
	10283	+ to_check = true;
	10284	+ } else if (priv->toisp_inf.link_mode == TOISP_UNITE &&
	10285	+ i == 1) {
	10286	+ to_check = true;
	10287	+ }
	10288	+ if (to_check)
	10289	+ rkcif_toisp_check_stop_status(priv, intstat_glb, i);
	10290	+ }
	10291	+}
	10292	+
	10293	+static int rkcif_check_group_sync_state(struct rkcif_device *cif_dev)
	10294	+{
	10295	+ struct rkcif_stream *detect_stream = &cif_dev->stream[0];
	10296	+ struct rkcif_stream *next_stream = NULL;
	10297	+ struct rkcif_hw *hw = cif_dev->hw_dev;
	10298	+ u64 fs_interval = 0;
	10299	+ int i = 0;
	10300	+ int ret = 0;
	10301	+ struct rkcif_multi_sync_config *sync_config;
	10302	+
	10303	+ sync_config = &hw->sync_config[cif_dev->sync_cfg.group];
	10304	+ sync_config->sync_code \|= BIT(cif_dev->csi_host_idx);
	10305	+ v4l2_dbg(3, rkcif_debug, &cif_dev->v4l2_dev,
	10306	+ "sync code 0x%x, mask 0x%x, update 0x%x, cache 0x%x, timestamp %llu\n",
	10307	+ sync_config->sync_code,
	10308	+ sync_config->sync_mask,
	10309	+ sync_config->update_code,
	10310	+ sync_config->update_cache,
	10311	+ detect_stream->readout.fs_timestamp);
	10312	+
	10313	+ if (sync_config->sync_code != sync_config->sync_mask)
	10314	+ return -EINVAL;
	10315	+
	10316	+ for (i = 0; i < sync_config->dev_cnt; i++) {
	10317	+ if (sync_config->mode == RKCIF_MASTER_MASTER) {
	10318	+ if (i < sync_config->ext_master.count)
	10319	+ next_stream = &sync_config->ext_master.cif_dev[i]->stream
	10320	+ [0];
	10321	+ else
	10322	+ next_stream = &sync_config->int_master.cif_dev[0]->stream
	10323	+ [0];
	10324	+ } else if (sync_config->mode == RKCIF_MASTER_SLAVE) {
	10325	+ if (i < sync_config->slave.count)
	10326	+ next_stream = &sync_config->slave.cif_dev[i]->stream
	10327	+ [0];
	10328	+ else
	10329	+ next_stream = &sync_config->int_master.cif_dev[0]->stream
	10330	+ [0];
	10331	+ } else {
	10332	+ v4l2_err(&cif_dev->v4l2_dev,
	10333	+ "ERROR: invalid group sync mode\n");
	10334	+ ret = -EINVAL;
	10335	+ break;
	10336	+ }
	10337	+ if (detect_stream == next_stream)
	10338	+ continue;
	10339	+ fs_interval = abs(detect_stream->readout.fs_timestamp - next_stream->readout.fs_timestamp);
	10340	+ if (fs_interval > RKCIF_MAX_INTERVAL_NS) {
	10341	+ ret = -EINVAL;
	10342	+ break;
	10343	+ }
	10344	+ }
	10345	+ return ret;
	10346	+}
	10347	+
	10348	+static void rkcif_deal_sof(struct rkcif_device *cif_dev)
	10349	+{
	10350	+ struct rkcif_stream *detect_stream = &cif_dev->stream[0];
	10351	+ struct rkcif_hw *hw = cif_dev->hw_dev;
	10352	+ struct rkcif_device *tmp_dev = NULL;
	10353	+ unsigned long flags;
	10354	+ int i = 0;
	10355	+ int ret = 0;
	10356	+
	10357	+ if (cif_dev->chip_id < CHIP_RK3588_CIF)
	10358	+ detect_stream->fs_cnt_in_single_frame++;
	10359	+ spin_lock_irqsave(&detect_stream->fps_lock, flags);
	10360	+ detect_stream->readout.fs_timestamp = rkcif_time_get_ns(cif_dev);
	10361	+ spin_unlock_irqrestore(&detect_stream->fps_lock, flags);
	10362	+
	10363	+ if (cif_dev->sync_cfg.type != RKCIF_NOSYNC_MODE) {
	10364	+ struct rkcif_multi_sync_config *sync_config;
	10365	+
	10366	+ sync_config = &hw->sync_config[cif_dev->sync_cfg.group];
	10367	+ ret = rkcif_check_group_sync_state(cif_dev);
	10368	+ if (!ret) {
	10369	+ sync_config->sync_code = 0;
	10370	+ sync_config->frame_idx++;
	10371	+ spin_lock_irqsave(&hw->group_lock, flags);
	10372	+ sync_config->update_cache = sync_config->sync_mask;
	10373	+ if (!sync_config->update_code) {
	10374	+ sync_config->update_code = sync_config->update_cache;
	10375	+ sync_config->update_cache = 0;
	10376	+ }
	10377	+ spin_unlock_irqrestore(&hw->group_lock, flags);
	10378	+ for (i = 0; i < sync_config->dev_cnt; i++) {
	10379	+ if (sync_config->mode == RKCIF_MASTER_MASTER) {
	10380	+ if (i < sync_config->ext_master.count)
	10381	+ tmp_dev = sync_config->ext_master.cif_dev[i];
	10382	+ else
	10383	+ tmp_dev = sync_config->int_master.cif_dev[0];
	10384	+ } else if (sync_config->mode == RKCIF_MASTER_SLAVE) {
	10385	+ if (i < sync_config->slave.count)
	10386	+ tmp_dev = sync_config->slave.cif_dev[i];
	10387	+ else
	10388	+ tmp_dev = sync_config->int_master.cif_dev[0];
	10389	+ } else {
	10390	+ v4l2_err(&cif_dev->v4l2_dev,
	10391	+ "ERROR: invalid group sync mode\n");
	10392	+ }
	10393	+ if (tmp_dev) {
	10394	+ rkcif_send_sof(tmp_dev);
	10395	+ tmp_dev->stream[0].frame_idx = sync_config->frame_idx;
	10396	+ }
	10397	+ }
	10398	+ }
	10399	+ } else {
	10400	+ if (!cif_dev->sditf[0] \|\| cif_dev->sditf[0]->mode.rdbk_mode) {
	10401	+ rkcif_send_sof(cif_dev);
	10402	+ detect_stream->frame_idx++;
	10403	+ }
	10404	+ if (detect_stream->cifdev->rdbk_debug &&
	10405	+ detect_stream->frame_idx < 15 &&
	10406	+ (!cif_dev->sditf[0] \|\| cif_dev->sditf[0]->mode.rdbk_mode))
	10407	+ v4l2_info(&cif_dev->v4l2_dev,
	10408	+ "stream[%d] sof %d %lld\n",
	10409	+ detect_stream->id,
	10410	+ detect_stream->frame_idx - 1,
	10411	+ rkcif_time_get_ns(cif_dev));
	10412	+ }
	10413	+}
	10414	+
	10415	+unsigned int rkcif_irq_global(struct rkcif_device *cif_dev)
	10416	+{
	10417	+ unsigned int intstat_glb = 0;
	10418	+
	10419	+ intstat_glb = rkcif_read_register(cif_dev, CIF_REG_GLB_INTST);
	10420	+ if (intstat_glb)
	10421	+ v4l2_dbg(2, rkcif_debug, &cif_dev->v4l2_dev,
	10422	+ "intstat_glb 0x%x\n",
	10423	+ intstat_glb);
	10424	+ else
	10425	+ return intstat_glb;
	10426	+
	10427	+ if (intstat_glb & SCALE_TOISP_AXI0_ERR) {
	10428	+ v4l2_err(&cif_dev->v4l2_dev,
	10429	+ "ERROR: scale channel, AXI0 bus err intstat_glb:0x%x !!\n",
	10430	+ intstat_glb);
	10431	+ return 0;
	10432	+ }
	10433	+ if (intstat_glb & SCALE_TOISP_AXI1_ERR) {
	10434	+ v4l2_err(&cif_dev->v4l2_dev,
	10435	+ "ERROR: scale channel, AXI1 bus err intstat_glb:0x%x !!\n",
	10436	+ intstat_glb);
	10437	+ return 0;
	10438	+ }
	10439	+ rkcif_irq_handle_scale(cif_dev, intstat_glb);
	10440	+ return intstat_glb;
	10441	+}
	10442	+
	10443	+static bool rkcif_check_buffer_prepare(struct rkcif_stream *stream)
	10444	+{
	10445	+ struct rkcif_device *cif_dev = stream->cifdev;
	10446	+ unsigned long flags;
	10447	+ bool is_update = false;
	10448	+ struct rkcif_multi_sync_config *sync_config;
	10449	+
	10450	+ spin_lock_irqsave(&cif_dev->hw_dev->group_lock, flags);
	10451	+ sync_config = &cif_dev->hw_dev->sync_config[cif_dev->sync_cfg.group];
	10452	+ if (stream->id == 0 &&
	10453	+ sync_config->update_code & BIT(cif_dev->csi_host_idx)) {
	10454	+ is_update = true;
	10455	+ sync_config->update_code &= ~(BIT(cif_dev->csi_host_idx));
	10456	+ if (!sync_config->update_code &&
	10457	+ sync_config->update_cache) {
	10458	+ sync_config->update_code = sync_config->update_cache;
	10459	+ sync_config->update_cache = 0;
	10460	+ }
	10461	+ } else {
	10462	+ if (cif_dev->rdbk_buf[RDBK_L])
	10463	+ is_update = true;
	10464	+ }
	10465	+
	10466	+ spin_unlock_irqrestore(&cif_dev->hw_dev->group_lock, flags);
	10467	+ return is_update;
	10468	+}
	10469	+
	10470	+static bool rkcif_check_single_dev_stream_on(struct rkcif_hw *hw)
	10471	+{
	10472	+ struct rkcif_device *cif_dev = NULL;
	10473	+ struct rkcif_stream *stream = NULL;
	10474	+ int i = 0, j = 0;
	10475	+ int stream_cnt = 0;
	10476	+
	10477	+ if (hw->dev_num == 1)
	10478	+ return true;
	10479	+ for (i = 0; i < hw->dev_num; i++) {
	10480	+ cif_dev = hw->cif_dev[i];
	10481	+ for (j = 0; j < RKCIF_MAX_STREAM_MIPI; j++) {
	10482	+ stream = &cif_dev->stream[j];
	10483	+ if (stream->state == RKCIF_STATE_STREAMING \|\|
	10484	+ stream->state == RKCIF_STATE_RESET_IN_STREAMING) {
	10485	+ stream_cnt++;
	10486	+ break;
	10487	+ }
	10488	+ }
	10489	+ }
	10490	+ if (stream_cnt > 1)
	10491	+ return false;
	10492	+ return true;
	10493	+}
	10494	+
	10495	+static void rkcif_get_resmem_head(struct rkcif_device *cif_dev)
	10496	+{
	10497	+ void *resmem_va = phys_to_virt(cif_dev->resmem_pa);
	10498	+ struct rkisp_thunderboot_resmem_head *head = NULL;
	10499	+ int size = 0;
	10500	+ int offset = 0;
	10501	+ int ret = 0;
	10502	+ int cam_idx = 0;
	10503	+ char cam_idx_str[3] = {0};
	10504	+
	10505	+ if (!cif_dev->is_rtt_suspend)
	10506	+ return;
	10507	+ strscpy(cam_idx_str, cif_dev->terminal_sensor.sd->name + 1, 2);
	10508	+ cam_idx_str[2] = '\0';
	10509	+ ret = kstrtoint(cam_idx_str, 0, &cam_idx);
	10510	+ if (ret) {
	10511	+ v4l2_err(&cif_dev->v4l2_dev,
	10512	+ "get camera index fail\n");
	10513	+ return;
	10514	+ }
	10515	+
	10516	+ if (cif_dev->chip_id == CHIP_RV1106_CIF) {
	10517	+ size = sizeof(struct rkisp32_thunderboot_resmem_head);
	10518	+ offset = size * cam_idx;
	10519	+ }
	10520	+ /* currently, thunderboot with mcu only run one camera */
	10521	+ offset = 0;
	10522	+
	10523	+ if (size && size < cif_dev->resmem_size) {
	10524	+ dma_sync_single_for_cpu(cif_dev->dev, cif_dev->resmem_addr + offset,
	10525	+ size, DMA_FROM_DEVICE);
	10526	+ if (cif_dev->chip_id == CHIP_RV1106_CIF) {
	10527	+ struct rkisp32_thunderboot_resmem_head *tmp = resmem_va + offset;
	10528	+
	10529	+ head = &tmp->head;
	10530	+ cif_dev->resume_mode = head->rtt_mode;
	10531	+ cif_dev->nr_buf_size = head->nr_buf_size;
	10532	+ cif_dev->share_mem_size = head->share_mem_size;
	10533	+ cif_dev->thunderboot_sensor_num = head->camera_num;
	10534	+ }
	10535	+ }
	10536	+ v4l2_err(&cif_dev->v4l2_dev,
	10537	+ "get camera index %02x, resume_mode 0x%x, nr_buf_size %d\n",
	10538	+ cam_idx, cif_dev->resume_mode, cif_dev->nr_buf_size);
	10539	+}
	10540	+
	10541	+static int rkcif_subdevs_set_power(struct rkcif_device *cif_dev, int on)
	10542	+{
	10543	+ struct sditf_priv *priv = cif_dev->sditf[0];
	10544	+ int ret = 0;
	10545	+ int i = 0;
	10546	+
	10547	+ if (cif_dev->terminal_sensor.sd)
	10548	+ ret = v4l2_subdev_call(cif_dev->terminal_sensor.sd,
	10549	+ core, s_power, on);
	10550	+ if (priv && priv->is_combine_mode && cif_dev->sditf_cnt <= RKCIF_MAX_SDITF) {
	10551	+ for (i = 0; i < cif_dev->sditf_cnt; i++) {
	10552	+ if (cif_dev->sditf[i] && cif_dev->sditf[i]->sensor_sd)
	10553	+ v4l2_subdev_call(cif_dev->sditf[i]->sensor_sd, core,
	10554	+ s_power, on);
	10555	+ }
	10556	+ }
	10557	+ return ret;
	10558	+}
	10559	+
	10560	+static void rkcif_sensor_quick_streaming_cb(void *data)
	10561	+{
	10562	+ struct v4l2_subdev subdevs = (struct v4l2_subdev )data;
	10563	+ int on = 1;
	10564	+
	10565	+ v4l2_subdev_call(subdevs, core, ioctl,
	10566	+ RKMODULE_SET_QUICK_STREAM, &on);
	10567	+}
	10568	+
	10569	+static int rkcif_subdevs_set_stream(struct rkcif_device *cif_dev, int on)
	10570	+{
	10571	+ struct rkcif_pipeline *p = &cif_dev->pipe;
	10572	+ struct rkcif_sensor_info *terminal_sensor = &cif_dev->terminal_sensor;
	10573	+ struct sditf_priv *priv = cif_dev->sditf[0];
	10574	+ int i = 0;
	10575	+ int ret = 0;
	10576	+
	10577	+ for (i = 0; i < p->num_subdevs; i++) {
	10578	+ if (p->subdevs[i] == terminal_sensor->sd && on)
	10579	+ rkcif_set_sof(cif_dev, cif_dev->stream[0].frame_idx);
	10580	+ if (p->subdevs[i] == terminal_sensor->sd &&
	10581	+ cif_dev->chip_id == CHIP_RV1106_CIF) {
	10582	+ if (!rk_tb_mcu_is_done() && on) {
	10583	+ cif_dev->tb_client.data = p->subdevs[i];
	10584	+ cif_dev->tb_client.cb = rkcif_sensor_quick_streaming_cb;
	10585	+ rk_tb_client_register_cb(&cif_dev->tb_client);
	10586	+ } else {
	10587	+ ret = v4l2_subdev_call(p->subdevs[i], core, ioctl,
	10588	+ RKMODULE_SET_QUICK_STREAM, &on);
	10589	+ if (ret)
	10590	+ v4l2_dbg(1, rkcif_debug, &cif_dev->v4l2_dev,
	10591	+ "%s:quick stream %s subdev:%s failed\n",
	10592	+ __func__, on ? "on" : "off",
	10593	+ p->subdevs[i]->name);
	10594	+ }
	10595	+ } else {
	10596	+ ret = v4l2_subdev_call(p->subdevs[i], video, s_stream, on);
	10597	+ if (ret)
	10598	+ v4l2_dbg(1, rkcif_debug, &cif_dev->v4l2_dev,
	10599	+ "%s:stream %s subdev:%s failed\n",
	10600	+ __func__, on ? "on" : "off", p->subdevs[i]->name);
	10601	+ }
	10602	+ }
	10603	+
	10604	+ if (priv && priv->is_combine_mode && cif_dev->sditf_cnt <= RKCIF_MAX_SDITF) {
	10605	+ for (i = 0; i < cif_dev->sditf_cnt; i++) {
	10606	+ if (cif_dev->sditf[i] && cif_dev->sditf[i]->sensor_sd) {
	10607	+ ret = v4l2_subdev_call(cif_dev->sditf[i]->sensor_sd, video, s_stream, on);
	10608	+ if (ret)
	10609	+ v4l2_dbg(1, rkcif_debug, &cif_dev->v4l2_dev,
	10610	+ "%s:stream %s subdev:%s failed\n",
	10611	+ __func__, on ? "on" : "off",
	10612	+ cif_dev->sditf[i]->sensor_sd->name);
	10613	+ }
	10614	+ }
	10615	+ }
	10616	+ return ret;
	10617	+}
	10618	+
	10619	+int rkcif_stream_suspend(struct rkcif_device *cif_dev, int mode)
	10620	+{
	10621	+ struct rkcif_stream *stream = NULL;
	10622	+ struct rkcif_resume_info *resume_info = &cif_dev->reset_work.resume_info;
	10623	+ struct sditf_priv *priv = cif_dev->sditf[0];
	10624	+ int ret = 0;
	10625	+ int i = 0;
	10626	+ int sof_cnt = 0;
	10627	+ int on = 0;
	10628	+ int suspend_cnt = 0;
	10629	+
	10630	+ mutex_lock(&cif_dev->stream_lock);
	10631	+
	10632	+ if (priv && priv->mode.rdbk_mode == RKISP_VICAP_ONLINE && mode == RKCIF_RESUME_CIF)
	10633	+ goto out_suspend;
	10634	+
	10635	+ for (i = 0; i < RKCIF_MAX_STREAM_MIPI; i++) {
	10636	+ stream = &cif_dev->stream[i];
	10637	+
	10638	+ if (stream->state == RKCIF_STATE_STREAMING) {
	10639	+ suspend_cnt++;
	10640	+ v4l2_dbg(1, rkcif_debug, &cif_dev->v4l2_dev,
	10641	+ "stream[%d] stopping\n", stream->id);
	10642	+ if (stream->dma_en) {
	10643	+ stream->stopping = true;
	10644	+ ret = wait_event_timeout(stream->wq_stopped,
	10645	+ stream->state != RKCIF_STATE_STREAMING,
	10646	+ msecs_to_jiffies(500));
	10647	+ if (!ret) {
	10648	+ rkcif_stream_stop(stream);
	10649	+ stream->stopping = false;
	10650	+ }
	10651	+ } else {
	10652	+ rkcif_stream_stop(stream);
	10653	+ }
	10654	+
	10655	+ if (stream->id == RKCIF_STREAM_MIPI_ID0) {
	10656	+ sof_cnt = rkcif_get_sof(cif_dev);
	10657	+ v4l2_dbg(1, rkcif_debug, &cif_dev->v4l2_dev,
	10658	+ "%s: stream[%d] sync frmid & csi_sof, frm_id:%d, csi_sof:%d\n",
	10659	+ __func__,
	10660	+ stream->id,
	10661	+ stream->frame_idx,
	10662	+ sof_cnt);
	10663	+
	10664	+ resume_info->frm_sync_seq = stream->frame_idx;
	10665	+ }
	10666	+
	10667	+ stream->state = RKCIF_STATE_RESET_IN_STREAMING;
	10668	+ stream->is_fs_fe_not_paired = false;
	10669	+ stream->fs_cnt_in_single_frame = 0;
	10670	+
	10671	+ v4l2_dbg(1, rkcif_debug, &cif_dev->v4l2_dev,
	10672	+ "%s stop stream[%d] in streaming, frm_id:%d, csi_sof:%d\n",
	10673	+ __func__, stream->id, stream->frame_idx, rkcif_get_sof(cif_dev));
	10674	+
	10675	+ }
	10676	+ }
	10677	+
	10678	+ if (suspend_cnt == 0)
	10679	+ goto out_suspend;
	10680	+
	10681	+ if (!cif_dev->resume_mode)
	10682	+ rkcif_subdevs_set_power(cif_dev, on);
	10683	+
	10684	+ rkcif_subdevs_set_stream(cif_dev, on);
	10685	+
	10686	+out_suspend:
	10687	+ mutex_unlock(&cif_dev->stream_lock);
	10688	+ return 0;
	10689	+}
	10690	+
	10691	+int rkcif_stream_resume(struct rkcif_device *cif_dev, int mode)
	10692	+{
	10693	+ struct rkcif_stream *stream = NULL;
	10694	+ struct sditf_priv *priv = cif_dev->sditf[0];
	10695	+ struct v4l2_subdev *sd = NULL;
	10696	+ int ret = 0;
	10697	+ int i = 0;
	10698	+ u32 capture_mode = 0;
	10699	+ int on = 1;
	10700	+ int resume_cnt = 0;
	10701	+ unsigned long flags;
	10702	+ bool is_single_dev = false;
	10703	+ bool is_can_be_online = false;
	10704	+ struct rkisp_vicap_mode vicap_mode;
	10705	+
	10706	+ mutex_lock(&cif_dev->stream_lock);
	10707	+
	10708	+ rkcif_get_resmem_head(cif_dev);
	10709	+ is_single_dev = rkcif_check_single_dev_stream_on(cif_dev->hw_dev);
	10710	+ is_can_be_online = rkcif_check_can_be_online(cif_dev);
	10711	+ if (cif_dev->resume_mode == RKISP_RTT_MODE_ONE_FRAME) {
	10712	+ if (cif_dev->is_rtt_suspend) {
	10713	+ capture_mode = RKCIF_STREAM_MODE_TOISP_RDBK;
	10714	+ if (priv)
	10715	+ priv->mode.rdbk_mode = RKISP_VICAP_RDBK_AUTO;
	10716	+ } else {
	10717	+ if (priv && priv->mode.rdbk_mode == RKISP_VICAP_ONLINE) {
	10718	+ if (cif_dev->chip_id == CHIP_RV1106_CIF) {
	10719	+ capture_mode = RKCIF_STREAM_MODE_TOISP_RDBK;
	10720	+ priv->mode.rdbk_mode = RKISP_VICAP_RDBK_AUTO;
	10721	+ } else {
	10722	+ capture_mode = RKCIF_STREAM_MODE_TOISP;
	10723	+ }
	10724	+ } else if (priv &&
	10725	+ (priv->mode.rdbk_mode == RKISP_VICAP_RDBK_AUTO \|\|
	10726	+ priv->mode.rdbk_mode == RKISP_VICAP_RDBK_AUTO_ONE_FRAME)) {
	10727	+ capture_mode = RKCIF_STREAM_MODE_TOISP_RDBK;
	10728	+ } else {
	10729	+ capture_mode = RKCIF_STREAM_MODE_CAPTURE;
	10730	+ }
	10731	+ }
	10732	+ } else if (cif_dev->resume_mode == RKISP_RTT_MODE_MULTI_FRAME) {
	10733	+ if (is_single_dev && is_can_be_online) {
	10734	+ capture_mode = RKCIF_STREAM_MODE_TOISP;
	10735	+ if (priv)
	10736	+ priv->mode.rdbk_mode = RKISP_VICAP_ONLINE;
	10737	+ } else {
	10738	+ if (cif_dev->is_thunderboot) {
	10739	+ capture_mode = RKCIF_STREAM_MODE_TOISP_RDBK;
	10740	+ if (priv)
	10741	+ priv->mode.rdbk_mode = RKISP_VICAP_RDBK_AUTO;
	10742	+ } else {
	10743	+ capture_mode = RKCIF_STREAM_MODE_CAPTURE;
	10744	+ if (priv)
	10745	+ priv->mode.rdbk_mode = RKISP_VICAP_RDBK_AIQ;
	10746	+ }
	10747	+ }
	10748	+ } else {
	10749	+ if (priv && priv->mode.rdbk_mode == RKISP_VICAP_ONLINE)
	10750	+ capture_mode = RKCIF_STREAM_MODE_TOISP;
	10751	+ else if (priv &&
	10752	+ (priv->mode.rdbk_mode == RKISP_VICAP_RDBK_AUTO \|\|
	10753	+ priv->mode.rdbk_mode == RKISP_VICAP_RDBK_AUTO_ONE_FRAME))
	10754	+ capture_mode = RKCIF_STREAM_MODE_TOISP_RDBK;
	10755	+ else
	10756	+ capture_mode = RKCIF_STREAM_MODE_CAPTURE;
	10757	+ }
	10758	+ if (priv && priv->mode.rdbk_mode == RKISP_VICAP_ONLINE && mode == RKCIF_RESUME_CIF)
	10759	+ goto out_resume;
	10760	+
	10761	+ for (i = 0; i < RKCIF_MAX_STREAM_MIPI; i++) {
	10762	+ stream = &cif_dev->stream[i];
	10763	+ if (stream->state != RKCIF_STATE_RESET_IN_STREAMING)
	10764	+ continue;
	10765	+
	10766	+ stream->fs_cnt_in_single_frame = 0;
	10767	+ if (cif_dev->resume_mode == RKISP_RTT_MODE_ONE_FRAME)
	10768	+ stream->is_single_cap = true;
	10769	+ spin_lock_irqsave(&stream->vbq_lock, flags);
	10770	+ if (!priv \|\| priv->mode.rdbk_mode == RKISP_VICAP_RDBK_AIQ) {
	10771	+ if (stream->cif_fmt_in->field == V4L2_FIELD_INTERLACED) {
	10772	+ if (stream->curr_buf == stream->next_buf) {
	10773	+ if (stream->curr_buf)
	10774	+ list_add_tail(&stream->curr_buf->queue, &stream->buf_head);
	10775	+ } else {
	10776	+ if (stream->curr_buf)
	10777	+ list_add_tail(&stream->curr_buf->queue, &stream->buf_head);
	10778	+ if (stream->next_buf)
	10779	+ list_add_tail(&stream->next_buf->queue, &stream->buf_head);
	10780	+ }
	10781	+ stream->curr_buf = NULL;
	10782	+ stream->next_buf = NULL;
	10783	+ }
	10784	+ } else {
	10785	+ if (priv->mode.rdbk_mode == RKISP_VICAP_ONLINE) {
	10786	+ if (stream->curr_buf_toisp == stream->next_buf_toisp) {
	10787	+ if (stream->curr_buf_toisp)
	10788	+ list_add_tail(&stream->curr_buf_toisp->list, &stream->rx_buf_head);
	10789	+ } else {
	10790	+ if (stream->curr_buf_toisp)
	10791	+ list_add_tail(&stream->curr_buf_toisp->list, &stream->rx_buf_head);
	10792	+ if (stream->next_buf_toisp)
	10793	+ list_add_tail(&stream->next_buf_toisp->list, &stream->rx_buf_head);
	10794	+ }
	10795	+ stream->curr_buf_toisp = NULL;
	10796	+ stream->next_buf_toisp = NULL;
	10797	+ } else {
	10798	+ if (stream->curr_buf_toisp == stream->next_buf_toisp) {
	10799	+ if (stream->curr_buf_toisp)
	10800	+ list_add_tail(&stream->curr_buf_toisp->list, &stream->rx_buf_head);
	10801	+ } else {
	10802	+ if (stream->curr_buf_toisp)
	10803	+ list_add_tail(&stream->curr_buf_toisp->list, &stream->rx_buf_head);
	10804	+ if (stream->next_buf_toisp)
	10805	+ list_add_tail(&stream->next_buf_toisp->list, &stream->rx_buf_head);
	10806	+ }
	10807	+ stream->curr_buf_toisp = NULL;
	10808	+ stream->next_buf_toisp = NULL;
	10809	+ }
	10810	+ }
	10811	+
	10812	+ spin_unlock_irqrestore(&stream->vbq_lock, flags);
	10813	+
	10814	+ if (priv) {
	10815	+ if (priv->mode.rdbk_mode == RKISP_VICAP_ONLINE) {
	10816	+ sditf_change_to_online(priv);
	10817	+ if (cif_dev->resume_mode == RKISP_RTT_MODE_MULTI_FRAME &&
	10818	+ stream->rx_buf_num &&
	10819	+ (priv->hdr_cfg.hdr_mode == NO_HDR \|\|
	10820	+ (priv->hdr_cfg.hdr_mode == HDR_X2 && stream->id == 1) \|\|
	10821	+ (priv->hdr_cfg.hdr_mode == HDR_X3 && stream->id == 2)))
	10822	+ rkcif_free_rx_buf(stream, priv->buf_num);
	10823	+ else if (!stream->rx_buf_num &&
	10824	+ ((priv->hdr_cfg.hdr_mode == HDR_X2 && stream->id == 0) \|\|
	10825	+ (priv->hdr_cfg.hdr_mode == HDR_X3 && (stream->id == 0 \|\| stream->id == 1))))
	10826	+ rkcif_init_rx_buf(stream, 1);
	10827	+ } else {
	10828	+ if (stream->is_single_cap && stream->id == 0) {
	10829	+ vicap_mode = priv->mode;
	10830	+ vicap_mode.rdbk_mode = RKISP_VICAP_RDBK_AUTO_ONE_FRAME;
	10831	+ sd = get_rkisp_sd(priv);
	10832	+ if (sd) {
	10833	+ ret = v4l2_subdev_call(sd, core, ioctl,
	10834	+ RKISP_VICAP_CMD_MODE, &vicap_mode);
	10835	+ if (ret)
	10836	+ v4l2_err(&cif_dev->v4l2_dev,
	10837	+ "set isp work mode rdbk aotu oneframe fail\n");
	10838	+
	10839	+ }
	10840	+ }
	10841	+ sditf_disable_immediately(priv);
	10842	+ if (!stream->rx_buf_num &&
	10843	+ capture_mode == RKCIF_STREAM_MODE_TOISP_RDBK) {
	10844	+ if (cif_dev->resume_mode == RKISP_RTT_MODE_ONE_FRAME)
	10845	+ rkcif_init_rx_buf(stream, 1);
	10846	+ else
	10847	+ rkcif_init_rx_buf(stream, priv->buf_num);
	10848	+ }
	10849	+ }
	10850	+ }
	10851	+
	10852	+ stream->lack_buf_cnt = 0;
	10853	+ if (cif_dev->active_sensor &&
	10854	+ (cif_dev->active_sensor->mbus.type == V4L2_MBUS_CSI2_DPHY \|\|
	10855	+ cif_dev->active_sensor->mbus.type == V4L2_MBUS_CSI2_CPHY \|\|
	10856	+ cif_dev->active_sensor->mbus.type == V4L2_MBUS_CCP2))
	10857	+ ret = rkcif_csi_stream_start(stream, capture_mode);
	10858	+ else
	10859	+ ret = rkcif_stream_start(stream, capture_mode);
	10860	+ if (ret)
	10861	+ v4l2_err(&cif_dev->v4l2_dev, "%s:resume stream[%d] failed\n",
	10862	+ __func__, stream->id);
	10863	+
	10864	+ resume_cnt++;
	10865	+ stream->cur_skip_frame = stream->skip_frame;
	10866	+ v4l2_dbg(1, rkcif_debug, &cif_dev->v4l2_dev,
	10867	+ "resume stream[%d], frm_idx:%d, csi_sof:%d\n",
	10868	+ stream->id, stream->frame_idx,
	10869	+ rkcif_get_sof(cif_dev));
	10870	+ }
	10871	+
	10872	+ if (resume_cnt == 0)
	10873	+ goto out_resume;
	10874	+
	10875	+ if (!cif_dev->resume_mode)
	10876	+ rkcif_subdevs_set_power(cif_dev, on);
	10877	+
	10878	+ atomic_set(&cif_dev->streamoff_cnt, 0);
	10879	+ rkcif_subdevs_set_stream(cif_dev, on);
	10880	+
	10881	+ if (cif_dev->chip_id < CHIP_RK3588_CIF)
	10882	+ rkcif_start_luma(&cif_dev->luma_vdev,
	10883	+ cif_dev->stream[RKCIF_STREAM_MIPI_ID0].cif_fmt_in);
	10884	+out_resume:
	10885	+ mutex_unlock(&cif_dev->stream_lock);
	10886	+ return 0;
	10887	+}
	10888	+
	10889	+void rkcif_err_print_work(struct work_struct *work)
	10890	+{
	10891	+ struct rkcif_err_state_work *err_state_work = container_of(work,
	10892	+ struct rkcif_err_state_work,
	10893	+ work);
	10894	+ struct rkcif_device *dev = container_of(err_state_work,
	10895	+ struct rkcif_device,
	10896	+ err_state_work);
	10897	+ u32 err_state = 0;
	10898	+ int intstat = 0;
	10899	+ int lastline = 0;
	10900	+ int lastpixel = 0;
	10901	+ u64 cur_time = 0;
	10902	+ bool is_print = false;
	10903	+
	10904	+ cur_time = rkcif_time_get_ns(dev);
	10905	+ if (err_state_work->last_timestamp == 0) {
	10906	+ is_print = true;
	10907	+ } else {
	10908	+ if (cur_time - err_state_work->last_timestamp > 500000000)
	10909	+ is_print = true;
	10910	+ }
	10911	+ err_state_work->last_timestamp = cur_time;
	10912	+ err_state = err_state_work->err_state;
	10913	+ intstat = err_state_work->intstat;
	10914	+ lastline = err_state_work->lastline;
	10915	+ lastpixel = err_state_work->lastpixel;
	10916	+ if (err_state & RKCIF_ERR_ID0_NOT_BUF && is_print)
	10917	+ v4l2_err(&dev->v4l2_dev,
	10918	+ "stream[0] not active buffer, frame num %d, cnt %llu\n",
	10919	+ dev->stream[0].frame_idx, dev->irq_stats.not_active_buf_cnt[0]);
	10920	+ if (err_state & RKCIF_ERR_ID1_NOT_BUF && is_print)
	10921	+ v4l2_err(&dev->v4l2_dev,
	10922	+ "stream[1] not active buffer, frame num %d, cnt %llu\n",
	10923	+ dev->stream[1].frame_idx, dev->irq_stats.not_active_buf_cnt[1]);
	10924	+ if (err_state & RKCIF_ERR_ID2_NOT_BUF && is_print)
	10925	+ v4l2_err(&dev->v4l2_dev,
	10926	+ "stream[2] not active buffer, frame num %d, cnt %llu\n",
	10927	+ dev->stream[2].frame_idx, dev->irq_stats.not_active_buf_cnt[2]);
	10928	+ if (err_state & RKCIF_ERR_ID3_NOT_BUF && is_print)
	10929	+ v4l2_err(&dev->v4l2_dev,
	10930	+ "stream[3] not active buffer, frame num %d, cnt %llu\n",
	10931	+ dev->stream[3].frame_idx, dev->irq_stats.not_active_buf_cnt[3]);
	10932	+ if (err_state & RKCIF_ERR_ID0_TRIG_SIMULT && is_print)
	10933	+ v4l2_err(&dev->v4l2_dev,
	10934	+ "stream[0], frm0/frm1 end simultaneously,frm id:%d, cnt %llu\n",
	10935	+ dev->stream[0].frame_idx, dev->irq_stats.trig_simult_cnt[0]);
	10936	+ if (err_state & RKCIF_ERR_ID1_TRIG_SIMULT && is_print)
	10937	+ v4l2_err(&dev->v4l2_dev,
	10938	+ "stream[1], frm0/frm1 end simultaneously,frm id:%d, cnt %llu\n",
	10939	+ dev->stream[1].frame_idx, dev->irq_stats.trig_simult_cnt[1]);
	10940	+ if (err_state & RKCIF_ERR_ID2_TRIG_SIMULT && is_print)
	10941	+ v4l2_err(&dev->v4l2_dev,
	10942	+ "stream[2], frm0/frm1 end simultaneously,frm id:%d, cnt %llu\n",
	10943	+ dev->stream[2].frame_idx, dev->irq_stats.trig_simult_cnt[2]);
	10944	+ if (err_state & RKCIF_ERR_ID3_TRIG_SIMULT && is_print)
	10945	+ v4l2_err(&dev->v4l2_dev,
	10946	+ "stream[3], frm0/frm1 end simultaneously,frm id:%d, cnt %llu\n",
	10947	+ dev->stream[3].frame_idx, dev->irq_stats.trig_simult_cnt[3]);
	10948	+ if (err_state & RKCIF_ERR_SIZE) {
	10949	+ if (dev->chip_id >= CHIP_RK3588_CIF)
	10950	+ v4l2_err(&dev->v4l2_dev,
	10951	+ "ERROR: csi size err, intstat:0x%x, size:0x%x,0x%x,0x%x,0x%x, cnt %llu\n",
	10952	+ intstat, err_state_work->size_id0, err_state_work->size_id1,
	10953	+ err_state_work->size_id2, err_state_work->size_id3,
	10954	+ dev->irq_stats.csi_size_err_cnt);
	10955	+ else
	10956	+ v4l2_err(&dev->v4l2_dev,
	10957	+ "ERROR: csi size err, intstat:0x%x, lastline:0x%x, cnt %llu\n",
	10958	+ intstat, lastline, dev->irq_stats.csi_size_err_cnt);
	10959	+ }
	10960	+ if (err_state & RKCIF_ERR_OVERFLOW)
	10961	+ v4l2_err(&dev->v4l2_dev,
	10962	+ "ERROR: csi fifo overflow, intstat:0x%x, lastline:0x%x, cnt %llu\n",
	10963	+ intstat, lastline, dev->irq_stats.csi_overflow_cnt);
	10964	+ if (err_state & RKCIF_ERR_BANDWIDTH_LACK)
	10965	+ v4l2_err(&dev->v4l2_dev,
	10966	+ "ERROR: csi bandwidth lack, intstat:0x%x, lastline:0x%x, cnt %llu\n",
	10967	+ intstat, lastline, dev->irq_stats.csi_bwidth_lack_cnt);
	10968	+ if (err_state & RKCIF_ERR_ID0_MULTI_FS)
	10969	+ v4l2_err(&dev->v4l2_dev,
	10970	+ "ERR: multi fs in oneframe in id0, fs_num:%u\n",
	10971	+ dev->stream[0].fs_cnt_in_single_frame);
	10972	+ if (err_state & RKCIF_ERR_BUS)
	10973	+ v4l2_err(&dev->v4l2_dev, "dvp bus err, intstat 0x%x, last line 0x%x\n",
	10974	+ intstat, lastline);
	10975	+ if (err_state & RKCIF_ERR_PIXEL)
	10976	+ v4l2_err(&dev->v4l2_dev, "dvp pix err, intstat 0x%x, last pixel 0x%x\n",
	10977	+ intstat, lastpixel);
	10978	+ if (err_state & RKCIF_ERR_LINE)
	10979	+ v4l2_err(&dev->v4l2_dev, "dvp line err, intstat 0x%x, last line 0x%x\n",
	10980	+ intstat, lastline);
	10981	+}
	10982	+
	10983	+/* pingpong irq for rk3588 and next */
	10984	+void rkcif_irq_pingpong_v1(struct rkcif_device *cif_dev)
	10985	+{
	10986	+ struct rkcif_stream *stream;
	10987	+ struct rkcif_stream *detect_stream = &cif_dev->stream[0];
	10988	+ struct v4l2_mbus_config *mbus;
	10989	+ unsigned int intstat, i = 0xff;
	10990	+ unsigned long flags;
	10991	+ bool is_update = false;
	10992	+ int ret = 0;
	10993	+ int on = 0;
	10994	+
	10995	+ if (!cif_dev->active_sensor)
	10996	+ return;
	10997	+
	10998	+ mbus = &cif_dev->active_sensor->mbus;
	10999	+ if (mbus->type == V4L2_MBUS_CSI2_DPHY \|\|
	11000	+ mbus->type == V4L2_MBUS_CSI2_CPHY \|\|
	11001	+ mbus->type == V4L2_MBUS_CCP2) {
	11002	+ int mipi_id;
	11003	+ u32 lastline = 0;
	11004	+
	11005	+ intstat = rkcif_read_register(cif_dev, CIF_REG_MIPI_LVDS_INTSTAT);
	11006	+ lastline = rkcif_read_register(cif_dev, CIF_REG_MIPI_LVDS_LINE_LINE_CNT_ID0_1);
	11007	+ cif_dev->err_state_work.lastline = lastline;
	11008	+ cif_dev->err_state_work.intstat = intstat;
	11009	+
	11010	+ /* clear all interrupts that has been triggered */
	11011	+ if (intstat) {
	11012	+ rkcif_write_register(cif_dev, CIF_REG_MIPI_LVDS_INTSTAT, intstat);
	11013	+ v4l2_dbg(2, rkcif_debug, &cif_dev->v4l2_dev,
	11014	+ "intstat 0x%x\n", intstat);
	11015	+ } else {
	11016	+ return;
	11017	+ }
	11018	+
	11019	+ if (intstat & CSI_SIZE_ERR) {
	11020	+ if (cif_dev->chip_id >= CHIP_RK3588_CIF) {
	11021	+ cif_dev->err_state_work.size_id0 = rkcif_read_register(cif_dev,
	11022	+ CIF_REG_MIPI_FRAME_SIZE_ID0);
	11023	+ cif_dev->err_state_work.size_id1 = rkcif_read_register(cif_dev,
	11024	+ CIF_REG_MIPI_FRAME_SIZE_ID1);
	11025	+ cif_dev->err_state_work.size_id2 = rkcif_read_register(cif_dev,
	11026	+ CIF_REG_MIPI_FRAME_SIZE_ID2);
	11027	+ cif_dev->err_state_work.size_id3 = rkcif_read_register(cif_dev,
	11028	+ CIF_REG_MIPI_FRAME_SIZE_ID3);
	11029	+ }
	11030	+ cif_dev->irq_stats.csi_size_err_cnt++;
	11031	+ cif_dev->err_state \|= RKCIF_ERR_SIZE;
	11032	+ rkcif_write_register_or(cif_dev, CIF_REG_MIPI_LVDS_CTRL, 0x000A0000);
	11033	+ return;
	11034	+ }
	11035	+
	11036	+ if (intstat & CSI_FIFO_OVERFLOW_V1) {
	11037	+ cif_dev->irq_stats.csi_overflow_cnt++;
	11038	+ cif_dev->err_state \|= RKCIF_ERR_OVERFLOW;
	11039	+ return;
	11040	+ }
	11041	+
	11042	+ if (intstat & CSI_BANDWIDTH_LACK_V1 &&
	11043	+ cif_dev->intr_mask & CSI_BANDWIDTH_LACK_V1) {
	11044	+ cif_dev->irq_stats.csi_bwidth_lack_cnt++;
	11045	+ cif_dev->err_state \|= RKCIF_ERR_BANDWIDTH_LACK;
	11046	+ if (cif_dev->irq_stats.csi_bwidth_lack_cnt > 10) {
	11047	+ rkcif_write_register_and(cif_dev, CIF_REG_MIPI_LVDS_INTEN, ~(CSI_BANDWIDTH_LACK_V1));
	11048	+ cif_dev->intr_mask &= ~(CSI_BANDWIDTH_LACK_V1);
	11049	+ schedule_delayed_work(&cif_dev->work_deal_err, msecs_to_jiffies(1000));
	11050	+ }
	11051	+ }
	11052	+
	11053	+ if (intstat & CSI_ALL_ERROR_INTEN_V1) {
	11054	+ cif_dev->irq_stats.all_err_cnt++;
	11055	+ return;
	11056	+ }
	11057	+
	11058	+ for (i = 0; i < RKCIF_MAX_STREAM_MIPI; i++) {
	11059	+ mipi_id = rkcif_csi_g_mipi_id(&cif_dev->v4l2_dev,
	11060	+ intstat);
	11061	+ if (mipi_id < 0)
	11062	+ continue;
	11063	+
	11064	+ stream = &cif_dev->stream[mipi_id];
	11065	+ if (!cif_dev->sditf[0] \|\|
	11066	+ cif_dev->sditf[0]->mode.rdbk_mode)
	11067	+ stream->buf_wake_up_cnt++;
	11068	+
	11069	+ if (stream->stopping && (!stream->dma_en)) {
	11070	+ rkcif_stream_stop(stream);
	11071	+ stream->stopping = false;
	11072	+ wake_up(&stream->wq_stopped);
	11073	+ continue;
	11074	+ }
	11075	+
	11076	+ if (stream->state != RKCIF_STATE_STREAMING)
	11077	+ continue;
	11078	+ stream->is_in_vblank = true;
	11079	+ switch (mipi_id) {
	11080	+ case RKCIF_STREAM_MIPI_ID0:
	11081	+ stream->frame_phase = SW_FRM_END_ID0(intstat);
	11082	+ intstat &= ~CSI_FRAME_END_ID0;
	11083	+ break;
	11084	+ case RKCIF_STREAM_MIPI_ID1:
	11085	+ stream->frame_phase = SW_FRM_END_ID1(intstat);
	11086	+ intstat &= ~CSI_FRAME_END_ID1;
	11087	+ break;
	11088	+ case RKCIF_STREAM_MIPI_ID2:
	11089	+ stream->frame_phase = SW_FRM_END_ID2(intstat);
	11090	+ intstat &= ~CSI_FRAME_END_ID2;
	11091	+ break;
	11092	+ case RKCIF_STREAM_MIPI_ID3:
	11093	+ stream->frame_phase = SW_FRM_END_ID3(intstat);
	11094	+ intstat &= ~CSI_FRAME_END_ID3;
	11095	+ break;
	11096	+ }
	11097	+ if (stream->cifdev->rdbk_debug &&
	11098	+ stream->frame_idx < 15 &&
	11099	+ (!cif_dev->sditf[0] \|\| cif_dev->sditf[0]->mode.rdbk_mode))
	11100	+ v4l2_info(&cif_dev->v4l2_dev,
	11101	+ "stream[%d] fe %d, phase %d, %lld\n",
	11102	+ stream->id,
	11103	+ stream->frame_idx - 1,
	11104	+ stream->frame_phase,
	11105	+ rkcif_time_get_ns(cif_dev));
	11106	+ if (stream->is_finish_stop_dma && stream->is_wait_dma_stop) {
	11107	+ stream->is_wait_dma_stop = false;
	11108	+ wake_up(&stream->wq_stopped);
	11109	+ stream->is_finish_stop_dma = false;
	11110	+ continue;
	11111	+ }
	11112	+
	11113	+ if (stream->is_finish_stop_dma && stream->is_wait_stop_complete) {
	11114	+ stream->is_finish_stop_dma = false;
	11115	+ stream->is_wait_stop_complete = false;
	11116	+ complete(&stream->stop_complete);
	11117	+ }
	11118	+
	11119	+ if (stream->crop_dyn_en)
	11120	+ rkcif_dynamic_crop(stream);
	11121	+
	11122	+ if (stream->dma_en & RKCIF_DMAEN_BY_VICAP) {
	11123	+ if (cif_dev->sync_cfg.type == RKCIF_NOSYNC_MODE)
	11124	+ is_update = true;
	11125	+ else
	11126	+ is_update = rkcif_check_buffer_prepare(stream);
	11127	+ v4l2_dbg(4, rkcif_debug, &cif_dev->v4l2_dev,
	11128	+ "dma capture by vicap, is_updata %d, group mode %d, dma_en %d\n",
	11129	+ is_update, cif_dev->sync_cfg.type, stream->dma_en);
	11130	+ if (is_update)
	11131	+ rkcif_update_stream(cif_dev, stream, mipi_id);
	11132	+ } else if (stream->dma_en & RKCIF_DMAEN_BY_ISP) {
	11133	+ v4l2_dbg(4, rkcif_debug, &cif_dev->v4l2_dev,
	11134	+ "dma capture by isp, dma_en 0x%x\n",
	11135	+ stream->dma_en);
	11136	+ rkcif_update_stream_toisp(cif_dev, stream, mipi_id);
	11137	+ } else if (stream->dma_en & RKCIF_DMAEN_BY_ROCKIT) {
	11138	+ v4l2_dbg(4, rkcif_debug, &cif_dev->v4l2_dev,
	11139	+ "dma capture by rockit, dma_en 0x%x\n",
	11140	+ stream->dma_en);
	11141	+ rkcif_update_stream_rockit(cif_dev, stream, mipi_id);
	11142	+ }
	11143	+ if (stream->is_single_cap && !stream->cur_skip_frame) {
	11144	+ if (stream->dma_en & RKCIF_DMAEN_BY_ISP)
	11145	+ stream->to_stop_dma = RKCIF_DMAEN_BY_ISP;
	11146	+ else if (stream->dma_en & RKCIF_DMAEN_BY_VICAP)
	11147	+ stream->to_stop_dma = RKCIF_DMAEN_BY_VICAP;
	11148	+ else if (stream->dma_en & RKCIF_DMAEN_BY_ROCKIT)
	11149	+ stream->to_stop_dma = RKCIF_DMAEN_BY_ROCKIT;
	11150	+ rkcif_stop_dma_capture(stream);
	11151	+ stream->is_single_cap = false;
	11152	+ if ((cif_dev->hdr.hdr_mode == NO_HDR && atomic_read(&cif_dev->streamoff_cnt) == 1) \|\|
	11153	+ (cif_dev->hdr.hdr_mode == HDR_X2 && atomic_read(&cif_dev->streamoff_cnt) == 2) \|\|
	11154	+ (cif_dev->hdr.hdr_mode == HDR_X3 && atomic_read(&cif_dev->streamoff_cnt) == 3)) {
	11155	+ rkcif_dphy_quick_stream(stream->cifdev, on);
	11156	+ cif_dev->sensor_work.on = 0;
	11157	+ schedule_work(&cif_dev->sensor_work.work);
	11158	+ }
	11159	+ }
	11160	+
	11161	+ if (stream->cur_skip_frame)
	11162	+ stream->cur_skip_frame--;
	11163	+
	11164	+ if (cif_dev->chip_id >= CHIP_RV1106_CIF)
	11165	+ rkcif_modify_frame_skip_config(stream);
	11166	+ if (stream->is_change_toisp) {
	11167	+ stream->is_change_toisp = false;
	11168	+ if ((cif_dev->hdr.hdr_mode == HDR_X2 && stream->id != 1) \|\|
	11169	+ (cif_dev->hdr.hdr_mode == HDR_X3 && stream->id != 2))
	11170	+ rkcif_release_unnecessary_buf_for_online(stream,
	11171	+ stream->curr_buf_toisp);
	11172	+ else
	11173	+ sditf_change_to_online(cif_dev->sditf[0]);
	11174	+ }
	11175	+
	11176	+ spin_lock_irqsave(&stream->vbq_lock, flags);
	11177	+ if (!(stream->cur_stream_mode & RKCIF_STREAM_MODE_TOISP) &&
	11178	+ stream->lack_buf_cnt == 2) {
	11179	+ stream->to_stop_dma = RKCIF_DMAEN_BY_VICAP;
	11180	+ rkcif_stop_dma_capture(stream);
	11181	+ v4l2_dbg(4, rkcif_debug, &cif_dev->v4l2_dev,
	11182	+ "stream[%d] to stop dma, lack_buf_cnt %d\n",
	11183	+ stream->id, stream->lack_buf_cnt);
	11184	+ }
	11185	+ spin_unlock_irqrestore(&stream->vbq_lock, flags);
	11186	+ if (stream->to_en_scale) {
	11187	+ stream->to_en_scale = false;
	11188	+ rkcif_scale_start(stream->scale_vdev);
	11189	+ }
	11190	+ rkcif_detect_wake_up_mode_change(stream);
	11191	+ if (cif_dev->chip_id < CHIP_RK3588_CIF &&
	11192	+ mipi_id == RKCIF_STREAM_MIPI_ID0) {
	11193	+ if ((intstat & (CSI_FRAME1_START_ID0 \| CSI_FRAME0_START_ID0)) == 0 &&
	11194	+ detect_stream->fs_cnt_in_single_frame > 1) {
	11195	+ cif_dev->err_state \|= RKCIF_ERR_ID0_MULTI_FS;
	11196	+ detect_stream->is_fs_fe_not_paired = true;
	11197	+ detect_stream->fs_cnt_in_single_frame = 0;
	11198	+ } else {
	11199	+ detect_stream->fs_cnt_in_single_frame--;
	11200	+ }
	11201	+ }
	11202	+ rkcif_monitor_reset_event(cif_dev);
	11203	+ cif_dev->irq_stats.frm_end_cnt[stream->id]++;
	11204	+ }
	11205	+
	11206	+ for (i = 0; i < RKCIF_MAX_STREAM_MIPI; i++) {
	11207	+ if (intstat & CSI_START_INTSTAT(i)) {
	11208	+ stream = &cif_dev->stream[i];
	11209	+ if (i == 0) {
	11210	+ if (!stream->cur_skip_frame)
	11211	+ rkcif_deal_sof(cif_dev);
	11212	+ } else {
	11213	+ spin_lock_irqsave(&stream->fps_lock, flags);
	11214	+ stream->readout.fs_timestamp = rkcif_time_get_ns(cif_dev);
	11215	+ stream->frame_idx++;
	11216	+ spin_unlock_irqrestore(&stream->fps_lock, flags);
	11217	+ }
	11218	+ stream->is_in_vblank = false;
	11219	+ spin_lock_irqsave(&stream->vbq_lock, flags);
	11220	+ if (stream->stopping && stream->dma_en) {
	11221	+ if (stream->dma_en & RKCIF_DMAEN_BY_VICAP)
	11222	+ stream->to_stop_dma = RKCIF_DMAEN_BY_VICAP;
	11223	+ else if (stream->dma_en & RKCIF_DMAEN_BY_ISP)
	11224	+ stream->to_stop_dma = RKCIF_DMAEN_BY_ISP;
	11225	+ stream->is_stop_capture = true;
	11226	+ }
	11227	+ if (stream->to_stop_dma) {
	11228	+ ret = rkcif_stop_dma_capture(stream);
	11229	+ if (!ret)
	11230	+ stream->is_finish_stop_dma = true;
	11231	+ }
	11232	+ if (stream->to_en_dma)
	11233	+ rkcif_enable_dma_capture(stream, false);
	11234	+ spin_unlock_irqrestore(&stream->vbq_lock, flags);
	11235	+ }
	11236	+ if (intstat & CSI_LINE_INTSTAT_V1(i)) {
	11237	+ stream = &cif_dev->stream[i];
	11238	+ if (stream->is_line_inten) {
	11239	+ stream->line_int_cnt++;
	11240	+ if (cif_dev->rdbk_debug > 1 &&
	11241	+ stream->frame_idx < 15)
	11242	+ v4l2_info(&cif_dev->v4l2_dev,
	11243	+ "line int %lld\n",
	11244	+ stream->line_int_cnt);
	11245	+ if (cif_dev->sditf[0] && (cif_dev->sditf[0]->mode.rdbk_mode == RKISP_VICAP_RDBK_AUTO \|\|
	11246	+ cif_dev->sditf[0]->mode.rdbk_mode == RKISP_VICAP_RDBK_AUTO_ONE_FRAME))
	11247	+ rkcif_line_wake_up_rdbk(stream, stream->id);
	11248	+ else
	11249	+ rkcif_line_wake_up(stream, stream->id);
	11250	+ rkcif_modify_line_int(stream, false);
	11251	+ stream->is_line_inten = false;
	11252	+ }
	11253	+ v4l2_dbg(3, rkcif_debug, &cif_dev->v4l2_dev,
	11254	+ "%s: id0 cur line:%d\n", __func__, lastline & 0x3fff);
	11255	+ }
	11256	+ }
	11257	+ } else {
	11258	+ struct rkcif_stream *stream;
	11259	+ int ch_id;
	11260	+ int lastline;
	11261	+
	11262	+ intstat = rkcif_read_register(cif_dev, CIF_REG_DVP_INTSTAT);
	11263	+ if (intstat)
	11264	+ rkcif_write_register(cif_dev, CIF_REG_DVP_INTSTAT, intstat);
	11265	+ else
	11266	+ return;
	11267	+ lastline = rkcif_read_register(cif_dev, CIF_REG_DVP_LINE_CNT);
	11268	+ cif_dev->err_state_work.lastline = lastline;
	11269	+ cif_dev->err_state_work.intstat = intstat;
	11270	+ stream = &cif_dev->stream[RKCIF_STREAM_CIF];
	11271	+
	11272	+ if (intstat & DVP_SIZE_ERR) {
	11273	+ cif_dev->irq_stats.dvp_size_err_cnt++;
	11274	+ rkcif_write_register_or(cif_dev, CIF_REG_DVP_CTRL, 0x000A0000);
	11275	+ cif_dev->err_state \|= RKCIF_ERR_SIZE;
	11276	+ }
	11277	+
	11278	+ if (intstat & DVP_FIFO_OVERFLOW) {
	11279	+ cif_dev->irq_stats.dvp_overflow_cnt++;
	11280	+ cif_dev->err_state \|= RKCIF_ERR_OVERFLOW;
	11281	+ }
	11282	+
	11283	+ if (intstat & DVP_BANDWIDTH_LACK) {
	11284	+ cif_dev->irq_stats.dvp_bwidth_lack_cnt++;
	11285	+ cif_dev->err_state \|= RKCIF_ERR_BANDWIDTH_LACK;
	11286	+ }
	11287	+
	11288	+ if (intstat & INTSTAT_ERR_RK3588) {
	11289	+ cif_dev->irq_stats.all_err_cnt++;
	11290	+ }
	11291	+
	11292	+ for (i = 0; i < RKCIF_MAX_STREAM_DVP; i++) {
	11293	+ ch_id = rkcif_dvp_g_ch_id_by_fe(&cif_dev->v4l2_dev, intstat);
	11294	+
	11295	+ if (ch_id < 0)
	11296	+ continue;
	11297	+
	11298	+ stream = &cif_dev->stream[ch_id];
	11299	+ if (!cif_dev->sditf[0] \|\|
	11300	+ cif_dev->sditf[0]->mode.rdbk_mode)
	11301	+ stream->buf_wake_up_cnt++;
	11302	+
	11303	+ if (stream->stopping) {
	11304	+ rkcif_stream_stop(stream);
	11305	+ stream->stopping = false;
	11306	+ wake_up(&stream->wq_stopped);
	11307	+ continue;
	11308	+ }
	11309	+
	11310	+ if (stream->state != RKCIF_STATE_STREAMING)
	11311	+ continue;
	11312	+ stream->is_in_vblank = true;
	11313	+ switch (ch_id) {
	11314	+ case RKCIF_STREAM_MIPI_ID0:
	11315	+ stream->frame_phase = SW_FRM_END_ID0(intstat);
	11316	+ intstat &= ~DVP_ALL_END_ID0;
	11317	+ break;
	11318	+ case RKCIF_STREAM_MIPI_ID1:
	11319	+ stream->frame_phase = SW_FRM_END_ID1(intstat);
	11320	+ intstat &= ~DVP_ALL_END_ID1;
	11321	+ break;
	11322	+ case RKCIF_STREAM_MIPI_ID2:
	11323	+ stream->frame_phase = SW_FRM_END_ID2(intstat);
	11324	+ intstat &= ~DVP_ALL_END_ID2;
	11325	+ break;
	11326	+ case RKCIF_STREAM_MIPI_ID3:
	11327	+ stream->frame_phase = SW_FRM_END_ID3(intstat);
	11328	+ intstat &= ~DVP_ALL_END_ID3;
	11329	+ break;
	11330	+ }
	11331	+ if (stream->dma_en & RKCIF_DMAEN_BY_VICAP) {
	11332	+ if (cif_dev->sync_cfg.type == RKCIF_NOSYNC_MODE)
	11333	+ is_update = true;
	11334	+ else
	11335	+ is_update = rkcif_check_buffer_prepare(stream);
	11336	+ if (is_update)
	11337	+ rkcif_update_stream(cif_dev, stream, ch_id);
	11338	+ } else if (stream->dma_en & RKCIF_DMAEN_BY_ISP) {
	11339	+ rkcif_update_stream_toisp(cif_dev, stream, ch_id);
	11340	+ } else if (stream->dma_en & RKCIF_DMAEN_BY_ROCKIT) {
	11341	+ rkcif_update_stream_rockit(cif_dev, stream, ch_id);
	11342	+ }
	11343	+
	11344	+ if (stream->to_en_dma)
	11345	+ rkcif_enable_dma_capture(stream, false);
	11346	+ if (stream->to_stop_dma) {
	11347	+ rkcif_stop_dma_capture(stream);
	11348	+ wake_up(&stream->wq_stopped);
	11349	+ }
	11350	+ cif_dev->irq_stats.frm_end_cnt[stream->id]++;
	11351	+ }
	11352	+
	11353	+ if (intstat & DVP_FRAME0_START_ID0 \|\| intstat & DVP_FRAME1_START_ID0) {
	11354	+ stream->is_in_vblank = false;
	11355	+ if (!stream->cur_skip_frame)
	11356	+ rkcif_deal_sof(cif_dev);
	11357	+ }
	11358	+
	11359	+ if (stream->crop_dyn_en)
	11360	+ rkcif_dynamic_crop(stream);
	11361	+ }
	11362	+}
	11363	+
6089	11364	void rkcif_irq_pingpong(struct rkcif_device *cif_dev)
6090	11365	{
6091	11366	struct rkcif_stream *stream;
6092	11367	struct rkcif_stream *detect_stream = &cif_dev->stream[0];
6093	11368	struct v4l2_mbus_config *mbus;
6094		- unsigned int intstat = 0x0, i = 0xff, bak_intstat = 0x0;
	11369	+ unsigned int intstat = 0x0, i = 0xff;
6095	11370	unsigned long flags;
6096	11371	int ret = 0;
6097	11372
..	..	@@ -6099,7 +11374,8 @@
6099	11374	return;
6100	11375
6101	11376	mbus = &cif_dev->active_sensor->mbus;
6102		- if ((mbus->type == V4L2_MBUS_CSI2 \|\|
	11377	+ if ((mbus->type == V4L2_MBUS_CSI2_DPHY \|\|
	11378	+ mbus->type == V4L2_MBUS_CSI2_CPHY \|\|
6103	11379	mbus->type == V4L2_MBUS_CCP2) &&
6104	11380	(cif_dev->chip_id == CHIP_RK1808_CIF \|\|
6105	11381	cif_dev->chip_id == CHIP_RV1126_CIF \|\|
..	..	@@ -6109,51 +11385,26 @@
6109	11385
6110	11386	intstat = rkcif_read_register(cif_dev, CIF_REG_MIPI_LVDS_INTSTAT);
6111	11387	lastline = rkcif_read_register(cif_dev, CIF_REG_MIPI_LVDS_LINE_LINE_CNT_ID0_1);
	11388	+ cif_dev->err_state_work.lastline = lastline;
	11389	+ cif_dev->err_state_work.intstat = intstat;
6112	11390
6113	11391	/* clear all interrupts that has been triggered */
6114	11392	rkcif_write_register(cif_dev, CIF_REG_MIPI_LVDS_INTSTAT, intstat);
6115	11393
6116	11394	if (intstat & CSI_FIFO_OVERFLOW) {
6117	11395	cif_dev->irq_stats.csi_overflow_cnt++;
6118		- v4l2_err(&cif_dev->v4l2_dev,
6119		- "ERROR: csi fifo overflow, intstat:0x%x, lastline:%d!!\n",
6120		- intstat, lastline);
6121		- return;
	11396	+ cif_dev->err_state \|= RKCIF_ERR_OVERFLOW;
6122	11397	}
6123	11398
6124	11399	if (intstat & CSI_BANDWIDTH_LACK) {
6125	11400	cif_dev->irq_stats.csi_bwidth_lack_cnt++;
6126		- v4l2_err(&cif_dev->v4l2_dev,
6127		- "ERROR: csi bandwidth lack, intstat:0x%x!!\n",
6128		- intstat);
	11401	+ cif_dev->err_state \|= RKCIF_ERR_BANDWIDTH_LACK;
6129	11402	return;
6130	11403	}
6131	11404
6132	11405	if (intstat & CSI_ALL_ERROR_INTEN) {
6133	11406	cif_dev->irq_stats.all_err_cnt++;
6134		- v4l2_err(&cif_dev->v4l2_dev,
6135		- "ERROR: CSI_ALL_ERROR_INTEN:0x%x!!\n", intstat);
6136	11407	return;
6137		- }
6138		-
6139		- if ((intstat & (CSI_FRAME0_START_ID0 \| CSI_FRAME1_START_ID0)) ==
6140		- (CSI_FRAME0_START_ID0 \| CSI_FRAME1_START_ID0)) {
6141		- v4l2_err(&cif_dev->v4l2_dev, "%s:ERR: double fs in one fs int\n",
6142		- __func__);
6143		- }
6144		-
6145		- for (i = 0; i < RKCIF_MAX_STREAM_MIPI; i++) {
6146		- if (intstat & CSI_LINE_INTSTAT(i)) {
6147		- stream = &cif_dev->stream[i];
6148		- if (stream->is_line_inten) {
6149		- stream->line_int_cnt++;
6150		- rkcif_line_wake_up(stream, stream->id);
6151		- rkcif_modify_line_int(stream, false);
6152		- stream->is_line_inten = false;
6153		- }
6154		- v4l2_dbg(1, rkcif_debug, &cif_dev->v4l2_dev,
6155		- "%s: id0 cur line:%d\n", __func__, lastline & 0x3fff);
6156		- }
6157	11408	}
6158	11409
6159	11410	for (i = 0; i < RKCIF_MAX_STREAM_MIPI; i++) {
..	..	@@ -6197,42 +11448,45 @@
6197	11448
6198	11449	rkcif_update_stream(cif_dev, stream, mipi_id);
6199	11450	rkcif_detect_wake_up_mode_change(stream);
	11451	+ rkcif_monitor_reset_event(cif_dev);
6200	11452	if (mipi_id == RKCIF_STREAM_MIPI_ID0) {
6201	11453	if ((intstat & (CSI_FRAME1_START_ID0 \| CSI_FRAME0_START_ID0)) == 0 &&
6202	11454	detect_stream->fs_cnt_in_single_frame > 1) {
6203		- v4l2_dbg(2, rkcif_debug, &cif_dev->v4l2_dev,
6204		- "%s ERR: multi fs in oneframe, bak_int:0x%x, fs_num:%u\n",
6205		- __func__,
6206		- bak_intstat,
6207		- detect_stream->fs_cnt_in_single_frame);
	11455	+ cif_dev->err_state \|= RKCIF_ERR_ID0_MULTI_FS;
6208	11456	detect_stream->is_fs_fe_not_paired = true;
6209	11457	detect_stream->fs_cnt_in_single_frame = 0;
6210	11458	} else {
6211	11459	detect_stream->fs_cnt_in_single_frame--;
6212	11460	}
6213	11461	}
6214		- cif_dev->irq_stats.all_frm_end_cnt++;
	11462	+ cif_dev->irq_stats.frm_end_cnt[stream->id]++;
6215	11463	}
6216	11464	for (i = 0; i < RKCIF_MAX_STREAM_MIPI; i++) {
6217	11465	if (intstat & CSI_START_INTSTAT(i)) {
6218	11466	stream = &cif_dev->stream[i];
6219	11467	if (i == 0) {
6220		- if (mbus->type == V4L2_MBUS_CSI2)
6221		- rkcif_csi2_event_inc_sof();
6222		- else if (mbus->type == V4L2_MBUS_CCP2)
6223		- rkcif_lvds_event_inc_sof(cif_dev);
6224		- stream->fs_cnt_in_single_frame++;
6225		- spin_lock_irqsave(&stream->fps_lock, flags);
6226		- stream->readout.fs_timestamp = ktime_get_ns();
6227		- spin_unlock_irqrestore(&stream->fps_lock, flags);
	11468	+ if (!stream->cur_skip_frame)
	11469	+ rkcif_deal_sof(cif_dev);
6228	11470	} else {
6229	11471	spin_lock_irqsave(&stream->fps_lock, flags);
6230		- stream->readout.fs_timestamp = ktime_get_ns();
	11472	+ stream->readout.fs_timestamp = rkcif_time_get_ns(cif_dev);
	11473	+ stream->frame_idx++;
6231	11474	spin_unlock_irqrestore(&stream->fps_lock, flags);
6232	11475	}
	11476	+ stream->is_in_vblank = false;
	11477	+ }
	11478	+ if (intstat & CSI_LINE_INTSTAT(i)) {
	11479	+ stream = &cif_dev->stream[i];
	11480	+ if (stream->is_line_inten) {
	11481	+ stream->line_int_cnt++;
	11482	+ rkcif_line_wake_up(stream, stream->id);
	11483	+ rkcif_modify_line_int(stream, false);
	11484	+ stream->is_line_inten = false;
	11485	+ }
	11486	+ v4l2_dbg(3, rkcif_debug, &cif_dev->v4l2_dev,
	11487	+ "%s: id0 cur line:%d\n", __func__, lastline & 0x3fff);
6233	11488	}
6234	11489	}
6235		-
6236	11490	} else {
6237	11491	u32 lastline, lastpix, ctl;
6238	11492	u32 cif_frmst, frmid, int_en;
..	..	@@ -6246,49 +11500,36 @@
6246	11500	lastpix = rkcif_read_register(cif_dev, CIF_REG_DVP_LAST_PIX);
6247	11501	lastpix = CIF_FETCH_Y_LAST_LINE(lastpix);
6248	11502	ctl = rkcif_read_register(cif_dev, CIF_REG_DVP_CTRL);
	11503	+ cif_dev->err_state_work.lastline = lastline;
	11504	+ cif_dev->err_state_work.lastpixel = lastpix;
	11505	+ cif_dev->err_state_work.intstat = intstat;
6249	11506
6250	11507	rkcif_write_register(cif_dev, CIF_REG_DVP_INTSTAT, intstat);
6251	11508
6252	11509	stream = &cif_dev->stream[RKCIF_STREAM_CIF];
6253	11510
6254		- if ((intstat & LINE_INT_END) && !(intstat & FRAME_END) &&
6255		- (cif_dev->dvp_sof_in_oneframe == 0)) {
6256		- if ((intstat & (PRE_INF_FRAME_END \| PST_INF_FRAME_END)) == 0x0) {
6257		- if ((intstat & INTSTAT_ERR) == 0x0) {
6258		- rkcif_dvp_event_inc_sof(cif_dev);
6259		- int_en = rkcif_read_register(cif_dev, CIF_REG_DVP_INTEN);
6260		- int_en &= ~LINE_INT_EN;
6261		- rkcif_write_register(cif_dev, CIF_REG_DVP_INTEN, int_en);
6262		- cif_dev->dvp_sof_in_oneframe = 1;
6263		- }
6264		- }
6265		- }
6266		-
6267	11511	if (intstat & BUS_ERR) {
6268	11512	cif_dev->irq_stats.dvp_bus_err_cnt++;
6269		- v4l2_info(&cif_dev->v4l2_dev, "dvp bus err\n");
	11513	+ cif_dev->err_state \|= RKCIF_ERR_BUS;
6270	11514	}
6271	11515
6272	11516	if (intstat & DVP_ALL_OVERFLOW) {
6273	11517	cif_dev->irq_stats.dvp_overflow_cnt++;
6274		- v4l2_info(&cif_dev->v4l2_dev, "dvp overflow err\n");
	11518	+ cif_dev->err_state \|= RKCIF_ERR_OVERFLOW;
6275	11519	}
6276	11520
6277	11521	if (intstat & LINE_ERR) {
6278	11522	cif_dev->irq_stats.dvp_line_err_cnt++;
6279		- v4l2_info(&cif_dev->v4l2_dev, "dvp line err\n");
	11523	+ cif_dev->err_state \|= RKCIF_ERR_LINE;
6280	11524	}
6281	11525
6282	11526	if (intstat & PIX_ERR) {
6283	11527	cif_dev->irq_stats.dvp_pix_err_cnt++;
6284		- v4l2_info(&cif_dev->v4l2_dev, "dvp pix err\n");
	11528	+ cif_dev->err_state \|= RKCIF_ERR_PIXEL;
6285	11529	}
6286	11530
6287		- if (intstat & INTSTAT_ERR) {
	11531	+ if (intstat & INTSTAT_ERR)
6288	11532	cif_dev->irq_stats.all_err_cnt++;
6289		- v4l2_err(&cif_dev->v4l2_dev,
6290		- "ERROR: DVP_ALL_ERROR_INTEN:0x%x!!\n", intstat);
6291		- }
6292	11533
6293	11534	/* There are two irqs enabled:
6294	11535	* - PST_INF_FRAME_END: cif FIFO is ready,
..	..	@@ -6309,7 +11550,7 @@
6309	11550	stream = &cif_dev->stream[RKCIF_STREAM_CIF];
6310	11551
6311	11552	if ((intstat & FRAME_END)) {
6312		- struct vb2_v4l2_buffer *vb_done = NULL;
	11553	+ struct rkcif_buffer *active_buf = NULL;
6313	11554
6314	11555	int_en = rkcif_read_register(cif_dev, CIF_REG_DVP_INTEN);
6315	11556	int_en \|= LINE_INT_EN;
..	..	@@ -6329,7 +11570,7 @@
6329	11570	v4l2_info(&cif_dev->v4l2_dev, "frmid:%d, frmstat:0x%x\n",
6330	11571	frmid, cif_frmst);
6331	11572	rkcif_write_register(cif_dev, CIF_REG_DVP_FRAME_STATUS,
6332		- cif_frmst & 0x3);
	11573	+ FRAME_STAT_CLS);
6333	11574	}
6334	11575
6335	11576	if (lastline != stream->pixm.height \|\|
..	..	@@ -6345,30 +11586,30 @@
6345	11586
6346	11587	if (cif_frmst & CIF_F0_READY) {
6347	11588	if (stream->curr_buf)
6348		- vb_done = &stream->curr_buf->vb;
	11589	+ active_buf = stream->curr_buf;
6349	11590	stream->frame_phase = CIF_CSI_FRAME0_READY;
6350	11591	} else if (cif_frmst & CIF_F1_READY) {
6351	11592	if (stream->next_buf)
6352		- vb_done = &stream->next_buf->vb;
	11593	+ active_buf = stream->next_buf;
6353	11594	stream->frame_phase = CIF_CSI_FRAME1_READY;
6354	11595	}
6355	11596
6356	11597	spin_lock_irqsave(&stream->fps_lock, flags);
6357	11598	if (stream->frame_phase & CIF_CSI_FRAME0_READY)
6358		- stream->fps_stats.frm0_timestamp = ktime_get_ns();
	11599	+ stream->fps_stats.frm0_timestamp = rkcif_time_get_ns(cif_dev);
6359	11600	else if (stream->frame_phase & CIF_CSI_FRAME1_READY)
6360		- stream->fps_stats.frm1_timestamp = ktime_get_ns();
	11601	+ stream->fps_stats.frm1_timestamp = rkcif_time_get_ns(cif_dev);
6361	11602	spin_unlock_irqrestore(&stream->fps_lock, flags);
6362	11603
6363	11604	ret = rkcif_assign_new_buffer_oneframe(stream,
6364	11605	RKCIF_YUV_ADDR_STATE_UPDATE);
6365	11606
6366		- if (vb_done && (!ret)) {
6367		- vb_done->sequence = stream->frame_idx;
6368		- rkcif_vb_done_oneframe(stream, vb_done);
	11607	+ if (active_buf && (!ret)) {
	11608	+ active_buf->vb.sequence = stream->frame_idx;
	11609	+ rkcif_vb_done_tasklet(stream, active_buf);
6369	11610	}
6370	11611	stream->frame_idx++;
6371		- cif_dev->irq_stats.all_frm_end_cnt++;
	11612	+ cif_dev->irq_stats.frm_end_cnt[stream->id]++;
6372	11613	}
6373	11614	} else {
6374	11615	for (i = 0; i < RKCIF_MAX_STREAM_DVP; i++) {
..	..	@@ -6417,10 +11658,24 @@
6417	11658	v4l2_info(&cif_dev->v4l2_dev, "frmid:%d, frmstat:0x%x\n",
6418	11659	frmid, cif_frmst);
6419	11660	rkcif_write_register(cif_dev, CIF_REG_DVP_FRAME_STATUS,
6420		- cif_frmst & 0xffff);
	11661	+ FRAME_STAT_CLS);
6421	11662	}
6422	11663	rkcif_update_stream(cif_dev, stream, ch_id);
6423		- cif_dev->irq_stats.all_frm_end_cnt++;
	11664	+ cif_dev->irq_stats.frm_end_cnt[stream->id]++;
	11665	+ }
	11666	+ }
	11667	+
	11668	+ if ((intstat & LINE_INT_END) && !(intstat & FRAME_END) &&
	11669	+ (cif_dev->dvp_sof_in_oneframe == 0)) {
	11670	+ if ((intstat & (PRE_INF_FRAME_END \| PST_INF_FRAME_END)) == 0x0) {
	11671	+ if ((intstat & INTSTAT_ERR) == 0x0) {
	11672	+ if (!stream->cur_skip_frame)
	11673	+ rkcif_deal_sof(cif_dev);
	11674	+ int_en = rkcif_read_register(cif_dev, CIF_REG_DVP_INTEN);
	11675	+ int_en &= ~LINE_INT_EN;
	11676	+ rkcif_write_register(cif_dev, CIF_REG_DVP_INTEN, int_en);
	11677	+ cif_dev->dvp_sof_in_oneframe = 1;
	11678	+ }
6424	11679	}
6425	11680	}
6426	11681
..	..	@@ -6442,6 +11697,8 @@
6442	11697
6443	11698	intstat = rkcif_read_register(cif_dev, CIF_REG_MIPI_LVDS_INTSTAT);
6444	11699	lastline = rkcif_read_register(cif_dev, CIF_REG_MIPI_LVDS_LINE_INT_NUM_ID0_1);
	11700	+ cif_dev->err_state_work.lastline = lastline;
	11701	+ cif_dev->err_state_work.intstat = intstat;
6445	11702
6446	11703	/* clear all interrupts that has been triggered */
6447	11704	rkcif_write_register(cif_dev, CIF_REG_MIPI_LVDS_INTSTAT, intstat);
..	..	@@ -6465,18 +11722,6 @@
6465	11722	"ERROR: cif lite lvds all err:0x%x!!\n", intstat);
6466	11723	return;
6467	11724	}
6468		-
6469		- if (intstat & CSI_FRAME0_START_ID0)
6470		- rkcif_lvds_event_inc_sof(cif_dev);
6471		-
6472		-
6473		- if (intstat & CSI_FRAME1_START_ID0)
6474		- rkcif_lvds_event_inc_sof(cif_dev);
6475		-
6476		- /* if do not reach frame dma end, return irq */
6477		- mipi_id = rkcif_csi_g_mipi_id(&cif_dev->v4l2_dev, intstat);
6478		- if (mipi_id < 0)
6479		- return;
6480	11725
6481	11726	for (i = 0; i < RKCIF_MAX_STREAM_MIPI; i++) {
6482	11727	mipi_id = rkcif_csi_g_mipi_id(&cif_dev->v4l2_dev,
..	..	@@ -6516,7 +11761,31 @@
6516	11761	}
6517	11762
6518	11763	rkcif_update_stream(cif_dev, stream, mipi_id);
	11764	+ rkcif_monitor_reset_event(cif_dev);
	11765	+ cif_dev->irq_stats.frm_end_cnt[stream->id]++;
6519	11766	}
6520		- cif_dev->irq_stats.all_frm_end_cnt++;
	11767	+
	11768	+ if (intstat & CSI_FRAME0_START_ID0)
	11769	+ rkcif_lvds_event_inc_sof(cif_dev);
	11770	+
	11771	+ if (intstat & CSI_FRAME1_START_ID0)
	11772	+ rkcif_lvds_event_inc_sof(cif_dev);
6521	11773	}
6522	11774	}
	11775	+
	11776	+int rkcif_sditf_disconnect(struct video_device *vdev)
	11777	+{
	11778	+ struct rkcif_vdev_node *vnode = vdev_to_node(vdev);
	11779	+ struct rkcif_stream *stream = to_rkcif_stream(vnode);
	11780	+ struct rkcif_device *cifdev = stream->cifdev;
	11781	+ struct media_link *link;
	11782	+ int ret;
	11783	+
	11784	+ link = list_first_entry(&cifdev->sditf[0]->sd.entity.links, struct media_link, list);
	11785	+ ret = media_entity_setup_link(link, 0);
	11786	+ if (ret)
	11787	+ dev_err(cifdev->dev, "failed to disable link of sditf with isp");
	11788	+
	11789	+ return ret;
	11790	+}
	11791	+EXPORT_SYMBOL(rkcif_sditf_disconnect);