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