FILE
stm32-csi.c
File preview with syntax highlighting.
linux-6.19.14/drivers/media/platform/st/stm32/stm32-csi.c
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 119enum stm32_csi_pads { 120 STM32_CSI_PAD_SINK, 121 STM32_CSI_PAD_SOURCE, 122 STM32_CSI_PAD_MAX, 123}; 124 125struct stm32_csi_event { 126 u32 mask; 127 const char * const name; 128}; 129 130static 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 142static 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 161enum 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 168static const char * const stm32_csi_clks_id[] = { 169 "pclk", 170 "txesc", 171 "csi2phy", 172}; 173 174struct 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 203struct 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 } 217static 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 253struct 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 */ 264static 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 330static 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 341static 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 352static inline struct stm32_csi_dev *to_csidev(struct v4l2_subdev *sd) 353{ 354 return container_of(sd, struct stm32_csi_dev, sd); 355} 356 357static 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 377static 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 443static int stm32_csi_start(struct stm32_csi_dev *csidev, 444 struct v4l2_subdev_state *state) 445{ 446 struct media_pad *src_pad; 447 const struct stm32_csi_mbps_phy_reg *phy_regs = NULL; 448 struct v4l2_mbus_framefmt *sink_fmt; 449 const struct stm32_csi_fmts *fmt; 450 unsigned long phy_clk_frate; 451 u32 lanes_ie, lanes_en; 452 unsigned int mbps; 453 unsigned int i; 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 src_pad = &csidev->s_subdev->entity.pads[csidev->s_subdev_pad_nb]; 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 573error_put: 574 pm_runtime_put(csidev->dev); 575 return ret; 576} 577 578static 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 595static 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 637static 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 662static 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 684static 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 709failed_enable_streams: 710 stm32_csi_stop_vc(csidev, 0); 711failed_start_vc: 712 stm32_csi_stop(csidev); 713 return ret; 714} 715 716static 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 727static 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 738static 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 766static 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 793static const struct v4l2_subdev_core_ops stm32_csi_core_ops = { 794 .log_status = stm32_csi_log_status, 795}; 796 797static const struct v4l2_subdev_video_ops stm32_csi_video_ops = { 798 .s_stream = v4l2_subdev_s_stream_helper, 799}; 800 801static 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 809static 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 815static const struct v4l2_subdev_internal_ops stm32_csi_subdev_internal_ops = { 816 .init_state = stm32_csi_init_state, 817}; 818 819static 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 846static const struct v4l2_async_notifier_operations stm32_csi_notifier_ops = { 847 .bound = stm32_csi_async_bound, 848}; 849 850static 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 881static 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 922static 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 975out: 976 fwnode_handle_put(ep); 977 return ret; 978} 979 980static 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 1061err_cleanup: 1062 v4l2_async_nf_cleanup(&csidev->notifier); 1063 return ret; 1064} 1065 1066static 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 1075static 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 1090static 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 1106error_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); 1110error_out: 1111 dev_err(csidev->dev, "Failed to resume: %d\n", ret); 1112 1113 return ret; 1114} 1115 1116static const struct of_device_id stm32_csi_of_table[] = { 1117 { .compatible = "st,stm32mp25-csi", }, 1118 { /* end node */ }, 1119}; 1120MODULE_DEVICE_TABLE(of, stm32_csi_of_table); 1121 1122static 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 1127static 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 1137module_platform_driver(stm32_csi_driver); 1138 1139MODULE_AUTHOR("Alain Volmat <alain.volmat@foss.st.com>"); 1140MODULE_DESCRIPTION("STM32 CSI controller"); 1141MODULE_LICENSE("GPL");
~/
Read-only file preview.