diff --git a/drivers/st/clk/clk-stm32-core.c b/drivers/st/clk/clk-stm32-core.c
index 9fe8c8cca..6787d504c 100644
--- a/drivers/st/clk/clk-stm32-core.c
+++ b/drivers/st/clk/clk-stm32-core.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2022, STMicroelectronics - All Rights Reserved
+ * Copyright (C) 2022-2024, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
  */
@@ -28,7 +28,7 @@ struct stm32_clk_priv *clk_stm32_get_priv(void)
 	return stm32_clock_data;
 }
 
-static void stm32mp1_clk_lock(struct spinlock *lock)
+static void _clk_lock(struct spinlock *lock)
 {
 	if (stm32mp_lock_available()) {
 		/* Assume interrupts are masked */
@@ -36,21 +36,21 @@ static void stm32mp1_clk_lock(struct spinlock *lock)
 	}
 }
 
-static void stm32mp1_clk_unlock(struct spinlock *lock)
+static void _clk_unlock(struct spinlock *lock)
 {
 	if (stm32mp_lock_available()) {
 		spin_unlock(lock);
 	}
 }
 
-void stm32mp1_clk_rcc_regs_lock(void)
+void clk_stm32_rcc_regs_lock(void)
 {
-	stm32mp1_clk_lock(&reg_lock);
+	_clk_lock(&reg_lock);
 }
 
-void stm32mp1_clk_rcc_regs_unlock(void)
+void clk_stm32_rcc_regs_unlock(void)
 {
-	stm32mp1_clk_unlock(&reg_lock);
+	_clk_unlock(&reg_lock);
 }
 
 #define TIMEOUT_US_1S	U(1000000)
@@ -224,6 +224,15 @@ const struct clk_stm32 *_clk_get(struct stm32_clk_priv *priv, int id)
 	return NULL;
 }
 
