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 <[email protected]>
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 { /* sentinel */ }
329 };
330
331 static const struct v4l2_mbus_framefmt fmt_default = {
332 .width = 640,
333 .height = 480,
334 .code = MEDIA_BUS_FMT_RGB565_1X16,
335 .field = V4L2_FIELD_NONE,
336 .colorspace = V4L2_COLORSPACE_REC709,
337 .ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT,
338 .quantization = V4L2_QUANTIZATION_DEFAULT,
339 .xfer_func = V4L2_XFER_FUNC_DEFAULT,
340 };
341
stm32_csi_code_to_fmt(unsigned int code)342 static const struct stm32_csi_fmts *stm32_csi_code_to_fmt(unsigned int code)
343 {
344 unsigned int i;
345
346 for (i = 0; i < ARRAY_SIZE(stm32_csi_formats); i++)
347 if (stm32_csi_formats[i].code == code)
348 return &stm32_csi_formats[i];
349
350 return NULL;
351 }
352
to_csidev(struct v4l2_subdev * sd)353 static inline struct stm32_csi_dev *to_csidev(struct v4l2_subdev *sd)
354 {
355 return container_of(sd, struct stm32_csi_dev, sd);
356 }
357
stm32_csi_setup_lane_merger(struct stm32_csi_dev * csidev)358 static int stm32_csi_setup_lane_merger(struct stm32_csi_dev *csidev)
359 {
360 u32 lmcfgr = 0;
361 int i;
362
363 for (i = 0; i < csidev->num_lanes; i++) {
364 if (!csidev->lanes[i] || csidev->lanes[i] > STM32_CSI_LANES_MAX) {
365 dev_err(csidev->dev, "Invalid lane id (%d)\n", csidev->lanes[i]);
366 return -EINVAL;
367 }
368 lmcfgr |= (csidev->lanes[i] << ((i * 4) + STM32_CSI_LMCFGR_DLMAP_SHIFT));
369 }
370
371 lmcfgr |= (csidev->num_lanes << STM32_CSI_LMCFGR_LANENB_SHIFT);
372
373 writel_relaxed(lmcfgr, csidev->base + STM32_CSI_LMCFGR);
374
375 return 0;
376 }
377
stm32_csi_phy_reg_write(struct stm32_csi_dev * csidev,u32 addr,u32 val)378 static void stm32_csi_phy_reg_write(struct stm32_csi_dev *csidev,
379 u32 addr, u32 val)
380 {
381 /* Based on sequence described at section 5.2.3.2 of DesignWave document */
382 /* For writing the 4-bit testcode MSBs */
383 /* Set testen to high */
384 writel_relaxed(STM32_CSI_PTCR1_TWM, csidev->base + STM32_CSI_PTCR1);
385
386 /* Set testclk to high */
387 writel_relaxed(STM32_CSI_PTCR0_TCKEN, csidev->base + STM32_CSI_PTCR0);
388
389 /* Place 0x00 in testdin */
390 writel_relaxed(STM32_CSI_PTCR1_TWM, csidev->base + STM32_CSI_PTCR1);
391
392 /*
393 * Set testclk to low (with the falling edge on testclk, the testdin
394 * signal content is latched internally)
395 */
396 writel_relaxed(0, csidev->base + STM32_CSI_PTCR0);
397
398 /* Set testen to low */
399 writel_relaxed(0, csidev->base + STM32_CSI_PTCR1);
400
401 /* Place the 8-bit word corresponding to the testcode MSBs in testdin */
402 writel_relaxed(((addr >> 8) & STM32_CSI_PTCR1_TDI_MASK) << STM32_CSI_PTCR1_TDI_SHIFT,
403 csidev->base + STM32_CSI_PTCR1);
404
405 /* Set testclk to high */
406 writel_relaxed(STM32_CSI_PTCR0_TCKEN, csidev->base + STM32_CSI_PTCR0);
407
408 /* For writing the 8-bit testcode LSBs */
409 /* Set testclk to low */
410 writel_relaxed(0, csidev->base + STM32_CSI_PTCR0);
411
412 /* Set testen to high */
413 writel_relaxed(STM32_CSI_PTCR1_TWM, csidev->base + STM32_CSI_PTCR1);
414
415 /* Set testclk to high */
416 writel_relaxed(STM32_CSI_PTCR0_TCKEN, csidev->base + STM32_CSI_PTCR0);
417
418 /* Place the 8-bit word test data in testdin */
419 writel_relaxed((addr & STM32_CSI_PTCR1_TDI_MASK) <<
420 STM32_CSI_PTCR1_TDI_SHIFT | STM32_CSI_PTCR1_TWM,
421 csidev->base + STM32_CSI_PTCR1);
422
423 /*
424 * Set testclk to low (with the falling edge on testclk, the testdin
425 * signal content is latched internally)
426 */
427 writel_relaxed(0, csidev->base + STM32_CSI_PTCR0);
428
429 /* Set testen to low */
430 writel_relaxed(0, csidev->base + STM32_CSI_PTCR1);
431
432 /* For writing the data */
433 /* Place the 8-bit word corresponding to the page offset in testdin */
434 writel_relaxed((val & STM32_CSI_PTCR1_TDI_MASK) << STM32_CSI_PTCR1_TDI_SHIFT,
435 csidev->base + STM32_CSI_PTCR1);
436
437 /* Set testclk to high (test data is programmed internally */
438 writel_relaxed(STM32_CSI_PTCR0_TCKEN, csidev->base + STM32_CSI_PTCR0);
439
440 /* Finish by setting testclk to low */
441 writel_relaxed(0, csidev->base + STM32_CSI_PTCR0);
442 }
443
stm32_csi_start(struct stm32_csi_dev * csidev,struct v4l2_subdev_state * state)444 static int stm32_csi_start(struct stm32_csi_dev *csidev,
445 struct v4l2_subdev_state *state)
446 {
447 const struct stm32_csi_mbps_phy_reg *phy_regs;
448 struct v4l2_mbus_framefmt *sink_fmt;
449 const struct stm32_csi_fmts *fmt;
450 unsigned long phy_clk_frate;
451 unsigned int mbps;
452 u32 lanes_ie = 0;
453 u32 lanes_en = 0;
454 s64 link_freq;
455 int ret;
456 u32 ccfr;
457
458 dev_dbg(csidev->dev, "Starting the CSI2\n");
459
460 /* Get the bpp value on pad0 (input of CSI) */
461 sink_fmt = v4l2_subdev_state_get_format(state, STM32_CSI_PAD_SINK);
462 fmt = stm32_csi_code_to_fmt(sink_fmt->code);
463
464 /* Get the remote sensor link frequency */
465 if (!csidev->s_subdev)
466 return -EIO;
467
468 link_freq = v4l2_get_link_freq(csidev->s_subdev->ctrl_handler,
469 fmt->bpp, 2 * csidev->num_lanes);
470 if (link_freq < 0)
471 return link_freq;
472
473 /* MBPS is expressed in Mbps, hence link_freq / 100000 * 2 */
474 mbps = div_s64(link_freq, 500000);
475 dev_dbg(csidev->dev, "Computed Mbps: %u\n", mbps);
476
477 for (phy_regs = snps_stm32mp25; phy_regs->mbps != 0; phy_regs++)
478 if (phy_regs->mbps >= mbps)
479 break;
480
481 if (!phy_regs->mbps) {
482 dev_err(csidev->dev, "Unsupported PHY speed (%u Mbps)", mbps);
483 return -ERANGE;
484 }
485
486 dev_dbg(csidev->dev, "PHY settings: (%u Mbps, %u HS FRange, %u OSC Freq)\n",
487 phy_regs->mbps, phy_regs->hsfreqrange,
488 phy_regs->osc_freq_target);
489
490 /* Prepare lanes related configuration bits */
491 lanes_ie |= STM32_CSI_SR1_DL0_ERRORS;
492 lanes_en |= STM32_CSI_PCR_DL0EN;
493 if (csidev->num_lanes == 2) {
494 lanes_ie |= STM32_CSI_SR1_DL1_ERRORS;
495 lanes_en |= STM32_CSI_PCR_DL1EN;
496 }
497
498 ret = pm_runtime_get_sync(csidev->dev);
499 if (ret < 0)
500 return ret;
501
502 /* Retrieve CSI2PHY clock rate to compute CCFR value */
503 phy_clk_frate = clk_get_rate(csidev->clks[STM32_CSI_CLK_CSI2PHY].clk);
504 if (!phy_clk_frate) {
505 pm_runtime_put(csidev->dev);
506 dev_err(csidev->dev, "CSI2PHY clock rate invalid (0)\n");
507 return ret;
508 }
509
510 ret = stm32_csi_setup_lane_merger(csidev);
511 if (ret) {
512 pm_runtime_put(csidev->dev);
513 return ret;
514 }
515
516 /* Enable the CSI */
517 writel_relaxed(STM32_CSI_CR_CSIEN, csidev->base + STM32_CSI_CR);
518
519 /* Enable some global CSI related interrupts - bits are same as SR0 */
520 writel_relaxed(STM32_CSI_SR0_ERRORS, csidev->base + STM32_CSI_IER0);
521
522 /* Enable lanes related error interrupts */
523 writel_relaxed(lanes_ie, csidev->base + STM32_CSI_IER1);
524
525 /* Initialization of the D-PHY */
526 /* Stop the D-PHY */
527 writel_relaxed(0, csidev->base + STM32_CSI_PRCR);
528
529 /* Keep the D-PHY in power down state */
530 writel_relaxed(0, csidev->base + STM32_CSI_PCR);
531
532 /* Enable testclr clock during 15ns */
533 writel_relaxed(STM32_CSI_PTCR0_TCKEN, csidev->base + STM32_CSI_PTCR0);
534 udelay(1);
535 writel_relaxed(0, csidev->base + STM32_CSI_PTCR0);
536
537 /* Set hsfreqrange */
538 phy_clk_frate /= 1000000;
539 ccfr = (phy_clk_frate - 17) * 4;
540 writel_relaxed((ccfr << STM32_CSI_PFCR_CCFR_SHIFT) |
541 (phy_regs->hsfreqrange << STM32_CSI_PFCR_HSFR_SHIFT),
542 csidev->base + STM32_CSI_PFCR);
543
544 /* set reg @08 deskew_polarity_rw 1'b1 */
545 stm32_csi_phy_reg_write(csidev, 0x08, 0x38);
546
547 /* set reg @0xE4 counter_for_des_en_config_if_rx 0x10 + DLL prog EN */
548 /* This is because 13<= cfgclkfreqrange[5:0]<=38 */
549 stm32_csi_phy_reg_write(csidev, 0xe4, 0x11);
550
551 /* set reg @0xe2 & reg @0xe3 value DLL target oscilation freq */
552 /* Based on the table page 77, osc_freq_target */
553 stm32_csi_phy_reg_write(csidev, 0xe2, phy_regs->osc_freq_target & 0xFF);
554 stm32_csi_phy_reg_write(csidev, 0xe3, (phy_regs->osc_freq_target >> 8) & 0x0F);
555
556 writel_relaxed(STM32_CSI_PFCR_DLD | readl_relaxed(csidev->base + STM32_CSI_PFCR),
557 csidev->base + STM32_CSI_PFCR);
558
559 /* Enable Lanes */
560 writel_relaxed(lanes_en | STM32_CSI_PCR_CLEN, csidev->base + STM32_CSI_PCR);
561 writel_relaxed(lanes_en | STM32_CSI_PCR_CLEN | STM32_CSI_PCR_PWRDOWN,
562 csidev->base + STM32_CSI_PCR);
563
564 writel_relaxed(STM32_CSI_PRCR_PEN, csidev->base + STM32_CSI_PRCR);
565
566 /* Remove the force */
567 writel_relaxed(0, csidev->base + STM32_CSI_PMCR);
568
569 return ret;
570 }
571
stm32_csi_stop(struct stm32_csi_dev * csidev)572 static void stm32_csi_stop(struct stm32_csi_dev *csidev)
573 {
574 dev_dbg(csidev->dev, "Stopping the CSI2\n");
575
576 /* Disable the D-PHY */
577 writel_relaxed(0, csidev->base + STM32_CSI_PCR);
578
579 /* Disable ITs */
580 writel_relaxed(0, csidev->base + STM32_CSI_IER0);
581 writel_relaxed(0, csidev->base + STM32_CSI_IER1);
582
583 /* Disable the CSI */
584 writel_relaxed(0, csidev->base + STM32_CSI_CR);
585
586 pm_runtime_put(csidev->dev);
587 }
588
stm32_csi_start_vc(struct stm32_csi_dev * csidev,struct v4l2_subdev_state * state,u32 vc)589 static int stm32_csi_start_vc(struct stm32_csi_dev *csidev,
590 struct v4l2_subdev_state *state, u32 vc)
591 {
592 struct v4l2_mbus_framefmt *mbus_fmt;
593 const struct stm32_csi_fmts *fmt;
594 u32 cfgr1 = 0;
595 int ret = 0;
596 u32 status;
597
598 mbus_fmt = v4l2_subdev_state_get_format(state, STM32_CSI_PAD_SOURCE);
599 fmt = stm32_csi_code_to_fmt(mbus_fmt->code);
600
601 /* If the mbus code is JPEG, don't enable filtering */
602 if (mbus_fmt->code == MEDIA_BUS_FMT_JPEG_1X8) {
603 cfgr1 |= STM32_CSI_VCXCFGR1_ALLDT;
604 cfgr1 |= fmt->input_fmt << STM32_CSI_VCXCFGR1_CDTFT_SHIFT;
605 dev_dbg(csidev->dev, "VC%d: enable AllDT mode\n", vc);
606 } else {
607 cfgr1 |= fmt->datatype << STM32_CSI_VCXCFGR1_DT0_SHIFT;
608 cfgr1 |= fmt->input_fmt << STM32_CSI_VCXCFGR1_DT0FT_SHIFT;
609 cfgr1 |= STM32_CSI_VCXCFGR1_DT0EN;
610 dev_dbg(csidev->dev, "VC%d: enable DT0(0x%x)/DT0FT(0x%x)\n",
611 vc, fmt->datatype, fmt->input_fmt);
612 }
613 writel_relaxed(cfgr1, csidev->base + STM32_CSI_VCXCFGR1(vc));
614
615 /* Enable processing of the virtual-channel and wait for its status */
616 writel_relaxed(STM32_CSI_CR_VCXSTART(vc) | STM32_CSI_CR_CSIEN,
617 csidev->base + STM32_CSI_CR);
618
619 ret = readl_relaxed_poll_timeout(csidev->base + STM32_CSI_SR0,
620 status,
621 status & STM32_CSI_SR0_VCXSTATEF(vc),
622 1000, 1000000);
623 if (ret) {
624 dev_err(csidev->dev, "failed to start VC(%d)\n", vc);
625 return ret;
626 }
627
628 return 0;
629 }
630
stm32_csi_stop_vc(struct stm32_csi_dev * csidev,u32 vc)631 static int stm32_csi_stop_vc(struct stm32_csi_dev *csidev, u32 vc)
632 {
633 int ret = 0;
634 u32 status;
635
636 /* Stop the Virtual Channel */
637 writel_relaxed(STM32_CSI_CR_VCXSTOP(vc) | STM32_CSI_CR_CSIEN,
638 csidev->base + STM32_CSI_CR);
639
640 ret = readl_relaxed_poll_timeout(csidev->base + STM32_CSI_SR0,
641 status,
642 !(status & STM32_CSI_SR0_VCXSTATEF(vc)),
643 1000, 1000000);
644 if (ret) {
645 dev_err(csidev->dev, "failed to stop VC(%d)\n", vc);
646 return ret;
647 }
648
649 /* Disable all DTs */
650 writel_relaxed(0, csidev->base + STM32_CSI_VCXCFGR1(vc));
651 writel_relaxed(0, csidev->base + STM32_CSI_VCXCFGR2(vc));
652
653 return 0;
654 }
655
stm32_csi_disable_streams(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,u32 pad,u64 streams_mask)656 static int stm32_csi_disable_streams(struct v4l2_subdev *sd,
657 struct v4l2_subdev_state *state, u32 pad,
658 u64 streams_mask)
659 {
660 struct stm32_csi_dev *csidev = to_csidev(sd);
661 int ret;
662
663 ret = v4l2_subdev_disable_streams(csidev->s_subdev,
664 csidev->s_subdev_pad_nb, BIT_ULL(0));
665 if (ret)
666 return ret;
667
668 /* Stop the VC0 */
669 ret = stm32_csi_stop_vc(csidev, 0);
670 if (ret)
671 dev_err(csidev->dev, "Failed to stop VC0\n");
672
673 stm32_csi_stop(csidev);
674
675 return 0;
676 }
677
stm32_csi_enable_streams(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,u32 pad,u64 streams_mask)678 static int stm32_csi_enable_streams(struct v4l2_subdev *sd,
679 struct v4l2_subdev_state *state, u32 pad,
680 u64 streams_mask)
681 {
682 struct stm32_csi_dev *csidev = to_csidev(sd);
683 int ret;
684
685 ret = stm32_csi_start(csidev, state);
686 if (ret)
687 return ret;
688
689 /* Configure & start the VC0 */
690 ret = stm32_csi_start_vc(csidev, state, 0);
691 if (ret) {
692 dev_err(csidev->dev, "Failed to start VC0\n");
693 stm32_csi_stop(csidev);
694 return ret;
695 }
696
697 ret = v4l2_subdev_enable_streams(csidev->s_subdev,
698 csidev->s_subdev_pad_nb, BIT_ULL(0));
699 if (ret) {
700 stm32_csi_stop_vc(csidev, 0);
701 stm32_csi_stop(csidev);
702 return ret;
703 }
704
705 return 0;
706 }
707
stm32_csi_init_state(struct v4l2_subdev * sd,struct v4l2_subdev_state * state)708 static int stm32_csi_init_state(struct v4l2_subdev *sd,
709 struct v4l2_subdev_state *state)
710 {
711 int i;
712
713 for (i = 0; i < sd->entity.num_pads; i++)
714 *v4l2_subdev_state_get_format(state, i) = fmt_default;
715
716 return 0;
717 }
718
stm32_csi_enum_mbus_code(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,struct v4l2_subdev_mbus_code_enum * code)719 static int stm32_csi_enum_mbus_code(struct v4l2_subdev *sd,
720 struct v4l2_subdev_state *state,
721 struct v4l2_subdev_mbus_code_enum *code)
722 {
723 if (code->index >= ARRAY_SIZE(stm32_csi_formats))
724 return -EINVAL;
725
726 code->code = stm32_csi_formats[code->index].code;
727 return 0;
728 }
729
stm32_csi_set_pad_format(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,struct v4l2_subdev_format * format)730 static int stm32_csi_set_pad_format(struct v4l2_subdev *sd,
731 struct v4l2_subdev_state *state,
732 struct v4l2_subdev_format *format)
733 {
734 struct stm32_csi_dev *csidev = to_csidev(sd);
735 struct v4l2_mbus_framefmt *framefmt;
736 const struct stm32_csi_fmts *fmt;
737
738 fmt = stm32_csi_code_to_fmt(format->format.code);
739 if (!fmt) {
740 dev_dbg(csidev->dev, "Unsupported code %d, use default\n",
741 format->format.code);
742 format->format.code = fmt_default.code;
743 }
744
745 framefmt = v4l2_subdev_state_get_format(state, STM32_CSI_PAD_SINK);
746
747 if (format->pad == STM32_CSI_PAD_SOURCE)
748 format->format = *framefmt;
749 else
750 *framefmt = format->format;
751
752 framefmt = v4l2_subdev_state_get_format(state, STM32_CSI_PAD_SOURCE);
753 *framefmt = format->format;
754
755 return 0;
756 }
757
stm32_csi_log_status(struct v4l2_subdev * sd)758 static int stm32_csi_log_status(struct v4l2_subdev *sd)
759 {
760 struct stm32_csi_dev *csidev = to_csidev(sd);
761 unsigned long flags;
762 unsigned int i;
763
764 spin_lock_irqsave(&csidev->slock, flags);
765
766 for (i = 0; i < STM32_CSI_NUM_SR0_EVENTS; i++) {
767 if (csidev->sr0_counters[i])
768 dev_info(csidev->dev, "%s events: %d\n",
769 stm32_csi_events_sr0[i].name,
770 csidev->sr0_counters[i]);
771 }
772
773 for (i = 0; i < STM32_CSI_NUM_SR1_EVENTS; i++) {
774 if (csidev->sr1_counters[i])
775 dev_info(csidev->dev, "%s events: %d\n",
776 stm32_csi_events_sr1[i].name,
777 csidev->sr1_counters[i]);
778 }
779
780 spin_unlock_irqrestore(&csidev->slock, flags);
781
782 return 0;
783 }
784
785 static const struct v4l2_subdev_core_ops stm32_csi_core_ops = {
786 .log_status = stm32_csi_log_status,
787 };
788
789 static const struct v4l2_subdev_video_ops stm32_csi_video_ops = {
790 .s_stream = v4l2_subdev_s_stream_helper,
791 };
792
793 static const struct v4l2_subdev_pad_ops stm32_csi_pad_ops = {
794 .enum_mbus_code = stm32_csi_enum_mbus_code,
795 .set_fmt = stm32_csi_set_pad_format,
796 .get_fmt = v4l2_subdev_get_fmt,
797 .enable_streams = stm32_csi_enable_streams,
798 .disable_streams = stm32_csi_disable_streams,
799 };
800
801 static const struct v4l2_subdev_ops stm32_csi_subdev_ops = {
802 .core = &stm32_csi_core_ops,
803 .pad = &stm32_csi_pad_ops,
804 .video = &stm32_csi_video_ops,
805 };
806
807 static const struct v4l2_subdev_internal_ops stm32_csi_subdev_internal_ops = {
808 .init_state = stm32_csi_init_state,
809 };
810
stm32_csi_async_bound(struct v4l2_async_notifier * notifier,struct v4l2_subdev * s_subdev,struct v4l2_async_connection * asd)811 static int stm32_csi_async_bound(struct v4l2_async_notifier *notifier,
812 struct v4l2_subdev *s_subdev,
813 struct v4l2_async_connection *asd)
814 {
815 struct v4l2_subdev *sd = notifier->sd;
816 struct stm32_csi_dev *csidev = to_csidev(sd);
817 int remote_pad;
818
819 remote_pad = media_entity_get_fwnode_pad(&s_subdev->entity,
820 s_subdev->fwnode,
821 MEDIA_PAD_FL_SOURCE);
822 if (remote_pad < 0) {
823 dev_err(csidev->dev, "Couldn't find output pad for subdev %s\n",
824 s_subdev->name);
825 return remote_pad;
826 }
827
828 csidev->s_subdev = s_subdev;
829 csidev->s_subdev_pad_nb = remote_pad;
830
831 return media_create_pad_link(&csidev->s_subdev->entity,
832 remote_pad, &csidev->sd.entity,
833 STM32_CSI_PAD_SINK,
834 MEDIA_LNK_FL_ENABLED |
835 MEDIA_LNK_FL_IMMUTABLE);
836 }
837
838 static const struct v4l2_async_notifier_operations stm32_csi_notifier_ops = {
839 .bound = stm32_csi_async_bound,
840 };
841
stm32_csi_irq_thread(int irq,void * arg)842 static irqreturn_t stm32_csi_irq_thread(int irq, void *arg)
843 {
844 struct stm32_csi_dev *csidev = arg;
845 unsigned long flags;
846 u32 sr0, sr1;
847 int i;
848
849 sr0 = readl_relaxed(csidev->base + STM32_CSI_SR0);
850 sr1 = readl_relaxed(csidev->base + STM32_CSI_SR1);
851
852 /* Clear interrupt */
853 writel_relaxed(sr0 & STM32_CSI_SR0_ERRORS,
854 csidev->base + STM32_CSI_FCR0);
855 writel_relaxed(sr1 & STM32_CSI_SR1_ERRORS,
856 csidev->base + STM32_CSI_FCR1);
857
858 spin_lock_irqsave(&csidev->slock, flags);
859
860 for (i = 0; i < STM32_CSI_NUM_SR0_EVENTS; i++)
861 if (sr0 & stm32_csi_events_sr0[i].mask)
862 csidev->sr0_counters[i]++;
863
864 for (i = 0; i < STM32_CSI_NUM_SR1_EVENTS; i++)
865 if (sr1 & stm32_csi_events_sr1[i].mask)
866 csidev->sr1_counters[i]++;
867
868 spin_unlock_irqrestore(&csidev->slock, flags);
869
870 return IRQ_HANDLED;
871 }
872
stm32_csi_get_resources(struct stm32_csi_dev * csidev,struct platform_device * pdev)873 static int stm32_csi_get_resources(struct stm32_csi_dev *csidev,
874 struct platform_device *pdev)
875 {
876 int irq, ret, i;
877
878 csidev->base = devm_platform_get_and_ioremap_resource(pdev, 0, NULL);
879 if (IS_ERR(csidev->base))
880 return dev_err_probe(&pdev->dev, PTR_ERR(csidev->base),
881 "Failed to ioremap resource\n");
882
883 for (i = 0; i < STM32_CSI_CLK_NB; i++)
884 csidev->clks[i].id = stm32_csi_clks_id[i];
885
886 ret = devm_clk_bulk_get(&pdev->dev, STM32_CSI_CLK_NB,
887 csidev->clks);
888 if (ret < 0)
889 return dev_err_probe(&pdev->dev, ret, "Couldn't get clks\n");
890
891 csidev->supplies[0].supply = "vdd";
892 csidev->supplies[1].supply = "vdda18";
893 ret = devm_regulator_bulk_get(&pdev->dev, ARRAY_SIZE(csidev->supplies),
894 csidev->supplies);
895 if (ret)
896 return dev_err_probe(&pdev->dev, ret,
897 "Failed to request regulator vdd\n");
898
899 irq = platform_get_irq(pdev, 0);
900 if (irq < 0)
901 return irq;
902
903 ret = devm_request_threaded_irq(&pdev->dev, irq, NULL,
904 stm32_csi_irq_thread, IRQF_ONESHOT,
905 dev_name(&pdev->dev), csidev);
906 if (ret)
907 return dev_err_probe(&pdev->dev, ret,
908 "Unable to request irq");
909
910 return 0;
911 }
912
stm32_csi_parse_dt(struct stm32_csi_dev * csidev)913 static int stm32_csi_parse_dt(struct stm32_csi_dev *csidev)
914 {
915 struct v4l2_fwnode_endpoint v4l2_ep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
916 struct v4l2_async_connection *asd;
917 struct fwnode_handle *ep;
918 int ret;
919
920 /* Get bus characteristics from devicetree */
921 ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(csidev->dev), 0, 0,
922 FWNODE_GRAPH_ENDPOINT_NEXT);
923 if (!ep) {
924 dev_err(csidev->dev, "Could not find the endpoint\n");
925 return -ENODEV;
926 }
927
928 ret = v4l2_fwnode_endpoint_parse(ep, &v4l2_ep);
929 fwnode_handle_put(ep);
930 if (ret) {
931 dev_err(csidev->dev, "Could not parse v4l2 endpoint\n");
932 return ret;
933 }
934
935 csidev->num_lanes = v4l2_ep.bus.mipi_csi2.num_data_lanes;
936 if (csidev->num_lanes > STM32_CSI_LANES_MAX) {
937 dev_err(csidev->dev, "Unsupported number of data-lanes: %d\n",
938 csidev->num_lanes);
939 return -EINVAL;
940 }
941
942 memcpy(csidev->lanes, v4l2_ep.bus.mipi_csi2.data_lanes,
943 sizeof(csidev->lanes));
944
945 ep = fwnode_graph_get_next_endpoint(dev_fwnode(csidev->dev), NULL);
946 if (!ep) {
947 dev_err(csidev->dev, "Failed to get next endpoint\n");
948 return -EINVAL;
949 }
950
951 v4l2_async_subdev_nf_init(&csidev->notifier, &csidev->sd);
952
953 asd = v4l2_async_nf_add_fwnode_remote(&csidev->notifier, ep,
954 struct v4l2_async_connection);
955
956 fwnode_handle_put(ep);
957
958 if (IS_ERR(asd)) {
959 dev_err(csidev->dev, "Failed to add fwnode remote subdev\n");
960 return PTR_ERR(asd);
961 }
962
963 csidev->notifier.ops = &stm32_csi_notifier_ops;
964
965 ret = v4l2_async_nf_register(&csidev->notifier);
966 if (ret) {
967 dev_err(csidev->dev, "Failed to register notifier\n");
968 v4l2_async_nf_cleanup(&csidev->notifier);
969 return ret;
970 }
971
972 return ret;
973 }
974
stm32_csi_probe(struct platform_device * pdev)975 static int stm32_csi_probe(struct platform_device *pdev)
976 {
977 struct stm32_csi_dev *csidev;
978 struct reset_control *rstc;
979 int ret;
980
981 csidev = devm_kzalloc(&pdev->dev, sizeof(*csidev), GFP_KERNEL);
982 if (!csidev)
983 return -ENOMEM;
984
985 platform_set_drvdata(pdev, csidev);
986 csidev->dev = &pdev->dev;
987
988 spin_lock_init(&csidev->slock);
989
990 ret = stm32_csi_get_resources(csidev, pdev);
991 if (ret)
992 goto err_free_priv;
993
994 ret = stm32_csi_parse_dt(csidev);
995 if (ret)
996 goto err_free_priv;
997
998 csidev->sd.owner = THIS_MODULE;
999 csidev->sd.dev = &pdev->dev;
1000 csidev->sd.internal_ops = &stm32_csi_subdev_internal_ops;
1001 v4l2_subdev_init(&csidev->sd, &stm32_csi_subdev_ops);
1002 v4l2_set_subdevdata(&csidev->sd, &pdev->dev);
1003 snprintf(csidev->sd.name, sizeof(csidev->sd.name), "%s",
1004 dev_name(&pdev->dev));
1005
1006 /* Create our media pads */
1007 csidev->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
1008 csidev->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1009 csidev->pads[STM32_CSI_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
1010 csidev->pads[STM32_CSI_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
1011
1012 ret = media_entity_pads_init(&csidev->sd.entity, STM32_CSI_PAD_MAX,
1013 csidev->pads);
1014 if (ret)
1015 goto err_cleanup;
1016
1017 ret = v4l2_subdev_init_finalize(&csidev->sd);
1018 if (ret < 0)
1019 goto err_cleanup;
1020
1021 ret = v4l2_async_register_subdev(&csidev->sd);
1022 if (ret < 0)
1023 goto err_cleanup;
1024
1025 /* Reset device */
1026 rstc = devm_reset_control_get_exclusive(&pdev->dev, NULL);
1027 if (IS_ERR(rstc)) {
1028 ret = dev_err_probe(&pdev->dev, PTR_ERR(rstc),
1029 "Couldn't get reset control\n");
1030 goto err_cleanup;
1031 }
1032
1033 ret = reset_control_assert(rstc);
1034 if (ret) {
1035 ret = dev_err_probe(&pdev->dev, ret,
1036 "Failed to assert the reset line\n");
1037 goto err_cleanup;
1038 }
1039
1040 usleep_range(3000, 5000);
1041
1042 ret = reset_control_deassert(rstc);
1043 if (ret) {
1044 ret = dev_err_probe(&pdev->dev, ret,
1045 "Failed to deassert the reset line\n");
1046 goto err_cleanup;
1047 }
1048
1049 pm_runtime_enable(&pdev->dev);
1050
1051 dev_info(&pdev->dev,
1052 "Probed CSI with %u lanes\n", csidev->num_lanes);
1053
1054 return 0;
1055
1056 err_cleanup:
1057 v4l2_async_nf_cleanup(&csidev->notifier);
1058 err_free_priv:
1059 return ret;
1060 }
1061
stm32_csi_remove(struct platform_device * pdev)1062 static void stm32_csi_remove(struct platform_device *pdev)
1063 {
1064 struct stm32_csi_dev *csidev = platform_get_drvdata(pdev);
1065
1066 v4l2_async_unregister_subdev(&csidev->sd);
1067
1068 pm_runtime_disable(&pdev->dev);
1069 }
1070
stm32_csi_runtime_suspend(struct device * dev)1071 static int stm32_csi_runtime_suspend(struct device *dev)
1072 {
1073 struct stm32_csi_dev *csidev = dev_get_drvdata(dev);
1074 int ret;
1075
1076 clk_bulk_disable_unprepare(STM32_CSI_CLK_NB, csidev->clks);
1077
1078 ret = regulator_bulk_disable(ARRAY_SIZE(csidev->supplies),
1079 csidev->supplies);
1080 if (ret < 0)
1081 dev_err(dev, "cannot disable regulators %d\n", ret);
1082
1083 return 0;
1084 }
1085
stm32_csi_runtime_resume(struct device * dev)1086 static int stm32_csi_runtime_resume(struct device *dev)
1087 {
1088 struct stm32_csi_dev *csidev = dev_get_drvdata(dev);
1089 int ret;
1090
1091 ret = regulator_bulk_enable(ARRAY_SIZE(csidev->supplies),
1092 csidev->supplies);
1093 if (ret)
1094 goto error_out;
1095
1096 ret = clk_bulk_prepare_enable(STM32_CSI_CLK_NB, csidev->clks);
1097 if (ret)
1098 goto error_disable_supplies;
1099
1100 return 0;
1101
1102 error_disable_supplies:
1103 ret = regulator_bulk_disable(ARRAY_SIZE(csidev->supplies), csidev->supplies);
1104 if (ret < 0)
1105 dev_err(dev, "cannot disable regulators %d\n", ret);
1106 error_out:
1107 dev_err(csidev->dev, "Failed to resume: %d\n", ret);
1108
1109 return ret;
1110 }
1111
1112 static const struct of_device_id stm32_csi_of_table[] = {
1113 { .compatible = "st,stm32mp25-csi", },
1114 { /* end node */ },
1115 };
1116 MODULE_DEVICE_TABLE(of, stm32_csi_of_table);
1117
1118 static const struct dev_pm_ops stm32_csi_pm_ops = {
1119 RUNTIME_PM_OPS(stm32_csi_runtime_suspend,
1120 stm32_csi_runtime_resume, NULL)
1121 };
1122
1123 static struct platform_driver stm32_csi_driver = {
1124 .driver = {
1125 .name = "stm32-csi",
1126 .of_match_table = stm32_csi_of_table,
1127 .pm = pm_ptr(&stm32_csi_pm_ops),
1128 },
1129 .probe = stm32_csi_probe,
1130 .remove = stm32_csi_remove,
1131 };
1132
1133 module_platform_driver(stm32_csi_driver);
1134
1135 MODULE_AUTHOR("Alain Volmat <[email protected]>");
1136 MODULE_DESCRIPTION("STM32 CSI controller");
1137 MODULE_LICENSE("GPL");
1138