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 { /* 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 <alain.volmat@foss.st.com>"); 1136 MODULE_DESCRIPTION("STM32 CSI controller"); 1137 MODULE_LICENSE("GPL"); 1138