~hc/RK356X_SDK_RELEASE.git

..	..	@@ -13,159 +13,21 @@
13	13	#include <linux/module.h>
14	14	#include <linux/of.h>
15	15	#include <linux/of_graph.h>
	16	+#include <linux/of_platform.h>
16	17	#include <linux/platform_device.h>
17	18	#include <linux/reset.h>
18		-#include <media/v4l2-device.h>
19		-#include <media/v4l2-fwnode.h>
20		-#include <media/v4l2-subdev.h>
21		-#include <media/v4l2-event.h>
22	19	#include <linux/rk-camera-module.h>
23	20	#include <media/v4l2-ioctl.h>
24	21	#include "mipi-csi2.h"
	22	+#include <linux/regulator/consumer.h>
25	23
26	24	static int csi2_debug;
27	25	module_param_named(debug_csi2, csi2_debug, int, 0644);
28	26	MODULE_PARM_DESC(debug_csi2, "Debug level (0-1)");
29	27
30		-/*
31		- * there must be 5 pads: 1 input pad from sensor, and
32		- * the 4 virtual channel output pads
33		- */
34		-#define CSI2_SINK_PAD 0
35		-#define CSI2_NUM_SINK_PADS 1
36		-#define CSI2_NUM_SRC_PADS 4
37		-#define CSI2_NUM_PADS 5
38		-#define CSI2_NUM_PADS_SINGLE_LINK 2
39		-#define MAX_CSI2_SENSORS 2
40		-
41		-#define RKCIF_DEFAULT_WIDTH 640
42		-#define RKCIF_DEFAULT_HEIGHT 480
43		-
44		-/*
45		- * The default maximum bit-rate per lane in Mbps, if the
46		- * source subdev does not provide V4L2_CID_LINK_FREQ.
47		- */
48		-#define CSI2_DEFAULT_MAX_MBPS 849
49		-
50		-#define IMX_MEDIA_GRP_ID_CSI2 BIT(8)
51		-#define CSIHOST_MAX_ERRINT_COUNT 10
52		-
53		-/*
54		- * add new chip id in tail in time order
55		- * by increasing to distinguish csi2 host version
56		- */
57		-enum rkcsi2_chip_id {
58		- CHIP_PX30_CSI2,
59		- CHIP_RK1808_CSI2,
60		- CHIP_RK3128_CSI2,
61		- CHIP_RK3288_CSI2,
62		- CHIP_RV1126_CSI2,
63		- CHIP_RK3568_CSI2,
64		-};
65		-
66		-enum csi2_pads {
67		- RK_CSI2_PAD_SINK = 0,
68		- RK_CSI2X_PAD_SOURCE0,
69		- RK_CSI2X_PAD_SOURCE1,
70		- RK_CSI2X_PAD_SOURCE2,
71		- RK_CSI2X_PAD_SOURCE3
72		-};
73		-
74		-enum csi2_err {
75		- RK_CSI2_ERR_SOTSYN = 0x0,
76		- RK_CSI2_ERR_FS_FE_MIS,
77		- RK_CSI2_ERR_FRM_SEQ_ERR,
78		- RK_CSI2_ERR_CRC_ONCE,
79		- RK_CSI2_ERR_CRC,
80		- RK_CSI2_ERR_ALL,
81		- RK_CSI2_ERR_MAX
82		-};
83		-
84		-enum host_type_t {
85		- RK_CSI_RXHOST,
86		- RK_DSI_RXHOST
87		-};
88		-
89		-struct csi2_match_data {
90		- int chip_id;
91		- int num_pads;
92		-};
93		-
94		-struct csi2_sensor {
95		- struct v4l2_subdev *sd;
96		- struct v4l2_mbus_config mbus;
97		- int lanes;
98		-};
99		-
100		-struct csi2_err_stats {
101		- unsigned int cnt;
102		-};
103		-
104		-struct csi2_dev {
105		- struct device *dev;
106		- struct v4l2_subdev sd;
107		- struct media_pad pad[CSI2_NUM_PADS];
108		- struct clk_bulk_data *clks_bulk;
109		- int clks_num;
110		- struct reset_control *rsts_bulk;
111		-
112		- void __iomem *base;
113		- struct v4l2_async_notifier notifier;
114		- struct v4l2_fwnode_bus_mipi_csi2 bus;
115		-
116		- /* lock to protect all members below */
117		- struct mutex lock;
118		-
119		- struct v4l2_mbus_framefmt format_mbus;
120		- struct v4l2_rect crop;
121		- int stream_count;
122		- struct v4l2_subdev *src_sd;
123		- bool sink_linked[CSI2_NUM_SRC_PADS];
124		- struct csi2_sensor sensors[MAX_CSI2_SENSORS];
125		- const struct csi2_match_data *match_data;
126		- int num_sensors;
127		- atomic_t frm_sync_seq;
128		- struct csi2_err_stats err_list[RK_CSI2_ERR_MAX];
129		- int dsi_input_en;
130		-};
131		-
132		-#define DEVICE_NAME "rockchip-mipi-csi2"
133		-
134		-/* CSI Host Registers Define */
135		-#define CSIHOST_N_LANES 0x04
136		-#define CSIHOST_DPHY_SHUTDOWNZ 0x08
137		-#define CSIHOST_PHY_RSTZ 0x0c
138		-#define CSIHOST_RESETN 0x10
139		-#define CSIHOST_PHY_STATE 0x14
140		-#define CSIHOST_ERR1 0x20
141		-#define CSIHOST_ERR2 0x24
142		-#define CSIHOST_MSK1 0x28
143		-#define CSIHOST_MSK2 0x2c
144		-#define CSIHOST_CONTROL 0x40
145		-
146		-#define CSIHOST_ERR1_PHYERR_SPTSYNCHS 0x0000000f
147		-#define CSIHOST_ERR1_ERR_BNDRY_MATCH 0x000000f0
148		-#define CSIHOST_ERR1_ERR_SEQ 0x00000f00
149		-#define CSIHOST_ERR1_ERR_FRM_DATA 0x0000f000
150		-#define CSIHOST_ERR1_ERR_CRC 0x1f000000
151		-
152		-#define CSIHOST_ERR2_PHYERR_ESC 0x0000000f
153		-#define CSIHOST_ERR2_PHYERR_SOTHS 0x000000f0
154		-#define CSIHOST_ERR2_ECC_CORRECTED 0x00000f00
155		-#define CSIHOST_ERR2_ERR_ID 0x0000f000
156		-#define CSIHOST_ERR2_PHYERR_CODEHS 0x01000000
157		-
158		-#define SW_CPHY_EN(x) ((x) << 0)
159		-#define SW_DSI_EN(x) ((x) << 4)
160		-#define SW_DATATYPE_FS(x) ((x) << 8)
161		-#define SW_DATATYPE_FE(x) ((x) << 14)
162		-#define SW_DATATYPE_LS(x) ((x) << 20)
163		-#define SW_DATATYPE_LE(x) ((x) << 26)
164		-
165	28	#define write_csihost_reg(base, addr, val) writel(val, (addr) + (base))
166	29	#define read_csihost_reg(base, addr) readl((addr) + (base))
167	30
168		-static struct csi2_dev *g_csi2_dev;
169	31	static ATOMIC_NOTIFIER_HEAD(g_csi_host_chain);
170	32
171	33	int rkcif_csi2_register_notifier(struct notifier_block *nb)
..	..	@@ -183,7 +45,7 @@
183	45	return container_of(sdev, struct csi2_dev, sd);
184	46	}
185	47
186		-static struct csi2_sensor sd_to_sensor(struct csi2_dev csi2,
	48	+static struct csi2_sensor_info sd_to_sensor(struct csi2_dev csi2,
187	49	struct v4l2_subdev *sd)
188	50	{
189	51	int i;
..	..	@@ -227,6 +89,7 @@
227	89	*sensor_sd = NULL;
228	90	return;
229	91	}
	92	+
230	93	media_graph_walk_start(&graph, entity);
231	94	while ((entity = media_graph_walk_next(&graph))) {
232	95	if (entity->function == MEDIA_ENT_F_CAM_SENSOR)
..	..	@@ -234,6 +97,7 @@
234	97	}
235	98	mutex_unlock(&mdev->graph_mutex);
236	99	media_graph_walk_cleanup(&graph);
	100	+
237	101	if (entity)
238	102	*sensor_sd = media_entity_to_v4l2_subdev(entity);
239	103	else
..	..	@@ -242,24 +106,25 @@
242	106
243	107	static void csi2_update_sensor_info(struct csi2_dev *csi2)
244	108	{
245		- struct csi2_sensor *sensor = &csi2->sensors[0];
246	109	struct v4l2_subdev *terminal_sensor_sd = NULL;
	110	+ struct csi2_sensor_info *sensor = &csi2->sensors[0];
247	111	struct v4l2_mbus_config mbus;
248	112	int ret = 0;
249	113
250		- ret = v4l2_subdev_call(sensor->sd, video, g_mbus_config, &mbus);
	114	+ ret = v4l2_subdev_call(sensor->sd, pad, get_mbus_config, 0, &mbus);
251	115	if (ret) {
252	116	v4l2_err(&csi2->sd, "update sensor info failed!\n");
253	117	return;
254	118	}
255	119
256	120	get_remote_terminal_sensor(&csi2->sd, &terminal_sensor_sd);
257		- if (terminal_sensor_sd) {
258		- ret = v4l2_subdev_call(terminal_sensor_sd, core, ioctl,
259		- RKMODULE_GET_CSI_DSI_INFO, &csi2->dsi_input_en);
260		- if (ret)
261		- csi2->dsi_input_en = 0;
	121	+ ret = v4l2_subdev_call(terminal_sensor_sd, core, ioctl,
	122	+ RKMODULE_GET_CSI_DSI_INFO, &csi2->dsi_input_en);
	123	+ if (ret) {
	124	+ v4l2_dbg(1, csi2_debug, &csi2->sd, "get CSI/DSI sel failed, default csi!\n");
	125	+ csi2->dsi_input_en = 0;
262	126	}
	127	+
263	128	csi2->bus.flags = mbus.flags;
264	129	switch (csi2->bus.flags & V4L2_MBUS_CSI2_LANES) {
265	130	case V4L2_MBUS_CSI2_1_LANE:
..	..	@@ -282,61 +147,83 @@
282	147
283	148	}
284	149
285		-static void csi2_hw_do_reset(struct csi2_dev *csi2)
	150	+static void csi2_hw_do_reset(struct csi2_hw *csi2_hw)
286	151	{
287		- reset_control_assert(csi2->rsts_bulk);
	152	+
	153	+ if (!csi2_hw->rsts_bulk)
	154	+ return;
	155	+
	156	+ reset_control_assert(csi2_hw->rsts_bulk);
288	157
289	158	udelay(5);
290	159
291		- reset_control_deassert(csi2->rsts_bulk);
	160	+ reset_control_deassert(csi2_hw->rsts_bulk);
292	161	}
293	162
294		-static int csi2_enable_clks(struct csi2_dev *csi2)
	163	+static int csi2_enable_clks(struct csi2_hw *csi2_hw)
295	164	{
296	165	int ret = 0;
297	166
298		- ret = clk_bulk_prepare_enable(csi2->clks_num, csi2->clks_bulk);
	167	+ if (!csi2_hw->clks_bulk)
	168	+ return -EINVAL;
	169	+
	170	+ ret = clk_bulk_prepare_enable(csi2_hw->clks_num, csi2_hw->clks_bulk);
299	171	if (ret)
300		- dev_err(csi2->dev, "failed to enable clks\n");
	172	+ dev_err(csi2_hw->dev, "failed to enable clks\n");
301	173
302	174	return ret;
303	175	}
304	176
305		-static void csi2_disable_clks(struct csi2_dev *csi2)
	177	+static void csi2_disable_clks(struct csi2_hw *csi2_hw)
306	178	{
307		- clk_bulk_disable_unprepare(csi2->clks_num, csi2->clks_bulk);
	179	+ if (!csi2_hw->clks_bulk)
	180	+ return;
	181	+ clk_bulk_disable_unprepare(csi2_hw->clks_num, csi2_hw->clks_bulk);
308	182	}
309	183
310		-static void csi2_disable(struct csi2_dev *csi2)
	184	+static void csi2_disable(struct csi2_hw *csi2_hw)
311	185	{
312		- void __iomem *base = csi2->base;
313		-
314		- write_csihost_reg(base, CSIHOST_RESETN, 0);
315		- write_csihost_reg(base, CSIHOST_MSK1, 0xffffffff);
316		- write_csihost_reg(base, CSIHOST_MSK2, 0xffffffff);
	186	+ write_csihost_reg(csi2_hw->base, CSIHOST_RESETN, 0);
	187	+ write_csihost_reg(csi2_hw->base, CSIHOST_MSK1, 0xffffffff);
	188	+ write_csihost_reg(csi2_hw->base, CSIHOST_MSK2, 0xffffffff);
317	189	}
318	190
319		-static void csi2_enable(struct csi2_dev *csi2,
	191	+static int csi2_g_mbus_config(struct v4l2_subdev *sd, unsigned int pad_id,
	192	+ struct v4l2_mbus_config *mbus);
	193	+
	194	+static void csi2_enable(struct csi2_hw *csi2_hw,
320	195	enum host_type_t host_type)
321	196	{
322		- void __iomem *base = csi2->base;
	197	+ void __iomem *base = csi2_hw->base;
	198	+ struct csi2_dev *csi2 = csi2_hw->csi2;
323	199	int lanes = csi2->bus.num_data_lanes;
	200	+ struct v4l2_mbus_config mbus;
	201	+ u32 val = 0;
	202	+
	203	+ csi2_g_mbus_config(&csi2->sd, 0, &mbus);
	204	+ if (mbus.type == V4L2_MBUS_CSI2_DPHY)
	205	+ val = SW_CPHY_EN(0);
	206	+ else if (mbus.type == V4L2_MBUS_CSI2_CPHY)
	207	+ val = SW_CPHY_EN(1);
324	208
325	209	write_csihost_reg(base, CSIHOST_N_LANES, lanes - 1);
326	210
327	211	if (host_type == RK_DSI_RXHOST) {
328		- write_csihost_reg(base, CSIHOST_CONTROL,
329		- SW_CPHY_EN(0) \| SW_DSI_EN(1) \|
330		- SW_DATATYPE_FS(0x01) \| SW_DATATYPE_FE(0x11) \|
331		- SW_DATATYPE_LS(0x21) \| SW_DATATYPE_LE(0x31));
	212	+ val \|= SW_DSI_EN(1) \| SW_DATATYPE_FS(0x01) \|
	213	+ SW_DATATYPE_FE(0x11) \| SW_DATATYPE_LS(0x21) \|
	214	+ SW_DATATYPE_LE(0x31);
	215	+ write_csihost_reg(base, CSIHOST_CONTROL, val);
332	216	/* Disable some error interrupt when HOST work on DSI RX mode */
333	217	write_csihost_reg(base, CSIHOST_MSK1, 0xe00000f0);
334	218	write_csihost_reg(base, CSIHOST_MSK2, 0xff00);
335	219	} else {
336		- write_csihost_reg(base, CSIHOST_CONTROL,
337		- SW_CPHY_EN(0) \| SW_DSI_EN(0));
338		- write_csihost_reg(base, CSIHOST_MSK1, 0);
	220	+ val \|= SW_DSI_EN(0) \| SW_DATATYPE_FS(0x0) \|
	221	+ SW_DATATYPE_FE(0x01) \| SW_DATATYPE_LS(0x02) \|
	222	+ SW_DATATYPE_LE(0x03);
	223	+ write_csihost_reg(base, CSIHOST_CONTROL, val);
	224	+ write_csihost_reg(base, CSIHOST_MSK1, 0x0);
339	225	write_csihost_reg(base, CSIHOST_MSK2, 0xf000);
	226	+ csi2->is_check_sot_sync = true;
340	227	}
341	228
342	229	write_csihost_reg(base, CSIHOST_RESETN, 1);
..	..	@@ -346,15 +233,9 @@
346	233	{
347	234	enum host_type_t host_type;
348	235	int ret, i;
	236	+ int csi_idx = 0;
349	237
350	238	atomic_set(&csi2->frm_sync_seq, 0);
351		-
352		- csi2_hw_do_reset(csi2);
353		- ret = csi2_enable_clks(csi2);
354		- if (ret) {
355		- v4l2_err(&csi2->sd, "%s: enable clks failed\n", __func__);
356		- return ret;
357		- }
358	239
359	240	csi2_update_sensor_info(csi2);
360	241
..	..	@@ -363,7 +244,16 @@
363	244	else
364	245	host_type = RK_CSI_RXHOST;
365	246
366		- csi2_enable(csi2, host_type);
	247	+ for (i = 0; i < csi2->csi_info.csi_num; i++) {
	248	+ csi_idx = csi2->csi_info.csi_idx[i];
	249	+ csi2_hw_do_reset(csi2->csi2_hw[csi_idx]);
	250	+ ret = csi2_enable_clks(csi2->csi2_hw[csi_idx]);
	251	+ if (ret) {
	252	+ v4l2_err(&csi2->sd, "%s: enable clks failed\n", __func__);
	253	+ return ret;
	254	+ }
	255	+ csi2_enable(csi2->csi2_hw[csi_idx], host_type);
	256	+ }
367	257
368	258	pr_debug("stream sd: %s\n", csi2->src_sd->name);
369	259	ret = v4l2_subdev_call(csi2->src_sd, video, s_stream, 1);
..	..	@@ -377,20 +267,29 @@
377	267	return 0;
378	268
379	269	err_assert_reset:
380		- csi2_disable(csi2);
381		- csi2_disable_clks(csi2);
	270	+ for (i = 0; i < csi2->csi_info.csi_num; i++) {
	271	+ csi_idx = csi2->csi_info.csi_idx[i];
	272	+ csi2_disable(csi2->csi2_hw[csi_idx]);
	273	+ csi2_disable_clks(csi2->csi2_hw[csi_idx]);
	274	+ }
382	275
383	276	return ret;
384	277	}
385	278
386	279	static void csi2_stop(struct csi2_dev *csi2)
387	280	{
	281	+ int i = 0;
	282	+ int csi_idx = 0;
	283	+
388	284	/* stop upstream */
389	285	v4l2_subdev_call(csi2->src_sd, video, s_stream, 0);
390	286
391		- csi2_disable(csi2);
392		- csi2_hw_do_reset(csi2);
393		- csi2_disable_clks(csi2);
	287	+ for (i = 0; i < csi2->csi_info.csi_num; i++) {
	288	+ csi_idx = csi2->csi_info.csi_idx[i];
	289	+ csi2_disable(csi2->csi2_hw[csi_idx]);
	290	+ csi2_hw_do_reset(csi2->csi2_hw[csi_idx]);
	291	+ csi2_disable_clks(csi2->csi2_hw[csi_idx]);
	292	+ }
394	293	}
395	294
396	295	/*
..	..	@@ -556,10 +455,12 @@
556	455	switch (sel->target) {
557	456	case V4L2_SEL_TGT_CROP_BOUNDS:
558	457	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
	458	+ sel->pad = 0;
559	459	ret = v4l2_subdev_call(sensor, pad, get_selection,
560	460	cfg, sel);
561	461	if (ret) {
562	462	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
	463	+ fmt.pad = 0;
563	464	ret = v4l2_subdev_call(sensor, pad, get_fmt, NULL, &fmt);
564	465	if (!ret) {
565	466	csi2->format_mbus = fmt.format;
..	..	@@ -608,16 +509,16 @@
608	509	return ret;
609	510	}
610	511
611		-static int csi2_g_mbus_config(struct v4l2_subdev *sd,
	512	+static int csi2_g_mbus_config(struct v4l2_subdev *sd, unsigned int pad_id,
612	513	struct v4l2_mbus_config *mbus)
613	514	{
614	515	struct csi2_dev *csi2 = sd_to_dev(sd);
615	516	struct v4l2_subdev *sensor_sd = get_remote_sensor(sd);
616	517	int ret;
617	518
618		- ret = v4l2_subdev_call(sensor_sd, video, g_mbus_config, mbus);
	519	+ ret = v4l2_subdev_call(sensor_sd, pad, get_mbus_config, 0, mbus);
619	520	if (ret) {
620		- mbus->type = V4L2_MBUS_CSI2;
	521	+ mbus->type = V4L2_MBUS_CSI2_DPHY;
621	522	mbus->flags = csi2->bus.flags;
622	523	mbus->flags \|= BIT(csi2->bus.num_data_lanes - 1);
623	524	}
..	..	@@ -630,50 +531,118 @@
630	531	.link_validate = v4l2_subdev_link_validate,
631	532	};
632	533
633		-void rkcif_csi2_event_inc_sof(void)
	534	+void rkcif_csi2_event_reset_pipe(struct csi2_dev *csi2_dev, int reset_src)
634	535	{
635		- if (g_csi2_dev) {
	536	+ if (csi2_dev) {
636	537	struct v4l2_event event = {
637		- .type = V4L2_EVENT_FRAME_SYNC,
638		- .u.frame_sync.frame_sequence =
639		- atomic_inc_return(&g_csi2_dev->frm_sync_seq) - 1,
	538	+ .type = V4L2_EVENT_RESET_DEV,
	539	+ .reserved[0] = reset_src,
640	540	};
641		- v4l2_event_queue(g_csi2_dev->sd.devnode, &event);
	541	+ v4l2_event_queue(csi2_dev->sd.devnode, &event);
642	542	}
643	543	}
644	544
645		-u32 rkcif_csi2_get_sof(void)
	545	+void rkcif_csi2_event_inc_sof(struct csi2_dev *csi2_dev)
646	546	{
647		- if (g_csi2_dev) {
648		- return atomic_read(&g_csi2_dev->frm_sync_seq) - 1;
	547	+ if (csi2_dev) {
	548	+ struct v4l2_event event = {
	549	+ .type = V4L2_EVENT_FRAME_SYNC,
	550	+ .u.frame_sync.frame_sequence =
	551	+ atomic_inc_return(&csi2_dev->frm_sync_seq) - 1,
	552	+ };
	553	+ v4l2_event_queue(csi2_dev->sd.devnode, &event);
649	554	}
	555	+}
	556	+
	557	+u32 rkcif_csi2_get_sof(struct csi2_dev *csi2_dev)
	558	+{
	559	+ if (csi2_dev)
	560	+ return atomic_read(&csi2_dev->frm_sync_seq) - 1;
650	561
651	562	return 0;
652	563	}
653	564
654		-void rkcif_csi2_set_sof(u32 seq)
	565	+void rkcif_csi2_set_sof(struct csi2_dev *csi2_dev, u32 seq)
655	566	{
656		- if (g_csi2_dev) {
657		- atomic_set(&g_csi2_dev->frm_sync_seq, seq);
658		- }
	567	+ if (csi2_dev)
	568	+ atomic_set(&csi2_dev->frm_sync_seq, seq);
659	569	}
660	570
661	571	static int rkcif_csi2_subscribe_event(struct v4l2_subdev sd, struct v4l2_fh fh,
662	572	struct v4l2_event_subscription *sub)
663	573	{
664		- if (sub->type != V4L2_EVENT_FRAME_SYNC)
	574	+ if (sub->type == V4L2_EVENT_FRAME_SYNC \|\|
	575	+ sub->type == V4L2_EVENT_RESET_DEV)
	576	+ return v4l2_event_subscribe(fh, sub, RKCIF_V4L2_EVENT_ELEMS, NULL);
	577	+ else
665	578	return -EINVAL;
666		-
667		- return v4l2_event_subscribe(fh, sub, 0, NULL);
668	579	}
	580	+
	581	+static int rkcif_csi2_s_power(struct v4l2_subdev *sd, int on)
	582	+{
	583	+ return 0;
	584	+}
	585	+
	586	+static long rkcif_csi2_ioctl(struct v4l2_subdev sd, unsigned int cmd, void arg)
	587	+{
	588	+ struct csi2_dev *csi2 = sd_to_dev(sd);
	589	+ struct v4l2_subdev *sensor = get_remote_sensor(sd);
	590	+ long ret = 0;
	591	+ int i = 0;
	592	+
	593	+ switch (cmd) {
	594	+ case RKCIF_CMD_SET_CSI_IDX:
	595	+ csi2->csi_info = ((struct rkcif_csi_info )arg);
	596	+ for (i = 0; i < csi2->csi_info.csi_num; i++)
	597	+ csi2->csi2_hw[csi2->csi_info.csi_idx[i]]->csi2 = csi2;
	598	+ if (csi2->match_data->chip_id > CHIP_RV1126_CSI2)
	599	+ ret = v4l2_subdev_call(sensor, core, ioctl,
	600	+ RKCIF_CMD_SET_CSI_IDX,
	601	+ arg);
	602	+ break;
	603	+ default:
	604	+ ret = -ENOIOCTLCMD;
	605	+ break;
	606	+ }
	607	+
	608	+ return ret;
	609	+}
	610	+
	611	+#ifdef CONFIG_COMPAT
	612	+static long rkcif_csi2_compat_ioctl32(struct v4l2_subdev *sd,
	613	+ unsigned int cmd, unsigned long arg)
	614	+{
	615	+ void __user *up = compat_ptr(arg);
	616	+ struct rkcif_csi_info csi_info;
	617	+ long ret;
	618	+
	619	+ switch (cmd) {
	620	+ case RKCIF_CMD_SET_CSI_IDX:
	621	+ if (copy_from_user(&csi_info, up, sizeof(struct rkcif_csi_info)))
	622	+ return -EFAULT;
	623	+
	624	+ ret = rkcif_csi2_ioctl(sd, cmd, &csi_info);
	625	+ break;
	626	+ default:
	627	+ ret = -ENOIOCTLCMD;
	628	+ break;
	629	+ }
	630	+
	631	+ return ret;
	632	+}
	633	+#endif
669	634
670	635	static const struct v4l2_subdev_core_ops csi2_core_ops = {
671	636	.subscribe_event = rkcif_csi2_subscribe_event,
672	637	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
	638	+ .s_power = rkcif_csi2_s_power,
	639	+ .ioctl = rkcif_csi2_ioctl,
	640	+#ifdef CONFIG_COMPAT
	641	+ .compat_ioctl32 = rkcif_csi2_compat_ioctl32,
	642	+#endif
673	643	};
674	644
675	645	static const struct v4l2_subdev_video_ops csi2_video_ops = {
676		- .g_mbus_config = csi2_g_mbus_config,
677	646	.s_stream = csi2_s_stream,
678	647	};
679	648
..	..	@@ -682,6 +651,7 @@
682	651	.set_fmt = csi2_get_set_fmt,
683	652	.get_selection = csi2_get_selection,
684	653	.set_selection = csi2_set_selection,
	654	+ .get_mbus_config = csi2_g_mbus_config,
685	655	};
686	656
687	657	static const struct v4l2_subdev_ops csi2_subdev_ops = {
..	..	@@ -716,7 +686,7 @@
716	686	struct csi2_dev *csi2 = container_of(notifier,
717	687	struct csi2_dev,
718	688	notifier);
719		- struct csi2_sensor *sensor;
	689	+ struct csi2_sensor_info *sensor;
720	690	struct media_link *link;
721	691	unsigned int pad, ret;
722	692
..	..	@@ -772,10 +742,10 @@
772	742	struct csi2_dev *csi2 = container_of(notifier,
773	743	struct csi2_dev,
774	744	notifier);
775		- struct csi2_sensor *sensor = sd_to_sensor(csi2, sd);
	745	+ struct csi2_sensor_info *sensor = sd_to_sensor(csi2, sd);
776	746
777		- sensor->sd = NULL;
778		-
	747	+ if (sensor)
	748	+ sensor->sd = NULL;
779	749	}
780	750
781	751	static const struct
..	..	@@ -784,65 +754,123 @@
784	754	.unbind = csi2_notifier_unbind,
785	755	};
786	756
	757	+static void csi2_find_err_vc(int val, char *vc_info)
	758	+{
	759	+ int i;
	760	+ char cur_str[CSI_VCINFO_LEN] = {0};
	761	+
	762	+ memset(vc_info, 0, sizeof(*vc_info));
	763	+ for (i = 0; i < 4; i++) {
	764	+ if ((val >> i) & 0x1) {
	765	+ snprintf(cur_str, CSI_VCINFO_LEN, " %d", i);
	766	+ if (strlen(vc_info) + strlen(cur_str) < CSI_VCINFO_LEN)
	767	+ strncat(vc_info, cur_str, strlen(cur_str));
	768	+ }
	769	+ }
	770	+}
	771	+
	772	+#define csi2_err_strncat(dst_str, src_str) {\
	773	+ if (strlen(dst_str) + strlen(src_str) < CSI_ERRSTR_LEN)\
	774	+ strncat(dst_str, src_str, strlen(src_str)); }
	775	+
787	776	static irqreturn_t rk_csirx_irq1_handler(int irq, void *ctx)
788	777	{
789	778	struct device *dev = ctx;
790		- struct csi2_dev *csi2 = sd_to_dev(dev_get_drvdata(dev));
	779	+ struct csi2_hw *csi2_hw = dev_get_drvdata(dev);
	780	+ struct csi2_dev *csi2 = csi2_hw->csi2;
791	781	struct csi2_err_stats *err_list = NULL;
792	782	unsigned long err_stat = 0;
793	783	u32 val;
	784	+ char err_str[CSI_ERRSTR_LEN] = {0};
	785	+ char cur_str[CSI_ERRSTR_LEN] = {0};
	786	+ char vc_info[CSI_VCINFO_LEN] = {0};
	787	+ bool is_add_cnt = false;
794	788
795		- val = read_csihost_reg(csi2->base, CSIHOST_ERR1);
	789	+ val = read_csihost_reg(csi2_hw->base, CSIHOST_ERR1);
796	790	if (val) {
797		- write_csihost_reg(csi2->base,
798		- CSIHOST_ERR1, 0x0);
799		-
800	791	if (val & CSIHOST_ERR1_PHYERR_SPTSYNCHS) {
801	792	err_list = &csi2->err_list[RK_CSI2_ERR_SOTSYN];
802	793	err_list->cnt++;
803		- v4l2_err(&csi2->sd,
804		- "ERR1: start of transmission error(no synchronization achieved), reg: 0x%x,cnt:%d\n",
805		- val, err_list->cnt);
	794	+ if (csi2->match_data->chip_id == CHIP_RK3588_CSI2) {
	795	+ if (err_list->cnt > 3 &&
	796	+ csi2->err_list[RK_CSI2_ERR_ALL].cnt <= err_list->cnt) {
	797	+ csi2->is_check_sot_sync = false;
	798	+ write_csihost_reg(csi2_hw->base, CSIHOST_MSK1, 0xf);
	799	+ }
	800	+ if (csi2->is_check_sot_sync) {
	801	+ csi2_find_err_vc(val & 0xf, vc_info);
	802	+ snprintf(cur_str, CSI_ERRSTR_LEN, "(sot sync,lane:%s) ", vc_info);
	803	+ csi2_err_strncat(err_str, cur_str);
	804	+ }
	805	+ } else {
	806	+ csi2_find_err_vc(val & 0xf, vc_info);
	807	+ snprintf(cur_str, CSI_ERRSTR_LEN, "(sot sync,lane:%s) ", vc_info);
	808	+ csi2_err_strncat(err_str, cur_str);
	809	+ is_add_cnt = true;
	810	+ }
806	811	}
807	812
808	813	if (val & CSIHOST_ERR1_ERR_BNDRY_MATCH) {
809	814	err_list = &csi2->err_list[RK_CSI2_ERR_FS_FE_MIS];
810	815	err_list->cnt++;
811		- v4l2_err(&csi2->sd,
812		- "ERR1: error matching frame start with frame end, reg: 0x%x,cnt:%d\n",
813		- val, err_list->cnt);
	816	+ csi2_find_err_vc((val >> 4) & 0xf, vc_info);
	817	+ snprintf(cur_str, CSI_ERRSTR_LEN, "(fs/fe mis,vc:%s) ", vc_info);
	818	+ csi2_err_strncat(err_str, cur_str);
	819	+ if (csi2->match_data->chip_id < CHIP_RK3588_CSI2)
	820	+ is_add_cnt = true;
814	821	}
815	822
816	823	if (val & CSIHOST_ERR1_ERR_SEQ) {
817	824	err_list = &csi2->err_list[RK_CSI2_ERR_FRM_SEQ_ERR];
818	825	err_list->cnt++;
819		- v4l2_err(&csi2->sd,
820		- "ERR1: incorrect frame sequence detected, reg: 0x%x,cnt:%d\n",
821		- val, err_list->cnt);
	826	+ csi2_find_err_vc((val >> 8) & 0xf, vc_info);
	827	+ snprintf(cur_str, CSI_ERRSTR_LEN, "(f_seq,vc:%s) ", vc_info);
	828	+ csi2_err_strncat(err_str, cur_str);
822	829	}
823	830
824	831	if (val & CSIHOST_ERR1_ERR_FRM_DATA) {
825	832	err_list = &csi2->err_list[RK_CSI2_ERR_CRC_ONCE];
	833	+ is_add_cnt = true;
826	834	err_list->cnt++;
827		- v4l2_dbg(1, csi2_debug, &csi2->sd,
828		- "ERR1: at least one crc error, reg: 0x%x\n,cnt:%d", val, err_list->cnt);
	835	+ csi2_find_err_vc((val >> 12) & 0xf, vc_info);
	836	+ snprintf(cur_str, CSI_ERRSTR_LEN, "(err_data,vc:%s) ", vc_info);
	837	+ csi2_err_strncat(err_str, cur_str);
829	838	}
830	839
831	840	if (val & CSIHOST_ERR1_ERR_CRC) {
832	841	err_list = &csi2->err_list[RK_CSI2_ERR_CRC];
833	842	err_list->cnt++;
834		- v4l2_err(&csi2->sd,
835		- "ERR1: crc errors, reg: 0x%x, cnt:%d\n",
836		- val, err_list->cnt);
	843	+ is_add_cnt = true;
	844	+ csi2_find_err_vc((val >> 24) & 0xf, vc_info);
	845	+ snprintf(cur_str, CSI_ERRSTR_LEN, "(crc,vc:%s) ", vc_info);
	846	+ csi2_err_strncat(err_str, cur_str);
837	847	}
838	848
839		- csi2->err_list[RK_CSI2_ERR_ALL].cnt++;
840		- err_stat = ((csi2->err_list[RK_CSI2_ERR_FS_FE_MIS].cnt & 0xff) << 8) \|
841		- ((csi2->err_list[RK_CSI2_ERR_ALL].cnt) & 0xff);
	849	+ if (val & CSIHOST_ERR1_ERR_ECC2) {
	850	+ err_list = &csi2->err_list[RK_CSI2_ERR_CRC];
	851	+ err_list->cnt++;
	852	+ is_add_cnt = true;
	853	+ snprintf(cur_str, CSI_ERRSTR_LEN, "(ecc2) ");
	854	+ csi2_err_strncat(err_str, cur_str);
	855	+ }
842	856
843		- atomic_notifier_call_chain(&g_csi_host_chain,
844		- err_stat,
845		- NULL);
	857	+ if (val & CSIHOST_ERR1_ERR_CTRL) {
	858	+ csi2_find_err_vc((val >> 16) & 0xf, vc_info);
	859	+ snprintf(cur_str, CSI_ERRSTR_LEN, "(ctrl,vc:%s) ", vc_info);
	860	+ csi2_err_strncat(err_str, cur_str);
	861	+ }
	862	+
	863	+ pr_err("%s ERR1:0x%x %s\n", csi2_hw->dev_name, val, err_str);
	864	+
	865	+ if (is_add_cnt) {
	866	+ csi2->err_list[RK_CSI2_ERR_ALL].cnt++;
	867	+ err_stat = ((csi2->err_list[RK_CSI2_ERR_FS_FE_MIS].cnt & 0xff) << 8) \|
	868	+ ((csi2->err_list[RK_CSI2_ERR_ALL].cnt) & 0xff);
	869	+
	870	+ atomic_notifier_call_chain(&g_csi_host_chain,
	871	+ err_stat,
	872	+ &csi2->csi_info.csi_idx[csi2->csi_info.csi_num - 1]);
	873	+ }
846	874
847	875	}
848	876
..	..	@@ -852,27 +880,40 @@
852	880	static irqreturn_t rk_csirx_irq2_handler(int irq, void *ctx)
853	881	{
854	882	struct device *dev = ctx;
855		- struct csi2_dev *csi2 = sd_to_dev(dev_get_drvdata(dev));
	883	+ struct csi2_hw *csi2_hw = dev_get_drvdata(dev);
856	884	u32 val;
	885	+ char cur_str[CSI_ERRSTR_LEN] = {0};
	886	+ char err_str[CSI_ERRSTR_LEN] = {0};
	887	+ char vc_info[CSI_VCINFO_LEN] = {0};
857	888
858		- val = read_csihost_reg(csi2->base, CSIHOST_ERR2);
	889	+ val = read_csihost_reg(csi2_hw->base, CSIHOST_ERR2);
859	890	if (val) {
860		- if (val & CSIHOST_ERR2_PHYERR_ESC)
861		- v4l2_err(&csi2->sd, "ERR2: escape entry error(ULPM), reg: 0x%x\n", val);
862		- if (val & CSIHOST_ERR2_PHYERR_SOTHS)
863		- v4l2_err(&csi2->sd,
864		- "ERR2: start of transmission error(synchronization can still be achieved), reg: 0x%x\n",
865		- val);
866		- if (val & CSIHOST_ERR2_ECC_CORRECTED)
867		- v4l2_dbg(1, csi2_debug, &csi2->sd,
868		- "ERR2: header error detected and corrected, reg: 0x%x\n",
869		- val);
870		- if (val & CSIHOST_ERR2_ERR_ID)
871		- v4l2_err(&csi2->sd,
872		- "ERR2: unrecognized or unimplemented data type detected, reg: 0x%x\n",
873		- val);
874		- if (val & CSIHOST_ERR2_PHYERR_CODEHS)
875		- v4l2_err(&csi2->sd, "ERR2: receiv error code, reg: 0x%x\n", val);
	891	+ if (val & CSIHOST_ERR2_PHYERR_ESC) {
	892	+ csi2_find_err_vc(val & 0xf, vc_info);
	893	+ snprintf(cur_str, CSI_ERRSTR_LEN, "(ULPM,lane:%s) ", vc_info);
	894	+ csi2_err_strncat(err_str, cur_str);
	895	+ }
	896	+ if (val & CSIHOST_ERR2_PHYERR_SOTHS) {
	897	+ csi2_find_err_vc((val >> 4) & 0xf, vc_info);
	898	+ snprintf(cur_str, CSI_ERRSTR_LEN, "(sot,lane:%s) ", vc_info);
	899	+ csi2_err_strncat(err_str, cur_str);
	900	+ }
	901	+ if (val & CSIHOST_ERR2_ECC_CORRECTED) {
	902	+ csi2_find_err_vc((val >> 8) & 0xf, vc_info);
	903	+ snprintf(cur_str, CSI_ERRSTR_LEN, "(ecc,vc:%s) ", vc_info);
	904	+ csi2_err_strncat(err_str, cur_str);
	905	+ }
	906	+ if (val & CSIHOST_ERR2_ERR_ID) {
	907	+ csi2_find_err_vc((val >> 12) & 0xf, vc_info);
	908	+ snprintf(cur_str, CSI_ERRSTR_LEN, "(err id,vc:%s) ", vc_info);
	909	+ csi2_err_strncat(err_str, cur_str);
	910	+ }
	911	+ if (val & CSIHOST_ERR2_PHYERR_CODEHS) {
	912	+ snprintf(cur_str, CSI_ERRSTR_LEN, "(err code) ");
	913	+ csi2_err_strncat(err_str, cur_str);
	914	+ }
	915	+
	916	+ pr_err("%s ERR2:0x%x %s\n", csi2_hw->dev_name, val, err_str);
876	917	}
877	918
878	919	return IRQ_HANDLED;
..	..	@@ -883,15 +924,14 @@
883	924	struct v4l2_async_notifier *ntf = &csi2->notifier;
884	925	int ret;
885	926
	927	+ v4l2_async_notifier_init(ntf);
	928	+
886	929	ret = v4l2_async_notifier_parse_fwnode_endpoints_by_port(csi2->dev,
887	930	&csi2->notifier,
888	931	sizeof(struct v4l2_async_subdev), 0,
889	932	csi2_parse_endpoint);
890	933	if (ret < 0)
891	934	return ret;
892		-
893		- if (!ntf->num_subdevs)
894		- return -ENODEV; /* no endpoint */
895	935
896	936	csi2->sd.subdev_notifier = &csi2->notifier;
897	937	csi2->notifier.ops = &csi2_async_ops;
..	..	@@ -912,21 +952,43 @@
912	952	static const struct csi2_match_data rk1808_csi2_match_data = {
913	953	.chip_id = CHIP_RK1808_CSI2,
914	954	.num_pads = CSI2_NUM_PADS,
	955	+ .num_hw = 1,
915	956	};
916	957
917	958	static const struct csi2_match_data rk3288_csi2_match_data = {
918	959	.chip_id = CHIP_RK3288_CSI2,
919	960	.num_pads = CSI2_NUM_PADS_SINGLE_LINK,
	961	+ .num_hw = 1,
920	962	};
921	963
922	964	static const struct csi2_match_data rv1126_csi2_match_data = {
923	965	.chip_id = CHIP_RV1126_CSI2,
924	966	.num_pads = CSI2_NUM_PADS,
	967	+ .num_hw = 1,
925	968	};
926	969
927	970	static const struct csi2_match_data rk3568_csi2_match_data = {
928	971	.chip_id = CHIP_RK3568_CSI2,
929	972	.num_pads = CSI2_NUM_PADS,
	973	+ .num_hw = 1,
	974	+};
	975	+
	976	+static const struct csi2_match_data rk3588_csi2_match_data = {
	977	+ .chip_id = CHIP_RK3588_CSI2,
	978	+ .num_pads = CSI2_NUM_PADS_MAX,
	979	+ .num_hw = 6,
	980	+};
	981	+
	982	+static const struct csi2_match_data rv1106_csi2_match_data = {
	983	+ .chip_id = CHIP_RV1106_CSI2,
	984	+ .num_pads = CSI2_NUM_PADS_MAX,
	985	+ .num_hw = 2,
	986	+};
	987	+
	988	+static const struct csi2_match_data rk3562_csi2_match_data = {
	989	+ .chip_id = CHIP_RK3562_CSI2,
	990	+ .num_pads = CSI2_NUM_PADS_MAX,
	991	+ .num_hw = 4,
930	992	};
931	993
932	994	static const struct of_device_id csi2_dt_ids[] = {
..	..	@@ -946,19 +1008,64 @@
946	1008	.compatible = "rockchip,rv1126-mipi-csi2",
947	1009	.data = &rv1126_csi2_match_data,
948	1010	},
	1011	+ {
	1012	+ .compatible = "rockchip,rk3588-mipi-csi2",
	1013	+ .data = &rk3588_csi2_match_data,
	1014	+ },
	1015	+ {
	1016	+ .compatible = "rockchip,rv1106-mipi-csi2",
	1017	+ .data = &rv1106_csi2_match_data,
	1018	+ },
	1019	+ {
	1020	+ .compatible = "rockchip,rk3562-mipi-csi2",
	1021	+ .data = &rk3562_csi2_match_data,
	1022	+ },
949	1023	{ /* sentinel */ }
950	1024	};
951	1025	MODULE_DEVICE_TABLE(of, csi2_dt_ids);
952	1026
	1027	+static int csi2_attach_hw(struct csi2_dev *csi2)
	1028	+{
	1029	+ struct device_node *np;
	1030	+ struct platform_device *pdev;
	1031	+ struct csi2_hw *hw;
	1032	+ int i = 0;
	1033	+
	1034	+ for (i = 0; i < csi2->match_data->num_hw; i++) {
	1035	+ np = of_parse_phandle(csi2->dev->of_node, "rockchip,hw", i);
	1036	+ if (!np \|\| !of_device_is_available(np)) {
	1037	+ dev_err(csi2->dev, "failed to get csi2 hw node\n");
	1038	+ return -ENODEV;
	1039	+ }
	1040	+
	1041	+ pdev = of_find_device_by_node(np);
	1042	+ of_node_put(np);
	1043	+ if (!pdev) {
	1044	+ dev_err(csi2->dev, "failed to get csi2 hw from node\n");
	1045	+ return -ENODEV;
	1046	+ }
	1047	+
	1048	+ hw = platform_get_drvdata(pdev);
	1049	+ if (!hw) {
	1050	+ dev_err(csi2->dev, "failed attach csi2 hw\n");
	1051	+ return -EINVAL;
	1052	+ }
	1053	+
	1054	+ hw->csi2 = csi2;
	1055	+ csi2->csi2_hw[i] = hw;
	1056	+ }
	1057	+ dev_info(csi2->dev, "attach to csi2 hw node\n");
	1058	+
	1059	+ return 0;
	1060	+}
	1061	+
953	1062	static int csi2_probe(struct platform_device *pdev)
954	1063	{
955	1064	const struct of_device_id *match;
956		- struct device *dev = &pdev->dev;
957	1065	struct device_node *node = pdev->dev.of_node;
958	1066	struct csi2_dev *csi2 = NULL;
959		- struct resource *res;
960	1067	const struct csi2_match_data *data;
961		- int ret, irq;
	1068	+ int ret;
962	1069
963	1070	match = of_match_node(csi2_dt_ids, node);
964	1071	if (IS_ERR(match))
..	..	@@ -972,6 +1079,7 @@
972	1079	csi2->dev = &pdev->dev;
973	1080	csi2->match_data = data;
974	1081
	1082	+ csi2->dev_name = node->name;
975	1083	v4l2_subdev_init(&csi2->sd, &csi2_subdev_ops);
976	1084	v4l2_set_subdevdata(&csi2->sd, &pdev->dev);
977	1085	csi2->sd.entity.ops = &csi2_entity_ops;
..	..	@@ -983,59 +1091,11 @@
983	1091	v4l2_err(&csi2->sd, "failed to copy name\n");
984	1092	platform_set_drvdata(pdev, &csi2->sd);
985	1093
986		- csi2->clks_num = devm_clk_bulk_get_all(dev, &csi2->clks_bulk);
987		- if (csi2->clks_num < 0)
988		- dev_err(dev, "failed to get csi2 clks\n");
989		-
990		- csi2->rsts_bulk = devm_reset_control_array_get_optional_exclusive(dev);
991		- if (IS_ERR(csi2->rsts_bulk)) {
992		- if (PTR_ERR(csi2->rsts_bulk) != -EPROBE_DEFER)
993		- dev_err(dev, "failed to get csi2 reset\n");
994		- return PTR_ERR(csi2->rsts_bulk);
	1094	+ ret = csi2_attach_hw(csi2);
	1095	+ if (ret) {
	1096	+ v4l2_err(&csi2->sd, "must enable all mipi csi2 hw node\n");
	1097	+ return -EINVAL;
995	1098	}
996		-
997		- res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
998		- csi2->base = devm_ioremap_resource(&pdev->dev, res);
999		- if (IS_ERR(csi2->base)) {
1000		- resource_size_t offset = res->start;
1001		- resource_size_t size = resource_size(res);
1002		-
1003		- dev_warn(&pdev->dev, "avoid secondary mipi resource check!\n");
1004		-
1005		- csi2->base = devm_ioremap(&pdev->dev, offset, size);
1006		- if (IS_ERR(csi2->base)) {
1007		- dev_err(&pdev->dev, "Failed to ioremap resource\n");
1008		-
1009		- return PTR_ERR(csi2->base);
1010		- }
1011		- }
1012		-
1013		- irq = platform_get_irq_byname(pdev, "csi-intr1");
1014		- if (irq > 0) {
1015		- ret = devm_request_irq(&pdev->dev, irq,
1016		- rk_csirx_irq1_handler, 0,
1017		- dev_driver_string(&pdev->dev),
1018		- &pdev->dev);
1019		- if (ret < 0)
1020		- v4l2_err(&csi2->sd, "request csi-intr1 irq failed: %d\n",
1021		- ret);
1022		- } else {
1023		- v4l2_err(&csi2->sd, "No found irq csi-intr1\n");
1024		- }
1025		-
1026		- irq = platform_get_irq_byname(pdev, "csi-intr2");
1027		- if (irq > 0) {
1028		- ret = devm_request_irq(&pdev->dev, irq,
1029		- rk_csirx_irq2_handler, 0,
1030		- dev_driver_string(&pdev->dev),
1031		- &pdev->dev);
1032		- if (ret < 0)
1033		- v4l2_err(&csi2->sd, "request csi-intr2 failed: %d\n",
1034		- ret);
1035		- } else {
1036		- v4l2_err(&csi2->sd, "No found irq csi-intr2\n");
1037		- }
1038		-
1039	1099	mutex_init(&csi2->lock);
1040	1100
1041	1101	ret = csi2_media_init(&csi2->sd);
..	..	@@ -1044,10 +1104,6 @@
1044	1104	ret = csi2_notifier(csi2);
1045	1105	if (ret)
1046	1106	goto rmmutex;
1047		-
1048		- csi2_hw_do_reset(csi2);
1049		-
1050		- g_csi2_dev = csi2;
1051	1107
1052	1108	v4l2_info(&csi2->sd, "probe success, v4l2_dev:%s!\n", csi2->sd.v4l2_dev->name);
1053	1109
..	..	@@ -1079,16 +1135,188 @@
1079	1135	.remove = csi2_remove,
1080	1136	};
1081	1137
1082		-int __init rkcif_csi2_plat_drv_init(void)
	1138	+int rkcif_csi2_plat_drv_init(void)
1083	1139	{
1084	1140	return platform_driver_register(&csi2_driver);
1085	1141	}
1086	1142
1087		-void __exit rkcif_csi2_plat_drv_exit(void)
	1143	+void rkcif_csi2_plat_drv_exit(void)
1088	1144	{
1089	1145	platform_driver_unregister(&csi2_driver);
1090	1146	}
1091	1147
	1148	+static const struct csi2_hw_match_data rk1808_csi2_hw_match_data = {
	1149	+ .chip_id = CHIP_RK1808_CSI2,
	1150	+};
	1151	+
	1152	+static const struct csi2_hw_match_data rk3288_csi2_hw_match_data = {
	1153	+ .chip_id = CHIP_RK3288_CSI2,
	1154	+};
	1155	+
	1156	+static const struct csi2_hw_match_data rv1126_csi2_hw_match_data = {
	1157	+ .chip_id = CHIP_RV1126_CSI2,
	1158	+};
	1159	+
	1160	+static const struct csi2_hw_match_data rk3568_csi2_hw_match_data = {
	1161	+ .chip_id = CHIP_RK3568_CSI2,
	1162	+};
	1163	+
	1164	+static const struct csi2_hw_match_data rk3588_csi2_hw_match_data = {
	1165	+ .chip_id = CHIP_RK3588_CSI2,
	1166	+};
	1167	+
	1168	+static const struct csi2_hw_match_data rv1106_csi2_hw_match_data = {
	1169	+ .chip_id = CHIP_RV1106_CSI2,
	1170	+};
	1171	+
	1172	+static const struct csi2_hw_match_data rk3562_csi2_hw_match_data = {
	1173	+ .chip_id = CHIP_RK3562_CSI2,
	1174	+};
	1175	+
	1176	+static const struct of_device_id csi2_hw_ids[] = {
	1177	+ {
	1178	+ .compatible = "rockchip,rk1808-mipi-csi2-hw",
	1179	+ .data = &rk1808_csi2_hw_match_data,
	1180	+ },
	1181	+ {
	1182	+ .compatible = "rockchip,rk3288-mipi-csi2-hw",
	1183	+ .data = &rk3288_csi2_hw_match_data,
	1184	+ },
	1185	+ {
	1186	+ .compatible = "rockchip,rk3568-mipi-csi2-hw",
	1187	+ .data = &rk3568_csi2_hw_match_data,
	1188	+ },
	1189	+ {
	1190	+ .compatible = "rockchip,rv1126-mipi-csi2-hw",
	1191	+ .data = &rv1126_csi2_hw_match_data,
	1192	+ },
	1193	+ {
	1194	+ .compatible = "rockchip,rk3588-mipi-csi2-hw",
	1195	+ .data = &rk3588_csi2_hw_match_data,
	1196	+ },
	1197	+ {
	1198	+ .compatible = "rockchip,rv1106-mipi-csi2-hw",
	1199	+ .data = &rv1106_csi2_hw_match_data,
	1200	+ },
	1201	+ {
	1202	+ .compatible = "rockchip,rk3562-mipi-csi2-hw",
	1203	+ .data = &rk3588_csi2_hw_match_data,
	1204	+ },
	1205	+ { /* sentinel */ }
	1206	+};
	1207	+MODULE_DEVICE_TABLE(of, csi2_hw_ids);
	1208	+
	1209	+static int csi2_hw_probe(struct platform_device *pdev)
	1210	+{
	1211	+ const struct of_device_id *match;
	1212	+ struct device *dev = &pdev->dev;
	1213	+ struct device_node *node = pdev->dev.of_node;
	1214	+ struct csi2_hw *csi2_hw = NULL;
	1215	+ struct resource *res;
	1216	+ const struct csi2_hw_match_data *data;
	1217	+ int ret, irq;
	1218	+
	1219	+ dev_info(&pdev->dev, "enter mipi csi2 hw probe!\n");
	1220	+ match = of_match_node(csi2_hw_ids, node);
	1221	+ if (IS_ERR(match))
	1222	+ return PTR_ERR(match);
	1223	+ data = match->data;
	1224	+
	1225	+ csi2_hw = devm_kzalloc(&pdev->dev, sizeof(*csi2_hw), GFP_KERNEL);
	1226	+ if (!csi2_hw)
	1227	+ return -ENOMEM;
	1228	+
	1229	+ csi2_hw->dev = &pdev->dev;
	1230	+ csi2_hw->match_data = data;
	1231	+
	1232	+ csi2_hw->dev_name = node->name;
	1233	+
	1234	+ csi2_hw->clks_num = devm_clk_bulk_get_all(dev, &csi2_hw->clks_bulk);
	1235	+ if (csi2_hw->clks_num < 0) {
	1236	+ csi2_hw->clks_num = 0;
	1237	+ dev_err(dev, "failed to get csi2 clks\n");
	1238	+ }
	1239	+
	1240	+ csi2_hw->rsts_bulk = devm_reset_control_array_get_optional_exclusive(dev);
	1241	+ if (IS_ERR(csi2_hw->rsts_bulk)) {
	1242	+ if (PTR_ERR(csi2_hw->rsts_bulk) != -EPROBE_DEFER)
	1243	+ dev_err(dev, "failed to get csi2 reset\n");
	1244	+ csi2_hw->rsts_bulk = NULL;
	1245	+ }
	1246	+
	1247	+ res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
	1248	+ csi2_hw->base = devm_ioremap_resource(&pdev->dev, res);
	1249	+ if (IS_ERR(csi2_hw->base)) {
	1250	+ resource_size_t offset = res->start;
	1251	+ resource_size_t size = resource_size(res);
	1252	+
	1253	+ dev_warn(&pdev->dev, "avoid secondary mipi resource check!\n");
	1254	+
	1255	+ csi2_hw->base = devm_ioremap(&pdev->dev, offset, size);
	1256	+ if (IS_ERR(csi2_hw->base)) {
	1257	+ dev_err(&pdev->dev, "Failed to ioremap resource\n");
	1258	+
	1259	+ return PTR_ERR(csi2_hw->base);
	1260	+ }
	1261	+ }
	1262	+
	1263	+ irq = platform_get_irq_byname(pdev, "csi-intr1");
	1264	+ if (irq > 0) {
	1265	+ ret = devm_request_irq(&pdev->dev, irq,
	1266	+ rk_csirx_irq1_handler, 0,
	1267	+ dev_driver_string(&pdev->dev),
	1268	+ &pdev->dev);
	1269	+ if (ret < 0)
	1270	+ dev_err(&pdev->dev, "request csi-intr1 irq failed: %d\n",
	1271	+ ret);
	1272	+ csi2_hw->irq1 = irq;
	1273	+ } else {
	1274	+ dev_err(&pdev->dev, "No found irq csi-intr1\n");
	1275	+ }
	1276	+
	1277	+ irq = platform_get_irq_byname(pdev, "csi-intr2");
	1278	+ if (irq > 0) {
	1279	+ ret = devm_request_irq(&pdev->dev, irq,
	1280	+ rk_csirx_irq2_handler, 0,
	1281	+ dev_driver_string(&pdev->dev),
	1282	+ &pdev->dev);
	1283	+ if (ret < 0)
	1284	+ dev_err(&pdev->dev, "request csi-intr2 failed: %d\n",
	1285	+ ret);
	1286	+ csi2_hw->irq2 = irq;
	1287	+ } else {
	1288	+ dev_err(&pdev->dev, "No found irq csi-intr2\n");
	1289	+ }
	1290	+ platform_set_drvdata(pdev, csi2_hw);
	1291	+ dev_info(&pdev->dev, "probe success, v4l2_dev:%s!\n", csi2_hw->dev_name);
	1292	+
	1293	+ return 0;
	1294	+}
	1295	+
	1296	+static int csi2_hw_remove(struct platform_device *pdev)
	1297	+{
	1298	+ return 0;
	1299	+}
	1300	+
	1301	+static struct platform_driver csi2_hw_driver = {
	1302	+ .driver = {
	1303	+ .name = DEVICE_NAME_HW,
	1304	+ .of_match_table = csi2_hw_ids,
	1305	+ },
	1306	+ .probe = csi2_hw_probe,
	1307	+ .remove = csi2_hw_remove,
	1308	+};
	1309	+
	1310	+int rkcif_csi2_hw_plat_drv_init(void)
	1311	+{
	1312	+ return platform_driver_register(&csi2_hw_driver);
	1313	+}
	1314	+
	1315	+void rkcif_csi2_hw_plat_drv_exit(void)
	1316	+{
	1317	+ platform_driver_unregister(&csi2_hw_driver);
	1318	+}
	1319	+
1092	1320	MODULE_DESCRIPTION("Rockchip MIPI CSI2 driver");
1093	1321	MODULE_AUTHOR("Macrofly.xu <xuhf@rock-chips.com>");
1094	1322	MODULE_LICENSE("GPL");