+static const struct stm32_clk_ops *_clk_get_ops(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+
+	assert(clk->ops != NO_OPS);
+
+	return priv->ops_array[clk->ops];
+}
+
 #define clk_div_mask(_width) GENMASK(((_width) - 1U), 0U)
 
 static unsigned int _get_table_div(const struct clk_div_table *table,
@@ -377,7 +386,7 @@ int _clk_stm32_set_parent_by_index(struct stm32_clk_priv *priv, int clk, int sel
 
 int _clk_stm32_get_parent(struct stm32_clk_priv *priv, int clk_id)
 {
-	const struct clk_stm32 *clk = _clk_get(priv, clk_id);
+	const struct stm32_clk_ops *ops = _clk_get_ops(priv, clk_id);
 	const struct parent_cfg *parent;
 	uint16_t mux_id;
 	int sel;
@@ -394,8 +403,8 @@ int _clk_stm32_get_parent(struct stm32_clk_priv *priv, int clk_id)
 	mux_id &= MUX_PARENT_MASK;
 	parent = &priv->parents[mux_id];
 
-	if (clk->ops->get_parent != NULL) {
-		sel = clk->ops->get_parent(priv, clk_id);
+	if (ops->get_parent != NULL) {
+		sel = ops->get_parent(priv, clk_id);
 	} else {
 		sel = clk_mux_get_parent(priv, mux_id);
 	}
@@ -464,7 +473,7 @@ int clk_get_index(struct stm32_clk_priv *priv, unsigned long binding_id)
 
 unsigned long _clk_stm32_get_rate(struct stm32_clk_priv *priv, int id)
 {
-	const struct clk_stm32 *clk = _clk_get(priv, id);
+	const struct stm32_clk_ops *ops = _clk_get_ops(priv, id);
 	int parent;
 
 	if ((unsigned int)id >= priv->num) {
@@ -476,14 +485,14 @@ unsigned long _clk_stm32_get_rate(struct stm32_clk_priv *priv, int id)
 		return 0UL;
 	}
 
-	if (clk->ops->recalc_rate != NULL) {
+	if (ops->recalc_rate != NULL) {
 		unsigned long prate = 0UL;
 
 		if (parent != CLK_IS_ROOT) {
 			prate = _clk_stm32_get_rate(priv, parent);
 		}
 
-		return clk->ops->recalc_rate(priv, id, prate);
+		return ops->recalc_rate(priv, id, prate);
 	}
 
 	if (parent == CLK_IS_ROOT) {
@@ -520,10 +529,10 @@ bool _stm32_clk_is_flags(struct stm32_clk_priv *priv, int id, uint8_t flag)
 
 int clk_stm32_enable_call_ops(struct stm32_clk_priv *priv, uint16_t id)
 {
-	const struct clk_stm32 *clk = _clk_get(priv, id);
+	const struct stm32_clk_ops *ops = _clk_get_ops(priv, id);
 
-	if (clk->ops->enable != NULL) {
-		clk->ops->enable(priv, id);
+	if (ops->enable != NULL) {
+		ops->enable(priv, id);
 	}
 
 	return 0;
@@ -550,7 +559,7 @@ static int _clk_stm32_enable_core(struct stm32_clk_priv *priv, int id)
 
 	priv->gate_refcounts[id]++;
 
-	if (priv->gate_refcounts[id] == UINT_MAX) {
+	if (priv->gate_refcounts[id] == UINT8_MAX) {
 		ERROR("%s: %d max enable count !", __func__, id);
 		panic();
 	}
@@ -562,19 +571,19 @@ int _clk_stm32_enable(struct stm32_clk_priv *priv, int id)
 {
 	int ret;
 
-	stm32mp1_clk_lock(&refcount_lock);
+	_clk_lock(&refcount_lock);
 	ret = _clk_stm32_enable_core(priv, id);
-	stm32mp1_clk_unlock(&refcount_lock);
+	_clk_unlock(&refcount_lock);
 
 	return ret;
 }
 
 void clk_stm32_disable_call_ops(struct stm32_clk_priv *priv, uint16_t id)
 {
-	const struct clk_stm32 *clk = _clk_get(priv, id);
+	const struct stm32_clk_ops *ops = _clk_get_ops(priv, id);
 
-	if (clk->ops->disable != NULL) {
-		clk->ops->disable(priv, id);
+	if (ops->disable != NULL) {
+		ops->disable(priv, id);
 	}
 }
 
@@ -610,19 +619,19 @@ static void _clk_stm32_disable_core(struct stm32_clk_priv *priv, int id)
 
 void _clk_stm32_disable(struct stm32_clk_priv *priv, int id)
 {
-	stm32mp1_clk_lock(&refcount_lock);
+	_clk_lock(&refcount_lock);
 
 	_clk_stm32_disable_core(priv, id);
 
-	stm32mp1_clk_unlock(&refcount_lock);
+	_clk_unlock(&refcount_lock);
 }
 
 bool _clk_stm32_is_enabled(struct stm32_clk_priv *priv, int id)
 {
-	const struct clk_stm32 *clk = _clk_get(priv, id);
+	const struct stm32_clk_ops *ops = _clk_get_ops(priv, id);
 
-	if (clk->ops->is_enabled != NULL) {
-		return clk->ops->is_enabled(priv, id);
+	if (ops->is_enabled != NULL) {
+		return ops->is_enabled(priv, id);
 	}
 
 	return priv->gate_refcounts[id];
@@ -957,6 +966,10 @@ int clk_stm32_osc_gate_enable(struct stm32_clk_priv *priv, int id)
 {
 	struct clk_oscillator_data *osc_data = clk_oscillator_get_data(priv, id);
 
+	if (osc_data->frequency == 0UL) {
+		return 0;
+	}
+
 	_clk_stm32_gate_enable(priv, osc_data->gate_id);
 
 	if (_clk_stm32_gate_wait_ready(priv, osc_data->gate_rdy_id, true) != 0U) {
@@ -971,6 +984,10 @@ void clk_stm32_osc_gate_disable(struct stm32_clk_priv *priv, int id)
 {
 	struct clk_oscillator_data *osc_data = clk_oscillator_get_data(priv, id);
 
+	if (osc_data->frequency == 0UL) {
+		return;
+	}
+
 	_clk_stm32_gate_disable(priv, osc_data->gate_id);
 
 	if (_clk_stm32_gate_wait_ready(priv, osc_data->gate_rdy_id, false) != 0U) {
@@ -1073,12 +1090,10 @@ int clk_stm32_init(struct stm32_clk_priv *priv, uintptr_t base)
 	priv->base = base;
 
 	for (i = 0U; i < priv->num; i++) {
-		const struct clk_stm32 *clk = _clk_get(priv, i);
+		const struct stm32_clk_ops *ops = _clk_get_ops(priv, i);
 
-		assert(clk->ops != NULL);
-
-		if (clk->ops->init != NULL) {
-			clk->ops->init(priv, i);
+		if (ops->init != NULL) {
+			ops->init(priv, i);
 		}
 	}
 
diff --git a/drivers/st/clk/clk-stm32-core.h b/drivers/st/clk/clk-stm32-core.h
index 8bfb5134f..bfb5f1199 100644
--- a/drivers/st/clk/clk-stm32-core.h
+++ b/drivers/st/clk/clk-stm32-core.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2022, STMicroelectronics - All Rights Reserved
+ * Copyright (C) 2022-2024, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
  */
@@ -21,23 +21,23 @@ struct gate_cfg {
 };
 
 struct clk_div_table {
-	unsigned int val;
-	unsigned int div;
+	uint16_t val;
+	uint16_t div;
 };
 
 struct div_cfg {
+	const struct clk_div_table *table;
 	uint16_t offset;
 	uint8_t shift;
 	uint8_t width;
 	uint8_t flags;
 	uint8_t bitrdy;
-	const struct clk_div_table *table;
 };
 
 struct parent_cfg {
-	uint8_t num_parents;
 	const uint16_t *id_parents;
 	struct mux_cfg *mux;
+	uint8_t num_parents;
 };
 
 struct stm32_clk_priv;
@@ -56,9 +56,9 @@ struct stm32_clk_ops {
 struct clk_stm32 {
 	uint16_t binding;
 	uint16_t parent;
+	uint8_t ops;
 	uint8_t flags;
 	void *clock_cfg;
-	const struct stm32_clk_ops *ops;
 };
 
 struct stm32_clk_priv {
@@ -73,8 +73,9 @@ struct stm32_clk_priv {
 	const uint32_t nb_div;
 	struct clk_oscillator_data *osci_data;
 	const uint32_t nb_osci_data;
-	uint32_t *gate_refcounts;
+	uint8_t *gate_refcounts;
 	void *pdata;
+	const struct stm32_clk_ops **ops_array;
 };
 
 struct stm32_clk_bypass {
@@ -97,18 +98,14 @@ struct stm32_clk_drive {
 
 struct clk_oscillator_data {
 	const char *name;
-	uint16_t id_clk;
-	unsigned long frequency;
-	uint16_t gate_id;
-	uint16_t gate_rdy_id;
 	struct stm32_clk_bypass *bypass;
 	struct stm32_clk_css *css;
 	struct stm32_clk_drive *drive;
-};
+	unsigned long frequency;
+	uint16_t id_clk;
+	uint16_t gate_id;
+	uint16_t gate_rdy_id;
 
-struct clk_fixed_rate {
-	const char *name;
-	unsigned long fixed_rate;
 };
 
 struct clk_gate_cfg {
@@ -144,6 +141,9 @@ struct clk_gate_cfg {
 #define MASK_WIDTH_SHIFT(_width, _shift) \
 	GENMASK(((_width) + (_shift) - 1U), (_shift))
 
+void clk_stm32_rcc_regs_lock(void);
+void clk_stm32_rcc_regs_unlock(void);
+
 int clk_stm32_init(struct stm32_clk_priv *priv, uintptr_t base);
 void clk_stm32_enable_critical_clocks(void);
 
@@ -218,7 +218,7 @@ void clk_stm32_display_clock_info(void);
 #endif
 
 struct clk_stm32_div_cfg {
-	int id;
+	uint8_t id;
 };
 
 #define STM32_DIV(idx, _binding, _parent, _flags, _div_id) \
@@ -229,11 +229,11 @@ struct clk_stm32_div_cfg {
 		.clock_cfg	= &(struct clk_stm32_div_cfg){\
 			.id	= (_div_id),\
 		},\
-		.ops		= &clk_stm32_divider_ops,\
+		.ops		= STM32_DIVIDER_OPS,\
 	}
 
 struct clk_stm32_gate_cfg {
-	int id;
+	uint8_t id;
 };
 
 #define STM32_GATE(idx, _binding, _parent, _flags, _gate_id) \
@@ -244,12 +244,12 @@ struct clk_stm32_gate_cfg {
 		.clock_cfg	= &(struct clk_stm32_gate_cfg){\
 			.id	= (_gate_id),\
 		},\
-		.ops		= &clk_stm32_gate_ops,\
+		.ops		= STM32_GATE_OPS,\
 	}
 
 struct fixed_factor_cfg {
-	unsigned int mult;
-	unsigned int div;
+	uint8_t mult;
+	uint8_t div;
 };
 
 unsigned long fixed_factor_recalc_rate(struct stm32_clk_priv *priv,
@@ -263,7 +263,7 @@ unsigned long fixed_factor_recalc_rate(struct stm32_clk_priv *priv,
 			.mult	= (_mult),\
 			.div	= (_div),\
 		},\
-		.ops		= &clk_fixed_factor_ops,\
+		.ops		= FIXED_FACTOR_OPS,\
 	}
 
 #define GATE(idx, _binding, _parent, _flags, _offset, _bit_idx) \
@@ -275,7 +275,7 @@ unsigned long fixed_factor_recalc_rate(struct stm32_clk_priv *priv,
 			.offset		= (_offset),\
 			.bit_idx	= (_bit_idx),\
 		},\
-		.ops		= &clk_gate_ops,\
+		.ops		= GATE_OPS,\
 	}
 
 #define STM32_MUX(idx, _binding, _mux_id, _flags) \
@@ -284,7 +284,7 @@ unsigned long fixed_factor_recalc_rate(struct stm32_clk_priv *priv,
 		.parent		= (MUX(_mux_id)),\
 		.flags		= (_flags),\
 		.clock_cfg	= NULL,\
-		.ops		= (&clk_mux_ops),\
+		.ops		= STM32_MUX_OPS\
 	}
 
 struct clk_timer_cfg {
@@ -301,7 +301,7 @@ struct clk_timer_cfg {
 			.apbdiv = (_apbdiv),\
 			.timpre = (_timpre),\
 		},\
-		.ops		= &clk_timer_ops,\
+		.ops		= STM32_TIMER_OPS,\
 	}
 
 struct clk_stm32_fixed_rate_cfg {
@@ -315,7 +315,7 @@ struct clk_stm32_fixed_rate_cfg {
 		.clock_cfg	= &(struct clk_stm32_fixed_rate_cfg){\
 			.rate	= (_rate),\
 		},\
-		.ops		= &clk_stm32_fixed_rate_ops,\
+		.ops		= STM32_FIXED_RATE_OPS,\
 	}
 
 #define BYPASS(_offset, _bit_byp, _bit_digbyp) &(struct stm32_clk_bypass){\
@@ -355,7 +355,7 @@ int clk_stm32_osc_gate_enable(struct stm32_clk_priv *priv, int id);
 void clk_stm32_osc_gate_disable(struct stm32_clk_priv *priv, int id);
 
 struct stm32_osc_cfg {
-	int osc_id;
+	uint8_t osc_id;
 };
 
 #define CLK_OSC(idx, _idx, _parent, _osc_id) \
@@ -366,7 +366,7 @@ struct stm32_osc_cfg {
 		.clock_cfg	= &(struct stm32_osc_cfg){\
 			.osc_id = (_osc_id),\
 		},\
-		.ops		= &clk_stm32_osc_ops,\
+		.ops		= STM32_OSC_OPS,\
 	}
 
 #define CLK_OSC_FIXED(idx, _idx, _parent, _osc_id) \
@@ -377,7 +377,7 @@ struct stm32_osc_cfg {
 		.clock_cfg	= &(struct stm32_osc_cfg){\
 			.osc_id	= (_osc_id),\
 		},\
-		.ops		= &clk_stm32_osc_nogate_ops,\
+		.ops		= STM32_OSC_NOGATE_OPS,\
 	}
 
 extern const struct stm32_clk_ops clk_mux_ops;
@@ -390,4 +390,19 @@ extern const struct stm32_clk_ops clk_stm32_fixed_rate_ops;
 extern const struct stm32_clk_ops clk_stm32_osc_ops;
 extern const struct stm32_clk_ops clk_stm32_osc_nogate_ops;
 
+enum {
+	NO_OPS,
+	FIXED_FACTOR_OPS,
+	GATE_OPS,
+	STM32_MUX_OPS,
+	STM32_DIVIDER_OPS,
+	STM32_GATE_OPS,
+	STM32_TIMER_OPS,
+	STM32_FIXED_RATE_OPS,
+	STM32_OSC_OPS,
+	STM32_OSC_NOGATE_OPS,
+
+	STM32_LAST_OPS
+};
+
 #endif /* CLK_STM32_CORE_H */
diff --git a/drivers/st/clk/clk-stm32mp13.c b/drivers/st/clk/clk-stm32mp13.c
index 01d176400..fd620492f 100644
--- a/drivers/st/clk/clk-stm32mp13.c
+++ b/drivers/st/clk/clk-stm32mp13.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2022-2023, STMicroelectronics - All Rights Reserved
+ * Copyright (C) 2022-2024, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
  */
@@ -186,65 +186,7 @@ enum stm32_clock {
 	_MCE,
 	_FMC_K,
 	_QSPI_K,
-#if defined(IMAGE_BL32)
-	_LTDC,
-	_DMA1,
-	_DMA2,
-	_MDMA,
-	_ETH1MAC,
-	_USBH,
-	_TIM2,
-	_TIM3,
-	_TIM4,
-	_TIM5,
-	_TIM6,
-	_TIM7,
-	_LPTIM1_K,
-	_SPI2_K,
-	_SPI3_K,
-	_SPDIF_K,
-	_TIM1,
-	_TIM8,
-	_SPI1_K,
-	_SAI1_K,
-	_SAI2_K,
-	_DFSDM,
-	_FDCAN_K,
-	_TIM13,
-	_TIM14,
-	_TIM16,
-	_TIM17,
-	_SPI4_K,
-	_SPI5_K,
-	_I2C1_K,
-	_I2C2_K,
-	_ADFSDM,
-	_LPTIM2_K,
-	_LPTIM3_K,
-	_LPTIM4_K,
-	_LPTIM5_K,
-	_VREF,
-	_DTS,
-	_PMBCTRL,
-	_HDP,
-	_STGENRO,
-	_DCMIPP_K,
-	_DMAMUX1,
-	_DMAMUX2,
-	_DMA3,
-	_ADC1_K,
-	_ADC2_K,
-	_TSC,
-	_AXIMC,
-	_ETH1CK,
-	_ETH1TX,
-	_ETH1RX,
-	_CRC1,
-	_ETH2CK,
-	_ETH2TX,
-	_ETH2RX,
-	_ETH2MAC,
-#endif
+
 	CK_LAST
 };
 
@@ -947,7 +889,7 @@ static bool pll4_bootrom;
 #endif
 
 /* RCC clock device driver private */
-static unsigned int refcounts_mp13[CK_LAST];
+static uint8_t refcounts_mp13[CK_LAST];
 
 static const struct stm32_clk_pll *clk_st32_pll_data(unsigned int idx);
 
@@ -1007,6 +949,11 @@ static void stm32_enable_oscillator_lse(struct stm32_clk_priv *priv)
 		return;
 	}
 
+	/* Do not reconfigure LSE if already enabled */
+	if (_clk_stm32_gate_is_enabled(priv, osc_data->gate_id)) {
+		return;
+	}
+
 	clk_oscillator_set_bypass(priv, _CK_LSE, digbyp, bypass);
 
 	clk_oscillator_set_drive(priv, _CK_LSE, drive);
@@ -1027,8 +974,8 @@ static int stm32mp1_set_hsidiv(uint8_t hsidiv)
 	timeout = timeout_init_us(HSIDIV_TIMEOUT);
 	while ((mmio_read_32(address) & RCC_OCRDYR_HSIDIVRDY) == 0U) {
 		if (timeout_elapsed(timeout)) {
-			ERROR("HSIDIV failed @ 0x%lx: 0x%x\n",
-			      address, mmio_read_32(address));
+			EARLY_ERROR("HSIDIV failed @ 0x%lx: 0x%x\n",
+				    address, mmio_read_32(address));
 			return -ETIMEDOUT;
 		}
 	}
@@ -1050,7 +997,7 @@ static int stm32mp1_hsidiv(unsigned long hsifreq)
 	}
 
 	if (hsidiv == 4U) {
-		ERROR("Invalid clk-hsi frequency\n");
+		EARLY_ERROR("Invalid clk-hsi frequency\n");
 		return -EINVAL;
 	}
 
@@ -1292,7 +1239,7 @@ static void clk_stm32_pll_config_vco(struct stm32_clk_priv *priv,
 	uint32_t value = 0;
 
 	if (clk_stm32_pll_compute_cfgr1(priv, pll, vco, &value) != 0) {
-		ERROR("Invalid Vref clock !\n");
+		EARLY_ERROR("Invalid Vref clock !\n");
 		panic();
 	}
 
@@ -1352,6 +1299,123 @@ static inline struct stm32_pll_dt_cfg *clk_stm32_pll_get_pdata(int pll_idx)
 	return &pdata->pll[pll_idx];
 }
 
+/* Define characteristic for PLL1 : PLL_2000 */
+#define POST_DIVM_MIN	8000000U
+#define POST_DIVM_MAX	16000000U
+#define DIVM_MIN	0U
+#define DIVM_MAX	63U
+#define DIVN_MIN	24U
+#define DIVN_MAX	99U
+#define DIVP_MIN	0U
+#define DIVP_MAX	127U
+#define FRAC_MAX	8192U
+#define VCO_MIN		992000000U
+#define VCO_MAX		2000000000U
+
+static int clk_compute_pll1_settings(uint32_t freq_khz)
+{
+	struct stm32_clk_priv *priv = clk_stm32_get_priv();
+	struct stm32_pll_dt_cfg *pll1 = clk_stm32_pll_get_pdata(_PLL1);
+	struct stm32_pll_dt_cfg *pll2 = clk_stm32_pll_get_pdata(_PLL2);
+	unsigned long long best_diff = ULLONG_MAX;
+	unsigned int divm;
+	unsigned long input_freq = 0UL;
+	uint32_t src =  pll2->vco.src;
+
+	/* PLL1 share the same clock source than PLL2 */
+	switch (src) {
+	case CLK_PLL12_HSI:
+		input_freq = _clk_stm32_get_rate(priv, _CK_HSI);
+		break;
+	case CLK_PLL12_HSE:
+		input_freq = _clk_stm32_get_rate(priv, _CK_HSE);
+		break;
+	default:
+		break;
+	}
+
+	if (input_freq == 0UL) {
+		panic();
+	}
+
+	/* Following parameters have always the same value */
+	pll1->output.output[PLL_CFG_Q] = 0U;
+	pll1->output.output[PLL_CFG_R] = 0U;
+
+	for (divm = (DIVM_MAX + 1U); divm != DIVM_MIN; divm--) {
+		unsigned long post_divm = input_freq / divm;
+		unsigned int divp;
+
+		if ((post_divm < POST_DIVM_MIN) || (post_divm > POST_DIVM_MAX)) {
+			continue;
+		}
+
+		for (divp = DIVP_MIN; divp <= DIVP_MAX; divp++) {
+			unsigned long long output_freq = freq_khz * 1000ULL;
+			unsigned long long freq;
+			unsigned long long divn;
+			unsigned long long frac;
+			unsigned int i;
+
+			freq = output_freq * divm * (divp + 1U);
+
+			divn = (freq / input_freq) - 1U;
+			if ((divn < DIVN_MIN) || (divn > DIVN_MAX)) {
+				continue;
+			}
+
+			frac = ((freq * FRAC_MAX) / input_freq) - ((divn + 1U) * FRAC_MAX);
+
+			/* 2 loops to refine the fractional part */
+			for (i = 2U; i != 0U; i--) {
+				unsigned long long diff;
+				unsigned long long vco;
+
+				if (frac > FRAC_MAX) {
+					break;
+				}
+
+				vco = (post_divm * (divn + 1U)) + ((post_divm * frac) / FRAC_MAX);
+
+				if ((vco < (VCO_MIN / 2U)) || (vco > (VCO_MAX / 2U))) {
+					frac++;
+					continue;
+				}
+
+				freq = vco / (divp + 1U);
+				if (output_freq < freq) {
+					diff = freq - output_freq;
+				} else {
+					diff = output_freq - freq;
+				}
+
+				if (diff < best_diff)  {
+					pll1->vco.src = src;
+					pll1->vco.status = RCC_PLLNCR_DIVPEN | RCC_PLLNCR_PLLON;
+					pll1->vco.div_mn[PLL_CFG_M] = divm - 1U;
+					pll1->vco.div_mn[PLL_CFG_N] = (uint32_t)divn;
+					pll1->vco.frac = (uint32_t)frac;
+					pll1->output.output[PLL_CFG_P] = divp;
+
+					if (diff == 0U) {
+						return 0;
+					}
+
+					best_diff = diff;
+				}
+
+				frac++;
+			}
+		}
+	}
+
+	if (best_diff == ULLONG_MAX) {
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 static bool _clk_stm32_pll_is_enabled(struct stm32_clk_priv *priv, const struct stm32_clk_pll *pll)
 {
 	uintptr_t pll_base = priv->base + pll->reg_pllxcr;
@@ -1388,8 +1452,8 @@ static int _clk_stm32_pll_wait_ready_on(struct stm32_clk_priv *priv,
 	/* Wait PLL lock */
 	while ((mmio_read_32(pll_base) & RCC_PLLNCR_PLLRDY) == 0U) {
 		if (timeout_elapsed(timeout)) {
-			ERROR("%d clock start failed @ 0x%x: 0x%x\n",
-			      pll->clk_id, pll->reg_pllxcr, mmio_read_32(pll_base));
+			ERROR("PLL%d start failed @ 0x%x: 0x%x\n",
+			      pll->clk_id - _CK_PLL1 + 1, pll->reg_pllxcr, mmio_read_32(pll_base));
 			return -EINVAL;
 		}
 	}
@@ -1406,8 +1470,8 @@ static int _clk_stm32_pll_wait_ready_off(struct stm32_clk_priv *priv,
 	/* Wait PLL lock */
 	while ((mmio_read_32(pll_base) & RCC_PLLNCR_PLLRDY) != 0U) {
 		if (timeout_elapsed(timeout)) {
-			ERROR("%d clock stop failed @ 0x%x: 0x%x\n",
-			      pll->clk_id, pll->reg_pllxcr, mmio_read_32(pll_base));
+			ERROR("PLL%d stop failed @ 0x%x: 0x%x\n",
+			      pll->clk_id - _CK_PLL1 + 1, pll->reg_pllxcr, mmio_read_32(pll_base));
 			return -EINVAL;
 		}
 	}
@@ -1629,7 +1693,7 @@ static const struct stm32_clk_pll *clk_st32_pll_data(unsigned int idx)
 }
 
 struct stm32_pll_cfg {
-	int pll_id;
+	uint8_t pll_id;
 };
 
 static unsigned long clk_stm32_pll_recalc_rate(struct stm32_clk_priv *priv,  int id,
@@ -1711,12 +1775,12 @@ static const struct stm32_clk_ops clk_stm32_pll_ops = {
 	.clock_cfg	= &(struct stm32_pll_cfg) {\
 		.pll_id = _pll_id,\
 	},\
-	.ops = &clk_stm32_pll_ops,\
+	.ops = STM32_PLL_OPS,\
 }
 
 struct clk_stm32_composite_cfg {
-	int gate_id;
-	int div_id;
+	uint8_t gate_id;
+	uint8_t div_id;
 };
 
 static unsigned long clk_stm32_composite_recalc_rate(struct stm32_clk_priv *priv,
@@ -1768,9 +1832,32 @@ static const struct stm32_clk_ops clk_stm32_composite_ops = {
 		.gate_id	= (_gate_id),\
 		.div_id	= (_div_id),\
 	},\
-	.ops = &clk_stm32_composite_ops,\
+	.ops = STM32_COMPOSITE_OPS,\
 }
 
+enum {
+	STM32_PLL_OPS = STM32_LAST_OPS,
+	STM32_COMPOSITE_OPS,
+
+	MP13_LAST_OPS
+};
+
+static const struct stm32_clk_ops *ops_array_mp13[MP13_LAST_OPS] = {
+	[NO_OPS] =  NULL,
+	[FIXED_FACTOR_OPS] = &clk_fixed_factor_ops,
+	[GATE_OPS] = &clk_gate_ops,
+	[STM32_MUX_OPS] = &clk_mux_ops,
+	[STM32_DIVIDER_OPS] = &clk_stm32_divider_ops,
+	[STM32_GATE_OPS] = &clk_stm32_gate_ops,
+	[STM32_TIMER_OPS] = &clk_timer_ops,
+	[STM32_FIXED_RATE_OPS] = &clk_stm32_fixed_rate_ops,
+	[STM32_OSC_OPS] = &clk_stm32_osc_ops,
+	[STM32_OSC_NOGATE_OPS] = &clk_stm32_osc_nogate_ops,
+
+	[STM32_PLL_OPS] = &clk_stm32_pll_ops,
+	[STM32_COMPOSITE_OPS] = &clk_stm32_composite_ops
+};
+
 static const struct clk_stm32 stm32mp13_clk[CK_LAST] = {
 	/* ROOT CLOCKS */
 	CLK_FIXED_RATE(_CK_OFF, _NO_ID, 0),
@@ -1882,7 +1969,6 @@ static const struct clk_stm32 stm32mp13_clk[CK_LAST] = {
 	STM32_GATE(_SDMMC2_K, SDMMC2_K, MUX(MUX_SDMMC2), 0, GATE_SDMMC2),
 	STM32_GATE(_DBGCK, CK_DBG, _CKAXI, 0, GATE_DBGCK),
 
-/* TODO: CHECK CLOCK FOR BL2/BL32 AND IF ONLY FOR TEST OR NOT */
 	STM32_GATE(_USART3_K, USART3_K, MUX(MUX_UART35), 0, GATE_USART3),
 	STM32_GATE(_UART4_K, UART4_K, MUX(MUX_UART4), 0, GATE_UART4),
 	STM32_GATE(_UART5_K, UART5_K, MUX(MUX_UART35), 0, GATE_UART5),
@@ -1896,61 +1982,6 @@ static const struct clk_stm32 stm32mp13_clk[CK_LAST] = {
 	STM32_COMPOSITE(_MCO1_K, CK_MCO1, MUX(MUX_MCO1), 0, GATE_MCO1, DIV_MCO1),
 	STM32_COMPOSITE(_MCO2_K, CK_MCO2, MUX(MUX_MCO2), 0, GATE_MCO2, DIV_MCO2),
 	STM32_COMPOSITE(_TRACECK, CK_TRACE, _CKAXI, 0, GATE_TRACECK, DIV_TRACE),
-
-#if defined(IMAGE_BL32)
-	STM32_GATE(_TIM2, TIM2_K, _CKTIMG1, 0, GATE_TIM2),
-	STM32_GATE(_TIM3, TIM3_K, _CKTIMG1, 0, GATE_TIM3),
-	STM32_GATE(_TIM4, TIM4_K, _CKTIMG1, 0, GATE_TIM4),
-	STM32_GATE(_TIM5, TIM5_K, _CKTIMG1, 0, GATE_TIM5),
-	STM32_GATE(_TIM6, TIM6_K, _CKTIMG1, 0, GATE_TIM6),
-	STM32_GATE(_TIM7, TIM7_K, _CKTIMG1, 0, GATE_TIM7),
-	STM32_GATE(_TIM13, TIM13_K, _CKTIMG3, 0, GATE_TIM13),
-	STM32_GATE(_TIM14, TIM14_K, _CKTIMG3, 0, GATE_TIM14),
-	STM32_GATE(_LPTIM1_K, LPTIM1_K, MUX(MUX_LPTIM1), 0, GATE_LPTIM1),
-	STM32_GATE(_SPI2_K, SPI2_K, MUX(MUX_SPI23), 0, GATE_SPI2),
-	STM32_GATE(_SPI3_K, SPI3_K, MUX(MUX_SPI23), 0, GATE_SPI3),
-	STM32_GATE(_SPDIF_K, SPDIF_K, MUX(MUX_SPDIF), 0, GATE_SPDIF),
-	STM32_GATE(_TIM1, TIM1_K, _CKTIMG2, 0, GATE_TIM1),
-	STM32_GATE(_TIM8, TIM8_K, _CKTIMG2, 0, GATE_TIM8),
-	STM32_GATE(_TIM16, TIM16_K, _CKTIMG3, 0, GATE_TIM16),
-	STM32_GATE(_TIM17, TIM17_K, _CKTIMG3, 0, GATE_TIM17),
-	STM32_GATE(_SPI1_K, SPI1_K, MUX(MUX_SPI1), 0, GATE_SPI1),
-	STM32_GATE(_SPI4_K, SPI4_K, MUX(MUX_SPI4), 0, GATE_SPI4),
-	STM32_GATE(_SPI5_K, SPI5_K, MUX(MUX_SPI5), 0, GATE_SPI5),
-	STM32_GATE(_SAI1_K, SAI1_K, MUX(MUX_SAI1), 0, GATE_SAI1),
-	STM32_GATE(_SAI2_K, SAI2_K, MUX(MUX_SAI2), 0, GATE_SAI2),
-	STM32_GATE(_DFSDM, DFSDM_K, MUX(MUX_SAI1), 0, GATE_DFSDM),
-	STM32_GATE(_FDCAN_K, FDCAN_K, MUX(MUX_FDCAN), 0, GATE_FDCAN),
-	STM32_GATE(_USBH, USBH, _CKAXI, 0, GATE_USBH),
-	STM32_GATE(_I2C1_K, I2C1_K, MUX(MUX_I2C12), 0, GATE_I2C1),
-	STM32_GATE(_I2C2_K, I2C2_K, MUX(MUX_I2C12), 0, GATE_I2C2),
-	STM32_GATE(_ADFSDM, ADFSDM_K, MUX(MUX_SAI1), 0, GATE_ADFSDM),
-	STM32_GATE(_LPTIM2_K, LPTIM2_K, MUX(MUX_LPTIM2), 0, GATE_LPTIM2),
-	STM32_GATE(_LPTIM3_K, LPTIM3_K, MUX(MUX_LPTIM3), 0, GATE_LPTIM3),
-	STM32_GATE(_LPTIM4_K, LPTIM4_K, MUX(MUX_LPTIM45), 0, GATE_LPTIM4),
-	STM32_GATE(_LPTIM5_K, LPTIM5_K, MUX(MUX_LPTIM45), 0, GATE_LPTIM5),
-	STM32_GATE(_VREF, VREF, _PCLK3, 0, GATE_VREF),
-	STM32_GATE(_DTS, TMPSENS, _PCLK3, 0, GATE_DTS),
-	STM32_GATE(_PMBCTRL, PMBCTRL, _PCLK3, 0, GATE_HDP),
-	STM32_GATE(_HDP, HDP, _PCLK3, 0, GATE_PMBCTRL),
-	STM32_GATE(_STGENRO, STGENRO, _PCLK4, 0, GATE_DCMIPP),
-	STM32_GATE(_DCMIPP_K, DCMIPP_K, MUX(MUX_DCMIPP), 0, GATE_DCMIPP),
-	STM32_GATE(_DMAMUX1, DMAMUX1, _CKAXI, 0, GATE_DMAMUX1),
-	STM32_GATE(_DMAMUX2, DMAMUX2, _CKAXI, 0, GATE_DMAMUX2),
-	STM32_GATE(_DMA3, DMA3, _CKAXI, 0, GATE_DMAMUX2),
-	STM32_GATE(_ADC1_K, ADC1_K, MUX(MUX_ADC1), 0, GATE_ADC1),
-	STM32_GATE(_ADC2_K, ADC2_K, MUX(MUX_ADC2), 0, GATE_ADC2),
-	STM32_GATE(_TSC, TSC, _CKAXI, 0, GATE_TSC),
-	STM32_GATE(_AXIMC, AXIMC, _CKAXI, 0, GATE_AXIMC),
-	STM32_GATE(_CRC1, CRC1, _CKAXI, 0, GATE_ETH1TX),
-	STM32_GATE(_ETH1CK, ETH1CK_K, MUX(MUX_ETH1), 0, GATE_ETH1CK),
-	STM32_GATE(_ETH1TX, ETH1TX, _CKAXI, 0, GATE_ETH1TX),
-	STM32_GATE(_ETH1RX, ETH1RX, _CKAXI, 0, GATE_ETH1RX),
-	STM32_GATE(_ETH2CK, ETH2CK_K, MUX(MUX_ETH2), 0, GATE_ETH2CK),
-	STM32_GATE(_ETH2TX, ETH2TX, _CKAXI, 0, GATE_ETH2TX),
-	STM32_GATE(_ETH2RX, ETH2RX, _CKAXI, 0, GATE_ETH2RX),
-	STM32_GATE(_ETH2MAC, ETH2MAC, _CKAXI, 0, GATE_ETH2MAC),
-#endif
 };
 
 static struct stm32_pll_dt_cfg mp13_pll[_PLL_NB];
@@ -1986,6 +2017,7 @@ static struct stm32_clk_priv stm32mp13_clock_data = {
 	.nb_osci_data	= ARRAY_SIZE(stm32mp13_osc_data),
 	.gate_refcounts	= refcounts_mp13,
 	.pdata		= &stm32mp13_clock_pdata,
+	.ops_array	= ops_array_mp13,
 };
 
 static int stm32mp1_init_clock_tree(void)
@@ -2072,8 +2104,6 @@ static int stm32mp1_init_clock_tree(void)
 	return 0;
 }
 
-#define LSEDRV_MEDIUM_HIGH 2
-
 static int clk_stm32_parse_oscillator_fdt(void *fdt, int node, const char *name,
 					  struct stm32_osci_dt_cfg *osci)
 {
@@ -2241,7 +2271,8 @@ static int stm32_clk_parse_fdt_all_pll(void *fdt, int node, struct stm32_clk_pla
 {
 	size_t i = 0U;
 
-	for (i = _PLL1; i < pdata->npll; i++) {
+	/* PLL1 is not configurable with device tree */
+	for (i = _PLL2; i < pdata->npll; i++) {
 		struct stm32_pll_dt_cfg *pll = &pdata->pll[i];
 		char name[RCC_PLL_NAME_SIZE];
 		int subnode = 0;
@@ -2301,8 +2332,33 @@ static int stm32_clk_parse_fdt(struct stm32_clk_platdata *pdata)
 	return 0;
 }
 
+void stm32mp1_clk_rcc_regs_lock(void)
+{
+	clk_stm32_rcc_regs_lock();
+}
+
+void stm32mp1_clk_rcc_regs_unlock(void)
+{
+	clk_stm32_rcc_regs_unlock();
+}
+
 int stm32mp1_clk_init(void)
 {
+	int ret;
+
+	/* compute the PLL1 settings, not read in device tree */
+	ret = clk_compute_pll1_settings(PLL1_NOMINAL_FREQ_IN_KHZ);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = stm32mp1_init_clock_tree();
+	if (ret != 0) {
+		return ret;
+	}
+
+	clk_stm32_enable_critical_clocks();
+
 	return 0;
 }
 
@@ -2317,16 +2373,6 @@ int stm32mp1_clk_probe(void)
 	}
 
 	ret = clk_stm32_init(&stm32mp13_clock_data, base);
-	if (ret != 0) {
-		return ret;
-	}
 
-	ret = stm32mp1_init_clock_tree();
-	if (ret != 0) {
-		return ret;
-	}
-
-	clk_stm32_enable_critical_clocks();
-
-	return 0;
+	return ret;
 }
diff --git a/drivers/st/clk/stm32mp1_clk.c b/drivers/st/clk/stm32mp1_clk.c
index 818fd8596..3d352afb0 100644
--- a/drivers/st/clk/stm32mp1_clk.c
+++ b/drivers/st/clk/stm32mp1_clk.c
@@ -27,6 +27,71 @@
 
 #include <platform_def.h>
 
+enum stm32mp1_pllcfg {
+	PLLCFG_M,
+	PLLCFG_N,
+	PLL_DIV_MN_NB,
+	PLLCFG_P = PLL_DIV_MN_NB,
+	PLLCFG_Q,
+	PLLCFG_R,
+	PLLCFG_O,
+	PLLCFG_NB
+};
+
+#define PLL_DIV_MN_NB	2
+#define PLL_DIV_PQR_NB	3
+
+enum stm32mp1_pllcsg {
+	PLLCSG_MOD_PER,
+	PLLCSG_INC_STEP,
+	PLLCSG_SSCG_MODE,
+	PLLCSG_NB
+};
+
+struct stm32_pll_dt_cfg {
+	bool status;
+	uint32_t src;
+	uint32_t cfg[PLLCFG_NB];
+	uint32_t frac;
+	bool csg_enabled;
+	uint32_t csg[PLLCSG_NB];
+};
+
+struct stm32_clk_platdata {
+	uint32_t npll;
+	struct stm32_pll_dt_cfg *pll;
+	uint32_t nclksrc;
+	uint32_t *clksrc;
+	uint32_t nclkdiv;
+	uint32_t *clkdiv;
+	bool lse_css;
+};
+
+struct stm32_clk_priv {
+	uintptr_t base;
+	const struct mux_cfg *parents;
+	const uint32_t nb_parents;
+	const struct div_cfg *div;
+	const uint32_t nb_div;
+	void *pdata;
+};
+
+static struct stm32_clk_priv *stm32_clock_data;
+
+static struct stm32_clk_priv *clk_stm32_get_priv(void)
+{
+	return stm32_clock_data;
+}
+
+static int clk_stm32_init(struct stm32_clk_priv *priv, uintptr_t base)
+{
+	stm32_clock_data = priv;
+
+	priv->base = base;
+
+	return 0;
+}
+
 #define MAX_HSI_HZ		64000000
 #define USB_PHY_48_MHZ		48000000
 
@@ -39,6 +104,199 @@
 #define HSIDIV_TIMEOUT		TIMEOUT_US_200MS
 #define OSCRDY_TIMEOUT		TIMEOUT_US_1S
 
+struct mux_cfg {
+	uint16_t offset;
+	uint8_t shift;
+	uint8_t width;
+	uint8_t bitrdy;
+};
+
+struct div_cfg {
+	uint16_t offset;
+	uint8_t shift;
+	uint8_t width;
+	uint8_t bitrdy;
+};
+
+#define DIV_NO_BIT_RDY UINT8_MAX
+
+#define DIV_CFG(_id, _offset, _shift, _width,  _bitrdy)\
+	[(_id)] = {\
+		.offset	= (_offset),\
+		.shift	= (_shift),\
+		.width	= (_width),\
+		.bitrdy	= (_bitrdy),\
+	}
+
+static const struct div_cfg dividers_mp15[] = {
+	DIV_CFG(DIV_MPU, RCC_MPCKDIVR, 0, 4, 31),
+	DIV_CFG(DIV_AXI, RCC_AXIDIVR, 0, 3, 31),
+	DIV_CFG(DIV_MCU, RCC_MCUDIVR, 0, 4, 31),
+	DIV_CFG(DIV_APB1, RCC_APB1DIVR, 0, 3, 31),
+	DIV_CFG(DIV_APB2, RCC_APB2DIVR, 0, 3, 31),
+	DIV_CFG(DIV_APB3, RCC_APB3DIVR, 0, 3, 31),
+	DIV_CFG(DIV_APB4, RCC_APB4DIVR, 0, 3, 31),
+	DIV_CFG(DIV_APB5, RCC_APB5DIVR, 0, 3, 31),
+	DIV_CFG(DIV_RTC, RCC_RTCDIVR, 0, 6, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_MCO1, RCC_MCO1CFGR, 4, 4, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_MCO2, RCC_MCO2CFGR, 4, 4, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_TRACE, RCC_DBGCFGR, 0, 3, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_ETHPTP, RCC_ETHCKSELR, 4, 4, DIV_NO_BIT_RDY),
+};
+
+/*
+ * MUX CONFIG
+ */
+
+#define MUX_NO_BIT_RDY		UINT8_MAX
+
+#define MUXRDY_CFG(_id, _offset, _shift, _width,  _bitrdy)\
+	[(_id)] = {\
+		.offset	= (_offset),\
+		.shift	= (_shift),\
+		.width	= (_width),\
+		.bitrdy = (_bitrdy),\
+	}
+
+#define MUX_CFG(_id, _offset, _shift, _width)\
+	MUXRDY_CFG(_id, _offset, _shift, _width,  MUX_NO_BIT_RDY)
+
+static const struct mux_cfg parent_mp15[MUX_NB] = {
+	MUX_CFG(MUX_PLL12,	RCC_RCK12SELR, 0, 2),
+	MUX_CFG(MUX_PLL3,	RCC_RCK3SELR, 0, 2),
+	MUX_CFG(MUX_PLL4,	RCC_RCK4SELR, 0, 2),
+	MUX_CFG(MUX_CKPER,	RCC_CPERCKSELR, 0, 2),
+	MUXRDY_CFG(MUX_MPU,	RCC_MPCKSELR, 0, 2, 31),
+	MUXRDY_CFG(MUX_AXI,	RCC_ASSCKSELR, 0, 3, 31),
+	MUXRDY_CFG(MUX_MCU,	RCC_MSSCKSELR, 0, 2, 31),
+	MUX_CFG(MUX_RTC,	RCC_BDCR, 16, 2),
+	MUX_CFG(MUX_SDMMC12,	RCC_SDMMC12CKSELR, 0, 3),
+	MUX_CFG(MUX_SPI2S23,	RCC_SPI2S23CKSELR, 0, 3),
+	MUX_CFG(MUX_SPI45,	RCC_SPI45CKSELR, 0, 3),
+	MUX_CFG(MUX_I2C12,	RCC_I2C12CKSELR, 0, 3),
+	MUX_CFG(MUX_I2C35,	RCC_I2C35CKSELR, 0, 3),
+	MUX_CFG(MUX_LPTIM23,	RCC_LPTIM23CKSELR, 0, 3),
+	MUX_CFG(MUX_LPTIM45,	RCC_LPTIM45CKSELR, 0, 3),
+	MUX_CFG(MUX_UART24,	RCC_UART24CKSELR, 0, 3),
+	MUX_CFG(MUX_UART35,	RCC_UART35CKSELR, 0, 3),
+	MUX_CFG(MUX_UART78,	RCC_UART78CKSELR, 0, 3),
+	MUX_CFG(MUX_SAI1,	RCC_SAI1CKSELR, 0, 3),
+	MUX_CFG(MUX_ETH,	RCC_ETHCKSELR, 0, 2),
+	MUX_CFG(MUX_I2C46,	RCC_I2C46CKSELR, 0, 3),
+	MUX_CFG(MUX_RNG2,	RCC_RNG2CKSELR, 0, 2),
+	MUX_CFG(MUX_SDMMC3,	RCC_SDMMC3CKSELR, 0, 3),
+	MUX_CFG(MUX_FMC,	RCC_FMCCKSELR, 0, 2),
+	MUX_CFG(MUX_QSPI,	RCC_QSPICKSELR, 0, 2),
+	MUX_CFG(MUX_USBPHY,	RCC_USBCKSELR, 0, 2),
+	MUX_CFG(MUX_USBO,	RCC_USBCKSELR, 4, 1),
+	MUX_CFG(MUX_SPDIF,	RCC_SPDIFCKSELR, 0, 2),
+	MUX_CFG(MUX_SPI2S1,	RCC_SPI2S1CKSELR, 0, 3),
+	MUX_CFG(MUX_CEC,	RCC_CECCKSELR, 0, 2),
+	MUX_CFG(MUX_LPTIM1,	RCC_LPTIM1CKSELR, 0, 3),
+	MUX_CFG(MUX_UART6,	RCC_UART6CKSELR, 0, 3),
+	MUX_CFG(MUX_FDCAN,	RCC_FDCANCKSELR, 0, 2),
+	MUX_CFG(MUX_SAI2,	RCC_SAI2CKSELR, 0, 3),
+	MUX_CFG(MUX_SAI3,	RCC_SAI3CKSELR, 0, 3),
+	MUX_CFG(MUX_SAI4,	RCC_SAI4CKSELR, 0, 3),
+	MUX_CFG(MUX_ADC,	RCC_ADCCKSELR, 0, 2),
+	MUX_CFG(MUX_DSI,	RCC_DSICKSELR, 0, 1),
+	MUX_CFG(MUX_RNG1,	RCC_RNG1CKSELR, 0, 2),
+	MUX_CFG(MUX_STGEN,	RCC_STGENCKSELR, 0, 2),
+	MUX_CFG(MUX_UART1,	RCC_UART1CKSELR, 0, 3),
+	MUX_CFG(MUX_SPI6,	RCC_SPI6CKSELR, 0, 3),
+	MUX_CFG(MUX_MCO1,	RCC_MCO1CFGR, 0, 3),
+	MUX_CFG(MUX_MCO2,	RCC_MCO2CFGR, 0, 3),
+};
+
+#define MASK_WIDTH_SHIFT(_width, _shift) \
+	GENMASK(((_width) + (_shift) - 1U), (_shift))
+
+int clk_mux_get_parent(struct stm32_clk_priv *priv, uint32_t mux_id)
+{
+	const struct mux_cfg *mux;
+	uint32_t mask;
+
+	if (mux_id >= priv->nb_parents) {
+		panic();
+	}
+
+	mux = &priv->parents[mux_id];
+
+	mask = MASK_WIDTH_SHIFT(mux->width, mux->shift);
+
+	return (mmio_read_32(priv->base + mux->offset) & mask) >> mux->shift;
+}
+
+static int clk_mux_set_parent(struct stm32_clk_priv *priv, uint16_t pid, uint8_t sel)
+{
+	const struct mux_cfg *mux = &priv->parents[pid];
+	uintptr_t address = priv->base + mux->offset;
+	uint32_t mask;
+	uint64_t timeout;
+
+	mask = MASK_WIDTH_SHIFT(mux->width, mux->shift);
+
+	mmio_clrsetbits_32(address, mask, (sel << mux->shift) & mask);
+
+	if (mux->bitrdy == MUX_NO_BIT_RDY) {
+		return 0;
+	}
+
+	timeout = timeout_init_us(CLKSRC_TIMEOUT);
+
+	mask = BIT(mux->bitrdy);
+
+	while ((mmio_read_32(address) & mask) == 0U) {
+		if (timeout_elapsed(timeout)) {
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+static int stm32_clk_configure_mux(struct stm32_clk_priv *priv, uint32_t val)
+{
+	uint32_t data = val & CMD_DATA_MASK;
+	int mux = (data & MUX_ID_MASK) >> MUX_ID_SHIFT;
+	int sel = (data & MUX_SEL_MASK) >> MUX_SEL_SHIFT;
+
+	return clk_mux_set_parent(priv, mux, sel);
+}
+
+int clk_stm32_set_div(struct stm32_clk_priv *priv, uint32_t div_id, uint32_t value)
+{
+	const struct div_cfg *divider;
+	uintptr_t address;
+	uint64_t timeout;
+	uint32_t mask;
+
+	if (div_id >= priv->nb_div) {
+		panic();
+	}
+
+	divider = &priv->div[div_id];
+	address = priv->base + divider->offset;
+
+	mask = MASK_WIDTH_SHIFT(divider->width, divider->shift);
+	mmio_clrsetbits_32(address, mask, (value << divider->shift) & mask);
+
+	if (divider->bitrdy == DIV_NO_BIT_RDY) {
+		return 0;
+	}
+
+	timeout = timeout_init_us(CLKSRC_TIMEOUT);
+	mask = BIT(divider->bitrdy);
+
+	while ((mmio_read_32(address) & mask) == 0U) {
+		if (timeout_elapsed(timeout)) {
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
 const char *stm32mp_osc_node_label[NB_OSC] = {
 	[_LSI] = "clk-lsi",
 	[_LSE] = "clk-lse",
@@ -206,23 +464,6 @@ enum stm32mp1_clkdiv_id {
 	CLKDIV_NB
 };
 
-enum stm32mp1_pllcfg {
-	PLLCFG_M,
-	PLLCFG_N,
-	PLLCFG_P,
-	PLLCFG_Q,
-	PLLCFG_R,
-	PLLCFG_O,
-	PLLCFG_NB
-};
-
-enum stm32mp1_pllcsg {
-	PLLCSG_MOD_PER,
-	PLLCSG_INC_STEP,
-	PLLCSG_SSCG_MODE,
-	PLLCSG_NB
-};
-
 enum stm32mp1_plltype {
 	PLL_800,
 	PLL_1600,
@@ -537,7 +778,18 @@ static const struct stm32mp1_clk_sel stm32mp1_clk_sel[_PARENT_SEL_NB] = {
 };
 
 /* Define characteristic of PLL according type */
-#define DIVN_MIN	24
+#define POST_DIVM_MIN	8000000U
+#define POST_DIVM_MAX	16000000U
+#define DIVM_MIN	0U
+#define DIVM_MAX	63U
+#define DIVN_MIN	24U
+#define DIVN_MAX	99U
+#define DIVP_MIN	0U
+#define DIVP_MAX	127U
+#define FRAC_MAX	8192U
+#define VCO_MIN		800000000U
+#define VCO_MAX		1600000000U
+
 static const struct stm32mp1_pll stm32mp1_pll[PLL_TYPE_NB] = {
 	[PLL_800] = {
 		.refclk_min = 4,
@@ -1327,6 +1579,11 @@ static void stm32mp1_lse_enable(bool bypass, bool digbyp, uint32_t lsedrv)
 	uint32_t value;
 	uintptr_t rcc_base = stm32mp_rcc_base();
 
+	/* Do not reconfigure LSE if it is already ON */
+	if ((mmio_read_32(rcc_base + RCC_BDCR) & RCC_BDCR_LSEON) == RCC_BDCR_LSEON) {
+		return;
+	}
+
 	if (digbyp) {
 		mmio_setbits_32(rcc_base + RCC_BDCR, RCC_BDCR_DIGBYP);
 	}
@@ -1360,7 +1617,7 @@ static void stm32mp1_lse_enable(bool bypass, bool digbyp, uint32_t lsedrv)
 static void stm32mp1_lse_wait(void)
 {
 	if (stm32mp1_osc_wait(true, RCC_BDCR, RCC_BDCR_LSERDY) != 0) {
-		VERBOSE("%s: failed\n", __func__);
+		EARLY_ERROR("%s: failed\n", __func__);
 	}
 }
 
@@ -1369,7 +1626,7 @@ static void stm32mp1_lsi_set(bool enable)
 	stm32mp1_ls_osc_set(enable, RCC_RDLSICR, RCC_RDLSICR_LSION);
 
 	if (stm32mp1_osc_wait(enable, RCC_RDLSICR, RCC_RDLSICR_LSIRDY) != 0) {
-		VERBOSE("%s: failed\n", __func__);
+		EARLY_ERROR("%s: failed\n", __func__);
 	}
 }
 
@@ -1387,7 +1644,7 @@ static void stm32mp1_hse_enable(bool bypass, bool digbyp, bool css)
 
 	stm32mp1_hs_ocs_set(true, RCC_OCENR_HSEON);
 	if (stm32mp1_osc_wait(true, RCC_OCRDYR, RCC_OCRDYR_HSERDY) != 0) {
-		VERBOSE("%s: failed\n", __func__);
+		EARLY_ERROR("%s: failed\n", __func__);
 	}
 
 	if (css) {
@@ -1406,7 +1663,7 @@ static void stm32mp1_csi_set(bool enable)
 {
 	stm32mp1_hs_ocs_set(enable, RCC_OCENR_CSION);
 	if (stm32mp1_osc_wait(enable, RCC_OCRDYR, RCC_OCRDYR_CSIRDY) != 0) {
-		VERBOSE("%s: failed\n", __func__);
+		EARLY_ERROR("%s: failed\n", __func__);
 	}
 }
 
@@ -1414,7 +1671,7 @@ static void stm32mp1_hsi_set(bool enable)
 {
 	stm32mp1_hs_ocs_set(enable, RCC_OCENR_HSION);
 	if (stm32mp1_osc_wait(enable, RCC_OCRDYR, RCC_OCRDYR_HSIRDY) != 0) {
-		VERBOSE("%s: failed\n", __func__);
+		EARLY_ERROR("%s: failed\n", __func__);
 	}
 }
 
@@ -1454,7 +1711,7 @@ static int stm32mp1_hsidiv(unsigned long hsifreq)
 	}
 
 	if (hsidiv == 4U) {
-		ERROR("Invalid clk-hsi frequency\n");
+		EARLY_ERROR("Invalid clk-hsi frequency\n");
 		return -1;
 	}
 
@@ -1467,7 +1724,7 @@ static int stm32mp1_hsidiv(unsigned long hsifreq)
 
 static bool stm32mp1_check_pll_conf(enum stm32mp1_pll_id pll_id,
 				    unsigned int clksrc,
-				    uint32_t *pllcfg, int plloff)
+				    uint32_t *pllcfg, uint32_t fracv)
 {
 	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
 	uintptr_t rcc_base = stm32mp_rcc_base();
@@ -1476,8 +1733,7 @@ static bool stm32mp1_check_pll_conf(enum stm32mp1_pll_id pll_id,
 	uintptr_t clksrc_address = rcc_base + (clksrc >> 4);
 	unsigned long refclk;
 	uint32_t ifrge = 0U;
-	uint32_t src, value, fracv = 0;
-	void *fdt;
+	uint32_t src, value;
 
 	/* Check PLL output */
 	if (mmio_read_32(pllxcr) != RCC_PLLNCR_PLLON) {
@@ -1516,10 +1772,6 @@ static bool stm32mp1_check_pll_conf(enum stm32mp1_pll_id pll_id,
 	}
 
 	/* Fractional configuration */
-	if (fdt_get_address(&fdt) == 1) {
-		fracv = fdt_read_uint32_default(fdt, plloff, "frac", 0);
-	}
-
 	value = fracv << RCC_PLLNFRACR_FRACV_SHIFT;
 	value |= RCC_PLLNFRACR_FRACLE;
 	if (mmio_read_32(rcc_base + pll->pllxfracr) != value) {
@@ -1561,8 +1813,8 @@ static int stm32mp1_pll_output(enum stm32mp1_pll_id pll_id, uint32_t output)
 	/* Wait PLL lock */
 	while ((mmio_read_32(pllxcr) & RCC_PLLNCR_PLLRDY) == 0U) {
 		if (timeout_elapsed(timeout)) {
-			ERROR("PLL%u start failed @ 0x%lx: 0x%x\n",
-			      pll_id, pllxcr, mmio_read_32(pllxcr));
+			EARLY_ERROR("PLL%u start failed @ 0x%lx: 0x%x\n",
+				    pll_id, pllxcr, mmio_read_32(pllxcr));
 			return -ETIMEDOUT;
 		}
 	}
@@ -1590,8 +1842,8 @@ static int stm32mp1_pll_stop(enum stm32mp1_pll_id pll_id)
 	/* Wait PLL stopped */
 	while ((mmio_read_32(pllxcr) & RCC_PLLNCR_PLLRDY) != 0U) {
 		if (timeout_elapsed(timeout)) {
-			ERROR("PLL%u stop failed @ 0x%lx: 0x%x\n",
-			      pll_id, pllxcr, mmio_read_32(pllxcr));
+			EARLY_ERROR("PLL%u stop failed @ 0x%lx: 0x%x\n",
+				    pll_id, pllxcr, mmio_read_32(pllxcr));
 			return -ETIMEDOUT;
 		}
 	}
@@ -1683,190 +1935,270 @@ static void stm32mp1_pll_csg(enum stm32mp1_pll_id pll_id, uint32_t *csg)
 			RCC_PLLNCR_SSCG_CTRL);
 }
 
-static int stm32mp1_set_clksrc(unsigned int clksrc)
+static int clk_compute_pll1_settings(unsigned long input_freq,
+				     uint32_t freq_khz,
+				     uint32_t *pllcfg, uint32_t *fracv)
 {
-	uintptr_t clksrc_address = stm32mp_rcc_base() + (clksrc >> 4);
-	uint64_t timeout;
+	unsigned long long best_diff = ULLONG_MAX;
+	unsigned int divm;
 
-	mmio_clrsetbits_32(clksrc_address, RCC_SELR_SRC_MASK,
-			   clksrc & RCC_SELR_SRC_MASK);
+	/* Following parameters have always the same value */
+	pllcfg[PLLCFG_Q] = 0U;
+	pllcfg[PLLCFG_R] = 0U;
+	pllcfg[PLLCFG_O] = PQR(1, 0, 0);
 
-	timeout = timeout_init_us(CLKSRC_TIMEOUT);
-	while ((mmio_read_32(clksrc_address) & RCC_SELR_SRCRDY) == 0U) {
-		if (timeout_elapsed(timeout)) {
-			ERROR("CLKSRC %x start failed @ 0x%lx: 0x%x\n", clksrc,
-			      clksrc_address, mmio_read_32(clksrc_address));
-			return -ETIMEDOUT;
-		}
-	}
+	for (divm = (DIVM_MAX + 1U); divm != DIVM_MIN; divm--) {
+		unsigned long post_divm = input_freq / divm;
+		unsigned int divp;
 
-	return 0;
-}
-
-static int stm32mp1_set_clkdiv(unsigned int clkdiv, uintptr_t address)
-{
-	uint64_t timeout;
-
-	mmio_clrsetbits_32(address, RCC_DIVR_DIV_MASK,
-			   clkdiv & RCC_DIVR_DIV_MASK);
-
-	timeout = timeout_init_us(CLKDIV_TIMEOUT);
-	while ((mmio_read_32(address) & RCC_DIVR_DIVRDY) == 0U) {
-		if (timeout_elapsed(timeout)) {
-			ERROR("CLKDIV %x start failed @ 0x%lx: 0x%x\n",
-			      clkdiv, address, mmio_read_32(address));
-			return -ETIMEDOUT;
-		}
-	}
-
-	return 0;
-}
-
-static void stm32mp1_mco_csg(uint32_t clksrc, uint32_t clkdiv)
-{
-	uintptr_t clksrc_address = stm32mp_rcc_base() + (clksrc >> 4);
-
-	/*
-	 * Binding clksrc :
-	 *      bit15-4 offset
-	 *      bit3:   disable
-	 *      bit2-0: MCOSEL[2:0]
-	 */
-	if ((clksrc & 0x8U) != 0U) {
-		mmio_clrbits_32(clksrc_address, RCC_MCOCFG_MCOON);
-	} else {
-		mmio_clrsetbits_32(clksrc_address,
-				   RCC_MCOCFG_MCOSRC_MASK,
-				   clksrc & RCC_MCOCFG_MCOSRC_MASK);
-		mmio_clrsetbits_32(clksrc_address,
-				   RCC_MCOCFG_MCODIV_MASK,
-				   clkdiv << RCC_MCOCFG_MCODIV_SHIFT);
-		mmio_setbits_32(clksrc_address, RCC_MCOCFG_MCOON);
-	}
-}
-
-static void stm32mp1_set_rtcsrc(unsigned int clksrc, bool lse_css)
-{
-	uintptr_t address = stm32mp_rcc_base() + RCC_BDCR;
-
-	if (((mmio_read_32(address) & RCC_BDCR_RTCCKEN) == 0U) ||
-	    (clksrc != (uint32_t)CLK_RTC_DISABLED)) {
-		mmio_clrsetbits_32(address,
-				   RCC_BDCR_RTCSRC_MASK,
-				   (clksrc & RCC_SELR_SRC_MASK) << RCC_BDCR_RTCSRC_SHIFT);
-
-		mmio_setbits_32(address, RCC_BDCR_RTCCKEN);
-	}
-
-	if (lse_css) {
-		mmio_setbits_32(address, RCC_BDCR_LSECSSON);
-	}
-}
-
-static void stm32mp1_pkcs_config(uint32_t pkcs)
-{
-	uintptr_t address = stm32mp_rcc_base() + ((pkcs >> 4) & 0xFFFU);
-	uint32_t value = pkcs & 0xFU;
-	uint32_t mask = 0xFU;
-
-	if ((pkcs & BIT(31)) != 0U) {
-		mask <<= 4;
-		value <<= 4;
-	}
-
-	mmio_clrsetbits_32(address, mask, value);
-}
-
-static int clk_get_pll_settings_from_dt(int plloff, unsigned int *pllcfg,
-					uint32_t *fracv, uint32_t *csg,
-					bool *csg_set)
-{
-	void *fdt;
-	int ret;
-
-	if (fdt_get_address(&fdt) == 0) {
-		return -FDT_ERR_NOTFOUND;
-	}
-
-	ret = fdt_read_uint32_array(fdt, plloff, "cfg", (uint32_t)PLLCFG_NB,
-				    pllcfg);
-	if (ret < 0) {
-		return -FDT_ERR_NOTFOUND;
-	}
-
-	*fracv = fdt_read_uint32_default(fdt, plloff, "frac", 0);
-
-	ret = fdt_read_uint32_array(fdt, plloff, "csg", (uint32_t)PLLCSG_NB,
-				    csg);
-
-	*csg_set = (ret == 0);
-
-	if (ret == -FDT_ERR_NOTFOUND) {
-		ret = 0;
-	}
-
-	return ret;
-}
-
-int stm32mp1_clk_init(void)
-{
-	uintptr_t rcc_base = stm32mp_rcc_base();
-	uint32_t pllfracv[_PLL_NB];
-	uint32_t pllcsg[_PLL_NB][PLLCSG_NB];
-	unsigned int clksrc[CLKSRC_NB];
-	unsigned int clkdiv[CLKDIV_NB];
-	unsigned int pllcfg[_PLL_NB][PLLCFG_NB];
-	int plloff[_PLL_NB];
-	int ret, len;
-	enum stm32mp1_pll_id i;
-	bool pllcsg_set[_PLL_NB];
-	bool pllcfg_valid[_PLL_NB];
-	bool lse_css = false;
-	bool pll3_preserve = false;
-	bool pll4_preserve = false;
-	bool pll4_bootrom = false;
-	const fdt32_t *pkcs_cell;
-	void *fdt;
-	int stgen_p = stm32mp1_clk_get_parent(STGEN_K);
-	int usbphy_p = stm32mp1_clk_get_parent(USBPHY_K);
-
-	if (fdt_get_address(&fdt) == 0) {
-		return -FDT_ERR_NOTFOUND;
-	}
-
-	ret = fdt_rcc_read_uint32_array("st,clksrc", (uint32_t)CLKSRC_NB,
-					clksrc);
-	if (ret < 0) {
-		return -FDT_ERR_NOTFOUND;
-	}
-
-	ret = fdt_rcc_read_uint32_array("st,clkdiv", (uint32_t)CLKDIV_NB,
-					clkdiv);
-	if (ret < 0) {
-		return -FDT_ERR_NOTFOUND;
-	}
-
-	for (i = (enum stm32mp1_pll_id)0; i < _PLL_NB; i++) {
-		char name[12];
-
-		snprintf(name, sizeof(name), "st,pll@%u", i);
-		plloff[i] = fdt_rcc_subnode_offset(name);
-
-		pllcfg_valid[i] = fdt_check_node(plloff[i]);
-		if (!pllcfg_valid[i]) {
+		if ((post_divm < POST_DIVM_MIN) || (post_divm > POST_DIVM_MAX)) {
 			continue;
 		}
 
-		ret = clk_get_pll_settings_from_dt(plloff[i], pllcfg[i],
-						   &pllfracv[i], pllcsg[i],
-						   &pllcsg_set[i]);
+		for (divp = DIVP_MIN; divp <= DIVP_MAX; divp++) {
+			unsigned long long output_freq = freq_khz * 1000ULL;
+			unsigned long long freq;
+			unsigned long long divn;
+			unsigned long long frac;
+			unsigned int i;
+
+			freq = output_freq * divm * (divp + 1U);
+
+			divn = (freq / input_freq) - 1U;
+			if ((divn < DIVN_MIN) || (divn > DIVN_MAX)) {
+				continue;
+			}
+
+			frac = ((freq * FRAC_MAX) / input_freq) - ((divn + 1U) * FRAC_MAX);
+
+			/* 2 loops to refine the fractional part */
+			for (i = 2U; i != 0U; i--) {
+				unsigned long long diff;
+				unsigned long long vco;
+
+				if (frac > FRAC_MAX) {
+					break;
+				}
+
+				vco = (post_divm * (divn + 1U)) + ((post_divm * frac) / FRAC_MAX);
+
+				if ((vco < (VCO_MIN / 2U)) || (vco > (VCO_MAX / 2U))) {
+					frac++;
+					continue;
+				}
+
+				freq = vco / (divp + 1U);
+				if (output_freq < freq) {
+					diff = freq - output_freq;
+				} else {
+					diff = output_freq - freq;
+				}
+
+				if (diff < best_diff)  {
+					pllcfg[PLLCFG_M] = divm - 1U;
+					pllcfg[PLLCFG_N] = (uint32_t)divn;
+					pllcfg[PLLCFG_P] = divp;
+					*fracv = (uint32_t)frac;
+
+					if (diff == 0U) {
+						return 0;
+					}
+
+					best_diff = diff;
+				}
+
+				frac++;
+			}
+		}
+	}
+
+	if (best_diff == ULLONG_MAX) {
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int clk_get_pll1_settings(uint32_t clksrc, uint32_t freq_khz,
+				 uint32_t *pllcfg, uint32_t *fracv)
+{
+	unsigned long input_freq = 0UL;
+
+	assert(pllcfg != NULL);
+	assert(fracv != NULL);
+
+	switch (clksrc) {
+	case CLK_PLL12_HSI:
+		input_freq = stm32mp_clk_get_rate(CK_HSI);
+		break;
+	case CLK_PLL12_HSE:
+		input_freq = stm32mp_clk_get_rate(CK_HSE);
+		break;
+	default:
+		break;
+	}
+
+	if (input_freq == 0UL) {
+		panic();
+	}
+
+	return clk_compute_pll1_settings(input_freq, freq_khz, pllcfg, fracv);
+}
+
+static int stm32_clk_dividers_configure(struct stm32_clk_priv *priv)
+{
+	struct stm32_clk_platdata *pdata = priv->pdata;
+	uint32_t i;
+
+	for (i = 0U; i < pdata->nclkdiv; i++) {
+		uint32_t div_id, div_n;
+		uint32_t val;
+		int ret;
+
+		val = pdata->clkdiv[i] & CMD_DATA_MASK;
+		div_id = (val & DIV_ID_MASK) >> DIV_ID_SHIFT;
+		div_n = (val & DIV_DIVN_MASK) >> DIV_DIVN_SHIFT;
+
+		ret = clk_stm32_set_div(priv, div_id, div_n);
 		if (ret != 0) {
 			return ret;
 		}
 	}
 
-	stm32mp1_mco_csg(clksrc[CLKSRC_MCO1], clkdiv[CLKDIV_MCO1]);
-	stm32mp1_mco_csg(clksrc[CLKSRC_MCO2], clkdiv[CLKDIV_MCO2]);
+	return 0;
+}
+
+static int stm32_clk_configure_clk(struct stm32_clk_priv *priv, uint32_t data)
+{
+	uint32_t sel = (data & CLK_SEL_MASK) >> CLK_SEL_SHIFT;
+	uint32_t enable = (data & CLK_ON_MASK) >> CLK_ON_SHIFT;
+	unsigned long binding_id = ((unsigned long)data & CLK_ID_MASK) >> CLK_ID_SHIFT;
+	struct stm32_clk_platdata *pdata = priv->pdata;
+
+	if (binding_id == RTC) {
+		uintptr_t address = stm32mp_rcc_base() + RCC_BDCR;
+
+		if (((mmio_read_32(address) & RCC_BDCR_RTCCKEN) == 0U) || (enable != 0U)) {
+			mmio_clrsetbits_32(address, RCC_BDCR_RTCSRC_MASK,
+					   (sel & RCC_SELR_SRC_MASK) << RCC_BDCR_RTCSRC_SHIFT);
+
+			mmio_setbits_32(address, RCC_BDCR_RTCCKEN);
+			/* Configure LSE CSS */
+			if (pdata->lse_css) {
+				mmio_setbits_32(priv->base + RCC_BDCR, RCC_BDCR_LSECSSON);
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int stm32_clk_configure_by_addr_val(struct stm32_clk_priv *priv,
+					   uint32_t data)
+{
+	uint32_t addr = data >> CLK_ADDR_SHIFT;
+	uint32_t val = data & CLK_ADDR_VAL_MASK;
+
+	mmio_setbits_32(priv->base + addr, val);
+
+	return 0;
+}
+
+static int stm32_clk_source_configure(struct stm32_clk_priv *priv)
+{
+	struct stm32_clk_platdata *pdata = priv->pdata;
+	bool ckper_disabled = false;
+	uint32_t i;
+
+	for (i = 0U; i < pdata->nclksrc; i++) {
+		uint32_t val = pdata->clksrc[i];
+		uint32_t cmd, cmd_data;
+		int ret;
+
+		if (val & CMD_ADDR_BIT) {
+			ret = stm32_clk_configure_by_addr_val(priv, val & ~CMD_ADDR_BIT);
+			if (ret != 0) {
+				return ret;
+			}
+
+			continue;
+		}
+
+		if (val == (uint32_t)CLK_CKPER_DISABLED) {
+			ckper_disabled = true;
+			continue;
+		}
+
+		cmd = (val & CMD_MASK) >> CMD_SHIFT;
+		cmd_data = val & ~CMD_MASK;
+
+		switch (cmd) {
+		case CMD_MUX:
+			ret = stm32_clk_configure_mux(priv, cmd_data);
+			break;
+
+		case CMD_CLK:
+			ret = stm32_clk_configure_clk(priv, cmd_data);
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	/*
+	 * CKPER is source for some peripheral clocks
+	 * (FMC-NAND / QPSI-NOR) and switching source is allowed
+	 * only if previous clock is still ON
+	 * => deactivate CKPER only after switching clock
+	 */
+	if (!ckper_disabled) {
+		return 0;
+	}
+
+	return stm32_clk_configure_mux(priv, CLK_CKPER_DISABLED);
+}
+
+static int stm32mp1_pll_configure_src(struct stm32_clk_priv *priv, int pll_idx)
+{
+	struct stm32_clk_platdata *pdata = priv->pdata;
+	struct stm32_pll_dt_cfg *pll_conf = &pdata->pll[pll_idx];
+
+	if (!pll_conf->status) {
+		return 0;
+	}
+
+	return stm32_clk_configure_mux(priv, pll_conf->src);
+}
+
+int stm32mp1_clk_init(void)
+{
+	struct stm32_clk_priv *priv = clk_stm32_get_priv();
+	struct stm32_clk_platdata *pdata = priv->pdata;
+	struct stm32_pll_dt_cfg *pll_conf = pdata->pll;
+	int ret;
+	enum stm32mp1_pll_id i;
+	bool pll3_preserve = false;
+	bool pll4_preserve = false;
+	bool pll4_bootrom = false;
+	int stgen_p = stm32mp1_clk_get_parent(STGEN_K);
+	int usbphy_p = stm32mp1_clk_get_parent(USBPHY_K);
+	uint32_t usbreg_bootrom = 0U;
+
+	if (!pll_conf[_PLL1].status) {
+		ret = clk_get_pll1_settings(pll_conf[_PLL2].src, PLL1_NOMINAL_FREQ_IN_KHZ,
+					    pll_conf[_PLL1].cfg, &pll_conf[_PLL1].frac);
+		if (ret != 0) {
+			return ret;
+		}
+
+		pll_conf[_PLL1].status = true;
+		pll_conf[_PLL1].src = pll_conf[_PLL2].src;
+	}
 
 	/*
 	 * Switch ON oscillator found in device-tree.
@@ -1882,7 +2214,7 @@ int stm32mp1_clk_init(void)
 
 		bypass = fdt_clk_read_bool(name, "st,bypass");
 		digbyp = fdt_clk_read_bool(name, "st,digbypass");
-		lse_css = fdt_clk_read_bool(name, "st,css");
+		pdata->lse_css = fdt_clk_read_bool(name, "st,css");
 		lsedrv = fdt_clk_read_uint32_default(name, "st,drive",
 						     LSEDRV_MEDIUM_HIGH);
 		stm32mp1_lse_enable(bypass, digbyp, lsedrv);
@@ -1903,36 +2235,28 @@ int stm32mp1_clk_init(void)
 	stm32mp1_csi_set(true);
 
 	/* Come back to HSI */
-	ret = stm32mp1_set_clksrc(CLK_MPU_HSI);
+	ret = stm32_clk_configure_mux(priv, CLK_MPU_HSI);
 	if (ret != 0) {
 		return ret;
 	}
-	ret = stm32mp1_set_clksrc(CLK_AXI_HSI);
+	ret = stm32_clk_configure_mux(priv, CLK_AXI_HSI);
 	if (ret != 0) {
 		return ret;
 	}
-	ret = stm32mp1_set_clksrc(CLK_MCU_HSI);
+	ret = stm32_clk_configure_mux(priv, CLK_MCU_HSI);
 	if (ret != 0) {
 		return ret;
 	}
-
-	if ((mmio_read_32(rcc_base + RCC_MP_RSTSCLRR) &
+	if ((mmio_read_32(priv->base + RCC_MP_RSTSCLRR) &
 	     RCC_MP_RSTSCLRR_MPUP0RSTF) != 0) {
-		if (pllcfg_valid[_PLL3]) {
-			pll3_preserve =
-				stm32mp1_check_pll_conf(_PLL3,
-							clksrc[CLKSRC_PLL3],
-							pllcfg[_PLL3],
-							plloff[_PLL3]);
-		}
-
-		if (pllcfg_valid[_PLL4]) {
-			pll4_preserve =
-				stm32mp1_check_pll_conf(_PLL4,
-							clksrc[CLKSRC_PLL4],
-							pllcfg[_PLL4],
-							plloff[_PLL4]);
-		}
+		pll3_preserve = stm32mp1_check_pll_conf(_PLL3,
+							pll_conf[_PLL3].src,
+							pll_conf[_PLL3].cfg,
+							pll_conf[_PLL3].frac);
+		pll4_preserve = stm32mp1_check_pll_conf(_PLL4,
+							pll_conf[_PLL4].src,
+							pll_conf[_PLL4].cfg,
+							pll_conf[_PLL4].frac);
 	}
 	/* Don't initialize PLL4, when used by BOOTROM */
 	if ((stm32mp_get_boot_itf_selected() ==
@@ -1964,58 +2288,27 @@ int stm32mp1_clk_init(void)
 		stm32mp_stgen_config(stm32mp_clk_get_rate(STGEN_K));
 	}
 
-	/* Select DIV */
-	/* No ready bit when MPUSRC != CLK_MPU_PLL1P_DIV, MPUDIV is disabled */
-	mmio_write_32(rcc_base + RCC_MPCKDIVR,
-		      clkdiv[CLKDIV_MPU] & RCC_DIVR_DIV_MASK);
-	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_AXI], rcc_base + RCC_AXIDIVR);
+	/* Configure dividers */
+	ret = stm32_clk_dividers_configure(priv);
 	if (ret != 0) {
 		return ret;
 	}
-	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_APB4], rcc_base + RCC_APB4DIVR);
-	if (ret != 0) {
-		return ret;
-	}
-	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_APB5], rcc_base + RCC_APB5DIVR);
-	if (ret != 0) {
-		return ret;
-	}
-	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_MCU], rcc_base + RCC_MCUDIVR);
-	if (ret != 0) {
-		return ret;
-	}
-	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_APB1], rcc_base + RCC_APB1DIVR);
-	if (ret != 0) {
-		return ret;
-	}
-	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_APB2], rcc_base + RCC_APB2DIVR);
-	if (ret != 0) {
-		return ret;
-	}
-	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_APB3], rcc_base + RCC_APB3DIVR);
-	if (ret != 0) {
-		return ret;
-	}
-
-	/* No ready bit for RTC */
-	mmio_write_32(rcc_base + RCC_RTCDIVR,
-		      clkdiv[CLKDIV_RTC] & RCC_DIVR_DIV_MASK);
 
 	/* Configure PLLs source */
-	ret = stm32mp1_set_clksrc(clksrc[CLKSRC_PLL12]);
+	ret = stm32mp1_pll_configure_src(priv, _PLL1);
 	if (ret != 0) {
 		return ret;
 	}
 
 	if (!pll3_preserve) {
-		ret = stm32mp1_set_clksrc(clksrc[CLKSRC_PLL3]);
+		ret = stm32mp1_pll_configure_src(priv, _PLL3);
 		if (ret != 0) {
 			return ret;
 		}
 	}
 
 	if (!pll4_preserve) {
-		ret = stm32mp1_set_clksrc(clksrc[CLKSRC_PLL4]);
+		ret = stm32mp1_pll_configure_src(priv, _PLL4);
 		if (ret != 0) {
 			return ret;
 		}
@@ -2028,34 +2321,34 @@ int stm32mp1_clk_init(void)
 			continue;
 		}
 
-		if (!pllcfg_valid[i]) {
+		if (!pll_conf[i].status) {
 			continue;
 		}
 
 		if ((i == _PLL4) && pll4_bootrom) {
 			/* Set output divider if not done by the Bootrom */
-			stm32mp1_pll_config_output(i, pllcfg[i]);
+			stm32mp1_pll_config_output(i, pll_conf[i].cfg);
 			continue;
 		}
 
-		ret = stm32mp1_pll_config(i, pllcfg[i], pllfracv[i]);
+		ret = stm32mp1_pll_config(i, pll_conf[i].cfg, pll_conf[i].frac);
 		if (ret != 0) {
 			return ret;
 		}
 
-		if (pllcsg_set[i]) {
-			stm32mp1_pll_csg(i, pllcsg[i]);
+		if (pll_conf[i].csg_enabled) {
+			stm32mp1_pll_csg(i, pll_conf[i].csg);
 		}
 
 		stm32mp1_pll_start(i);
 	}
 	/* Wait and start PLLs output when ready */
 	for (i = (enum stm32mp1_pll_id)0; i < _PLL_NB; i++) {
-		if (!pllcfg_valid[i]) {
+		if (!pll_conf[i].status) {
 			continue;
 		}
 
-		ret = stm32mp1_pll_output(i, pllcfg[i][PLLCFG_O]);
+		ret = stm32mp1_pll_output(i, pll_conf[i].cfg[PLLCFG_O]);
 		if (ret != 0) {
 			return ret;
 		}
@@ -2065,69 +2358,32 @@ int stm32mp1_clk_init(void)
 		stm32mp1_lse_wait();
 	}
 
+	if (pll4_bootrom) {
+		usbreg_bootrom = mmio_read_32(priv->base + RCC_USBCKSELR);
+	}
+
 	/* Configure with expected clock source */
-	ret = stm32mp1_set_clksrc(clksrc[CLKSRC_MPU]);
+	ret = stm32_clk_source_configure(priv);
 	if (ret != 0) {
-		return ret;
+		panic();
 	}
-	ret = stm32mp1_set_clksrc(clksrc[CLKSRC_AXI]);
-	if (ret != 0) {
-		return ret;
-	}
-	ret = stm32mp1_set_clksrc(clksrc[CLKSRC_MCU]);
-	if (ret != 0) {
-		return ret;
-	}
-	stm32mp1_set_rtcsrc(clksrc[CLKSRC_RTC], lse_css);
 
-	/* Configure PKCK */
-	pkcs_cell = fdt_rcc_read_prop("st,pkcs", &len);
-	if (pkcs_cell != NULL) {
-		bool ckper_disabled = false;
-		uint32_t j;
-		uint32_t usbreg_bootrom = 0U;
+	if (pll4_bootrom) {
+		uint32_t usbreg_value, usbreg_mask;
+		const struct stm32mp1_clk_sel *sel;
 
-		if (pll4_bootrom) {
-			usbreg_bootrom = mmio_read_32(rcc_base + RCC_USBCKSELR);
-		}
+		sel = clk_sel_ref(_USBPHY_SEL);
+		usbreg_mask = (uint32_t)sel->msk << sel->src;
+		sel = clk_sel_ref(_USBO_SEL);
+		usbreg_mask |= (uint32_t)sel->msk << sel->src;
 
-		for (j = 0; j < ((uint32_t)len / sizeof(uint32_t)); j++) {
-			uint32_t pkcs = fdt32_to_cpu(pkcs_cell[j]);
-
-			if (pkcs == (uint32_t)CLK_CKPER_DISABLED) {
-				ckper_disabled = true;
-				continue;
-			}
-			stm32mp1_pkcs_config(pkcs);
-		}
-
-		/*
-		 * CKPER is source for some peripheral clocks
-		 * (FMC-NAND / QPSI-NOR) and switching source is allowed
-		 * only if previous clock is still ON
-		 * => deactivated CKPER only after switching clock
-		 */
-		if (ckper_disabled) {
-			stm32mp1_pkcs_config(CLK_CKPER_DISABLED);
-		}
-
-		if (pll4_bootrom) {
-			uint32_t usbreg_value, usbreg_mask;
-			const struct stm32mp1_clk_sel *sel;
-
-			sel = clk_sel_ref(_USBPHY_SEL);
-			usbreg_mask = (uint32_t)sel->msk << sel->src;
-			sel = clk_sel_ref(_USBO_SEL);
-			usbreg_mask |= (uint32_t)sel->msk << sel->src;
-
-			usbreg_value = mmio_read_32(rcc_base + RCC_USBCKSELR) &
-				       usbreg_mask;
-			usbreg_bootrom &= usbreg_mask;
-			if (usbreg_bootrom != usbreg_value) {
-				VERBOSE("forbidden new USB clk path\n");
-				VERBOSE("vs bootrom on USB boot\n");
-				return -FDT_ERR_BADVALUE;
-			}
+		usbreg_value = mmio_read_32(priv->base + RCC_USBCKSELR) &
+			       usbreg_mask;
+		usbreg_bootrom &= usbreg_mask;
+		if (usbreg_bootrom != usbreg_value) {
+			EARLY_ERROR("forbidden new USB clk path\n");
+			EARLY_ERROR("vs bootrom on USB boot\n");
+			return -FDT_ERR_BADVALUE;
 		}
 	}
 
@@ -2139,7 +2395,7 @@ int stm32mp1_clk_init(void)
 	stm32mp_stgen_config(stm32mp_clk_get_rate(STGEN_K));
 
 	/* Software Self-Refresh mode (SSR) during DDR initilialization */
-	mmio_clrsetbits_32(rcc_base + RCC_DDRITFCR,
+	mmio_clrsetbits_32(priv->base + RCC_DDRITFCR,
 			   RCC_DDRITFCR_DDRCKMOD_MASK,
 			   RCC_DDRITFCR_DDRCKMOD_SSR <<
 			   RCC_DDRITFCR_DDRCKMOD_SHIFT);
@@ -2366,8 +2622,199 @@ static const struct clk_ops stm32mp_clk_ops = {
 	.get_parent	= stm32mp1_clk_get_parent,
 };
 
+struct stm32_pll_dt_cfg mp15_pll[_PLL_NB];
+uint32_t mp15_clksrc[MUX_NB];
+uint32_t mp15_clkdiv[DIV_NB];
+
+struct stm32_clk_platdata stm32mp15_clock_pdata = {
+	.pll		= mp15_pll,
+	.npll		= _PLL_NB,
+	.clksrc		= mp15_clksrc,
+	.nclksrc	= MUX_NB,
+	.clkdiv		= mp15_clkdiv,
+	.nclkdiv	= DIV_NB,
+};
+
+static struct stm32_clk_priv stm32mp15_clock_data = {
+	.base		= RCC_BASE,
+	.parents	= parent_mp15,
+	.nb_parents	= ARRAY_SIZE(parent_mp15),
+	.div		= dividers_mp15,
+	.nb_div		= ARRAY_SIZE(dividers_mp15),
+	.pdata		= &stm32mp15_clock_pdata,
+};
+
+static int stm32_clk_parse_fdt_by_name(void *fdt, int node, const char *name,
+				       uint32_t *tab, uint32_t *nb)
+{
+	const fdt32_t *cell;
+	int len = 0;
+	uint32_t i;
+
+	cell = fdt_getprop(fdt, node, name, &len);
+	if (cell == NULL) {
+		*nb = 0U;
+		return 0;
+	}
+
+	for (i = 0U; i < ((uint32_t)len / sizeof(uint32_t)); i++) {
+		tab[i] = fdt32_to_cpu(cell[i]);
+	}
+
+	*nb = (uint32_t)len / sizeof(uint32_t);
+
+	return 0;
+}
+
+#define RCC_PLL_NAME_SIZE 12
+
+static int clk_stm32_load_vco_config(void *fdt, int subnode, struct stm32_pll_dt_cfg *pll)
+{
+	int err;
+
+	err = fdt_read_uint32_array(fdt, subnode, "divmn", (int)PLL_DIV_MN_NB, &pll->cfg[PLLCFG_M]);
+	if (err != 0) {
+		return err;
+	}
+
+	err = fdt_read_uint32_array(fdt, subnode, "csg", (int)PLLCSG_NB, pll->csg);
+	if (err == 0) {
+		pll->csg_enabled = true;
+	} else if (err == -FDT_ERR_NOTFOUND) {
+		pll->csg_enabled = false;
+	} else {
+		return err;
+	}
+
+	pll->status = true;
+
+	pll->frac = fdt_read_uint32_default(fdt, subnode, "frac", 0);
+
+	pll->src = fdt_read_uint32_default(fdt, subnode, "src", UINT32_MAX);
+
+	return 0;
+}
+
+static int clk_stm32_load_output_config(void *fdt, int subnode, struct stm32_pll_dt_cfg *pll)
+{
+	int err;
+
+	err = fdt_read_uint32_array(fdt, subnode, "st,pll_div_pqr", (int)PLL_DIV_PQR_NB,
+				    &pll->cfg[PLLCFG_P]);
+	if (err != 0) {
+		return err;
+	}
+
+	pll->cfg[PLLCFG_O] = PQR(1, 1, 1);
+
+	return 0;
+}
+
+static int clk_stm32_parse_pll_fdt(void *fdt, int subnode, struct stm32_pll_dt_cfg *pll)
+{
+	const fdt32_t *cuint;
+	int subnode_pll;
+	int subnode_vco;
+	int err;
+
+	cuint = fdt_getprop(fdt, subnode, "st,pll", NULL);
+	if (cuint == NULL) {
+		/* Case of no pll is defined */
+		return 0;
+	}
+
+	subnode_pll = fdt_node_offset_by_phandle(fdt, fdt32_to_cpu(*cuint));
+	if (subnode_pll < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	cuint = fdt_getprop(fdt, subnode_pll, "st,pll_vco", NULL);
+	if (cuint == NULL) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	subnode_vco = fdt_node_offset_by_phandle(fdt, fdt32_to_cpu(*cuint));
+	if (subnode_vco < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	err = clk_stm32_load_vco_config(fdt, subnode_vco, pll);
+	if (err != 0) {
+		return err;
+	}
+
+	err = clk_stm32_load_output_config(fdt, subnode_pll, pll);
+	if (err != 0) {
+		return err;
+	}
+
+	return 0;
+}
+
+static int stm32_clk_parse_fdt_all_pll(void *fdt, int node, struct stm32_clk_platdata *pdata)
+{
+	size_t i = 0U;
+
+	for (i = _PLL1; i < pdata->npll; i++) {
+		struct stm32_pll_dt_cfg *pll = pdata->pll + i;
+		char name[RCC_PLL_NAME_SIZE];
+		int subnode;
+		int err;
+
+		snprintf(name, sizeof(name), "st,pll@%u", i);
+
+		subnode = fdt_subnode_offset(fdt, node, name);
+		if (!fdt_check_node(subnode)) {
+			continue;
+		}
+
+		err = clk_stm32_parse_pll_fdt(fdt, subnode, pll);
+		if (err != 0) {
+			panic();
+		}
+	}
+
+	return 0;
+}
+
+static int stm32_clk_parse_fdt(struct stm32_clk_platdata *pdata)
+{
+	void *fdt = NULL;
+	int node;
+	uint32_t err;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	node = fdt_node_offset_by_compatible(fdt, -1, DT_RCC_CLK_COMPAT);
+	if (node < 0) {
+		panic();
+	}
+
+	err = stm32_clk_parse_fdt_all_pll(fdt, node, pdata);
+	if (err != 0) {
+		return err;
+	}
+
+	err = stm32_clk_parse_fdt_by_name(fdt, node, "st,clkdiv", pdata->clkdiv, &pdata->nclkdiv);
+	if (err != 0) {
+		return err;
+	}
+
+	err = stm32_clk_parse_fdt_by_name(fdt, node, "st,clksrc", pdata->clksrc, &pdata->nclksrc);
+	if (err != 0) {
+		return err;
+	}
+
+	return 0;
+}
+
 int stm32mp1_clk_probe(void)
 {
+	uintptr_t base = RCC_BASE;
+	int ret;
+
 #if defined(IMAGE_BL32)
 	if (!fdt_get_rcc_secure_state()) {
 		mmio_write_32(stm32mp_rcc_base() + RCC_TZCR, 0U);
@@ -2376,6 +2823,16 @@ int stm32mp1_clk_probe(void)
 
 	stm32mp1_osc_init();
 
+	ret = stm32_clk_parse_fdt(&stm32mp15_clock_pdata);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = clk_stm32_init(&stm32mp15_clock_data, base);
+	if (ret != 0) {
+		return ret;
+	}
+
 	sync_earlyboot_clocks_state();
 
 	clk_register(&stm32mp_clk_ops);
diff --git a/drivers/st/clk/stm32mp_clkfunc.c b/drivers/st/clk/stm32mp_clkfunc.c
index 379547fd1..69922fd2d 100644
--- a/drivers/st/clk/stm32mp_clkfunc.c
+++ b/drivers/st/clk/stm32mp_clkfunc.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2023, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2024, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -334,6 +334,19 @@ static void stgen_set_counter(unsigned long long counter)
 #endif
 }
 
+/*******************************************************************************
+ * This function returns the STGEN counter value.
+ ******************************************************************************/
+static unsigned long long stm32mp_stgen_get_counter(void)
+{
+#ifdef __aarch64__
+	return mmio_read_64(STGEN_BASE + CNTCV_OFF);
+#else
+	return (((unsigned long long)mmio_read_32(STGEN_BASE + CNTCVU_OFF) << 32) |
+		mmio_read_32(STGEN_BASE + CNTCVL_OFF));
+#endif
+}
+
 /*******************************************************************************
  * This function configures and restores the STGEN counter depending on the
  * connected clock.
@@ -350,9 +363,11 @@ void stm32mp_stgen_config(unsigned long rate)
 	}
 
 	mmio_clrbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN);
-	counter = stm32mp_stgen_get_counter() * rate / cntfid0;
 
-	stgen_set_counter(counter);
+	if (cntfid0 != 0U) {
+		counter = stm32mp_stgen_get_counter() * rate / cntfid0;
+		stgen_set_counter(counter);
+	}
 	mmio_write_32(STGEN_BASE + CNTFID_OFF, rate);
 	mmio_setbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN);
 
@@ -363,32 +378,13 @@ void stm32mp_stgen_config(unsigned long rate)
 }
 
 /*******************************************************************************
- * This function returns the STGEN counter value.
+ * This function restores CPU generic timer rate from the STGEN clock rate.
  ******************************************************************************/
-unsigned long long stm32mp_stgen_get_counter(void)
+void stm32mp_stgen_restore_rate(void)
 {
-#ifdef __aarch64__
-	return mmio_read_64(STGEN_BASE + CNTCV_OFF);
-#else
-	return (((unsigned long long)mmio_read_32(STGEN_BASE + CNTCVU_OFF) << 32) |
-		mmio_read_32(STGEN_BASE + CNTCVL_OFF));
-#endif
-}
-
-/*******************************************************************************
- * This function restores the STGEN counter value.
- * It takes a first input value as a counter backup value to be restored and a
- * offset in ms to be added.
- ******************************************************************************/
-void stm32mp_stgen_restore_counter(unsigned long long value,
-				   unsigned long long offset_in_ms)
-{
-	unsigned long long cnt;
-
-	cnt = value + ((offset_in_ms *
-			mmio_read_32(STGEN_BASE + CNTFID_OFF)) / 1000U);
-
-	mmio_clrbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN);
-	stgen_set_counter(cnt);
-	mmio_setbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN);
+	unsigned long rate;
+
+	rate = mmio_read_32(STGEN_BASE + CNTFID_OFF);
+
+	write_cntfrq_el0((u_register_t)rate);
 }
diff --git a/fdts/stm32mp135f-dk.dts b/fdts/stm32mp135f-dk.dts
index 7a7d461b7..08fbbb9a5 100644
--- a/fdts/stm32mp135f-dk.dts
+++ b/fdts/stm32mp135f-dk.dts
@@ -190,7 +190,6 @@
 		CLK_AXI_PLL2P
 		CLK_MLAHBS_PLL3
 		CLK_CKPER_HSE
-		CLK_RTC_LSE
 		CLK_SDMMC1_PLL4P
 		CLK_SDMMC2_PLL4P
 		CLK_STGEN_HSE
@@ -212,16 +211,9 @@
 		DIV(DIV_APB4, 1)
 		DIV(DIV_APB5, 2)
 		DIV(DIV_APB6, 1)
-		DIV(DIV_RTC, 0)
 	>;
 
 	st,pll_vco {
-		pll1_vco_1300Mhz: pll1-vco-1300Mhz {
-			src = < CLK_PLL12_HSE >;
-			divmn = < 2 80 >;
-			frac = < 0x800 >;
-		};
-
 		pll2_vco_1066Mhz: pll2-vco-1066Mhz {
 			src = <CLK_PLL12_HSE>;
 			divmn = <2 65>;
@@ -240,19 +232,6 @@
 		};
 	};
 
-	/* VCO = 1300.0 MHz => P = 650 (CPU) */
-	pll1:st,pll@0 {
-		compatible = "st,stm32mp1-pll";
-		reg = <0>;
-
-		st,pll = < &pll1_cfg1 >;
-
-		pll1_cfg1: pll1_cfg1 {
-			st,pll_vco = < &pll1_vco_1300Mhz >;
-			st,pll_div_pqr = < 0 1 1 >;
-		};
-	};
-
 	/* VCO = 1066.0 MHz => P = 266 (AXI), Q = 266, R = 533 (DDR) */
 	pll2:st,pll@1 {
 		compatible = "st,stm32mp1-pll";
diff --git a/fdts/stm32mp151a-prtt1a.dts b/fdts/stm32mp151a-prtt1a.dts
index 363462081..9742dcb63 100644
--- a/fdts/stm32mp151a-prtt1a.dts
+++ b/fdts/stm32mp151a-prtt1a.dts
@@ -1,6 +1,7 @@
 // SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
 /*
  * Copyright (C) 2023, Protonic Holland - All Rights Reserved
+ * Copyright (C) 2024, STMicroelectronics - All Rights Reserved
  * Author: David Jander <david@protonic.nl>
  */
 /dts-v1/;
@@ -123,7 +124,7 @@
 		CLK_SAI2_PLL3Q
 		CLK_SAI3_PLL3Q
 		CLK_SAI4_PLL3Q
-		CLK_RNG1_LSI
+		CLK_RNG1_CSI
 		CLK_RNG2_LSI
 		CLK_LPTIM1_PCLK1
 		CLK_LPTIM23_PCLK3
diff --git a/fdts/stm32mp157a-avenger96.dts b/fdts/stm32mp157a-avenger96.dts
index f0da350bb..7135970c0 100644
--- a/fdts/stm32mp157a-avenger96.dts
+++ b/fdts/stm32mp157a-avenger96.dts
@@ -175,29 +175,9 @@
 		CLK_MPU_PLL1P
 		CLK_AXI_PLL2P
 		CLK_MCU_PLL3P
-		CLK_PLL12_HSE
-		CLK_PLL3_HSE
-		CLK_PLL4_HSE
 		CLK_RTC_LSE
 		CLK_MCO1_DISABLED
 		CLK_MCO2_DISABLED
-	>;
-
-	st,clkdiv = <
-		1 /*MPU*/
-		0 /*AXI*/
-		0 /*MCU*/
-		1 /*APB1*/
-		1 /*APB2*/
-		1 /*APB3*/
-		1 /*APB4*/
-		2 /*APB5*/
-		23 /*RTC*/
-		0 /*MCO1*/
-		0 /*MCO2*/
-	>;
-
-	st,pkcs = <
 		CLK_CKPER_HSE
 		CLK_FMC_ACLK
 		CLK_QSPI_ACLK
@@ -228,42 +208,83 @@
 		CLK_SAI2_PLL3Q
 		CLK_SAI3_PLL3Q
 		CLK_SAI4_PLL3Q
-		CLK_RNG1_LSI
+		CLK_RNG1_CSI
 		CLK_RNG2_LSI
 		CLK_LPTIM1_PCLK1
 		CLK_LPTIM23_PCLK3
 		CLK_LPTIM45_LSE
 	>;
 
-	/* VCO = 1300.0 MHz => P = 650 (CPU) */
-	pll1: st,pll@0 {
-		compatible = "st,stm32mp1-pll";
-		reg = <0>;
-		cfg = <2 80 0 0 0 PQR(1,0,0)>;
-		frac = <0x800>;
+	st,clkdiv = <
+		DIV(DIV_MPU, 1)
+		DIV(DIV_AXI, 0)
+		DIV(DIV_MCU, 0)
+		DIV(DIV_APB1, 1)
+		DIV(DIV_APB2, 1)
+		DIV(DIV_APB3, 1)
+		DIV(DIV_APB4, 1)
+		DIV(DIV_APB5, 2)
+		DIV(DIV_RTC, 23)
+		DIV(DIV_MCO1, 0)
+		DIV(DIV_MCO2, 0)
+	>;
+
+	st,pll_vco {
+		pll2_vco_1066Mhz: pll2-vco-1066Mhz {
+			src = <CLK_PLL12_HSE>;
+			divmn = <2 65>;
+			frac = <0x1400>;
+		};
+
+		pll3_vco_417Mhz: pll3-vco-417Mhz {
+			src = <CLK_PLL3_HSE>;
+			divmn = <1 33>;
+			frac = <0x1a04>;
+		};
+
+		pll4_vco_480Mhz: pll4-vco-480Mhz {
+			src = <CLK_PLL4_HSE>;
+			divmn = <1 39>;
+		};
 	};
 
 	/* VCO = 1066.0 MHz => P = 266 (AXI), Q = 533 (GPU), R = 533 (DDR) */
 	pll2: st,pll@1 {
 		compatible = "st,stm32mp1-pll";
 		reg = <1>;
-		cfg = <2 65 1 0 0 PQR(1,1,1)>;
-		frac = <0x1400>;
+
+		st,pll = <&pll2_cfg1>;
+
+		pll2_cfg1: pll2_cfg1 {
+			st,pll_vco = <&pll2_vco_1066Mhz>;
+			st,pll_div_pqr = <1 0 0>;
+		};
 	};
 
 	/* VCO = 417.8 MHz => P = 209, Q = 24, R = 11 */
 	pll3: st,pll@2 {
 		compatible = "st,stm32mp1-pll";
 		reg = <2>;
-		cfg = <1 33 1 16 36 PQR(1,1,1)>;
-		frac = <0x1a04>;
+
+		st,pll = <&pll3_cfg1>;
+
+		pll3_cfg1: pll3_cfg1 {
+			st,pll_vco = <&pll3_vco_417Mhz>;
+			st,pll_div_pqr = <1 16 36>;
+		};
 	};
 
 	/* VCO = 480.0 MHz => P = 120, Q = 40, R = 96 */
 	pll4: st,pll@3 {
 		compatible = "st,stm32mp1-pll";
 		reg = <3>;
-		cfg = <1 39 3 11 4 PQR(1,1,1)>;
+
+		st,pll = <&pll4_cfg1>;
+
+		pll4_cfg1: pll4_cfg1 {
+			st,pll_vco = <&pll4_vco_480Mhz>;
+			st,pll_div_pqr = <3 11 4>;
+		};
 	};
 };
 
diff --git a/fdts/stm32mp157c-ed1.dts b/fdts/stm32mp157c-ed1.dts
index d7bcc844c..d85221b03 100644
--- a/fdts/stm32mp157c-ed1.dts
+++ b/fdts/stm32mp157c-ed1.dts
@@ -194,29 +194,8 @@
 		CLK_MPU_PLL1P
 		CLK_AXI_PLL2P
 		CLK_MCU_PLL3P
-		CLK_PLL12_HSE
-		CLK_PLL3_HSE
-		CLK_PLL4_HSE
-		CLK_RTC_LSE
 		CLK_MCO1_DISABLED
 		CLK_MCO2_DISABLED
-	>;
-
-	st,clkdiv = <
-		1 /*MPU*/
-		0 /*AXI*/
-		0 /*MCU*/
-		1 /*APB1*/
-		1 /*APB2*/
-		1 /*APB3*/
-		1 /*APB4*/
-		2 /*APB5*/
-		23 /*RTC*/
-		0 /*MCO1*/
-		0 /*MCO2*/
-	>;
-
-	st,pkcs = <
 		CLK_CKPER_HSE
 		CLK_FMC_ACLK
 		CLK_QSPI_ACLK
@@ -247,42 +226,82 @@
 		CLK_SAI2_PLL3Q
 		CLK_SAI3_PLL3Q
 		CLK_SAI4_PLL3Q
-		CLK_RNG1_LSI
+		CLK_RNG1_CSI
 		CLK_RNG2_LSI
 		CLK_LPTIM1_PCLK1
 		CLK_LPTIM23_PCLK3
 		CLK_LPTIM45_LSE
 	>;
 
-	/* VCO = 1300.0 MHz => P = 650 (CPU) */
-	pll1: st,pll@0 {
-		compatible = "st,stm32mp1-pll";
-		reg = <0>;
-		cfg = <2 80 0 0 0 PQR(1,0,0)>;
-		frac = <0x800>;
+	st,clkdiv = <
+		DIV(DIV_MPU, 1)
+		DIV(DIV_AXI, 0)
+		DIV(DIV_MCU, 0)
+		DIV(DIV_APB1, 1)
+		DIV(DIV_APB2, 1)
+		DIV(DIV_APB3, 1)
+		DIV(DIV_APB4, 1)
+		DIV(DIV_APB5, 2)
+		DIV(DIV_MCO1, 0)
+		DIV(DIV_MCO2, 0)
+	>;
+
+	st,pll_vco {
+		pll2_vco_1066Mhz: pll2-vco-1066Mhz {
+			src = <CLK_PLL12_HSE>;
+			divmn = <2 65>;
+			frac = <0x1400>;
+		};
+
+		pll3_vco_417Mhz: pll3-vco-417Mhz {
+			src = <CLK_PLL3_HSE>;
+			divmn = <1 33>;
+			frac = <0x1a04>;
+		};
+
+		pll4_vco_594Mhz: pll4-vco-594Mhz {
+			src = <CLK_PLL4_HSE>;
+			divmn = <3 98>;
+		};
 	};
 
 	/* VCO = 1066.0 MHz => P = 266 (AXI), Q = 533 (GPU), R = 533 (DDR) */
 	pll2: st,pll@1 {
 		compatible = "st,stm32mp1-pll";
 		reg = <1>;
-		cfg = <2 65 1 0 0 PQR(1,1,1)>;
-		frac = <0x1400>;
+
+		st,pll = <&pll2_cfg1>;
+
+		pll2_cfg1: pll2_cfg1 {
+			st,pll_vco = <&pll2_vco_1066Mhz>;
+			st,pll_div_pqr = <1 0 0>;
+		};
 	};
 
 	/* VCO = 417.8 MHz => P = 209, Q = 24, R = 11 */
 	pll3: st,pll@2 {
 		compatible = "st,stm32mp1-pll";
 		reg = <2>;
-		cfg = <1 33 1 16 36 PQR(1,1,1)>;
-		frac = <0x1a04>;
+
+		st,pll = <&pll3_cfg1>;
+
+		pll3_cfg1: pll3_cfg1 {
+			st,pll_vco = <&pll3_vco_417Mhz>;
+			st,pll_div_pqr = <1 16 36>;
+		};
 	};
 
 	/* VCO = 594.0 MHz => P = 99, Q = 74, R = 74 */
 	pll4: st,pll@3 {
 		compatible = "st,stm32mp1-pll";
 		reg = <3>;
-		cfg = <3 98 5 7 7 PQR(1,1,1)>;
+
+		st,pll = <&pll4_cfg1>;
+
+		pll4_cfg1: pll4_cfg1 {
+			st,pll_vco = <&pll4_vco_594Mhz>;
+			st,pll_div_pqr = <5 7 7>;
+		};
 	};
 };
 
diff --git a/fdts/stm32mp157c-odyssey-som.dtsi b/fdts/stm32mp157c-odyssey-som.dtsi
index a0be7183c..e5d41fc79 100644
--- a/fdts/stm32mp157c-odyssey-som.dtsi
+++ b/fdts/stm32mp157c-odyssey-som.dtsi
@@ -207,29 +207,9 @@
 		CLK_MPU_PLL1P
 		CLK_AXI_PLL2P
 		CLK_MCU_PLL3P
-		CLK_PLL12_HSE
-		CLK_PLL3_HSE
-		CLK_PLL4_HSE
 		CLK_RTC_LSE
 		CLK_MCO1_DISABLED
 		CLK_MCO2_DISABLED
-	>;
-
-	st,clkdiv = <
-		1 /*MPU*/
-		0 /*AXI*/
-		0 /*MCU*/
-		1 /*APB1*/
-		1 /*APB2*/
-		1 /*APB3*/
-		1 /*APB4*/
-		2 /*APB5*/
-		23 /*RTC*/
-		0 /*MCO1*/
-		0 /*MCO2*/
-	>;
-
-	st,pkcs = <
 		CLK_CKPER_HSE
 		CLK_FMC_ACLK
 		CLK_QSPI_ACLK
@@ -260,42 +240,83 @@
 		CLK_SAI2_PLL3Q
 		CLK_SAI3_PLL3Q
 		CLK_SAI4_PLL3Q
-		CLK_RNG1_LSI
+		CLK_RNG1_CSI
 		CLK_RNG2_LSI
 		CLK_LPTIM1_PCLK1
 		CLK_LPTIM23_PCLK3
 		CLK_LPTIM45_LSE
 	>;
 
-	/* VCO = 1300.0 MHz => P = 650 (CPU) */
-	pll1: st,pll@0 {
-		compatible = "st,stm32mp1-pll";
-		reg = <0>;
-		cfg = <2 80 0 0 0 PQR(1,0,0)>;
-		frac = <0x800>;
+	st,clkdiv = <
+		DIV(DIV_MPU, 1)
+		DIV(DIV_AXI, 0)
+		DIV(DIV_MCU, 0)
+		DIV(DIV_APB1, 1)
+		DIV(DIV_APB2, 1)
+		DIV(DIV_APB3, 1)
+		DIV(DIV_APB4, 1)
+		DIV(DIV_APB5, 2)
+		DIV(DIV_RTC, 23)
+		DIV(DIV_MCO1, 0)
+		DIV(DIV_MCO2, 0)
+	>;
+
+	st,pll_vco {
+		pll2_vco_1066Mhz: pll2-vco-1066Mhz {
+			src = <CLK_PLL12_HSE>;
+			divmn = <2 65>;
+			frac = <0x1400>;
+		};
+
+		pll3_vco_417Mhz: pll3-vco-417Mhz {
+			src = <CLK_PLL3_HSE>;
+			divmn = <1 33>;
+			frac = <0x1a04>;
+		};
+
+		pll4_vco_594Mhz: pll4-vco-594Mhz {
+			src = <CLK_PLL4_HSE>;
+			divmn = <3 98>;
+		};
 	};
 
 	/* VCO = 1066.0 MHz => P = 266 (AXI), Q = 533 (GPU), R = 533 (DDR) */
 	pll2: st,pll@1 {
 		compatible = "st,stm32mp1-pll";
 		reg = <1>;
-		cfg = <2 65 1 0 0 PQR(1,1,1)>;
-		frac = <0x1400>;
+
+		st,pll = <&pll2_cfg1>;
+
+		pll2_cfg1: pll2_cfg1 {
+			st,pll_vco = <&pll2_vco_1066Mhz>;
+			st,pll_div_pqr = <1 0 0>;
+		};
 	};
 
 	/* VCO = 417.8 MHz => P = 209, Q = 24, R = 11 */
 	pll3: st,pll@2 {
 		compatible = "st,stm32mp1-pll";
 		reg = <2>;
-		cfg = <1 33 1 16 36 PQR(1,1,1)>;
-		frac = <0x1a04>;
+
+		st,pll = <&pll3_cfg1>;
+
+		pll3_cfg1: pll3_cfg1 {
+			st,pll_vco = <&pll3_vco_417Mhz>;
+			st,pll_div_pqr = <1 16 36>;
+		};
 	};
 
 	/* VCO = 594.0 MHz => P = 99, Q = 74, R = 74 */
 	pll4: st,pll@3 {
 		compatible = "st,stm32mp1-pll";
 		reg = <3>;
-		cfg = <3 98 5 7 7 PQR(1,1,1)>;
+
+		st,pll = <&pll4_cfg1>;
+
+		pll4_cfg1: pll4_cfg1 {
+			st,pll_vco = <&pll4_vco_594Mhz>;
+			st,pll_div_pqr = <5 7 7>;
+		};
 	};
 };
 
diff --git a/fdts/stm32mp15xx-dhcom-som.dtsi b/fdts/stm32mp15xx-dhcom-som.dtsi
index 513886874..12846db8b 100644
--- a/fdts/stm32mp15xx-dhcom-som.dtsi
+++ b/fdts/stm32mp15xx-dhcom-som.dtsi
@@ -193,29 +193,9 @@
 		CLK_MPU_PLL1P
 		CLK_AXI_PLL2P
 		CLK_MCU_PLL3P
-		CLK_PLL12_HSE
-		CLK_PLL3_HSE
-		CLK_PLL4_HSE
 		CLK_RTC_LSE
 		CLK_MCO1_DISABLED
 		CLK_MCO2_PLL4P
-	>;
-
-	st,clkdiv = <
-		1 /*MPU*/
-		0 /*AXI*/
-		0 /*MCU*/
-		1 /*APB1*/
-		1 /*APB2*/
-		1 /*APB3*/
-		1 /*APB4*/
-		2 /*APB5*/
-		23 /*RTC*/
-		0 /*MCO1*/
-		1 /*MCO2*/
-	>;
-
-	st,pkcs = <
 		CLK_CKPER_HSE
 		CLK_FMC_ACLK
 		CLK_QSPI_ACLK
@@ -246,42 +226,83 @@
 		CLK_SAI2_PLL3Q
 		CLK_SAI3_PLL3Q
 		CLK_SAI4_PLL3Q
-		CLK_RNG1_LSI
+		CLK_RNG1_CSI
 		CLK_RNG2_LSI
 		CLK_LPTIM1_PCLK1
 		CLK_LPTIM23_PCLK3
 		CLK_LPTIM45_LSE
 	>;
 
-	/* VCO = 1300.0 MHz => P = 650 (CPU) */
-	pll1: st,pll@0 {
-		compatible = "st,stm32mp1-pll";
-		reg = <0>;
-		cfg = <2 80 0 0 0 PQR(1,0,0)>;
-		frac = <0x800>;
+	st,clkdiv = <
+		DIV(DIV_MPU, 1)
+		DIV(DIV_AXI, 0)
+		DIV(DIV_MCU, 0)
+		DIV(DIV_APB1, 1)
+		DIV(DIV_APB2, 1)
+		DIV(DIV_APB3, 1)
+		DIV(DIV_APB4, 1)
+		DIV(DIV_APB5, 2)
+		DIV(DIV_RTC, 23)
+		DIV(DIV_MCO1, 0)
+		DIV(DIV_MCO2, 1)
+	>;
+
+	st,pll_vco {
+		pll2_vco_1066Mhz: pll2-vco-1066Mhz {
+			src = <CLK_PLL12_HSE>;
+			divmn = <2 65>;
+			frac = <0x1400>;
+		};
+
+		pll3_vco_417Mhz: pll3-vco-417Mhz {
+			src = <CLK_PLL3_HSE>;
+			divmn = <1 33>;
+			frac = <0x1a04>;
+		};
+
+		pll4_vco_600Mhz: pll4-vco-600hz {
+			src = <CLK_PLL4_HSE>;
+			divmn = <1 49>;
+		};
 	};
 
 	/* VCO = 1066.0 MHz => P = 266 (AXI), Q = 533 (GPU), R = 533 (DDR) */
 	pll2: st,pll@1 {
 		compatible = "st,stm32mp1-pll";
 		reg = <1>;
-		cfg = <2 65 1 0 0 PQR(1,1,1)>;
-		frac = <0x1400>;
+
+		st,pll = <&pll2_cfg1>;
+
+		pll2_cfg1: pll2_cfg1 {
+			st,pll_vco = <&pll2_vco_1066Mhz>;
+			st,pll_div_pqr = <1 0 0>;
+		};
 	};
 
 	/* VCO = 417.8 MHz => P = 209, Q = 24, R = 11 */
 	pll3: st,pll@2 {
 		compatible = "st,stm32mp1-pll";
 		reg = <2>;
-		cfg = <1 33 1 16 36 PQR(1,1,1)>;
-		frac = <0x1a04>;
+
+		st,pll = <&pll3_cfg1>;
+
+		pll3_cfg1: pll3_cfg1 {
+			st,pll_vco = <&pll3_vco_417Mhz>;
+			st,pll_div_pqr = <1 16 36>;
+		};
 	};
 
 	/* VCO = 600.0 MHz => P = 50, Q = 50, R = 50 */
 	pll4: st,pll@3 {
 		compatible = "st,stm32mp1-pll";
 		reg = <3>;
-		cfg = <1 49 5 11 11 PQR(1,1,1)>;
+
+		st,pll = <&pll4_cfg1>;
+
+		pll4_cfg1: pll4_cfg1 {
+			st,pll_vco = <&pll4_vco_600Mhz>;
+			st,pll_div_pqr = <5 11 11>;
+		};
 	};
 };
 
diff --git a/fdts/stm32mp15xx-dhcor-som.dtsi b/fdts/stm32mp15xx-dhcor-som.dtsi
index 8d829a416..2ebfb2de8 100644
--- a/fdts/stm32mp15xx-dhcor-som.dtsi
+++ b/fdts/stm32mp15xx-dhcor-som.dtsi
@@ -4,7 +4,7 @@
  * Author: Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>
  * Copyright (C) 2020 Marek Vasut <marex@denx.de>
  * Copyright (C) 2022 DH electronics GmbH
- * Copyright (C) 2023, STMicroelectronics - All Rights Reserved
+ * Copyright (C) 2023-2024, STMicroelectronics - All Rights Reserved
  */
 
 #include "stm32mp15-pinctrl.dtsi"
@@ -188,29 +188,9 @@
 		CLK_MPU_PLL1P
 		CLK_AXI_PLL2P
 		CLK_MCU_PLL3P
-		CLK_PLL12_HSE
-		CLK_PLL3_HSE
-		CLK_PLL4_HSE
 		CLK_RTC_LSE
 		CLK_MCO1_DISABLED
 		CLK_MCO2_DISABLED
-	>;
-
-	st,clkdiv = <
-		1 /*MPU*/
-		0 /*AXI*/
-		0 /*MCU*/
-		1 /*APB1*/
-		1 /*APB2*/
-		1 /*APB3*/
-		1 /*APB4*/
-		2 /*APB5*/
-		23 /*RTC*/
-		0 /*MCO1*/
-		0 /*MCO2*/
-	>;
-
-	st,pkcs = <
 		CLK_CKPER_HSE
 		CLK_FMC_ACLK
 		CLK_QSPI_ACLK
@@ -241,42 +221,84 @@
 		CLK_SAI2_PLL3Q
 		CLK_SAI3_PLL3Q
 		CLK_SAI4_PLL3Q
-		CLK_RNG1_LSI
+		CLK_RNG1_CSI
 		CLK_RNG2_LSI
 		CLK_LPTIM1_PCLK1
 		CLK_LPTIM23_PCLK3
 		CLK_LPTIM45_LSE
 	>;
 
-	/* VCO = 1300.0 MHz => P = 650 (CPU) */
-	pll1: st,pll@0 {
-		compatible = "st,stm32mp1-pll";
-		reg = <0>;
-		cfg = <2 80 0 0 0 PQR(1,0,0)>;
-		frac = <0x800>;
+	st,clkdiv = <
+		DIV(DIV_MPU, 1)
+		DIV(DIV_AXI, 0)
+		DIV(DIV_MCU, 0)
+		DIV(DIV_APB1, 1)
+		DIV(DIV_APB2, 1)
+		DIV(DIV_APB3, 1)
+		DIV(DIV_APB4, 1)
+		DIV(DIV_APB5, 2)
+		DIV(DIV_RTC, 23)
+		DIV(DIV_MCO1, 0)
+		DIV(DIV_MCO2, 0)
+	>;
+
+	st,pll_vco {
+		pll2_vco_1066Mhz: pll2-vco-1066Mhz {
+			src = <CLK_PLL12_HSE>;
+			divmn = <2 65>;
+			frac = <0x1400>;
+		};
+
+		pll3_vco_417Mhz: pll3-vco-417Mhz {
+			src = <CLK_PLL3_HSE>;
+			divmn = <1 33>;
+			frac = <0x1a04>;
+		};
+
+		pll4_vco_594Mhz: pll4-vco-594Mhz {
+			src = <CLK_PLL4_HSE>;
+			divmn = <3 98>;
+		};
 	};
 
 	/* VCO = 1066.0 MHz => P = 266 (AXI), Q = 533 (GPU), R = 533 (DDR) */
 	pll2: st,pll@1 {
 		compatible = "st,stm32mp1-pll";
 		reg = <1>;
-		cfg = <2 65 1 0 0 PQR(1,1,1)>;
-		frac = <0x1400>;
+
+		st,pll = <&pll2_cfg1>;
+
+		pll2_cfg1: pll2_cfg1 {
+			st,pll_vco = <&pll2_vco_1066Mhz>;
+			st,pll_div_pqr = <1 0 0>;
+		};
 	};
 
 	/* VCO = 417.8 MHz => P = 209, Q = 24, R = 11 */
 	pll3: st,pll@2 {
 		compatible = "st,stm32mp1-pll";
 		reg = <2>;
-		cfg = <1 33 1 16 36 PQR(1,1,1)>;
-		frac = <0x1a04>;
+
+		st,pll = <&pll3_cfg1>;
+
+		pll3_cfg1: pll3_cfg1 {
+			st,pll_vco = <&pll3_vco_417Mhz>;
+			st,pll_div_pqr = <1 16 36>;
+		};
 	};
 
-	/* VCO = 600.0 MHz => P = 99, Q = 74, R = 99 */
+	/* VCO = 600.0 MHz => P = 99, Q = 74, R = 99 */ /* @TOCHECK */
+	/* VCO = 594.0 MHz => P = 99, Q = 74, R = 74 */
 	pll4: st,pll@3 {
 		compatible = "st,stm32mp1-pll";
 		reg = <3>;
-		cfg = <3 98 5 7 5 PQR(1,1,1)>;
+
+		st,pll = <&pll4_cfg1>;
+
+		pll4_cfg1: pll4_cfg1 {
+			st,pll_vco = <&pll4_vco_594Mhz>;
+			st,pll_div_pqr = <5 7 7>;
+		};
 	};
 };
 
diff --git a/fdts/stm32mp15xx-dkx.dtsi b/fdts/stm32mp15xx-dkx.dtsi
index d8b7c484c..bac9e053d 100644
--- a/fdts/stm32mp15xx-dkx.dtsi
+++ b/fdts/stm32mp15xx-dkx.dtsi
@@ -198,29 +198,8 @@
 		CLK_MPU_PLL1P
 		CLK_AXI_PLL2P
 		CLK_MCU_PLL3P
-		CLK_PLL12_HSE
-		CLK_PLL3_HSE
-		CLK_PLL4_HSE
-		CLK_RTC_LSE
 		CLK_MCO1_DISABLED
 		CLK_MCO2_DISABLED
-	>;
-
-	st,clkdiv = <
-		1 /*MPU*/
-		0 /*AXI*/
-		0 /*MCU*/
-		1 /*APB1*/
-		1 /*APB2*/
-		1 /*APB3*/
-		1 /*APB4*/
-		2 /*APB5*/
-		23 /*RTC*/
-		0 /*MCO1*/
-		0 /*MCO2*/
-	>;
-
-	st,pkcs = <
 		CLK_CKPER_HSE
 		CLK_FMC_ACLK
 		CLK_QSPI_ACLK
@@ -251,42 +230,82 @@
 		CLK_SAI2_PLL3Q
 		CLK_SAI3_PLL3Q
 		CLK_SAI4_PLL3Q
-		CLK_RNG1_LSI
+		CLK_RNG1_CSI
 		CLK_RNG2_LSI
 		CLK_LPTIM1_PCLK1
 		CLK_LPTIM23_PCLK3
 		CLK_LPTIM45_LSE
 	>;
 
-	/* VCO = 1300.0 MHz => P = 650 (CPU) */
-	pll1: st,pll@0 {
-		compatible = "st,stm32mp1-pll";
-		reg = <0>;
-		cfg = < 2 80 0 0 0 PQR(1,0,0) >;
-		frac = < 0x800 >;
+	st,clkdiv = <
+		DIV(DIV_MPU, 1)
+		DIV(DIV_AXI, 0)
+		DIV(DIV_MCU, 0)
+		DIV(DIV_APB1, 1)
+		DIV(DIV_APB2, 1)
+		DIV(DIV_APB3, 1)
+		DIV(DIV_APB4, 1)
+		DIV(DIV_APB5, 2)
+		DIV(DIV_MCO1, 0)
+		DIV(DIV_MCO2, 0)
+	>;
+
+	st,pll_vco {
+		pll2_vco_1066Mhz: pll2-vco-1066Mhz {
+			src = <CLK_PLL12_HSE>;
+			divmn = <2 65>;
+			frac = <0x1400>;
+		};
+
+		pll3_vco_417Mhz: pll3-vco-417Mhz {
+			src = <CLK_PLL3_HSE>;
+			divmn = <1 33>;
+			frac = <0x1a04>;
+		};
+
+		pll4_vco_594Mhz: pll4-vco-594Mhz {
+			src = <CLK_PLL4_HSE>;
+			divmn = <3 98>;
+		};
 	};
 
 	/* VCO = 1066.0 MHz => P = 266 (AXI), Q = 533 (GPU), R = 533 (DDR) */
 	pll2: st,pll@1 {
 		compatible = "st,stm32mp1-pll";
 		reg = <1>;
-		cfg = <2 65 1 0 0 PQR(1,1,1)>;
-		frac = <0x1400>;
+
+		st,pll = <&pll2_cfg1>;
+
+		pll2_cfg1: pll2_cfg1 {
+			st,pll_vco = <&pll2_vco_1066Mhz>;
+			st,pll_div_pqr = <1 0 0>;
+		};
 	};
 
 	/* VCO = 417.8 MHz => P = 209, Q = 24, R = 11 */
 	pll3: st,pll@2 {
 		compatible = "st,stm32mp1-pll";
 		reg = <2>;
-		cfg = <1 33 1 16 36 PQR(1,1,1)>;
-		frac = <0x1a04>;
+
+		st,pll = <&pll3_cfg1>;
+
+		pll3_cfg1: pll3_cfg1 {
+			st,pll_vco = <&pll3_vco_417Mhz>;
+			st,pll_div_pqr = <1 16 36>;
+		};
 	};
 
 	/* VCO = 594.0 MHz => P = 99, Q = 74, R = 74 */
 	pll4: st,pll@3 {
 		compatible = "st,stm32mp1-pll";
 		reg = <3>;
-		cfg = <3 98 5 7 7 PQR(1,1,1)>;
+
+		st,pll = <&pll4_cfg1>;
+
+		pll4_cfg1: pll4_cfg1 {
+			st,pll_vco = <&pll4_vco_594Mhz>;
+			st,pll_div_pqr = <5 7 7>;
+		};
 	};
 };
 
diff --git a/fdts/stm32mp15xx-osd32.dtsi b/fdts/stm32mp15xx-osd32.dtsi
index ef4c3c0e2..6e27b413f 100644
--- a/fdts/stm32mp15xx-osd32.dtsi
+++ b/fdts/stm32mp15xx-osd32.dtsi
@@ -185,29 +185,9 @@
 		CLK_MPU_PLL1P
 		CLK_AXI_PLL2P
 		CLK_MCU_PLL3P
-		CLK_PLL12_HSE
-		CLK_PLL3_HSE
-		CLK_PLL4_HSE
 		CLK_RTC_LSE
 		CLK_MCO1_DISABLED
 		CLK_MCO2_DISABLED
-	>;
-
-	st,clkdiv = <
-		1 /*MPU*/
-		0 /*AXI*/
-		0 /*MCU*/
-		1 /*APB1*/
-		1 /*APB2*/
-		1 /*APB3*/
-		1 /*APB4*/
-		2 /*APB5*/
-		23 /*RTC*/
-		0 /*MCO1*/
-		0 /*MCO2*/
-	>;
-
-	st,pkcs = <
 		CLK_CKPER_HSE
 		CLK_FMC_ACLK
 		CLK_QSPI_ACLK
@@ -238,41 +218,82 @@
 		CLK_SAI2_PLL3Q
 		CLK_SAI3_PLL3Q
 		CLK_SAI4_PLL3Q
-		CLK_RNG1_LSI
+		CLK_RNG1_CSI
 		CLK_RNG2_LSI
 		CLK_LPTIM1_PCLK1
 		CLK_LPTIM23_PCLK3
 		CLK_LPTIM45_LSE
 	>;
 
-	/* VCO = 1300.0 MHz => P = 650 (CPU) */
-	pll1: st,pll@0 {
-		compatible = "st,stm32mp1-pll";
-		reg = <0>;
-		cfg = < 2 80 0 0 0 PQR(1,0,0) >;
-		frac = < 0x800 >;
+	st,clkdiv = <
+		DIV(DIV_MPU, 1)
+		DIV(DIV_AXI, 0)
+		DIV(DIV_MCU, 0)
+		DIV(DIV_APB1, 1)
+		DIV(DIV_APB2, 1)
+		DIV(DIV_APB3, 1)
+		DIV(DIV_APB4, 1)
+		DIV(DIV_APB5, 2)
+		DIV(DIV_RTC, 23)
+		DIV(DIV_MCO1, 0)
+		DIV(DIV_MCO2, 0)
+	>;
+
+	st,pll_vco {
+		pll2_vco_1066Mhz: pll2-vco-1066Mhz {
+			src = <CLK_PLL12_HSE>;
+			divmn = <2 65>;
+			frac = <0x1400>;
+		};
+
+		pll3_vco_417Mhz: pll3-vco-417Mhz {
+			src = <CLK_PLL3_HSE>;
+			divmn = <1 33>;
+			frac = <0x1a04>;
+		};
+
+		pll4_vco_594Mhz: pll4-vco-594Mhz {
+			src = <CLK_PLL4_HSE>;
+			divmn = <3 98>;
+		};
 	};
 
 	/* VCO = 1066.0 MHz => P = 266 (AXI), Q = 533 (GPU), R = 533 (DDR) */
 	pll2: st,pll@1 {
 		compatible = "st,stm32mp1-pll";
 		reg = <1>;
-		cfg = <2 65 1 0 0 PQR(1,1,1)>;
-		frac = <0x1400>;
+
+		st,pll = <&pll2_cfg1>;
+
+		pll2_cfg1: pll2_cfg1 {
+			st,pll_vco = <&pll2_vco_1066Mhz>;
+			st,pll_div_pqr = <1 0 0>;
+		};
 	};
 
 	/* VCO = 417.8 MHz => P = 209, Q = 24, R = 11 */
 	pll3: st,pll@2 {
 		compatible = "st,stm32mp1-pll";
 		reg = <2>;
-		cfg = <1 33 1 16 36 PQR(1,1,1)>;
-		frac = <0x1a04>;
+
+		st,pll = <&pll3_cfg1>;
+
+		pll3_cfg1: pll3_cfg1 {
+			st,pll_vco = <&pll3_vco_417Mhz>;
+			st,pll_div_pqr = <1 16 36>;
+		};
 	};
 
 	/* VCO = 594.0 MHz => P = 99, Q = 74, R = 74 */
 	pll4: st,pll@3 {
 		compatible = "st,stm32mp1-pll";
 		reg = <3>;
-		cfg = <3 98 5 7 7 PQR(1,1,1)>;
+
+		st,pll = <&pll4_cfg1>;
+
+		pll4_cfg1: pll4_cfg1 {
+			st,pll_vco = <&pll4_vco_594Mhz>;
+			st,pll_div_pqr = <5 7 7>;
+		};
 	};
 };
diff --git a/include/drivers/st/stm32mp_clkfunc.h b/include/drivers/st/stm32mp_clkfunc.h
index 61286b224..8fd5ed141 100644
--- a/include/drivers/st/stm32mp_clkfunc.h
+++ b/include/drivers/st/stm32mp_clkfunc.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2022, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2024, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -29,8 +29,6 @@ int fdt_get_clock_id(int node);
 unsigned long fdt_get_uart_clock_freq(uintptr_t instance);
 
 void stm32mp_stgen_config(unsigned long rate);
-void stm32mp_stgen_restore_counter(unsigned long long value,
-				   unsigned long long offset_in_ms);
-unsigned long long stm32mp_stgen_get_counter(void);
+void stm32mp_stgen_restore_rate(void);
 
 #endif /* STM32MP_CLKFUNC_H */
diff --git a/include/dt-bindings/clock/stm32mp13-clks.h b/include/dt-bindings/clock/stm32mp13-clks.h
index 1d5bb7838..031e43232 100644
--- a/include/dt-bindings/clock/stm32mp13-clks.h
+++ b/include/dt-bindings/clock/stm32mp13-clks.h
@@ -1,6 +1,6 @@
 /* SPDX-License-Identifier: GPL-2.0+ or BSD-3-Clause */
 /*
- * Copyright (C) STMicroelectronics 2022 - All Rights Reserved
+ * Copyright (C) STMicroelectronics 2022-2024 - All Rights Reserved
  * Author: Gabriel Fernandez <gabriel.fernandez@st.com> for STMicroelectronics.
  */
 
@@ -193,7 +193,13 @@
 #define SAI1		160
 #define SAI2		161
 
-#define STM32MP1_LAST_CLK 162
+#define SPI1		162
+#define SPI2		163
+#define SPI3		164
+#define SPI4		165
+#define SPI5		166
+
+#define STM32MP1_LAST_CLK 167
 
 /* SCMI clock identifiers */
 #define CK_SCMI0_HSE		0
diff --git a/include/dt-bindings/clock/stm32mp15-clksrc.h b/include/dt-bindings/clock/stm32mp15-clksrc.h
index 3a3792da3..e601b484f 100644
--- a/include/dt-bindings/clock/stm32mp15-clksrc.h
+++ b/include/dt-bindings/clock/stm32mp15-clksrc.h
@@ -1,273 +1,413 @@
 /* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */
 /*
- * Copyright (C) 2017-2022, STMicroelectronics - All Rights Reserved
+ * Copyright (C) 2017-2024, STMicroelectronics - All Rights Reserved
  */
 
 #ifndef _DT_BINDINGS_CLOCK_STM32MP15_CLKSRC_H_
 #define _DT_BINDINGS_CLOCK_STM32MP15_CLKSRC_H_
 
+#include <lib/utils_def.h>
+
+#define CMD_DIV		0
+#define CMD_MUX		1
+#define CMD_CLK		2
+
+#define CMD_ADDR_BIT	BIT(31)
+
+#define CMD_SHIFT	26
+#define CMD_MASK	GENMASK_32(31, 26)
+#define CMD_DATA_MASK	GENMASK_32(25, 0)
+
+#define DIV_ID_SHIFT	8
+#define DIV_ID_MASK	GENMASK_32(15, 8)
+
+#define DIV_DIVN_SHIFT	0
+#define DIV_DIVN_MASK	GENMASK_32(7, 0)
+
+#define MUX_ID_SHIFT	4
+#define MUX_ID_MASK	GENMASK_32(11, 4)
+
+#define MUX_SEL_SHIFT	0
+#define MUX_SEL_MASK	GENMASK_32(3, 0)
+
+#define CLK_ID_MASK	GENMASK_32(19, 11)
+#define CLK_ID_SHIFT	11
+#define CLK_ON_MASK	0x00000400
+#define CLK_ON_SHIFT	10
+#define CLK_DIV_MASK	GENMASK_32(9, 4)
+#define CLK_DIV_SHIFT	4
+#define CLK_SEL_MASK	GENMASK_32(3, 0)
+#define CLK_SEL_SHIFT	0
+
+#define DIV(div_id, div)	((CMD_DIV << CMD_SHIFT) |\
+				 ((div_id) << DIV_ID_SHIFT) |\
+				 (div))
+
+#define CLKSRC(mux_id, sel)	((CMD_MUX << CMD_SHIFT) |\
+				 ((mux_id) << MUX_ID_SHIFT) |\
+				 (sel))
+
+/* CLK output is enable */
+#define CLK_SRC(clk_id, sel)	((CMD_CLK << CMD_SHIFT) |\
+				 ((clk_id) << CLK_ID_SHIFT) |\
+				 (sel) | CLK_ON_MASK)
+
+#define CLK_DISABLED(clk_id)	((CMD_CLK << CMD_SHIFT) |\
+				 ((clk_id) << CLK_ID_SHIFT))
+
+#define CLK_ADDR_SHIFT		16
+#define CLK_ADDR_MASK		GENMASK_32(30, 16)
+#define CLK_ADDR_VAL_MASK	GENMASK_32(15, 0)
+
+#define DIV_PLL1DIVP	0
+#define DIV_PLL2DIVP	1
+#define DIV_PLL2DIVQ	2
+#define DIV_PLL2DIVR	3
+#define DIV_PLL3DIVP	4
+#define DIV_PLL3DIVQ	5
+#define DIV_PLL3DIVR	6
+#define DIV_PLL4DIVP	7
+#define DIV_PLL4DIVQ	8
+#define DIV_PLL4DIVR	9
+#define DIV_MPU		10
+#define DIV_AXI		11
+#define DIV_MCU		12
+#define DIV_APB1	13
+#define DIV_APB2	14
+#define DIV_APB3	15
+#define DIV_APB4	16
+#define DIV_APB5	17
+#define DIV_RTC		19
+#define DIV_MCO1	20
+#define DIV_MCO2	21
+#define DIV_HSI		22
+#define DIV_TRACE	23
+#define DIV_ETHPTP	24
+#define DIV_NB		25
+
+#define MUX_MPU		0
+#define MUX_AXI		1
+#define MUX_MCU		2
+#define MUX_PLL12	3
+#define MUX_PLL3	4
+#define MUX_PLL4	5
+#define MUX_CKPER	6
+#define MUX_RTC		7
+#define MUX_SDMMC12	8
+#define MUX_SDMMC3	9
+#define MUX_FMC		10
+#define MUX_QSPI	11
+#define MUX_RNG1	12
+#define MUX_RNG2	13
+#define MUX_USBPHY	14
+#define MUX_USBO	15
+#define MUX_STGEN	16
+#define MUX_SPDIF	17
+#define MUX_SPI2S1	18
+#define MUX_SPI2S23	19
+#define MUX_SPI45	20
+#define MUX_SPI6	21
+#define MUX_CEC		22
+#define MUX_I2C12	23
+#define MUX_I2C35	24
+#define MUX_I2C46	25
+#define MUX_LPTIM1	26
+#define MUX_LPTIM23	27
+#define MUX_LPTIM45	28
+#define MUX_UART1	29
+#define MUX_UART24	30
+#define MUX_UART35	31
+#define MUX_UART6	32
+#define MUX_UART78	33
+#define MUX_SAI1	34
+#define MUX_SAI2	35
+#define MUX_SAI3	36
+#define MUX_SAI4	37
+#define MUX_DSI		38
+#define MUX_FDCAN	39
+#define MUX_ADC		40
+#define MUX_ETH		41
+#define MUX_MCO1	42
+#define MUX_MCO2	43
+#define MUX_NB		44
+
 /* PLL output is enable when x=1, with x=p,q or r */
 #define PQR(p, q, r)	(((p) & 1) | (((q) & 1) << 1) | (((r) & 1) << 2))
 
-/* st,clksrc: mandatory clock source */
-#define CLK_MPU_HSI		0x00000200
-#define CLK_MPU_HSE		0x00000201
-#define CLK_MPU_PLL1P		0x00000202
-#define CLK_MPU_PLL1P_DIV	0x00000203
+/* st,clksrc: clock sources */
+#define CLK_MPU_HSI		CLKSRC(MUX_MPU, 0)
+#define CLK_MPU_HSE		CLKSRC(MUX_MPU, 1)
+#define CLK_MPU_PLL1P		CLKSRC(MUX_MPU, 2)
+#define CLK_MPU_PLL1P_DIV	CLKSRC(MUX_MPU, 3)
 
-#define CLK_AXI_HSI		0x00000240
-#define CLK_AXI_HSE		0x00000241
-#define CLK_AXI_PLL2P		0x00000242
+#define CLK_AXI_HSI		CLKSRC(MUX_AXI, 0)
+#define CLK_AXI_HSE		CLKSRC(MUX_AXI, 1)
+#define CLK_AXI_PLL2P		CLKSRC(MUX_AXI, 2)
 
-#define CLK_MCU_HSI		0x00000480
-#define CLK_MCU_HSE		0x00000481
-#define CLK_MCU_CSI		0x00000482
-#define CLK_MCU_PLL3P		0x00000483
+#define CLK_MCU_HSI		CLKSRC(MUX_MCU, 0)
+#define CLK_MCU_HSE		CLKSRC(MUX_MCU, 1)
+#define CLK_MCU_CSI		CLKSRC(MUX_MCU, 2)
+#define CLK_MCU_PLL3P		CLKSRC(MUX_MCU, 3)
 
-#define CLK_PLL12_HSI		0x00000280
-#define CLK_PLL12_HSE		0x00000281
+#define CLK_PLL12_HSI		CLKSRC(MUX_PLL12, 0)
+#define CLK_PLL12_HSE		CLKSRC(MUX_PLL12, 1)
 
-#define CLK_PLL3_HSI		0x00008200
-#define CLK_PLL3_HSE		0x00008201
-#define CLK_PLL3_CSI		0x00008202
+#define CLK_PLL3_HSI		CLKSRC(MUX_PLL3, 0)
+#define CLK_PLL3_HSE		CLKSRC(MUX_PLL3, 1)
+#define CLK_PLL3_CSI		CLKSRC(MUX_PLL3, 2)
 
-#define CLK_PLL4_HSI		0x00008240
-#define CLK_PLL4_HSE		0x00008241
-#define CLK_PLL4_CSI		0x00008242
-#define CLK_PLL4_I2SCKIN	0x00008243
+#define CLK_PLL4_HSI		CLKSRC(MUX_PLL4, 0)
+#define CLK_PLL4_HSE		CLKSRC(MUX_PLL4, 1)
+#define CLK_PLL4_CSI		CLKSRC(MUX_PLL4, 2)
+#define CLK_PLL4_I2SCKIN	CLKSRC(MUX_PLL4, 3)
 
-#define CLK_RTC_DISABLED	0x00001400
-#define CLK_RTC_LSE		0x00001401
-#define CLK_RTC_LSI		0x00001402
-#define CLK_RTC_HSE		0x00001403
+#define CLK_RTC_DISABLED	CLK_DISABLED(RTC)
+#define CLK_RTC_LSE		CLK_SRC(RTC, 1)
+#define CLK_RTC_LSI		CLK_SRC(RTC, 2)
+#define CLK_RTC_HSE		CLK_SRC(RTC, 3)
 
-#define CLK_MCO1_HSI		0x00008000
-#define CLK_MCO1_HSE		0x00008001
-#define CLK_MCO1_CSI		0x00008002
-#define CLK_MCO1_LSI		0x00008003
-#define CLK_MCO1_LSE		0x00008004
-#define CLK_MCO1_DISABLED	0x0000800F
+/* Register addresses of MCO1 & MCO2 */
+#define MCO1			0x800
+#define MCO2			0x804
 
-#define CLK_MCO2_MPU		0x00008040
-#define CLK_MCO2_AXI		0x00008041
-#define CLK_MCO2_MCU		0x00008042
-#define CLK_MCO2_PLL4P		0x00008043
-#define CLK_MCO2_HSE		0x00008044
-#define CLK_MCO2_HSI		0x00008045
-#define CLK_MCO2_DISABLED	0x0000804F
+#define MCO_OFF			0
+#define MCO_ON			1
+#define MCO_STATUS_SHIFT	12
 
-/* st,pkcs: peripheral kernel clock source */
+#define MCO_ON_CFG(addr, sel)	(CMD_ADDR_BIT |\
+				((addr) << CLK_ADDR_SHIFT) |\
+				(MCO_ON << MCO_STATUS_SHIFT) |\
+				(sel))
 
-#define CLK_I2C12_PCLK1		0x00008C00
-#define CLK_I2C12_PLL4R		0x00008C01
-#define CLK_I2C12_HSI		0x00008C02
-#define CLK_I2C12_CSI		0x00008C03
-#define CLK_I2C12_DISABLED	0x00008C07
+#define MCO_OFF_CFG(addr)	(CMD_ADDR_BIT |\
+				((addr) << CLK_ADDR_SHIFT) |\
+				(MCO_OFF << MCO_STATUS_SHIFT))
 
-#define CLK_I2C35_PCLK1		0x00008C40
-#define CLK_I2C35_PLL4R		0x00008C41
-#define CLK_I2C35_HSI		0x00008C42
-#define CLK_I2C35_CSI		0x00008C43
-#define CLK_I2C35_DISABLED	0x00008C47
+#define CLK_MCO1_HSI		MCO_ON_CFG(MCO1, 0)
+#define CLK_MCO1_HSE		MCO_ON_CFG(MCO1, 1)
+#define CLK_MCO1_CSI		MCO_ON_CFG(MCO1, 2)
+#define CLK_MCO1_LSI		MCO_ON_CFG(MCO1, 3)
+#define CLK_MCO1_LSE		MCO_ON_CFG(MCO1, 4)
+#define CLK_MCO1_DISABLED	MCO_OFF_CFG(MCO1)
 
-#define CLK_I2C46_PCLK5		0x00000C00
-#define CLK_I2C46_PLL3Q		0x00000C01
-#define CLK_I2C46_HSI		0x00000C02
-#define CLK_I2C46_CSI		0x00000C03
-#define CLK_I2C46_DISABLED	0x00000C07
+#define CLK_MCO2_MPU		MCO_ON_CFG(MCO2, 0)
+#define CLK_MCO2_AXI		MCO_ON_CFG(MCO2, 1)
+#define CLK_MCO2_MCU		MCO_ON_CFG(MCO2, 2)
+#define CLK_MCO2_PLL4		MCO_ON_CFG(MCO2, 3)
+#define CLK_MCO2_HSE		MCO_ON_CFG(MCO2, 4)
+#define CLK_MCO2_HSI		MCO_ON_CFG(MCO2, 5)
+#define CLK_MCO2_DISABLED	MCO_OFF_CFG(MCO2)
 
-#define CLK_SAI1_PLL4Q		0x00008C80
-#define CLK_SAI1_PLL3Q		0x00008C81
-#define CLK_SAI1_I2SCKIN	0x00008C82
-#define CLK_SAI1_CKPER		0x00008C83
-#define CLK_SAI1_PLL3R		0x00008C84
-#define CLK_SAI1_DISABLED	0x00008C87
+#define CLK_I2C12_PCLK1		CLKSRC(MUX_I2C12, 0)
+#define CLK_I2C12_PLL4R		CLKSRC(MUX_I2C12, 1)
+#define CLK_I2C12_HSI		CLKSRC(MUX_I2C12, 2)
+#define CLK_I2C12_CSI		CLKSRC(MUX_I2C12, 3)
+#define CLK_I2C12_DISABLED	CLKSRC(MUX_I2C12, 7)
 
-#define CLK_SAI2_PLL4Q		0x00008CC0
-#define CLK_SAI2_PLL3Q		0x00008CC1
-#define CLK_SAI2_I2SCKIN	0x00008CC2
-#define CLK_SAI2_CKPER		0x00008CC3
-#define CLK_SAI2_SPDIF		0x00008CC4
-#define CLK_SAI2_PLL3R		0x00008CC5
-#define CLK_SAI2_DISABLED	0x00008CC7
+#define CLK_I2C35_PCLK1		CLKSRC(MUX_I2C35, 0)
+#define CLK_I2C35_PLL4R		CLKSRC(MUX_I2C35, 1)
+#define CLK_I2C35_HSI		CLKSRC(MUX_I2C35, 2)
+#define CLK_I2C35_CSI		CLKSRC(MUX_I2C35, 3)
+#define CLK_I2C35_DISABLED	CLKSRC(MUX_I2C35, 7)
 
-#define CLK_SAI3_PLL4Q		0x00008D00
-#define CLK_SAI3_PLL3Q		0x00008D01
-#define CLK_SAI3_I2SCKIN	0x00008D02
-#define CLK_SAI3_CKPER		0x00008D03
-#define CLK_SAI3_PLL3R		0x00008D04
-#define CLK_SAI3_DISABLED	0x00008D07
+#define CLK_I2C46_PCLK5		CLKSRC(MUX_I2C46, 0)
+#define CLK_I2C46_PLL3Q		CLKSRC(MUX_I2C46, 1)
+#define CLK_I2C46_HSI		CLKSRC(MUX_I2C46, 2)
+#define CLK_I2C46_CSI		CLKSRC(MUX_I2C46, 3)
+#define CLK_I2C46_DISABLED	CLKSRC(MUX_I2C46, 7)
 
-#define CLK_SAI4_PLL4Q		0x00008D40
-#define CLK_SAI4_PLL3Q		0x00008D41
-#define CLK_SAI4_I2SCKIN	0x00008D42
-#define CLK_SAI4_CKPER		0x00008D43
-#define CLK_SAI4_PLL3R		0x00008D44
-#define CLK_SAI4_DISABLED	0x00008D47
+#define CLK_SAI1_PLL4Q		CLKSRC(MUX_SAI1, 0)
+#define CLK_SAI1_PLL3Q		CLKSRC(MUX_SAI1, 1)
+#define CLK_SAI1_I2SCKIN	CLKSRC(MUX_SAI1, 2)
+#define CLK_SAI1_CKPER		CLKSRC(MUX_SAI1, 3)
+#define CLK_SAI1_PLL3R		CLKSRC(MUX_SAI1, 4)
+#define CLK_SAI1_DISABLED	CLKSRC(MUX_SAI1, 7)
 
-#define CLK_SPI2S1_PLL4P	0x00008D80
-#define CLK_SPI2S1_PLL3Q	0x00008D81
-#define CLK_SPI2S1_I2SCKIN	0x00008D82
-#define CLK_SPI2S1_CKPER	0x00008D83
-#define CLK_SPI2S1_PLL3R	0x00008D84
-#define CLK_SPI2S1_DISABLED	0x00008D87
+#define CLK_SAI2_PLL4Q		CLKSRC(MUX_SAI2, 0)
+#define CLK_SAI2_PLL3Q		CLKSRC(MUX_SAI2, 1)
+#define CLK_SAI2_I2SCKIN	CLKSRC(MUX_SAI2, 2)
+#define CLK_SAI2_CKPER		CLKSRC(MUX_SAI2, 3)
+#define CLK_SAI2_SPDIF		CLKSRC(MUX_SAI2, 4)
+#define CLK_SAI2_PLL3R		CLKSRC(MUX_SAI2, 5)
+#define CLK_SAI2_DISABLED	CLKSRC(MUX_SAI2, 7)
 
-#define CLK_SPI2S23_PLL4P	0x00008DC0
-#define CLK_SPI2S23_PLL3Q	0x00008DC1
-#define CLK_SPI2S23_I2SCKIN	0x00008DC2
-#define CLK_SPI2S23_CKPER	0x00008DC3
-#define CLK_SPI2S23_PLL3R	0x00008DC4
-#define CLK_SPI2S23_DISABLED	0x00008DC7
+#define CLK_SAI3_PLL4Q		CLKSRC(MUX_SAI3, 0)
+#define CLK_SAI3_PLL3Q		CLKSRC(MUX_SAI3, 1)
+#define CLK_SAI3_I2SCKIN	CLKSRC(MUX_SAI3, 2)
+#define CLK_SAI3_CKPER		CLKSRC(MUX_SAI3, 3)
+#define CLK_SAI3_PLL3R		CLKSRC(MUX_SAI3, 4)
+#define CLK_SAI3_DISABLED	CLKSRC(MUX_SAI3, 7)
 
-#define CLK_SPI45_PCLK2		0x00008E00
-#define CLK_SPI45_PLL4Q		0x00008E01
-#define CLK_SPI45_HSI		0x00008E02
-#define CLK_SPI45_CSI		0x00008E03
-#define CLK_SPI45_HSE		0x00008E04
-#define CLK_SPI45_DISABLED	0x00008E07
+#define CLK_SAI4_PLL4Q		CLKSRC(MUX_SAI4, 0)
+#define CLK_SAI4_PLL3Q		CLKSRC(MUX_SAI4, 1)
+#define CLK_SAI4_I2SCKIN	CLKSRC(MUX_SAI4, 2)
+#define CLK_SAI4_CKPER		CLKSRC(MUX_SAI4, 3)
+#define CLK_SAI4_PLL3R		CLKSRC(MUX_SAI4, 4)
+#define CLK_SAI4_DISABLED	CLKSRC(MUX_SAI4, 7)
 
-#define CLK_SPI6_PCLK5		0x00000C40
-#define CLK_SPI6_PLL4Q		0x00000C41
-#define CLK_SPI6_HSI		0x00000C42
-#define CLK_SPI6_CSI		0x00000C43
-#define CLK_SPI6_HSE		0x00000C44
-#define CLK_SPI6_PLL3Q		0x00000C45
-#define CLK_SPI6_DISABLED	0x00000C47
+#define CLK_SPI2S1_PLL4P	CLKSRC(MUX_SPI2S1, 0)
+#define CLK_SPI2S1_PLL3Q	CLKSRC(MUX_SPI2S1, 1)
+#define CLK_SPI2S1_I2SCKIN	CLKSRC(MUX_SPI2S1, 2)
+#define CLK_SPI2S1_CKPER	CLKSRC(MUX_SPI2S1, 3)
+#define CLK_SPI2S1_PLL3R	CLKSRC(MUX_SPI2S1, 4)
+#define CLK_SPI2S1_DISABLED	CLKSRC(MUX_SPI2S1, 7)
 
-#define CLK_UART6_PCLK2		0x00008E40
-#define CLK_UART6_PLL4Q		0x00008E41
-#define CLK_UART6_HSI		0x00008E42
-#define CLK_UART6_CSI		0x00008E43
-#define CLK_UART6_HSE		0x00008E44
-#define CLK_UART6_DISABLED	0x00008E47
+#define CLK_SPI2S23_PLL4P	CLKSRC(MUX_SPI2S23, 0)
+#define CLK_SPI2S23_PLL3Q	CLKSRC(MUX_SPI2S23, 1)
+#define CLK_SPI2S23_I2SCKIN	CLKSRC(MUX_SPI2S23, 2)
+#define CLK_SPI2S23_CKPER	CLKSRC(MUX_SPI2S23, 3)
+#define CLK_SPI2S23_PLL3R	CLKSRC(MUX_SPI2S23, 4)
+#define CLK_SPI2S23_DISABLED	CLKSRC(MUX_SPI2S23, 7)
 
-#define CLK_UART24_PCLK1	0x00008E80
-#define CLK_UART24_PLL4Q	0x00008E81
-#define CLK_UART24_HSI		0x00008E82
-#define CLK_UART24_CSI		0x00008E83
-#define CLK_UART24_HSE		0x00008E84
-#define CLK_UART24_DISABLED	0x00008E87
+#define CLK_SPI45_PCLK2		CLKSRC(MUX_SPI45, 0)
+#define CLK_SPI45_PLL4Q		CLKSRC(MUX_SPI45, 1)
+#define CLK_SPI45_HSI		CLKSRC(MUX_SPI45, 2)
+#define CLK_SPI45_CSI		CLKSRC(MUX_SPI45, 3)
+#define CLK_SPI45_HSE		CLKSRC(MUX_SPI45, 4)
+#define CLK_SPI45_DISABLED	CLKSRC(MUX_SPI45, 7)
 
-#define CLK_UART35_PCLK1	0x00008EC0
-#define CLK_UART35_PLL4Q	0x00008EC1
-#define CLK_UART35_HSI		0x00008EC2
-#define CLK_UART35_CSI		0x00008EC3
-#define CLK_UART35_HSE		0x00008EC4
-#define CLK_UART35_DISABLED	0x00008EC7
+#define CLK_SPI6_PCLK5		CLKSRC(MUX_SPI6, 0)
+#define CLK_SPI6_PLL4Q		CLKSRC(MUX_SPI6, 1)
+#define CLK_SPI6_HSI		CLKSRC(MUX_SPI6, 2)
+#define CLK_SPI6_CSI		CLKSRC(MUX_SPI6, 3)
+#define CLK_SPI6_HSE		CLKSRC(MUX_SPI6, 4)
+#define CLK_SPI6_PLL3Q		CLKSRC(MUX_SPI6, 5)
+#define CLK_SPI6_DISABLED	CLKSRC(MUX_SPI6, 7)
 
-#define CLK_UART78_PCLK1	0x00008F00
-#define CLK_UART78_PLL4Q	0x00008F01
-#define CLK_UART78_HSI		0x00008F02
-#define CLK_UART78_CSI		0x00008F03
-#define CLK_UART78_HSE		0x00008F04
-#define CLK_UART78_DISABLED	0x00008F07
+#define CLK_UART6_PCLK2		CLKSRC(MUX_UART6, 0)
+#define CLK_UART6_PLL4Q		CLKSRC(MUX_UART6, 1)
+#define CLK_UART6_HSI		CLKSRC(MUX_UART6, 2)
+#define CLK_UART6_CSI		CLKSRC(MUX_UART6, 3)
+#define CLK_UART6_HSE		CLKSRC(MUX_UART6, 4)
+#define CLK_UART6_DISABLED	CLKSRC(MUX_UART6, 7)
 
-#define CLK_UART1_PCLK5		0x00000C80
-#define CLK_UART1_PLL3Q		0x00000C81
-#define CLK_UART1_HSI		0x00000C82
-#define CLK_UART1_CSI		0x00000C83
-#define CLK_UART1_PLL4Q		0x00000C84
-#define CLK_UART1_HSE		0x00000C85
-#define CLK_UART1_DISABLED	0x00000C87
+#define CLK_UART24_PCLK1	CLKSRC(MUX_UART24, 0)
+#define CLK_UART24_PLL4Q	CLKSRC(MUX_UART24, 1)
+#define CLK_UART24_HSI		CLKSRC(MUX_UART24, 2)
+#define CLK_UART24_CSI		CLKSRC(MUX_UART24, 3)
+#define CLK_UART24_HSE		CLKSRC(MUX_UART24, 4)
+#define CLK_UART24_DISABLED	CLKSRC(MUX_UART24, 7)
 
-#define CLK_SDMMC12_HCLK6	0x00008F40
-#define CLK_SDMMC12_PLL3R	0x00008F41
-#define CLK_SDMMC12_PLL4P	0x00008F42
-#define CLK_SDMMC12_HSI		0x00008F43
-#define CLK_SDMMC12_DISABLED	0x00008F47
+#define CLK_UART35_PCLK1	CLKSRC(MUX_UART35, 0)
+#define CLK_UART35_PLL4Q	CLKSRC(MUX_UART35, 1)
+#define CLK_UART35_HSI		CLKSRC(MUX_UART35, 2)
+#define CLK_UART35_CSI		CLKSRC(MUX_UART35, 3)
+#define CLK_UART35_HSE		CLKSRC(MUX_UART35, 4)
+#define CLK_UART35_DISABLED	CLKSRC(MUX_UART35, 7)
 
-#define CLK_SDMMC3_HCLK2	0x00008F80
-#define CLK_SDMMC3_PLL3R	0x00008F81
-#define CLK_SDMMC3_PLL4P	0x00008F82
-#define CLK_SDMMC3_HSI		0x00008F83
-#define CLK_SDMMC3_DISABLED	0x00008F87
+#define CLK_UART78_PCLK1	CLKSRC(MUX_UART78, 0)
+#define CLK_UART78_PLL4Q	CLKSRC(MUX_UART78, 1)
+#define CLK_UART78_HSI		CLKSRC(MUX_UART78, 2)
+#define CLK_UART78_CSI		CLKSRC(MUX_UART78, 3)
+#define CLK_UART78_HSE		CLKSRC(MUX_UART78, 4)
+#define CLK_UART78_DISABLED	CLKSRC(MUX_UART78, 7)
 
-#define CLK_ETH_PLL4P		0x00008FC0
-#define CLK_ETH_PLL3Q		0x00008FC1
-#define CLK_ETH_DISABLED	0x00008FC3
+#define CLK_UART1_PCLK5		CLKSRC(MUX_UART1, 0)
+#define CLK_UART1_PLL3Q		CLKSRC(MUX_UART1, 1)
+#define CLK_UART1_HSI		CLKSRC(MUX_UART1, 2)
+#define CLK_UART1_CSI		CLKSRC(MUX_UART1, 3)
+#define CLK_UART1_PLL4Q		CLKSRC(MUX_UART1, 4)
+#define CLK_UART1_HSE		CLKSRC(MUX_UART1, 5)
+#define CLK_UART1_DISABLED	CLKSRC(MUX_UART1, 7)
 
-#define CLK_QSPI_ACLK		0x00009000
-#define CLK_QSPI_PLL3R		0x00009001
-#define CLK_QSPI_PLL4P		0x00009002
-#define CLK_QSPI_CKPER		0x00009003
+#define CLK_SDMMC12_HCLK6	CLKSRC(MUX_SDMMC12, 0)
+#define CLK_SDMMC12_PLL3R	CLKSRC(MUX_SDMMC12, 1)
+#define CLK_SDMMC12_PLL4P	CLKSRC(MUX_SDMMC12, 2)
+#define CLK_SDMMC12_HSI		CLKSRC(MUX_SDMMC12, 3)
+#define CLK_SDMMC12_DISABLED	CLKSRC(MUX_SDMMC12, 7)
 
-#define CLK_FMC_ACLK		0x00009040
-#define CLK_FMC_PLL3R		0x00009041
-#define CLK_FMC_PLL4P		0x00009042
-#define CLK_FMC_CKPER		0x00009043
+#define CLK_SDMMC3_HCLK2	CLKSRC(MUX_SDMMC3, 0)
+#define CLK_SDMMC3_PLL3R	CLKSRC(MUX_SDMMC3, 1)
+#define CLK_SDMMC3_PLL4P	CLKSRC(MUX_SDMMC3, 2)
+#define CLK_SDMMC3_HSI		CLKSRC(MUX_SDMMC3, 3)
+#define CLK_SDMMC3_DISABLED	CLKSRC(MUX_SDMMC3, 7)
 
-#define CLK_FDCAN_HSE		0x000090C0
-#define CLK_FDCAN_PLL3Q		0x000090C1
-#define CLK_FDCAN_PLL4Q		0x000090C2
-#define CLK_FDCAN_PLL4R		0x000090C3
+#define CLK_ETH_PLL4P		CLKSRC(MUX_ETH, 0)
+#define CLK_ETH_PLL3Q		CLKSRC(MUX_ETH, 1)
+#define CLK_ETH_DISABLED	CLKSRC(MUX_ETH, 3)
 
-#define CLK_SPDIF_PLL4P		0x00009140
-#define CLK_SPDIF_PLL3Q		0x00009141
-#define CLK_SPDIF_HSI		0x00009142
-#define CLK_SPDIF_DISABLED	0x00009143
+#define CLK_QSPI_ACLK		CLKSRC(MUX_QSPI, 0)
+#define CLK_QSPI_PLL3R		CLKSRC(MUX_QSPI, 1)
+#define CLK_QSPI_PLL4P		CLKSRC(MUX_QSPI, 2)
+#define CLK_QSPI_CKPER		CLKSRC(MUX_QSPI, 3)
 
-#define CLK_CEC_LSE		0x00009180
-#define CLK_CEC_LSI		0x00009181
-#define CLK_CEC_CSI_DIV122	0x00009182
-#define CLK_CEC_DISABLED	0x00009183
+#define CLK_FMC_ACLK		CLKSRC(MUX_FMC, 0)
+#define CLK_FMC_PLL3R		CLKSRC(MUX_FMC, 1)
+#define CLK_FMC_PLL4P		CLKSRC(MUX_FMC, 2)
+#define CLK_FMC_CKPER		CLKSRC(MUX_FMC, 3)
 
-#define CLK_USBPHY_HSE		0x000091C0
-#define CLK_USBPHY_PLL4R	0x000091C1
-#define CLK_USBPHY_HSE_DIV2	0x000091C2
-#define CLK_USBPHY_DISABLED	0x000091C3
+#define CLK_FDCAN_HSE		CLKSRC(MUX_FDCAN, 0)
+#define CLK_FDCAN_PLL3Q		CLKSRC(MUX_FDCAN, 1)
+#define CLK_FDCAN_PLL4Q		CLKSRC(MUX_FDCAN, 2)
+#define CLK_FDCAN_PLL4R		CLKSRC(MUX_FDCAN, 3)
 
-#define CLK_USBO_PLL4R		0x800091C0
-#define CLK_USBO_USBPHY		0x800091C1
+#define CLK_SPDIF_PLL4P		CLKSRC(MUX_SPDIF, 0)
+#define CLK_SPDIF_PLL3Q		CLKSRC(MUX_SPDIF, 1)
+#define CLK_SPDIF_HSI		CLKSRC(MUX_SPDIF, 2)
+#define CLK_SPDIF_DISABLED	CLKSRC(MUX_SPDIF, 3)
 
-#define CLK_RNG1_CSI		0x00000CC0
-#define CLK_RNG1_PLL4R		0x00000CC1
-#define CLK_RNG1_LSE		0x00000CC2
-#define CLK_RNG1_LSI		0x00000CC3
+#define CLK_CEC_LSE		CLKSRC(MUX_CEC, 0)
+#define CLK_CEC_LSI		CLKSRC(MUX_CEC, 1)
+#define CLK_CEC_CSI_DIV122	CLKSRC(MUX_CEC, 2)
+#define CLK_CEC_DISABLED	CLKSRC(MUX_CEC, 3)
 
-#define CLK_RNG2_CSI		0x00009200
-#define CLK_RNG2_PLL4R		0x00009201
-#define CLK_RNG2_LSE		0x00009202
-#define CLK_RNG2_LSI		0x00009203
+#define CLK_USBPHY_HSE		CLKSRC(MUX_USBPHY, 0)
+#define CLK_USBPHY_PLL4R	CLKSRC(MUX_USBPHY, 1)
+#define CLK_USBPHY_HSE_DIV2	CLKSRC(MUX_USBPHY, 2)
+#define CLK_USBPHY_DISABLED	CLKSRC(MUX_USBPHY, 3)
 
-#define CLK_CKPER_HSI		0x00000D00
-#define CLK_CKPER_CSI		0x00000D01
-#define CLK_CKPER_HSE		0x00000D02
-#define CLK_CKPER_DISABLED	0x00000D03
+#define CLK_USBO_PLL4R		CLKSRC(MUX_USBO, 0)
+#define CLK_USBO_USBPHY		CLKSRC(MUX_USBO, 1)
 
-#define CLK_STGEN_HSI		0x00000D40
-#define CLK_STGEN_HSE		0x00000D41
-#define CLK_STGEN_DISABLED	0x00000D43
+#define CLK_RNG1_CSI		CLKSRC(MUX_RNG1, 0)
+#define CLK_RNG1_PLL4R		CLKSRC(MUX_RNG1, 1)
+#define CLK_RNG1_LSE		CLKSRC(MUX_RNG1, 2)
+#define CLK_RNG1_LSI		CLKSRC(MUX_RNG1, 3)
 
-#define CLK_DSI_DSIPLL		0x00009240
-#define CLK_DSI_PLL4P		0x00009241
+#define CLK_RNG2_CSI		CLKSRC(MUX_RNG2, 0)
+#define CLK_RNG2_PLL4R		CLKSRC(MUX_RNG2, 1)
+#define CLK_RNG2_LSE		CLKSRC(MUX_RNG2, 2)
+#define CLK_RNG2_LSI		CLKSRC(MUX_RNG2, 3)
 
-#define CLK_ADC_PLL4R		0x00009280
-#define CLK_ADC_CKPER		0x00009281
-#define CLK_ADC_PLL3Q		0x00009282
-#define CLK_ADC_DISABLED	0x00009283
+#define CLK_CKPER_HSI		CLKSRC(MUX_CKPER, 0)
+#define CLK_CKPER_CSI		CLKSRC(MUX_CKPER, 1)
+#define CLK_CKPER_HSE		CLKSRC(MUX_CKPER, 2)
+#define CLK_CKPER_DISABLED	CLKSRC(MUX_CKPER, 3)
 
-#define CLK_LPTIM45_PCLK3	0x000092C0
-#define CLK_LPTIM45_PLL4P	0x000092C1
-#define CLK_LPTIM45_PLL3Q	0x000092C2
-#define CLK_LPTIM45_LSE		0x000092C3
-#define CLK_LPTIM45_LSI		0x000092C4
-#define CLK_LPTIM45_CKPER	0x000092C5
-#define CLK_LPTIM45_DISABLED	0x000092C7
+#define CLK_STGEN_HSI		CLKSRC(MUX_STGEN, 0)
+#define CLK_STGEN_HSE		CLKSRC(MUX_STGEN, 1)
+#define CLK_STGEN_DISABLED	CLKSRC(MUX_STGEN, 3)
 
-#define CLK_LPTIM23_PCLK3	0x00009300
-#define CLK_LPTIM23_PLL4Q	0x00009301
-#define CLK_LPTIM23_CKPER	0x00009302
-#define CLK_LPTIM23_LSE		0x00009303
-#define CLK_LPTIM23_LSI		0x00009304
-#define CLK_LPTIM23_DISABLED	0x00009307
+#define CLK_DSI_DSIPLL		CLKSRC(MUX_DSI, 0)
+#define CLK_DSI_PLL4P		CLKSRC(MUX_DSI, 1)
 
-#define CLK_LPTIM1_PCLK1	0x00009340
-#define CLK_LPTIM1_PLL4P	0x00009341
-#define CLK_LPTIM1_PLL3Q	0x00009342
-#define CLK_LPTIM1_LSE		0x00009343
-#define CLK_LPTIM1_LSI		0x00009344
-#define CLK_LPTIM1_CKPER	0x00009345
-#define CLK_LPTIM1_DISABLED	0x00009347
+#define CLK_ADC_PLL4R		CLKSRC(MUX_ADC, 0)
+#define CLK_ADC_CKPER		CLKSRC(MUX_ADC, 1)
+#define CLK_ADC_PLL3Q		CLKSRC(MUX_ADC, 2)
+#define CLK_ADC_DISABLED	CLKSRC(MUX_ADC, 3)
+
+#define CLK_LPTIM45_PCLK3	CLKSRC(MUX_LPTIM45, 0)
+#define CLK_LPTIM45_PLL4P	CLKSRC(MUX_LPTIM45, 1)
+#define CLK_LPTIM45_PLL3Q	CLKSRC(MUX_LPTIM45, 2)
+#define CLK_LPTIM45_LSE		CLKSRC(MUX_LPTIM45, 3)
+#define CLK_LPTIM45_LSI		CLKSRC(MUX_LPTIM45, 4)
+#define CLK_LPTIM45_CKPER	CLKSRC(MUX_LPTIM45, 5)
+#define CLK_LPTIM45_DISABLED	CLKSRC(MUX_LPTIM45, 7)
+
+#define CLK_LPTIM23_PCLK3	CLKSRC(MUX_LPTIM23, 0)
+#define CLK_LPTIM23_PLL4Q	CLKSRC(MUX_LPTIM23, 1)
+#define CLK_LPTIM23_CKPER	CLKSRC(MUX_LPTIM23, 2)
+#define CLK_LPTIM23_LSE		CLKSRC(MUX_LPTIM23, 3)
+#define CLK_LPTIM23_LSI		CLKSRC(MUX_LPTIM23, 4)
+#define CLK_LPTIM23_DISABLED	CLKSRC(MUX_LPTIM23, 7)
+
+#define CLK_LPTIM1_PCLK1	CLKSRC(MUX_LPTIM1, 0)
+#define CLK_LPTIM1_PLL4P	CLKSRC(MUX_LPTIM1, 1)
+#define CLK_LPTIM1_PLL3Q	CLKSRC(MUX_LPTIM1, 2)
+#define CLK_LPTIM1_LSE		CLKSRC(MUX_LPTIM1, 3)
+#define CLK_LPTIM1_LSI		CLKSRC(MUX_LPTIM1, 4)
+#define CLK_LPTIM1_CKPER	CLKSRC(MUX_LPTIM1, 5)
+#define CLK_LPTIM1_DISABLED	CLKSRC(MUX_LPTIM1, 7)
 
 /* define for st,pll /csg */
 #define SSCG_MODE_CENTER_SPREAD	0
diff --git a/plat/st/stm32mp1/stm32mp1_def.h b/plat/st/stm32mp1/stm32mp1_def.h
index 0d401f945..8a1d76d19 100644
--- a/plat/st/stm32mp1/stm32mp1_def.h
+++ b/plat/st/stm32mp1/stm32mp1_def.h
@@ -633,6 +633,11 @@ static inline uintptr_t tamp_bkpr(uint32_t idx)
 /* 2 FIXED */
 #define PLAT_NB_FIXED_REGUS		U(2)
 
+/*******************************************************************************
+ * STM32MP1 CLOCKS
+ ******************************************************************************/
+#define PLL1_NOMINAL_FREQ_IN_KHZ	U(650000) /* 650MHz */
+
 /*******************************************************************************
  * Device Tree defines
  ******************************************************************************/