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 <[email protected]>
   7  * for STMicroelectronics.
   8  */
   9
  10 #include <linux/clk.h>
  11 #include <linux/delay.h>
  12 #include <linux/io.h>
  13 #include <linux/iopoll.h>
  14 #include <linux/module.h>
  15 #include <linux/platform_device.h>
  16 #include <linux/pm_runtime.h>
  17 #include <linux/reset.h>
  18 #include <linux/slab.h>
  19
  20 #include <media/mipi-csi2.h>
  21 #include <media/v4l2-fwnode.h>
  22 #include <media/v4l2-subdev.h>
  23
  24 #define STM32_CSI_CR                            0x0000
  25 #define STM32_CSI_CR_CSIEN                      BIT(0)
  26 #define STM32_CSI_CR_VCXSTART(x)                BIT(2 + ((x) * 4))
  27 #define STM32_CSI_CR_VCXSTOP(x)                 BIT(3 + ((x) * 4))
  28 #define STM32_CSI_PCR                           0x0004
  29 #define STM32_CSI_PCR_DL1EN                     BIT(3)
  30 #define STM32_CSI_PCR_DL0EN                     BIT(2)
  31 #define STM32_CSI_PCR_CLEN                      BIT(1)
  32 #define STM32_CSI_PCR_PWRDOWN                   BIT(0)
  33 #define STM32_CSI_VCXCFGR1(x)                   ((((x) + 1) * 0x0010) + 0x0)
  34 #define STM32_CSI_VCXCFGR1_ALLDT                BIT(0)
  35 #define STM32_CSI_VCXCFGR1_DT0EN                BIT(1)
  36 #define STM32_CSI_VCXCFGR1_DT1EN                BIT(2)
  37 #define STM32_CSI_VCXCFGR1_CDTFT_SHIFT          8
  38 #define STM32_CSI_VCXCFGR1_DT0_SHIFT            16
  39 #define STM32_CSI_VCXCFGR1_DT0FT_SHIFT          24
  40 #define STM32_CSI_VCXCFGR2(x)                   ((((x) + 1) * 0x0010) + 0x4)
  41 #define STM32_CSI_VCXCFGR2_DT1_SHIFT            0
  42 #define STM32_CSI_VCXCFGR2_DT1FT_SHIFT          8
  43 #define STM32_CSI_INPUT_BPP8                    2
  44 #define STM32_CSI_INPUT_BPP10                   3
  45 #define STM32_CSI_INPUT_BPP12                   4
  46 #define STM32_CSI_INPUT_BPP14                   5
  47 #define STM32_CSI_LMCFGR                        0x0070
  48 #define STM32_CSI_LMCFGR_LANENB_SHIFT           8
  49 #define STM32_CSI_LMCFGR_DLMAP_SHIFT            16
  50 #define STM32_CSI_IER0                          0x0080
  51 #define STM32_CSI_IER1                          0x0084
  52 #define STM32_CSI_SR0                           0x0090
  53 #define STM32_CSI_SR0_SYNCERRF                  BIT(30)
  54 #define STM32_CSI_SR0_SPKTERRF                  BIT(28)
  55 #define STM32_CSI_SR0_IDERRF                    BIT(27)
  56 #define STM32_CSI_SR0_CECCERRF                  BIT(26)
  57 #define STM32_CSI_SR0_ECCERRF                   BIT(25)
  58 #define STM32_CSI_SR0_CRCERRF                   BIT(24)
  59 #define STM32_CSI_SR0_CCFIFOFF                  BIT(21)
  60 #define STM32_CSI_SR0_VCXSTATEF(x)              BIT(17 + (x))
  61 #define STM32_CSI_SR1                           0x0094
  62 #define STM32_CSI_SR1_ECTRLDL1F                 BIT(12)
  63 #define STM32_CSI_SR1_ESYNCESCDL1F              BIT(11)
  64 #define STM32_CSI_SR1_EESCDL1F                  BIT(10)
  65 #define STM32_CSI_SR1_ESOTSYNCDL1F              BIT(9)
  66 #define STM32_CSI_SR1_ESOTDL1F                  BIT(8)
  67 #define STM32_CSI_SR1_ECTRLDL0F                 BIT(4)
  68 #define STM32_CSI_SR1_ESYNCESCDL0F              BIT(3)
  69 #define STM32_CSI_SR1_EESCDL0F                  BIT(2)
  70 #define STM32_CSI_SR1_ESOTSYNCDL0F              BIT(1)
  71 #define STM32_CSI_SR1_ESOTDL0F                  BIT(0)
  72 #define STM32_CSI_FCR0                          0x0100
  73 #define STM32_CSI_FCR1                          0x0104
  74 #define STM32_CSI_SPDFR                         0x0110
  75 #define STM32_CSI_DT_MASK                       0x3f
  76 #define STM32_CSI_VC_MASK                       0x03
  77 #define STM32_CSI_ERR1                          0x0114
  78 #define STM32_CSI_ERR1_IDVCERR_SHIFT            22
  79 #define STM32_CSI_ERR1_IDDTERR_SHIFT            16
  80 #define STM32_CSI_ERR1_CECCVCERR_SHIFT          14
  81 #define STM32_CSI_ERR1_CECCDTERR_SHIFT          8
  82 #define STM32_CSI_ERR1_CRCVCERR_SHIFT           6
  83 #define STM32_CSI_ERR1_CRCDTERR_SHIFT           0
  84 #define STM32_CSI_ERR2                          0x0118
  85 #define STM32_CSI_ERR2_SYNCVCERR_SHIFT          18
  86 #define STM32_CSI_ERR2_SPKTVCERR_SHIFT          6
  87 #define STM32_CSI_ERR2_SPKTDTERR_SHIFT          0
  88 #define STM32_CSI_PRCR                          0x1000
  89 #define STM32_CSI_PRCR_PEN                      BIT(1)
  90 #define STM32_CSI_PMCR                          0x1004
  91 #define STM32_CSI_PFCR                          0x1008
  92 #define STM32_CSI_PFCR_CCFR_MASK                GENMASK(5, 0)
  93 #define STM32_CSI_PFCR_CCFR_SHIFT               0
  94 #define STM32_CSI_PFCR_HSFR_MASK                GENMASK(14, 8)
  95 #define STM32_CSI_PFCR_HSFR_SHIFT               8
  96 #define STM32_CSI_PFCR_DLD                      BIT(16)
  97 #define STM32_CSI_PTCR0                         0x1010
  98 #define STM32_CSI_PTCR0_TCKEN                   BIT(0)
  99 #define STM32_CSI_PTCR1                         0x1014
 100 #define STM32_CSI_PTCR1_TWM                     BIT(16)
 101 #define STM32_CSI_PTCR1_TDI_MASK                GENMASK(7, 0)
 102 #define STM32_CSI_PTCR1_TDI_SHIFT               0
 103 #define STM32_CSI_PTSR                          0x1018
 104
 105 #define STM32_CSI_LANES_MAX     2
 106
 107 #define STM32_CSI_SR0_ERRORS    (STM32_CSI_SR0_SYNCERRF | STM32_CSI_SR0_SPKTERRF |\
 108                                  STM32_CSI_SR0_IDERRF | STM32_CSI_SR0_CECCERRF |\
 109                                  STM32_CSI_SR0_ECCERRF | STM32_CSI_SR0_CRCERRF |\
 110                                  STM32_CSI_SR0_CCFIFOFF)
 111 #define STM32_CSI_SR1_DL0_ERRORS        (STM32_CSI_SR1_ECTRLDL0F | STM32_CSI_SR1_ESYNCESCDL0F |\
 112                                  STM32_CSI_SR1_EESCDL0F | STM32_CSI_SR1_ESOTSYNCDL0F |\
 113                                  STM32_CSI_SR1_ESOTDL0F)
 114 #define STM32_CSI_SR1_DL1_ERRORS        (STM32_CSI_SR1_ECTRLDL1F | STM32_CSI_SR1_ESYNCESCDL1F |\
 115                                  STM32_CSI_SR1_EESCDL1F | STM32_CSI_SR1_ESOTSYNCDL1F |\
 116                                  STM32_CSI_SR1_ESOTDL1F)
 117 #define STM32_CSI_SR1_ERRORS    (STM32_CSI_SR1_DL0_ERRORS | STM32_CSI_SR1_DL1_ERRORS)
 118
 119 enum stm32_csi_pads {
 120         STM32_CSI_PAD_SINK,
 121         STM32_CSI_PAD_SOURCE,
 122         STM32_CSI_PAD_MAX,
 123 };
 124
 125 struct stm32_csi_event {
 126         u32 mask;
 127         const char * const name;
 128 };
 129
 130 static const struct stm32_csi_event stm32_csi_events_sr0[] = {
 131         {STM32_CSI_SR0_SYNCERRF,        "Synchronization error"},
 132         {STM32_CSI_SR0_SPKTERRF,        "Short packet error"},
 133         {STM32_CSI_SR0_IDERRF,          "Data type ID error"},
 134         {STM32_CSI_SR0_CECCERRF,        "Corrected ECC error"},
 135         {STM32_CSI_SR0_ECCERRF,         "ECC error"},
 136         {STM32_CSI_SR0_CRCERRF,         "CRC error"},
 137         {STM32_CSI_SR0_CCFIFOFF,        "Clk changer FIFO full error"},
 138 };
 139
 140 #define STM32_CSI_NUM_SR0_EVENTS ARRAY_SIZE(stm32_csi_events_sr0)
 141
 142 static const struct stm32_csi_event stm32_csi_events_sr1[] = {
 143         {STM32_CSI_SR1_ECTRLDL1F,       "L1: D-PHY control error"},
 144         {STM32_CSI_SR1_ESYNCESCDL1F,
 145                 "L1: D-PHY low power data transmission synchro error"},
 146         {STM32_CSI_SR1_EESCDL1F,        "L1: D-PHY escape entry error"},
 147         {STM32_CSI_SR1_ESOTSYNCDL1F,
 148                 "L1: Start of transmission synchro error"},
 149         {STM32_CSI_SR1_ESOTDL1F,        "L1: Start of transmission error"},
 150         {STM32_CSI_SR1_ECTRLDL0F,       "L0: D-PHY control error"},
 151         {STM32_CSI_SR1_ESYNCESCDL0F,
 152                 "L0: D-PHY low power data transmission synchro error"},
 153         {STM32_CSI_SR1_EESCDL0F,        "L0: D-PHY escape entry error"},
 154         {STM32_CSI_SR1_ESOTSYNCDL0F,
 155                 "L0: Start of transmission synchro error"},
 156         {STM32_CSI_SR1_ESOTDL0F,        "L0: Start of transmission error"},
 157 };
 158
 159 #define STM32_CSI_NUM_SR1_EVENTS ARRAY_SIZE(stm32_csi_events_sr1)
 160
 161 enum stm32_csi_clk {
 162         STM32_CSI_CLK_PCLK,
 163         STM32_CSI_CLK_TXESC,
 164         STM32_CSI_CLK_CSI2PHY,
 165         STM32_CSI_CLK_NB,
 166 };
 167
 168 static const char * const stm32_csi_clks_id[] = {
 169         "pclk",
 170         "txesc",
 171         "csi2phy",
 172 };
 173
 174 struct stm32_csi_dev {
 175         struct device                   *dev;
 176
 177         void __iomem                    *base;
 178
 179         struct clk_bulk_data            clks[STM32_CSI_CLK_NB];
 180         struct regulator_bulk_data      supplies[2];
 181
 182         u8                              lanes[STM32_CSI_LANES_MAX];
 183         u8                              num_lanes;
 184
 185         /*
 186          * spinlock slock is used to protect to srX_counters tables being
 187          * accessed from log_status and interrupt context
 188          */
 189         spinlock_t                      slock;
 190
 191         u32                             sr0_counters[STM32_CSI_NUM_SR0_EVENTS];
 192         u32                             sr1_counters[STM32_CSI_NUM_SR1_EVENTS];
 193
 194         struct v4l2_subdev              sd;
 195         struct v4l2_async_notifier      notifier;
 196         struct media_pad                pads[STM32_CSI_PAD_MAX];
 197
 198         /* Remote source */
 199         struct v4l2_subdev              *s_subdev;
 200         u32                             s_subdev_pad_nb;
 201 };
 202
 203 struct stm32_csi_fmts {
 204         u32 code;
 205         u32 datatype;
 206         u32 input_fmt;
 207         u8 bpp;
 208 };
 209
 210 #define FMT_MBUS_DT_DTFMT_BPP(mbus, dt, input, byteperpixel)            \
 211         {                                                               \
 212                 .code = MEDIA_BUS_FMT_##mbus,                           \
 213                 .datatype = MIPI_CSI2_DT_##dt,                          \
 214                 .input_fmt = STM32_CSI_INPUT_##input,   \
 215                 .bpp = byteperpixel,                                    \
 216         }
 217 static const struct stm32_csi_fmts stm32_csi_formats[] = {
 218         /* YUV 422 8 bit */
 219         FMT_MBUS_DT_DTFMT_BPP(UYVY8_1X16, YUV422_8B, BPP8, 8),
 220         FMT_MBUS_DT_DTFMT_BPP(YUYV8_1X16, YUV422_8B, BPP8, 8),
 221         FMT_MBUS_DT_DTFMT_BPP(YVYU8_1X16, YUV422_8B, BPP8, 8),
 222         FMT_MBUS_DT_DTFMT_BPP(VYUY8_1X16, YUV422_8B, BPP8, 8),
 223
 224         /* Raw Bayer */
 225         /* 8 bit */
 226         FMT_MBUS_DT_DTFMT_BPP(SBGGR8_1X8, RAW8, BPP8, 8),
 227         FMT_MBUS_DT_DTFMT_BPP(SGBRG8_1X8, RAW8, BPP8, 8),
 228         FMT_MBUS_DT_DTFMT_BPP(SGRBG8_1X8, RAW8, BPP8, 8),
 229         FMT_MBUS_DT_DTFMT_BPP(SRGGB8_1X8, RAW8, BPP8, 8),
 230         /* 10 bit */
 231         FMT_MBUS_DT_DTFMT_BPP(SRGGB10_1X10, RAW10, BPP10, 10),
 232         FMT_MBUS_DT_DTFMT_BPP(SGBRG10_1X10, RAW10, BPP10, 10),
 233         FMT_MBUS_DT_DTFMT_BPP(SGRBG10_1X10, RAW10, BPP10, 10),
 234         FMT_MBUS_DT_DTFMT_BPP(SRGGB10_1X10, RAW10, BPP10, 10),
 235         /* 12 bit */
 236         FMT_MBUS_DT_DTFMT_BPP(SRGGB12_1X12, RAW12, BPP12, 12),
 237         FMT_MBUS_DT_DTFMT_BPP(SGBRG12_1X12, RAW12, BPP12, 12),
 238         FMT_MBUS_DT_DTFMT_BPP(SGRBG12_1X12, RAW12, BPP12, 12),
 239         FMT_MBUS_DT_DTFMT_BPP(SRGGB12_1X12, RAW12, BPP12, 12),
 240         /* 14 bit */
 241         FMT_MBUS_DT_DTFMT_BPP(SRGGB14_1X14, RAW14, BPP14, 14),
 242         FMT_MBUS_DT_DTFMT_BPP(SGBRG14_1X14, RAW14, BPP14, 14),
 243         FMT_MBUS_DT_DTFMT_BPP(SGRBG14_1X14, RAW14, BPP14, 14),
 244         FMT_MBUS_DT_DTFMT_BPP(SRGGB14_1X14, RAW14, BPP14, 14),
 245
 246         /* RGB 565 */
 247         FMT_MBUS_DT_DTFMT_BPP(RGB565_1X16, RGB565, BPP8, 8),
 248
 249         /* JPEG (datatype isn't used) */
 250         FMT_MBUS_DT_DTFMT_BPP(JPEG_1X8, NULL, BPP8, 8),
 251 };
 252
 253 struct stm32_csi_mbps_phy_reg {
 254         unsigned int mbps;
 255         unsigned int hsfreqrange;
 256         unsigned int osc_freq_target;
 257 };
 258
 259 /*
 260  * Table describing configuration of the PHY depending on the
 261  * intended Bit Rate. From table 5-8 Frequency Ranges and Defaults
 262  * of the Synopsis DWC MIPI PHY databook
 263  */
 264 static const struct stm32_csi_mbps_phy_reg snps_stm32mp25[] = {
 265         { .mbps =   80, .hsfreqrange = 0x00,    .osc_freq_target = 460 },
 266         { .mbps =   90, .hsfreqrange = 0x10,    .osc_freq_target = 460 },
 267         { .mbps =  100, .hsfreqrange = 0x20,    .osc_freq_target = 460 },
 268         { .mbps =  110, .hsfreqrange = 0x30,    .osc_freq_target = 460 },
 269         { .mbps =  120, .hsfreqrange = 0x01,    .osc_freq_target = 460 },
 270         { .mbps =  130, .hsfreqrange = 0x11,    .osc_freq_target = 460 },
 271         { .mbps =  140, .hsfreqrange = 0x21,    .osc_freq_target = 460 },
 272         { .mbps =  150, .hsfreqrange = 0x31,    .osc_freq_target = 460 },
 273         { .mbps =  160, .hsfreqrange = 0x02,    .osc_freq_target = 460 },
 274         { .mbps =  170, .hsfreqrange = 0x12,    .osc_freq_target = 460 },
 275         { .mbps =  180, .hsfreqrange = 0x22,    .osc_freq_target = 460 },
 276         { .mbps =  190, .hsfreqrange = 0x32,    .osc_freq_target = 460 },
 277         { .mbps =  205, .hsfreqrange = 0x03,    .osc_freq_target = 460 },
 278         { .mbps =  220, .hsfreqrange = 0x13,    .osc_freq_target = 460 },
 279         { .mbps =  235, .hsfreqrange = 0x23,    .osc_freq_target = 460 },
 280         { .mbps =  250, .hsfreqrange = 0x33,    .osc_freq_target = 460 },
 281         { .mbps =  275, .hsfreqrange = 0x04,    .osc_freq_target = 460 },
 282         { .mbps =  300, .hsfreqrange = 0x14,    .osc_freq_target = 460 },
 283         { .mbps =  325, .hsfreqrange = 0x25,    .osc_freq_target = 460 },
 284         { .mbps =  350, .hsfreqrange = 0x35,    .osc_freq_target = 460 },
 285         { .mbps =  400, .hsfreqrange = 0x05,    .osc_freq_target = 460 },
 286         { .mbps =  450, .hsfreqrange = 0x16,    .osc_freq_target = 460 },
 287         { .mbps =  500, .hsfreqrange = 0x26,    .osc_freq_target = 460 },
 288         { .mbps =  550, .hsfreqrange = 0x37,    .osc_freq_target = 460 },
 289         { .mbps =  600, .hsfreqrange = 0x07,    .osc_freq_target = 460 },
 290         { .mbps =  650, .hsfreqrange = 0x18,    .osc_freq_target = 460 },
 291         { .mbps =  700, .hsfreqrange = 0x28,    .osc_freq_target = 460 },
 292         { .mbps =  750, .hsfreqrange = 0x39,    .osc_freq_target = 460 },
 293         { .mbps =  800, .hsfreqrange = 0x09,    .osc_freq_target = 460 },
 294         { .mbps =  850, .hsfreqrange = 0x19,    .osc_freq_target = 460 },
 295         { .mbps =  900, .hsfreqrange = 0x29,    .osc_freq_target = 460 },
 296         { .mbps =  950, .hsfreqrange = 0x3a,    .osc_freq_target = 460 },
 297         { .mbps = 1000, .hsfreqrange = 0x0a,    .osc_freq_target = 460 },
 298         { .mbps = 1050, .hsfreqrange = 0x1a,    .osc_freq_target = 460 },
 299         { .mbps = 1100, .hsfreqrange = 0x2a,    .osc_freq_target = 460 },
 300         { .mbps = 1150, .hsfreqrange = 0x3b,    .osc_freq_target = 460 },
 301         { .mbps = 1200, .hsfreqrange = 0x0b,    .osc_freq_target = 460 },
 302         { .mbps = 1250, .hsfreqrange = 0x1b,    .osc_freq_target = 460 },
 303         { .mbps = 1300, .hsfreqrange = 0x2b,    .osc_freq_target = 460 },
 304         { .mbps = 1350, .hsfreqrange = 0x3c,    .osc_freq_target = 460 },
 305         { .mbps = 1400, .hsfreqrange = 0x0c,    .osc_freq_target = 460 },
 306         { .mbps = 1450, .hsfreqrange = 0x1c,    .osc_freq_target = 460 },
 307         { .mbps = 1500, .hsfreqrange = 0x2c,    .osc_freq_target = 460 },
 308         { .mbps = 1550, .hsfreqrange = 0x3d,    .osc_freq_target = 285 },
 309         { .mbps = 1600, .hsfreqrange = 0x0d,    .osc_freq_target = 295 },
 310         { .mbps = 1650, .hsfreqrange = 0x1d,    .osc_freq_target = 304 },
 311         { .mbps = 1700, .hsfreqrange = 0x2e,    .osc_freq_target = 313 },
 312         { .mbps = 1750, .hsfreqrange = 0x3e,    .osc_freq_target = 322 },
 313         { .mbps = 1800, .hsfreqrange = 0x0e,    .osc_freq_target = 331 },
 314         { .mbps = 1850, .hsfreqrange = 0x1e,    .osc_freq_target = 341 },
 315         { .mbps = 1900, .hsfreqrange = 0x2f,    .osc_freq_target = 350 },
 316         { .mbps = 1950, .hsfreqrange = 0x3f,    .osc_freq_target = 359 },
 317         { .mbps = 2000, .hsfreqrange = 0x0f,    .osc_freq_target = 368 },
 318         { .mbps = 2050, .hsfreqrange = 0x40,    .osc_freq_target = 377 },
 319         { .mbps = 2100, .hsfreqrange = 0x41,    .osc_freq_target = 387 },
 320         { .mbps = 2150, .hsfreqrange = 0x42,    .osc_freq_target = 396 },
 321         { .mbps = 2200, .hsfreqrange = 0x43,    .osc_freq_target = 405 },
 322         { .mbps = 2250, .hsfreqrange = 0x44,    .osc_freq_target = 414 },
 323         { .mbps = 2300, .hsfreqrange = 0x45,    .osc_freq_target = 423 },
 324         { .mbps = 2350, .hsfreqrange = 0x46,    .osc_freq_target = 432 },
 325         { .mbps = 2400, .hsfreqrange = 0x47,    .osc_freq_target = 442 },
 326         { .mbps = 2450, .hsfreqrange = 0x48,    .osc_freq_target = 451 },
 327         { .mbps = 2500, .hsfreqrange = 0x49,    .osc_freq_target = 460 },
 328         { /* sentinel */ }
 329 };
 330
 331 static const struct v4l2_mbus_framefmt fmt_default = {
 332         .width = 640,
 333         .height = 480,
 334         .code = MEDIA_BUS_FMT_RGB565_1X16,
 335         .field = V4L2_FIELD_NONE,
 336         .colorspace = V4L2_COLORSPACE_REC709,
 337         .ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT,
 338         .quantization = V4L2_QUANTIZATION_DEFAULT,
 339         .xfer_func = V4L2_XFER_FUNC_DEFAULT,
 340 };
 341
 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 <[email protected]>");
1136 MODULE_DESCRIPTION("STM32 CSI controller");
1137 MODULE_LICENSE("GPL");