1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Driver for STM32 Camera Serial Interface
4  *
5  * Copyright (C) STMicroelectronics SA 2024
6  * Author: Alain Volmat <alain.volmat@foss.st.com>
7  * for STMicroelectronics.
8  */
9 
10 #include <linux/clk.h>
11 #include <linux/delay.h>
12 #include <linux/io.h>
13 #include <linux/iopoll.h>
14 #include <linux/module.h>
15 #include <linux/platform_device.h>
16 #include <linux/pm_runtime.h>
17 #include <linux/reset.h>
18 #include <linux/slab.h>
19 
20 #include <media/mipi-csi2.h>
21 #include <media/v4l2-fwnode.h>
22 #include <media/v4l2-subdev.h>
23 
24 #define STM32_CSI_CR				0x0000
25 #define STM32_CSI_CR_CSIEN			BIT(0)
26 #define STM32_CSI_CR_VCXSTART(x)		BIT(2 + ((x) * 4))
27 #define STM32_CSI_CR_VCXSTOP(x)			BIT(3 + ((x) * 4))
28 #define STM32_CSI_PCR				0x0004
29 #define STM32_CSI_PCR_DL1EN			BIT(3)
30 #define STM32_CSI_PCR_DL0EN			BIT(2)
31 #define STM32_CSI_PCR_CLEN			BIT(1)
32 #define STM32_CSI_PCR_PWRDOWN			BIT(0)
33 #define STM32_CSI_VCXCFGR1(x)			((((x) + 1) * 0x0010) + 0x0)
34 #define STM32_CSI_VCXCFGR1_ALLDT		BIT(0)
35 #define STM32_CSI_VCXCFGR1_DT0EN		BIT(1)
36 #define STM32_CSI_VCXCFGR1_DT1EN		BIT(2)
37 #define STM32_CSI_VCXCFGR1_CDTFT_SHIFT		8
38 #define STM32_CSI_VCXCFGR1_DT0_SHIFT		16
39 #define STM32_CSI_VCXCFGR1_DT0FT_SHIFT		24
40 #define STM32_CSI_VCXCFGR2(x)			((((x) + 1) * 0x0010) + 0x4)
41 #define STM32_CSI_VCXCFGR2_DT1_SHIFT		0
42 #define STM32_CSI_VCXCFGR2_DT1FT_SHIFT		8
43 #define STM32_CSI_INPUT_BPP8			2
44 #define STM32_CSI_INPUT_BPP10			3
45 #define STM32_CSI_INPUT_BPP12			4
46 #define STM32_CSI_INPUT_BPP14			5
47 #define STM32_CSI_LMCFGR			0x0070
48 #define STM32_CSI_LMCFGR_LANENB_SHIFT		8
49 #define STM32_CSI_LMCFGR_DLMAP_SHIFT		16
50 #define STM32_CSI_IER0				0x0080
51 #define STM32_CSI_IER1				0x0084
52 #define STM32_CSI_SR0				0x0090
53 #define STM32_CSI_SR0_SYNCERRF			BIT(30)
54 #define STM32_CSI_SR0_SPKTERRF			BIT(28)
55 #define STM32_CSI_SR0_IDERRF			BIT(27)
56 #define STM32_CSI_SR0_CECCERRF			BIT(26)
57 #define STM32_CSI_SR0_ECCERRF			BIT(25)
58 #define STM32_CSI_SR0_CRCERRF			BIT(24)
59 #define STM32_CSI_SR0_CCFIFOFF			BIT(21)
60 #define STM32_CSI_SR0_VCXSTATEF(x)		BIT(17 + (x))
61 #define STM32_CSI_SR1				0x0094
62 #define STM32_CSI_SR1_ECTRLDL1F			BIT(12)
63 #define STM32_CSI_SR1_ESYNCESCDL1F		BIT(11)
64 #define STM32_CSI_SR1_EESCDL1F			BIT(10)
65 #define STM32_CSI_SR1_ESOTSYNCDL1F		BIT(9)
66 #define STM32_CSI_SR1_ESOTDL1F			BIT(8)
67 #define STM32_CSI_SR1_ECTRLDL0F			BIT(4)
68 #define STM32_CSI_SR1_ESYNCESCDL0F		BIT(3)
69 #define STM32_CSI_SR1_EESCDL0F			BIT(2)
70 #define STM32_CSI_SR1_ESOTSYNCDL0F		BIT(1)
71 #define STM32_CSI_SR1_ESOTDL0F			BIT(0)
72 #define STM32_CSI_FCR0				0x0100
73 #define STM32_CSI_FCR1				0x0104
74 #define STM32_CSI_SPDFR				0x0110
75 #define STM32_CSI_DT_MASK			0x3f
76 #define STM32_CSI_VC_MASK			0x03
77 #define STM32_CSI_ERR1				0x0114
78 #define STM32_CSI_ERR1_IDVCERR_SHIFT		22
79 #define STM32_CSI_ERR1_IDDTERR_SHIFT		16
80 #define STM32_CSI_ERR1_CECCVCERR_SHIFT		14
81 #define STM32_CSI_ERR1_CECCDTERR_SHIFT		8
82 #define STM32_CSI_ERR1_CRCVCERR_SHIFT		6
83 #define STM32_CSI_ERR1_CRCDTERR_SHIFT		0
84 #define STM32_CSI_ERR2				0x0118
85 #define STM32_CSI_ERR2_SYNCVCERR_SHIFT		18
86 #define STM32_CSI_ERR2_SPKTVCERR_SHIFT		6
87 #define STM32_CSI_ERR2_SPKTDTERR_SHIFT		0
88 #define STM32_CSI_PRCR				0x1000
89 #define STM32_CSI_PRCR_PEN			BIT(1)
90 #define STM32_CSI_PMCR				0x1004
91 #define STM32_CSI_PFCR				0x1008
92 #define STM32_CSI_PFCR_CCFR_MASK		GENMASK(5, 0)
93 #define STM32_CSI_PFCR_CCFR_SHIFT		0
94 #define STM32_CSI_PFCR_HSFR_MASK		GENMASK(14, 8)
95 #define STM32_CSI_PFCR_HSFR_SHIFT		8
96 #define STM32_CSI_PFCR_DLD			BIT(16)
97 #define STM32_CSI_PTCR0				0x1010
98 #define STM32_CSI_PTCR0_TCKEN			BIT(0)
99 #define STM32_CSI_PTCR1				0x1014
100 #define STM32_CSI_PTCR1_TWM			BIT(16)
101 #define STM32_CSI_PTCR1_TDI_MASK		GENMASK(7, 0)
102 #define STM32_CSI_PTCR1_TDI_SHIFT		0
103 #define STM32_CSI_PTSR				0x1018
104 
105 #define STM32_CSI_LANES_MAX	2
106 
107 #define STM32_CSI_SR0_ERRORS	(STM32_CSI_SR0_SYNCERRF | STM32_CSI_SR0_SPKTERRF |\
108 				 STM32_CSI_SR0_IDERRF | STM32_CSI_SR0_CECCERRF |\
109 				 STM32_CSI_SR0_ECCERRF | STM32_CSI_SR0_CRCERRF |\
110 				 STM32_CSI_SR0_CCFIFOFF)
111 #define STM32_CSI_SR1_DL0_ERRORS	(STM32_CSI_SR1_ECTRLDL0F | STM32_CSI_SR1_ESYNCESCDL0F |\
112 				 STM32_CSI_SR1_EESCDL0F | STM32_CSI_SR1_ESOTSYNCDL0F |\
113 				 STM32_CSI_SR1_ESOTDL0F)
114 #define STM32_CSI_SR1_DL1_ERRORS	(STM32_CSI_SR1_ECTRLDL1F | STM32_CSI_SR1_ESYNCESCDL1F |\
115 				 STM32_CSI_SR1_EESCDL1F | STM32_CSI_SR1_ESOTSYNCDL1F |\
116 				 STM32_CSI_SR1_ESOTDL1F)
117 #define STM32_CSI_SR1_ERRORS	(STM32_CSI_SR1_DL0_ERRORS | STM32_CSI_SR1_DL1_ERRORS)
118 
119 enum stm32_csi_pads {
120 	STM32_CSI_PAD_SINK,
121 	STM32_CSI_PAD_SOURCE,
122 	STM32_CSI_PAD_MAX,
123 };
124 
125 struct stm32_csi_event {
126 	u32 mask;
127 	const char * const name;
128 };
129 
130 static const struct stm32_csi_event stm32_csi_events_sr0[] = {
131 	{STM32_CSI_SR0_SYNCERRF,	"Synchronization error"},
132 	{STM32_CSI_SR0_SPKTERRF,	"Short packet error"},
133 	{STM32_CSI_SR0_IDERRF,		"Data type ID error"},
134 	{STM32_CSI_SR0_CECCERRF,	"Corrected ECC error"},
135 	{STM32_CSI_SR0_ECCERRF,		"ECC error"},
136 	{STM32_CSI_SR0_CRCERRF,		"CRC error"},
137 	{STM32_CSI_SR0_CCFIFOFF,	"Clk changer FIFO full error"},
138 };
139 
140 #define STM32_CSI_NUM_SR0_EVENTS ARRAY_SIZE(stm32_csi_events_sr0)
141 
142 static const struct stm32_csi_event stm32_csi_events_sr1[] = {
143 	{STM32_CSI_SR1_ECTRLDL1F,	"L1: D-PHY control error"},
144 	{STM32_CSI_SR1_ESYNCESCDL1F,
145 		"L1: D-PHY low power data transmission synchro error"},
146 	{STM32_CSI_SR1_EESCDL1F,	"L1: D-PHY escape entry error"},
147 	{STM32_CSI_SR1_ESOTSYNCDL1F,
148 		"L1: Start of transmission synchro error"},
149 	{STM32_CSI_SR1_ESOTDL1F,	"L1: Start of transmission error"},
150 	{STM32_CSI_SR1_ECTRLDL0F,	"L0: D-PHY control error"},
151 	{STM32_CSI_SR1_ESYNCESCDL0F,
152 		"L0: D-PHY low power data transmission synchro error"},
153 	{STM32_CSI_SR1_EESCDL0F,	"L0: D-PHY escape entry error"},
154 	{STM32_CSI_SR1_ESOTSYNCDL0F,
155 		"L0: Start of transmission synchro error"},
156 	{STM32_CSI_SR1_ESOTDL0F,	"L0: Start of transmission error"},
157 };
158 
159 #define STM32_CSI_NUM_SR1_EVENTS ARRAY_SIZE(stm32_csi_events_sr1)
160 
161 enum stm32_csi_clk {
162 	STM32_CSI_CLK_PCLK,
163 	STM32_CSI_CLK_TXESC,
164 	STM32_CSI_CLK_CSI2PHY,
165 	STM32_CSI_CLK_NB,
166 };
167 
168 static const char * const stm32_csi_clks_id[] = {
169 	"pclk",
170 	"txesc",
171 	"csi2phy",
172 };
173 
174 struct stm32_csi_dev {
175 	struct device			*dev;
176 
177 	void __iomem			*base;
178 
179 	struct clk_bulk_data		clks[STM32_CSI_CLK_NB];
180 	struct regulator_bulk_data	supplies[2];
181 
182 	u8				lanes[STM32_CSI_LANES_MAX];
183 	u8				num_lanes;
184 
185 	/*
186 	 * spinlock slock is used to protect to srX_counters tables being
187 	 * accessed from log_status and interrupt context
188 	 */
189 	spinlock_t			slock;
190 
191 	u32				sr0_counters[STM32_CSI_NUM_SR0_EVENTS];
192 	u32				sr1_counters[STM32_CSI_NUM_SR1_EVENTS];
193 
194 	struct v4l2_subdev		sd;
195 	struct v4l2_async_notifier	notifier;
196 	struct media_pad		pads[STM32_CSI_PAD_MAX];
197 
198 	/* Remote source */
199 	struct v4l2_subdev		*s_subdev;
200 	u32				s_subdev_pad_nb;
201 };
202 
203 struct stm32_csi_fmts {
204 	u32 code;
205 	u32 datatype;
206 	u32 input_fmt;
207 	u8 bpp;
208 };
209 
210 #define FMT_MBUS_DT_DTFMT_BPP(mbus, dt, input, byteperpixel)		\
211 	{								\
212 		.code = MEDIA_BUS_FMT_##mbus,				\
213 		.datatype = MIPI_CSI2_DT_##dt,				\
214 		.input_fmt = STM32_CSI_INPUT_##input,	\
215 		.bpp = byteperpixel,					\
216 	}
217 static const struct stm32_csi_fmts stm32_csi_formats[] = {
218 	/* YUV 422 8 bit */
219 	FMT_MBUS_DT_DTFMT_BPP(UYVY8_1X16, YUV422_8B, BPP8, 8),
220 	FMT_MBUS_DT_DTFMT_BPP(YUYV8_1X16, YUV422_8B, BPP8, 8),
221 	FMT_MBUS_DT_DTFMT_BPP(YVYU8_1X16, YUV422_8B, BPP8, 8),
222 	FMT_MBUS_DT_DTFMT_BPP(VYUY8_1X16, YUV422_8B, BPP8, 8),
223 
224 	/* Raw Bayer */
225 	/* 8 bit */
226 	FMT_MBUS_DT_DTFMT_BPP(SBGGR8_1X8, RAW8, BPP8, 8),
227 	FMT_MBUS_DT_DTFMT_BPP(SGBRG8_1X8, RAW8, BPP8, 8),
228 	FMT_MBUS_DT_DTFMT_BPP(SGRBG8_1X8, RAW8, BPP8, 8),
229 	FMT_MBUS_DT_DTFMT_BPP(SRGGB8_1X8, RAW8, BPP8, 8),
230 	/* 10 bit */
231 	FMT_MBUS_DT_DTFMT_BPP(SRGGB10_1X10, RAW10, BPP10, 10),
232 	FMT_MBUS_DT_DTFMT_BPP(SGBRG10_1X10, RAW10, BPP10, 10),
233 	FMT_MBUS_DT_DTFMT_BPP(SGRBG10_1X10, RAW10, BPP10, 10),
234 	FMT_MBUS_DT_DTFMT_BPP(SRGGB10_1X10, RAW10, BPP10, 10),
235 	/* 12 bit */
236 	FMT_MBUS_DT_DTFMT_BPP(SRGGB12_1X12, RAW12, BPP12, 12),
237 	FMT_MBUS_DT_DTFMT_BPP(SGBRG12_1X12, RAW12, BPP12, 12),
238 	FMT_MBUS_DT_DTFMT_BPP(SGRBG12_1X12, RAW12, BPP12, 12),
239 	FMT_MBUS_DT_DTFMT_BPP(SRGGB12_1X12, RAW12, BPP12, 12),
240 	/* 14 bit */
241 	FMT_MBUS_DT_DTFMT_BPP(SRGGB14_1X14, RAW14, BPP14, 14),
242 	FMT_MBUS_DT_DTFMT_BPP(SGBRG14_1X14, RAW14, BPP14, 14),
243 	FMT_MBUS_DT_DTFMT_BPP(SGRBG14_1X14, RAW14, BPP14, 14),
244 	FMT_MBUS_DT_DTFMT_BPP(SRGGB14_1X14, RAW14, BPP14, 14),
245 
246 	/* RGB 565 */
247 	FMT_MBUS_DT_DTFMT_BPP(RGB565_1X16, RGB565, BPP8, 8),
248 
249 	/* JPEG (datatype isn't used) */
250 	FMT_MBUS_DT_DTFMT_BPP(JPEG_1X8, NULL, BPP8, 8),
251 };
252 
253 struct stm32_csi_mbps_phy_reg {
254 	unsigned int mbps;
255 	unsigned int hsfreqrange;
256 	unsigned int osc_freq_target;
257 };
258 
259 /*
260  * Table describing configuration of the PHY depending on the
261  * intended Bit Rate. From table 5-8 Frequency Ranges and Defaults
262  * of the Synopsis DWC MIPI PHY databook
263  */
264 static const struct stm32_csi_mbps_phy_reg snps_stm32mp25[] = {
265 	{ .mbps =   80,	.hsfreqrange = 0x00,	.osc_freq_target = 460 },
266 	{ .mbps =   90, .hsfreqrange = 0x10,	.osc_freq_target = 460 },
267 	{ .mbps =  100, .hsfreqrange = 0x20,	.osc_freq_target = 460 },
268 	{ .mbps =  110, .hsfreqrange = 0x30,	.osc_freq_target = 460 },
269 	{ .mbps =  120, .hsfreqrange = 0x01,	.osc_freq_target = 460 },
270 	{ .mbps =  130, .hsfreqrange = 0x11,	.osc_freq_target = 460 },
271 	{ .mbps =  140, .hsfreqrange = 0x21,	.osc_freq_target = 460 },
272 	{ .mbps =  150, .hsfreqrange = 0x31,	.osc_freq_target = 460 },
273 	{ .mbps =  160, .hsfreqrange = 0x02,	.osc_freq_target = 460 },
274 	{ .mbps =  170, .hsfreqrange = 0x12,	.osc_freq_target = 460 },
275 	{ .mbps =  180, .hsfreqrange = 0x22,	.osc_freq_target = 460 },
276 	{ .mbps =  190, .hsfreqrange = 0x32,	.osc_freq_target = 460 },
277 	{ .mbps =  205, .hsfreqrange = 0x03,	.osc_freq_target = 460 },
278 	{ .mbps =  220, .hsfreqrange = 0x13,	.osc_freq_target = 460 },
279 	{ .mbps =  235, .hsfreqrange = 0x23,	.osc_freq_target = 460 },
280 	{ .mbps =  250, .hsfreqrange = 0x33,	.osc_freq_target = 460 },
281 	{ .mbps =  275, .hsfreqrange = 0x04,	.osc_freq_target = 460 },
282 	{ .mbps =  300, .hsfreqrange = 0x14,	.osc_freq_target = 460 },
283 	{ .mbps =  325, .hsfreqrange = 0x25,	.osc_freq_target = 460 },
284 	{ .mbps =  350, .hsfreqrange = 0x35,	.osc_freq_target = 460 },
285 	{ .mbps =  400, .hsfreqrange = 0x05,	.osc_freq_target = 460 },
286 	{ .mbps =  450, .hsfreqrange = 0x16,	.osc_freq_target = 460 },
287 	{ .mbps =  500, .hsfreqrange = 0x26,	.osc_freq_target = 460 },
288 	{ .mbps =  550, .hsfreqrange = 0x37,	.osc_freq_target = 460 },
289 	{ .mbps =  600, .hsfreqrange = 0x07,	.osc_freq_target = 460 },
290 	{ .mbps =  650, .hsfreqrange = 0x18,	.osc_freq_target = 460 },
291 	{ .mbps =  700, .hsfreqrange = 0x28,	.osc_freq_target = 460 },
292 	{ .mbps =  750, .hsfreqrange = 0x39,	.osc_freq_target = 460 },
293 	{ .mbps =  800, .hsfreqrange = 0x09,	.osc_freq_target = 460 },
294 	{ .mbps =  850, .hsfreqrange = 0x19,	.osc_freq_target = 460 },
295 	{ .mbps =  900, .hsfreqrange = 0x29,	.osc_freq_target = 460 },
296 	{ .mbps =  950, .hsfreqrange = 0x3a,	.osc_freq_target = 460 },
297 	{ .mbps = 1000, .hsfreqrange = 0x0a,	.osc_freq_target = 460 },
298 	{ .mbps = 1050, .hsfreqrange = 0x1a,	.osc_freq_target = 460 },
299 	{ .mbps = 1100, .hsfreqrange = 0x2a,	.osc_freq_target = 460 },
300 	{ .mbps = 1150, .hsfreqrange = 0x3b,	.osc_freq_target = 460 },
301 	{ .mbps = 1200, .hsfreqrange = 0x0b,	.osc_freq_target = 460 },
302 	{ .mbps = 1250, .hsfreqrange = 0x1b,	.osc_freq_target = 460 },
303 	{ .mbps = 1300, .hsfreqrange = 0x2b,	.osc_freq_target = 460 },
304 	{ .mbps = 1350, .hsfreqrange = 0x3c,	.osc_freq_target = 460 },
305 	{ .mbps = 1400, .hsfreqrange = 0x0c,	.osc_freq_target = 460 },
306 	{ .mbps = 1450, .hsfreqrange = 0x1c,	.osc_freq_target = 460 },
307 	{ .mbps = 1500, .hsfreqrange = 0x2c,	.osc_freq_target = 460 },
308 	{ .mbps = 1550, .hsfreqrange = 0x3d,	.osc_freq_target = 285 },
309 	{ .mbps = 1600, .hsfreqrange = 0x0d,	.osc_freq_target = 295 },
310 	{ .mbps = 1650, .hsfreqrange = 0x1d,	.osc_freq_target = 304 },
311 	{ .mbps = 1700, .hsfreqrange = 0x2e,	.osc_freq_target = 313 },
312 	{ .mbps = 1750, .hsfreqrange = 0x3e,	.osc_freq_target = 322 },
313 	{ .mbps = 1800, .hsfreqrange = 0x0e,	.osc_freq_target = 331 },
314 	{ .mbps = 1850, .hsfreqrange = 0x1e,	.osc_freq_target = 341 },
315 	{ .mbps = 1900, .hsfreqrange = 0x2f,	.osc_freq_target = 350 },
316 	{ .mbps = 1950, .hsfreqrange = 0x3f,	.osc_freq_target = 359 },
317 	{ .mbps = 2000, .hsfreqrange = 0x0f,	.osc_freq_target = 368 },
318 	{ .mbps = 2050, .hsfreqrange = 0x40,	.osc_freq_target = 377 },
319 	{ .mbps = 2100, .hsfreqrange = 0x41,	.osc_freq_target = 387 },
320 	{ .mbps = 2150, .hsfreqrange = 0x42,	.osc_freq_target = 396 },
321 	{ .mbps = 2200, .hsfreqrange = 0x43,	.osc_freq_target = 405 },
322 	{ .mbps = 2250, .hsfreqrange = 0x44,	.osc_freq_target = 414 },
323 	{ .mbps = 2300, .hsfreqrange = 0x45,	.osc_freq_target = 423 },
324 	{ .mbps = 2350, .hsfreqrange = 0x46,	.osc_freq_target = 432 },
325 	{ .mbps = 2400, .hsfreqrange = 0x47,	.osc_freq_target = 442 },
326 	{ .mbps = 2450, .hsfreqrange = 0x48,	.osc_freq_target = 451 },
327 	{ .mbps = 2500, .hsfreqrange = 0x49,	.osc_freq_target = 460 },
328 };
329 
330 static const struct v4l2_mbus_framefmt fmt_default = {
331 	.width = 640,
332 	.height = 480,
333 	.code = MEDIA_BUS_FMT_RGB565_1X16,
334 	.field = V4L2_FIELD_NONE,
335 	.colorspace = V4L2_COLORSPACE_REC709,
336 	.ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT,
337 	.quantization = V4L2_QUANTIZATION_DEFAULT,
338 	.xfer_func = V4L2_XFER_FUNC_DEFAULT,
339 };
340 
stm32_csi_code_to_fmt(unsigned int code)341 static const struct stm32_csi_fmts *stm32_csi_code_to_fmt(unsigned int code)
342 {
343 	unsigned int i;
344 
345 	for (i = 0; i < ARRAY_SIZE(stm32_csi_formats); i++)
346 		if (stm32_csi_formats[i].code == code)
347 			return &stm32_csi_formats[i];
348 
349 	return NULL;
350 }
351 
to_csidev(struct v4l2_subdev * sd)352 static inline struct stm32_csi_dev *to_csidev(struct v4l2_subdev *sd)
353 {
354 	return container_of(sd, struct stm32_csi_dev, sd);
355 }
356 
stm32_csi_setup_lane_merger(struct stm32_csi_dev * csidev)357 static int stm32_csi_setup_lane_merger(struct stm32_csi_dev *csidev)
358 {
359 	u32 lmcfgr = 0;
360 	unsigned int i;
361 
362 	for (i = 0; i < csidev->num_lanes; i++) {
363 		if (!csidev->lanes[i] || csidev->lanes[i] > STM32_CSI_LANES_MAX) {
364 			dev_err(csidev->dev, "Invalid lane id (%d)\n", csidev->lanes[i]);
365 			return -EINVAL;
366 		}
367 		lmcfgr |= (csidev->lanes[i] << ((i * 4) + STM32_CSI_LMCFGR_DLMAP_SHIFT));
368 	}
369 
370 	lmcfgr |= (csidev->num_lanes << STM32_CSI_LMCFGR_LANENB_SHIFT);
371 
372 	writel_relaxed(lmcfgr, csidev->base + STM32_CSI_LMCFGR);
373 
374 	return 0;
375 }
376 
stm32_csi_phy_reg_write(struct stm32_csi_dev * csidev,u32 addr,u32 val)377 static void stm32_csi_phy_reg_write(struct stm32_csi_dev *csidev,
378 				    u32 addr, u32 val)
379 {
380 	/* Based on sequence described at section 5.2.3.2 of DesignWave document */
381 	/* For writing the 4-bit testcode MSBs */
382 	/* Set testen to high */
383 	writel_relaxed(STM32_CSI_PTCR1_TWM, csidev->base + STM32_CSI_PTCR1);
384 
385 	/* Set testclk to high */
386 	writel_relaxed(STM32_CSI_PTCR0_TCKEN, csidev->base + STM32_CSI_PTCR0);
387 
388 	/* Place 0x00 in testdin */
389 	writel_relaxed(STM32_CSI_PTCR1_TWM, csidev->base + STM32_CSI_PTCR1);
390 
391 	/*
392 	 * Set testclk to low (with the falling edge on testclk, the testdin
393 	 * signal content is latched internally)
394 	 */
395 	writel_relaxed(0, csidev->base + STM32_CSI_PTCR0);
396 
397 	/* Set testen to low */
398 	writel_relaxed(0, csidev->base + STM32_CSI_PTCR1);
399 
400 	/* Place the 8-bit word corresponding to the testcode MSBs in testdin */
401 	writel_relaxed(((addr >> 8) & STM32_CSI_PTCR1_TDI_MASK) << STM32_CSI_PTCR1_TDI_SHIFT,
402 		       csidev->base + STM32_CSI_PTCR1);
403 
404 	/* Set testclk to high */
405 	writel_relaxed(STM32_CSI_PTCR0_TCKEN, csidev->base + STM32_CSI_PTCR0);
406 
407 	/* For writing the 8-bit testcode LSBs */
408 	/* Set testclk to low */
409 	writel_relaxed(0, csidev->base + STM32_CSI_PTCR0);
410 
411 	/* Set testen to high */
412 	writel_relaxed(STM32_CSI_PTCR1_TWM, csidev->base + STM32_CSI_PTCR1);
413 
414 	/* Set testclk to high */
415 	writel_relaxed(STM32_CSI_PTCR0_TCKEN, csidev->base + STM32_CSI_PTCR0);
416 
417 	/* Place the 8-bit word test data in testdin */
418 	writel_relaxed((addr & STM32_CSI_PTCR1_TDI_MASK) <<
419 		       STM32_CSI_PTCR1_TDI_SHIFT | STM32_CSI_PTCR1_TWM,
420 		       csidev->base + STM32_CSI_PTCR1);
421 
422 	/*
423 	 * Set testclk to low (with the falling edge on testclk, the testdin
424 	 * signal content is latched internally)
425 	 */
426 	writel_relaxed(0, csidev->base + STM32_CSI_PTCR0);
427 
428 	/* Set testen to low */
429 	writel_relaxed(0, csidev->base + STM32_CSI_PTCR1);
430 
431 	/* For writing the data */
432 	/* Place the 8-bit word corresponding to the page offset in testdin */
433 	writel_relaxed((val & STM32_CSI_PTCR1_TDI_MASK) << STM32_CSI_PTCR1_TDI_SHIFT,
434 		       csidev->base + STM32_CSI_PTCR1);
435 
436 	/* Set testclk to high (test data is programmed internally */
437 	writel_relaxed(STM32_CSI_PTCR0_TCKEN, csidev->base + STM32_CSI_PTCR0);
438 
439 	/* Finish by setting testclk to low */
440 	writel_relaxed(0, csidev->base + STM32_CSI_PTCR0);
441 }
442 
stm32_csi_start(struct stm32_csi_dev * csidev,struct v4l2_subdev_state * state)443 static int stm32_csi_start(struct stm32_csi_dev *csidev,
444 			   struct v4l2_subdev_state *state)
445 {
446 	struct media_pad *src_pad =
447 		&csidev->s_subdev->entity.pads[csidev->s_subdev_pad_nb];
448 	const struct stm32_csi_mbps_phy_reg *phy_regs = NULL;
449 	struct v4l2_mbus_framefmt *sink_fmt;
450 	const struct stm32_csi_fmts *fmt;
451 	unsigned long phy_clk_frate;
452 	u32 lanes_ie, lanes_en;
453 	unsigned int mbps;
454 	unsigned int i;
455 	s64 link_freq;
456 	int ret;
457 	u32 ccfr;
458 
459 	dev_dbg(csidev->dev, "Starting the CSI2\n");
460 
461 	/* Get the bpp value on pad0 (input of CSI) */
462 	sink_fmt = v4l2_subdev_state_get_format(state, STM32_CSI_PAD_SINK);
463 	fmt = stm32_csi_code_to_fmt(sink_fmt->code);
464 
465 	/* Get the remote sensor link frequency */
466 	if (!csidev->s_subdev)
467 		return -EIO;
468 
469 	link_freq = v4l2_get_link_freq(src_pad,
470 				       fmt->bpp, 2 * csidev->num_lanes);
471 	if (link_freq < 0)
472 		return link_freq;
473 
474 	/* MBPS is expressed in Mbps, hence link_freq / 100000 * 2 */
475 	mbps = div_s64(link_freq, 500000);
476 	dev_dbg(csidev->dev, "Computed Mbps: %u\n", mbps);
477 
478 	for (i = 0; i < ARRAY_SIZE(snps_stm32mp25); i++) {
479 		if (snps_stm32mp25[i].mbps >= mbps) {
480 			phy_regs = &snps_stm32mp25[i];
481 			break;
482 		}
483 	}
484 
485 	if (!phy_regs) {
486 		dev_err(csidev->dev, "Unsupported PHY speed (%u Mbps)", mbps);
487 		return -ERANGE;
488 	}
489 
490 	dev_dbg(csidev->dev, "PHY settings: (%u Mbps, %u HS FRange, %u OSC Freq)\n",
491 		phy_regs->mbps, phy_regs->hsfreqrange,
492 		phy_regs->osc_freq_target);
493 
494 	/* Prepare lanes related configuration bits */
495 	lanes_ie = STM32_CSI_SR1_DL0_ERRORS;
496 	lanes_en = STM32_CSI_PCR_DL0EN;
497 	if (csidev->num_lanes == 2) {
498 		lanes_ie |= STM32_CSI_SR1_DL1_ERRORS;
499 		lanes_en |= STM32_CSI_PCR_DL1EN;
500 	}
501 
502 	ret = pm_runtime_get_sync(csidev->dev);
503 	if (ret < 0)
504 		goto error_put;
505 
506 	/* Retrieve CSI2PHY clock rate to compute CCFR value */
507 	phy_clk_frate = clk_get_rate(csidev->clks[STM32_CSI_CLK_CSI2PHY].clk);
508 	if (!phy_clk_frate) {
509 		dev_err(csidev->dev, "CSI2PHY clock rate invalid (0)\n");
510 		ret = -EINVAL;
511 		goto error_put;
512 	}
513 
514 	ret = stm32_csi_setup_lane_merger(csidev);
515 	if (ret)
516 		goto error_put;
517 
518 	/* Enable the CSI */
519 	writel_relaxed(STM32_CSI_CR_CSIEN, csidev->base + STM32_CSI_CR);
520 
521 	/* Enable some global CSI related interrupts - bits are same as SR0 */
522 	writel_relaxed(STM32_CSI_SR0_ERRORS, csidev->base + STM32_CSI_IER0);
523 
524 	/* Enable lanes related error interrupts */
525 	writel_relaxed(lanes_ie, csidev->base + STM32_CSI_IER1);
526 
527 	/* Initialization of the D-PHY */
528 	/* Stop the D-PHY */
529 	writel_relaxed(0, csidev->base + STM32_CSI_PRCR);
530 
531 	/* Keep the D-PHY in power down state */
532 	writel_relaxed(0, csidev->base + STM32_CSI_PCR);
533 
534 	/* Enable testclr clock during 15ns */
535 	writel_relaxed(STM32_CSI_PTCR0_TCKEN, csidev->base + STM32_CSI_PTCR0);
536 	udelay(1);
537 	writel_relaxed(0, csidev->base + STM32_CSI_PTCR0);
538 
539 	/* Set hsfreqrange */
540 	phy_clk_frate /= 1000000;
541 	ccfr = (phy_clk_frate - 17) * 4;
542 	writel_relaxed((ccfr << STM32_CSI_PFCR_CCFR_SHIFT) |
543 		       (phy_regs->hsfreqrange << STM32_CSI_PFCR_HSFR_SHIFT),
544 		       csidev->base + STM32_CSI_PFCR);
545 
546 	/* set reg @08 deskew_polarity_rw 1'b1 */
547 	stm32_csi_phy_reg_write(csidev, 0x08, 0x38);
548 
549 	/* set reg @0xE4 counter_for_des_en_config_if_rx 0x10 + DLL prog EN */
550 	/* This is because 13<= cfgclkfreqrange[5:0]<=38 */
551 	stm32_csi_phy_reg_write(csidev, 0xe4, 0x11);
552 
553 	/* set reg @0xe2 & reg @0xe3 value DLL target oscilation freq */
554 	/* Based on the table page 77, osc_freq_target */
555 	stm32_csi_phy_reg_write(csidev, 0xe2, phy_regs->osc_freq_target & 0xFF);
556 	stm32_csi_phy_reg_write(csidev, 0xe3, (phy_regs->osc_freq_target >> 8) & 0x0F);
557 
558 	writel_relaxed(STM32_CSI_PFCR_DLD | readl_relaxed(csidev->base + STM32_CSI_PFCR),
559 		       csidev->base + STM32_CSI_PFCR);
560 
561 	/* Enable Lanes */
562 	writel_relaxed(lanes_en | STM32_CSI_PCR_CLEN, csidev->base + STM32_CSI_PCR);
563 	writel_relaxed(lanes_en | STM32_CSI_PCR_CLEN | STM32_CSI_PCR_PWRDOWN,
564 		       csidev->base + STM32_CSI_PCR);
565 
566 	writel_relaxed(STM32_CSI_PRCR_PEN, csidev->base + STM32_CSI_PRCR);
567 
568 	/* Remove the force */
569 	writel_relaxed(0, csidev->base + STM32_CSI_PMCR);
570 
571 	return ret;
572 
573 error_put:
574 	pm_runtime_put(csidev->dev);
575 	return ret;
576 }
577 
stm32_csi_stop(struct stm32_csi_dev * csidev)578 static void stm32_csi_stop(struct stm32_csi_dev *csidev)
579 {
580 	dev_dbg(csidev->dev, "Stopping the CSI2\n");
581 
582 	/* Disable the D-PHY */
583 	writel_relaxed(0, csidev->base + STM32_CSI_PCR);
584 
585 	/* Disable ITs */
586 	writel_relaxed(0, csidev->base + STM32_CSI_IER0);
587 	writel_relaxed(0, csidev->base + STM32_CSI_IER1);
588 
589 	/* Disable the CSI */
590 	writel_relaxed(0, csidev->base + STM32_CSI_CR);
591 
592 	pm_runtime_put(csidev->dev);
593 }
594 
stm32_csi_start_vc(struct stm32_csi_dev * csidev,struct v4l2_subdev_state * state,u32 vc)595 static int stm32_csi_start_vc(struct stm32_csi_dev *csidev,
596 			      struct v4l2_subdev_state *state, u32 vc)
597 {
598 	struct v4l2_mbus_framefmt *mbus_fmt;
599 	const struct stm32_csi_fmts *fmt;
600 	u32 status;
601 	u32 cfgr1;
602 	int ret;
603 
604 	mbus_fmt = v4l2_subdev_state_get_format(state, STM32_CSI_PAD_SOURCE);
605 	fmt = stm32_csi_code_to_fmt(mbus_fmt->code);
606 
607 	/* If the mbus code is JPEG, don't enable filtering */
608 	if (mbus_fmt->code == MEDIA_BUS_FMT_JPEG_1X8) {
609 		cfgr1 = STM32_CSI_VCXCFGR1_ALLDT;
610 		cfgr1 |= fmt->input_fmt << STM32_CSI_VCXCFGR1_CDTFT_SHIFT;
611 		dev_dbg(csidev->dev, "VC%d: enable AllDT mode\n", vc);
612 	} else {
613 		cfgr1 = fmt->datatype << STM32_CSI_VCXCFGR1_DT0_SHIFT;
614 		cfgr1 |= fmt->input_fmt << STM32_CSI_VCXCFGR1_DT0FT_SHIFT;
615 		cfgr1 |= STM32_CSI_VCXCFGR1_DT0EN;
616 		dev_dbg(csidev->dev, "VC%d: enable DT0(0x%x)/DT0FT(0x%x)\n",
617 			vc, fmt->datatype, fmt->input_fmt);
618 	}
619 	writel_relaxed(cfgr1, csidev->base + STM32_CSI_VCXCFGR1(vc));
620 
621 	/* Enable processing of the virtual-channel and wait for its status */
622 	writel_relaxed(STM32_CSI_CR_VCXSTART(vc) | STM32_CSI_CR_CSIEN,
623 		       csidev->base + STM32_CSI_CR);
624 
625 	ret = readl_relaxed_poll_timeout(csidev->base + STM32_CSI_SR0,
626 					 status,
627 					 status & STM32_CSI_SR0_VCXSTATEF(vc),
628 					 1000, 1000000);
629 	if (ret) {
630 		dev_err(csidev->dev, "failed to start VC(%d)\n", vc);
631 		return ret;
632 	}
633 
634 	return 0;
635 }
636 
stm32_csi_stop_vc(struct stm32_csi_dev * csidev,u32 vc)637 static int stm32_csi_stop_vc(struct stm32_csi_dev *csidev, u32 vc)
638 {
639 	u32 status;
640 	int ret;
641 
642 	/* Stop the Virtual Channel */
643 	writel_relaxed(STM32_CSI_CR_VCXSTOP(vc) | STM32_CSI_CR_CSIEN,
644 		       csidev->base + STM32_CSI_CR);
645 
646 	ret = readl_relaxed_poll_timeout(csidev->base + STM32_CSI_SR0,
647 					 status,
648 					 !(status & STM32_CSI_SR0_VCXSTATEF(vc)),
649 					 1000, 1000000);
650 	if (ret) {
651 		dev_err(csidev->dev, "failed to stop VC(%d)\n", vc);
652 		return ret;
653 	}
654 
655 	/* Disable all DTs */
656 	writel_relaxed(0, csidev->base + STM32_CSI_VCXCFGR1(vc));
657 	writel_relaxed(0, csidev->base + STM32_CSI_VCXCFGR2(vc));
658 
659 	return 0;
660 }
661 
stm32_csi_disable_streams(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,u32 pad,u64 streams_mask)662 static int stm32_csi_disable_streams(struct v4l2_subdev *sd,
663 				     struct v4l2_subdev_state *state, u32 pad,
664 				     u64 streams_mask)
665 {
666 	struct stm32_csi_dev *csidev = to_csidev(sd);
667 	int ret;
668 
669 	ret = v4l2_subdev_disable_streams(csidev->s_subdev,
670 					  csidev->s_subdev_pad_nb, BIT_ULL(0));
671 	if (ret)
672 		return ret;
673 
674 	/* Stop the VC0 */
675 	ret = stm32_csi_stop_vc(csidev, 0);
676 	if (ret)
677 		dev_err(csidev->dev, "Failed to stop VC0\n");
678 
679 	stm32_csi_stop(csidev);
680 
681 	return 0;
682 }
683 
stm32_csi_enable_streams(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,u32 pad,u64 streams_mask)684 static int stm32_csi_enable_streams(struct v4l2_subdev *sd,
685 				    struct v4l2_subdev_state *state, u32 pad,
686 				    u64 streams_mask)
687 {
688 	struct stm32_csi_dev *csidev = to_csidev(sd);
689 	int ret;
690 
691 	ret = stm32_csi_start(csidev, state);
692 	if (ret)
693 		return ret;
694 
695 	/* Configure & start the VC0 */
696 	ret = stm32_csi_start_vc(csidev, state, 0);
697 	if (ret) {
698 		dev_err(csidev->dev, "Failed to start VC0\n");
699 		goto failed_start_vc;
700 	}
701 
702 	ret = v4l2_subdev_enable_streams(csidev->s_subdev,
703 					 csidev->s_subdev_pad_nb, BIT_ULL(0));
704 	if (ret)
705 		goto failed_enable_streams;
706 
707 	return 0;
708 
709 failed_enable_streams:
710 	stm32_csi_stop_vc(csidev, 0);
711 failed_start_vc:
712 	stm32_csi_stop(csidev);
713 	return ret;
714 }
715 
stm32_csi_init_state(struct v4l2_subdev * sd,struct v4l2_subdev_state * state)716 static int stm32_csi_init_state(struct v4l2_subdev *sd,
717 				struct v4l2_subdev_state *state)
718 {
719 	unsigned int i;
720 
721 	for (i = 0; i < sd->entity.num_pads; i++)
722 		*v4l2_subdev_state_get_format(state, i) = fmt_default;
723 
724 	return 0;
725 }
726 
stm32_csi_enum_mbus_code(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,struct v4l2_subdev_mbus_code_enum * code)727 static int stm32_csi_enum_mbus_code(struct v4l2_subdev *sd,
728 				    struct v4l2_subdev_state *state,
729 				    struct v4l2_subdev_mbus_code_enum *code)
730 {
731 	if (code->index >= ARRAY_SIZE(stm32_csi_formats))
732 		return -EINVAL;
733 
734 	code->code = stm32_csi_formats[code->index].code;
735 	return 0;
736 }
737 
stm32_csi_set_pad_format(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,struct v4l2_subdev_format * format)738 static int stm32_csi_set_pad_format(struct v4l2_subdev *sd,
739 				    struct v4l2_subdev_state *state,
740 				    struct v4l2_subdev_format *format)
741 {
742 	struct stm32_csi_dev *csidev = to_csidev(sd);
743 	struct v4l2_mbus_framefmt *framefmt;
744 	const struct stm32_csi_fmts *fmt;
745 
746 	fmt = stm32_csi_code_to_fmt(format->format.code);
747 	if (!fmt) {
748 		dev_dbg(csidev->dev, "Unsupported code %d, use default\n",
749 			format->format.code);
750 		format->format.code = fmt_default.code;
751 	}
752 
753 	framefmt = v4l2_subdev_state_get_format(state, STM32_CSI_PAD_SINK);
754 
755 	if (format->pad == STM32_CSI_PAD_SOURCE)
756 		format->format = *framefmt;
757 	else
758 		*framefmt = format->format;
759 
760 	framefmt = v4l2_subdev_state_get_format(state, STM32_CSI_PAD_SOURCE);
761 	*framefmt = format->format;
762 
763 	return 0;
764 }
765 
stm32_csi_log_status(struct v4l2_subdev * sd)766 static int stm32_csi_log_status(struct v4l2_subdev *sd)
767 {
768 	struct stm32_csi_dev *csidev = to_csidev(sd);
769 	unsigned long flags;
770 	unsigned int i;
771 
772 	spin_lock_irqsave(&csidev->slock, flags);
773 
774 	for (i = 0; i < STM32_CSI_NUM_SR0_EVENTS; i++) {
775 		if (csidev->sr0_counters[i])
776 			dev_info(csidev->dev, "%s events: %d\n",
777 				 stm32_csi_events_sr0[i].name,
778 				 csidev->sr0_counters[i]);
779 	}
780 
781 	for (i = 0; i < STM32_CSI_NUM_SR1_EVENTS; i++) {
782 		if (csidev->sr1_counters[i])
783 			dev_info(csidev->dev, "%s events: %d\n",
784 				 stm32_csi_events_sr1[i].name,
785 				 csidev->sr1_counters[i]);
786 	}
787 
788 	spin_unlock_irqrestore(&csidev->slock, flags);
789 
790 	return 0;
791 }
792 
793 static const struct v4l2_subdev_core_ops stm32_csi_core_ops = {
794 	.log_status = stm32_csi_log_status,
795 };
796 
797 static const struct v4l2_subdev_video_ops stm32_csi_video_ops = {
798 	.s_stream = v4l2_subdev_s_stream_helper,
799 };
800 
801 static const struct v4l2_subdev_pad_ops stm32_csi_pad_ops = {
802 	.enum_mbus_code = stm32_csi_enum_mbus_code,
803 	.set_fmt = stm32_csi_set_pad_format,
804 	.get_fmt = v4l2_subdev_get_fmt,
805 	.enable_streams = stm32_csi_enable_streams,
806 	.disable_streams = stm32_csi_disable_streams,
807 };
808 
809 static const struct v4l2_subdev_ops stm32_csi_subdev_ops = {
810 	.core		= &stm32_csi_core_ops,
811 	.pad		= &stm32_csi_pad_ops,
812 	.video		= &stm32_csi_video_ops,
813 };
814 
815 static const struct v4l2_subdev_internal_ops stm32_csi_subdev_internal_ops = {
816 	.init_state = stm32_csi_init_state,
817 };
818 
stm32_csi_async_bound(struct v4l2_async_notifier * notifier,struct v4l2_subdev * s_subdev,struct v4l2_async_connection * asd)819 static int stm32_csi_async_bound(struct v4l2_async_notifier *notifier,
820 				 struct v4l2_subdev *s_subdev,
821 				 struct v4l2_async_connection *asd)
822 {
823 	struct v4l2_subdev *sd = notifier->sd;
824 	struct stm32_csi_dev *csidev = to_csidev(sd);
825 	int remote_pad;
826 
827 	remote_pad = media_entity_get_fwnode_pad(&s_subdev->entity,
828 						 s_subdev->fwnode,
829 						 MEDIA_PAD_FL_SOURCE);
830 	if (remote_pad < 0) {
831 		dev_err(csidev->dev, "Couldn't find output pad for subdev %s\n",
832 			s_subdev->name);
833 		return remote_pad;
834 	}
835 
836 	csidev->s_subdev = s_subdev;
837 	csidev->s_subdev_pad_nb = remote_pad;
838 
839 	return media_create_pad_link(&csidev->s_subdev->entity,
840 				     remote_pad, &csidev->sd.entity,
841 				     STM32_CSI_PAD_SINK,
842 				     MEDIA_LNK_FL_ENABLED |
843 				     MEDIA_LNK_FL_IMMUTABLE);
844 }
845 
846 static const struct v4l2_async_notifier_operations stm32_csi_notifier_ops = {
847 	.bound		= stm32_csi_async_bound,
848 };
849 
stm32_csi_irq_thread(int irq,void * arg)850 static irqreturn_t stm32_csi_irq_thread(int irq, void *arg)
851 {
852 	struct stm32_csi_dev *csidev = arg;
853 	unsigned long flags;
854 	u32 sr0, sr1;
855 	int i;
856 
857 	sr0 = readl_relaxed(csidev->base + STM32_CSI_SR0);
858 	sr1 = readl_relaxed(csidev->base + STM32_CSI_SR1);
859 
860 	/* Clear interrupt */
861 	writel_relaxed(sr0 & STM32_CSI_SR0_ERRORS,
862 		       csidev->base + STM32_CSI_FCR0);
863 	writel_relaxed(sr1 & STM32_CSI_SR1_ERRORS,
864 		       csidev->base + STM32_CSI_FCR1);
865 
866 	spin_lock_irqsave(&csidev->slock, flags);
867 
868 	for (i = 0; i < STM32_CSI_NUM_SR0_EVENTS; i++)
869 		if (sr0 & stm32_csi_events_sr0[i].mask)
870 			csidev->sr0_counters[i]++;
871 
872 	for (i = 0; i < STM32_CSI_NUM_SR1_EVENTS; i++)
873 		if (sr1 & stm32_csi_events_sr1[i].mask)
874 			csidev->sr1_counters[i]++;
875 
876 	spin_unlock_irqrestore(&csidev->slock, flags);
877 
878 	return IRQ_HANDLED;
879 }
880 
stm32_csi_get_resources(struct stm32_csi_dev * csidev,struct platform_device * pdev)881 static int stm32_csi_get_resources(struct stm32_csi_dev *csidev,
882 				   struct platform_device *pdev)
883 {
884 	unsigned int i;
885 	int irq, ret;
886 
887 	csidev->base = devm_platform_get_and_ioremap_resource(pdev, 0, NULL);
888 	if (IS_ERR(csidev->base))
889 		return dev_err_probe(&pdev->dev, PTR_ERR(csidev->base),
890 				     "Failed to ioremap resource\n");
891 
892 	for (i = 0; i < STM32_CSI_CLK_NB; i++)
893 		csidev->clks[i].id = stm32_csi_clks_id[i];
894 
895 	ret = devm_clk_bulk_get(&pdev->dev, STM32_CSI_CLK_NB,
896 				csidev->clks);
897 	if (ret < 0)
898 		return dev_err_probe(&pdev->dev, ret, "Couldn't get clks\n");
899 
900 	csidev->supplies[0].supply = "vdd";
901 	csidev->supplies[1].supply = "vdda18";
902 	ret = devm_regulator_bulk_get(&pdev->dev, ARRAY_SIZE(csidev->supplies),
903 				      csidev->supplies);
904 	if (ret)
905 		return dev_err_probe(&pdev->dev, ret,
906 				     "Failed to request regulator vdd\n");
907 
908 	irq = platform_get_irq(pdev, 0);
909 	if (irq < 0)
910 		return irq;
911 
912 	ret = devm_request_threaded_irq(&pdev->dev, irq, NULL,
913 					stm32_csi_irq_thread, IRQF_ONESHOT,
914 					dev_name(&pdev->dev), csidev);
915 	if (ret)
916 		return dev_err_probe(&pdev->dev, ret,
917 				     "Unable to request irq");
918 
919 	return 0;
920 }
921 
stm32_csi_parse_dt(struct stm32_csi_dev * csidev)922 static int stm32_csi_parse_dt(struct stm32_csi_dev *csidev)
923 {
924 	struct v4l2_fwnode_endpoint v4l2_ep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
925 	struct v4l2_async_connection *asd;
926 	struct fwnode_handle *ep;
927 	int ret;
928 
929 	/* Get bus characteristics from devicetree */
930 	ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(csidev->dev), 0, 0,
931 					     FWNODE_GRAPH_ENDPOINT_NEXT);
932 	if (!ep) {
933 		dev_err(csidev->dev, "Could not find the endpoint\n");
934 		return -ENODEV;
935 	}
936 
937 	ret = v4l2_fwnode_endpoint_parse(ep, &v4l2_ep);
938 	if (ret) {
939 		dev_err(csidev->dev, "Could not parse v4l2 endpoint\n");
940 		goto out;
941 	}
942 
943 	csidev->num_lanes = v4l2_ep.bus.mipi_csi2.num_data_lanes;
944 	if (csidev->num_lanes > STM32_CSI_LANES_MAX) {
945 		dev_err(csidev->dev, "Unsupported number of data-lanes: %d\n",
946 			csidev->num_lanes);
947 		ret = -EINVAL;
948 		goto out;
949 	}
950 
951 	memcpy(csidev->lanes, v4l2_ep.bus.mipi_csi2.data_lanes,
952 	       sizeof(csidev->lanes));
953 
954 	v4l2_async_subdev_nf_init(&csidev->notifier, &csidev->sd);
955 
956 	asd = v4l2_async_nf_add_fwnode_remote(&csidev->notifier, ep,
957 					      struct v4l2_async_connection);
958 
959 
960 	if (IS_ERR(asd)) {
961 		dev_err(csidev->dev, "Failed to add fwnode remote subdev\n");
962 		ret = PTR_ERR(asd);
963 		goto out;
964 	}
965 
966 	csidev->notifier.ops = &stm32_csi_notifier_ops;
967 
968 	ret = v4l2_async_nf_register(&csidev->notifier);
969 	if (ret) {
970 		dev_err(csidev->dev, "Failed to register notifier\n");
971 		v4l2_async_nf_cleanup(&csidev->notifier);
972 		goto out;
973 	}
974 
975 out:
976 	fwnode_handle_put(ep);
977 	return ret;
978 }
979 
stm32_csi_probe(struct platform_device * pdev)980 static int stm32_csi_probe(struct platform_device *pdev)
981 {
982 	struct stm32_csi_dev *csidev;
983 	struct reset_control *rstc;
984 	int ret;
985 
986 	csidev = devm_kzalloc(&pdev->dev, sizeof(*csidev), GFP_KERNEL);
987 	if (!csidev)
988 		return -ENOMEM;
989 
990 	platform_set_drvdata(pdev, csidev);
991 	csidev->dev = &pdev->dev;
992 
993 	spin_lock_init(&csidev->slock);
994 
995 	ret = stm32_csi_get_resources(csidev, pdev);
996 	if (ret)
997 		return ret;
998 
999 	ret = stm32_csi_parse_dt(csidev);
1000 	if (ret)
1001 		return ret;
1002 
1003 	csidev->sd.owner = THIS_MODULE;
1004 	csidev->sd.dev = &pdev->dev;
1005 	csidev->sd.internal_ops = &stm32_csi_subdev_internal_ops;
1006 	v4l2_subdev_init(&csidev->sd, &stm32_csi_subdev_ops);
1007 	v4l2_set_subdevdata(&csidev->sd, &pdev->dev);
1008 	snprintf(csidev->sd.name, sizeof(csidev->sd.name), "%s",
1009 		 dev_name(&pdev->dev));
1010 
1011 	/* Create our media pads */
1012 	csidev->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
1013 	csidev->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1014 	csidev->pads[STM32_CSI_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
1015 	csidev->pads[STM32_CSI_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
1016 
1017 	ret = media_entity_pads_init(&csidev->sd.entity, STM32_CSI_PAD_MAX,
1018 				     csidev->pads);
1019 	if (ret)
1020 		goto err_cleanup;
1021 
1022 	ret = v4l2_subdev_init_finalize(&csidev->sd);
1023 	if (ret < 0)
1024 		goto err_cleanup;
1025 
1026 	/* Reset device */
1027 	rstc = devm_reset_control_get_exclusive(&pdev->dev, NULL);
1028 	if (IS_ERR(rstc)) {
1029 		ret = dev_err_probe(&pdev->dev, PTR_ERR(rstc),
1030 				    "Couldn't get reset control\n");
1031 		goto err_cleanup;
1032 	}
1033 
1034 	ret = reset_control_assert(rstc);
1035 	if (ret) {
1036 		ret = dev_err_probe(&pdev->dev, ret,
1037 				    "Failed to assert the reset line\n");
1038 		goto err_cleanup;
1039 	}
1040 
1041 	usleep_range(3000, 5000);
1042 
1043 	ret = reset_control_deassert(rstc);
1044 	if (ret) {
1045 		ret = dev_err_probe(&pdev->dev, ret,
1046 				    "Failed to deassert the reset line\n");
1047 		goto err_cleanup;
1048 	}
1049 
1050 	pm_runtime_enable(&pdev->dev);
1051 
1052 	ret = v4l2_async_register_subdev(&csidev->sd);
1053 	if (ret < 0)
1054 		goto err_cleanup;
1055 
1056 	dev_info(&pdev->dev,
1057 		 "Probed CSI with %u lanes\n", csidev->num_lanes);
1058 
1059 	return 0;
1060 
1061 err_cleanup:
1062 	v4l2_async_nf_cleanup(&csidev->notifier);
1063 	return ret;
1064 }
1065 
stm32_csi_remove(struct platform_device * pdev)1066 static void stm32_csi_remove(struct platform_device *pdev)
1067 {
1068 	struct stm32_csi_dev *csidev = platform_get_drvdata(pdev);
1069 
1070 	v4l2_async_unregister_subdev(&csidev->sd);
1071 
1072 	pm_runtime_disable(&pdev->dev);
1073 }
1074 
stm32_csi_runtime_suspend(struct device * dev)1075 static int stm32_csi_runtime_suspend(struct device *dev)
1076 {
1077 	struct stm32_csi_dev *csidev = dev_get_drvdata(dev);
1078 	int ret;
1079 
1080 	clk_bulk_disable_unprepare(STM32_CSI_CLK_NB, csidev->clks);
1081 
1082 	ret = regulator_bulk_disable(ARRAY_SIZE(csidev->supplies),
1083 				     csidev->supplies);
1084 	if (ret < 0)
1085 		dev_err(dev, "cannot disable regulators %d\n", ret);
1086 
1087 	return 0;
1088 }
1089 
stm32_csi_runtime_resume(struct device * dev)1090 static int stm32_csi_runtime_resume(struct device *dev)
1091 {
1092 	struct stm32_csi_dev *csidev = dev_get_drvdata(dev);
1093 	int ret;
1094 
1095 	ret = regulator_bulk_enable(ARRAY_SIZE(csidev->supplies),
1096 				    csidev->supplies);
1097 	if (ret)
1098 		goto error_out;
1099 
1100 	ret = clk_bulk_prepare_enable(STM32_CSI_CLK_NB, csidev->clks);
1101 	if (ret)
1102 		goto error_disable_supplies;
1103 
1104 	return 0;
1105 
1106 error_disable_supplies:
1107 	ret = regulator_bulk_disable(ARRAY_SIZE(csidev->supplies), csidev->supplies);
1108 	if (ret < 0)
1109 		dev_err(dev, "cannot disable regulators %d\n", ret);
1110 error_out:
1111 	dev_err(csidev->dev, "Failed to resume: %d\n", ret);
1112 
1113 	return ret;
1114 }
1115 
1116 static const struct of_device_id stm32_csi_of_table[] = {
1117 	{ .compatible = "st,stm32mp25-csi", },
1118 	{ /* end node */ },
1119 };
1120 MODULE_DEVICE_TABLE(of, stm32_csi_of_table);
1121 
1122 static const struct dev_pm_ops stm32_csi_pm_ops = {
1123 	RUNTIME_PM_OPS(stm32_csi_runtime_suspend,
1124 		       stm32_csi_runtime_resume, NULL)
1125 };
1126 
1127 static struct platform_driver stm32_csi_driver = {
1128 	.driver	= {
1129 		.name = "stm32-csi",
1130 		.of_match_table = stm32_csi_of_table,
1131 		.pm = pm_ptr(&stm32_csi_pm_ops),
1132 	},
1133 	.probe	= stm32_csi_probe,
1134 	.remove = stm32_csi_remove,
1135 };
1136 
1137 module_platform_driver(stm32_csi_driver);
1138 
1139 MODULE_AUTHOR("Alain Volmat <alain.volmat@foss.st.com>");
1140 MODULE_DESCRIPTION("STM32 CSI controller");
1141 MODULE_LICENSE("GPL");
1142