drivers/clk/clk_stm32mp1.c

   1 // SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
   2 /*
   3  * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
   4  */
   5
   6 #define LOG_CATEGORY UCLASS_CLK
   7
   8 #include <common.h>
   9 #include <clk-uclass.h>
  10 #include <div64.h>
  11 #include <dm.h>
  12 #include <init.h>
  13 #include <log.h>
  14 #include <regmap.h>
  15 #include <spl.h>
  16 #include <syscon.h>
  17 #include <time.h>
  18 #include <vsprintf.h>
  19 #include <asm/arch/sys_proto.h>
  20 #include <asm/global_data.h>
  21 #include <dm/device_compat.h>
  22 #include <dt-bindings/clock/stm32mp1-clks.h>
  23 #include <dt-bindings/clock/stm32mp1-clksrc.h>
  24 #include <linux/bitops.h>
  25 #include <linux/io.h>
  26 #include <linux/iopoll.h>
  27
  28 DECLARE_GLOBAL_DATA_PTR;
  29
  30 #ifndef CONFIG_TFABOOT
  31 #if !defined(CONFIG_SPL) || defined(CONFIG_SPL_BUILD)
  32 /* activate clock tree initialization in the driver */
  33 #define STM32MP1_CLOCK_TREE_INIT
  34 #endif
  35 #endif
  36
  37 #define MAX_HSI_HZ              64000000
  38
  39 /* TIMEOUT */
  40 #define TIMEOUT_200MS           200000
  41 #define TIMEOUT_1S              1000000
  42
  43 /* STGEN registers */
  44 #define STGENC_CNTCR            0x00
  45 #define STGENC_CNTSR            0x04
  46 #define STGENC_CNTCVL           0x08
  47 #define STGENC_CNTCVU           0x0C
  48 #define STGENC_CNTFID0          0x20
  49
  50 #define STGENC_CNTCR_EN         BIT(0)
  51
  52 /* RCC registers */
  53 #define RCC_OCENSETR            0x0C
  54 #define RCC_OCENCLRR            0x10
  55 #define RCC_HSICFGR             0x18
  56 #define RCC_MPCKSELR            0x20
  57 #define RCC_ASSCKSELR           0x24
  58 #define RCC_RCK12SELR           0x28
  59 #define RCC_MPCKDIVR            0x2C
  60 #define RCC_AXIDIVR             0x30
  61 #define RCC_APB4DIVR            0x3C
  62 #define RCC_APB5DIVR            0x40
  63 #define RCC_RTCDIVR             0x44
  64 #define RCC_MSSCKSELR           0x48
  65 #define RCC_PLL1CR              0x80
  66 #define RCC_PLL1CFGR1           0x84
  67 #define RCC_PLL1CFGR2           0x88
  68 #define RCC_PLL1FRACR           0x8C
  69 #define RCC_PLL1CSGR            0x90
  70 #define RCC_PLL2CR              0x94
  71 #define RCC_PLL2CFGR1           0x98
  72 #define RCC_PLL2CFGR2           0x9C
  73 #define RCC_PLL2FRACR           0xA0
  74 #define RCC_PLL2CSGR            0xA4
  75 #define RCC_I2C46CKSELR         0xC0
  76 #define RCC_SPI6CKSELR          0xC4
  77 #define RCC_CPERCKSELR          0xD0
  78 #define RCC_STGENCKSELR         0xD4
  79 #define RCC_DDRITFCR            0xD8
  80 #define RCC_BDCR                0x140
  81 #define RCC_RDLSICR             0x144
  82 #define RCC_MP_APB4ENSETR       0x200
  83 #define RCC_MP_APB5ENSETR       0x208
  84 #define RCC_MP_AHB5ENSETR       0x210
  85 #define RCC_MP_AHB6ENSETR       0x218
  86 #define RCC_OCRDYR              0x808
  87 #define RCC_DBGCFGR             0x80C
  88 #define RCC_RCK3SELR            0x820
  89 #define RCC_RCK4SELR            0x824
  90 #define RCC_MCUDIVR             0x830
  91 #define RCC_APB1DIVR            0x834
  92 #define RCC_APB2DIVR            0x838
  93 #define RCC_APB3DIVR            0x83C
  94 #define RCC_PLL3CR              0x880
  95 #define RCC_PLL3CFGR1           0x884
  96 #define RCC_PLL3CFGR2           0x888
  97 #define RCC_PLL3FRACR           0x88C
  98 #define RCC_PLL3CSGR            0x890
  99 #define RCC_PLL4CR              0x894
 100 #define RCC_PLL4CFGR1           0x898
 101 #define RCC_PLL4CFGR2           0x89C
 102 #define RCC_PLL4FRACR           0x8A0
 103 #define RCC_PLL4CSGR            0x8A4
 104 #define RCC_I2C12CKSELR         0x8C0
 105 #define RCC_I2C35CKSELR         0x8C4
 106 #define RCC_SPI2S1CKSELR        0x8D8
 107 #define RCC_SPI2S23CKSELR       0x8DC
 108 #define RCC_SPI45CKSELR         0x8E0
 109 #define RCC_UART6CKSELR         0x8E4
 110 #define RCC_UART24CKSELR        0x8E8
 111 #define RCC_UART35CKSELR        0x8EC
 112 #define RCC_UART78CKSELR        0x8F0
 113 #define RCC_SDMMC12CKSELR       0x8F4
 114 #define RCC_SDMMC3CKSELR        0x8F8
 115 #define RCC_ETHCKSELR           0x8FC
 116 #define RCC_QSPICKSELR          0x900
 117 #define RCC_FMCCKSELR           0x904
 118 #define RCC_USBCKSELR           0x91C
 119 #define RCC_DSICKSELR           0x924
 120 #define RCC_ADCCKSELR           0x928
 121 #define RCC_MP_APB1ENSETR       0xA00
 122 #define RCC_MP_APB2ENSETR       0XA08
 123 #define RCC_MP_APB3ENSETR       0xA10
 124 #define RCC_MP_AHB2ENSETR       0xA18
 125 #define RCC_MP_AHB3ENSETR       0xA20
 126 #define RCC_MP_AHB4ENSETR       0xA28
 127
 128 /* used for most of SELR register */
 129 #define RCC_SELR_SRC_MASK       GENMASK(2, 0)
 130 #define RCC_SELR_SRCRDY         BIT(31)
 131
 132 /* Values of RCC_MPCKSELR register */
 133 #define RCC_MPCKSELR_HSI        0
 134 #define RCC_MPCKSELR_HSE        1
 135 #define RCC_MPCKSELR_PLL        2
 136 #define RCC_MPCKSELR_PLL_MPUDIV 3
 137
 138 /* Values of RCC_ASSCKSELR register */
 139 #define RCC_ASSCKSELR_HSI       0
 140 #define RCC_ASSCKSELR_HSE       1
 141 #define RCC_ASSCKSELR_PLL       2
 142
 143 /* Values of RCC_MSSCKSELR register */
 144 #define RCC_MSSCKSELR_HSI       0
 145 #define RCC_MSSCKSELR_HSE       1
 146 #define RCC_MSSCKSELR_CSI       2
 147 #define RCC_MSSCKSELR_PLL       3
 148
 149 /* Values of RCC_CPERCKSELR register */
 150 #define RCC_CPERCKSELR_HSI      0
 151 #define RCC_CPERCKSELR_CSI      1
 152 #define RCC_CPERCKSELR_HSE      2
 153
 154 /* used for most of DIVR register : max div for RTC */
 155 #define RCC_DIVR_DIV_MASK       GENMASK(5, 0)
 156 #define RCC_DIVR_DIVRDY         BIT(31)
 157
 158 /* Masks for specific DIVR registers */
 159 #define RCC_APBXDIV_MASK        GENMASK(2, 0)
 160 #define RCC_MPUDIV_MASK         GENMASK(2, 0)
 161 #define RCC_AXIDIV_MASK         GENMASK(2, 0)
 162 #define RCC_MCUDIV_MASK         GENMASK(3, 0)
 163
 164 /*  offset between RCC_MP_xxxENSETR and RCC_MP_xxxENCLRR registers */
 165 #define RCC_MP_ENCLRR_OFFSET    4
 166
 167 /* Fields of RCC_BDCR register */
 168 #define RCC_BDCR_LSEON          BIT(0)
 169 #define RCC_BDCR_LSEBYP         BIT(1)
 170 #define RCC_BDCR_LSERDY         BIT(2)
 171 #define RCC_BDCR_DIGBYP         BIT(3)
 172 #define RCC_BDCR_LSEDRV_MASK    GENMASK(5, 4)
 173 #define RCC_BDCR_LSEDRV_SHIFT   4
 174 #define RCC_BDCR_LSECSSON       BIT(8)
 175 #define RCC_BDCR_RTCCKEN        BIT(20)
 176 #define RCC_BDCR_RTCSRC_MASK    GENMASK(17, 16)
 177 #define RCC_BDCR_RTCSRC_SHIFT   16
 178
 179 /* Fields of RCC_RDLSICR register */
 180 #define RCC_RDLSICR_LSION       BIT(0)
 181 #define RCC_RDLSICR_LSIRDY      BIT(1)
 182
 183 /* used for ALL PLLNCR registers */
 184 #define RCC_PLLNCR_PLLON        BIT(0)
 185 #define RCC_PLLNCR_PLLRDY       BIT(1)
 186 #define RCC_PLLNCR_SSCG_CTRL    BIT(2)
 187 #define RCC_PLLNCR_DIVPEN       BIT(4)
 188 #define RCC_PLLNCR_DIVQEN       BIT(5)
 189 #define RCC_PLLNCR_DIVREN       BIT(6)
 190 #define RCC_PLLNCR_DIVEN_SHIFT  4
 191
 192 /* used for ALL PLLNCFGR1 registers */
 193 #define RCC_PLLNCFGR1_DIVM_SHIFT        16
 194 #define RCC_PLLNCFGR1_DIVM_MASK         GENMASK(21, 16)
 195 #define RCC_PLLNCFGR1_DIVN_SHIFT        0
 196 #define RCC_PLLNCFGR1_DIVN_MASK         GENMASK(8, 0)
 197 /* only for PLL3 and PLL4 */
 198 #define RCC_PLLNCFGR1_IFRGE_SHIFT       24
 199 #define RCC_PLLNCFGR1_IFRGE_MASK        GENMASK(25, 24)
 200
 201 /* used for ALL PLLNCFGR2 registers , using stm32mp1_div_id */
 202 #define RCC_PLLNCFGR2_SHIFT(div_id)     ((div_id) * 8)
 203 #define RCC_PLLNCFGR2_DIVX_MASK         GENMASK(6, 0)
 204 #define RCC_PLLNCFGR2_DIVP_SHIFT        RCC_PLLNCFGR2_SHIFT(_DIV_P)
 205 #define RCC_PLLNCFGR2_DIVP_MASK         GENMASK(6, 0)
 206 #define RCC_PLLNCFGR2_DIVQ_SHIFT        RCC_PLLNCFGR2_SHIFT(_DIV_Q)
 207 #define RCC_PLLNCFGR2_DIVQ_MASK         GENMASK(14, 8)
 208 #define RCC_PLLNCFGR2_DIVR_SHIFT        RCC_PLLNCFGR2_SHIFT(_DIV_R)
 209 #define RCC_PLLNCFGR2_DIVR_MASK         GENMASK(22, 16)
 210
 211 /* used for ALL PLLNFRACR registers */
 212 #define RCC_PLLNFRACR_FRACV_SHIFT       3
 213 #define RCC_PLLNFRACR_FRACV_MASK        GENMASK(15, 3)
 214 #define RCC_PLLNFRACR_FRACLE            BIT(16)
 215
 216 /* used for ALL PLLNCSGR registers */
 217 #define RCC_PLLNCSGR_INC_STEP_SHIFT     16
 218 #define RCC_PLLNCSGR_INC_STEP_MASK      GENMASK(30, 16)
 219 #define RCC_PLLNCSGR_MOD_PER_SHIFT      0
 220 #define RCC_PLLNCSGR_MOD_PER_MASK       GENMASK(12, 0)
 221 #define RCC_PLLNCSGR_SSCG_MODE_SHIFT    15
 222 #define RCC_PLLNCSGR_SSCG_MODE_MASK     BIT(15)
 223
 224 /* used for RCC_OCENSETR and RCC_OCENCLRR registers */
 225 #define RCC_OCENR_HSION                 BIT(0)
 226 #define RCC_OCENR_CSION                 BIT(4)
 227 #define RCC_OCENR_DIGBYP                BIT(7)
 228 #define RCC_OCENR_HSEON                 BIT(8)
 229 #define RCC_OCENR_HSEBYP                BIT(10)
 230 #define RCC_OCENR_HSECSSON              BIT(11)
 231
 232 /* Fields of RCC_OCRDYR register */
 233 #define RCC_OCRDYR_HSIRDY               BIT(0)
 234 #define RCC_OCRDYR_HSIDIVRDY            BIT(2)
 235 #define RCC_OCRDYR_CSIRDY               BIT(4)
 236 #define RCC_OCRDYR_HSERDY               BIT(8)
 237
 238 /* Fields of DDRITFCR register */
 239 #define RCC_DDRITFCR_DDRCKMOD_MASK      GENMASK(22, 20)
 240 #define RCC_DDRITFCR_DDRCKMOD_SHIFT     20
 241 #define RCC_DDRITFCR_DDRCKMOD_SSR       0
 242
 243 /* Fields of RCC_HSICFGR register */
 244 #define RCC_HSICFGR_HSIDIV_MASK         GENMASK(1, 0)
 245
 246 /* used for MCO related operations */
 247 #define RCC_MCOCFG_MCOON                BIT(12)
 248 #define RCC_MCOCFG_MCODIV_MASK          GENMASK(7, 4)
 249 #define RCC_MCOCFG_MCODIV_SHIFT         4
 250 #define RCC_MCOCFG_MCOSRC_MASK          GENMASK(2, 0)
 251
 252 enum stm32mp1_parent_id {
 253 /*
 254  * _HSI, _HSE, _CSI, _LSI, _LSE should not be moved
 255  * they are used as index in osc_clk[] as clock reference
 256  */
 257         _HSI,
 258         _HSE,
 259         _CSI,
 260         _LSI,
 261         _LSE,
 262         _I2S_CKIN,
 263         NB_OSC,
 264
 265 /* other parent source */
 266         _HSI_KER = NB_OSC,
 267         _HSE_KER,
 268         _HSE_KER_DIV2,
 269         _CSI_KER,
 270         _PLL1_P,
 271         _PLL1_Q,
 272         _PLL1_R,
 273         _PLL2_P,
 274         _PLL2_Q,
 275         _PLL2_R,
 276         _PLL3_P,
 277         _PLL3_Q,
 278         _PLL3_R,
 279         _PLL4_P,
 280         _PLL4_Q,
 281         _PLL4_R,
 282         _ACLK,
 283         _PCLK1,
 284         _PCLK2,
 285         _PCLK3,
 286         _PCLK4,
 287         _PCLK5,
 288         _HCLK6,
 289         _HCLK2,
 290         _CK_PER,
 291         _CK_MPU,
 292         _CK_MCU,
 293         _DSI_PHY,
 294         _USB_PHY_48,
 295         _PARENT_NB,
 296         _UNKNOWN_ID = 0xff,
 297 };
 298
 299 enum stm32mp1_parent_sel {
 300         _I2C12_SEL,
 301         _I2C35_SEL,
 302         _I2C46_SEL,
 303         _UART6_SEL,
 304         _UART24_SEL,
 305         _UART35_SEL,
 306         _UART78_SEL,
 307         _SDMMC12_SEL,
 308         _SDMMC3_SEL,
 309         _ETH_SEL,
 310         _QSPI_SEL,
 311         _FMC_SEL,
 312         _USBPHY_SEL,
 313         _USBO_SEL,
 314         _STGEN_SEL,
 315         _DSI_SEL,
 316         _ADC12_SEL,
 317         _SPI1_SEL,
 318         _SPI23_SEL,
 319         _SPI45_SEL,
 320         _SPI6_SEL,
 321         _RTC_SEL,
 322         _PARENT_SEL_NB,
 323         _UNKNOWN_SEL = 0xff,
 324 };
 325
 326 enum stm32mp1_pll_id {
 327         _PLL1,
 328         _PLL2,
 329         _PLL3,
 330         _PLL4,
 331         _PLL_NB
 332 };
 333
 334 enum stm32mp1_div_id {
 335         _DIV_P,
 336         _DIV_Q,
 337         _DIV_R,
 338         _DIV_NB,
 339 };
 340
 341 enum stm32mp1_clksrc_id {
 342         CLKSRC_MPU,
 343         CLKSRC_AXI,
 344         CLKSRC_MCU,
 345         CLKSRC_PLL12,
 346         CLKSRC_PLL3,
 347         CLKSRC_PLL4,
 348         CLKSRC_RTC,
 349         CLKSRC_MCO1,
 350         CLKSRC_MCO2,
 351         CLKSRC_NB
 352 };
 353
 354 enum stm32mp1_clkdiv_id {
 355         CLKDIV_MPU,
 356         CLKDIV_AXI,
 357         CLKDIV_MCU,
 358         CLKDIV_APB1,
 359         CLKDIV_APB2,
 360         CLKDIV_APB3,
 361         CLKDIV_APB4,
 362         CLKDIV_APB5,
 363         CLKDIV_RTC,
 364         CLKDIV_MCO1,
 365         CLKDIV_MCO2,
 366         CLKDIV_NB
 367 };
 368
 369 enum stm32mp1_pllcfg {
 370         PLLCFG_M,
 371         PLLCFG_N,
 372         PLLCFG_P,
 373         PLLCFG_Q,
 374         PLLCFG_R,
 375         PLLCFG_O,
 376         PLLCFG_NB
 377 };
 378
 379 enum stm32mp1_pllcsg {
 380         PLLCSG_MOD_PER,
 381         PLLCSG_INC_STEP,
 382         PLLCSG_SSCG_MODE,
 383         PLLCSG_NB
 384 };
 385
 386 enum stm32mp1_plltype {
 387         PLL_800,
 388         PLL_1600,
 389         PLL_TYPE_NB
 390 };
 391
 392 struct stm32mp1_pll {
 393         u8 refclk_min;
 394         u8 refclk_max;
 395         u8 divn_max;
 396 };
 397
 398 struct stm32mp1_clk_gate {
 399         u16 offset;
 400         u8 bit;
 401         u8 index;
 402         u8 set_clr;
 403         u8 sel;
 404         u8 fixed;
 405 };
 406
 407 struct stm32mp1_clk_sel {
 408         u16 offset;
 409         u8 src;
 410         u8 msk;
 411         u8 nb_parent;
 412         const u8 *parent;
 413 };
 414
 415 #define REFCLK_SIZE 4
 416 struct stm32mp1_clk_pll {
 417         enum stm32mp1_plltype plltype;
 418         u16 rckxselr;
 419         u16 pllxcfgr1;
 420         u16 pllxcfgr2;
 421         u16 pllxfracr;
 422         u16 pllxcr;
 423         u16 pllxcsgr;
 424         u8 refclk[REFCLK_SIZE];
 425 };
 426
 427 struct stm32mp1_clk_data {
 428         const struct stm32mp1_clk_gate *gate;
 429         const struct stm32mp1_clk_sel *sel;
 430         const struct stm32mp1_clk_pll *pll;
 431         const int nb_gate;
 432 };
 433
 434 struct stm32mp1_clk_priv {
 435         fdt_addr_t base;
 436         const struct stm32mp1_clk_data *data;
 437         struct clk osc_clk[NB_OSC];
 438 };
 439
 440 #define STM32MP1_CLK(off, b, idx, s)            \
 441         {                                       \
 442                 .offset = (off),                \
 443                 .bit = (b),                     \
 444                 .index = (idx),                 \
 445                 .set_clr = 0,                   \
 446                 .sel = (s),                     \
 447                 .fixed = _UNKNOWN_ID,           \
 448         }
 449
 450 #define STM32MP1_CLK_F(off, b, idx, f)          \
 451         {                                       \
 452                 .offset = (off),                \
 453                 .bit = (b),                     \
 454                 .index = (idx),                 \
 455                 .set_clr = 0,                   \
 456                 .sel = _UNKNOWN_SEL,            \
 457                 .fixed = (f),                   \
 458         }
 459
 460 #define STM32MP1_CLK_SET_CLR(off, b, idx, s)    \
 461         {                                       \
 462                 .offset = (off),                \
 463                 .bit = (b),                     \
 464                 .index = (idx),                 \
 465                 .set_clr = 1,                   \
 466                 .sel = (s),                     \
 467                 .fixed = _UNKNOWN_ID,           \
 468         }
 469
 470 #define STM32MP1_CLK_SET_CLR_F(off, b, idx, f)  \
 471         {                                       \
 472                 .offset = (off),                \
 473                 .bit = (b),                     \
 474                 .index = (idx),                 \
 475                 .set_clr = 1,                   \
 476                 .sel = _UNKNOWN_SEL,            \
 477                 .fixed = (f),                   \
 478         }
 479
 480 #define STM32MP1_CLK_PARENT(idx, off, s, m, p)   \
 481         [(idx)] = {                             \
 482                 .offset = (off),                \
 483                 .src = (s),                     \
 484                 .msk = (m),                     \
 485                 .parent = (p),                  \
 486                 .nb_parent = ARRAY_SIZE((p))    \
 487         }
 488
 489 #define STM32MP1_CLK_PLL(idx, type, off1, off2, off3, off4, off5, off6,\
 490                         p1, p2, p3, p4) \
 491         [(idx)] = {                             \
 492                 .plltype = (type),                      \
 493                 .rckxselr = (off1),             \
 494                 .pllxcfgr1 = (off2),            \
 495                 .pllxcfgr2 = (off3),            \
 496                 .pllxfracr = (off4),            \
 497                 .pllxcr = (off5),               \
 498                 .pllxcsgr = (off6),             \
 499                 .refclk[0] = (p1),              \
 500                 .refclk[1] = (p2),              \
 501                 .refclk[2] = (p3),              \
 502                 .refclk[3] = (p4),              \
 503         }
 504
 505 static const u8 stm32mp1_clks[][2] = {
 506         {CK_PER, _CK_PER},
 507         {CK_MPU, _CK_MPU},
 508         {CK_AXI, _ACLK},
 509         {CK_MCU, _CK_MCU},
 510         {CK_HSE, _HSE},
 511         {CK_CSI, _CSI},
 512         {CK_LSI, _LSI},
 513         {CK_LSE, _LSE},
 514         {CK_HSI, _HSI},
 515         {CK_HSE_DIV2, _HSE_KER_DIV2},
 516 };
 517
 518 static const struct stm32mp1_clk_gate stm32mp1_clk_gate[] = {
 519         STM32MP1_CLK(RCC_DDRITFCR, 0, DDRC1, _UNKNOWN_SEL),
 520         STM32MP1_CLK(RCC_DDRITFCR, 1, DDRC1LP, _UNKNOWN_SEL),
 521         STM32MP1_CLK(RCC_DDRITFCR, 2, DDRC2, _UNKNOWN_SEL),
 522         STM32MP1_CLK(RCC_DDRITFCR, 3, DDRC2LP, _UNKNOWN_SEL),
 523         STM32MP1_CLK_F(RCC_DDRITFCR, 4, DDRPHYC, _PLL2_R),
 524         STM32MP1_CLK(RCC_DDRITFCR, 5, DDRPHYCLP, _UNKNOWN_SEL),
 525         STM32MP1_CLK(RCC_DDRITFCR, 6, DDRCAPB, _UNKNOWN_SEL),
 526         STM32MP1_CLK(RCC_DDRITFCR, 7, DDRCAPBLP, _UNKNOWN_SEL),
 527         STM32MP1_CLK(RCC_DDRITFCR, 8, AXIDCG, _UNKNOWN_SEL),
 528         STM32MP1_CLK(RCC_DDRITFCR, 9, DDRPHYCAPB, _UNKNOWN_SEL),
 529         STM32MP1_CLK(RCC_DDRITFCR, 10, DDRPHYCAPBLP, _UNKNOWN_SEL),
 530
 531         STM32MP1_CLK_SET_CLR(RCC_MP_APB1ENSETR, 11, SPI2_K, _SPI23_SEL),
 532         STM32MP1_CLK_SET_CLR(RCC_MP_APB1ENSETR, 12, SPI3_K, _SPI23_SEL),
 533         STM32MP1_CLK_SET_CLR(RCC_MP_APB1ENSETR, 14, USART2_K, _UART24_SEL),
 534         STM32MP1_CLK_SET_CLR(RCC_MP_APB1ENSETR, 15, USART3_K, _UART35_SEL),
 535         STM32MP1_CLK_SET_CLR(RCC_MP_APB1ENSETR, 16, UART4_K, _UART24_SEL),
 536         STM32MP1_CLK_SET_CLR(RCC_MP_APB1ENSETR, 17, UART5_K, _UART35_SEL),
 537         STM32MP1_CLK_SET_CLR(RCC_MP_APB1ENSETR, 18, UART7_K, _UART78_SEL),
 538         STM32MP1_CLK_SET_CLR(RCC_MP_APB1ENSETR, 19, UART8_K, _UART78_SEL),
 539         STM32MP1_CLK_SET_CLR(RCC_MP_APB1ENSETR, 21, I2C1_K, _I2C12_SEL),
 540         STM32MP1_CLK_SET_CLR(RCC_MP_APB1ENSETR, 22, I2C2_K, _I2C12_SEL),
 541         STM32MP1_CLK_SET_CLR(RCC_MP_APB1ENSETR, 23, I2C3_K, _I2C35_SEL),
 542         STM32MP1_CLK_SET_CLR(RCC_MP_APB1ENSETR, 24, I2C5_K, _I2C35_SEL),
 543
 544         STM32MP1_CLK_SET_CLR(RCC_MP_APB2ENSETR, 8, SPI1_K, _SPI1_SEL),
 545         STM32MP1_CLK_SET_CLR(RCC_MP_APB2ENSETR, 9, SPI4_K, _SPI45_SEL),
 546         STM32MP1_CLK_SET_CLR(RCC_MP_APB2ENSETR, 10, SPI5_K, _SPI45_SEL),
 547         STM32MP1_CLK_SET_CLR(RCC_MP_APB2ENSETR, 13, USART6_K, _UART6_SEL),
 548
 549         STM32MP1_CLK_SET_CLR_F(RCC_MP_APB3ENSETR, 13, VREF, _PCLK3),
 550         STM32MP1_CLK_SET_CLR_F(RCC_MP_APB3ENSETR, 11, SYSCFG, _UNKNOWN_SEL),
 551
 552         STM32MP1_CLK_SET_CLR_F(RCC_MP_APB4ENSETR, 0, LTDC_PX, _PLL4_Q),
 553         STM32MP1_CLK_SET_CLR_F(RCC_MP_APB4ENSETR, 4, DSI_PX, _PLL4_Q),
 554         STM32MP1_CLK_SET_CLR(RCC_MP_APB4ENSETR, 4, DSI_K, _DSI_SEL),
 555         STM32MP1_CLK_SET_CLR(RCC_MP_APB4ENSETR, 8, DDRPERFM, _UNKNOWN_SEL),
 556         STM32MP1_CLK_SET_CLR(RCC_MP_APB4ENSETR, 15, IWDG2, _UNKNOWN_SEL),
 557         STM32MP1_CLK_SET_CLR(RCC_MP_APB4ENSETR, 16, USBPHY_K, _USBPHY_SEL),
 558
 559         STM32MP1_CLK_SET_CLR(RCC_MP_APB5ENSETR, 0, SPI6_K, _SPI6_SEL),
 560         STM32MP1_CLK_SET_CLR(RCC_MP_APB5ENSETR, 2, I2C4_K, _I2C46_SEL),
 561         STM32MP1_CLK_SET_CLR(RCC_MP_APB5ENSETR, 3, I2C6_K, _I2C46_SEL),
 562         STM32MP1_CLK_SET_CLR(RCC_MP_APB5ENSETR, 8, RTCAPB, _PCLK5),
 563         STM32MP1_CLK_SET_CLR(RCC_MP_APB5ENSETR, 16, BSEC, _UNKNOWN_SEL),
 564         STM32MP1_CLK_SET_CLR(RCC_MP_APB5ENSETR, 20, STGEN_K, _STGEN_SEL),
 565
 566         STM32MP1_CLK_SET_CLR_F(RCC_MP_AHB2ENSETR, 5, ADC12, _HCLK2),
 567         STM32MP1_CLK_SET_CLR(RCC_MP_AHB2ENSETR, 5, ADC12_K, _ADC12_SEL),
 568         STM32MP1_CLK_SET_CLR(RCC_MP_AHB2ENSETR, 8, USBO_K, _USBO_SEL),
 569         STM32MP1_CLK_SET_CLR(RCC_MP_AHB2ENSETR, 16, SDMMC3_K, _SDMMC3_SEL),
 570
 571         STM32MP1_CLK_SET_CLR(RCC_MP_AHB3ENSETR, 11, HSEM, _UNKNOWN_SEL),
 572         STM32MP1_CLK_SET_CLR(RCC_MP_AHB3ENSETR, 12, IPCC, _UNKNOWN_SEL),
 573
 574         STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 0, GPIOA, _UNKNOWN_SEL),
 575         STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 1, GPIOB, _UNKNOWN_SEL),
 576         STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 2, GPIOC, _UNKNOWN_SEL),
 577         STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 3, GPIOD, _UNKNOWN_SEL),
 578         STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 4, GPIOE, _UNKNOWN_SEL),
 579         STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 5, GPIOF, _UNKNOWN_SEL),
 580         STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 6, GPIOG, _UNKNOWN_SEL),
 581         STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 7, GPIOH, _UNKNOWN_SEL),
 582         STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 8, GPIOI, _UNKNOWN_SEL),
 583         STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 9, GPIOJ, _UNKNOWN_SEL),
 584         STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 10, GPIOK, _UNKNOWN_SEL),
 585
 586         STM32MP1_CLK_SET_CLR(RCC_MP_AHB5ENSETR, 0, GPIOZ, _UNKNOWN_SEL),
 587         STM32MP1_CLK_SET_CLR(RCC_MP_AHB5ENSETR, 6, RNG1_K, _UNKNOWN_SEL),
 588
 589         STM32MP1_CLK_SET_CLR(RCC_MP_AHB6ENSETR, 7, ETHCK_K, _ETH_SEL),
 590         STM32MP1_CLK_SET_CLR(RCC_MP_AHB6ENSETR, 8, ETHTX, _UNKNOWN_SEL),
 591         STM32MP1_CLK_SET_CLR(RCC_MP_AHB6ENSETR, 9, ETHRX, _UNKNOWN_SEL),
 592         STM32MP1_CLK_SET_CLR_F(RCC_MP_AHB6ENSETR, 10, ETHMAC, _ACLK),
 593         STM32MP1_CLK_SET_CLR(RCC_MP_AHB6ENSETR, 12, FMC_K, _FMC_SEL),
 594         STM32MP1_CLK_SET_CLR(RCC_MP_AHB6ENSETR, 14, QSPI_K, _QSPI_SEL),
 595         STM32MP1_CLK_SET_CLR(RCC_MP_AHB6ENSETR, 16, SDMMC1_K, _SDMMC12_SEL),
 596         STM32MP1_CLK_SET_CLR(RCC_MP_AHB6ENSETR, 17, SDMMC2_K, _SDMMC12_SEL),
 597         STM32MP1_CLK_SET_CLR(RCC_MP_AHB6ENSETR, 24, USBH, _UNKNOWN_SEL),
 598
 599         STM32MP1_CLK(RCC_DBGCFGR, 8, CK_DBG, _UNKNOWN_SEL),
 600
 601         STM32MP1_CLK(RCC_BDCR, 20, RTC, _RTC_SEL),
 602 };
 603
 604 static const u8 i2c12_parents[] = {_PCLK1, _PLL4_R, _HSI_KER, _CSI_KER};
 605 static const u8 i2c35_parents[] = {_PCLK1, _PLL4_R, _HSI_KER, _CSI_KER};
 606 static const u8 i2c46_parents[] = {_PCLK5, _PLL3_Q, _HSI_KER, _CSI_KER};
 607 static const u8 uart6_parents[] = {_PCLK2, _PLL4_Q, _HSI_KER, _CSI_KER,
 608                                         _HSE_KER};
 609 static const u8 uart24_parents[] = {_PCLK1, _PLL4_Q, _HSI_KER, _CSI_KER,
 610                                          _HSE_KER};
 611 static const u8 uart35_parents[] = {_PCLK1, _PLL4_Q, _HSI_KER, _CSI_KER,
 612                                          _HSE_KER};
 613 static const u8 uart78_parents[] = {_PCLK1, _PLL4_Q, _HSI_KER, _CSI_KER,
 614                                          _HSE_KER};
 615 static const u8 sdmmc12_parents[] = {_HCLK6, _PLL3_R, _PLL4_P, _HSI_KER};
 616 static const u8 sdmmc3_parents[] = {_HCLK2, _PLL3_R, _PLL4_P, _HSI_KER};
 617 static const u8 eth_parents[] = {_PLL4_P, _PLL3_Q};
 618 static const u8 qspi_parents[] = {_ACLK, _PLL3_R, _PLL4_P, _CK_PER};
 619 static const u8 fmc_parents[] = {_ACLK, _PLL3_R, _PLL4_P, _CK_PER};
 620 static const u8 usbphy_parents[] = {_HSE_KER, _PLL4_R, _HSE_KER_DIV2};
 621 static const u8 usbo_parents[] = {_PLL4_R, _USB_PHY_48};
 622 static const u8 stgen_parents[] = {_HSI_KER, _HSE_KER};
 623 static const u8 dsi_parents[] = {_DSI_PHY, _PLL4_P};
 624 static const u8 adc_parents[] = {_PLL4_R, _CK_PER, _PLL3_Q};
 625 /* same parents for SPI1=RCC_SPI2S1CKSELR and SPI2&3 = RCC_SPI2S23CKSELR */
 626 static const u8 spi_parents[] = {_PLL4_P, _PLL3_Q, _I2S_CKIN, _CK_PER,
 627                                  _PLL3_R};
 628 static const u8 spi45_parents[] = {_PCLK2, _PLL4_Q, _HSI_KER, _CSI_KER,
 629                                    _HSE_KER};
 630 static const u8 spi6_parents[] = {_PCLK5, _PLL4_Q, _HSI_KER, _CSI_KER,
 631                                   _HSE_KER, _PLL3_Q};
 632 static const u8 rtc_parents[] = {_UNKNOWN_ID, _LSE, _LSI, _HSE};
 633
 634 static const struct stm32mp1_clk_sel stm32mp1_clk_sel[_PARENT_SEL_NB] = {
 635         STM32MP1_CLK_PARENT(_I2C12_SEL, RCC_I2C12CKSELR, 0, 0x7, i2c12_parents),
 636         STM32MP1_CLK_PARENT(_I2C35_SEL, RCC_I2C35CKSELR, 0, 0x7, i2c35_parents),
 637         STM32MP1_CLK_PARENT(_I2C46_SEL, RCC_I2C46CKSELR, 0, 0x7, i2c46_parents),
 638         STM32MP1_CLK_PARENT(_UART6_SEL, RCC_UART6CKSELR, 0, 0x7, uart6_parents),
 639         STM32MP1_CLK_PARENT(_UART24_SEL, RCC_UART24CKSELR, 0, 0x7,
 640                             uart24_parents),
 641         STM32MP1_CLK_PARENT(_UART35_SEL, RCC_UART35CKSELR, 0, 0x7,
 642                             uart35_parents),
 643         STM32MP1_CLK_PARENT(_UART78_SEL, RCC_UART78CKSELR, 0, 0x7,
 644                             uart78_parents),
 645         STM32MP1_CLK_PARENT(_SDMMC12_SEL, RCC_SDMMC12CKSELR, 0, 0x7,
 646                             sdmmc12_parents),
 647         STM32MP1_CLK_PARENT(_SDMMC3_SEL, RCC_SDMMC3CKSELR, 0, 0x7,
 648                             sdmmc3_parents),
 649         STM32MP1_CLK_PARENT(_ETH_SEL, RCC_ETHCKSELR, 0, 0x3, eth_parents),
 650         STM32MP1_CLK_PARENT(_QSPI_SEL, RCC_QSPICKSELR, 0, 0x3, qspi_parents),
 651         STM32MP1_CLK_PARENT(_FMC_SEL, RCC_FMCCKSELR, 0, 0x3, fmc_parents),
 652         STM32MP1_CLK_PARENT(_USBPHY_SEL, RCC_USBCKSELR, 0, 0x3, usbphy_parents),
 653         STM32MP1_CLK_PARENT(_USBO_SEL, RCC_USBCKSELR, 4, 0x1, usbo_parents),
 654         STM32MP1_CLK_PARENT(_STGEN_SEL, RCC_STGENCKSELR, 0, 0x3, stgen_parents),
 655         STM32MP1_CLK_PARENT(_DSI_SEL, RCC_DSICKSELR, 0, 0x1, dsi_parents),
 656         STM32MP1_CLK_PARENT(_ADC12_SEL, RCC_ADCCKSELR, 0, 0x3, adc_parents),
 657         STM32MP1_CLK_PARENT(_SPI1_SEL, RCC_SPI2S1CKSELR, 0, 0x7, spi_parents),
 658         STM32MP1_CLK_PARENT(_SPI23_SEL, RCC_SPI2S23CKSELR, 0, 0x7, spi_parents),
 659         STM32MP1_CLK_PARENT(_SPI45_SEL, RCC_SPI45CKSELR, 0, 0x7, spi45_parents),
 660         STM32MP1_CLK_PARENT(_SPI6_SEL, RCC_SPI6CKSELR, 0, 0x7, spi6_parents),
 661         STM32MP1_CLK_PARENT(_RTC_SEL, RCC_BDCR, RCC_BDCR_RTCSRC_SHIFT,
 662                             (RCC_BDCR_RTCSRC_MASK >> RCC_BDCR_RTCSRC_SHIFT),
 663                             rtc_parents),
 664 };
 665
 666 #ifdef STM32MP1_CLOCK_TREE_INIT
 667
 668 /* define characteristic of PLL according type */
 669 #define DIVM_MIN        0
 670 #define DIVM_MAX        63
 671 #define DIVN_MIN        24
 672 #define DIVP_MIN        0
 673 #define DIVP_MAX        127
 674 #define FRAC_MAX        8192
 675
 676 #define PLL1600_VCO_MIN 800000000
 677 #define PLL1600_VCO_MAX 1600000000
 678
 679 static const struct stm32mp1_pll stm32mp1_pll[PLL_TYPE_NB] = {
 680         [PLL_800] = {
 681                 .refclk_min = 4,
 682                 .refclk_max = 16,
 683                 .divn_max = 99,
 684                 },
 685         [PLL_1600] = {
 686                 .refclk_min = 8,
 687                 .refclk_max = 16,
 688                 .divn_max = 199,
 689                 },
 690 };
 691 #endif /* STM32MP1_CLOCK_TREE_INIT */
 692
 693 static const struct stm32mp1_clk_pll stm32mp1_clk_pll[_PLL_NB] = {
 694         STM32MP1_CLK_PLL(_PLL1, PLL_1600,
 695                          RCC_RCK12SELR, RCC_PLL1CFGR1, RCC_PLL1CFGR2,
 696                          RCC_PLL1FRACR, RCC_PLL1CR, RCC_PLL1CSGR,
 697                          _HSI, _HSE, _UNKNOWN_ID, _UNKNOWN_ID),
 698         STM32MP1_CLK_PLL(_PLL2, PLL_1600,
 699                          RCC_RCK12SELR, RCC_PLL2CFGR1, RCC_PLL2CFGR2,
 700                          RCC_PLL2FRACR, RCC_PLL2CR, RCC_PLL2CSGR,
 701                          _HSI, _HSE, _UNKNOWN_ID, _UNKNOWN_ID),
 702         STM32MP1_CLK_PLL(_PLL3, PLL_800,
 703                          RCC_RCK3SELR, RCC_PLL3CFGR1, RCC_PLL3CFGR2,
 704                          RCC_PLL3FRACR, RCC_PLL3CR, RCC_PLL3CSGR,
 705                          _HSI, _HSE, _CSI, _UNKNOWN_ID),
 706         STM32MP1_CLK_PLL(_PLL4, PLL_800,
 707                          RCC_RCK4SELR, RCC_PLL4CFGR1, RCC_PLL4CFGR2,
 708                          RCC_PLL4FRACR, RCC_PLL4CR, RCC_PLL4CSGR,
 709                          _HSI, _HSE, _CSI, _I2S_CKIN),
 710 };
 711
 712 /* Prescaler table lookups for clock computation */
 713 /* div = /1 /2 /4 /8 / 16 /64 /128 /512 */
 714 static const u8 stm32mp1_mcu_div[16] = {
 715         0, 1, 2, 3, 4, 6, 7, 8, 9, 9, 9, 9, 9, 9, 9, 9
 716 };
 717
 718 /* div = /1 /2 /4 /8 /16 : same divider for pmu and apbx*/
 719 #define stm32mp1_mpu_div stm32mp1_mpu_apbx_div
 720 #define stm32mp1_apbx_div stm32mp1_mpu_apbx_div
 721 static const u8 stm32mp1_mpu_apbx_div[8] = {
 722         0, 1, 2, 3, 4, 4, 4, 4
 723 };
 724
 725 /* div = /1 /2 /3 /4 */
 726 static const u8 stm32mp1_axi_div[8] = {
 727         1, 2, 3, 4, 4, 4, 4, 4
 728 };
 729
 730 static const __maybe_unused
 731 char * const stm32mp1_clk_parent_name[_PARENT_NB] = {
 732         [_HSI] = "HSI",
 733         [_HSE] = "HSE",
 734         [_CSI] = "CSI",
 735         [_LSI] = "LSI",
 736         [_LSE] = "LSE",
 737         [_I2S_CKIN] = "I2S_CKIN",
 738         [_HSI_KER] = "HSI_KER",
 739         [_HSE_KER] = "HSE_KER",
 740         [_HSE_KER_DIV2] = "HSE_KER_DIV2",
 741         [_CSI_KER] = "CSI_KER",
 742         [_PLL1_P] = "PLL1_P",
 743         [_PLL1_Q] = "PLL1_Q",
 744         [_PLL1_R] = "PLL1_R",
 745         [_PLL2_P] = "PLL2_P",
 746         [_PLL2_Q] = "PLL2_Q",
 747         [_PLL2_R] = "PLL2_R",
 748         [_PLL3_P] = "PLL3_P",
 749         [_PLL3_Q] = "PLL3_Q",
 750         [_PLL3_R] = "PLL3_R",
 751         [_PLL4_P] = "PLL4_P",
 752         [_PLL4_Q] = "PLL4_Q",
 753         [_PLL4_R] = "PLL4_R",
 754         [_ACLK] = "ACLK",
 755         [_PCLK1] = "PCLK1",
 756         [_PCLK2] = "PCLK2",
 757         [_PCLK3] = "PCLK3",
 758         [_PCLK4] = "PCLK4",
 759         [_PCLK5] = "PCLK5",
 760         [_HCLK6] = "KCLK6",
 761         [_HCLK2] = "HCLK2",
 762         [_CK_PER] = "CK_PER",
 763         [_CK_MPU] = "CK_MPU",
 764         [_CK_MCU] = "CK_MCU",
 765         [_USB_PHY_48] = "USB_PHY_48",
 766         [_DSI_PHY] = "DSI_PHY_PLL",
 767 };
 768
 769 static const __maybe_unused
 770 char * const stm32mp1_clk_parent_sel_name[_PARENT_SEL_NB] = {
 771         [_I2C12_SEL] = "I2C12",
 772         [_I2C35_SEL] = "I2C35",
 773         [_I2C46_SEL] = "I2C46",
 774         [_UART6_SEL] = "UART6",
 775         [_UART24_SEL] = "UART24",
 776         [_UART35_SEL] = "UART35",
 777         [_UART78_SEL] = "UART78",
 778         [_SDMMC12_SEL] = "SDMMC12",
 779         [_SDMMC3_SEL] = "SDMMC3",
 780         [_ETH_SEL] = "ETH",
 781         [_QSPI_SEL] = "QSPI",
 782         [_FMC_SEL] = "FMC",
 783         [_USBPHY_SEL] = "USBPHY",
 784         [_USBO_SEL] = "USBO",
 785         [_STGEN_SEL] = "STGEN",
 786         [_DSI_SEL] = "DSI",
 787         [_ADC12_SEL] = "ADC12",
 788         [_SPI1_SEL] = "SPI1",
 789         [_SPI45_SEL] = "SPI45",
 790         [_RTC_SEL] = "RTC",
 791 };
 792
 793 static const struct stm32mp1_clk_data stm32mp1_data = {
 794         .gate = stm32mp1_clk_gate,
 795         .sel = stm32mp1_clk_sel,
 796         .pll = stm32mp1_clk_pll,
 797         .nb_gate = ARRAY_SIZE(stm32mp1_clk_gate),
 798 };
 799
 800 static ulong stm32mp1_clk_get_fixed(struct stm32mp1_clk_priv *priv, int idx)
 801 {
 802         if (idx >= NB_OSC) {
 803                 log_debug("clk id %d not found\n", idx);
 804                 return 0;
 805         }
 806
 807         return clk_get_rate(&priv->osc_clk[idx]);
 808 }
 809
 810 static int stm32mp1_clk_get_id(struct stm32mp1_clk_priv *priv, unsigned long id)
 811 {
 812         const struct stm32mp1_clk_gate *gate = priv->data->gate;
 813         int i, nb_clks = priv->data->nb_gate;
 814
 815         for (i = 0; i < nb_clks; i++) {
 816                 if (gate[i].index == id)
 817                         break;
 818         }
 819
 820         if (i == nb_clks) {
 821                 log_err("clk id %d not found\n", (u32)id);
 822                 return -EINVAL;
 823         }
 824
 825         return i;
 826 }
 827
 828 static int stm32mp1_clk_get_sel(struct stm32mp1_clk_priv *priv,
 829                                 int i)
 830 {
 831         const struct stm32mp1_clk_gate *gate = priv->data->gate;
 832
 833         if (gate[i].sel > _PARENT_SEL_NB) {
 834                 log_err("parents for clk id %d not found\n", i);
 835                 return -EINVAL;
 836         }
 837
 838         return gate[i].sel;
 839 }
 840
 841 static int stm32mp1_clk_get_fixed_parent(struct stm32mp1_clk_priv *priv,
 842                                          int i)
 843 {
 844         const struct stm32mp1_clk_gate *gate = priv->data->gate;
 845
 846         if (gate[i].fixed == _UNKNOWN_ID)
 847                 return -ENOENT;
 848
 849         return gate[i].fixed;
 850 }
 851
 852 static int stm32mp1_clk_get_parent(struct stm32mp1_clk_priv *priv,
 853                                    unsigned long id)
 854 {
 855         const struct stm32mp1_clk_sel *sel = priv->data->sel;
 856         int i;
 857         int s, p;
 858         unsigned int idx;
 859
 860         for (idx = 0; idx < ARRAY_SIZE(stm32mp1_clks); idx++)
 861                 if (stm32mp1_clks[idx][0] == id)
 862                         return stm32mp1_clks[idx][1];
 863
 864         i = stm32mp1_clk_get_id(priv, id);
 865         if (i < 0)
 866                 return i;
 867
 868         p = stm32mp1_clk_get_fixed_parent(priv, i);
 869         if (p >= 0 && p < _PARENT_NB)
 870                 return p;
 871
 872         s = stm32mp1_clk_get_sel(priv, i);
 873         if (s < 0)
 874                 return s;
 875
 876         p = (readl(priv->base + sel[s].offset) >> sel[s].src) & sel[s].msk;
 877
 878         if (p < sel[s].nb_parent) {
 879                 log_content("%s clock is the parent %s of clk id %d\n",
 880                             stm32mp1_clk_parent_name[sel[s].parent[p]],
 881                             stm32mp1_clk_parent_sel_name[s],
 882                             (u32)id);
 883                 return sel[s].parent[p];
 884         }
 885
 886         log_err("no parents defined for clk id %d\n", (u32)id);
 887
 888         return -EINVAL;
 889 }
 890
 891 static ulong  pll_get_fref_ck(struct stm32mp1_clk_priv *priv,
 892                               int pll_id)
 893 {
 894         const struct stm32mp1_clk_pll *pll = priv->data->pll;
 895         u32 selr;
 896         int src;
 897         ulong refclk;
 898
 899         /* Get current refclk */
 900         selr = readl(priv->base + pll[pll_id].rckxselr);
 901         src = selr & RCC_SELR_SRC_MASK;
 902
 903         refclk = stm32mp1_clk_get_fixed(priv, pll[pll_id].refclk[src]);
 904
 905         return refclk;
 906 }
 907
 908 /*
 909  * pll_get_fvco() : return the VCO or (VCO / 2) frequency for the requested PLL
 910  * - PLL1 & PLL2 => return VCO / 2 with Fpll_y_ck = FVCO / 2 * (DIVy + 1)
 911  * - PLL3 & PLL4 => return VCO     with Fpll_y_ck = FVCO / (DIVy + 1)
 912  * => in all the case Fpll_y_ck = pll_get_fvco() / (DIVy + 1)
 913  */
 914 static ulong pll_get_fvco(struct stm32mp1_clk_priv *priv,
 915                           int pll_id)
 916 {
 917         const struct stm32mp1_clk_pll *pll = priv->data->pll;
 918         int divm, divn;
 919         ulong refclk, fvco;
 920         u32 cfgr1, fracr;
 921
 922         cfgr1 = readl(priv->base + pll[pll_id].pllxcfgr1);
 923         fracr = readl(priv->base + pll[pll_id].pllxfracr);
 924
 925         divm = (cfgr1 & (RCC_PLLNCFGR1_DIVM_MASK)) >> RCC_PLLNCFGR1_DIVM_SHIFT;
 926         divn = cfgr1 & RCC_PLLNCFGR1_DIVN_MASK;
 927
 928         refclk = pll_get_fref_ck(priv, pll_id);
 929
 930         /* with FRACV :
 931          *   Fvco = Fck_ref * ((DIVN + 1) + FRACV / 2^13) / (DIVM + 1)
 932          * without FRACV
 933          *   Fvco = Fck_ref * ((DIVN + 1) / (DIVM + 1)
 934          */
 935         if (fracr & RCC_PLLNFRACR_FRACLE) {
 936                 u32 fracv = (fracr & RCC_PLLNFRACR_FRACV_MASK)
 937                             >> RCC_PLLNFRACR_FRACV_SHIFT;
 938                 fvco = (ulong)lldiv((unsigned long long)refclk *
 939                                      (((divn + 1) << 13) + fracv),
 940                                      ((unsigned long long)(divm + 1)) << 13);
 941         } else {
 942                 fvco = (ulong)(refclk * (divn + 1) / (divm + 1));
 943         }
 944
 945         return fvco;
 946 }
 947
 948 static ulong stm32mp1_read_pll_freq(struct stm32mp1_clk_priv *priv,
 949                                     int pll_id, int div_id)
 950 {
 951         const struct stm32mp1_clk_pll *pll = priv->data->pll;
 952         int divy;
 953         ulong dfout;
 954         u32 cfgr2;
 955
 956         if (div_id >= _DIV_NB)
 957                 return 0;
 958
 959         cfgr2 = readl(priv->base + pll[pll_id].pllxcfgr2);
 960         divy = (cfgr2 >> RCC_PLLNCFGR2_SHIFT(div_id)) & RCC_PLLNCFGR2_DIVX_MASK;
 961
 962         dfout = pll_get_fvco(priv, pll_id) / (divy + 1);
 963
 964         return dfout;
 965 }
 966
 967 static ulong stm32mp1_clk_get(struct stm32mp1_clk_priv *priv, int p)
 968 {
 969         u32 reg;
 970         ulong clock = 0;
 971
 972         switch (p) {
 973         case _CK_MPU:
 974         /* MPU sub system */
 975                 reg = readl(priv->base + RCC_MPCKSELR);
 976                 switch (reg & RCC_SELR_SRC_MASK) {
 977                 case RCC_MPCKSELR_HSI:
 978                         clock = stm32mp1_clk_get_fixed(priv, _HSI);
 979                         break;
 980                 case RCC_MPCKSELR_HSE:
 981                         clock = stm32mp1_clk_get_fixed(priv, _HSE);
 982                         break;
 983                 case RCC_MPCKSELR_PLL:
 984                 case RCC_MPCKSELR_PLL_MPUDIV:
 985                         clock = stm32mp1_read_pll_freq(priv, _PLL1, _DIV_P);
 986                         if ((reg & RCC_SELR_SRC_MASK) ==
 987                             RCC_MPCKSELR_PLL_MPUDIV) {
 988                                 reg = readl(priv->base + RCC_MPCKDIVR);
 989                                 clock >>= stm32mp1_mpu_div[reg &
 990                                         RCC_MPUDIV_MASK];
 991                         }
 992                         break;
 993                 }
 994                 break;
 995         /* AXI sub system */
 996         case _ACLK:
 997         case _HCLK2:
 998         case _HCLK6:
 999         case _PCLK4:
1000         case _PCLK5:
1001                 reg = readl(priv->base + RCC_ASSCKSELR);
1002                 switch (reg & RCC_SELR_SRC_MASK) {
1003                 case RCC_ASSCKSELR_HSI:
1004                         clock = stm32mp1_clk_get_fixed(priv, _HSI);
1005                         break;
1006                 case RCC_ASSCKSELR_HSE:
1007                         clock = stm32mp1_clk_get_fixed(priv, _HSE);
1008                         break;
1009                 case RCC_ASSCKSELR_PLL:
1010                         clock = stm32mp1_read_pll_freq(priv, _PLL2, _DIV_P);
1011                         break;
1012                 }
1013
1014                 /* System clock divider */
1015                 reg = readl(priv->base + RCC_AXIDIVR);
1016                 clock /= stm32mp1_axi_div[reg & RCC_AXIDIV_MASK];
1017
1018                 switch (p) {
1019                 case _PCLK4:
1020                         reg = readl(priv->base + RCC_APB4DIVR);
1021                         clock >>= stm32mp1_apbx_div[reg & RCC_APBXDIV_MASK];
1022                         break;
1023                 case _PCLK5:
1024                         reg = readl(priv->base + RCC_APB5DIVR);
1025                         clock >>= stm32mp1_apbx_div[reg & RCC_APBXDIV_MASK];
1026                         break;
1027                 default:
1028                         break;
1029                 }
1030                 break;
1031         /* MCU sub system */
1032         case _CK_MCU:
1033         case _PCLK1:
1034         case _PCLK2:
1035         case _PCLK3:
1036                 reg = readl(priv->base + RCC_MSSCKSELR);
1037                 switch (reg & RCC_SELR_SRC_MASK) {
1038                 case RCC_MSSCKSELR_HSI:
1039                         clock = stm32mp1_clk_get_fixed(priv, _HSI);
1040                         break;
1041                 case RCC_MSSCKSELR_HSE:
1042                         clock = stm32mp1_clk_get_fixed(priv, _HSE);
1043                         break;
1044                 case RCC_MSSCKSELR_CSI:
1045                         clock = stm32mp1_clk_get_fixed(priv, _CSI);
1046                         break;
1047                 case RCC_MSSCKSELR_PLL:
1048                         clock = stm32mp1_read_pll_freq(priv, _PLL3, _DIV_P);
1049                         break;
1050                 }
1051
1052                 /* MCU clock divider */
1053                 reg = readl(priv->base + RCC_MCUDIVR);
1054                 clock >>= stm32mp1_mcu_div[reg & RCC_MCUDIV_MASK];
1055
1056                 switch (p) {
1057                 case _PCLK1:
1058                         reg = readl(priv->base + RCC_APB1DIVR);
1059                         clock >>= stm32mp1_apbx_div[reg & RCC_APBXDIV_MASK];
1060                         break;
1061                 case _PCLK2:
1062                         reg = readl(priv->base + RCC_APB2DIVR);
1063                         clock >>= stm32mp1_apbx_div[reg & RCC_APBXDIV_MASK];
1064                         break;
1065                 case _PCLK3:
1066                         reg = readl(priv->base + RCC_APB3DIVR);
1067                         clock >>= stm32mp1_apbx_div[reg & RCC_APBXDIV_MASK];
1068                         break;
1069                 case _CK_MCU:
1070                 default:
1071                         break;
1072                 }
1073                 break;
1074         case _CK_PER:
1075                 reg = readl(priv->base + RCC_CPERCKSELR);
1076                 switch (reg & RCC_SELR_SRC_MASK) {
1077                 case RCC_CPERCKSELR_HSI:
1078                         clock = stm32mp1_clk_get_fixed(priv, _HSI);
1079                         break;
1080                 case RCC_CPERCKSELR_HSE:
1081                         clock = stm32mp1_clk_get_fixed(priv, _HSE);
1082                         break;
1083                 case RCC_CPERCKSELR_CSI:
1084                         clock = stm32mp1_clk_get_fixed(priv, _CSI);
1085                         break;
1086                 }
1087                 break;
1088         case _HSI:
1089         case _HSI_KER:
1090                 clock = stm32mp1_clk_get_fixed(priv, _HSI);
1091                 break;
1092         case _CSI:
1093         case _CSI_KER:
1094                 clock = stm32mp1_clk_get_fixed(priv, _CSI);
1095                 break;
1096         case _HSE:
1097         case _HSE_KER:
1098         case _HSE_KER_DIV2:
1099                 clock = stm32mp1_clk_get_fixed(priv, _HSE);
1100                 if (p == _HSE_KER_DIV2)
1101                         clock >>= 1;
1102                 break;
1103         case _LSI:
1104                 clock = stm32mp1_clk_get_fixed(priv, _LSI);
1105                 break;
1106         case _LSE:
1107                 clock = stm32mp1_clk_get_fixed(priv, _LSE);
1108                 break;
1109         /* PLL */
1110         case _PLL1_P:
1111         case _PLL1_Q:
1112         case _PLL1_R:
1113                 clock = stm32mp1_read_pll_freq(priv, _PLL1, p - _PLL1_P);
1114                 break;
1115         case _PLL2_P:
1116         case _PLL2_Q:
1117         case _PLL2_R:
1118                 clock = stm32mp1_read_pll_freq(priv, _PLL2, p - _PLL2_P);
1119                 break;
1120         case _PLL3_P:
1121         case _PLL3_Q:
1122         case _PLL3_R:
1123                 clock = stm32mp1_read_pll_freq(priv, _PLL3, p - _PLL3_P);
1124                 break;
1125         case _PLL4_P:
1126         case _PLL4_Q:
1127         case _PLL4_R:
1128                 clock = stm32mp1_read_pll_freq(priv, _PLL4, p - _PLL4_P);
1129                 break;
1130         /* other */
1131         case _USB_PHY_48:
1132                 clock = 48000000;
1133                 break;
1134         case _DSI_PHY:
1135         {
1136                 struct clk clk;
1137                 struct udevice *dev = NULL;
1138
1139                 if (!uclass_get_device_by_name(UCLASS_CLK, "ck_dsi_phy",
1140                                                &dev)) {
1141                         if (clk_request(dev, &clk)) {
1142                                 log_err("ck_dsi_phy request");
1143                         } else {
1144                                 clk.id = 0;
1145                                 clock = clk_get_rate(&clk);
1146                         }
1147                 }
1148                 break;
1149         }
1150         default:
1151                 break;
1152         }
1153
1154         log_debug("id=%d clock = %lx : %ld kHz\n", p, clock, clock / 1000);
1155
1156         return clock;
1157 }
1158
1159 static int stm32mp1_clk_enable(struct clk *clk)
1160 {
1161         struct stm32mp1_clk_priv *priv = dev_get_priv(clk->dev);
1162         const struct stm32mp1_clk_gate *gate = priv->data->gate;
1163         int i = stm32mp1_clk_get_id(priv, clk->id);
1164
1165         if (i < 0)
1166                 return i;
1167
1168         if (gate[i].set_clr)
1169                 writel(BIT(gate[i].bit), priv->base + gate[i].offset);
1170         else
1171                 setbits_le32(priv->base + gate[i].offset, BIT(gate[i].bit));
1172
1173         dev_dbg(clk->dev, "%s: id clock %d has been enabled\n", __func__, (u32)clk->id);
1174
1175         return 0;
1176 }
1177
1178 static int stm32mp1_clk_disable(struct clk *clk)
1179 {
1180         struct stm32mp1_clk_priv *priv = dev_get_priv(clk->dev);
1181         const struct stm32mp1_clk_gate *gate = priv->data->gate;
1182         int i = stm32mp1_clk_get_id(priv, clk->id);
1183
1184         if (i < 0)
1185                 return i;
1186
1187         if (gate[i].set_clr)
1188                 writel(BIT(gate[i].bit),
1189                        priv->base + gate[i].offset
1190                        + RCC_MP_ENCLRR_OFFSET);
1191         else
1192                 clrbits_le32(priv->base + gate[i].offset, BIT(gate[i].bit));
1193
1194         dev_dbg(clk->dev, "%s: id clock %d has been disabled\n", __func__, (u32)clk->id);
1195
1196         return 0;
1197 }
1198
1199 static ulong stm32mp1_clk_get_rate(struct clk *clk)
1200 {
1201         struct stm32mp1_clk_priv *priv = dev_get_priv(clk->dev);
1202         int p = stm32mp1_clk_get_parent(priv, clk->id);
1203         ulong rate;
1204
1205         if (p < 0)
1206                 return 0;
1207
1208         rate = stm32mp1_clk_get(priv, p);
1209
1210         dev_vdbg(clk->dev, "computed rate for id clock %d is %d (parent is %s)\n",
1211                  (u32)clk->id, (u32)rate, stm32mp1_clk_parent_name[p]);
1212
1213         return rate;
1214 }
1215
1216 #ifdef STM32MP1_CLOCK_TREE_INIT
1217
1218 bool stm32mp1_supports_opp(u32 opp_id, u32 cpu_type)
1219 {
1220         unsigned int id;
1221
1222         switch (opp_id) {
1223         case 1:
1224         case 2:
1225                 id = opp_id;
1226                 break;
1227         default:
1228                 id = 1; /* default value */
1229                 break;
1230         }
1231
1232         switch (cpu_type) {
1233         case CPU_STM32MP157Fxx:
1234         case CPU_STM32MP157Dxx:
1235         case CPU_STM32MP153Fxx:
1236         case CPU_STM32MP153Dxx:
1237         case CPU_STM32MP151Fxx:
1238         case CPU_STM32MP151Dxx:
1239                 return true;
1240         default:
1241                 return id == 1;
1242         }
1243 }
1244
1245 __weak void board_vddcore_init(u32 voltage_mv)
1246 {
1247 }
1248
1249 /*
1250  * gets OPP parameters (frequency in KHz and voltage in mV) from
1251  * an OPP table subnode. Platform HW support capabilities are also checked.
1252  * Returns 0 on success and a negative FDT error code on failure.
1253  */
1254 static int stm32mp1_get_opp(u32 cpu_type, ofnode subnode,
1255                             u32 *freq_khz, u32 *voltage_mv)
1256 {
1257         u32 opp_hw;
1258         u64 read_freq_64;
1259         u32 read_voltage_32;
1260
1261         *freq_khz = 0;
1262         *voltage_mv = 0;
1263
1264         opp_hw = ofnode_read_u32_default(subnode, "opp-supported-hw", 0);
1265         if (opp_hw)
1266                 if (!stm32mp1_supports_opp(opp_hw, cpu_type))
1267                         return -FDT_ERR_BADVALUE;
1268
1269         read_freq_64 = ofnode_read_u64_default(subnode, "opp-hz", 0) /
1270                        1000ULL;
1271         read_voltage_32 = ofnode_read_u32_default(subnode, "opp-microvolt", 0) /
1272                           1000U;
1273
1274         if (!read_voltage_32 || !read_freq_64)
1275                 return -FDT_ERR_NOTFOUND;
1276
1277         /* Frequency value expressed in KHz must fit on 32 bits */
1278         if (read_freq_64 > U32_MAX)
1279                 return -FDT_ERR_BADVALUE;
1280
1281         /* Millivolt value must fit on 16 bits */
1282         if (read_voltage_32 > U16_MAX)
1283                 return -FDT_ERR_BADVALUE;
1284
1285         *freq_khz = (u32)read_freq_64;
1286         *voltage_mv = read_voltage_32;
1287
1288         return 0;
1289 }
1290
1291 /*
1292  * parses OPP table in DT and finds the parameters for the
1293  * highest frequency supported by the HW platform.
1294  * Returns 0 on success and a negative FDT error code on failure.
1295  */
1296 int stm32mp1_get_max_opp_freq(struct stm32mp1_clk_priv *priv, u64 *freq_hz)
1297 {
1298         ofnode node, subnode;
1299         int ret;
1300         u32 freq = 0U, voltage = 0U;
1301         u32 cpu_type = get_cpu_type();
1302
1303         node = ofnode_by_compatible(ofnode_null(), "operating-points-v2");
1304         if (!ofnode_valid(node))
1305                 return -FDT_ERR_NOTFOUND;
1306
1307         ofnode_for_each_subnode(subnode, node) {
1308                 unsigned int read_freq;
1309                 unsigned int read_voltage;
1310
1311                 ret = stm32mp1_get_opp(cpu_type, subnode,
1312                                        &read_freq, &read_voltage);
1313                 if (ret)
1314                         continue;
1315
1316                 if (read_freq > freq) {
1317                         freq = read_freq;
1318                         voltage = read_voltage;
1319                 }
1320         }
1321
1322         if (!freq || !voltage)
1323                 return -FDT_ERR_NOTFOUND;
1324
1325         *freq_hz = (u64)1000U * freq;
1326         board_vddcore_init(voltage);
1327
1328         return 0;
1329 }
1330
1331 static int stm32mp1_pll1_opp(struct stm32mp1_clk_priv *priv, int clksrc,
1332                              u32 *pllcfg, u32 *fracv)
1333 {
1334         u32 post_divm;
1335         u32 input_freq;
1336         u64 output_freq;
1337         u64 freq;
1338         u64 vco;
1339         u32 divm, divn, divp, frac;
1340         int i, ret;
1341         u32 diff;
1342         u32 best_diff = U32_MAX;
1343
1344         /* PLL1 is 1600 */
1345         const u32 DIVN_MAX = stm32mp1_pll[PLL_1600].divn_max;
1346         const u32 POST_DIVM_MIN = stm32mp1_pll[PLL_1600].refclk_min * 1000000U;
1347         const u32 POST_DIVM_MAX = stm32mp1_pll[PLL_1600].refclk_max * 1000000U;
1348
1349         ret = stm32mp1_get_max_opp_freq(priv, &output_freq);
1350         if (ret) {
1351                 log_debug("PLL1 OPP configuration not found (%d).\n", ret);
1352                 return ret;
1353         }
1354
1355         switch (clksrc) {
1356         case CLK_PLL12_HSI:
1357                 input_freq = stm32mp1_clk_get_fixed(priv, _HSI);
1358                 break;
1359         case CLK_PLL12_HSE:
1360                 input_freq = stm32mp1_clk_get_fixed(priv, _HSE);
1361                 break;
1362         default:
1363                 return -EINTR;
1364         }
1365
1366         /* Following parameters have always the same value */
1367         pllcfg[PLLCFG_Q] = 0;
1368         pllcfg[PLLCFG_R] = 0;
1369         pllcfg[PLLCFG_O] = PQR(1, 0, 0);
1370
1371         for (divm = DIVM_MAX; divm >= DIVM_MIN; divm--) {
1372                 post_divm = (u32)(input_freq / (divm + 1));
1373                 if (post_divm < POST_DIVM_MIN || post_divm > POST_DIVM_MAX)
1374                         continue;
1375
1376                 for (divp = DIVP_MIN; divp <= DIVP_MAX; divp++) {
1377                         freq = output_freq * (divm + 1) * (divp + 1);
1378                         divn = (u32)((freq / input_freq) - 1);
1379                         if (divn < DIVN_MIN || divn > DIVN_MAX)
1380                                 continue;
1381
1382                         frac = (u32)(((freq * FRAC_MAX) / input_freq) -
1383                                      ((divn + 1) * FRAC_MAX));
1384                         /* 2 loops to refine the fractional part */
1385                         for (i = 2; i != 0; i--) {
1386                                 if (frac > FRAC_MAX)
1387                                         break;
1388
1389                                 vco = (post_divm * (divn + 1)) +
1390                                       ((post_divm * (u64)frac) /
1391                                        FRAC_MAX);
1392                                 if (vco < (PLL1600_VCO_MIN / 2) ||
1393                                     vco > (PLL1600_VCO_MAX / 2)) {
1394                                         frac++;
1395                                         continue;
1396                                 }
1397                                 freq = vco / (divp + 1);
1398                                 if (output_freq < freq)
1399                                         diff = (u32)(freq - output_freq);
1400                                 else
1401                                         diff = (u32)(output_freq - freq);
1402                                 if (diff < best_diff)  {
1403                                         pllcfg[PLLCFG_M] = divm;
1404                                         pllcfg[PLLCFG_N] = divn;
1405                                         pllcfg[PLLCFG_P] = divp;
1406                                         *fracv = frac;
1407
1408                                         if (diff == 0)
1409                                                 return 0;
1410
1411                                         best_diff = diff;
1412                                 }
1413                                 frac++;
1414                         }
1415                 }
1416         }
1417
1418         if (best_diff == U32_MAX)
1419                 return -1;
1420
1421         return 0;
1422 }
1423
1424 static void stm32mp1_ls_osc_set(int enable, fdt_addr_t rcc, u32 offset,
1425                                 u32 mask_on)
1426 {
1427         u32 address = rcc + offset;
1428
1429         if (enable)
1430                 setbits_le32(address, mask_on);
1431         else
1432                 clrbits_le32(address, mask_on);
1433 }
1434
1435 static void stm32mp1_hs_ocs_set(int enable, fdt_addr_t rcc, u32 mask_on)
1436 {
1437         writel(mask_on, rcc + (enable ? RCC_OCENSETR : RCC_OCENCLRR));
1438 }
1439
1440 static int stm32mp1_osc_wait(int enable, fdt_addr_t rcc, u32 offset,
1441                              u32 mask_rdy)
1442 {
1443         u32 mask_test = 0;
1444         u32 address = rcc + offset;
1445         u32 val;
1446         int ret;
1447
1448         if (enable)
1449                 mask_test = mask_rdy;
1450
1451         ret = readl_poll_timeout(address, val,
1452                                  (val & mask_rdy) == mask_test,
1453                                  TIMEOUT_1S);
1454
1455         if (ret)
1456                 log_err("OSC %x @ %x timeout for enable=%d : 0x%x\n",
1457                         mask_rdy, address, enable, readl(address));
1458
1459         return ret;
1460 }
1461
1462 static void stm32mp1_lse_enable(fdt_addr_t rcc, int bypass, int digbyp,
1463                                 u32 lsedrv)
1464 {
1465         u32 value;
1466
1467         if (digbyp)
1468                 setbits_le32(rcc + RCC_BDCR, RCC_BDCR_DIGBYP);
1469
1470         if (bypass || digbyp)
1471                 setbits_le32(rcc + RCC_BDCR, RCC_BDCR_LSEBYP);
1472
1473         /*
1474          * warning: not recommended to switch directly from "high drive"
1475          * to "medium low drive", and vice-versa.
1476          */
1477         value = (readl(rcc + RCC_BDCR) & RCC_BDCR_LSEDRV_MASK)
1478                 >> RCC_BDCR_LSEDRV_SHIFT;
1479
1480         while (value != lsedrv) {
1481                 if (value > lsedrv)
1482                         value--;
1483                 else
1484                         value++;
1485
1486                 clrsetbits_le32(rcc + RCC_BDCR,
1487                                 RCC_BDCR_LSEDRV_MASK,
1488                                 value << RCC_BDCR_LSEDRV_SHIFT);
1489         }
1490
1491         stm32mp1_ls_osc_set(1, rcc, RCC_BDCR, RCC_BDCR_LSEON);
1492 }
1493
1494 static void stm32mp1_lse_wait(fdt_addr_t rcc)
1495 {
1496         stm32mp1_osc_wait(1, rcc, RCC_BDCR, RCC_BDCR_LSERDY);
1497 }
1498
1499 static void stm32mp1_lsi_set(fdt_addr_t rcc, int enable)
1500 {
1501         stm32mp1_ls_osc_set(enable, rcc, RCC_RDLSICR, RCC_RDLSICR_LSION);
1502         stm32mp1_osc_wait(enable, rcc, RCC_RDLSICR, RCC_RDLSICR_LSIRDY);
1503 }
1504
1505 static void stm32mp1_hse_enable(fdt_addr_t rcc, int bypass, int digbyp, int css)
1506 {
1507         if (digbyp)
1508                 writel(RCC_OCENR_DIGBYP, rcc + RCC_OCENSETR);
1509         if (bypass || digbyp)
1510                 writel(RCC_OCENR_HSEBYP, rcc + RCC_OCENSETR);
1511
1512         stm32mp1_hs_ocs_set(1, rcc, RCC_OCENR_HSEON);
1513         stm32mp1_osc_wait(1, rcc, RCC_OCRDYR, RCC_OCRDYR_HSERDY);
1514
1515         if (css)
1516                 writel(RCC_OCENR_HSECSSON, rcc + RCC_OCENSETR);
1517 }
1518
1519 static void stm32mp1_csi_set(fdt_addr_t rcc, int enable)
1520 {
1521         stm32mp1_hs_ocs_set(enable, rcc, RCC_OCENR_CSION);
1522         stm32mp1_osc_wait(enable, rcc, RCC_OCRDYR, RCC_OCRDYR_CSIRDY);
1523 }
1524
1525 static void stm32mp1_hsi_set(fdt_addr_t rcc, int enable)
1526 {
1527         stm32mp1_hs_ocs_set(enable, rcc, RCC_OCENR_HSION);
1528         stm32mp1_osc_wait(enable, rcc, RCC_OCRDYR, RCC_OCRDYR_HSIRDY);
1529 }
1530
1531 static int stm32mp1_set_hsidiv(fdt_addr_t rcc, u8 hsidiv)
1532 {
1533         u32 address = rcc + RCC_OCRDYR;
1534         u32 val;
1535         int ret;
1536
1537         clrsetbits_le32(rcc + RCC_HSICFGR,
1538                         RCC_HSICFGR_HSIDIV_MASK,
1539                         RCC_HSICFGR_HSIDIV_MASK & hsidiv);
1540
1541         ret = readl_poll_timeout(address, val,
1542                                  val & RCC_OCRDYR_HSIDIVRDY,
1543                                  TIMEOUT_200MS);
1544         if (ret)
1545                 log_err("HSIDIV failed @ 0x%x: 0x%x\n",
1546                         address, readl(address));
1547
1548         return ret;
1549 }
1550
1551 static int stm32mp1_hsidiv(fdt_addr_t rcc, ulong hsifreq)
1552 {
1553         u8 hsidiv;
1554         u32 hsidivfreq = MAX_HSI_HZ;
1555
1556         for (hsidiv = 0; hsidiv < 4; hsidiv++,
1557              hsidivfreq = hsidivfreq / 2)
1558                 if (hsidivfreq == hsifreq)
1559                         break;
1560
1561         if (hsidiv == 4) {
1562                 log_err("hsi frequency invalid");
1563                 return -1;
1564         }
1565
1566         if (hsidiv > 0)
1567                 return stm32mp1_set_hsidiv(rcc, hsidiv);
1568
1569         return 0;
1570 }
1571
1572 static void pll_start(struct stm32mp1_clk_priv *priv, int pll_id)
1573 {
1574         const struct stm32mp1_clk_pll *pll = priv->data->pll;
1575
1576         clrsetbits_le32(priv->base + pll[pll_id].pllxcr,
1577                         RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN |
1578                         RCC_PLLNCR_DIVREN,
1579                         RCC_PLLNCR_PLLON);
1580 }
1581
1582 static int pll_output(struct stm32mp1_clk_priv *priv, int pll_id, int output)
1583 {
1584         const struct stm32mp1_clk_pll *pll = priv->data->pll;
1585         u32 pllxcr = priv->base + pll[pll_id].pllxcr;
1586         u32 val;
1587         int ret;
1588
1589         ret = readl_poll_timeout(pllxcr, val, val & RCC_PLLNCR_PLLRDY,
1590                                  TIMEOUT_200MS);
1591
1592         if (ret) {
1593                 log_err("PLL%d start failed @ 0x%x: 0x%x\n",
1594                         pll_id, pllxcr, readl(pllxcr));
1595                 return ret;
1596         }
1597
1598         /* start the requested output */
1599         setbits_le32(pllxcr, output << RCC_PLLNCR_DIVEN_SHIFT);
1600
1601         return 0;
1602 }
1603
1604 static int pll_stop(struct stm32mp1_clk_priv *priv, int pll_id)
1605 {
1606         const struct stm32mp1_clk_pll *pll = priv->data->pll;
1607         u32 pllxcr = priv->base + pll[pll_id].pllxcr;
1608         u32 val;
1609
1610         /* stop all output */
1611         clrbits_le32(pllxcr,
1612                      RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN | RCC_PLLNCR_DIVREN);
1613
1614         /* stop PLL */
1615         clrbits_le32(pllxcr, RCC_PLLNCR_PLLON);
1616
1617         /* wait PLL stopped */
1618         return readl_poll_timeout(pllxcr, val, (val & RCC_PLLNCR_PLLRDY) == 0,
1619                                   TIMEOUT_200MS);
1620 }
1621
1622 static void pll_config_output(struct stm32mp1_clk_priv *priv,
1623                               int pll_id, u32 *pllcfg)
1624 {
1625         const struct stm32mp1_clk_pll *pll = priv->data->pll;
1626         fdt_addr_t rcc = priv->base;
1627         u32 value;
1628
1629         value = (pllcfg[PLLCFG_P] << RCC_PLLNCFGR2_DIVP_SHIFT)
1630                 & RCC_PLLNCFGR2_DIVP_MASK;
1631         value |= (pllcfg[PLLCFG_Q] << RCC_PLLNCFGR2_DIVQ_SHIFT)
1632                  & RCC_PLLNCFGR2_DIVQ_MASK;
1633         value |= (pllcfg[PLLCFG_R] << RCC_PLLNCFGR2_DIVR_SHIFT)
1634                  & RCC_PLLNCFGR2_DIVR_MASK;
1635         writel(value, rcc + pll[pll_id].pllxcfgr2);
1636 }
1637
1638 static int pll_config(struct stm32mp1_clk_priv *priv, int pll_id,
1639                       u32 *pllcfg, u32 fracv)
1640 {
1641         const struct stm32mp1_clk_pll *pll = priv->data->pll;
1642         fdt_addr_t rcc = priv->base;
1643         enum stm32mp1_plltype type = pll[pll_id].plltype;
1644         int src;
1645         ulong refclk;
1646         u8 ifrge = 0;
1647         u32 value;
1648
1649         src = readl(priv->base + pll[pll_id].rckxselr) & RCC_SELR_SRC_MASK;
1650
1651         refclk = stm32mp1_clk_get_fixed(priv, pll[pll_id].refclk[src]) /
1652                  (pllcfg[PLLCFG_M] + 1);
1653
1654         if (refclk < (stm32mp1_pll[type].refclk_min * 1000000) ||
1655             refclk > (stm32mp1_pll[type].refclk_max * 1000000)) {
1656                 log_err("invalid refclk = %x\n", (u32)refclk);
1657                 return -EINVAL;
1658         }
1659         if (type == PLL_800 && refclk >= 8000000)
1660                 ifrge = 1;
1661
1662         value = (pllcfg[PLLCFG_N] << RCC_PLLNCFGR1_DIVN_SHIFT)
1663                  & RCC_PLLNCFGR1_DIVN_MASK;
1664         value |= (pllcfg[PLLCFG_M] << RCC_PLLNCFGR1_DIVM_SHIFT)
1665                  & RCC_PLLNCFGR1_DIVM_MASK;
1666         value |= (ifrge << RCC_PLLNCFGR1_IFRGE_SHIFT)
1667                  & RCC_PLLNCFGR1_IFRGE_MASK;
1668         writel(value, rcc + pll[pll_id].pllxcfgr1);
1669
1670         /* fractional configuration: load sigma-delta modulator (SDM) */
1671
1672         /* Write into FRACV the new fractional value , and FRACLE to 0 */
1673         writel(fracv << RCC_PLLNFRACR_FRACV_SHIFT,
1674                rcc + pll[pll_id].pllxfracr);
1675
1676         /* Write FRACLE to 1 : FRACV value is loaded into the SDM */
1677         setbits_le32(rcc + pll[pll_id].pllxfracr,
1678                      RCC_PLLNFRACR_FRACLE);
1679
1680         pll_config_output(priv, pll_id, pllcfg);
1681
1682         return 0;
1683 }
1684
1685 static void pll_csg(struct stm32mp1_clk_priv *priv, int pll_id, u32 *csg)
1686 {
1687         const struct stm32mp1_clk_pll *pll = priv->data->pll;
1688         u32 pllxcsg;
1689
1690         pllxcsg = ((csg[PLLCSG_MOD_PER] << RCC_PLLNCSGR_MOD_PER_SHIFT) &
1691                     RCC_PLLNCSGR_MOD_PER_MASK) |
1692                   ((csg[PLLCSG_INC_STEP] << RCC_PLLNCSGR_INC_STEP_SHIFT) &
1693                     RCC_PLLNCSGR_INC_STEP_MASK) |
1694                   ((csg[PLLCSG_SSCG_MODE] << RCC_PLLNCSGR_SSCG_MODE_SHIFT) &
1695                     RCC_PLLNCSGR_SSCG_MODE_MASK);
1696
1697         writel(pllxcsg, priv->base + pll[pll_id].pllxcsgr);
1698
1699         setbits_le32(priv->base + pll[pll_id].pllxcr, RCC_PLLNCR_SSCG_CTRL);
1700 }
1701
1702 static  __maybe_unused int pll_set_rate(struct udevice *dev,
1703                                         int pll_id,
1704                                         int div_id,
1705                                         unsigned long clk_rate)
1706 {
1707         struct stm32mp1_clk_priv *priv = dev_get_priv(dev);
1708         unsigned int pllcfg[PLLCFG_NB];
1709         ofnode plloff;
1710         char name[12];
1711         const struct stm32mp1_clk_pll *pll = priv->data->pll;
1712         enum stm32mp1_plltype type = pll[pll_id].plltype;
1713         int divm, divn, divy;
1714         int ret;
1715         ulong fck_ref;
1716         u32 fracv;
1717         u64 value;
1718
1719         if (div_id > _DIV_NB)
1720                 return -EINVAL;
1721
1722         sprintf(name, "st,pll@%d", pll_id);
1723         plloff = dev_read_subnode(dev, name);
1724         if (!ofnode_valid(plloff))
1725                 return -FDT_ERR_NOTFOUND;
1726
1727         ret = ofnode_read_u32_array(plloff, "cfg",
1728                                     pllcfg, PLLCFG_NB);
1729         if (ret < 0)
1730                 return -FDT_ERR_NOTFOUND;
1731
1732         fck_ref = pll_get_fref_ck(priv, pll_id);
1733
1734         divm = pllcfg[PLLCFG_M];
1735         /* select output divider = 0: for _DIV_P, 1:_DIV_Q 2:_DIV_R */
1736         divy = pllcfg[PLLCFG_P + div_id];
1737
1738         /* For: PLL1 & PLL2 => VCO is * 2 but ck_pll_y is also / 2
1739          * So same final result than PLL2 et 4
1740          * with FRACV
1741          * Fck_pll_y = Fck_ref * ((DIVN + 1) + FRACV / 2^13)
1742          *             / (DIVy + 1) * (DIVM + 1)
1743          * value = (DIVN + 1) * 2^13 + FRACV / 2^13
1744          *       = Fck_pll_y (DIVy + 1) * (DIVM + 1) * 2^13 / Fck_ref
1745          */
1746         value = ((u64)clk_rate * (divy + 1) * (divm + 1)) << 13;
1747         value = lldiv(value, fck_ref);
1748
1749         divn = (value >> 13) - 1;
1750         if (divn < DIVN_MIN ||
1751             divn > stm32mp1_pll[type].divn_max) {
1752                 dev_err(dev, "divn invalid = %d", divn);
1753                 return -EINVAL;
1754         }
1755         fracv = value - ((divn + 1) << 13);
1756         pllcfg[PLLCFG_N] = divn;
1757
1758         /* reconfigure PLL */
1759         pll_stop(priv, pll_id);
1760         pll_config(priv, pll_id, pllcfg, fracv);
1761         pll_start(priv, pll_id);
1762         pll_output(priv, pll_id, pllcfg[PLLCFG_O]);
1763
1764         return 0;
1765 }
1766
1767 static int set_clksrc(struct stm32mp1_clk_priv *priv, unsigned int clksrc)
1768 {
1769         u32 address = priv->base + (clksrc >> 4);
1770         u32 val;
1771         int ret;
1772
1773         clrsetbits_le32(address, RCC_SELR_SRC_MASK, clksrc & RCC_SELR_SRC_MASK);
1774         ret = readl_poll_timeout(address, val, val & RCC_SELR_SRCRDY,
1775                                  TIMEOUT_200MS);
1776         if (ret)
1777                 log_err("CLKSRC %x start failed @ 0x%x: 0x%x\n",
1778                         clksrc, address, readl(address));
1779
1780         return ret;
1781 }
1782
1783 static void stgen_config(struct stm32mp1_clk_priv *priv)
1784 {
1785         int p;
1786         u32 stgenc, cntfid0;
1787         ulong rate;
1788
1789         stgenc = STM32_STGEN_BASE;
1790         cntfid0 = readl(stgenc + STGENC_CNTFID0);
1791         p = stm32mp1_clk_get_parent(priv, STGEN_K);
1792         rate = stm32mp1_clk_get(priv, p);
1793
1794         if (cntfid0 != rate) {
1795                 u64 counter;
1796
1797                 log_debug("System Generic Counter (STGEN) update\n");
1798                 clrbits_le32(stgenc + STGENC_CNTCR, STGENC_CNTCR_EN);
1799                 counter = (u64)readl(stgenc + STGENC_CNTCVL);
1800                 counter |= ((u64)(readl(stgenc + STGENC_CNTCVU))) << 32;
1801                 counter = lldiv(counter * (u64)rate, cntfid0);
1802                 writel((u32)counter, stgenc + STGENC_CNTCVL);
1803                 writel((u32)(counter >> 32), stgenc + STGENC_CNTCVU);
1804                 writel(rate, stgenc + STGENC_CNTFID0);
1805                 setbits_le32(stgenc + STGENC_CNTCR, STGENC_CNTCR_EN);
1806
1807                 __asm__ volatile("mcr p15, 0, %0, c14, c0, 0" : : "r" (rate));
1808
1809                 /* need to update gd->arch.timer_rate_hz with new frequency */
1810                 timer_init();
1811         }
1812 }
1813
1814 static int set_clkdiv(unsigned int clkdiv, u32 address)
1815 {
1816         u32 val;
1817         int ret;
1818
1819         clrsetbits_le32(address, RCC_DIVR_DIV_MASK, clkdiv & RCC_DIVR_DIV_MASK);
1820         ret = readl_poll_timeout(address, val, val & RCC_DIVR_DIVRDY,
1821                                  TIMEOUT_200MS);
1822         if (ret)
1823                 log_err("CLKDIV %x start failed @ 0x%x: 0x%x\n",
1824                         clkdiv, address, readl(address));
1825
1826         return ret;
1827 }
1828
1829 static void stm32mp1_mco_csg(struct stm32mp1_clk_priv *priv,
1830                              u32 clksrc, u32 clkdiv)
1831 {
1832         u32 address = priv->base + (clksrc >> 4);
1833
1834         /*
1835          * binding clksrc : bit15-4 offset
1836          *                  bit3:   disable
1837          *                  bit2-0: MCOSEL[2:0]
1838          */
1839         if (clksrc & 0x8) {
1840                 clrbits_le32(address, RCC_MCOCFG_MCOON);
1841         } else {
1842                 clrsetbits_le32(address,
1843                                 RCC_MCOCFG_MCOSRC_MASK,
1844                                 clksrc & RCC_MCOCFG_MCOSRC_MASK);
1845                 clrsetbits_le32(address,
1846                                 RCC_MCOCFG_MCODIV_MASK,
1847                                 clkdiv << RCC_MCOCFG_MCODIV_SHIFT);
1848                 setbits_le32(address, RCC_MCOCFG_MCOON);
1849         }
1850 }
1851
1852 static void set_rtcsrc(struct stm32mp1_clk_priv *priv,
1853                        unsigned int clksrc,
1854                        int lse_css)
1855 {
1856         u32 address = priv->base + RCC_BDCR;
1857
1858         if (readl(address) & RCC_BDCR_RTCCKEN)
1859                 goto skip_rtc;
1860
1861         if (clksrc == CLK_RTC_DISABLED)
1862                 goto skip_rtc;
1863
1864         clrsetbits_le32(address,
1865                         RCC_BDCR_RTCSRC_MASK,
1866                         clksrc << RCC_BDCR_RTCSRC_SHIFT);
1867
1868         setbits_le32(address, RCC_BDCR_RTCCKEN);
1869
1870 skip_rtc:
1871         if (lse_css)
1872                 setbits_le32(address, RCC_BDCR_LSECSSON);
1873 }
1874
1875 static void pkcs_config(struct stm32mp1_clk_priv *priv, u32 pkcs)
1876 {
1877         u32 address = priv->base + ((pkcs >> 4) & 0xFFF);
1878         u32 value = pkcs & 0xF;
1879         u32 mask = 0xF;
1880
1881         if (pkcs & BIT(31)) {
1882                 mask <<= 4;
1883                 value <<= 4;
1884         }
1885         clrsetbits_le32(address, mask, value);
1886 }
1887
1888 static int stm32mp1_clktree(struct udevice *dev)
1889 {
1890         struct stm32mp1_clk_priv *priv = dev_get_priv(dev);
1891         fdt_addr_t rcc = priv->base;
1892         unsigned int clksrc[CLKSRC_NB];
1893         unsigned int clkdiv[CLKDIV_NB];
1894         unsigned int pllcfg[_PLL_NB][PLLCFG_NB];
1895         unsigned int pllfracv[_PLL_NB];
1896         unsigned int pllcsg[_PLL_NB][PLLCSG_NB];
1897         bool pllcfg_valid[_PLL_NB];
1898         bool pllcsg_set[_PLL_NB];
1899         int ret;
1900         int i, len;
1901         int lse_css = 0;
1902         const u32 *pkcs_cell;
1903
1904         /* check mandatory field */
1905         ret = dev_read_u32_array(dev, "st,clksrc", clksrc, CLKSRC_NB);
1906         if (ret < 0) {
1907                 dev_dbg(dev, "field st,clksrc invalid: error %d\n", ret);
1908                 return -FDT_ERR_NOTFOUND;
1909         }
1910
1911         ret = dev_read_u32_array(dev, "st,clkdiv", clkdiv, CLKDIV_NB);
1912         if (ret < 0) {
1913                 dev_dbg(dev, "field st,clkdiv invalid: error %d\n", ret);
1914                 return -FDT_ERR_NOTFOUND;
1915         }
1916
1917         /* check mandatory field in each pll */
1918         for (i = 0; i < _PLL_NB; i++) {
1919                 char name[12];
1920                 ofnode node;
1921
1922                 sprintf(name, "st,pll@%d", i);
1923                 node = dev_read_subnode(dev, name);
1924                 pllcfg_valid[i] = ofnode_valid(node);
1925                 pllcsg_set[i] = false;
1926                 if (pllcfg_valid[i]) {
1927                         dev_dbg(dev, "DT for PLL %d @ %s\n", i, name);
1928                         ret = ofnode_read_u32_array(node, "cfg",
1929                                                     pllcfg[i], PLLCFG_NB);
1930                         if (ret < 0) {
1931                                 dev_dbg(dev, "field cfg invalid: error %d\n", ret);
1932                                 return -FDT_ERR_NOTFOUND;
1933                         }
1934                         pllfracv[i] = ofnode_read_u32_default(node, "frac", 0);
1935
1936                         ret = ofnode_read_u32_array(node, "csg", pllcsg[i],
1937                                                     PLLCSG_NB);
1938                         if (!ret) {
1939                                 pllcsg_set[i] = true;
1940                         } else if (ret != -FDT_ERR_NOTFOUND) {
1941                                 dev_dbg(dev, "invalid csg node for pll@%d res=%d\n",
1942                                         i, ret);
1943                                 return ret;
1944                         }
1945                 } else if (i == _PLL1)  {
1946                         /* use OPP for PLL1 for A7 CPU */
1947                         dev_dbg(dev, "DT for PLL %d with OPP\n", i);
1948                         ret = stm32mp1_pll1_opp(priv,
1949                                                 clksrc[CLKSRC_PLL12],
1950                                                 pllcfg[i],
1951                                                 &pllfracv[i]);
1952                         if (ret) {
1953                                 dev_dbg(dev, "PLL %d with OPP error = %d\n", i, ret);
1954                                 return ret;
1955                         }
1956                         pllcfg_valid[i] = true;
1957                 }
1958         }
1959
1960         dev_dbg(dev, "configuration MCO\n");
1961         stm32mp1_mco_csg(priv, clksrc[CLKSRC_MCO1], clkdiv[CLKDIV_MCO1]);
1962         stm32mp1_mco_csg(priv, clksrc[CLKSRC_MCO2], clkdiv[CLKDIV_MCO2]);
1963
1964         dev_dbg(dev, "switch ON osillator\n");
1965         /*
1966          * switch ON oscillator found in device-tree,
1967          * HSI already ON after bootrom
1968          */
1969         if (clk_valid(&priv->osc_clk[_LSI]))
1970                 stm32mp1_lsi_set(rcc, 1);
1971
1972         if (clk_valid(&priv->osc_clk[_LSE])) {
1973                 int bypass, digbyp;
1974                 u32 lsedrv;
1975                 struct udevice *dev = priv->osc_clk[_LSE].dev;
1976
1977                 bypass = dev_read_bool(dev, "st,bypass");
1978                 digbyp = dev_read_bool(dev, "st,digbypass");
1979                 lse_css = dev_read_bool(dev, "st,css");
1980                 lsedrv = dev_read_u32_default(dev, "st,drive",
1981                                               LSEDRV_MEDIUM_HIGH);
1982
1983                 stm32mp1_lse_enable(rcc, bypass, digbyp, lsedrv);
1984         }
1985
1986         if (clk_valid(&priv->osc_clk[_HSE])) {
1987                 int bypass, digbyp, css;
1988                 struct udevice *dev = priv->osc_clk[_HSE].dev;
1989
1990                 bypass = dev_read_bool(dev, "st,bypass");
1991                 digbyp = dev_read_bool(dev, "st,digbypass");
1992                 css = dev_read_bool(dev, "st,css");
1993
1994                 stm32mp1_hse_enable(rcc, bypass, digbyp, css);
1995         }
1996         /* CSI is mandatory for automatic I/O compensation (SYSCFG_CMPCR)
1997          * => switch on CSI even if node is not present in device tree
1998          */
1999         stm32mp1_csi_set(rcc, 1);
2000
2001         /* come back to HSI */
2002         dev_dbg(dev, "come back to HSI\n");
2003         set_clksrc(priv, CLK_MPU_HSI);
2004         set_clksrc(priv, CLK_AXI_HSI);
2005         set_clksrc(priv, CLK_MCU_HSI);
2006
2007         dev_dbg(dev, "pll stop\n");
2008         for (i = 0; i < _PLL_NB; i++)
2009                 pll_stop(priv, i);
2010
2011         /* configure HSIDIV */
2012         dev_dbg(dev, "configure HSIDIV\n");
2013         if (clk_valid(&priv->osc_clk[_HSI])) {
2014                 stm32mp1_hsidiv(rcc, clk_get_rate(&priv->osc_clk[_HSI]));
2015                 stgen_config(priv);
2016         }
2017
2018         /* select DIV */
2019         dev_dbg(dev, "select DIV\n");
2020         /* no ready bit when MPUSRC != CLK_MPU_PLL1P_DIV, MPUDIV is disabled */
2021         writel(clkdiv[CLKDIV_MPU] & RCC_DIVR_DIV_MASK, rcc + RCC_MPCKDIVR);
2022         set_clkdiv(clkdiv[CLKDIV_AXI], rcc + RCC_AXIDIVR);
2023         set_clkdiv(clkdiv[CLKDIV_APB4], rcc + RCC_APB4DIVR);
2024         set_clkdiv(clkdiv[CLKDIV_APB5], rcc + RCC_APB5DIVR);
2025         set_clkdiv(clkdiv[CLKDIV_MCU], rcc + RCC_MCUDIVR);
2026         set_clkdiv(clkdiv[CLKDIV_APB1], rcc + RCC_APB1DIVR);
2027         set_clkdiv(clkdiv[CLKDIV_APB2], rcc + RCC_APB2DIVR);
2028         set_clkdiv(clkdiv[CLKDIV_APB3], rcc + RCC_APB3DIVR);
2029
2030         /* no ready bit for RTC */
2031         writel(clkdiv[CLKDIV_RTC] & RCC_DIVR_DIV_MASK, rcc + RCC_RTCDIVR);
2032
2033         /* configure PLLs source */
2034         dev_dbg(dev, "configure PLLs source\n");
2035         set_clksrc(priv, clksrc[CLKSRC_PLL12]);
2036         set_clksrc(priv, clksrc[CLKSRC_PLL3]);
2037         set_clksrc(priv, clksrc[CLKSRC_PLL4]);
2038
2039         /* configure and start PLLs */
2040         dev_dbg(dev, "configure PLLs\n");
2041         for (i = 0; i < _PLL_NB; i++) {
2042                 if (!pllcfg_valid[i])
2043                         continue;
2044                 dev_dbg(dev, "configure PLL %d\n", i);
2045                 pll_config(priv, i, pllcfg[i], pllfracv[i]);
2046                 if (pllcsg_set[i])
2047                         pll_csg(priv, i, pllcsg[i]);
2048                 pll_start(priv, i);
2049         }
2050
2051         /* wait and start PLLs ouptut when ready */
2052         for (i = 0; i < _PLL_NB; i++) {
2053                 if (!pllcfg_valid[i])
2054                         continue;
2055                 dev_dbg(dev, "output PLL %d\n", i);
2056                 pll_output(priv, i, pllcfg[i][PLLCFG_O]);
2057         }
2058
2059         /* wait LSE ready before to use it */
2060         if (clk_valid(&priv->osc_clk[_LSE]))
2061                 stm32mp1_lse_wait(rcc);
2062
2063         /* configure with expected clock source */
2064         dev_dbg(dev, "CLKSRC\n");
2065         set_clksrc(priv, clksrc[CLKSRC_MPU]);
2066         set_clksrc(priv, clksrc[CLKSRC_AXI]);
2067         set_clksrc(priv, clksrc[CLKSRC_MCU]);
2068         set_rtcsrc(priv, clksrc[CLKSRC_RTC], lse_css);
2069
2070         /* configure PKCK */
2071         dev_dbg(dev, "PKCK\n");
2072         pkcs_cell = dev_read_prop(dev, "st,pkcs", &len);
2073         if (pkcs_cell) {
2074                 bool ckper_disabled = false;
2075
2076                 for (i = 0; i < len / sizeof(u32); i++) {
2077                         u32 pkcs = (u32)fdt32_to_cpu(pkcs_cell[i]);
2078
2079                         if (pkcs == CLK_CKPER_DISABLED) {
2080                                 ckper_disabled = true;
2081                                 continue;
2082                         }
2083                         pkcs_config(priv, pkcs);
2084                 }
2085                 /* CKPER is source for some peripheral clock
2086                  * (FMC-NAND / QPSI-NOR) and switching source is allowed
2087                  * only if previous clock is still ON
2088                  * => deactivated CKPER only after switching clock
2089                  */
2090                 if (ckper_disabled)
2091                         pkcs_config(priv, CLK_CKPER_DISABLED);
2092         }
2093
2094         /* STGEN clock source can change with CLK_STGEN_XXX */
2095         stgen_config(priv);
2096
2097         dev_dbg(dev, "oscillator off\n");
2098         /* switch OFF HSI if not found in device-tree */
2099         if (!clk_valid(&priv->osc_clk[_HSI]))
2100                 stm32mp1_hsi_set(rcc, 0);
2101
2102         /* Software Self-Refresh mode (SSR) during DDR initilialization */
2103         clrsetbits_le32(priv->base + RCC_DDRITFCR,
2104                         RCC_DDRITFCR_DDRCKMOD_MASK,
2105                         RCC_DDRITFCR_DDRCKMOD_SSR <<
2106                         RCC_DDRITFCR_DDRCKMOD_SHIFT);
2107
2108         return 0;
2109 }
2110 #endif /* STM32MP1_CLOCK_TREE_INIT */
2111
2112 static int pll_set_output_rate(struct udevice *dev,
2113                                int pll_id,
2114                                int div_id,
2115                                unsigned long clk_rate)
2116 {
2117         struct stm32mp1_clk_priv *priv = dev_get_priv(dev);
2118         const struct stm32mp1_clk_pll *pll = priv->data->pll;
2119         u32 pllxcr = priv->base + pll[pll_id].pllxcr;
2120         int div;
2121         ulong fvco;
2122
2123         if (div_id > _DIV_NB)
2124                 return -EINVAL;
2125
2126         fvco = pll_get_fvco(priv, pll_id);
2127
2128         if (fvco <= clk_rate)
2129                 div = 1;
2130         else
2131                 div = DIV_ROUND_UP(fvco, clk_rate);
2132
2133         if (div > 128)
2134                 div = 128;
2135
2136         /* stop the requested output */
2137         clrbits_le32(pllxcr, 0x1 << div_id << RCC_PLLNCR_DIVEN_SHIFT);
2138         /* change divider */
2139         clrsetbits_le32(priv->base + pll[pll_id].pllxcfgr2,
2140                         RCC_PLLNCFGR2_DIVX_MASK << RCC_PLLNCFGR2_SHIFT(div_id),
2141                         (div - 1) << RCC_PLLNCFGR2_SHIFT(div_id));
2142         /* start the requested output */
2143         setbits_le32(pllxcr, 0x1 << div_id << RCC_PLLNCR_DIVEN_SHIFT);
2144
2145         return 0;
2146 }
2147
2148 static ulong stm32mp1_clk_set_rate(struct clk *clk, unsigned long clk_rate)
2149 {
2150         struct stm32mp1_clk_priv *priv = dev_get_priv(clk->dev);
2151         int p;
2152
2153         switch (clk->id) {
2154 #if defined(STM32MP1_CLOCK_TREE_INIT) && \
2155         defined(CONFIG_STM32MP1_DDR_INTERACTIVE)
2156         case DDRPHYC:
2157                 break;
2158 #endif
2159         case LTDC_PX:
2160         case DSI_PX:
2161                 break;
2162         default:
2163                 dev_err(clk->dev, "Set of clk %ld not supported", clk->id);
2164                 return -EINVAL;
2165         }
2166
2167         p = stm32mp1_clk_get_parent(priv, clk->id);
2168         dev_vdbg(clk->dev, "parent = %d:%s\n", p, stm32mp1_clk_parent_name[p]);
2169         if (p < 0)
2170                 return -EINVAL;
2171
2172         switch (p) {
2173 #if defined(STM32MP1_CLOCK_TREE_INIT) && \
2174         defined(CONFIG_STM32MP1_DDR_INTERACTIVE)
2175         case _PLL2_R: /* DDRPHYC */
2176         {
2177                 /* only for change DDR clock in interactive mode */
2178                 ulong result;
2179
2180                 set_clksrc(priv, CLK_AXI_HSI);
2181                 result = pll_set_rate(clk->dev,  _PLL2, _DIV_R, clk_rate);
2182                 set_clksrc(priv, CLK_AXI_PLL2P);
2183                 return result;
2184         }
2185 #endif
2186
2187         case _PLL4_Q:
2188                 /* for LTDC_PX and DSI_PX case */
2189                 return pll_set_output_rate(clk->dev, _PLL4, _DIV_Q, clk_rate);
2190         }
2191
2192         return -EINVAL;
2193 }
2194
2195 static void stm32mp1_osc_init(struct udevice *dev)
2196 {
2197         struct stm32mp1_clk_priv *priv = dev_get_priv(dev);
2198         int i;
2199         const char *name[NB_OSC] = {
2200                 [_LSI] = "lsi",
2201                 [_LSE] = "lse",
2202                 [_HSI] = "hsi",
2203                 [_HSE] = "hse",
2204                 [_CSI] = "csi",
2205                 [_I2S_CKIN] = "i2s_ckin",
2206         };
2207
2208         for (i = 0; i < NB_OSC; i++) {
2209                 if (clk_get_by_name(dev, name[i], &priv->osc_clk[i]))
2210                         dev_dbg(dev, "No source clock \"%s\"", name[i]);
2211                 else
2212                         dev_dbg(dev, "%s clock rate: %luHz\n",
2213                                 name[i], clk_get_rate(&priv->osc_clk[i]));
2214         }
2215 }
2216
2217 static void  __maybe_unused stm32mp1_clk_dump(struct stm32mp1_clk_priv *priv)
2218 {
2219         char buf[32];
2220         int i, s, p;
2221
2222         printf("Clocks:\n");
2223         for (i = 0; i < _PARENT_NB; i++) {
2224                 printf("- %s : %s MHz\n",
2225                        stm32mp1_clk_parent_name[i],
2226                        strmhz(buf, stm32mp1_clk_get(priv, i)));
2227         }
2228         printf("Source Clocks:\n");
2229         for (i = 0; i < _PARENT_SEL_NB; i++) {
2230                 p = (readl(priv->base + priv->data->sel[i].offset) >>
2231                      priv->data->sel[i].src) & priv->data->sel[i].msk;
2232                 if (p < priv->data->sel[i].nb_parent) {
2233                         s = priv->data->sel[i].parent[p];
2234                         printf("- %s(%d) => parent %s(%d)\n",
2235                                stm32mp1_clk_parent_sel_name[i], i,
2236                                stm32mp1_clk_parent_name[s], s);
2237                 } else {
2238                         printf("- %s(%d) => parent index %d is invalid\n",
2239                                stm32mp1_clk_parent_sel_name[i], i, p);
2240                 }
2241         }
2242 }
2243
2244 #ifdef CONFIG_CMD_CLK
2245 int soc_clk_dump(void)
2246 {
2247         struct udevice *dev;
2248         struct stm32mp1_clk_priv *priv;
2249         int ret;
2250
2251         ret = uclass_get_device_by_driver(UCLASS_CLK,
2252                                           DM_DRIVER_GET(stm32mp1_clock),
2253                                           &dev);
2254         if (ret)
2255                 return ret;
2256
2257         priv = dev_get_priv(dev);
2258
2259         stm32mp1_clk_dump(priv);
2260
2261         return 0;
2262 }
2263 #endif
2264
2265 static int stm32mp1_clk_probe(struct udevice *dev)
2266 {
2267         int result = 0;
2268         struct stm32mp1_clk_priv *priv = dev_get_priv(dev);
2269
2270         priv->base = dev_read_addr(dev->parent);
2271         if (priv->base == FDT_ADDR_T_NONE)
2272                 return -EINVAL;
2273
2274         priv->data = (void *)&stm32mp1_data;
2275
2276         if (!priv->data->gate || !priv->data->sel ||
2277             !priv->data->pll)
2278                 return -EINVAL;
2279
2280         stm32mp1_osc_init(dev);
2281
2282 #ifdef STM32MP1_CLOCK_TREE_INIT
2283         /* clock tree init is done only one time, before relocation */
2284         if (!(gd->flags & GD_FLG_RELOC))
2285                 result = stm32mp1_clktree(dev);
2286         if (result)
2287                 dev_err(dev, "clock tree initialization failed (%d)\n", result);
2288 #endif
2289
2290 #ifndef CONFIG_SPL_BUILD
2291 #if defined(VERBOSE_DEBUG)
2292         /* display debug information for probe after relocation */
2293         if (gd->flags & GD_FLG_RELOC)
2294                 stm32mp1_clk_dump(priv);
2295 #endif
2296
2297         gd->cpu_clk = stm32mp1_clk_get(priv, _CK_MPU);
2298         gd->bus_clk = stm32mp1_clk_get(priv, _ACLK);
2299         /* DDRPHYC father */
2300         gd->mem_clk = stm32mp1_clk_get(priv, _PLL2_R);
2301 #if defined(CONFIG_DISPLAY_CPUINFO)
2302         if (gd->flags & GD_FLG_RELOC) {
2303                 char buf[32];
2304
2305                 log_info("Clocks:\n");
2306                 log_info("- MPU : %s MHz\n", strmhz(buf, gd->cpu_clk));
2307                 log_info("- MCU : %s MHz\n",
2308                          strmhz(buf, stm32mp1_clk_get(priv, _CK_MCU)));
2309                 log_info("- AXI : %s MHz\n", strmhz(buf, gd->bus_clk));
2310                 log_info("- PER : %s MHz\n",
2311                          strmhz(buf, stm32mp1_clk_get(priv, _CK_PER)));
2312                 log_info("- DDR : %s MHz\n", strmhz(buf, gd->mem_clk));
2313         }
2314 #endif /* CONFIG_DISPLAY_CPUINFO */
2315 #endif
2316
2317         return result;
2318 }
2319
2320 static const struct clk_ops stm32mp1_clk_ops = {
2321         .enable = stm32mp1_clk_enable,
2322         .disable = stm32mp1_clk_disable,
2323         .get_rate = stm32mp1_clk_get_rate,
2324         .set_rate = stm32mp1_clk_set_rate,
2325 };
2326
2327 U_BOOT_DRIVER(stm32mp1_clock) = {
2328         .name = "stm32mp1_clk",
2329         .id = UCLASS_CLK,
2330         .ops = &stm32mp1_clk_ops,
2331         .priv_auto      = sizeof(struct stm32mp1_clk_priv),
2332         .probe = stm32mp1_clk_probe,
2333 };