forked from ~ljy/RK356X_SDK_RELEASE

blame | history | patch | commit | commitdiff
hc
2023-12-11 6778948f9de86c3cfaf36725a7c87dcff9ba247f 
 kernel/drivers/media/i2c/max96722.c
....@@ -2,17 +2,22 @@
22 /*
33  * max96722 GMSL2/GMSL1 to CSI-2 Deserializer driver
44  *
5
- * Copyright (C) 2022 Rockchip Electronics Co., Ltd.
5
+ * Copyright (C) 2023 Rockchip Electronics Co., Ltd.
66  *
77  * V0.0X01.0X00 first version.
8
+ * V1.0X00.0X00 Support New Driver Framework.
9
+ * V1.0X01.0X00 serdes read /write api depend on i2c id index.
810  *
911  */
1012 
1113 #include <linux/clk.h>
1214 #include <linux/device.h>
1315 #include <linux/delay.h>
16
+#include <linux/iopoll.h>
1417 #include <linux/gpio/consumer.h>
18
+#include <linux/pinctrl/consumer.h>
1519 #include <linux/i2c.h>
20
+#include <linux/regmap.h>
1621 #include <linux/module.h>
1722 #include <linux/pm_runtime.h>
1823 #include <linux/regulator/consumer.h>
....@@ -21,59 +26,94 @@
2126 #include <linux/version.h>
2227 #include <linux/compat.h>
2328 #include <linux/rk-camera-module.h>
29
+#include <linux/of_graph.h>
2430 #include <media/media-entity.h>
2531 #include <media/v4l2-async.h>
2632 #include <media/v4l2-ctrls.h>
2733 #include <media/v4l2-subdev.h>
28
-#include <linux/pinctrl/consumer.h>
34
+#include <media/v4l2-ctrls.h>
35
+#include <media/v4l2-fwnode.h>
36
+#include <media/v4l2-subdev.h>
2937 
30
-#define DRIVER_VERSION			KERNEL_VERSION(0, 0x01, 0x00)
38
+#define DRIVER_VERSION			KERNEL_VERSION(1, 0x01, 0x00)
3139 
3240 #ifndef V4L2_CID_DIGITAL_GAIN
3341 #define V4L2_CID_DIGITAL_GAIN		V4L2_CID_GAIN
3442 #endif
3543 
36
-#define MAX96722_LINK_FREQ_400MHZ	400000000UL
37
-/* pixel rate = link frequency * 2 * lanes / BITS_PER_SAMPLE */
38
-#define MAX96722_PIXEL_RATE		(MAX96722_LINK_FREQ_400MHZ * 2LL * 4LL / 24LL)
39
-#define MAX96722_XVCLK_FREQ		24000000
44
+#define MAX96722_LINK_FREQ_MHZ(x)	((x) * 1000000UL)
45
+#define MAX96722_XVCLK_FREQ		25000000
4046 
41
-#define CHIP_ID				0xA1
47
+#define MAX96722_CHIP_ID		0xA1
4248 #define MAX96722_REG_CHIP_ID		0x0D
4349 
44
-#define MAX96722_REG_CTRL_MODE		0x08a0
45
-#define MAX96722_MODE_SW_STANDBY	0x4
46
-#define MAX96722_MODE_STREAMING		0xa4
50
+#define MAX96715_CHIP_ID		0x45
51
+#define MAX96715_REG_CHIP_ID		0x1E
4752 
48
-#define MAX96722_REMOTE_CTRL		0x0003
49
-#define MAX96722_REMOTE_DISABLE		0xFF
53
+#define MAX9295_CHIP_ID			0x91
54
+#define MAX9295_REG_CHIP_ID		0x0D
5055 
51
-#define REG_NULL			0xFFFF
56
+#define MAX96717_CHIP_ID		0xBF
57
+#define MAX96717_REG_CHIP_ID		0x0D
5258 
53
-#define MAX96722_LANES			4
59
+/* max96722->link mask: link type = bit[7:4], link mask = bit[3:0] */
60
+#define MAXIM_GMSL_TYPE_LINK_A		BIT(4)
61
+#define MAXIM_GMSL_TYPE_LINK_B		BIT(5)
62
+#define MAXIM_GMSL_TYPE_LINK_C		BIT(6)
63
+#define MAXIM_GMSL_TYPE_LINK_D		BIT(7)
64
+#define MAXIM_GMSL_TYPE_MASK		0xF0 /* bit[7:4], GMSL link type: 0 = GMSL1, 1 = GMSL2 */
65
+
66
+#define MAXIM_GMSL_LOCK_LINK_A		BIT(0)
67
+#define MAXIM_GMSL_LOCK_LINK_B		BIT(1)
68
+#define MAXIM_GMSL_LOCK_LINK_C		BIT(2)
69
+#define MAXIM_GMSL_LOCK_LINK_D		BIT(3)
70
+#define MAXIM_GMSL_LOCK_MASK		0x0F /* bit[3:0], GMSL link mask: 1 = disable, 1 = enable */
71
+
72
+#define MAXIM_FORCE_ALL_CLOCK_EN	1 /* 1: enable, 0: disable */
5473 
5574 #define OF_CAMERA_PINCTRL_STATE_DEFAULT	"rockchip,camera_default"
5675 #define OF_CAMERA_PINCTRL_STATE_SLEEP	"rockchip,camera_sleep"
5776 
58
-#define MAX96722_REG_VALUE_08BIT	1
59
-#define MAX96722_REG_VALUE_16BIT	2
60
-#define MAX96722_REG_VALUE_24BIT	3
61
-
6277 #define MAX96722_NAME			"max96722"
63
-#define MAX96722_MEDIA_BUS_FMT		MEDIA_BUS_FMT_RGB888_1X24
6478 
65
-static const char * const max96722_supply_names[] = {
66
-	"avdd",		/* Analog power */
67
-	"dovdd",	/* Digital I/O power */
68
-	"dvdd",		/* Digital core power */
79
+#define REG_NULL			0xFFFF
80
+
81
+/* register length: 8bit or 16bit */
82
+#define DEV_REG_LENGTH_08BITS		1
83
+#define DEV_REG_LENGTH_16BITS		2
84
+
85
+/* register value: 8bit or 16bit or 24bit */
86
+#define DEV_REG_VALUE_08BITS		1
87
+#define DEV_REG_VALUE_16BITS		2
88
+#define DEV_REG_VALUE_24BITS		3
89
+
90
+/* i2c device default address */
91
+#define SER_I2C_ADDR			(0x40)
92
+#define CAM_I2C_ADDR			(0x30)
93
+
94
+/* Maxim Serdes I2C Device ID */
95
+enum {
96
+	I2C_DEV_DES = 0,
97
+	I2C_DEV_SER,
98
+	I2C_DEV_CAM,
99
+	I2C_DEV_MAX
69100 };
70101 
71
-#define MAX96722_NUM_SUPPLIES ARRAY_SIZE(max96722_supply_names)
102
+
103
+static const char *const max96722_supply_names[] = {
104
+	"avdd", /* Analog power */
105
+	"dovdd", /* Digital I/O power */
106
+	"dvdd", /* Digital core power */
107
+};
108
+
109
+#define MAX96722_NUM_SUPPLIES		ARRAY_SIZE(max96722_supply_names)
72110 
73111 struct regval {
74
-	u16 i2c_addr;
75
-	u16 addr;
112
+	u16 i2c_id;
113
+	u16 reg_len;
114
+	u16 reg;
76115 	u8 val;
116
+	u8 mask;
77117 	u16 delay;
78118 };
79119 
....@@ -85,227 +125,935 @@
85125 	u32 vts_def;
86126 	u32 exp_def;
87127 	u32 link_freq_idx;
128
+	u32 bus_fmt;
88129 	u32 bpp;
89130 	const struct regval *reg_list;
131
+	u32 vc[PAD_MAX];
90132 };
91133 
92134 struct max96722 {
93
-	struct i2c_client	*client;
94
-	struct clk		*xvclk;
95
-	struct gpio_desc	*power_gpio;
96
-	struct gpio_desc	*reset_gpio;
97
-	struct gpio_desc	*pwdn_gpio;
135
+	struct i2c_client *client;
136
+	u16 i2c_addr[I2C_DEV_MAX];
137
+	struct clk *xvclk;
138
+	struct gpio_desc *power_gpio;
139
+	struct gpio_desc *reset_gpio;
140
+	struct gpio_desc *pwdn_gpio;
141
+	struct gpio_desc *pocen_gpio;
142
+	struct gpio_desc *lock_gpio;
98143 	struct regulator_bulk_data supplies[MAX96722_NUM_SUPPLIES];
99144 
100
-	struct pinctrl		*pinctrl;
101
-	struct pinctrl_state	*pins_default;
102
-	struct pinctrl_state	*pins_sleep;
145
+	struct pinctrl *pinctrl;
146
+	struct pinctrl_state *pins_default;
147
+	struct pinctrl_state *pins_sleep;
103148 
104
-	struct v4l2_subdev	subdev;
105
-	struct media_pad	pad;
149
+	struct v4l2_subdev subdev;
150
+	struct media_pad pad;
106151 	struct v4l2_ctrl_handler ctrl_handler;
107
-	struct v4l2_ctrl	*exposure;
108
-	struct v4l2_ctrl	*anal_gain;
109
-	struct v4l2_ctrl	*digi_gain;
110
-	struct v4l2_ctrl	*hblank;
111
-	struct v4l2_ctrl	*vblank;
112
-	struct v4l2_ctrl	*pixel_rate;
113
-	struct v4l2_ctrl	*link_freq;
114
-	struct v4l2_ctrl	*test_pattern;
115
-	struct mutex		mutex;
116
-	bool			streaming;
117
-	bool			power_on;
118
-	bool			hot_plug;
119
-	u8			is_reset;
152
+	struct v4l2_ctrl *exposure;
153
+	struct v4l2_ctrl *anal_gain;
154
+	struct v4l2_ctrl *digi_gain;
155
+	struct v4l2_ctrl *hblank;
156
+	struct v4l2_ctrl *vblank;
157
+	struct v4l2_ctrl *pixel_rate;
158
+	struct v4l2_ctrl *link_freq;
159
+	struct v4l2_ctrl *test_pattern;
160
+	struct v4l2_fwnode_endpoint bus_cfg;
161
+
162
+	struct mutex mutex;
163
+	bool streaming;
164
+	bool power_on;
165
+	bool hot_plug;
166
+	u8 is_reset;
167
+	int hot_plug_irq;
168
+	u32 link_mask;
169
+	const struct max96722_mode *supported_modes;
120170 	const struct max96722_mode *cur_mode;
121
-	u32			module_index;
122
-	const char		*module_facing;
123
-	const char		*module_name;
124
-	const char		*len_name;
171
+	u32 cfg_modes_num;
172
+	u32 module_index;
173
+	u32 auto_init_deskew_mask;
174
+	u32 frame_sync_period;
175
+	const char *module_facing;
176
+	const char *module_name;
177
+	const char *len_name;
178
+	struct regmap *regmap;
125179 };
126180 
127
-#define to_max96722(sd) container_of(sd, struct max96722, subdev)
181
+static const struct regmap_config max96722_regmap_config = {
182
+	.reg_bits = 16,
183
+	.val_bits = 8,
184
+	.max_register = 0x1F17,
185
+};
128186 
129
-static const struct regval max96722_mipi_init[] = {
130
-	{0x6b, 0x0006, 0xF0, 0x00},
131
-	// Disable MIPI output
132
-	{0x6b, 0x040B, 0x00, 0x00},
133
-	// RGB888 software override for all pipes since connected GMSL1 is under parallel mode
134
-	{0x6b, 0x040B, 0xC0, 0x00},	//0b11000-000, bpp0=0x18
135
-	{0x6b, 0x040E, 0xA4, 0x00},	//0b10-100100, DT0=0x24
136
-	{0x6b, 0x040F, 0x04, 0x00},	//0b0000-0100, DT1=0x24
137
-	{0x6b, 0x0411, 0x18, 0x00},	//0b000-11000, bpp1=0x18
138
-	//Video pipe sel
139
-	{0x6b, 0x00F0, 0x40, 0x00},	//LINKA-pipex=pipe0, LINKB-pipex=pipe1
140
-	// Send RGB888, FS, and FE from Pipe 0 to Controller 1
141
-	{0x6b, 0x090B, 0x07, 0x00}, // Enable 3 Mappings
142
-	{0x6b, 0x092D, 0x15, 0x00},	//Map Data to Port A
143
-	// For the following MSB 2 bits = VC, LSB 6 bits =DT
144
-	{0x6b, 0x090D, 0x24, 0x00}, // SRC DT = RGB888
145
-	{0x6b, 0x090E, 0x24, 0x00}, // DEST DT = RGB888
146
-	{0x6b, 0x090F, 0x00, 0x00}, // SRC DT = Frame Start
147
-	{0x6b, 0x0910, 0x00, 0x00}, // DEST DT = Frame Start
148
-	{0x6b, 0x0911, 0x01, 0x00}, // SRC DT = Frame End
149
-	{0x6b, 0x0912, 0x01, 0x00}, // DEST DT = Frame End
150
-	//Send RGB888, FS, and FE from Pipe 1 to Controller 2
151
-	{0x6b, 0x094B, 0x07, 0x00},
152
-	{0x6b, 0x096D, 0xAA, 0x00}, // map to MIPI Controller 2
153
-	// For the following MSB 2 bits = VC, LSB 6 bits =DT
154
-	{0x6b, 0x094D, 0x24, 0x00},
155
-	{0x6b, 0x094E, 0x24, 0x00}, // map to VC0
156
-	{0x6b, 0x094F, 0x00, 0x00}, // frame start
157
-	{0x6b, 0x0950, 0x00, 0x00},
158
-	{0x6b, 0x0951, 0x01, 0x00},
159
-	{0x6b, 0x0952, 0x01, 0x00},
187
+static struct rkmodule_csi_dphy_param rk3588_dcphy_param = {
188
+	.vendor = PHY_VENDOR_SAMSUNG,
189
+	.lp_vol_ref = 3,
190
+	.lp_hys_sw = {3, 0, 0, 0},
191
+	.lp_escclk_pol_sel = {1, 0, 0, 0},
192
+	.skew_data_cal_clk = {0, 0, 0, 0},
193
+	.clk_hs_term_sel = 2,
194
+	.data_hs_term_sel = {2, 2, 2, 2},
195
+	.reserved = {0},
196
+};
197
+
198
+/* Max96715 */
199
+static const struct regval max96722_mipi_4lane_1280x800_30fps[] = {
200
+	// Link A/B/C/D all use GMSL1, and disabled
201
+	{ I2C_DEV_DES, 2, 0x0006, 0x00, 0x00, 0x00 }, // Link A/B/C/D: select GMSL1, Disabled
202
+	// Disable MIPI CSI output
203
+	{ I2C_DEV_DES, 2, 0x040B, 0x00, 0x00, 0x00 }, // CSI_OUT_EN=0, CSI output disabled
204
+	// Increase CMU voltage
205
+	{ I2C_DEV_DES, 2, 0x06C2, 0x10, 0x00, 0x0a }, // Increase CMU voltage to for wide temperature range
206
+	// VGAHiGain
207
+	{ I2C_DEV_DES, 2, 0x14D1, 0x03, 0x00, 0x00 }, // VGAHiGain
208
+	{ I2C_DEV_DES, 2, 0x15D1, 0x03, 0x00, 0x00 }, // VGAHiGain
209
+	{ I2C_DEV_DES, 2, 0x16D1, 0x03, 0x00, 0x00 }, // VGAHiGain
210
+	{ I2C_DEV_DES, 2, 0x17D1, 0x03, 0x00, 0x0a }, // VGAHiGain
211
+	// SSC Configuration
212
+	{ I2C_DEV_DES, 2, 0x1445, 0x00, 0x00, 0x00 }, // Disable SSC
213
+	{ I2C_DEV_DES, 2, 0x1545, 0x00, 0x00, 0x00 }, // Disable SSC
214
+	{ I2C_DEV_DES, 2, 0x1645, 0x00, 0x00, 0x00 }, // Disable SSC
215
+	{ I2C_DEV_DES, 2, 0x1745, 0x00, 0x00, 0x0a }, // Disable SSC
216
+	// GMSL1 configuration to match serializer
217
+	{ I2C_DEV_DES, 2, 0x0B07, 0x84, 0x00, 0x00 }, // Enable HVEN and DBL (application specific)
218
+	{ I2C_DEV_DES, 2, 0x0C07, 0x84, 0x00, 0x00 }, // Enable HVEN and DBL (application specific)
219
+	{ I2C_DEV_DES, 2, 0x0D07, 0x84, 0x00, 0x00 }, // Enable HVEN and DBL (application specific)
220
+	{ I2C_DEV_DES, 2, 0x0E07, 0x84, 0x00, 0x00 }, // Enable HVEN and DBL (application specific)
221
+	{ I2C_DEV_DES, 2, 0x0B0F, 0x01, 0x00, 0x00 }, // Disable processing HS and DE signals(required when paring with GMSL1 parallel serializers)
222
+	{ I2C_DEV_DES, 2, 0x0C0F, 0x01, 0x00, 0x00 }, // Disable processing HS and DE signals(required when paring with GMSL1 parallel serializers)
223
+	{ I2C_DEV_DES, 2, 0x0D0F, 0x01, 0x00, 0x00 }, // Disable processing HS and DE signals(required when paring with GMSL1 parallel serializers)
224
+	{ I2C_DEV_DES, 2, 0x0E0F, 0x01, 0x00, 0x00 }, // Disable processing HS and DE signals(required when paring with GMSL1 parallel serializers)
225
+	// Send YUV422, FS, and FE from Video Pipe 0 to Controller 1
226
+	{ I2C_DEV_DES, 2, 0x090B, 0x07, 0x00, 0x00 }, // Enable 0/1/2 SRC/DST Mappings
227
+	{ I2C_DEV_DES, 2, 0x092D, 0x15, 0x00, 0x00 }, // SRC/DST 0/1/2 -> CSI2 Controller 1;
228
+	// For the following MSB 2 bits = VC, LSB 6 bits = DT
229
+	{ I2C_DEV_DES, 2, 0x090D, 0x1e, 0x00, 0x00 }, // SRC0 VC = 0, DT = YUV422 8bit
230
+	{ I2C_DEV_DES, 2, 0x090E, 0x1e, 0x00, 0x00 }, // DST0 VC = 0, DT = YUV422 8bit
231
+	{ I2C_DEV_DES, 2, 0x090F, 0x00, 0x00, 0x00 }, // SRC1 VC = 0, DT = Frame Start
232
+	{ I2C_DEV_DES, 2, 0x0910, 0x00, 0x00, 0x00 }, // DST1 VC = 0, DT = Frame Start
233
+	{ I2C_DEV_DES, 2, 0x0911, 0x01, 0x00, 0x00 }, // SRC2 VC = 0, DT = Frame End
234
+	{ I2C_DEV_DES, 2, 0x0912, 0x01, 0x00, 0x00 }, // DST2 VC = 0, DT = Frame End
235
+	// Send YUV422, FS, and FE from Video Pipe 1 to Controller 1
236
+	{ I2C_DEV_DES, 2, 0x094B, 0x07, 0x00, 0x00 }, // Enable 0/1/2 SRC/DST Mappings
237
+	{ I2C_DEV_DES, 2, 0x096D, 0x15, 0x00, 0x00 }, // SRC/DST 0/1/2 -> CSI2 Controller 1;
238
+	// For the following MSB 2 bits = VC, LSB 6 bits = DT
239
+	{ I2C_DEV_DES, 2, 0x094D, 0x1e, 0x00, 0x00 }, // SRC0 VC = 0, DT = YUV422 8bit
240
+	{ I2C_DEV_DES, 2, 0x094E, 0x5e, 0x00, 0x00 }, // DST0 VC = 1, DT = YUV422 8bit
241
+	{ I2C_DEV_DES, 2, 0x094F, 0x00, 0x00, 0x00 }, // SRC1 VC = 0, DT = Frame Start
242
+	{ I2C_DEV_DES, 2, 0x0950, 0x40, 0x00, 0x00 }, // DST1 VC = 1, DT = Frame Start
243
+	{ I2C_DEV_DES, 2, 0x0951, 0x01, 0x00, 0x00 }, // SRC2 VC = 0, DT = Frame End
244
+	{ I2C_DEV_DES, 2, 0x0952, 0x41, 0x00, 0x00 }, // DST2 VC = 1, DT = Frame End
245
+	// Send YUV422, FS, and FE from Video Pipe 2 to Controller 1
246
+	{ I2C_DEV_DES, 2, 0x098B, 0x07, 0x00, 0x00 }, // Enable 0/1/2 SRC/DST Mappings
247
+	{ I2C_DEV_DES, 2, 0x09AD, 0x15, 0x00, 0x00 }, // SRC/DST 0/1/2 -> CSI2 Controller 1;
248
+	// For the following MSB 2 bits = VC, LSB 6 bits = DT
249
+	{ I2C_DEV_DES, 2, 0x098D, 0x1e, 0x00, 0x00 }, // SRC0 VC = 0, DT = YUV422 8bit
250
+	{ I2C_DEV_DES, 2, 0x098E, 0x9e, 0x00, 0x00 }, // DST0 VC = 2, DT = YUV422 8bit
251
+	{ I2C_DEV_DES, 2, 0x098F, 0x00, 0x00, 0x00 }, // SRC1 VC = 0, DT = Frame Start
252
+	{ I2C_DEV_DES, 2, 0x0990, 0x80, 0x00, 0x00 }, // DST1 VC = 2, DT = Frame Start
253
+	{ I2C_DEV_DES, 2, 0x0991, 0x01, 0x00, 0x00 }, // SRC2 VC = 0, DT = Frame End
254
+	{ I2C_DEV_DES, 2, 0x0992, 0x81, 0x00, 0x00 }, // DST2 VC = 2, DT = Frame End
255
+	// Send YUV422, FS, and FE from Video Pipe 3 to Controller 1
256
+	{ I2C_DEV_DES, 2, 0x09CB, 0x07, 0x00, 0x00 }, // Enable 0/1/2 SRC/DST Mappings
257
+	{ I2C_DEV_DES, 2, 0x09ED, 0x15, 0x00, 0x00 }, // SRC/DST 0/1/2 -> CSI2 Controller 1;
258
+	// For the following MSB 2 bits = VC, LSB 6 bits = DT
259
+	{ I2C_DEV_DES, 2, 0x09CD, 0x1e, 0x00, 0x00 }, // SRC0 VC = 0, DT = YUV422 8bit
260
+	{ I2C_DEV_DES, 2, 0x09CE, 0xde, 0x00, 0x00 }, // DST0 VC = 3, DT = YUV422 8bit
261
+	{ I2C_DEV_DES, 2, 0x09CF, 0x00, 0x00, 0x00 }, // SRC1 VC = 0, DT = Frame Start
262
+	{ I2C_DEV_DES, 2, 0x09D0, 0xc0, 0x00, 0x00 }, // DST1 VC = 3, DT = Frame Start
263
+	{ I2C_DEV_DES, 2, 0x09D1, 0x01, 0x00, 0x00 }, // SRC2 VC = 0, DT = Frame End
264
+	{ I2C_DEV_DES, 2, 0x09D2, 0xc1, 0x00, 0x00 }, // DST2 VC = 3, DT = Frame End
160265 	// MIPI PHY Setting
161
-	// Set Des in 2x4 mode
162
-	{0x6b, 0x08A0, 0x04, 0x00},
266
+	{ I2C_DEV_DES, 2, 0x08A0, 0x24, 0x00, 0x00 }, // DPHY0 enabled as clock, MIPI PHY Mode: 2x4 mode
163267 	// Set Lane Mapping for 4-lane port A
164
-	{0x6b, 0x08A3, 0xE4, 0x00},
165
-	{0x6b, 0x08A4, 0xE4, 0x00},
166
-	// Set 4 lane D-PHY
167
-	{0x6b, 0x090A, 0xC0, 0x00},
168
-	{0x6b, 0x094A, 0xC0, 0x00},
169
-	{0x6b, 0x098A, 0xC0, 0x00},
170
-	{0x6b, 0x09CA, 0xC0, 0x00},
268
+	{ I2C_DEV_DES, 2, 0x08A3, 0xe4, 0x00, 0x00 }, // PHY1 D1->D3, D0->D2; PHY0 D1->D1, D0->D0
269
+	// Set 4 lane D-PHY, 2bit VC
270
+	{ I2C_DEV_DES, 2, 0x090A, 0xc0, 0x00, 0x00 }, // MIPI PHY 0: 4 lanes, DPHY, 2bit VC
271
+	{ I2C_DEV_DES, 2, 0x094A, 0xc0, 0x00, 0x00 }, // MIPI PHY 1: 4 lanes, DPHY, 2bit VC
171272 	// Turn on MIPI PHYs
172
-	{0x6b, 0x08A2, 0xF0, 0x00},
173
-	// Hold DPLL in reset (config_soft_rst_n = 0) before changing the rate
174
-	{0x6b, 0x1C00, 0xF4, 0x00},
175
-	{0x6b, 0x1D00, 0xF4, 0x00},
176
-	{0x6b, 0x1E00, 0xF4, 0x00},
177
-	{0x6b, 0x1F00, 0xF4, 0x00},
178
-	// Set Data rate to be 800Mbps/lane for port A and enable software override
179
-	{0x6b, 0x0415, 0xE8, 0x00},	//override pipe0/1
180
-	{0x6b, 0x0418, 0x28, 0x00},
181
-	{0x6b, 0x041B, 0x28, 0x00},
182
-	{0x6b, 0x041E, 0x28, 0x00},
183
-	// Release reset to DPLL (config_soft_rst_n = 1)
184
-	{0x6b, 0x1C00, 0xF5, 0x00},
185
-	{0x6b, 0x1D00, 0xF5, 0x00},
186
-	{0x6b, 0x1E00, 0xF5, 0x00},
187
-	{0x6b, 0x1F00, 0xF5, 0x00},
188
-	{0x6b, 0x0003, 0xFF, 0x00},
189
-	{0x6b, 0x0006, 0xF3, 0x0a},
190
-	// {0x6b, 0x08A0, 0x84},
191
-	{0x6b, REG_NULL, 0x00, 0x00},
273
+	{ I2C_DEV_DES, 2, 0x08A2, 0x34, 0x00, 0x00 }, // Enable MIPI PHY 0/1, t_lpx = 106.7ns
274
+	// Enable software override for all pipes since GMSL1 data is parallel mode, bpp=8, dt=0x1e(yuv-8)
275
+	{ I2C_DEV_DES, 2, 0x040B, 0x40, 0x00, 0x00 }, // pipe 0 bpp=0x08: Datatypes = 0x2A, 0x10-12, 0x31-37
276
+	{ I2C_DEV_DES, 2, 0x040C, 0x00, 0x00, 0x00 }, // pipe 0 and 1 VC software override: 0x00
277
+	{ I2C_DEV_DES, 2, 0x040D, 0x00, 0x00, 0x00 }, // pipe 2 and 3 VC software override: 0x00
278
+	{ I2C_DEV_DES, 2, 0x040E, 0x5e, 0x00, 0x00 }, // pipe 0 DT=0x1E: YUV422 8-bit
279
+	{ I2C_DEV_DES, 2, 0x040F, 0x7e, 0x00, 0x00 }, // pipe 1 DT=0x1E: YUV422 8-bit
280
+	{ I2C_DEV_DES, 2, 0x0410, 0x7a, 0x00, 0x00 }, // pipe 2 DT=0x1E, pipe 3 DT=0x1E: YUV422 8-bit
281
+	{ I2C_DEV_DES, 2, 0x0411, 0x48, 0x00, 0x00 }, // pipe 1 bpp=0x08: Datatypes = 0x2A, 0x10-12, 0x31-37
282
+	{ I2C_DEV_DES, 2, 0x0412, 0x20, 0x00, 0x00 }, // pipe 2 bpp=0x08, pipe 3 bpp=0x08: Datatypes = 0x2A, 0x10-12, 0x31-37
283
+	{ I2C_DEV_DES, 2, 0x0415, 0xc0, 0xc0, 0x00 }, // pipe 0/1 enable software overide
284
+	{ I2C_DEV_DES, 2, 0x0418, 0xc0, 0xc0, 0x00 }, // pipe 2/3 enable software overide
285
+	{ I2C_DEV_DES, 2, 0x041A, 0xf0, 0x00, 0x00 }, // pipe 0/1/2/3: Enable YUV8-/10-bit mux mode
286
+	// Enable all links and pipes
287
+	{ I2C_DEV_DES, 2, 0x0003, 0xaa, 0x00, 0x00 }, // Enable Remote Control Channel Link A/B/C/D for Port 0
288
+	{ I2C_DEV_DES, 2, 0x0006, 0x0f, 0x00, 0x64 }, // Enable all links and pipes
289
+	// Serializer Setting
290
+	{ I2C_DEV_SER, 1, 0x04, 0x47, 0x00, 0x05 }, // main_control: Enable CLINK
291
+	{ I2C_DEV_SER, 1, 0x07, 0x84, 0x00, 0x00 }, // Config SerDes: DBL=1, BWS=0, HIBW=0, PXL_CRC=0, HVEN=1
292
+	{ I2C_DEV_SER, 1, 0x67, 0xc4, 0x00, 0x00 }, // Double Alignment Mode: Align at each rising edge of HS
293
+	{ I2C_DEV_SER, 1, 0x0F, 0xbf, 0x00, 0x00 }, // Enable Set GPO, GPO Output High
294
+	{ I2C_DEV_SER, 1, 0x3F, 0x08, 0x00, 0x00 }, // Crossbar HS: DIN8
295
+	{ I2C_DEV_SER, 1, 0x40, 0x2d, 0x00, 0x00 }, // Crossbar VS: DIN13, INVERT_MUX_VS
296
+	{ I2C_DEV_SER, 1, 0x20, 0x10, 0x00, 0x00 },
297
+	{ I2C_DEV_SER, 1, 0x21, 0x11, 0x00, 0x00 },
298
+	{ I2C_DEV_SER, 1, 0x22, 0x12, 0x00, 0x00 },
299
+	{ I2C_DEV_SER, 1, 0x23, 0x13, 0x00, 0x00 },
300
+	{ I2C_DEV_SER, 1, 0x24, 0x14, 0x00, 0x00 },
301
+	{ I2C_DEV_SER, 1, 0x25, 0x15, 0x00, 0x00 },
302
+	{ I2C_DEV_SER, 1, 0x26, 0x16, 0x00, 0x00 },
303
+	{ I2C_DEV_SER, 1, 0x27, 0x17, 0x00, 0x00 },
304
+	{ I2C_DEV_SER, 1, 0x30, 0x00, 0x00, 0x00 },
305
+	{ I2C_DEV_SER, 1, 0x31, 0x01, 0x00, 0x00 },
306
+	{ I2C_DEV_SER, 1, 0x32, 0x02, 0x00, 0x00 },
307
+	{ I2C_DEV_SER, 1, 0x33, 0x03, 0x00, 0x00 },
308
+	{ I2C_DEV_SER, 1, 0x34, 0x04, 0x00, 0x00 },
309
+	{ I2C_DEV_SER, 1, 0x35, 0x05, 0x00, 0x00 },
310
+	{ I2C_DEV_SER, 1, 0x36, 0x06, 0x00, 0x00 },
311
+	{ I2C_DEV_SER, 1, 0x37, 0x07, 0x00, 0x00 },
312
+	{ I2C_DEV_SER, 1, 0x04, 0x87, 0x00, 0x05 }, // main_control: Enable Serialization
313
+	{ I2C_DEV_DES, 2, REG_NULL, 0x00, 0x00, 0x00 },
192314 };
193315 
194
-static const struct max96722_mode supported_modes[] = {
316
+static const struct max96722_mode supported_modes_4lane[] = {
195317 	{
196
-		.width = 1920,
197
-		.height = 1080,
318
+		.width = 1280,
319
+		.height = 800,
198320 		.max_fps = {
199321 			.numerator = 10000,
200322 			.denominator = 300000,
201323 		},
202
-		.reg_list = max96722_mipi_init,
203
-		.link_freq_idx = 0,
324
+		.reg_list = max96722_mipi_4lane_1280x800_30fps,
325
+		.link_freq_idx = 20,
326
+		.bus_fmt = MEDIA_BUS_FMT_UYVY8_2X8,
327
+		.bpp = 16,
328
+		.vc[PAD0] = V4L2_MBUS_CSI2_CHANNEL_0,
329
+		.vc[PAD1] = V4L2_MBUS_CSI2_CHANNEL_1,
330
+		.vc[PAD2] = V4L2_MBUS_CSI2_CHANNEL_2,
331
+		.vc[PAD3] = V4L2_MBUS_CSI2_CHANNEL_3,
204332 	},
205333 };
206334 
335
+/* link freq = index * MAX96722_LINK_FREQ_MHZ(50) */
207336 static const s64 link_freq_items[] = {
208
-	MAX96722_LINK_FREQ_400MHZ,
337
+	MAX96722_LINK_FREQ_MHZ(0),
338
+	MAX96722_LINK_FREQ_MHZ(50),
339
+	MAX96722_LINK_FREQ_MHZ(100),
340
+	MAX96722_LINK_FREQ_MHZ(150),
341
+	MAX96722_LINK_FREQ_MHZ(200),
342
+	MAX96722_LINK_FREQ_MHZ(250),
343
+	MAX96722_LINK_FREQ_MHZ(300),
344
+	MAX96722_LINK_FREQ_MHZ(350),
345
+	MAX96722_LINK_FREQ_MHZ(400),
346
+	MAX96722_LINK_FREQ_MHZ(450),
347
+	MAX96722_LINK_FREQ_MHZ(500),
348
+	MAX96722_LINK_FREQ_MHZ(550),
349
+	MAX96722_LINK_FREQ_MHZ(600),
350
+	MAX96722_LINK_FREQ_MHZ(650),
351
+	MAX96722_LINK_FREQ_MHZ(700),
352
+	MAX96722_LINK_FREQ_MHZ(750),
353
+	MAX96722_LINK_FREQ_MHZ(800),
354
+	MAX96722_LINK_FREQ_MHZ(850),
355
+	MAX96722_LINK_FREQ_MHZ(900),
356
+	MAX96722_LINK_FREQ_MHZ(950),
357
+	MAX96722_LINK_FREQ_MHZ(1000),
358
+	MAX96722_LINK_FREQ_MHZ(1050),
359
+	MAX96722_LINK_FREQ_MHZ(1100),
360
+	MAX96722_LINK_FREQ_MHZ(1150),
361
+	MAX96722_LINK_FREQ_MHZ(1200),
362
+	MAX96722_LINK_FREQ_MHZ(1250),
209363 };
210364 
211
-/* Write registers up to 4 at a time */
212
-static int max96722_write_reg(struct i2c_client *client, u16 reg,
213
-			     u32 len, u32 val)
365
+static int max96722_write_reg(struct max96722 *max96722, u8 i2c_id,
366
+			u16 reg, u16 reg_len, u16 val_len, u32 val)
214367 {
368
+	struct i2c_client *client = max96722->client;
369
+	u16 client_addr = max96722->i2c_addr[i2c_id];
215370 	u32 buf_i, val_i;
216371 	u8 buf[6];
217372 	u8 *val_p;
218373 	__be32 val_be;
219374 
220
-	dev_dbg(&client->dev, "write reg(0x%x val:0x%x)!\n", reg, val);
375
+	dev_info(&client->dev, "addr(0x%02x) write reg(0x%04x, %d, 0x%02x)\n",
376
+		client_addr, reg, reg_len, val);
221377 
222
-	if (len > 4)
378
+	if (val_len > 4)
223379 		return -EINVAL;
224380 
225
-	buf[0] = reg >> 8;
226
-	buf[1] = reg & 0xff;
381
+	if (reg_len == 2) {
382
+		buf[0] = reg >> 8;
383
+		buf[1] = reg & 0xff;
384
+
385
+		buf_i = 2;
386
+	} else {
387
+		buf[0] = reg & 0xff;
388
+
389
+		buf_i = 1;
390
+	}
227391 
228392 	val_be = cpu_to_be32(val);
229393 	val_p = (u8 *)&val_be;
230
-	buf_i = 2;
231
-	val_i = 4 - len;
394
+	val_i = 4 - val_len;
232395 
233396 	while (val_i < 4)
234397 		buf[buf_i++] = val_p[val_i++];
235398 
236
-	if (i2c_master_send(client, buf, len + 2) != len + 2) {
237
-		dev_err(&client->dev, "%s: writing register 0x%x from 0x%x failed\n",
238
-				__func__, reg, client->addr);
399
+	client->addr = client_addr;
400
+
401
+	if (i2c_master_send(client, buf, (val_len + reg_len)) != (val_len + reg_len)) {
402
+		dev_err(&client->dev,
403
+			"%s: writing register 0x%04x from 0x%02x failed\n",
404
+			__func__, reg, client->addr);
239405 		return -EIO;
240406 	}
241407 
242408 	return 0;
243409 }
244410 
245
-static int max96722_write_array(struct i2c_client *client,
246
-			       const struct regval *regs)
411
+static int max96722_read_reg(struct max96722 *max96722, u8 i2c_id,
412
+			u16 reg, u16 reg_len, u16 val_len, u8 *val)
247413 {
248
-	u32 i;
249
-	int ret = 0;
250
-
251
-	for (i = 0; ret == 0 && regs[i].addr != REG_NULL; i++) {
252
-		client->addr = regs[i].i2c_addr;
253
-		ret = max96722_write_reg(client, regs[i].addr,
254
-					MAX96722_REG_VALUE_08BIT,
255
-					regs[i].val);
256
-		msleep(regs[i].delay);
257
-	}
258
-
259
-	return ret;
260
-}
261
-
262
-/* Read registers up to 4 at a time */
263
-static int max96722_read_reg(struct i2c_client *client, u16 reg,
264
-			    unsigned int len, u32 *val)
265
-{
414
+	struct i2c_client *client = max96722->client;
415
+	u16 client_addr = max96722->i2c_addr[i2c_id];
266416 	struct i2c_msg msgs[2];
267417 	u8 *data_be_p;
268418 	__be32 data_be = 0;
269419 	__be16 reg_addr_be = cpu_to_be16(reg);
420
+	u8 *reg_be_p;
270421 	int ret;
271422 
272
-	if (len > 4 || !len)
423
+	if (val_len > 4 || !val_len)
273424 		return -EINVAL;
274425 
426
+	client->addr = client_addr;
275427 	data_be_p = (u8 *)&data_be;
428
+	reg_be_p = (u8 *)&reg_addr_be;
429
+
276430 	/* Write register address */
277431 	msgs[0].addr = client->addr;
278432 	msgs[0].flags = 0;
279
-	msgs[0].len = 2;
280
-	msgs[0].buf = (u8 *)&reg_addr_be;
433
+	msgs[0].len = reg_len;
434
+	msgs[0].buf = &reg_be_p[2 - reg_len];
281435 
282436 	/* Read data from register */
283437 	msgs[1].addr = client->addr;
284438 	msgs[1].flags = I2C_M_RD;
285
-	msgs[1].len = len;
286
-	msgs[1].buf = &data_be_p[4 - len];
439
+	msgs[1].len = val_len;
440
+	msgs[1].buf = &data_be_p[4 - val_len];
287441 
288442 	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
289443 	if (ret != ARRAY_SIZE(msgs)) {
290
-		dev_err(&client->dev, "%s: reading register 0x%x from 0x%x failed\n",
291
-				__func__, reg, client->addr);
444
+		dev_err(&client->dev,
445
+			"%s: reading register 0x%x from 0x%x failed\n",
446
+			__func__, reg, client->addr);
292447 		return -EIO;
293448 	}
294449 
295450 	*val = be32_to_cpu(data_be);
296451 
452
+#if 0
453
+	dev_info(&client->dev, "addr(0x%02x) read reg(0x%04x, %d, 0x%02x)\n",
454
+		client_addr, reg, reg_len, *val);
455
+#endif
456
+
297457 	return 0;
298458 }
299459 
460
+static int max96722_update_reg_bits(struct max96722 *max96722, u8 i2c_id,
461
+				u16 reg, u16 reg_len, u8 mask, u8 val)
462
+{
463
+	u8 value;
464
+	u32 val_len = DEV_REG_VALUE_08BITS;
465
+	int ret;
466
+
467
+	ret = max96722_read_reg(max96722, i2c_id, reg, reg_len, val_len, &value);
468
+	if (ret)
469
+		return ret;
470
+
471
+	value &= ~mask;
472
+	value |= (val & mask);
473
+	ret = max96722_write_reg(max96722, i2c_id, reg, reg_len, val_len, value);
474
+	if (ret)
475
+		return ret;
476
+
477
+	return 0;
478
+}
479
+
480
+static int max96722_write_array(struct max96722 *max96722,
481
+				const struct regval *regs)
482
+{
483
+	u32 i;
484
+	int ret = 0;
485
+
486
+	for (i = 0; ret == 0 && regs[i].reg != REG_NULL; i++) {
487
+		if (regs[i].mask != 0)
488
+			ret = max96722_update_reg_bits(max96722, regs[i].i2c_id,
489
+					regs[i].reg, regs[i].reg_len,
490
+					regs[i].mask, regs[i].val);
491
+		else
492
+			ret = max96722_write_reg(max96722, regs[i].i2c_id,
493
+					regs[i].reg, regs[i].reg_len,
494
+					DEV_REG_VALUE_08BITS, regs[i].val);
495
+
496
+		if (regs[i].delay != 0)
497
+			msleep(regs[i].delay);
498
+	}
499
+
500
+	return ret;
501
+}
502
+
503
+static int max96722_check_local_chipid(struct max96722 *max96722)
504
+{
505
+	struct device *dev = &max96722->client->dev;
506
+	int ret;
507
+	u8 id = 0;
508
+
509
+	ret = max96722_read_reg(max96722, I2C_DEV_DES,
510
+			MAX96722_REG_CHIP_ID, DEV_REG_LENGTH_16BITS,
511
+			DEV_REG_VALUE_08BITS, &id);
512
+	if ((ret != 0) || (id != MAX96722_CHIP_ID)) {
513
+		dev_err(dev, "Unexpected MAX96722 chip id(%02x), ret(%d)\n", id, ret);
514
+		return -ENODEV;
515
+	}
516
+
517
+	dev_info(dev, "Detected MAX96722 chipid: %02x\n", id);
518
+
519
+	return 0;
520
+}
521
+
522
+static int __maybe_unused max96722_check_remote_chipid(struct max96722 *max96722)
523
+{
524
+	struct device *dev = &max96722->client->dev;
525
+	int ret = 0;
526
+	u8 id;
527
+
528
+	dev_info(dev, "Check remote chipid\n");
529
+
530
+	id = 0;
531
+#if 0
532
+	// max96717
533
+	ret = max96722_read_reg(max96722, I2C_DEV_SER,
534
+			MAX96717_REG_CHIP_ID, DEV_REG_LENGTH_16BITS,
535
+			DEV_REG_VALUE_08BITS, &id);
536
+	if ((ret != 0) || (id != MAX96717_CHIP_ID)) {
537
+		dev_err(dev, "Unexpected MAX96717 chip id(%02x), ret(%d)\n", id, ret);
538
+		return -ENODEV;
539
+	}
540
+	dev_info(dev, "Detected MAX96717 chipid: 0x%02x\n", id);
541
+#endif
542
+
543
+#if 0
544
+	// max9295
545
+	ret = max96722_read_reg(max96722, I2C_DEV_SER,
546
+			MAX9295_REG_CHIP_ID, DEV_REG_LENGTH_16BITS,
547
+			DEV_REG_VALUE_08BITS, &id);
548
+	if ((ret != 0) || (id != MAX9295_CHIP_ID)) {
549
+		dev_err(dev, "Unexpected MAX9295 chip id(%02x), ret(%d)\n", id, ret);
550
+		return -ENODEV;
551
+	}
552
+	dev_info(dev, "Detected MAX9295 chipid: 0x%02x\n", id);
553
+#endif
554
+
555
+#if 0
556
+	// max96715
557
+	ret = max96722_read_reg(max96722, I2C_DEV_SER,
558
+			MAX96715_REG_CHIP_ID, DEV_REG_LENGTH_08BITS,
559
+			DEV_REG_VALUE_08BITS, &id);
560
+	if ((ret != 0) || (id != MAX96715_CHIP_ID)) {
561
+		dev_err(dev, "Unexpected MAX96715 chip id(%02x), ret(%d)\n", id, ret);
562
+		return -ENODEV;
563
+	}
564
+	dev_info(dev, "Detected MAX96715 chipid: 0x%02x\n", id);
565
+#endif
566
+
567
+	return ret;
568
+}
569
+
570
+static u8 max96722_get_link_lock_state(struct max96722 *max96722, u8 link_mask)
571
+{
572
+	struct device *dev = &max96722->client->dev;
573
+	u8 lock = 0, lock_state = 0;
574
+	u8 link_type = 0;
575
+
576
+	link_type = max96722->link_mask & MAXIM_GMSL_TYPE_MASK;
577
+
578
+	if (link_mask & MAXIM_GMSL_LOCK_LINK_A) {
579
+		if (link_type & MAXIM_GMSL_TYPE_LINK_A) {
580
+			// GMSL2 LinkA
581
+			max96722_read_reg(max96722, I2C_DEV_DES,
582
+				0x001a, DEV_REG_LENGTH_16BITS,
583
+				DEV_REG_VALUE_08BITS, &lock);
584
+			if (lock & BIT(3)) {
585
+				lock_state |= MAXIM_GMSL_LOCK_LINK_A;
586
+				dev_info(dev, "GMSL2 LinkA locked\n");
587
+			}
588
+		} else {
589
+			// GMSL1 LinkA
590
+			max96722_read_reg(max96722, I2C_DEV_DES,
591
+				0x0bcb, DEV_REG_LENGTH_16BITS,
592
+				DEV_REG_VALUE_08BITS, &lock);
593
+			if (lock & BIT(0)) {
594
+				lock_state |= MAXIM_GMSL_LOCK_LINK_A;
595
+				dev_info(dev, "GMSL1 LinkA locked\n");
596
+			}
597
+		}
598
+	}
599
+
600
+	if (link_mask & MAXIM_GMSL_LOCK_LINK_B) {
601
+		if (link_type & MAXIM_GMSL_TYPE_LINK_B) {
602
+			// GMSL2 LinkB
603
+			max96722_read_reg(max96722, I2C_DEV_DES,
604
+				0x000a, DEV_REG_LENGTH_16BITS,
605
+				DEV_REG_VALUE_08BITS, &lock);
606
+			if (lock & BIT(3)) {
607
+				lock_state |= MAXIM_GMSL_LOCK_LINK_B;
608
+				dev_info(dev, "GMSL2 LinkB locked\n");
609
+			}
610
+		} else {
611
+			// GMSL1 LinkB
612
+			max96722_read_reg(max96722, I2C_DEV_DES,
613
+				0x0ccb, DEV_REG_LENGTH_16BITS,
614
+				DEV_REG_VALUE_08BITS, &lock);
615
+			if (lock & BIT(0)) {
616
+				lock_state |= MAXIM_GMSL_LOCK_LINK_B;
617
+				dev_info(dev, "GMSL1 LinkB locked\n");
618
+			}
619
+		}
620
+	}
621
+
622
+	if (link_mask & MAXIM_GMSL_LOCK_LINK_C) {
623
+		if (link_type & MAXIM_GMSL_TYPE_LINK_C) {
624
+			// GMSL2 LinkC
625
+			max96722_read_reg(max96722, I2C_DEV_DES,
626
+				0x000b, DEV_REG_LENGTH_16BITS,
627
+				DEV_REG_VALUE_08BITS, &lock);
628
+			if (lock & BIT(3)) {
629
+				lock_state |= MAXIM_GMSL_LOCK_LINK_C;
630
+				dev_info(dev, "GMSL2 LinkC locked\n");
631
+			}
632
+		} else {
633
+			// GMSL1 LinkC
634
+			max96722_read_reg(max96722, I2C_DEV_DES,
635
+				0x0dcb, DEV_REG_LENGTH_16BITS,
636
+				DEV_REG_VALUE_08BITS, &lock);
637
+			if (lock & BIT(0)) {
638
+				lock_state |= MAXIM_GMSL_LOCK_LINK_C;
639
+				dev_info(dev, "GMSL1 LinkC locked\n");
640
+			}
641
+		}
642
+	}
643
+
644
+	if (link_mask & MAXIM_GMSL_LOCK_LINK_D) {
645
+		if (link_type & MAXIM_GMSL_TYPE_LINK_D) {
646
+			// GMSL2 LinkD
647
+			max96722_read_reg(max96722, I2C_DEV_DES,
648
+				0x000c, DEV_REG_LENGTH_16BITS,
649
+				DEV_REG_VALUE_08BITS, &lock);
650
+			if (lock & BIT(3)) {
651
+				lock_state |= MAXIM_GMSL_LOCK_LINK_D;
652
+				dev_info(dev, "GMSL2 LinkD locked\n");
653
+			}
654
+		} else {
655
+			// GMSL1 LinkD
656
+			max96722_read_reg(max96722, I2C_DEV_DES,
657
+				0x0ecb, DEV_REG_LENGTH_16BITS,
658
+				DEV_REG_VALUE_08BITS, &lock);
659
+			if (lock & BIT(0)) {
660
+				lock_state |= MAXIM_GMSL_LOCK_LINK_D;
661
+				dev_info(dev, "GMSL1 LinkD locked\n");
662
+			}
663
+		}
664
+	}
665
+
666
+	return lock_state;
667
+}
668
+
669
+static int max96722_check_link_lock_state(struct max96722 *max96722)
670
+{
671
+	struct device *dev = &max96722->client->dev;
672
+	u8 lock_state = 0, link_mask = 0, link_type = 0;
673
+	int ret, i, time_ms;
674
+
675
+	ret = max96722_check_local_chipid(max96722);
676
+	if (ret)
677
+		return ret;
678
+
679
+	/* IF VDD = 1.2V: Enable REG_ENABLE and REG_MNL
680
+	 *	CTRL0: Enable REG_ENABLE
681
+	 *	CTRL2: Enable REG_MNL
682
+	 */
683
+	max96722_update_reg_bits(max96722, I2C_DEV_DES,
684
+			0x0017, DEV_REG_LENGTH_16BITS, BIT(2), BIT(2));
685
+	max96722_update_reg_bits(max96722, I2C_DEV_DES,
686
+			0x0019, DEV_REG_LENGTH_16BITS, BIT(4), BIT(4));
687
+
688
+	// CSI output disabled
689
+	max96722_write_reg(max96722, I2C_DEV_DES,
690
+			0x040B, DEV_REG_LENGTH_16BITS,
691
+			DEV_REG_VALUE_08BITS, 0x00);
692
+
693
+	// All links select mode by link_type and disable at beginning.
694
+	link_type = max96722->link_mask & MAXIM_GMSL_TYPE_MASK;
695
+	max96722_write_reg(max96722, I2C_DEV_DES,
696
+			0x0006, DEV_REG_LENGTH_16BITS,
697
+			DEV_REG_VALUE_08BITS, link_type);
698
+
699
+	// Link Rate
700
+	// Link A ~ Link D Transmitter Rate: 187.5Mbps, Receiver Rate: 3Gbps
701
+	max96722_write_reg(max96722, I2C_DEV_DES,
702
+				0x0010, DEV_REG_LENGTH_16BITS,
703
+				DEV_REG_VALUE_08BITS, 0x11);
704
+	max96722_write_reg(max96722, I2C_DEV_DES,
705
+				0x0011, DEV_REG_LENGTH_16BITS,
706
+				DEV_REG_VALUE_08BITS, 0x11);
707
+
708
+	// GMSL1: Enable HIM on deserializer on Link A/B/C/D
709
+	if ((link_type & MAXIM_GMSL_TYPE_LINK_A) == 0) {
710
+		max96722_write_reg(max96722, I2C_DEV_DES,
711
+				0x0B06, DEV_REG_LENGTH_16BITS,
712
+				DEV_REG_VALUE_08BITS, 0xEF);
713
+	}
714
+	if ((link_type & MAXIM_GMSL_TYPE_LINK_B) == 0) {
715
+		max96722_write_reg(max96722, I2C_DEV_DES,
716
+				0x0C06, DEV_REG_LENGTH_16BITS,
717
+				DEV_REG_VALUE_08BITS, 0xEF);
718
+	}
719
+	if ((link_type & MAXIM_GMSL_TYPE_LINK_C) == 0) {
720
+		max96722_write_reg(max96722, I2C_DEV_DES,
721
+				0x0D06, DEV_REG_LENGTH_16BITS,
722
+				DEV_REG_VALUE_08BITS, 0xEF);
723
+	}
724
+	if ((link_type & MAXIM_GMSL_TYPE_LINK_D) == 0) {
725
+		max96722_write_reg(max96722, I2C_DEV_DES,
726
+				0x0E06, DEV_REG_LENGTH_16BITS,
727
+				DEV_REG_VALUE_08BITS, 0xEF);
728
+	}
729
+
730
+	// Link A ~ Link D One-Shot Reset depend on link_mask
731
+	link_mask = max96722->link_mask & MAXIM_GMSL_LOCK_MASK;
732
+	max96722_write_reg(max96722, I2C_DEV_DES,
733
+			0x0018, DEV_REG_LENGTH_16BITS,
734
+			DEV_REG_VALUE_08BITS, link_mask);
735
+
736
+	// Link A ~ Link D enable depend on link_type and link_mask
737
+	max96722_write_reg(max96722, I2C_DEV_DES,
738
+			0x0006, DEV_REG_LENGTH_16BITS,
739
+			DEV_REG_VALUE_08BITS, link_type | link_mask);
740
+
741
+	time_ms = 50;
742
+	msleep(time_ms);
743
+
744
+	for (i = 0; i < 20; i++) {
745
+		if ((lock_state & MAXIM_GMSL_LOCK_LINK_A) == 0)
746
+			if (max96722_get_link_lock_state(max96722, MAXIM_GMSL_LOCK_LINK_A)) {
747
+				lock_state |= MAXIM_GMSL_LOCK_LINK_A;
748
+				dev_info(dev, "LinkA locked time: %d ms\n", time_ms);
749
+			}
750
+
751
+		if ((lock_state & MAXIM_GMSL_LOCK_LINK_B) == 0)
752
+			if (max96722_get_link_lock_state(max96722, MAXIM_GMSL_LOCK_LINK_B)) {
753
+				lock_state |= MAXIM_GMSL_LOCK_LINK_B;
754
+				dev_info(dev, "LinkB locked time: %d ms\n", time_ms);
755
+			}
756
+
757
+		if ((lock_state & MAXIM_GMSL_LOCK_LINK_C) == 0)
758
+			if (max96722_get_link_lock_state(max96722, MAXIM_GMSL_LOCK_LINK_C)) {
759
+				lock_state |= MAXIM_GMSL_LOCK_LINK_C;
760
+				dev_info(dev, "LinkC locked time: %d ms\n", time_ms);
761
+			}
762
+
763
+		if ((lock_state & MAXIM_GMSL_LOCK_LINK_D) == 0)
764
+			if (max96722_get_link_lock_state(max96722, MAXIM_GMSL_LOCK_LINK_D)) {
765
+				lock_state |= MAXIM_GMSL_LOCK_LINK_D;
766
+				dev_info(dev, "LinkD locked time: %d ms\n", time_ms);
767
+			}
768
+
769
+		if ((lock_state & link_mask) == link_mask) {
770
+			dev_info(dev, "All Links are locked: 0x%x, time_ms = %d\n", lock_state, time_ms);
771
+#if 0
772
+			max96722_check_remote_chipid(max96722);
773
+#endif
774
+			return 0;
775
+		}
776
+
777
+		msleep(10);
778
+		time_ms += 10;
779
+	}
780
+
781
+	if ((lock_state & link_mask) != 0) {
782
+		dev_info(dev, "Partial links are locked: 0x%x, time_ms = %d\n", lock_state, time_ms);
783
+		return 0;
784
+	} else {
785
+		dev_err(dev, "Failed to detect camera link, time_ms = %d!\n", time_ms);
786
+		return -ENODEV;
787
+	}
788
+}
789
+
790
+static irqreturn_t max96722_hot_plug_detect_irq_handler(int irq, void *dev_id)
791
+{
792
+	struct max96722 *max96722 = dev_id;
793
+	struct device *dev = &max96722->client->dev;
794
+	u8 lock_state = 0, link_mask = 0;
795
+
796
+	link_mask = max96722->link_mask & MAXIM_GMSL_LOCK_MASK;
797
+	if (max96722->streaming) {
798
+		lock_state = max96722_get_link_lock_state(max96722, link_mask);
799
+		if (lock_state == link_mask) {
800
+			dev_info(dev, "serializer plug in, lock_state = 0x%02x\n", lock_state);
801
+		} else {
802
+			dev_info(dev, "serializer plug out, lock_state = 0x%02x\n", lock_state);
803
+		}
804
+	}
805
+
806
+	return IRQ_HANDLED;
807
+}
808
+
809
+static int max96722_dphy_dpll_predef_set(struct max96722 *max96722, u32 link_freq_mhz)
810
+{
811
+	struct device *dev = &max96722->client->dev;
812
+	int ret = 0;
813
+	u8 dpll_val = 0, dpll_lock = 0;
814
+	u8 mipi_tx_phy_enable = 0;
815
+
816
+	ret = max96722_read_reg(max96722, I2C_DEV_DES,
817
+			0x08A2, DEV_REG_LENGTH_16BITS,
818
+			DEV_REG_VALUE_08BITS, &mipi_tx_phy_enable);
819
+	if (ret)
820
+		return ret;
821
+	mipi_tx_phy_enable = (mipi_tx_phy_enable & 0xF0) >> 4;
822
+
823
+	dev_info(dev, "DPLL predef set: mipi_tx_phy_enable = 0x%02x, link_freq_mhz = %d\n",
824
+			mipi_tx_phy_enable, link_freq_mhz);
825
+
826
+	// dphy max data rate is 2500MHz
827
+	if (link_freq_mhz > (2500 >> 1))
828
+		link_freq_mhz = (2500 >> 1);
829
+
830
+	dpll_val = DIV_ROUND_UP(link_freq_mhz * 2, 100) & 0x1F;
831
+	// Disable software override for frequency fine tuning
832
+	dpll_val |= BIT(5);
833
+
834
+	// MIPI PHY0
835
+	if (mipi_tx_phy_enable & BIT(0)) {
836
+		// Hold DPLL in reset (config_soft_rst_n = 0) before changing the rate
837
+		ret |= max96722_write_reg(max96722, I2C_DEV_DES,
838
+				0x1C00, DEV_REG_LENGTH_16BITS,
839
+				DEV_REG_VALUE_08BITS, 0xf4);
840
+		// Set data rate and enable software override
841
+		ret |= max96722_update_reg_bits(max96722, I2C_DEV_DES,
842
+				0x0415, DEV_REG_LENGTH_16BITS, 0x3F, dpll_val);
843
+		// Release reset to DPLL (config_soft_rst_n = 1)
844
+		ret |= max96722_write_reg(max96722, I2C_DEV_DES,
845
+				0x1C00, DEV_REG_LENGTH_16BITS,
846
+				DEV_REG_VALUE_08BITS, 0xf5);
847
+	}
848
+
849
+	// MIPI PHY1
850
+	if (mipi_tx_phy_enable & BIT(1)) {
851
+		// Hold DPLL in reset (config_soft_rst_n = 0) before changing the rate
852
+		ret |= max96722_write_reg(max96722, I2C_DEV_DES,
853
+				0x1D00, DEV_REG_LENGTH_16BITS,
854
+				DEV_REG_VALUE_08BITS, 0xf4);
855
+		// Set data rate and enable software override
856
+		ret |= max96722_update_reg_bits(max96722, I2C_DEV_DES,
857
+				0x0418, DEV_REG_LENGTH_16BITS, 0x3F, dpll_val);
858
+		// Release reset to DPLL (config_soft_rst_n = 1)
859
+		ret |= max96722_write_reg(max96722, I2C_DEV_DES,
860
+				0x1D00, DEV_REG_LENGTH_16BITS,
861
+				DEV_REG_VALUE_08BITS, 0xf5);
862
+	}
863
+
864
+	// MIPI PHY2
865
+	if (mipi_tx_phy_enable & BIT(2)) {
866
+		// Hold DPLL in reset (config_soft_rst_n = 0) before changing the rate
867
+		ret |= max96722_write_reg(max96722, I2C_DEV_DES,
868
+				0x1E00, DEV_REG_LENGTH_16BITS,
869
+				DEV_REG_VALUE_08BITS, 0xf4);
870
+		// Set data rate and enable software override
871
+		ret |= max96722_update_reg_bits(max96722, I2C_DEV_DES,
872
+				0x041B, DEV_REG_LENGTH_16BITS, 0x3F, dpll_val);
873
+		// Release reset to DPLL (config_soft_rst_n = 1)
874
+		ret |= max96722_write_reg(max96722, I2C_DEV_DES,
875
+				0x1E00, DEV_REG_LENGTH_16BITS,
876
+				DEV_REG_VALUE_08BITS, 0xf5);
877
+	}
878
+
879
+	// MIPI PHY3
880
+	if (mipi_tx_phy_enable & BIT(3)) {
881
+		// Hold DPLL in reset (config_soft_rst_n = 0) before changing the rate
882
+		ret |= max96722_write_reg(max96722, I2C_DEV_DES,
883
+				0x1F00, DEV_REG_LENGTH_16BITS,
884
+				DEV_REG_VALUE_08BITS, 0xf4);
885
+		// Set data rate and enable software override
886
+		ret |= max96722_update_reg_bits(max96722, I2C_DEV_DES,
887
+				0x041E, DEV_REG_LENGTH_16BITS, 0x3F, dpll_val);
888
+		// Release reset to DPLL (config_soft_rst_n = 1)
889
+		ret |= max96722_write_reg(max96722, I2C_DEV_DES,
890
+				0x1F00, DEV_REG_LENGTH_16BITS,
891
+				DEV_REG_VALUE_08BITS, 0xf5);
892
+	}
893
+
894
+	if (ret) {
895
+		dev_err(dev, "DPLL predef set error!\n");
896
+		return ret;
897
+	}
898
+
899
+	ret = read_poll_timeout(max96722_read_reg, ret,
900
+				!(ret < 0) && (dpll_lock & 0xF0),
901
+				1000, 10000, false,
902
+				max96722, I2C_DEV_DES,
903
+				0x0400, DEV_REG_LENGTH_16BITS,
904
+				DEV_REG_VALUE_08BITS, &dpll_lock);
905
+	if (ret < 0) {
906
+		dev_err(dev, "DPLL is not locked, dpll_lock = 0x%02x\n", dpll_lock);
907
+		return ret;
908
+	} else {
909
+		dev_err(dev, "DPLL is locked, dpll_lock = 0x%02x\n", dpll_lock);
910
+		return 0;
911
+	}
912
+}
913
+
914
+static int max96722_auto_init_deskew(struct max96722 *max96722, u32 deskew_mask)
915
+{
916
+	struct device *dev = &max96722->client->dev;
917
+	int ret = 0;
918
+
919
+	dev_info(dev, "Auto initial deskew: deskew_mask = 0x%02x\n", deskew_mask);
920
+
921
+	// D-PHY Deskew Initial Calibration Control
922
+	if (deskew_mask & BIT(0)) // MIPI PHY0
923
+		ret |= max96722_write_reg(max96722, I2C_DEV_DES,
924
+				0x0903, DEV_REG_LENGTH_16BITS,
925
+				DEV_REG_VALUE_08BITS, 0x80);
926
+
927
+	if (deskew_mask & BIT(1)) // MIPI PHY1
928
+		ret |= max96722_write_reg(max96722, I2C_DEV_DES,
929
+				0x0943, DEV_REG_LENGTH_16BITS,
930
+				DEV_REG_VALUE_08BITS, 0x80);
931
+
932
+	if (deskew_mask & BIT(2)) // MIPI PHY2
933
+		ret |= max96722_write_reg(max96722, I2C_DEV_DES,
934
+				0x0983, DEV_REG_LENGTH_16BITS,
935
+				DEV_REG_VALUE_08BITS, 0x80);
936
+
937
+	if (deskew_mask & BIT(3)) // MIPI PHY3
938
+		ret |= max96722_write_reg(max96722, I2C_DEV_DES,
939
+				0x09C3, DEV_REG_LENGTH_16BITS,
940
+				DEV_REG_VALUE_08BITS, 0x80);
941
+
942
+	return ret;
943
+}
944
+
945
+static int max96722_frame_sync_period(struct max96722 *max96722, u32 period)
946
+{
947
+	struct device *dev = &max96722->client->dev;
948
+	u32 pclk, fsync_peroid;
949
+	u8 fsync_peroid_h, fsync_peroid_m, fsync_peroid_l;
950
+	int ret = 0;
951
+
952
+	if (period == 0)
953
+		return 0;
954
+
955
+	dev_info(dev, "Frame sync period = %d\n", period);
956
+
957
+#if 1 // TODO: Sensor slave mode
958
+	// sendor slave mode enable
959
+#endif
960
+
961
+	// Master link Video 0 for frame sync generation
962
+	ret |= max96722_write_reg(max96722, I2C_DEV_DES,
963
+			0x04A2, DEV_REG_LENGTH_16BITS,
964
+			DEV_REG_VALUE_08BITS, 0x00);
965
+	// Disable Vsync-Fsync overlap window
966
+	ret |= max96722_write_reg(max96722, I2C_DEV_DES,
967
+			0x04AA, DEV_REG_LENGTH_16BITS,
968
+			DEV_REG_VALUE_08BITS, 0x00);
969
+	ret |= max96722_write_reg(max96722, I2C_DEV_DES,
970
+			0x04AB, DEV_REG_LENGTH_16BITS,
971
+			DEV_REG_VALUE_08BITS, 0x00);
972
+
973
+	// Set FSYNC period to 25M/30 clock cycles. PCLK = 25MHz. Sync freq = 30Hz
974
+	pclk = 25 * 1000 * 1000;
975
+	fsync_peroid = DIV_ROUND_UP(pclk, period) - 1;
976
+	fsync_peroid_l = (fsync_peroid >> 0) & 0xFF;
977
+	fsync_peroid_m = (fsync_peroid >> 8) & 0xFF;
978
+	fsync_peroid_h = (fsync_peroid >> 16) & 0xFF;
979
+	dev_info(dev, "Frame sync period: H = 0x%02x, M = 0x%02x, L = 0x%02x\n",
980
+			fsync_peroid_h, fsync_peroid_m, fsync_peroid_l);
981
+	// FSYNC_PERIOD_H
982
+	ret |= max96722_write_reg(max96722, I2C_DEV_DES,
983
+			0x04A7, DEV_REG_LENGTH_16BITS,
984
+			DEV_REG_VALUE_08BITS, fsync_peroid_h);
985
+	// FSYNC_PERIOD_M
986
+	ret |= max96722_write_reg(max96722, I2C_DEV_DES,
987
+			0x04A6, DEV_REG_LENGTH_16BITS,
988
+			DEV_REG_VALUE_08BITS, fsync_peroid_m);
989
+	// FSYNC_PERIOD_L
990
+	ret |= max96722_write_reg(max96722, I2C_DEV_DES,
991
+			0x04A5, DEV_REG_LENGTH_16BITS,
992
+			DEV_REG_VALUE_08BITS, fsync_peroid_l);
993
+
994
+	// FSYNC is GMSL2 type, use osc for fsync, include all links/pipes in fsync gen
995
+	ret |= max96722_write_reg(max96722, I2C_DEV_DES,
996
+			0x04AF, DEV_REG_LENGTH_16BITS,
997
+			DEV_REG_VALUE_08BITS, 0xcf);
998
+
999
+#if 1 // TODO: Desrializer MFP
1000
+	// FSYNC_TX_ID: set 4 to match MFP4 on serializer side
1001
+	ret |= max96722_write_reg(max96722, I2C_DEV_DES,
1002
+			0x04B1, DEV_REG_LENGTH_16BITS,
1003
+			DEV_REG_VALUE_08BITS, 0x20);
1004
+#endif
1005
+
1006
+#if 1 // TODO: Serializer MFP
1007
+	// Enable GPIO_RX_EN on serializer MFP4
1008
+	ret |= max96722_write_reg(max96722, I2C_DEV_SER,
1009
+			0x02CA, DEV_REG_LENGTH_16BITS,
1010
+			DEV_REG_VALUE_08BITS, 0x84);
1011
+#endif
1012
+
1013
+	// MFP2, VS not gen internally, GPIO not used to gen fsync, manual mode
1014
+	ret |= max96722_write_reg(max96722, I2C_DEV_DES,
1015
+			0x04A0, DEV_REG_LENGTH_16BITS,
1016
+			DEV_REG_VALUE_08BITS, 0x04);
1017
+
1018
+	return ret;
1019
+}
1020
+
1021
+static int max96722_mipi_enable(struct max96722 *max96722, bool enable)
1022
+{
1023
+	int ret = 0;
1024
+
1025
+	if (enable) {
1026
+#if MAXIM_FORCE_ALL_CLOCK_EN
1027
+		// Force all MIPI clocks running
1028
+		ret |= max96722_update_reg_bits(max96722, I2C_DEV_DES,
1029
+				0x08A0, DEV_REG_LENGTH_16BITS, BIT(7), BIT(7));
1030
+#endif
1031
+		// CSI output enabled
1032
+		ret |= max96722_update_reg_bits(max96722, I2C_DEV_DES,
1033
+				0x040B, DEV_REG_LENGTH_16BITS, BIT(1), BIT(1));
1034
+	} else {
1035
+#if MAXIM_FORCE_ALL_CLOCK_EN
1036
+		// Normal mode
1037
+		ret |= max96722_update_reg_bits(max96722, I2C_DEV_DES,
1038
+				0x08A0, DEV_REG_LENGTH_16BITS, BIT(7), 0x00);
1039
+#endif
1040
+		// CSI output disabled
1041
+		ret |= max96722_update_reg_bits(max96722, I2C_DEV_DES,
1042
+				0x040B, DEV_REG_LENGTH_16BITS, BIT(1), 0x00);
1043
+	}
1044
+
1045
+	return ret;
1046
+}
1047
+
3001048 static int max96722_get_reso_dist(const struct max96722_mode *mode,
301
-				 struct v4l2_mbus_framefmt *framefmt)
1049
+				  struct v4l2_mbus_framefmt *framefmt)
3021050 {
3031051 	return abs(mode->width - framefmt->width) +
304
-		abs(mode->height - framefmt->height);
1052
+	       abs(mode->height - framefmt->height);
3051053 }
3061054 
3071055 static const struct max96722_mode *
308
-max96722_find_best_fit(struct v4l2_subdev_format *fmt)
1056
+max96722_find_best_fit(struct max96722 *max96722, struct v4l2_subdev_format *fmt)
3091057 {
3101058 	struct v4l2_mbus_framefmt *framefmt = &fmt->format;
3111059 	int dist;
....@@ -313,28 +1061,32 @@
3131061 	int cur_best_fit_dist = -1;
3141062 	unsigned int i;
3151063 
316
-	for (i = 0; i < ARRAY_SIZE(supported_modes); i++) {
317
-		dist = max96722_get_reso_dist(&supported_modes[i], framefmt);
318
-		if (cur_best_fit_dist == -1 || dist < cur_best_fit_dist) {
1064
+	for (i = 0; i < max96722->cfg_modes_num; i++) {
1065
+		dist = max96722_get_reso_dist(&max96722->supported_modes[i], framefmt);
1066
+		if ((cur_best_fit_dist == -1 || dist < cur_best_fit_dist)
1067
+				&& (max96722->supported_modes[i].bus_fmt == framefmt->code)) {
3191068 			cur_best_fit_dist = dist;
3201069 			cur_best_fit = i;
3211070 		}
3221071 	}
3231072 
324
-	return &supported_modes[cur_best_fit];
1073
+	return &max96722->supported_modes[cur_best_fit];
3251074 }
3261075 
3271076 static int max96722_set_fmt(struct v4l2_subdev *sd,
328
-			   struct v4l2_subdev_pad_config *cfg,
329
-			  struct v4l2_subdev_format *fmt)
1077
+			    struct v4l2_subdev_pad_config *cfg,
1078
+			    struct v4l2_subdev_format *fmt)
3301079 {
331
-	struct max96722 *max96722 = to_max96722(sd);
1080
+	struct max96722 *max96722 = v4l2_get_subdevdata(sd);
3321081 	const struct max96722_mode *mode;
1082
+	u64 pixel_rate = 0;
1083
+	u8 data_lanes;
3331084 
3341085 	mutex_lock(&max96722->mutex);
3351086 
336
-	mode = max96722_find_best_fit(fmt);
337
-	fmt->format.code = MAX96722_MEDIA_BUS_FMT;
1087
+	mode = max96722_find_best_fit(max96722, fmt);
1088
+
1089
+	fmt->format.code = mode->bus_fmt;
3381090 	fmt->format.width = mode->width;
3391091 	fmt->format.height = mode->height;
3401092 	fmt->format.field = V4L2_FIELD_NONE;
....@@ -350,6 +1102,19 @@
3501102 			mutex_unlock(&max96722->mutex);
3511103 			return -EBUSY;
3521104 		}
1105
+
1106
+		max96722->cur_mode = mode;
1107
+
1108
+		__v4l2_ctrl_s_ctrl(max96722->link_freq, mode->link_freq_idx);
1109
+		/* pixel rate = link frequency * 2 * lanes / BITS_PER_SAMPLE */
1110
+		data_lanes = max96722->bus_cfg.bus.mipi_csi2.num_data_lanes;
1111
+		pixel_rate = (u32)link_freq_items[mode->link_freq_idx] / mode->bpp * 2 * data_lanes;
1112
+		__v4l2_ctrl_s_ctrl_int64(max96722->pixel_rate, pixel_rate);
1113
+
1114
+		dev_info(&max96722->client->dev, "mipi_freq_idx = %d, mipi_link_freq = %lld\n",
1115
+				mode->link_freq_idx, link_freq_items[mode->link_freq_idx]);
1116
+		dev_info(&max96722->client->dev, "pixel_rate = %lld, bpp = %d\n",
1117
+				pixel_rate, mode->bpp);
3531118 	}
3541119 
3551120 	mutex_unlock(&max96722->mutex);
....@@ -358,10 +1123,10 @@
3581123 }
3591124 
3601125 static int max96722_get_fmt(struct v4l2_subdev *sd,
361
-			   struct v4l2_subdev_pad_config *cfg,
362
-			   struct v4l2_subdev_format *fmt)
1126
+			    struct v4l2_subdev_pad_config *cfg,
1127
+			    struct v4l2_subdev_format *fmt)
3631128 {
364
-	struct max96722 *max96722 = to_max96722(sd);
1129
+	struct max96722 *max96722 = v4l2_get_subdevdata(sd);
3651130 	const struct max96722_mode *mode = max96722->cur_mode;
3661131 
3671132 	mutex_lock(&max96722->mutex);
....@@ -375,8 +1140,12 @@
3751140 	} else {
3761141 		fmt->format.width = mode->width;
3771142 		fmt->format.height = mode->height;
378
-		fmt->format.code = MAX96722_MEDIA_BUS_FMT;
1143
+		fmt->format.code = mode->bus_fmt;
3791144 		fmt->format.field = V4L2_FIELD_NONE;
1145
+		if (fmt->pad < PAD_MAX && fmt->pad >= PAD0)
1146
+			fmt->reserved[0] = mode->vc[fmt->pad];
1147
+		else
1148
+			fmt->reserved[0] = mode->vc[PAD0];
3801149 	}
3811150 	mutex_unlock(&max96722->mutex);
3821151 
....@@ -384,38 +1153,43 @@
3841153 }
3851154 
3861155 static int max96722_enum_mbus_code(struct v4l2_subdev *sd,
387
-				  struct v4l2_subdev_pad_config *cfg,
388
-				  struct v4l2_subdev_mbus_code_enum *code)
1156
+				   struct v4l2_subdev_pad_config *cfg,
1157
+				   struct v4l2_subdev_mbus_code_enum *code)
3891158 {
1159
+	struct max96722 *max96722 = v4l2_get_subdevdata(sd);
1160
+	const struct max96722_mode *mode = max96722->cur_mode;
1161
+
3901162 	if (code->index != 0)
3911163 		return -EINVAL;
392
-	code->code = MAX96722_MEDIA_BUS_FMT;
1164
+	code->code = mode->bus_fmt;
3931165 
3941166 	return 0;
3951167 }
3961168 
3971169 static int max96722_enum_frame_sizes(struct v4l2_subdev *sd,
398
-				    struct v4l2_subdev_pad_config *cfg,
399
-				   struct v4l2_subdev_frame_size_enum *fse)
1170
+				     struct v4l2_subdev_pad_config *cfg,
1171
+				     struct v4l2_subdev_frame_size_enum *fse)
4001172 {
401
-	if (fse->index >= ARRAY_SIZE(supported_modes))
1173
+	struct max96722 *max96722 = v4l2_get_subdevdata(sd);
1174
+
1175
+	if (fse->index >= max96722->cfg_modes_num)
4021176 		return -EINVAL;
4031177 
404
-	if (fse->code != MAX96722_MEDIA_BUS_FMT)
1178
+	if (fse->code != max96722->supported_modes[fse->index].bus_fmt)
4051179 		return -EINVAL;
4061180 
407
-	fse->min_width  = supported_modes[fse->index].width;
408
-	fse->max_width  = supported_modes[fse->index].width;
409
-	fse->max_height = supported_modes[fse->index].height;
410
-	fse->min_height = supported_modes[fse->index].height;
1181
+	fse->min_width  = max96722->supported_modes[fse->index].width;
1182
+	fse->max_width  = max96722->supported_modes[fse->index].width;
1183
+	fse->max_height = max96722->supported_modes[fse->index].height;
1184
+	fse->min_height = max96722->supported_modes[fse->index].height;
4111185 
4121186 	return 0;
4131187 }
4141188 
4151189 static int max96722_g_frame_interval(struct v4l2_subdev *sd,
416
-				    struct v4l2_subdev_frame_interval *fi)
1190
+				     struct v4l2_subdev_frame_interval *fi)
4171191 {
418
-	struct max96722 *max96722 = to_max96722(sd);
1192
+	struct max96722 *max96722 = v4l2_get_subdevdata(sd);
4191193 	const struct max96722_mode *mode = max96722->cur_mode;
4201194 
4211195 	mutex_lock(&max96722->mutex);
....@@ -426,7 +1200,7 @@
4261200 }
4271201 
4281202 static void max96722_get_module_inf(struct max96722 *max96722,
429
-				   struct rkmodule_inf *inf)
1203
+				    struct rkmodule_inf *inf)
4301204 {
4311205 	memset(inf, 0, sizeof(*inf));
4321206 	strscpy(inf->base.sensor, MAX96722_NAME, sizeof(inf->base.sensor));
....@@ -435,26 +1209,30 @@
4351209 	strscpy(inf->base.lens, max96722->len_name, sizeof(inf->base.lens));
4361210 }
4371211 
438
-static void max96722_get_vicap_rst_inf(struct max96722 *max96722,
439
-				   struct rkmodule_vicap_reset_info *rst_info)
1212
+static void
1213
+max96722_get_vicap_rst_inf(struct max96722 *max96722,
1214
+			   struct rkmodule_vicap_reset_info *rst_info)
4401215 {
4411216 	struct i2c_client *client = max96722->client;
4421217 
4431218 	rst_info->is_reset = max96722->hot_plug;
4441219 	max96722->hot_plug = false;
4451220 	rst_info->src = RKCIF_RESET_SRC_ERR_HOTPLUG;
446
-	dev_info(&client->dev, "%s: rst_info->is_reset:%d.\n", __func__, rst_info->is_reset);
1221
+	dev_info(&client->dev, "%s: rst_info->is_reset:%d.\n", __func__,
1222
+		 rst_info->is_reset);
4471223 }
4481224 
449
-static void max96722_set_vicap_rst_inf(struct max96722 *max96722,
450
-				   struct rkmodule_vicap_reset_info rst_info)
1225
+static void
1226
+max96722_set_vicap_rst_inf(struct max96722 *max96722,
1227
+			   struct rkmodule_vicap_reset_info rst_info)
4511228 {
4521229 	max96722->is_reset = rst_info.is_reset;
4531230 }
4541231 
4551232 static long max96722_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
4561233 {
457
-	struct max96722 *max96722 = to_max96722(sd);
1234
+	struct max96722 *max96722 = v4l2_get_subdevdata(sd);
1235
+	struct rkmodule_csi_dphy_param *dphy_param;
4581236 	long ret = 0;
4591237 	u32 stream = 0;
4601238 
....@@ -463,30 +1241,34 @@
4631241 		max96722_get_module_inf(max96722, (struct rkmodule_inf *)arg);
4641242 		break;
4651243 	case RKMODULE_SET_QUICK_STREAM:
466
-
4671244 		stream = *((u32 *)arg);
4681245 
4691246 		if (stream)
470
-			ret = max96722_write_reg(max96722->client,
471
-				 MAX96722_REG_CTRL_MODE,
472
-				 MAX96722_REG_VALUE_08BIT,
473
-				 MAX96722_MODE_STREAMING);
1247
+			ret = max96722_mipi_enable(max96722, true);
4741248 		else
475
-			ret = max96722_write_reg(max96722->client,
476
-				 MAX96722_REG_CTRL_MODE,
477
-				 MAX96722_REG_VALUE_08BIT,
478
-				 MAX96722_MODE_SW_STANDBY);
1249
+			ret = max96722_mipi_enable(max96722, false);
4791250 		break;
4801251 	case RKMODULE_GET_VICAP_RST_INFO:
481
-		max96722_get_vicap_rst_inf(max96722,
482
-			(struct rkmodule_vicap_reset_info *)arg);
1252
+		max96722_get_vicap_rst_inf(
1253
+			max96722, (struct rkmodule_vicap_reset_info *)arg);
4831254 		break;
4841255 	case RKMODULE_SET_VICAP_RST_INFO:
485
-		max96722_set_vicap_rst_inf(max96722,
486
-			*(struct rkmodule_vicap_reset_info *)arg);
1256
+		max96722_set_vicap_rst_inf(
1257
+			max96722, *(struct rkmodule_vicap_reset_info *)arg);
4871258 		break;
488
-	case RKMODULE_GET_CSI_DSI_INFO:
489
-		*(int *)arg = RKMODULE_CSI_INPUT;
1259
+	case RKMODULE_GET_START_STREAM_SEQ:
1260
+		break;
1261
+	case RKMODULE_SET_CSI_DPHY_PARAM:
1262
+		dphy_param = (struct rkmodule_csi_dphy_param *)arg;
1263
+		if (dphy_param->vendor == rk3588_dcphy_param.vendor)
1264
+			rk3588_dcphy_param = *dphy_param;
1265
+		dev_dbg(&max96722->client->dev, "sensor set dphy param\n");
1266
+		break;
1267
+	case RKMODULE_GET_CSI_DPHY_PARAM:
1268
+		dphy_param = (struct rkmodule_csi_dphy_param *)arg;
1269
+		if (dphy_param->vendor == rk3588_dcphy_param.vendor)
1270
+			*dphy_param = rk3588_dcphy_param;
1271
+		dev_dbg(&max96722->client->dev, "sensor get dphy param\n");
4901272 		break;
4911273 	default:
4921274 		ret = -ENOIOCTLCMD;
....@@ -497,13 +1279,14 @@
4971279 }
4981280 
4991281 #ifdef CONFIG_COMPAT
500
-static long max96722_compat_ioctl32(struct v4l2_subdev *sd,
501
-				   unsigned int cmd, unsigned long arg)
1282
+static long max96722_compat_ioctl32(struct v4l2_subdev *sd, unsigned int cmd,
1283
+				    unsigned long arg)
5021284 {
5031285 	void __user *up = compat_ptr(arg);
5041286 	struct rkmodule_inf *inf;
5051287 	struct rkmodule_awb_cfg *cfg;
5061288 	struct rkmodule_vicap_reset_info *vicap_rst_inf;
1289
+	struct rkmodule_csi_dphy_param *dphy_param;
5071290 	long ret = 0;
5081291 	int *seq;
5091292 	u32 stream = 0;
....@@ -547,7 +1330,8 @@
5471330 
5481331 		ret = max96722_ioctl(sd, cmd, vicap_rst_inf);
5491332 		if (!ret) {
550
-			ret = copy_to_user(up, vicap_rst_inf, sizeof(*vicap_rst_inf));
1333
+			ret = copy_to_user(up, vicap_rst_inf,
1334
+					   sizeof(*vicap_rst_inf));
5511335 			if (ret)
5521336 				ret = -EFAULT;
5531337 		}
....@@ -589,20 +1373,34 @@
5891373 		else
5901374 			ret = -EFAULT;
5911375 		break;
592
-	case RKMODULE_GET_CSI_DSI_INFO:
593
-		seq = kzalloc(sizeof(*seq), GFP_KERNEL);
594
-		if (!seq) {
1376
+	case RKMODULE_SET_CSI_DPHY_PARAM:
1377
+		dphy_param = kzalloc(sizeof(*dphy_param), GFP_KERNEL);
1378
+		if (!dphy_param) {
5951379 			ret = -ENOMEM;
5961380 			return ret;
5971381 		}
5981382 
599
-		ret = max96722_ioctl(sd, cmd, seq);
1383
+		ret = copy_from_user(dphy_param, up, sizeof(*dphy_param));
1384
+		if (!ret)
1385
+			ret = max96722_ioctl(sd, cmd, dphy_param);
1386
+		else
1387
+			ret = -EFAULT;
1388
+		kfree(dphy_param);
1389
+		break;
1390
+	case RKMODULE_GET_CSI_DPHY_PARAM:
1391
+		dphy_param = kzalloc(sizeof(*dphy_param), GFP_KERNEL);
1392
+		if (!dphy_param) {
1393
+			ret = -ENOMEM;
1394
+			return ret;
1395
+		}
1396
+
1397
+		ret = max96722_ioctl(sd, cmd, dphy_param);
6001398 		if (!ret) {
601
-			ret = copy_to_user(up, seq, sizeof(*seq));
1399
+			ret = copy_to_user(up, dphy_param, sizeof(*dphy_param));
6021400 			if (ret)
6031401 				ret = -EFAULT;
6041402 		}
605
-		kfree(seq);
1403
+		kfree(dphy_param);
6061404 		break;
6071405 	default:
6081406 		ret = -ENOIOCTLCMD;
....@@ -616,10 +1414,39 @@
6161414 static int __max96722_start_stream(struct max96722 *max96722)
6171415 {
6181416 	int ret;
1417
+	u32 link_freq_mhz, link_freq_idx;
6191418 
620
-	ret = max96722_write_array(max96722->client, max96722->cur_mode->reg_list);
1419
+	ret = max96722_check_link_lock_state(max96722);
6211420 	if (ret)
6221421 		return ret;
1422
+
1423
+	if (max96722->hot_plug_irq > 0)
1424
+		enable_irq(max96722->hot_plug_irq);
1425
+
1426
+	ret = max96722_write_array(max96722,
1427
+				   max96722->cur_mode->reg_list);
1428
+	if (ret)
1429
+		return ret;
1430
+
1431
+	link_freq_idx = max96722->cur_mode->link_freq_idx;
1432
+	link_freq_mhz = (u32)div_s64(link_freq_items[link_freq_idx], 1000000L);
1433
+	ret = max96722_dphy_dpll_predef_set(max96722, link_freq_mhz);
1434
+	if (ret)
1435
+		return ret;
1436
+
1437
+	if (max96722->auto_init_deskew_mask != 0) {
1438
+		ret = max96722_auto_init_deskew(max96722,
1439
+					max96722->auto_init_deskew_mask);
1440
+		if (ret)
1441
+			return ret;
1442
+	}
1443
+
1444
+	if (max96722->frame_sync_period != 0) {
1445
+		ret = max96722_frame_sync_period(max96722,
1446
+					max96722->frame_sync_period);
1447
+		if (ret)
1448
+			return ret;
1449
+	}
6231450 
6241451 	/* In case these controls are set before streaming */
6251452 	mutex_unlock(&max96722->mutex);
....@@ -628,31 +1455,28 @@
6281455 	if (ret)
6291456 		return ret;
6301457 
631
-	return max96722_write_reg(max96722->client,
632
-				 MAX96722_REG_CTRL_MODE,
633
-				 MAX96722_REG_VALUE_08BIT,
634
-				 MAX96722_MODE_STREAMING);
1458
+	return max96722_mipi_enable(max96722, true);
1459
+
6351460 }
6361461 
6371462 static int __max96722_stop_stream(struct max96722 *max96722)
6381463 {
639
-	return max96722_write_reg(max96722->client,
640
-				 MAX96722_REG_CTRL_MODE,
641
-				 MAX96722_REG_VALUE_08BIT,
642
-				 MAX96722_MODE_SW_STANDBY);
1464
+	if (max96722->hot_plug_irq > 0)
1465
+		disable_irq(max96722->hot_plug_irq);
1466
+
1467
+	return max96722_mipi_enable(max96722, false);
6431468 }
6441469 
6451470 static int max96722_s_stream(struct v4l2_subdev *sd, int on)
6461471 {
647
-	struct max96722 *max96722 = to_max96722(sd);
1472
+	struct max96722 *max96722 = v4l2_get_subdevdata(sd);
6481473 	struct i2c_client *client = max96722->client;
6491474 	int ret = 0;
6501475 
6511476 	dev_info(&client->dev, "%s: on: %d, %dx%d@%d\n", __func__, on,
652
-				max96722->cur_mode->width,
653
-				max96722->cur_mode->height,
1477
+		max96722->cur_mode->width, max96722->cur_mode->height,
6541478 		DIV_ROUND_CLOSEST(max96722->cur_mode->max_fps.denominator,
655
-				  max96722->cur_mode->max_fps.numerator));
1479
+				max96722->cur_mode->max_fps.numerator));
6561480 
6571481 	mutex_lock(&max96722->mutex);
6581482 	on = !!on;
....@@ -687,7 +1511,7 @@
6871511 
6881512 static int max96722_s_power(struct v4l2_subdev *sd, int on)
6891513 {
690
-	struct max96722 *max96722 = to_max96722(sd);
1514
+	struct max96722 *max96722 = v4l2_get_subdevdata(sd);
6911515 	struct i2c_client *client = max96722->client;
6921516 	int ret = 0;
6931517 
....@@ -728,14 +1552,19 @@
7281552 	u32 delay_us;
7291553 	struct device *dev = &max96722->client->dev;
7301554 
731
-	if (!IS_ERR(max96722->power_gpio))
1555
+	if (!IS_ERR(max96722->power_gpio)) {
7321556 		gpiod_set_value_cansleep(max96722->power_gpio, 1);
1557
+		usleep_range(5000, 10000);
1558
+	}
7331559 
734
-	usleep_range(1000, 2000);
1560
+	if (!IS_ERR(max96722->pocen_gpio)) {
1561
+		gpiod_set_value_cansleep(max96722->pocen_gpio, 1);
1562
+		usleep_range(5000, 10000);
1563
+	}
7351564 
7361565 	if (!IS_ERR_OR_NULL(max96722->pins_default)) {
7371566 		ret = pinctrl_select_state(max96722->pinctrl,
738
-					   max96722->pins_default);
1567
+					max96722->pins_default);
7391568 		if (ret < 0)
7401569 			dev_err(dev, "could not set pins\n");
7411570 	}
....@@ -748,11 +1577,11 @@
7481577 		dev_err(dev, "Failed to enable regulators\n");
7491578 		goto disable_clk;
7501579 	}
751
-
752
-	if (!IS_ERR(max96722->reset_gpio))
1580
+	if (!IS_ERR(max96722->reset_gpio)) {
7531581 		gpiod_set_value_cansleep(max96722->reset_gpio, 1);
1582
+		usleep_range(500, 1000);
1583
+	}
7541584 
755
-	usleep_range(500, 1000);
7561585 	if (!IS_ERR(max96722->pwdn_gpio))
7571586 		gpiod_set_value_cansleep(max96722->pwdn_gpio, 1);
7581587 
....@@ -776,6 +1605,7 @@
7761605 	if (!IS_ERR(max96722->pwdn_gpio))
7771606 		gpiod_set_value_cansleep(max96722->pwdn_gpio, 0);
7781607 	clk_disable_unprepare(max96722->xvclk);
1608
+
7791609 	if (!IS_ERR(max96722->reset_gpio))
7801610 		gpiod_set_value_cansleep(max96722->reset_gpio, 0);
7811611 
....@@ -785,17 +1615,21 @@
7851615 		if (ret < 0)
7861616 			dev_dbg(dev, "could not set pins\n");
7871617 	}
788
-	if (!IS_ERR(max96722->power_gpio))
789
-		gpiod_set_value_cansleep(max96722->power_gpio, 0);
7901618 
7911619 	regulator_bulk_disable(MAX96722_NUM_SUPPLIES, max96722->supplies);
1620
+
1621
+	if (!IS_ERR(max96722->pocen_gpio))
1622
+		gpiod_set_value_cansleep(max96722->pocen_gpio, 0);
1623
+
1624
+	if (!IS_ERR(max96722->power_gpio))
1625
+		gpiod_set_value_cansleep(max96722->power_gpio, 0);
7921626 }
7931627 
7941628 static int max96722_runtime_resume(struct device *dev)
7951629 {
7961630 	struct i2c_client *client = to_i2c_client(dev);
7971631 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
798
-	struct max96722 *max96722 = to_max96722(sd);
1632
+	struct max96722 *max96722 = v4l2_get_subdevdata(sd);
7991633 
8001634 	return __max96722_power_on(max96722);
8011635 }
....@@ -804,7 +1638,7 @@
8041638 {
8051639 	struct i2c_client *client = to_i2c_client(dev);
8061640 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
807
-	struct max96722 *max96722 = to_max96722(sd);
1641
+	struct max96722 *max96722 = v4l2_get_subdevdata(sd);
8081642 
8091643 	__max96722_power_off(max96722);
8101644 
....@@ -814,16 +1648,16 @@
8141648 #ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
8151649 static int max96722_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
8161650 {
817
-	struct max96722 *max96722 = to_max96722(sd);
1651
+	struct max96722 *max96722 = v4l2_get_subdevdata(sd);
8181652 	struct v4l2_mbus_framefmt *try_fmt =
819
-				v4l2_subdev_get_try_format(sd, fh->pad, 0);
820
-	const struct max96722_mode *def_mode = &supported_modes[0];
1653
+		v4l2_subdev_get_try_format(sd, fh->pad, 0);
1654
+	const struct max96722_mode *def_mode = &max96722->supported_modes[0];
8211655 
8221656 	mutex_lock(&max96722->mutex);
8231657 	/* Initialize try_fmt */
8241658 	try_fmt->width = def_mode->width;
8251659 	try_fmt->height = def_mode->height;
826
-	try_fmt->code = MAX96722_MEDIA_BUS_FMT;
1660
+	try_fmt->code = def_mode->bus_fmt;
8271661 	try_fmt->field = V4L2_FIELD_NONE;
8281662 
8291663 	mutex_unlock(&max96722->mutex);
....@@ -833,38 +1667,60 @@
8331667 }
8341668 #endif
8351669 
836
-static int max96722_enum_frame_interval(struct v4l2_subdev *sd,
837
-				       struct v4l2_subdev_pad_config *cfg,
838
-				       struct v4l2_subdev_frame_interval_enum *fie)
1670
+static int
1671
+max96722_enum_frame_interval(struct v4l2_subdev *sd,
1672
+			     struct v4l2_subdev_pad_config *cfg,
1673
+			     struct v4l2_subdev_frame_interval_enum *fie)
8391674 {
840
-	if (fie->index >= ARRAY_SIZE(supported_modes))
1675
+	struct max96722 *max96722 = v4l2_get_subdevdata(sd);
1676
+
1677
+	if (fie->index >= max96722->cfg_modes_num)
8411678 		return -EINVAL;
8421679 
843
-	fie->code = MAX96722_MEDIA_BUS_FMT;
844
-
845
-	fie->width = supported_modes[fie->index].width;
846
-	fie->height = supported_modes[fie->index].height;
847
-	fie->interval = supported_modes[fie->index].max_fps;
1680
+	fie->code = max96722->supported_modes[fie->index].bus_fmt;
1681
+	fie->width = max96722->supported_modes[fie->index].width;
1682
+	fie->height = max96722->supported_modes[fie->index].height;
1683
+	fie->interval = max96722->supported_modes[fie->index].max_fps;
8481684 
8491685 	return 0;
8501686 }
8511687 
8521688 static int max96722_g_mbus_config(struct v4l2_subdev *sd, unsigned int pad,
853
-				struct v4l2_mbus_config *config)
1689
+				  struct v4l2_mbus_config *config)
8541690 {
1691
+	struct max96722 *max96722 = v4l2_get_subdevdata(sd);
1692
+	u32 val = 0;
1693
+	u8 data_lanes = max96722->bus_cfg.bus.mipi_csi2.num_data_lanes;
1694
+
1695
+	val |= V4L2_MBUS_CSI2_CONTINUOUS_CLOCK;
1696
+	val |= (1 << (data_lanes - 1));
1697
+	switch (data_lanes) {
1698
+	case 4:
1699
+		val |= V4L2_MBUS_CSI2_CHANNEL_3;
1700
+		fallthrough;
1701
+	case 3:
1702
+		val |= V4L2_MBUS_CSI2_CHANNEL_2;
1703
+		fallthrough;
1704
+	case 2:
1705
+		val |= V4L2_MBUS_CSI2_CHANNEL_1;
1706
+		fallthrough;
1707
+	case 1:
1708
+	default:
1709
+		val |= V4L2_MBUS_CSI2_CHANNEL_0;
1710
+		break;
1711
+	}
1712
+
8551713 	config->type = V4L2_MBUS_CSI2_DPHY;
856
-	config->flags = V4L2_MBUS_CSI2_4_LANE |
857
-			V4L2_MBUS_CSI2_CHANNEL_0 |
858
-			V4L2_MBUS_CSI2_CONTINUOUS_CLOCK;
1714
+	config->flags = val;
8591715 
8601716 	return 0;
8611717 }
8621718 
8631719 static int max96722_get_selection(struct v4l2_subdev *sd,
864
-				struct v4l2_subdev_pad_config *cfg,
865
-				struct v4l2_subdev_selection *sel)
1720
+				  struct v4l2_subdev_pad_config *cfg,
1721
+				  struct v4l2_subdev_selection *sel)
8661722 {
867
-	struct max96722 *max96722 = to_max96722(sd);
1723
+	struct max96722 *max96722 = v4l2_get_subdevdata(sd);
8681724 
8691725 	if (sel->target == V4L2_SEL_TGT_CROP_BOUNDS) {
8701726 		sel->r.left = 0;
....@@ -877,10 +1733,8 @@
8771733 	return -EINVAL;
8781734 }
8791735 
880
-static const struct dev_pm_ops max96722_pm_ops = {
881
-	SET_RUNTIME_PM_OPS(max96722_runtime_suspend,
882
-			   max96722_runtime_resume, NULL)
883
-};
1736
+static const struct dev_pm_ops max96722_pm_ops = { SET_RUNTIME_PM_OPS(
1737
+	max96722_runtime_suspend, max96722_runtime_resume, NULL) };
8841738 
8851739 #ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
8861740 static const struct v4l2_subdev_internal_ops max96722_internal_ops = {
....@@ -912,15 +1766,17 @@
9121766 };
9131767 
9141768 static const struct v4l2_subdev_ops max96722_subdev_ops = {
915
-	.core	= &max96722_core_ops,
916
-	.video	= &max96722_video_ops,
917
-	.pad	= &max96722_pad_ops,
1769
+	.core = &max96722_core_ops,
1770
+	.video = &max96722_video_ops,
1771
+	.pad = &max96722_pad_ops,
9181772 };
9191773 
9201774 static int max96722_initialize_controls(struct max96722 *max96722)
9211775 {
9221776 	const struct max96722_mode *mode;
9231777 	struct v4l2_ctrl_handler *handler;
1778
+	u64 pixel_rate;
1779
+	u8 data_lanes;
9241780 	int ret;
9251781 
9261782 	handler = &max96722->ctrl_handler;
....@@ -932,21 +1788,25 @@
9321788 	handler->lock = &max96722->mutex;
9331789 
9341790 	max96722->link_freq = v4l2_ctrl_new_int_menu(handler, NULL,
935
-			V4L2_CID_LINK_FREQ,
936
-			1, 0, link_freq_items);
1791
+				V4L2_CID_LINK_FREQ,
1792
+				ARRAY_SIZE(link_freq_items) - 1, 0,
1793
+				link_freq_items);
1794
+	__v4l2_ctrl_s_ctrl(max96722->link_freq, mode->link_freq_idx);
1795
+	dev_info(&max96722->client->dev, "mipi_freq_idx = %d, mipi_link_freq = %lld\n",
1796
+			mode->link_freq_idx, link_freq_items[mode->link_freq_idx]);
9371797 
938
-	max96722->pixel_rate = v4l2_ctrl_new_std(handler, NULL,
939
-			V4L2_CID_PIXEL_RATE,
940
-			0, MAX96722_PIXEL_RATE,
941
-			1, MAX96722_PIXEL_RATE);
942
-
943
-	__v4l2_ctrl_s_ctrl(max96722->link_freq,
944
-			   mode->link_freq_idx);
1798
+	/* pixel rate = link frequency * 2 * lanes / BITS_PER_SAMPLE */
1799
+	data_lanes = max96722->bus_cfg.bus.mipi_csi2.num_data_lanes;
1800
+	pixel_rate = (u32)link_freq_items[mode->link_freq_idx] / mode->bpp * 2 * data_lanes;
1801
+	max96722->pixel_rate =
1802
+		v4l2_ctrl_new_std(handler, NULL, V4L2_CID_PIXEL_RATE, 0,
1803
+				  pixel_rate, 1, pixel_rate);
1804
+	dev_info(&max96722->client->dev, "pixel_rate = %lld, bpp = %d\n",
1805
+			pixel_rate, mode->bpp);
9451806 
9461807 	if (handler->error) {
9471808 		ret = handler->error;
948
-		dev_err(&max96722->client->dev,
949
-			"Failed to init controls(%d)\n", ret);
1809
+		dev_err(&max96722->client->dev, "Failed to init controls(%d)\n", ret);
9501810 		goto err_free_handler;
9511811 	}
9521812 
....@@ -960,25 +1820,6 @@
9601820 	return ret;
9611821 }
9621822 
963
-static int max96722_check_sensor_id(struct max96722 *max96722,
964
-				   struct i2c_client *client)
965
-{
966
-	struct device *dev = &max96722->client->dev;
967
-	u32 id = 0;
968
-	int ret;
969
-
970
-	ret = max96722_read_reg(client, MAX96722_REG_CHIP_ID,
971
-			       MAX96722_REG_VALUE_08BIT, &id);
972
-	if (id != CHIP_ID) {
973
-		dev_err(dev, "Unexpected sensor id(%02x), ret(%d)\n", id, ret);
974
-		return -ENODEV;
975
-	}
976
-
977
-	dev_info(dev, "Detected %02x sensor\n", id);
978
-
979
-	return 0;
980
-}
981
-
9821823 static int max96722_configure_regulators(struct max96722 *max96722)
9831824 {
9841825 	unsigned int i;
....@@ -987,44 +1828,132 @@
9871828 		max96722->supplies[i].supply = max96722_supply_names[i];
9881829 
9891830 	return devm_regulator_bulk_get(&max96722->client->dev,
990
-					MAX96722_NUM_SUPPLIES,
991
-					max96722->supplies);
1831
+				       MAX96722_NUM_SUPPLIES,
1832
+				       max96722->supplies);
1833
+}
1834
+
1835
+static int max96722_parse_dt(struct max96722 *max96722)
1836
+{
1837
+	struct device *dev = &max96722->client->dev;
1838
+	struct device_node *node = dev->of_node;
1839
+	u8 mipi_data_lanes = max96722->bus_cfg.bus.mipi_csi2.num_data_lanes;
1840
+	u32 value = 0;
1841
+	int ret = 0;
1842
+
1843
+	/* serializer i2c address */
1844
+	ret = of_property_read_u32(node, "ser-i2c-addr", &value);
1845
+	if (ret) {
1846
+		max96722->i2c_addr[I2C_DEV_SER] = SER_I2C_ADDR;
1847
+	} else {
1848
+		dev_info(dev, "ser-i2c-addr property: %d\n", value);
1849
+		max96722->i2c_addr[I2C_DEV_SER] = value;
1850
+	}
1851
+	dev_info(dev, "serializer i2c address: 0x%02x\n", max96722->i2c_addr[I2C_DEV_SER]);
1852
+
1853
+	/* max96722 link mask:
1854
+	 *     bit[3:0] = link enable mask: 0 = disable, 1 = enable:
1855
+	 *         bit0 - LinkA, bit1 - LinkB, bit2 - LinkC, bit3 - LinkD
1856
+	 *     bit[7:4] = link type, 0 = GMSL1, 1 = GMSL2:
1857
+	 *         bit4 - LinkA, bit5 - LinkB, bit6 - LinkC, bit7 = LinkD
1858
+	 */
1859
+	ret = of_property_read_u32(node, "link-mask", &max96722->link_mask);
1860
+	if (ret) {
1861
+		/* default link mask */
1862
+		if (mipi_data_lanes == 4)
1863
+			max96722->link_mask = 0xFF; /* Link A/B/C/D: GMSL2 and enable */
1864
+		else
1865
+			max96722->link_mask = 0x33; /* Link A/B: GMSL2 and enable */
1866
+	} else {
1867
+		dev_info(dev, "link-mask property: 0x%08x\n", max96722->link_mask);
1868
+	}
1869
+	dev_info(dev, "serdes link mask: 0x%02x\n", max96722->link_mask);
1870
+
1871
+	/* auto initial deskew mask */
1872
+	ret = of_property_read_u32(node, "auto-init-deskew-mask",
1873
+				&max96722->auto_init_deskew_mask);
1874
+	if (ret)
1875
+		max96722->auto_init_deskew_mask = 0x0F; // 0x0F: default enable all
1876
+	dev_info(dev, "auto init deskew mask: 0x%02x\n", max96722->auto_init_deskew_mask);
1877
+
1878
+	/* FSYNC period config */
1879
+	ret = of_property_read_u32(node, "frame-sync-period",
1880
+				&max96722->frame_sync_period);
1881
+	if (ret)
1882
+		max96722->frame_sync_period = 0; // 0: disable (default)
1883
+	dev_info(dev, "frame sync period: %d\n", max96722->frame_sync_period);
1884
+
1885
+	return 0;
9921886 }
9931887 
9941888 static int max96722_probe(struct i2c_client *client,
995
-			 const struct i2c_device_id *id)
1889
+			  const struct i2c_device_id *id)
9961890 {
9971891 	struct device *dev = &client->dev;
9981892 	struct device_node *node = dev->of_node;
9991893 	struct max96722 *max96722;
10001894 	struct v4l2_subdev *sd;
1895
+	struct device_node *endpoint;
10011896 	char facing[2];
1897
+	u8 mipi_data_lanes;
10021898 	int ret;
10031899 
1004
-	dev_info(dev, "driver version: %02x.%02x.%02x",
1005
-		DRIVER_VERSION >> 16,
1006
-		(DRIVER_VERSION & 0xff00) >> 8,
1007
-		DRIVER_VERSION & 0x00ff);
1900
+	dev_info(dev, "driver version: %02x.%02x.%02x", DRIVER_VERSION >> 16,
1901
+		 (DRIVER_VERSION & 0xff00) >> 8, DRIVER_VERSION & 0x00ff);
10081902 
10091903 	max96722 = devm_kzalloc(dev, sizeof(*max96722), GFP_KERNEL);
10101904 	if (!max96722)
10111905 		return -ENOMEM;
10121906 
10131907 	ret = of_property_read_u32(node, RKMODULE_CAMERA_MODULE_INDEX,
1014
-				   &max96722->module_index);
1908
+					&max96722->module_index);
10151909 	ret |= of_property_read_string(node, RKMODULE_CAMERA_MODULE_FACING,
1016
-				       &max96722->module_facing);
1910
+					&max96722->module_facing);
10171911 	ret |= of_property_read_string(node, RKMODULE_CAMERA_MODULE_NAME,
1018
-				       &max96722->module_name);
1912
+					&max96722->module_name);
10191913 	ret |= of_property_read_string(node, RKMODULE_CAMERA_LENS_NAME,
1020
-				       &max96722->len_name);
1914
+					&max96722->len_name);
10211915 	if (ret) {
10221916 		dev_err(dev, "could not get module information!\n");
10231917 		return -EINVAL;
10241918 	}
10251919 
1920
+	max96722->regmap = devm_regmap_init_i2c(client, &max96722_regmap_config);
1921
+	if (IS_ERR(max96722->regmap)) {
1922
+		dev_err(dev, "Failed to regmap initialize I2C\n");
1923
+		return PTR_ERR(max96722->regmap);
1924
+	}
1925
+
10261926 	max96722->client = client;
1027
-	max96722->cur_mode = &supported_modes[0];
1927
+	i2c_set_clientdata(client, max96722);
1928
+
1929
+	/* i2c default address init */
1930
+	max96722->i2c_addr[I2C_DEV_DES] = client->addr;
1931
+	max96722->i2c_addr[I2C_DEV_SER] = SER_I2C_ADDR;
1932
+	max96722->i2c_addr[I2C_DEV_CAM] = CAM_I2C_ADDR;
1933
+
1934
+	endpoint = of_graph_get_next_endpoint(dev->of_node, NULL);
1935
+	if (!endpoint) {
1936
+		dev_err(dev, "Failed to get endpoint\n");
1937
+		return -EINVAL;
1938
+	}
1939
+
1940
+	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(endpoint),
1941
+				&max96722->bus_cfg);
1942
+	if (ret) {
1943
+		dev_err(dev, "Failed to get bus config\n");
1944
+		return -EINVAL;
1945
+	}
1946
+	mipi_data_lanes = max96722->bus_cfg.bus.mipi_csi2.num_data_lanes;
1947
+	dev_info(dev, "mipi csi2 phy data lanes %d\n", mipi_data_lanes);
1948
+
1949
+	if (mipi_data_lanes == 4) {
1950
+		max96722->supported_modes = supported_modes_4lane;
1951
+		max96722->cfg_modes_num = ARRAY_SIZE(supported_modes_4lane);
1952
+	} else {
1953
+		dev_err(dev, "Not support mipi data lane: %d\n", mipi_data_lanes);
1954
+		return -EINVAL;
1955
+	}
1956
+	max96722->cur_mode = &max96722->supported_modes[0];
10281957 
10291958 	max96722->power_gpio = devm_gpiod_get(dev, "power", GPIOD_OUT_LOW);
10301959 	if (IS_ERR(max96722->power_gpio))
....@@ -1038,6 +1967,14 @@
10381967 	if (IS_ERR(max96722->pwdn_gpio))
10391968 		dev_warn(dev, "Failed to get pwdn-gpios\n");
10401969 
1970
+	max96722->pocen_gpio = devm_gpiod_get(dev, "pocen", GPIOD_OUT_LOW);
1971
+	if (IS_ERR(max96722->pocen_gpio))
1972
+		dev_warn(dev, "Failed to get pocen-gpios\n");
1973
+
1974
+	max96722->lock_gpio = devm_gpiod_get(dev, "lock", GPIOD_IN);
1975
+	if (IS_ERR(max96722->lock_gpio))
1976
+		dev_warn(dev, "Failed to get lock-gpios\n");
1977
+
10411978 	ret = max96722_configure_regulators(max96722);
10421979 	if (ret) {
10431980 		dev_err(dev, "Failed to get power regulators\n");
....@@ -1046,18 +1983,18 @@
10461983 
10471984 	max96722->pinctrl = devm_pinctrl_get(dev);
10481985 	if (!IS_ERR(max96722->pinctrl)) {
1049
-		max96722->pins_default =
1050
-			pinctrl_lookup_state(max96722->pinctrl,
1051
-					     OF_CAMERA_PINCTRL_STATE_DEFAULT);
1986
+		max96722->pins_default = pinctrl_lookup_state(
1987
+			max96722->pinctrl, OF_CAMERA_PINCTRL_STATE_DEFAULT);
10521988 		if (IS_ERR(max96722->pins_default))
10531989 			dev_err(dev, "could not get default pinstate\n");
10541990 
1055
-		max96722->pins_sleep =
1056
-			pinctrl_lookup_state(max96722->pinctrl,
1057
-					     OF_CAMERA_PINCTRL_STATE_SLEEP);
1991
+		max96722->pins_sleep = pinctrl_lookup_state(
1992
+			max96722->pinctrl, OF_CAMERA_PINCTRL_STATE_SLEEP);
10581993 		if (IS_ERR(max96722->pins_sleep))
10591994 			dev_err(dev, "could not get sleep pinstate\n");
10601995 	}
1996
+
1997
+	max96722_parse_dt(max96722);
10611998 
10621999 	mutex_init(&max96722->mutex);
10632000 
....@@ -1071,16 +2008,7 @@
10712008 	if (ret)
10722009 		goto err_free_handler;
10732010 
1074
-	ret = max96722_write_reg(max96722->client,
1075
-				 MAX96722_REMOTE_CTRL,
1076
-				 MAX96722_REG_VALUE_08BIT,
1077
-				 MAX96722_REMOTE_DISABLE);
1078
-	if (ret) {
1079
-		dev_err(dev, "disable i2c remote control error\n");
1080
-		goto err_power_off;
1081
-	}
1082
-
1083
-	ret = max96722_check_sensor_id(max96722, client);
2011
+	ret = max96722_check_local_chipid(max96722);
10842012 	if (ret)
10852013 		goto err_power_off;
10862014 
....@@ -1102,13 +2030,36 @@
11022030 	else
11032031 		facing[0] = 'f';
11042032 
2033
+	v4l2_set_subdevdata(sd, max96722);
2034
+
11052035 	snprintf(sd->name, sizeof(sd->name), "m%02d_%s_%s %s",
1106
-		 max96722->module_index, facing,
1107
-		 MAX96722_NAME, dev_name(sd->dev));
2036
+		max96722->module_index, facing, MAX96722_NAME,
2037
+		dev_name(sd->dev));
11082038 	ret = v4l2_async_register_subdev_sensor_common(sd);
11092039 	if (ret) {
11102040 		dev_err(dev, "v4l2 async register subdev failed\n");
11112041 		goto err_clean_entity;
2042
+	}
2043
+
2044
+	if (!IS_ERR(max96722->lock_gpio)) {
2045
+		max96722->hot_plug_irq = gpiod_to_irq(max96722->lock_gpio);
2046
+		if (max96722->hot_plug_irq < 0) {
2047
+			dev_err(dev, "failed to get hot plug irq\n");
2048
+		} else {
2049
+			ret = devm_request_threaded_irq(dev,
2050
+					max96722->hot_plug_irq,
2051
+					NULL,
2052
+					max96722_hot_plug_detect_irq_handler,
2053
+					IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING | IRQF_ONESHOT,
2054
+					"max96722_hot_plug",
2055
+					max96722);
2056
+			if (ret) {
2057
+				dev_err(dev, "failed to request hot plug irq (%d)\n", ret);
2058
+				max96722->hot_plug_irq = -1;
2059
+			} else {
2060
+				disable_irq(max96722->hot_plug_irq);
2061
+			}
2062
+		}
11122063 	}
11132064 
11142065 	pm_runtime_set_active(dev);
....@@ -1134,7 +2085,7 @@
11342085 static int max96722_remove(struct i2c_client *client)
11352086 {
11362087 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
1137
-	struct max96722 *max96722 = to_max96722(sd);
2088
+	struct max96722 *max96722 = v4l2_get_subdevdata(sd);
11382089 
11392090 	v4l2_async_unregister_subdev(sd);
11402091 #if defined(CONFIG_MEDIA_CONTROLLER)
....@@ -1185,7 +2136,7 @@
11852136 	i2c_del_driver(&max96722_i2c_driver);
11862137 }
11872138 
1188
-device_initcall_sync(sensor_mod_init);
2139
+module_init(sensor_mod_init);
11892140 module_exit(sensor_mod_exit);
11902141 
11912142 MODULE_DESCRIPTION("Maxim max96722 deserializer driver");