Adding upstream version 2.10.0+dfsg.upstream/2.10.0+dfsgupstream

Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>

5 files changed, 6580 insertions, 0 deletions

diff --git a/drivers/st/clk/clk-stm32-core.c b/drivers/st/clk/clk-stm32-core.c
new file mode 100644
index 0000000..9fe8c8c
--- /dev/null
+++ b/drivers/st/clk/clk-stm32-core.c
@@ -0,0 +1,1088 @@
+/*
+ * Copyright (C) 2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+
+#include "clk-stm32-core.h"
+#include <common/debug.h>
+#include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32mp_clkfunc.h>
+#include <lib/mmio.h>
+#include <lib/spinlock.h>
+
+static struct spinlock reg_lock;
+static struct spinlock refcount_lock;
+
+static struct stm32_clk_priv *stm32_clock_data;
+
+const struct stm32_clk_ops clk_mux_ops;
+
+struct stm32_clk_priv *clk_stm32_get_priv(void)
+{
+	return stm32_clock_data;
+}
+
+static void stm32mp1_clk_lock(struct spinlock *lock)
+{
+	if (stm32mp_lock_available()) {
+		/* Assume interrupts are masked */
+		spin_lock(lock);
+	}
+}
+
+static void stm32mp1_clk_unlock(struct spinlock *lock)
+{
+	if (stm32mp_lock_available()) {
+		spin_unlock(lock);
+	}
+}
+
+void stm32mp1_clk_rcc_regs_lock(void)
+{
+	stm32mp1_clk_lock(&reg_lock);
+}
+
+void stm32mp1_clk_rcc_regs_unlock(void)
+{
+	stm32mp1_clk_unlock(&reg_lock);
+}
+
+#define TIMEOUT_US_1S	U(1000000)
+#define OSCRDY_TIMEOUT	TIMEOUT_US_1S
+
+struct clk_oscillator_data *clk_oscillator_get_data(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct stm32_osc_cfg *osc_cfg = clk->clock_cfg;
+	int osc_id = osc_cfg->osc_id;
+
+	return &priv->osci_data[osc_id];
+}
+
+void clk_oscillator_set_bypass(struct stm32_clk_priv *priv, int id, bool digbyp, bool bypass)
+{
+	struct clk_oscillator_data *osc_data = clk_oscillator_get_data(priv, id);
+
+	struct stm32_clk_bypass *bypass_data = osc_data->bypass;
+	uintptr_t address;
+
+	if (bypass_data == NULL) {
+		return;
+	}
+
+	address = priv->base + bypass_data->offset;
+
+	if (digbyp) {
+		mmio_setbits_32(address, BIT(bypass_data->bit_digbyp));
+	}
+
+	if (bypass || digbyp) {
+		mmio_setbits_32(address, BIT(bypass_data->bit_byp));
+	}
+}
+
+void clk_oscillator_set_css(struct stm32_clk_priv *priv, int id, bool css)
+{
+	struct clk_oscillator_data *osc_data = clk_oscillator_get_data(priv, id);
+
+	struct stm32_clk_css *css_data = osc_data->css;
+	uintptr_t address;
+
+	if (css_data == NULL) {
+		return;
+	}
+
+	address = priv->base + css_data->offset;
+
+	if (css) {
+		mmio_setbits_32(address, BIT(css_data->bit_css));
+	}
+}
+
+void clk_oscillator_set_drive(struct stm32_clk_priv *priv, int id, uint8_t lsedrv)
+{
+	struct clk_oscillator_data *osc_data = clk_oscillator_get_data(priv, id);
+
+	struct stm32_clk_drive *drive_data = osc_data->drive;
+	uintptr_t address;
+	uint32_t mask;
+	uint32_t value;
+
+	if (drive_data == NULL) {
+		return;
+	}
+
+	address = priv->base + drive_data->offset;
+
+	mask = (BIT(drive_data->drv_width) - 1U) <<  drive_data->drv_shift;
+
+	/*
+	 * Warning: not recommended to switch directly from "high drive"
+	 * to "medium low drive", and vice-versa.
+	 */
+	value = (mmio_read_32(address) & mask) >> drive_data->drv_shift;
+
+	while (value != lsedrv) {
+		if (value > lsedrv) {
+			value--;
+		} else {
+			value++;
+		}
+
+		mmio_clrsetbits_32(address, mask, value << drive_data->drv_shift);
+	}
+}
+
+int clk_oscillator_wait_ready(struct stm32_clk_priv *priv, int id, bool ready_on)
+{
+	struct clk_oscillator_data *osc_data = clk_oscillator_get_data(priv, id);
+
+	return _clk_stm32_gate_wait_ready(priv, osc_data->gate_rdy_id, ready_on);
+}
+
+int clk_oscillator_wait_ready_on(struct stm32_clk_priv *priv, int id)
+{
+	return clk_oscillator_wait_ready(priv, id, true);
+}
+
+int clk_oscillator_wait_ready_off(struct stm32_clk_priv *priv, int id)
+{
+	return clk_oscillator_wait_ready(priv, id, false);
+}
+
+static int clk_gate_enable(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct clk_gate_cfg *cfg = clk->clock_cfg;
+
+	mmio_setbits_32(priv->base + cfg->offset, BIT(cfg->bit_idx));
+
+	return 0;
+}
+
+static void clk_gate_disable(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct clk_gate_cfg *cfg = clk->clock_cfg;
+
+	mmio_clrbits_32(priv->base + cfg->offset, BIT(cfg->bit_idx));
+}
+
+static bool clk_gate_is_enabled(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct clk_gate_cfg *cfg = clk->clock_cfg;
+
+	return ((mmio_read_32(priv->base + cfg->offset) & BIT(cfg->bit_idx)) != 0U);
+}
+
+const struct stm32_clk_ops clk_gate_ops = {
+	.enable		= clk_gate_enable,
+	.disable	= clk_gate_disable,
+	.is_enabled	= clk_gate_is_enabled,
+};
+
+void _clk_stm32_gate_disable(struct stm32_clk_priv *priv, uint16_t gate_id)
+{
+	const struct gate_cfg *gate = &priv->gates[gate_id];
+	uintptr_t addr = priv->base + gate->offset;
+
+	if (gate->set_clr != 0U) {
+		mmio_write_32(addr + RCC_MP_ENCLRR_OFFSET, BIT(gate->bit_idx));
+	} else {
+		mmio_clrbits_32(addr, BIT(gate->bit_idx));
+	}
+}
+
+int _clk_stm32_gate_enable(struct stm32_clk_priv *priv, uint16_t gate_id)
+{
+	const struct gate_cfg *gate = &priv->gates[gate_id];
+	uintptr_t addr = priv->base + gate->offset;
+
+	if (gate->set_clr != 0U) {
+		mmio_write_32(addr, BIT(gate->bit_idx));
+
+	} else {
+		mmio_setbits_32(addr, BIT(gate->bit_idx));
+	}
+
+	return 0;
+}
+
+const struct clk_stm32 *_clk_get(struct stm32_clk_priv *priv, int id)
+{
+	if ((unsigned int)id < priv->num) {
+		return &priv->clks[id];
+	}
+
+	return NULL;
+}
+
+#define clk_div_mask(_width) GENMASK(((_width) - 1U), 0U)
+
+static unsigned int _get_table_div(const struct clk_div_table *table,
+				   unsigned int val)
+{
+	const struct clk_div_table *clkt;
+
+	for (clkt = table; clkt->div; clkt++) {
+		if (clkt->val == val) {
+			return clkt->div;
+		}
+	}
+
+	return 0;
+}
+
+static unsigned int _get_div(const struct clk_div_table *table,
+			     unsigned int val, unsigned long flags,
+			     uint8_t width)
+{
+	if ((flags & CLK_DIVIDER_ONE_BASED) != 0UL) {
+		return val;
+	}
+
+	if ((flags & CLK_DIVIDER_POWER_OF_TWO) != 0UL) {
+		return BIT(val);
+	}
+
+	if ((flags & CLK_DIVIDER_MAX_AT_ZERO) != 0UL) {
+		return (val != 0U) ? val : BIT(width);
+	}
+
+	if (table != NULL) {
+		return _get_table_div(table, val);
+	}
+
+	return val + 1U;
+}
+
+#define TIMEOUT_US_200MS	U(200000)
+#define CLKSRC_TIMEOUT		TIMEOUT_US_200MS
+
+int clk_mux_set_parent(struct stm32_clk_priv *priv, uint16_t pid, uint8_t sel)
+{
+	const struct parent_cfg *parents = &priv->parents[pid & MUX_PARENT_MASK];
+	const struct mux_cfg *mux = parents->mux;
+	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;
+}
+
+int _clk_stm32_set_parent(struct stm32_clk_priv *priv, int clk, int clkp)
+{
+	const struct parent_cfg *parents;
+	uint16_t pid;
+	uint8_t sel;
+	int old_parent;
+
+	pid = priv->clks[clk].parent;
+
+	if ((pid == CLK_IS_ROOT) || (pid < MUX_MAX_PARENTS)) {
+		return -EINVAL;
+	}
+
+	old_parent = _clk_stm32_get_parent(priv, clk);
+	if (old_parent < 0) {
+		return old_parent;
+	}
+	if (old_parent == clkp) {
+		return 0;
+	}
+
+	parents = &priv->parents[pid & MUX_PARENT_MASK];
+
+	for (sel = 0; sel <  parents->num_parents; sel++) {
+		if (parents->id_parents[sel] == (uint16_t)clkp) {
+			bool clk_was_enabled = _clk_stm32_is_enabled(priv, clk);
+			int err = 0;
+
+			/* Enable the parents (for glitch free mux) */
+			_clk_stm32_enable(priv, clkp);
+			_clk_stm32_enable(priv, old_parent);
+
+			err = clk_mux_set_parent(priv, pid, sel);
+
+			_clk_stm32_disable(priv, old_parent);
+
+			if (clk_was_enabled) {
+				_clk_stm32_disable(priv, old_parent);
+			} else {
+				_clk_stm32_disable(priv, clkp);
+			}
+
+			return err;
+		}
+	}
+
+	return -EINVAL;
+}
+
+int clk_mux_get_parent(struct stm32_clk_priv *priv, uint32_t mux_id)
+{
+	const struct parent_cfg *parent;
+	const struct mux_cfg *mux;
+	uint32_t mask;
+
+	if (mux_id >= priv->nb_parents) {
+		panic();
+	}
+
+	parent = &priv->parents[mux_id];
+	mux = parent->mux;
+
+	mask = MASK_WIDTH_SHIFT(mux->width, mux->shift);
+
+	return (mmio_read_32(priv->base + mux->offset) & mask) >> mux->shift;
+}
+
+int _clk_stm32_set_parent_by_index(struct stm32_clk_priv *priv, int clk, int sel)
+{
+	uint16_t pid;
+
+	pid = priv->clks[clk].parent;
+
+	if ((pid == CLK_IS_ROOT) || (pid < MUX_MAX_PARENTS)) {
+		return -EINVAL;
+	}
+
+	return clk_mux_set_parent(priv, pid, 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 parent_cfg *parent;
+	uint16_t mux_id;
+	int sel;
+
+	mux_id = priv->clks[clk_id].parent;
+	if (mux_id == CLK_IS_ROOT) {
+		return CLK_IS_ROOT;
+	}
+
+	if (mux_id < MUX_MAX_PARENTS) {
+		return mux_id & MUX_PARENT_MASK;
+	}
+
+	mux_id &= MUX_PARENT_MASK;
+	parent = &priv->parents[mux_id];
+
+	if (clk->ops->get_parent != NULL) {
+		sel = clk->ops->get_parent(priv, clk_id);
+	} else {
+		sel = clk_mux_get_parent(priv, mux_id);
+	}
+
+	if ((sel >= 0) && (sel < parent->num_parents)) {
+		return parent->id_parents[sel];
+	}
+
+	return -EINVAL;
+}
+
+int _clk_stm32_get_parent_index(struct stm32_clk_priv *priv, int clk_id)
+{
+	uint16_t mux_id;
+
+	mux_id = priv->clks[clk_id].parent;
+	if (mux_id == CLK_IS_ROOT) {
+		return CLK_IS_ROOT;
+	}
+
+	if (mux_id < MUX_MAX_PARENTS) {
+		return mux_id & MUX_PARENT_MASK;
+	}
+
+	mux_id &= MUX_PARENT_MASK;
+
+	return clk_mux_get_parent(priv, mux_id);
+}
+
+int _clk_stm32_get_parent_by_index(struct stm32_clk_priv *priv, int clk_id, int idx)
+{
+	const struct parent_cfg *parent;
+	uint16_t mux_id;
+
+	mux_id = priv->clks[clk_id].parent;
+	if (mux_id == CLK_IS_ROOT) {
+		return CLK_IS_ROOT;
+	}
+
+	if (mux_id < MUX_MAX_PARENTS) {
+		return mux_id & MUX_PARENT_MASK;
+	}
+
+	mux_id &= MUX_PARENT_MASK;
+	parent = &priv->parents[mux_id];
+
+	if (idx < parent->num_parents) {
+		return parent->id_parents[idx];
+	}
+
+	return -EINVAL;
+}
+
+int clk_get_index(struct stm32_clk_priv *priv, unsigned long binding_id)
+{
+	unsigned int i;
+
+	for (i = 0U; i < priv->num; i++) {
+		if (binding_id == priv->clks[i].binding) {
+			return (int)i;
+		}
+	}
+
+	return -EINVAL;
+}
+
+unsigned long _clk_stm32_get_rate(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	int parent;
+
+	if ((unsigned int)id >= priv->num) {
+		return 0UL;
+	}
+
+	parent = _clk_stm32_get_parent(priv, id);
+	if (parent < 0) {
+		return 0UL;
+	}
+
+	if (clk->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);
+	}
+
+	if (parent == CLK_IS_ROOT) {
+		panic();
+	}
+
+	return _clk_stm32_get_rate(priv, parent);
+}
+
+unsigned long _clk_stm32_get_parent_rate(struct stm32_clk_priv *priv, int id)
+{
+	int parent_id = _clk_stm32_get_parent(priv, id);
+
+	if (parent_id < 0) {
+		return 0UL;
+	}
+
+	return _clk_stm32_get_rate(priv, parent_id);
+}
+
+static uint8_t _stm32_clk_get_flags(struct stm32_clk_priv *priv, int id)
+{
+	return priv->clks[id].flags;
+}
+
+bool _stm32_clk_is_flags(struct stm32_clk_priv *priv, int id, uint8_t flag)
+{
+	if ((_stm32_clk_get_flags(priv, id) & flag) != 0U) {
+		return true;
+	}
+
+	return false;
+}
+
+int clk_stm32_enable_call_ops(struct stm32_clk_priv *priv, uint16_t id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+
+	if (clk->ops->enable != NULL) {
+		clk->ops->enable(priv, id);
+	}
+
+	return 0;
+}
+
+static int _clk_stm32_enable_core(struct stm32_clk_priv *priv, int id)
+{
+	int parent;
+	int ret = 0;
+
+	if (priv->gate_refcounts[id] == 0U) {
+		parent = _clk_stm32_get_parent(priv, id);
+		if (parent < 0) {
+			return parent;
+		}
+		if (parent != CLK_IS_ROOT) {
+			ret = _clk_stm32_enable_core(priv, parent);
+			if (ret != 0) {
+				return ret;
+			}
+		}
+		clk_stm32_enable_call_ops(priv, id);
+	}
+
+	priv->gate_refcounts[id]++;
+
+	if (priv->gate_refcounts[id] == UINT_MAX) {
+		ERROR("%s: %d max enable count !", __func__, id);
+		panic();
+	}
+
+	return 0;
+}
+
+int _clk_stm32_enable(struct stm32_clk_priv *priv, int id)
+{
+	int ret;
+
+	stm32mp1_clk_lock(&refcount_lock);
+	ret = _clk_stm32_enable_core(priv, id);
+	stm32mp1_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);
+
+	if (clk->ops->disable != NULL) {
+		clk->ops->disable(priv, id);
+	}
+}
+
+static void _clk_stm32_disable_core(struct stm32_clk_priv *priv, int id)
+{
+	int parent;
+
+	if ((priv->gate_refcounts[id] == 1U) && _stm32_clk_is_flags(priv, id, CLK_IS_CRITICAL)) {
+		return;
+	}
+
+	if (priv->gate_refcounts[id] == 0U) {
+		/* case of clock ignore unused */
+		if (_clk_stm32_is_enabled(priv, id)) {
+			clk_stm32_disable_call_ops(priv, id);
+			return;
+		}
+		VERBOSE("%s: %d already disabled !\n\n", __func__, id);
+		return;
+	}
+
+	if (--priv->gate_refcounts[id] > 0U) {
+		return;
+	}
+
+	clk_stm32_disable_call_ops(priv, id);
+
+	parent = _clk_stm32_get_parent(priv, id);
+	if ((parent >= 0) && (parent != CLK_IS_ROOT)) {
+		_clk_stm32_disable_core(priv, parent);
+	}
+}
+
+void _clk_stm32_disable(struct stm32_clk_priv *priv, int id)
+{
+	stm32mp1_clk_lock(&refcount_lock);
+
+	_clk_stm32_disable_core(priv, id);
+
+	stm32mp1_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);
+
+	if (clk->ops->is_enabled != NULL) {
+		return clk->ops->is_enabled(priv, id);
+	}
+
+	return priv->gate_refcounts[id];
+}
+
+static int clk_stm32_enable(unsigned long binding_id)
+{
+	struct stm32_clk_priv *priv = clk_stm32_get_priv();
+	int id;
+
+	id = clk_get_index(priv, binding_id);
+	if (id == -EINVAL) {
+		return id;
+	}
+
+	return _clk_stm32_enable(priv, id);
+}
+
+static void clk_stm32_disable(unsigned long binding_id)
+{
+	struct stm32_clk_priv *priv = clk_stm32_get_priv();
+	int id;
+
+	id = clk_get_index(priv, binding_id);
+	if (id != -EINVAL) {
+		_clk_stm32_disable(priv, id);
+	}
+}
+
+static bool clk_stm32_is_enabled(unsigned long binding_id)
+{
+	struct stm32_clk_priv *priv = clk_stm32_get_priv();
+	int id;
+
+	id = clk_get_index(priv, binding_id);
+	if (id == -EINVAL) {
+		return false;
+	}
+
+	return _clk_stm32_is_enabled(priv, id);
+}
+
+static unsigned long clk_stm32_get_rate(unsigned long binding_id)
+{
+	struct stm32_clk_priv *priv = clk_stm32_get_priv();
+	int id;
+
+	id = clk_get_index(priv, binding_id);
+	if (id == -EINVAL) {
+		return 0UL;
+	}
+
+	return _clk_stm32_get_rate(priv, id);
+}
+
+static int clk_stm32_get_parent(unsigned long binding_id)
+{
+	struct stm32_clk_priv *priv = clk_stm32_get_priv();
+	int id;
+
+	id = clk_get_index(priv, binding_id);
+	if (id == -EINVAL) {
+		return id;
+	}
+
+	return _clk_stm32_get_parent(priv, id);
+}
+
+static const struct clk_ops stm32mp_clk_ops = {
+	.enable		= clk_stm32_enable,
+	.disable	= clk_stm32_disable,
+	.is_enabled	= clk_stm32_is_enabled,
+	.get_rate	= clk_stm32_get_rate,
+	.get_parent	= clk_stm32_get_parent,
+};
+
+void clk_stm32_enable_critical_clocks(void)
+{
+	struct stm32_clk_priv *priv = clk_stm32_get_priv();
+	unsigned int i;
+
+	for (i = 0U; i < priv->num; i++) {
+		if (_stm32_clk_is_flags(priv, i, CLK_IS_CRITICAL)) {
+			_clk_stm32_enable(priv, i);
+		}
+	}
+}
+
+static void stm32_clk_register(void)
+{
+	clk_register(&stm32mp_clk_ops);
+}
+
+uint32_t clk_stm32_div_get_value(struct stm32_clk_priv *priv, int div_id)
+{
+	const struct div_cfg *divider = &priv->div[div_id];
+	uint32_t val = 0;
+
+	val = mmio_read_32(priv->base + divider->offset) >> divider->shift;
+	val &= clk_div_mask(divider->width);
+
+	return val;
+}
+
+unsigned long _clk_stm32_divider_recalc(struct stm32_clk_priv *priv,
+					int div_id,
+					unsigned long prate)
+{
+	const struct div_cfg *divider = &priv->div[div_id];
+	uint32_t val = clk_stm32_div_get_value(priv, div_id);
+	unsigned int div = 0U;
+
+	div = _get_div(divider->table, val, divider->flags, divider->width);
+	if (div == 0U) {
+		return prate;
+	}
+
+	return div_round_up((uint64_t)prate, div);
+}
+
+unsigned long clk_stm32_divider_recalc(struct stm32_clk_priv *priv, int id,
+				       unsigned long prate)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct clk_stm32_div_cfg *div_cfg = clk->clock_cfg;
+
+	return _clk_stm32_divider_recalc(priv, div_cfg->id, prate);
+}
+
+const struct stm32_clk_ops clk_stm32_divider_ops = {
+	.recalc_rate	= clk_stm32_divider_recalc,
+};
+
+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;
+}
+
+int _clk_stm32_gate_wait_ready(struct stm32_clk_priv *priv, uint16_t gate_id,
+			       bool ready_on)
+{
+	const struct gate_cfg *gate = &priv->gates[gate_id];
+	uintptr_t address = priv->base + gate->offset;
+	uint32_t mask_rdy = BIT(gate->bit_idx);
+	uint64_t timeout;
+	uint32_t mask_test;
+
+	if (ready_on) {
+		mask_test = BIT(gate->bit_idx);
+	} else {
+		mask_test = 0U;
+	}
+
+	timeout = timeout_init_us(OSCRDY_TIMEOUT);
+
+	while ((mmio_read_32(address) & mask_rdy) != mask_test) {
+		if (timeout_elapsed(timeout)) {
+			break;
+		}
+	}
+
+	if ((mmio_read_32(address) & mask_rdy) != mask_test) {
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+int clk_stm32_gate_enable(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct clk_stm32_gate_cfg *cfg = clk->clock_cfg;
+	const struct gate_cfg *gate = &priv->gates[cfg->id];
+	uintptr_t addr = priv->base + gate->offset;
+
+	if (gate->set_clr != 0U) {
+		mmio_write_32(addr, BIT(gate->bit_idx));
+
+	} else {
+		mmio_setbits_32(addr, BIT(gate->bit_idx));
+	}
+
+	return 0;
+}
+
+void clk_stm32_gate_disable(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct clk_stm32_gate_cfg *cfg = clk->clock_cfg;
+	const struct gate_cfg *gate = &priv->gates[cfg->id];
+	uintptr_t addr = priv->base + gate->offset;
+
+	if (gate->set_clr != 0U) {
+		mmio_write_32(addr + RCC_MP_ENCLRR_OFFSET, BIT(gate->bit_idx));
+	} else {
+		mmio_clrbits_32(addr, BIT(gate->bit_idx));
+	}
+}
+
+bool _clk_stm32_gate_is_enabled(struct stm32_clk_priv *priv, int gate_id)
+{
+	const struct gate_cfg *gate;
+	uint32_t addr;
+
+	gate = &priv->gates[gate_id];
+	addr = priv->base + gate->offset;
+
+	return ((mmio_read_32(addr) & BIT(gate->bit_idx)) != 0U);
+}
+
+bool clk_stm32_gate_is_enabled(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct clk_stm32_gate_cfg *cfg = clk->clock_cfg;
+
+	return _clk_stm32_gate_is_enabled(priv, cfg->id);
+}
+
+const struct stm32_clk_ops clk_stm32_gate_ops = {
+	.enable		= clk_stm32_gate_enable,
+	.disable	= clk_stm32_gate_disable,
+	.is_enabled	= clk_stm32_gate_is_enabled,
+};
+
+const struct stm32_clk_ops clk_fixed_factor_ops = {
+	.recalc_rate	= fixed_factor_recalc_rate,
+};
+
+unsigned long fixed_factor_recalc_rate(struct stm32_clk_priv *priv,
+				       int id, unsigned long prate)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	const struct fixed_factor_cfg *cfg = clk->clock_cfg;
+	unsigned long long rate;
+
+	rate = (unsigned long long)prate * cfg->mult;
+
+	if (cfg->div == 0U) {
+		ERROR("division by zero\n");
+		panic();
+	}
+
+	return (unsigned long)(rate / cfg->div);
+};
+
+#define APB_DIV_MASK	GENMASK(2, 0)
+#define TIM_PRE_MASK	BIT(0)
+
+static unsigned long timer_recalc_rate(struct stm32_clk_priv *priv,
+				       int id, unsigned long prate)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	const struct clk_timer_cfg *cfg = clk->clock_cfg;
+	uint32_t prescaler, timpre;
+	uintptr_t rcc_base = priv->base;
+
+	prescaler = mmio_read_32(rcc_base + cfg->apbdiv) &
+		APB_DIV_MASK;
+
+	timpre = mmio_read_32(rcc_base + cfg->timpre) &
+		TIM_PRE_MASK;
+
+	if (prescaler == 0U) {
+		return prate;
+	}
+
+	return prate * (timpre + 1U) * 2U;
+};
+
+const struct stm32_clk_ops clk_timer_ops = {
+	.recalc_rate	= timer_recalc_rate,
+};
+
+static unsigned long clk_fixed_rate_recalc(struct stm32_clk_priv *priv, int id,
+					   unsigned long prate)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct clk_stm32_fixed_rate_cfg *cfg = clk->clock_cfg;
+
+	return cfg->rate;
+}
+
+const struct stm32_clk_ops clk_stm32_fixed_rate_ops = {
+	.recalc_rate	= clk_fixed_rate_recalc,
+};
+
+static unsigned long clk_stm32_osc_recalc_rate(struct stm32_clk_priv *priv,
+					       int id, unsigned long prate)
+{
+	struct clk_oscillator_data *osc_data = clk_oscillator_get_data(priv, id);
+
+	return osc_data->frequency;
+};
+
+bool clk_stm32_osc_gate_is_enabled(struct stm32_clk_priv *priv, int id)
+{
+	struct clk_oscillator_data *osc_data = clk_oscillator_get_data(priv, id);
+
+	return _clk_stm32_gate_is_enabled(priv, osc_data->gate_id);
+
+}
+
+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);
+
+	_clk_stm32_gate_enable(priv, osc_data->gate_id);
+
+	if (_clk_stm32_gate_wait_ready(priv, osc_data->gate_rdy_id, true) != 0U) {
+		ERROR("%s: %s (%d)\n", __func__, osc_data->name, __LINE__);
+		panic();
+	}
+
+	return 0;
+}
+
+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);
+
+	_clk_stm32_gate_disable(priv, osc_data->gate_id);
+
+	if (_clk_stm32_gate_wait_ready(priv, osc_data->gate_rdy_id, false) != 0U) {
+		ERROR("%s: %s (%d)\n", __func__, osc_data->name, __LINE__);
+		panic();
+	}
+}
+
+static unsigned long clk_stm32_get_dt_oscillator_frequency(const char *name)
+{
+	void *fdt = NULL;
+	int node = 0;
+	int subnode = 0;
+
+	if (fdt_get_address(&fdt) == 0) {
+		panic();
+	}
+
+	node = fdt_path_offset(fdt, "/clocks");
+	if (node < 0) {
+		return 0UL;
+	}
+
+	fdt_for_each_subnode(subnode, fdt, node) {
+		const char *cchar = NULL;
+		const fdt32_t *cuint = NULL;
+		int ret = 0;
+
+		cchar = fdt_get_name(fdt, subnode, &ret);
+		if (cchar == NULL) {
+			continue;
+		}
+
+		if (strncmp(cchar, name, (size_t)ret) ||
+		    fdt_get_status(subnode) == DT_DISABLED) {
+			continue;
+		}
+
+		cuint = fdt_getprop(fdt, subnode, "clock-frequency", &ret);
+		if (cuint == NULL) {
+			return 0UL;
+		}
+
+		return fdt32_to_cpu(*cuint);
+	}
+
+	return 0UL;
+}
+
+void clk_stm32_osc_init(struct stm32_clk_priv *priv, int id)
+{
+	struct clk_oscillator_data *osc_data = clk_oscillator_get_data(priv, id);
+	const char *name = osc_data->name;
+
+	osc_data->frequency = clk_stm32_get_dt_oscillator_frequency(name);
+}
+
+const struct stm32_clk_ops clk_stm32_osc_ops = {
+	.recalc_rate	= clk_stm32_osc_recalc_rate,
+	.is_enabled	= clk_stm32_osc_gate_is_enabled,
+	.enable		= clk_stm32_osc_gate_enable,
+	.disable	= clk_stm32_osc_gate_disable,
+	.init		= clk_stm32_osc_init,
+};
+
+const struct stm32_clk_ops clk_stm32_osc_nogate_ops = {
+	.recalc_rate	= clk_stm32_osc_recalc_rate,
+	.init		= clk_stm32_osc_init,
+};
+
+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 = 0; i < ((uint32_t)len / sizeof(uint32_t)); i++) {
+		uint32_t val = fdt32_to_cpu(cell[i]);
+
+		tab[i] = val;
+	}
+
+	*nb = (uint32_t)len / sizeof(uint32_t);
+
+	return 0;
+}
+
+int clk_stm32_init(struct stm32_clk_priv *priv, uintptr_t base)
+{
+	unsigned int i;
+
+	stm32_clock_data = priv;
+
+	priv->base = base;
+
+	for (i = 0U; i < priv->num; i++) {
+		const struct clk_stm32 *clk = _clk_get(priv, i);
+
+		assert(clk->ops != NULL);
+
+		if (clk->ops->init != NULL) {
+			clk->ops->init(priv, i);
+		}
+	}
+
+	stm32_clk_register();
+
+	return 0;
+}
diff --git a/drivers/st/clk/clk-stm32-core.h b/drivers/st/clk/clk-stm32-core.h
new file mode 100644
index 0000000..8bfb513
--- /dev/null
+++ b/drivers/st/clk/clk-stm32-core.h
@@ -0,0 +1,393 @@
+/*
+ * Copyright (C) 2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+ */
+
+#ifndef CLK_STM32_CORE_H
+#define CLK_STM32_CORE_H
+
+struct mux_cfg {
+	uint16_t offset;
+	uint8_t shift;
+	uint8_t width;
+	uint8_t bitrdy;
+};
+
+struct gate_cfg {
+	uint16_t offset;
+	uint8_t bit_idx;
+	uint8_t set_clr;
+};
+
+struct clk_div_table {
+	unsigned int val;
+	unsigned int div;
+};
+
+struct div_cfg {
+	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;
+};
+
+struct stm32_clk_priv;
+
+struct stm32_clk_ops {
+	unsigned long (*recalc_rate)(struct stm32_clk_priv *priv, int id, unsigned long rate);
+	int (*get_parent)(struct stm32_clk_priv *priv, int id);
+	int (*set_rate)(struct stm32_clk_priv *priv, int id, unsigned long rate,
+			unsigned long prate);
+	int (*enable)(struct stm32_clk_priv *priv, int id);
+	void (*disable)(struct stm32_clk_priv *priv, int id);
+	bool (*is_enabled)(struct stm32_clk_priv *priv, int id);
+	void (*init)(struct stm32_clk_priv *priv, int id);
+};
+
+struct clk_stm32 {
+	uint16_t binding;
+	uint16_t parent;
+	uint8_t flags;
+	void *clock_cfg;
+	const struct stm32_clk_ops *ops;
+};
+
+struct stm32_clk_priv {
+	uintptr_t base;
+	const uint32_t num;
+	const struct clk_stm32 *clks;
+	const struct parent_cfg *parents;
+	const uint32_t nb_parents;
+	const struct gate_cfg *gates;
+	const uint32_t nb_gates;
+	const struct div_cfg *div;
+	const uint32_t nb_div;
+	struct clk_oscillator_data *osci_data;
+	const uint32_t nb_osci_data;
+	uint32_t *gate_refcounts;
+	void *pdata;
+};
+
+struct stm32_clk_bypass {
+	uint16_t offset;
+	uint8_t bit_byp;
+	uint8_t bit_digbyp;
+};
+
+struct stm32_clk_css {
+	uint16_t offset;
+	uint8_t bit_css;
+};
+
+struct stm32_clk_drive {
+	uint16_t offset;
+	uint8_t drv_shift;
+	uint8_t drv_width;
+	uint8_t drv_default;
+};
+
+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;
+};
+
+struct clk_fixed_rate {
+	const char *name;
+	unsigned long fixed_rate;
+};
+
+struct clk_gate_cfg {
+	uint32_t offset;
+	uint8_t bit_idx;
+};
+
+/* CLOCK FLAGS */
+#define CLK_IS_CRITICAL			BIT(0)
+#define CLK_IGNORE_UNUSED		BIT(1)
+#define CLK_SET_RATE_PARENT		BIT(2)
+
+#define CLK_DIVIDER_ONE_BASED		BIT(0)
+#define CLK_DIVIDER_POWER_OF_TWO	BIT(1)
+#define CLK_DIVIDER_ALLOW_ZERO		BIT(2)
+#define CLK_DIVIDER_HIWORD_MASK		BIT(3)
+#define CLK_DIVIDER_ROUND_CLOSEST	BIT(4)
+#define CLK_DIVIDER_READ_ONLY		BIT(5)
+#define CLK_DIVIDER_MAX_AT_ZERO		BIT(6)
+#define CLK_DIVIDER_BIG_ENDIAN		BIT(7)
+
+#define MUX_MAX_PARENTS			U(0x8000)
+#define MUX_PARENT_MASK			GENMASK(14, 0)
+#define MUX_FLAG			U(0x8000)
+#define MUX(mux)			((mux) | MUX_FLAG)
+
+#define NO_GATE				0
+#define _NO_ID				UINT16_MAX
+#define CLK_IS_ROOT			UINT16_MAX
+#define MUX_NO_BIT_RDY			UINT8_MAX
+#define DIV_NO_BIT_RDY			UINT8_MAX
+
+#define MASK_WIDTH_SHIFT(_width, _shift) \
+	GENMASK(((_width) + (_shift) - 1U), (_shift))
+
+int clk_stm32_init(struct stm32_clk_priv *priv, uintptr_t base);
+void clk_stm32_enable_critical_clocks(void);
+
+struct stm32_clk_priv *clk_stm32_get_priv(void);
+
+int clk_get_index(struct stm32_clk_priv *priv, unsigned long binding_id);
+const struct clk_stm32 *_clk_get(struct stm32_clk_priv *priv, int id);
+
+void clk_oscillator_set_bypass(struct stm32_clk_priv *priv, int id, bool digbyp, bool bypass);
+void clk_oscillator_set_drive(struct stm32_clk_priv *priv, int id, uint8_t lsedrv);
+void clk_oscillator_set_css(struct stm32_clk_priv *priv, int id, bool css);
+
+int _clk_stm32_gate_wait_ready(struct stm32_clk_priv *priv, uint16_t gate_id, bool ready_on);
+
+int clk_oscillator_wait_ready(struct stm32_clk_priv *priv, int id, bool ready_on);
+int clk_oscillator_wait_ready_on(struct stm32_clk_priv *priv, int id);
+int clk_oscillator_wait_ready_off(struct stm32_clk_priv *priv, int id);
+
+int clk_stm32_get_counter(unsigned long binding_id);
+
+void _clk_stm32_gate_disable(struct stm32_clk_priv *priv, uint16_t gate_id);
+int _clk_stm32_gate_enable(struct stm32_clk_priv *priv, uint16_t gate_id);
+
+int _clk_stm32_set_parent(struct stm32_clk_priv *priv, int id, int src_id);
+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 id);
+int _clk_stm32_get_parent_by_index(struct stm32_clk_priv *priv, int clk_id, int idx);
+int _clk_stm32_get_parent_index(struct stm32_clk_priv *priv, int clk_id);
+
+unsigned long _clk_stm32_get_rate(struct stm32_clk_priv *priv, int id);
+unsigned long _clk_stm32_get_parent_rate(struct stm32_clk_priv *priv, int id);
+
+bool _stm32_clk_is_flags(struct stm32_clk_priv *priv, int id, uint8_t flag);
+
+int _clk_stm32_enable(struct stm32_clk_priv *priv, int id);
+void _clk_stm32_disable(struct stm32_clk_priv *priv, int id);
+
+int clk_stm32_enable_call_ops(struct stm32_clk_priv *priv, uint16_t id);
+void clk_stm32_disable_call_ops(struct stm32_clk_priv *priv, uint16_t id);
+
+bool _clk_stm32_is_enabled(struct stm32_clk_priv *priv, int id);
+
+int _clk_stm32_divider_set_rate(struct stm32_clk_priv *priv, int div_id,
+				unsigned long rate, unsigned long parent_rate);
+
+int clk_stm32_divider_set_rate(struct stm32_clk_priv *priv, int id, unsigned long rate,
+			       unsigned long prate);
+
+unsigned long _clk_stm32_divider_recalc(struct stm32_clk_priv *priv,
+					int div_id,
+					unsigned long prate);
+
+unsigned long clk_stm32_divider_recalc(struct stm32_clk_priv *priv, int idx,
+				       unsigned long prate);
+
+int clk_stm32_gate_enable(struct stm32_clk_priv *priv, int idx);
+void clk_stm32_gate_disable(struct stm32_clk_priv *priv, int idx);
+
+bool _clk_stm32_gate_is_enabled(struct stm32_clk_priv *priv, int gate_id);
+bool clk_stm32_gate_is_enabled(struct stm32_clk_priv *priv, int idx);
+
+uint32_t clk_stm32_div_get_value(struct stm32_clk_priv *priv, int div_id);
+int clk_stm32_set_div(struct stm32_clk_priv *priv, uint32_t div_id, uint32_t value);
+int clk_mux_set_parent(struct stm32_clk_priv *priv, uint16_t pid, uint8_t sel);
+int clk_mux_get_parent(struct stm32_clk_priv *priv, uint32_t mux_id);
+
+int stm32_clk_parse_fdt_by_name(void *fdt, int node, const char *name, uint32_t *tab, uint32_t *nb);
+
+#ifdef CFG_STM32_CLK_DEBUG
+void clk_stm32_display_clock_info(void);
+#endif
+
+struct clk_stm32_div_cfg {
+	int id;
+};
+
+#define STM32_DIV(idx, _binding, _parent, _flags, _div_id) \
+	[(idx)] = (struct clk_stm32){ \
+		.binding	= (_binding),\
+		.parent		=  (_parent),\
+		.flags		= (_flags),\
+		.clock_cfg	= &(struct clk_stm32_div_cfg){\
+			.id	= (_div_id),\
+		},\
+		.ops		= &clk_stm32_divider_ops,\
+	}
+
+struct clk_stm32_gate_cfg {
+	int id;
+};
+
+#define STM32_GATE(idx, _binding, _parent, _flags, _gate_id) \
+	[(idx)] = (struct clk_stm32){ \
+		.binding	= (_binding),\
+		.parent		=  (_parent),\
+		.flags		= (_flags),\
+		.clock_cfg	= &(struct clk_stm32_gate_cfg){\
+			.id	= (_gate_id),\
+		},\
+		.ops		= &clk_stm32_gate_ops,\
+	}
+
+struct fixed_factor_cfg {
+	unsigned int mult;
+	unsigned int div;
+};
+
+unsigned long fixed_factor_recalc_rate(struct stm32_clk_priv *priv,
+				       int _idx, unsigned long prate);
+
+#define FIXED_FACTOR(idx, _idx, _parent, _mult, _div) \
+	[(idx)] = (struct clk_stm32){ \
+		.binding	= (_idx),\
+		.parent		= (_parent),\
+		.clock_cfg	= &(struct fixed_factor_cfg){\
+			.mult	= (_mult),\
+			.div	= (_div),\
+		},\
+		.ops		= &clk_fixed_factor_ops,\
+	}
+
+#define GATE(idx, _binding, _parent, _flags, _offset, _bit_idx) \
+	[(idx)] = (struct clk_stm32){ \
+		.binding	= (_binding),\
+		.parent		=  (_parent),\
+		.flags		= (_flags),\
+		.clock_cfg	= &(struct clk_gate_cfg){\
+			.offset		= (_offset),\
+			.bit_idx	= (_bit_idx),\
+		},\
+		.ops		= &clk_gate_ops,\
+	}
+
+#define STM32_MUX(idx, _binding, _mux_id, _flags) \
+	[(idx)] = (struct clk_stm32){ \
+		.binding	= (_binding),\
+		.parent		= (MUX(_mux_id)),\
+		.flags		= (_flags),\
+		.clock_cfg	= NULL,\
+		.ops		= (&clk_mux_ops),\
+	}
+
+struct clk_timer_cfg {
+	uint32_t apbdiv;
+	uint32_t timpre;
+};
+
+#define CK_TIMER(idx, _idx, _parent, _flags, _apbdiv, _timpre) \
+	[(idx)] = (struct clk_stm32){ \
+		.binding	= (_idx),\
+		.parent		= (_parent),\
+		.flags		= (CLK_SET_RATE_PARENT | (_flags)),\
+		.clock_cfg	= &(struct clk_timer_cfg){\
+			.apbdiv = (_apbdiv),\
+			.timpre = (_timpre),\
+		},\
+		.ops		= &clk_timer_ops,\
+	}
+
+struct clk_stm32_fixed_rate_cfg {
+	unsigned long rate;
+};
+
+#define CLK_FIXED_RATE(idx, _binding, _rate) \
+	[(idx)] = (struct clk_stm32){ \
+		.binding	= (_binding),\
+		.parent		= (CLK_IS_ROOT),\
+		.clock_cfg	= &(struct clk_stm32_fixed_rate_cfg){\
+			.rate	= (_rate),\
+		},\
+		.ops		= &clk_stm32_fixed_rate_ops,\
+	}
+
+#define BYPASS(_offset, _bit_byp, _bit_digbyp) &(struct stm32_clk_bypass){\
+	.offset		= (_offset),\
+	.bit_byp	= (_bit_byp),\
+	.bit_digbyp	= (_bit_digbyp),\
+}
+
+#define CSS(_offset, _bit_css)	&(struct stm32_clk_css){\
+	.offset		= (_offset),\
+	.bit_css	= (_bit_css),\
+}
+
+#define DRIVE(_offset, _shift, _width, _default) &(struct stm32_clk_drive){\
+	.offset		= (_offset),\
+	.drv_shift	= (_shift),\
+	.drv_width	= (_width),\
+	.drv_default	= (_default),\
+}
+
+#define OSCILLATOR(idx_osc, _id, _name, _gate_id, _gate_rdy_id, _bypass, _css, _drive) \
+	[(idx_osc)] = (struct clk_oscillator_data){\
+		.name		= (_name),\
+		.id_clk		= (_id),\
+		.gate_id	= (_gate_id),\
+		.gate_rdy_id	= (_gate_rdy_id),\
+		.bypass		= (_bypass),\
+		.css		= (_css),\
+		.drive		= (_drive),\
+	}
+
+struct clk_oscillator_data *clk_oscillator_get_data(struct stm32_clk_priv *priv, int id);
+
+void clk_stm32_osc_init(struct stm32_clk_priv *priv, int id);
+bool clk_stm32_osc_gate_is_enabled(struct stm32_clk_priv *priv, int id);
+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;
+};
+
+#define CLK_OSC(idx, _idx, _parent, _osc_id) \
+	[(idx)] = (struct clk_stm32){ \
+		.binding	= (_idx),\
+		.parent		= (_parent),\
+		.flags		= CLK_IS_CRITICAL,\
+		.clock_cfg	= &(struct stm32_osc_cfg){\
+			.osc_id = (_osc_id),\
+		},\
+		.ops		= &clk_stm32_osc_ops,\
+	}
+
+#define CLK_OSC_FIXED(idx, _idx, _parent, _osc_id) \
+	[(idx)] = (struct clk_stm32){ \
+		.binding	= (_idx),\
+		.parent		= (_parent),\
+		.flags		= CLK_IS_CRITICAL,\
+		.clock_cfg	= &(struct stm32_osc_cfg){\
+			.osc_id	= (_osc_id),\
+		},\
+		.ops		= &clk_stm32_osc_nogate_ops,\
+	}
+
+extern const struct stm32_clk_ops clk_mux_ops;
+extern const struct stm32_clk_ops clk_stm32_divider_ops;
+extern const struct stm32_clk_ops clk_stm32_gate_ops;
+extern const struct stm32_clk_ops clk_fixed_factor_ops;
+extern const struct stm32_clk_ops clk_gate_ops;
+extern const struct stm32_clk_ops clk_timer_ops;
+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;
+
+#endif /* CLK_STM32_CORE_H */
diff --git a/drivers/st/clk/clk-stm32mp13.c b/drivers/st/clk/clk-stm32mp13.c
new file mode 100644
index 0000000..01d1764
--- /dev/null
+++ b/drivers/st/clk/clk-stm32mp13.c
@@ -0,0 +1,2332 @@
+/*
+ * Copyright (C) 2022-2023, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <limits.h>
+#include <stdint.h>
+#include <stdio.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include "clk-stm32-core.h"
+#include <common/debug.h>
+#include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32mp13_rcc.h>
+#include <drivers/st/stm32mp1_clk.h>
+#include <drivers/st/stm32mp_clkfunc.h>
+#include <dt-bindings/clock/stm32mp13-clksrc.h>
+#include <lib/mmio.h>
+#include <lib/spinlock.h>
+#include <lib/utils_def.h>
+#include <libfdt.h>
+#include <plat/common/platform.h>
+
+#include <platform_def.h>
+
+struct stm32_osci_dt_cfg {
+	unsigned long freq;
+	bool bypass;
+	bool digbyp;
+	bool css;
+	uint32_t drive;
+};
+
+enum pll_mn {
+	PLL_CFG_M,
+	PLL_CFG_N,
+	PLL_DIV_MN_NB
+};
+
+enum pll_pqr {
+	PLL_CFG_P,
+	PLL_CFG_Q,
+	PLL_CFG_R,
+	PLL_DIV_PQR_NB
+};
+
+enum pll_csg {
+	PLL_CSG_MOD_PER,
+	PLL_CSG_INC_STEP,
+	PLL_CSG_SSCG_MODE,
+	PLL_CSG_NB
+};
+
+struct stm32_pll_vco {
+	uint32_t status;
+	uint32_t src;
+	uint32_t div_mn[PLL_DIV_MN_NB];
+	uint32_t frac;
+	bool csg_enabled;
+	uint32_t csg[PLL_CSG_NB];
+};
+
+struct stm32_pll_output {
+	uint32_t output[PLL_DIV_PQR_NB];
+};
+
+struct stm32_pll_dt_cfg {
+	struct stm32_pll_vco vco;
+	struct stm32_pll_output output;
+};
+
+struct stm32_clk_platdata {
+	uint32_t nosci;
+	struct stm32_osci_dt_cfg *osci;
+	uint32_t npll;
+	struct stm32_pll_dt_cfg *pll;
+	uint32_t nclksrc;
+	uint32_t *clksrc;
+	uint32_t nclkdiv;
+	uint32_t *clkdiv;
+};
+
+enum stm32_clock {
+	/* ROOT CLOCKS */
+	_CK_OFF,
+	_CK_HSI,
+	_CK_HSE,
+	_CK_CSI,
+	_CK_LSI,
+	_CK_LSE,
+	_I2SCKIN,
+	_CSI_DIV122,
+	_HSE_DIV,
+	_HSE_DIV2,
+	_CK_PLL1,
+	_CK_PLL2,
+	_CK_PLL3,
+	_CK_PLL4,
+	_PLL1P,
+	_PLL1P_DIV,
+	_PLL2P,
+	_PLL2Q,
+	_PLL2R,
+	_PLL3P,
+	_PLL3Q,
+	_PLL3R,
+	_PLL4P,
+	_PLL4Q,
+	_PLL4R,
+	_PCLK1,
+	_PCLK2,
+	_PCLK3,
+	_PCLK4,
+	_PCLK5,
+	_PCLK6,
+	_CKMPU,
+	_CKAXI,
+	_CKMLAHB,
+	_CKPER,
+	_CKTIMG1,
+	_CKTIMG2,
+	_CKTIMG3,
+	_USB_PHY_48,
+	_MCO1_K,
+	_MCO2_K,
+	_TRACECK,
+	/* BUS and KERNEL CLOCKS */
+	_DDRC1,
+	_DDRC1LP,
+	_DDRPHYC,
+	_DDRPHYCLP,
+	_DDRCAPB,
+	_DDRCAPBLP,
+	_AXIDCG,
+	_DDRPHYCAPB,
+	_DDRPHYCAPBLP,
+	_SYSCFG,
+	_DDRPERFM,
+	_IWDG2APB,
+	_USBPHY_K,
+	_USBO_K,
+	_RTCAPB,
+	_TZC,
+	_ETZPC,
+	_IWDG1APB,
+	_BSEC,
+	_STGENC,
+	_USART1_K,
+	_USART2_K,
+	_I2C3_K,
+	_I2C4_K,
+	_I2C5_K,
+	_TIM12,
+	_TIM15,
+	_RTCCK,
+	_GPIOA,
+	_GPIOB,
+	_GPIOC,
+	_GPIOD,
+	_GPIOE,
+	_GPIOF,
+	_GPIOG,
+	_GPIOH,
+	_GPIOI,
+	_PKA,
+	_SAES_K,
+	_CRYP1,
+	_HASH1,
+	_RNG1_K,
+	_BKPSRAM,
+	_SDMMC1_K,
+	_SDMMC2_K,
+	_DBGCK,
+	_USART3_K,
+	_UART4_K,
+	_UART5_K,
+	_UART7_K,
+	_UART8_K,
+	_USART6_K,
+	_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
+};
+
+/* PARENT CONFIG */
+static const uint16_t RTC_src[] = {
+	 _CK_OFF, _CK_LSE, _CK_LSI, _CK_HSE
+};
+
+static const uint16_t MCO1_src[] = {
+	 _CK_HSI, _CK_HSE, _CK_CSI, _CK_LSI, _CK_LSE
+};
+
+static const uint16_t MCO2_src[] = {
+	 _CKMPU, _CKAXI, _CKMLAHB, _PLL4P, _CK_HSE, _CK_HSI
+};
+
+static const uint16_t PLL12_src[] = {
+	 _CK_HSI, _CK_HSE
+};
+
+static const uint16_t PLL3_src[] = {
+	 _CK_HSI, _CK_HSE, _CK_CSI
+};
+
+static const uint16_t PLL4_src[] = {
+	 _CK_HSI, _CK_HSE, _CK_CSI, _I2SCKIN
+};
+
+static const uint16_t MPU_src[] = {
+	 _CK_HSI, _CK_HSE, _PLL1P, _PLL1P_DIV
+};
+
+static const uint16_t AXI_src[] = {
+	 _CK_HSI, _CK_HSE, _PLL2P
+};
+
+static const uint16_t MLAHBS_src[] = {
+	 _CK_HSI, _CK_HSE, _CK_CSI, _PLL3P
+};
+
+static const uint16_t CKPER_src[] = {
+	 _CK_HSI, _CK_CSI, _CK_HSE, _CK_OFF
+};
+
+static const uint16_t I2C12_src[] = {
+	 _PCLK1, _PLL4R, _CK_HSI, _CK_CSI
+};
+
+static const uint16_t I2C3_src[] = {
+	 _PCLK6, _PLL4R, _CK_HSI, _CK_CSI
+};
+
+static const uint16_t I2C4_src[] = {
+	 _PCLK6, _PLL4R, _CK_HSI, _CK_CSI
+};
+
+static const uint16_t I2C5_src[] = {
+	 _PCLK6, _PLL4R, _CK_HSI, _CK_CSI
+};
+
+static const uint16_t SPI1_src[] = {
+	 _PLL4P, _PLL3Q, _I2SCKIN, _CKPER, _PLL3R
+};
+
+static const uint16_t SPI23_src[] = {
+	 _PLL4P, _PLL3Q, _I2SCKIN, _CKPER, _PLL3R
+};
+
+static const uint16_t SPI4_src[] = {
+	 _PCLK6, _PLL4Q, _CK_HSI, _CK_CSI, _CK_HSE, _I2SCKIN
+};
+
+static const uint16_t SPI5_src[] = {
+	 _PCLK6, _PLL4Q, _CK_HSI, _CK_CSI, _CK_HSE
+};
+
+static const uint16_t UART1_src[] = {
+	 _PCLK6, _PLL3Q, _CK_HSI, _CK_CSI, _PLL4Q, _CK_HSE
+};
+
+static const uint16_t UART2_src[] = {
+	 _PCLK6, _PLL3Q, _CK_HSI, _CK_CSI, _PLL4Q, _CK_HSE
+};
+
+static const uint16_t UART35_src[] = {
+	 _PCLK1, _PLL4Q, _CK_HSI, _CK_CSI, _CK_HSE
+};
+
+static const uint16_t UART4_src[] = {
+	 _PCLK1, _PLL4Q, _CK_HSI, _CK_CSI, _CK_HSE
+};
+
+static const uint16_t UART6_src[] = {
+	 _PCLK2, _PLL4Q, _CK_HSI, _CK_CSI, _CK_HSE
+};
+
+static const uint16_t UART78_src[] = {
+	 _PCLK1, _PLL4Q, _CK_HSI, _CK_CSI, _CK_HSE
+};
+
+static const uint16_t LPTIM1_src[] = {
+	 _PCLK1, _PLL4P, _PLL3Q, _CK_LSE, _CK_LSI, _CKPER
+};
+
+static const uint16_t LPTIM2_src[] = {
+	 _PCLK3, _PLL4Q, _CKPER, _CK_LSE, _CK_LSI
+};
+
+static const uint16_t LPTIM3_src[] = {
+	 _PCLK3, _PLL4Q, _CKPER, _CK_LSE, _CK_LSI
+};
+
+static const uint16_t LPTIM45_src[] = {
+	 _PCLK3, _PLL4P, _PLL3Q, _CK_LSE, _CK_LSI, _CKPER
+};
+
+static const uint16_t SAI1_src[] = {
+	 _PLL4Q, _PLL3Q, _I2SCKIN, _CKPER, _PLL3R
+};
+
+static const uint16_t SAI2_src[] = {
+	 _PLL4Q, _PLL3Q, _I2SCKIN, _CKPER, _NO_ID, _PLL3R
+};
+
+static const uint16_t FDCAN_src[] = {
+	 _CK_HSE, _PLL3Q, _PLL4Q, _PLL4R
+};
+
+static const uint16_t SPDIF_src[] = {
+	 _PLL4P, _PLL3Q, _CK_HSI
+};
+
+static const uint16_t ADC1_src[] = {
+	 _PLL4R, _CKPER, _PLL3Q
+};
+
+static const uint16_t ADC2_src[] = {
+	 _PLL4R, _CKPER, _PLL3Q
+};
+
+static const uint16_t SDMMC1_src[] = {
+	 _CKAXI, _PLL3R, _PLL4P, _CK_HSI
+};
+
+static const uint16_t SDMMC2_src[] = {
+	 _CKAXI, _PLL3R, _PLL4P, _CK_HSI
+};
+
+static const uint16_t ETH1_src[] = {
+	 _PLL4P, _PLL3Q
+};
+
+static const uint16_t ETH2_src[] = {
+	 _PLL4P, _PLL3Q
+};
+
+static const uint16_t USBPHY_src[] = {
+	 _CK_HSE, _PLL4R, _HSE_DIV2
+};
+
+static const uint16_t USBO_src[] = {
+	 _PLL4R, _USB_PHY_48
+};
+
+static const uint16_t QSPI_src[] = {
+	 _CKAXI, _PLL3R, _PLL4P, _CKPER
+};
+
+static const uint16_t FMC_src[] = {
+	 _CKAXI, _PLL3R, _PLL4P, _CKPER
+};
+
+/* Position 2 of RNG1 mux is reserved */
+static const uint16_t RNG1_src[] = {
+	 _CK_CSI, _PLL4R, _CK_OFF, _CK_LSI
+};
+
+static const uint16_t STGEN_src[] = {
+	 _CK_HSI, _CK_HSE
+};
+
+static const uint16_t DCMIPP_src[] = {
+	 _CKAXI, _PLL2Q, _PLL4P, _CKPER
+};
+
+static const uint16_t SAES_src[] = {
+	 _CKAXI, _CKPER, _PLL4R, _CK_LSI
+};
+
+#define MUX_CFG(id, src, _offset, _shift, _witdh)[id] = {\
+	.id_parents	= src,\
+	.num_parents	= ARRAY_SIZE(src),\
+	.mux		= &(struct mux_cfg) {\
+		.offset	= (_offset),\
+		.shift	= (_shift),\
+		.width	= (_witdh),\
+		.bitrdy = MUX_NO_BIT_RDY,\
+	},\
+}
+
+#define MUX_RDY_CFG(id, src, _offset, _shift, _witdh)[id] = {\
+	.id_parents	= src,\
+	.num_parents	= ARRAY_SIZE(src),\
+	.mux		= &(struct mux_cfg) {\
+		.offset	= (_offset),\
+		.shift	= (_shift),\
+		.width	= (_witdh),\
+		.bitrdy = 31,\
+	},\
+}
+
+static const struct parent_cfg parent_mp13[MUX_MAX] = {
+	MUX_CFG(MUX_ADC1,	ADC1_src,	RCC_ADC12CKSELR, 0, 2),
+	MUX_CFG(MUX_ADC2,	ADC2_src,	RCC_ADC12CKSELR, 2, 2),
+	MUX_RDY_CFG(MUX_AXI,	AXI_src,	RCC_ASSCKSELR, 0, 3),
+	MUX_CFG(MUX_CKPER,	CKPER_src,	RCC_CPERCKSELR, 0, 2),
+	MUX_CFG(MUX_DCMIPP,	DCMIPP_src,	RCC_DCMIPPCKSELR, 0, 2),
+	MUX_CFG(MUX_ETH1,	ETH1_src,	RCC_ETH12CKSELR, 0, 2),
+	MUX_CFG(MUX_ETH2,	ETH2_src,	RCC_ETH12CKSELR, 8, 2),
+	MUX_CFG(MUX_FDCAN,	FDCAN_src,	RCC_FDCANCKSELR, 0, 2),
+	MUX_CFG(MUX_FMC,	FMC_src,	RCC_FMCCKSELR, 0, 2),
+	MUX_CFG(MUX_I2C12,	I2C12_src,	RCC_I2C12CKSELR, 0, 3),
+	MUX_CFG(MUX_I2C3,	I2C3_src,	RCC_I2C345CKSELR, 0, 3),
+	MUX_CFG(MUX_I2C4,	I2C4_src,	RCC_I2C345CKSELR, 3, 3),
+	MUX_CFG(MUX_I2C5,	I2C5_src,	RCC_I2C345CKSELR, 6, 3),
+	MUX_CFG(MUX_LPTIM1,	LPTIM1_src,	RCC_LPTIM1CKSELR, 0, 3),
+	MUX_CFG(MUX_LPTIM2,	LPTIM2_src,	RCC_LPTIM23CKSELR, 0, 3),
+	MUX_CFG(MUX_LPTIM3,	LPTIM3_src,	RCC_LPTIM23CKSELR, 3, 3),
+	MUX_CFG(MUX_LPTIM45,	LPTIM45_src,	RCC_LPTIM45CKSELR, 0, 3),
+	MUX_CFG(MUX_MCO1,	MCO1_src,	RCC_MCO1CFGR, 0, 3),
+	MUX_CFG(MUX_MCO2,	MCO2_src,	RCC_MCO2CFGR, 0, 3),
+	MUX_RDY_CFG(MUX_MLAHB,	MLAHBS_src,	RCC_MSSCKSELR, 0, 2),
+	MUX_RDY_CFG(MUX_MPU,	MPU_src,	RCC_MPCKSELR, 0, 2),
+	MUX_RDY_CFG(MUX_PLL12,	PLL12_src,	RCC_RCK12SELR, 0, 2),
+	MUX_RDY_CFG(MUX_PLL3,	PLL3_src,	RCC_RCK3SELR, 0, 2),
+	MUX_RDY_CFG(MUX_PLL4,	PLL4_src,	RCC_RCK4SELR, 0, 2),
+	MUX_CFG(MUX_QSPI,	QSPI_src,	RCC_QSPICKSELR, 0, 2),
+	MUX_CFG(MUX_RNG1,	RNG1_src,	RCC_RNG1CKSELR, 0, 2),
+	MUX_CFG(MUX_RTC,	RTC_src,	RCC_BDCR, 16, 2),
+	MUX_CFG(MUX_SAES,	SAES_src,	RCC_SAESCKSELR, 0, 2),
+	MUX_CFG(MUX_SAI1,	SAI1_src,	RCC_SAI1CKSELR, 0, 3),
+	MUX_CFG(MUX_SAI2,	SAI2_src,	RCC_SAI2CKSELR, 0, 3),
+	MUX_CFG(MUX_SDMMC1,	SDMMC1_src,	RCC_SDMMC12CKSELR, 0, 3),
+	MUX_CFG(MUX_SDMMC2,	SDMMC2_src,	RCC_SDMMC12CKSELR, 3, 3),
+	MUX_CFG(MUX_SPDIF,	SPDIF_src,	RCC_SPDIFCKSELR, 0, 2),
+	MUX_CFG(MUX_SPI1,	SPI1_src,	RCC_SPI2S1CKSELR, 0, 3),
+	MUX_CFG(MUX_SPI23,	SPI23_src,	RCC_SPI2S23CKSELR, 0, 3),
+	MUX_CFG(MUX_SPI4,	SPI4_src,	RCC_SPI45CKSELR, 0, 3),
+	MUX_CFG(MUX_SPI5,	SPI5_src,	RCC_SPI45CKSELR, 3, 3),
+	MUX_CFG(MUX_STGEN,	STGEN_src,	RCC_STGENCKSELR, 0, 2),
+	MUX_CFG(MUX_UART1,	UART1_src,	RCC_UART12CKSELR, 0, 3),
+	MUX_CFG(MUX_UART2,	UART2_src,	RCC_UART12CKSELR, 3, 3),
+	MUX_CFG(MUX_UART35,	UART35_src,	RCC_UART35CKSELR, 0, 3),
+	MUX_CFG(MUX_UART4,	UART4_src,	RCC_UART4CKSELR, 0, 3),
+	MUX_CFG(MUX_UART6,	UART6_src,	RCC_UART6CKSELR, 0, 3),
+	MUX_CFG(MUX_UART78,	UART78_src,	RCC_UART78CKSELR, 0, 3),
+	MUX_CFG(MUX_USBO,	USBO_src,	RCC_USBCKSELR, 4, 1),
+	MUX_CFG(MUX_USBPHY,	USBPHY_src,	RCC_USBCKSELR, 0, 2),
+};
+
+/*
+ * GATE CONFIG
+ */
+
+enum enum_gate_cfg {
+	GATE_ZERO, /* reserved for no gate */
+	GATE_LSE,
+	GATE_RTCCK,
+	GATE_LSI,
+	GATE_HSI,
+	GATE_CSI,
+	GATE_HSE,
+	GATE_LSI_RDY,
+	GATE_CSI_RDY,
+	GATE_LSE_RDY,
+	GATE_HSE_RDY,
+	GATE_HSI_RDY,
+	GATE_MCO1,
+	GATE_MCO2,
+	GATE_DBGCK,
+	GATE_TRACECK,
+	GATE_PLL1,
+	GATE_PLL1_DIVP,
+	GATE_PLL1_DIVQ,
+	GATE_PLL1_DIVR,
+	GATE_PLL2,
+	GATE_PLL2_DIVP,
+	GATE_PLL2_DIVQ,
+	GATE_PLL2_DIVR,
+	GATE_PLL3,
+	GATE_PLL3_DIVP,
+	GATE_PLL3_DIVQ,
+	GATE_PLL3_DIVR,
+	GATE_PLL4,
+	GATE_PLL4_DIVP,
+	GATE_PLL4_DIVQ,
+	GATE_PLL4_DIVR,
+	GATE_DDRC1,
+	GATE_DDRC1LP,
+	GATE_DDRPHYC,
+	GATE_DDRPHYCLP,
+	GATE_DDRCAPB,
+	GATE_DDRCAPBLP,
+	GATE_AXIDCG,
+	GATE_DDRPHYCAPB,
+	GATE_DDRPHYCAPBLP,
+	GATE_TIM2,
+	GATE_TIM3,
+	GATE_TIM4,
+	GATE_TIM5,
+	GATE_TIM6,
+	GATE_TIM7,
+	GATE_LPTIM1,
+	GATE_SPI2,
+	GATE_SPI3,
+	GATE_USART3,
+	GATE_UART4,
+	GATE_UART5,
+	GATE_UART7,
+	GATE_UART8,
+	GATE_I2C1,
+	GATE_I2C2,
+	GATE_SPDIF,
+	GATE_TIM1,
+	GATE_TIM8,
+	GATE_SPI1,
+	GATE_USART6,
+	GATE_SAI1,
+	GATE_SAI2,
+	GATE_DFSDM,
+	GATE_ADFSDM,
+	GATE_FDCAN,
+	GATE_LPTIM2,
+	GATE_LPTIM3,
+	GATE_LPTIM4,
+	GATE_LPTIM5,
+	GATE_VREF,
+	GATE_DTS,
+	GATE_PMBCTRL,
+	GATE_HDP,
+	GATE_SYSCFG,
+	GATE_DCMIPP,
+	GATE_DDRPERFM,
+	GATE_IWDG2APB,
+	GATE_USBPHY,
+	GATE_STGENRO,
+	GATE_LTDC,
+	GATE_RTCAPB,
+	GATE_TZC,
+	GATE_ETZPC,
+	GATE_IWDG1APB,
+	GATE_BSEC,
+	GATE_STGENC,
+	GATE_USART1,
+	GATE_USART2,
+	GATE_SPI4,
+	GATE_SPI5,
+	GATE_I2C3,
+	GATE_I2C4,
+	GATE_I2C5,
+	GATE_TIM12,
+	GATE_TIM13,
+	GATE_TIM14,
+	GATE_TIM15,
+	GATE_TIM16,
+	GATE_TIM17,
+	GATE_DMA1,
+	GATE_DMA2,
+	GATE_DMAMUX1,
+	GATE_DMA3,
+	GATE_DMAMUX2,
+	GATE_ADC1,
+	GATE_ADC2,
+	GATE_USBO,
+	GATE_TSC,
+	GATE_GPIOA,
+	GATE_GPIOB,
+	GATE_GPIOC,
+	GATE_GPIOD,
+	GATE_GPIOE,
+	GATE_GPIOF,
+	GATE_GPIOG,
+	GATE_GPIOH,
+	GATE_GPIOI,
+	GATE_PKA,
+	GATE_SAES,
+	GATE_CRYP1,
+	GATE_HASH1,
+	GATE_RNG1,
+	GATE_BKPSRAM,
+	GATE_AXIMC,
+	GATE_MCE,
+	GATE_ETH1CK,
+	GATE_ETH1TX,
+	GATE_ETH1RX,
+	GATE_ETH1MAC,
+	GATE_FMC,
+	GATE_QSPI,
+	GATE_SDMMC1,
+	GATE_SDMMC2,
+	GATE_CRC1,
+	GATE_USBH,
+	GATE_ETH2CK,
+	GATE_ETH2TX,
+	GATE_ETH2RX,
+	GATE_ETH2MAC,
+	GATE_MDMA,
+
+	LAST_GATE
+};
+
+#define GATE_CFG(id, _offset, _bit_idx, _offset_clr)[id] = {\
+	.offset		= (_offset),\
+	.bit_idx	= (_bit_idx),\
+	.set_clr	= (_offset_clr),\
+}
+
+static const struct gate_cfg gates_mp13[LAST_GATE] = {
+	GATE_CFG(GATE_LSE,		RCC_BDCR,	0,	0),
+	GATE_CFG(GATE_RTCCK,		RCC_BDCR,	20,	0),
+	GATE_CFG(GATE_LSI,		RCC_RDLSICR,	0,	0),
+	GATE_CFG(GATE_HSI,		RCC_OCENSETR,	0,	1),
+	GATE_CFG(GATE_CSI,		RCC_OCENSETR,	4,	1),
+	GATE_CFG(GATE_HSE,		RCC_OCENSETR,	8,	1),
+	GATE_CFG(GATE_LSI_RDY,		RCC_RDLSICR,	1,	0),
+	GATE_CFG(GATE_CSI_RDY,		RCC_OCRDYR,	4,	0),
+	GATE_CFG(GATE_LSE_RDY,		RCC_BDCR,	2,	0),
+	GATE_CFG(GATE_HSE_RDY,		RCC_OCRDYR,	8,	0),
+	GATE_CFG(GATE_HSI_RDY,		RCC_OCRDYR,	0,	0),
+	GATE_CFG(GATE_MCO1,		RCC_MCO1CFGR,	12,	0),
+	GATE_CFG(GATE_MCO2,		RCC_MCO2CFGR,	12,	0),
+	GATE_CFG(GATE_DBGCK,		RCC_DBGCFGR,	8,	0),
+	GATE_CFG(GATE_TRACECK,		RCC_DBGCFGR,	9,	0),
+	GATE_CFG(GATE_PLL1,		RCC_PLL1CR,	0,	0),
+	GATE_CFG(GATE_PLL1_DIVP,	RCC_PLL1CR,	4,	0),
+	GATE_CFG(GATE_PLL1_DIVQ,	RCC_PLL1CR,	5,	0),
+	GATE_CFG(GATE_PLL1_DIVR,	RCC_PLL1CR,	6,	0),
+	GATE_CFG(GATE_PLL2,		RCC_PLL2CR,	0,	0),
+	GATE_CFG(GATE_PLL2_DIVP,	RCC_PLL2CR,	4,	0),
+	GATE_CFG(GATE_PLL2_DIVQ,	RCC_PLL2CR,	5,	0),
+	GATE_CFG(GATE_PLL2_DIVR,	RCC_PLL2CR,	6,	0),
+	GATE_CFG(GATE_PLL3,		RCC_PLL3CR,	0,	0),
+	GATE_CFG(GATE_PLL3_DIVP,	RCC_PLL3CR,	4,	0),
+	GATE_CFG(GATE_PLL3_DIVQ,	RCC_PLL3CR,	5,	0),
+	GATE_CFG(GATE_PLL3_DIVR,	RCC_PLL3CR,	6,	0),
+	GATE_CFG(GATE_PLL4,		RCC_PLL4CR,	0,	0),
+	GATE_CFG(GATE_PLL4_DIVP,	RCC_PLL4CR,	4,	0),
+	GATE_CFG(GATE_PLL4_DIVQ,	RCC_PLL4CR,	5,	0),
+	GATE_CFG(GATE_PLL4_DIVR,	RCC_PLL4CR,	6,	0),
+	GATE_CFG(GATE_DDRC1,		RCC_DDRITFCR,	0,	0),
+	GATE_CFG(GATE_DDRC1LP,		RCC_DDRITFCR,	1,	0),
+	GATE_CFG(GATE_DDRPHYC,		RCC_DDRITFCR,	4,	0),
+	GATE_CFG(GATE_DDRPHYCLP,	RCC_DDRITFCR,	5,	0),
+	GATE_CFG(GATE_DDRCAPB,		RCC_DDRITFCR,	6,	0),
+	GATE_CFG(GATE_DDRCAPBLP,	RCC_DDRITFCR,	7,	0),
+	GATE_CFG(GATE_AXIDCG,		RCC_DDRITFCR,	8,	0),
+	GATE_CFG(GATE_DDRPHYCAPB,	RCC_DDRITFCR,	9,	0),
+	GATE_CFG(GATE_DDRPHYCAPBLP,	RCC_DDRITFCR,	10,	0),
+	GATE_CFG(GATE_TIM2,		RCC_MP_APB1ENSETR,	0,	1),
+	GATE_CFG(GATE_TIM3,		RCC_MP_APB1ENSETR,	1,	1),
+	GATE_CFG(GATE_TIM4,		RCC_MP_APB1ENSETR,	2,	1),
+	GATE_CFG(GATE_TIM5,		RCC_MP_APB1ENSETR,	3,	1),
+	GATE_CFG(GATE_TIM6,		RCC_MP_APB1ENSETR,	4,	1),
+	GATE_CFG(GATE_TIM7,		RCC_MP_APB1ENSETR,	5,	1),
+	GATE_CFG(GATE_LPTIM1,		RCC_MP_APB1ENSETR,	9,	1),
+	GATE_CFG(GATE_SPI2,		RCC_MP_APB1ENSETR,	11,	1),
+	GATE_CFG(GATE_SPI3,		RCC_MP_APB1ENSETR,	12,	1),
+	GATE_CFG(GATE_USART3,		RCC_MP_APB1ENSETR,	15,	1),
+	GATE_CFG(GATE_UART4,		RCC_MP_APB1ENSETR,	16,	1),
+	GATE_CFG(GATE_UART5,		RCC_MP_APB1ENSETR,	17,	1),
+	GATE_CFG(GATE_UART7,		RCC_MP_APB1ENSETR,	18,	1),
+	GATE_CFG(GATE_UART8,		RCC_MP_APB1ENSETR,	19,	1),
+	GATE_CFG(GATE_I2C1,		RCC_MP_APB1ENSETR,	21,	1),
+	GATE_CFG(GATE_I2C2,		RCC_MP_APB1ENSETR,	22,	1),
+	GATE_CFG(GATE_SPDIF,		RCC_MP_APB1ENSETR,	26,	1),
+	GATE_CFG(GATE_TIM1,		RCC_MP_APB2ENSETR,	0,	1),
+	GATE_CFG(GATE_TIM8,		RCC_MP_APB2ENSETR,	1,	1),
+	GATE_CFG(GATE_SPI1,		RCC_MP_APB2ENSETR,	8,	1),
+	GATE_CFG(GATE_USART6,		RCC_MP_APB2ENSETR,	13,	1),
+	GATE_CFG(GATE_SAI1,		RCC_MP_APB2ENSETR,	16,	1),
+	GATE_CFG(GATE_SAI2,		RCC_MP_APB2ENSETR,	17,	1),
+	GATE_CFG(GATE_DFSDM,		RCC_MP_APB2ENSETR,	20,	1),
+	GATE_CFG(GATE_ADFSDM,		RCC_MP_APB2ENSETR,	21,	1),
+	GATE_CFG(GATE_FDCAN,		RCC_MP_APB2ENSETR,	24,	1),
+	GATE_CFG(GATE_LPTIM2,		RCC_MP_APB3ENSETR,	0,	1),
+	GATE_CFG(GATE_LPTIM3,		RCC_MP_APB3ENSETR,	1,	1),
+	GATE_CFG(GATE_LPTIM4,		RCC_MP_APB3ENSETR,	2,	1),
+	GATE_CFG(GATE_LPTIM5,		RCC_MP_APB3ENSETR,	3,	1),
+	GATE_CFG(GATE_VREF,		RCC_MP_APB3ENSETR,	13,	1),
+	GATE_CFG(GATE_DTS,		RCC_MP_APB3ENSETR,	16,	1),
+	GATE_CFG(GATE_PMBCTRL,		RCC_MP_APB3ENSETR,	17,	1),
+	GATE_CFG(GATE_HDP,		RCC_MP_APB3ENSETR,	20,	1),
+	GATE_CFG(GATE_SYSCFG,		RCC_MP_S_APB3ENSETR,	0,	1),
+	GATE_CFG(GATE_DCMIPP,		RCC_MP_APB4ENSETR,	1,	1),
+	GATE_CFG(GATE_DDRPERFM,		RCC_MP_APB4ENSETR,	8,	1),
+	GATE_CFG(GATE_IWDG2APB,		RCC_MP_APB4ENSETR,	15,	1),
+	GATE_CFG(GATE_USBPHY,		RCC_MP_APB4ENSETR,	16,	1),
+	GATE_CFG(GATE_STGENRO,		RCC_MP_APB4ENSETR,	20,	1),
+	GATE_CFG(GATE_LTDC,		RCC_MP_S_APB4ENSETR,	0,	1),
+	GATE_CFG(GATE_RTCAPB,		RCC_MP_APB5ENSETR,	8,	1),
+	GATE_CFG(GATE_TZC,		RCC_MP_APB5ENSETR,	11,	1),
+	GATE_CFG(GATE_ETZPC,		RCC_MP_APB5ENSETR,	13,	1),
+	GATE_CFG(GATE_IWDG1APB,		RCC_MP_APB5ENSETR,	15,	1),
+	GATE_CFG(GATE_BSEC,		RCC_MP_APB5ENSETR,	16,	1),
+	GATE_CFG(GATE_STGENC,		RCC_MP_APB5ENSETR,	20,	1),
+	GATE_CFG(GATE_USART1,		RCC_MP_APB6ENSETR,	0,	1),
+	GATE_CFG(GATE_USART2,		RCC_MP_APB6ENSETR,	1,	1),
+	GATE_CFG(GATE_SPI4,		RCC_MP_APB6ENSETR,	2,	1),
+	GATE_CFG(GATE_SPI5,		RCC_MP_APB6ENSETR,	3,	1),
+	GATE_CFG(GATE_I2C3,		RCC_MP_APB6ENSETR,	4,	1),
+	GATE_CFG(GATE_I2C4,		RCC_MP_APB6ENSETR,	5,	1),
+	GATE_CFG(GATE_I2C5,		RCC_MP_APB6ENSETR,	6,	1),
+	GATE_CFG(GATE_TIM12,		RCC_MP_APB6ENSETR,	7,	1),
+	GATE_CFG(GATE_TIM13,		RCC_MP_APB6ENSETR,	8,	1),
+	GATE_CFG(GATE_TIM14,		RCC_MP_APB6ENSETR,	9,	1),
+	GATE_CFG(GATE_TIM15,		RCC_MP_APB6ENSETR,	10,	1),
+	GATE_CFG(GATE_TIM16,		RCC_MP_APB6ENSETR,	11,	1),
+	GATE_CFG(GATE_TIM17,		RCC_MP_APB6ENSETR,	12,	1),
+	GATE_CFG(GATE_DMA1,		RCC_MP_AHB2ENSETR,	0,	1),
+	GATE_CFG(GATE_DMA2,		RCC_MP_AHB2ENSETR,	1,	1),
+	GATE_CFG(GATE_DMAMUX1,		RCC_MP_AHB2ENSETR,	2,	1),
+	GATE_CFG(GATE_DMA3,		RCC_MP_AHB2ENSETR,	3,	1),
+	GATE_CFG(GATE_DMAMUX2,		RCC_MP_AHB2ENSETR,	4,	1),
+	GATE_CFG(GATE_ADC1,		RCC_MP_AHB2ENSETR,	5,	1),
+	GATE_CFG(GATE_ADC2,		RCC_MP_AHB2ENSETR,	6,	1),
+	GATE_CFG(GATE_USBO,		RCC_MP_AHB2ENSETR,	8,	1),
+	GATE_CFG(GATE_TSC,		RCC_MP_AHB4ENSETR,	15,	1),
+
+	GATE_CFG(GATE_GPIOA,		RCC_MP_S_AHB4ENSETR,	0,	1),
+	GATE_CFG(GATE_GPIOB,		RCC_MP_S_AHB4ENSETR,	1,	1),
+	GATE_CFG(GATE_GPIOC,		RCC_MP_S_AHB4ENSETR,	2,	1),
+	GATE_CFG(GATE_GPIOD,		RCC_MP_S_AHB4ENSETR,	3,	1),
+	GATE_CFG(GATE_GPIOE,		RCC_MP_S_AHB4ENSETR,	4,	1),
+	GATE_CFG(GATE_GPIOF,		RCC_MP_S_AHB4ENSETR,	5,	1),
+	GATE_CFG(GATE_GPIOG,		RCC_MP_S_AHB4ENSETR,	6,	1),
+	GATE_CFG(GATE_GPIOH,		RCC_MP_S_AHB4ENSETR,	7,	1),
+	GATE_CFG(GATE_GPIOI,		RCC_MP_S_AHB4ENSETR,	8,	1),
+
+	GATE_CFG(GATE_PKA,		RCC_MP_AHB5ENSETR,	2,	1),
+	GATE_CFG(GATE_SAES,		RCC_MP_AHB5ENSETR,	3,	1),
+	GATE_CFG(GATE_CRYP1,		RCC_MP_AHB5ENSETR,	4,	1),
+	GATE_CFG(GATE_HASH1,		RCC_MP_AHB5ENSETR,	5,	1),
+	GATE_CFG(GATE_RNG1,		RCC_MP_AHB5ENSETR,	6,	1),
+	GATE_CFG(GATE_BKPSRAM,		RCC_MP_AHB5ENSETR,	8,	1),
+	GATE_CFG(GATE_AXIMC,		RCC_MP_AHB5ENSETR,	16,	1),
+	GATE_CFG(GATE_MCE,		RCC_MP_AHB6ENSETR,	1,	1),
+	GATE_CFG(GATE_ETH1CK,		RCC_MP_AHB6ENSETR,	7,	1),
+	GATE_CFG(GATE_ETH1TX,		RCC_MP_AHB6ENSETR,	8,	1),
+	GATE_CFG(GATE_ETH1RX,		RCC_MP_AHB6ENSETR,	9,	1),
+	GATE_CFG(GATE_ETH1MAC,		RCC_MP_AHB6ENSETR,	10,	1),
+	GATE_CFG(GATE_FMC,		RCC_MP_AHB6ENSETR,	12,	1),
+	GATE_CFG(GATE_QSPI,		RCC_MP_AHB6ENSETR,	14,	1),
+	GATE_CFG(GATE_SDMMC1,		RCC_MP_AHB6ENSETR,	16,	1),
+	GATE_CFG(GATE_SDMMC2,		RCC_MP_AHB6ENSETR,	17,	1),
+	GATE_CFG(GATE_CRC1,		RCC_MP_AHB6ENSETR,	20,	1),
+	GATE_CFG(GATE_USBH,		RCC_MP_AHB6ENSETR,	24,	1),
+	GATE_CFG(GATE_ETH2CK,		RCC_MP_AHB6ENSETR,	27,	1),
+	GATE_CFG(GATE_ETH2TX,		RCC_MP_AHB6ENSETR,	28,	1),
+	GATE_CFG(GATE_ETH2RX,		RCC_MP_AHB6ENSETR,	29,	1),
+	GATE_CFG(GATE_ETH2MAC,		RCC_MP_AHB6ENSETR,	30,	1),
+	GATE_CFG(GATE_MDMA,		RCC_MP_S_AHB6ENSETR,	0,	1),
+};
+
+/*
+ * DIV CONFIG
+ */
+
+static const struct clk_div_table axi_div_table[] = {
+	{ 0, 1 }, { 1, 2 }, { 2, 3 }, { 3, 4 },
+	{ 4, 4 }, { 5, 4 }, { 6, 4 }, { 7, 4 },
+	{ 0 },
+};
+
+static const struct clk_div_table mlahb_div_table[] = {
+	{ 0, 1 }, { 1, 2 }, { 2, 4 }, { 3, 8 },
+	{ 4, 16 }, { 5, 32 }, { 6, 64 }, { 7, 128 },
+	{ 8, 256 }, { 9, 512 }, { 10, 512}, { 11, 512 },
+	{ 12, 512 }, { 13, 512 }, { 14, 512}, { 15, 512 },
+	{ 0 },
+};
+
+static const struct clk_div_table apb_div_table[] = {
+	{ 0, 1 }, { 1, 2 }, { 2, 4 }, { 3, 8 },
+	{ 4, 16 }, { 5, 16 }, { 6, 16 }, { 7, 16 },
+	{ 0 },
+};
+
+#define DIV_CFG(id, _offset, _shift, _width, _flags, _table, _bitrdy)[id] = {\
+		.offset	= _offset,\
+		.shift	= _shift,\
+		.width	= _width,\
+		.flags	= _flags,\
+		.table	= _table,\
+		.bitrdy	= _bitrdy,\
+}
+
+static const struct div_cfg dividers_mp13[DIV_MAX] = {
+	DIV_CFG(DIV_PLL1DIVP, RCC_PLL1CFGR2, 0, 7, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_PLL2DIVP, RCC_PLL2CFGR2, 0, 7, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_PLL2DIVQ, RCC_PLL2CFGR2, 8, 7, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_PLL2DIVR, RCC_PLL2CFGR2, 16, 7, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_PLL3DIVP, RCC_PLL3CFGR2, 0, 7, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_PLL3DIVQ, RCC_PLL3CFGR2, 8, 7, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_PLL3DIVR, RCC_PLL3CFGR2, 16, 7, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_PLL4DIVP, RCC_PLL4CFGR2, 0, 7, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_PLL4DIVQ, RCC_PLL4CFGR2, 8, 7, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_PLL4DIVR, RCC_PLL4CFGR2, 16, 7, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_MPU, RCC_MPCKDIVR, 0, 4, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_AXI, RCC_AXIDIVR, 0, 3, 0, axi_div_table, 31),
+	DIV_CFG(DIV_MLAHB, RCC_MLAHBDIVR, 0, 4, 0, mlahb_div_table, 31),
+	DIV_CFG(DIV_APB1, RCC_APB1DIVR, 0, 3, 0, apb_div_table, 31),
+	DIV_CFG(DIV_APB2, RCC_APB2DIVR, 0, 3, 0, apb_div_table, 31),
+	DIV_CFG(DIV_APB3, RCC_APB3DIVR, 0, 3, 0, apb_div_table, 31),
+	DIV_CFG(DIV_APB4, RCC_APB4DIVR, 0, 3, 0, apb_div_table, 31),
+	DIV_CFG(DIV_APB5, RCC_APB5DIVR, 0, 3, 0, apb_div_table, 31),
+	DIV_CFG(DIV_APB6, RCC_APB6DIVR, 0, 3, 0, apb_div_table, 31),
+	DIV_CFG(DIV_RTC, RCC_RTCDIVR, 0, 6, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_MCO1, RCC_MCO1CFGR, 4, 4, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_MCO2, RCC_MCO2CFGR, 4, 4, 0, NULL, DIV_NO_BIT_RDY),
+
+	DIV_CFG(DIV_HSI, RCC_HSICFGR, 0, 2, CLK_DIVIDER_POWER_OF_TWO, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_TRACE, RCC_DBGCFGR, 0, 3, CLK_DIVIDER_POWER_OF_TWO, NULL, DIV_NO_BIT_RDY),
+
+	DIV_CFG(DIV_ETH1PTP, RCC_ETH12CKSELR, 4, 4, 0, NULL, DIV_NO_BIT_RDY),
+	DIV_CFG(DIV_ETH2PTP, RCC_ETH12CKSELR, 12, 4, 0, NULL, DIV_NO_BIT_RDY),
+};
+
+#define MAX_HSI_HZ		64000000
+#define USB_PHY_48_MHZ		48000000
+
+#define TIMEOUT_US_200MS	U(200000)
+#define TIMEOUT_US_1S		U(1000000)
+
+#define PLLRDY_TIMEOUT		TIMEOUT_US_200MS
+#define CLKSRC_TIMEOUT		TIMEOUT_US_200MS
+#define CLKDIV_TIMEOUT		TIMEOUT_US_200MS
+#define HSIDIV_TIMEOUT		TIMEOUT_US_200MS
+#define OSCRDY_TIMEOUT		TIMEOUT_US_1S
+
+enum stm32_osc {
+	OSC_HSI,
+	OSC_HSE,
+	OSC_CSI,
+	OSC_LSI,
+	OSC_LSE,
+	OSC_I2SCKIN,
+	NB_OSCILLATOR
+};
+
+enum stm32mp1_pll_id {
+	_PLL1,
+	_PLL2,
+	_PLL3,
+	_PLL4,
+	_PLL_NB
+};
+
+enum stm32mp1_plltype {
+	PLL_800,
+	PLL_1600,
+	PLL_2000,
+	PLL_TYPE_NB
+};
+
+#define RCC_OFFSET_PLLXCR		0
+#define RCC_OFFSET_PLLXCFGR1		4
+#define RCC_OFFSET_PLLXCFGR2		8
+#define RCC_OFFSET_PLLXFRACR		12
+#define RCC_OFFSET_PLLXCSGR		16
+
+struct stm32_clk_pll {
+	enum stm32mp1_plltype plltype;
+	uint16_t clk_id;
+	uint16_t reg_pllxcr;
+};
+
+struct stm32mp1_pll {
+	uint8_t refclk_min;
+	uint8_t refclk_max;
+};
+
+/* Define characteristic of PLL according type */
+static const struct stm32mp1_pll stm32mp1_pll[PLL_TYPE_NB] = {
+	[PLL_800] = {
+		.refclk_min = 4,
+		.refclk_max = 16,
+	},
+	[PLL_1600] = {
+		.refclk_min = 8,
+		.refclk_max = 16,
+	},
+	[PLL_2000] = {
+		.refclk_min = 8,
+		.refclk_max = 16,
+	},
+};
+
+#if STM32MP_USB_PROGRAMMER
+static bool pll4_bootrom;
+#endif
+
+/* RCC clock device driver private */
+static unsigned int refcounts_mp13[CK_LAST];
+
+static const struct stm32_clk_pll *clk_st32_pll_data(unsigned int idx);
+
+#if STM32MP_UART_PROGRAMMER || STM32MP_USB_PROGRAMMER
+static void clk_oscillator_check_bypass(struct stm32_clk_priv *priv, int idx,
+					bool digbyp, bool bypass)
+{
+	struct clk_oscillator_data *osc_data = clk_oscillator_get_data(priv, idx);
+	struct stm32_clk_bypass *bypass_data = osc_data->bypass;
+	uintptr_t address;
+
+	if (bypass_data == NULL) {
+		return;
+	}
+
+	address = priv->base + bypass_data->offset;
+	if ((mmio_read_32(address) & RCC_OCENR_HSEBYP) &&
+	    (!(digbyp || bypass))) {
+		panic();
+	}
+}
+#endif
+
+static void stm32_enable_oscillator_hse(struct stm32_clk_priv *priv)
+{
+	struct stm32_clk_platdata *pdata = priv->pdata;
+	struct stm32_osci_dt_cfg *osci = &pdata->osci[OSC_HSE];
+	bool digbyp =  osci->digbyp;
+	bool bypass = osci->bypass;
+	bool css = osci->css;
+
+	if (_clk_stm32_get_rate(priv, _CK_HSE) == 0U) {
+		return;
+	}
+
+	clk_oscillator_set_bypass(priv, _CK_HSE, digbyp, bypass);
+
+	_clk_stm32_enable(priv, _CK_HSE);
+
+#if STM32MP_UART_PROGRAMMER || STM32MP_USB_PROGRAMMER
+	clk_oscillator_check_bypass(priv, _CK_HSE, digbyp, bypass);
+#endif
+
+	clk_oscillator_set_css(priv, _CK_HSE, css);
+}
+
+static void stm32_enable_oscillator_lse(struct stm32_clk_priv *priv)
+{
+	struct clk_oscillator_data *osc_data = clk_oscillator_get_data(priv, _CK_LSE);
+	struct stm32_clk_platdata *pdata = priv->pdata;
+	struct stm32_osci_dt_cfg *osci = &pdata->osci[OSC_LSE];
+	bool digbyp =  osci->digbyp;
+	bool bypass = osci->bypass;
+	uint8_t drive = osci->drive;
+
+	if (_clk_stm32_get_rate(priv, _CK_LSE) == 0U) {
+		return;
+	}
+
+	clk_oscillator_set_bypass(priv, _CK_LSE, digbyp, bypass);
+
+	clk_oscillator_set_drive(priv, _CK_LSE, drive);
+
+	_clk_stm32_gate_enable(priv, osc_data->gate_id);
+}
+
+static int stm32mp1_set_hsidiv(uint8_t hsidiv)
+{
+	uint64_t timeout;
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	uintptr_t address = rcc_base + RCC_OCRDYR;
+
+	mmio_clrsetbits_32(rcc_base + RCC_HSICFGR,
+			   RCC_HSICFGR_HSIDIV_MASK,
+			   RCC_HSICFGR_HSIDIV_MASK & (uint32_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));
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+static int stm32mp1_hsidiv(unsigned long hsifreq)
+{
+	uint8_t hsidiv;
+	uint32_t hsidivfreq = MAX_HSI_HZ;
+
+	for (hsidiv = 0; hsidiv < 4U; hsidiv++) {
+		if (hsidivfreq == hsifreq) {
+			break;
+		}
+
+		hsidivfreq /= 2U;
+	}
+
+	if (hsidiv == 4U) {
+		ERROR("Invalid clk-hsi frequency\n");
+		return -EINVAL;
+	}
+
+	if (hsidiv != 0U) {
+		return stm32mp1_set_hsidiv(hsidiv);
+	}
+
+	return 0;
+}
+
+static int stm32_clk_oscillators_lse_set_css(struct stm32_clk_priv *priv)
+{
+	struct stm32_clk_platdata *pdata = priv->pdata;
+	struct stm32_osci_dt_cfg *osci = &pdata->osci[OSC_LSE];
+
+	clk_oscillator_set_css(priv, _CK_LSE, osci->css);
+
+	return 0;
+}
+
+static int stm32mp1_come_back_to_hsi(void)
+{
+	int ret;
+	struct stm32_clk_priv *priv = clk_stm32_get_priv();
+
+	/* Come back to HSI */
+	ret = _clk_stm32_set_parent(priv, _CKMPU, _CK_HSI);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = _clk_stm32_set_parent(priv, _CKAXI, _CK_HSI);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = _clk_stm32_set_parent(priv, _CKMLAHB, _CK_HSI);
+	if (ret != 0) {
+		return ret;
+	}
+
+	return 0;
+}
+
+static int stm32_clk_configure_clk_get_binding_id(struct stm32_clk_priv *priv, uint32_t data)
+{
+	unsigned long binding_id = ((unsigned long)data & CLK_ID_MASK) >> CLK_ID_SHIFT;
+
+	return clk_get_index(priv, binding_id);
+}
+
+static int stm32_clk_configure_clk(struct stm32_clk_priv *priv, uint32_t data)
+{
+	int sel = (data & CLK_SEL_MASK) >> CLK_SEL_SHIFT;
+	int enable = (data & CLK_ON_MASK) >> CLK_ON_SHIFT;
+	int clk_id;
+	int ret;
+
+	clk_id = stm32_clk_configure_clk_get_binding_id(priv, data);
+	if (clk_id < 0) {
+		return clk_id;
+	}
+
+	ret = _clk_stm32_set_parent_by_index(priv, clk_id, sel);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (enable != 0) {
+		clk_stm32_enable_call_ops(priv, clk_id);
+	} else {
+		clk_stm32_disable_call_ops(priv, clk_id);
+	}
+
+	return 0;
+}
+
+static int stm32_clk_configure_mux(struct stm32_clk_priv *priv, uint32_t data)
+{
+	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);
+}
+
+static int stm32_clk_dividers_configure(struct stm32_clk_priv *priv)
+{
+	struct stm32_clk_platdata *pdata = priv->pdata;
+	uint32_t i;
+
+	for (i = 0; i < pdata->nclkdiv; i++) {
+		int div_id, div_n;
+		int 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;
+		}
+	}
+
+	return 0;
+}
+
+static int stm32_clk_source_configure(struct stm32_clk_priv *priv)
+{
+	struct stm32_clk_platdata *pdata = priv->pdata;
+	bool ckper_disabled = false;
+	int clk_id;
+	int ret;
+	uint32_t i;
+
+	for (i = 0; i < pdata->nclksrc; i++) {
+		uint32_t val = pdata->clksrc[i];
+		uint32_t cmd, cmd_data;
+
+		if (val == (uint32_t)CLK_CKPER_DISABLED) {
+			ckper_disabled = true;
+			continue;
+		}
+
+		if (val == (uint32_t)CLK_RTC_DISABLED) {
+			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:
+			clk_id = stm32_clk_configure_clk_get_binding_id(priv, cmd_data);
+
+			if (clk_id == _RTCCK) {
+				if ((_clk_stm32_is_enabled(priv, _RTCCK) == true)) {
+					continue;
+				}
+			}
+
+			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) {
+		ret = stm32_clk_configure_mux(priv, CLK_CKPER_DISABLED);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int stm32_clk_stgen_configure(struct stm32_clk_priv *priv, int id)
+{
+	unsigned long stgen_freq;
+
+	stgen_freq = _clk_stm32_get_rate(priv, id);
+
+	stm32mp_stgen_config(stgen_freq);
+
+	return 0;
+}
+
+#define CLK_PLL_CFG(_idx, _clk_id, _type, _reg)\
+	[(_idx)] = {\
+		.clk_id = (_clk_id),\
+		.plltype = (_type),\
+		.reg_pllxcr = (_reg),\
+	}
+
+static int clk_stm32_pll_compute_cfgr1(struct stm32_clk_priv *priv,
+				       const struct stm32_clk_pll *pll,
+				       struct stm32_pll_vco *vco,
+				       uint32_t *value)
+{
+	uint32_t divm = vco->div_mn[PLL_CFG_M];
+	uint32_t divn = vco->div_mn[PLL_CFG_N];
+	unsigned long prate = 0UL;
+	unsigned long refclk = 0UL;
+
+	prate = _clk_stm32_get_parent_rate(priv, pll->clk_id);
+	refclk = prate / (divm + 1U);
+
+	if ((refclk < (stm32mp1_pll[pll->plltype].refclk_min * 1000000U)) ||
+	    (refclk > (stm32mp1_pll[pll->plltype].refclk_max * 1000000U))) {
+		return -EINVAL;
+	}
+
+	*value = 0;
+
+	if ((pll->plltype == PLL_800) && (refclk >= 8000000U)) {
+		*value = 1U << RCC_PLLNCFGR1_IFRGE_SHIFT;
+	}
+
+	*value |= (divn << RCC_PLLNCFGR1_DIVN_SHIFT) & RCC_PLLNCFGR1_DIVN_MASK;
+	*value |= (divm << RCC_PLLNCFGR1_DIVM_SHIFT) & RCC_PLLNCFGR1_DIVM_MASK;
+
+	return 0;
+}
+
+static uint32_t  clk_stm32_pll_compute_cfgr2(struct stm32_pll_output *out)
+{
+	uint32_t value = 0;
+
+	value |= (out->output[PLL_CFG_P] << RCC_PLLNCFGR2_DIVP_SHIFT) & RCC_PLLNCFGR2_DIVP_MASK;
+	value |= (out->output[PLL_CFG_Q] << RCC_PLLNCFGR2_DIVQ_SHIFT) & RCC_PLLNCFGR2_DIVQ_MASK;
+	value |= (out->output[PLL_CFG_R] << RCC_PLLNCFGR2_DIVR_SHIFT) & RCC_PLLNCFGR2_DIVR_MASK;
+
+	return value;
+}
+
+static void clk_stm32_pll_config_vco(struct stm32_clk_priv *priv,
+				     const struct stm32_clk_pll *pll,
+				     struct stm32_pll_vco *vco)
+{
+	uintptr_t pll_base = priv->base + pll->reg_pllxcr;
+	uint32_t value = 0;
+
+	if (clk_stm32_pll_compute_cfgr1(priv, pll, vco, &value) != 0) {
+		ERROR("Invalid Vref clock !\n");
+		panic();
+	}
+
+	/* Write N / M / IFREGE fields */
+	mmio_write_32(pll_base + RCC_OFFSET_PLLXCFGR1, value);
+
+	/* Fractional configuration */
+	mmio_write_32(pll_base + RCC_OFFSET_PLLXFRACR, 0);
+
+	/* Frac must be enabled only once its configuration is loaded */
+	mmio_write_32(pll_base + RCC_OFFSET_PLLXFRACR, vco->frac << RCC_PLLNFRACR_FRACV_SHIFT);
+	mmio_setbits_32(pll_base + RCC_OFFSET_PLLXFRACR, RCC_PLLNFRACR_FRACLE);
+}
+
+static void clk_stm32_pll_config_csg(struct stm32_clk_priv *priv,
+				     const struct stm32_clk_pll *pll,
+				     struct stm32_pll_vco *vco)
+{
+	uintptr_t pll_base = priv->base + pll->reg_pllxcr;
+	uint32_t mod_per = 0;
+	uint32_t inc_step = 0;
+	uint32_t sscg_mode = 0;
+	uint32_t value = 0;
+
+	if (!vco->csg_enabled) {
+		return;
+	}
+
+	mod_per = vco->csg[PLL_CSG_MOD_PER];
+	inc_step = vco->csg[PLL_CSG_INC_STEP];
+	sscg_mode = vco->csg[PLL_CSG_SSCG_MODE];
+
+	value |= (mod_per << RCC_PLLNCSGR_MOD_PER_SHIFT) & RCC_PLLNCSGR_MOD_PER_MASK;
+	value |= (inc_step << RCC_PLLNCSGR_INC_STEP_SHIFT) & RCC_PLLNCSGR_INC_STEP_MASK;
+	value |= (sscg_mode << RCC_PLLNCSGR_SSCG_MODE_SHIFT) & RCC_PLLNCSGR_SSCG_MODE_MASK;
+
+	mmio_write_32(pll_base + RCC_OFFSET_PLLXCSGR, value);
+	mmio_setbits_32(pll_base + RCC_OFFSET_PLLXCR, RCC_PLLNCR_SSCG_CTRL);
+}
+
+static void clk_stm32_pll_config_out(struct stm32_clk_priv *priv, const struct stm32_clk_pll *pll,
+				     struct stm32_pll_output *out)
+{
+	uintptr_t pll_base = priv->base + pll->reg_pllxcr;
+	uint32_t value = 0;
+
+	value = clk_stm32_pll_compute_cfgr2(out);
+
+	mmio_write_32(pll_base + RCC_OFFSET_PLLXCFGR2, value);
+}
+
+static inline struct stm32_pll_dt_cfg *clk_stm32_pll_get_pdata(int pll_idx)
+{
+	struct stm32_clk_priv *priv = clk_stm32_get_priv();
+	struct stm32_clk_platdata *pdata = priv->pdata;
+
+	return &pdata->pll[pll_idx];
+}
+
+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;
+
+	return ((mmio_read_32(pll_base) & RCC_PLLNCR_PLLON) != 0U);
+}
+
+static void _clk_stm32_pll_set_on(struct stm32_clk_priv *priv, const struct stm32_clk_pll *pll)
+{
+	uintptr_t pll_base = priv->base + pll->reg_pllxcr;
+
+	/* Preserve RCC_PLLNCR_SSCG_CTRL value */
+	mmio_clrsetbits_32(pll_base, RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN | RCC_PLLNCR_DIVREN,
+			   RCC_PLLNCR_PLLON);
+}
+
+static void _clk_stm32_pll_set_off(struct stm32_clk_priv *priv, const struct stm32_clk_pll *pll)
+{
+	uintptr_t pll_base = priv->base + pll->reg_pllxcr;
+
+	/* Stop all output */
+	mmio_clrbits_32(pll_base, RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN | RCC_PLLNCR_DIVREN);
+
+	/* Stop PLL */
+	mmio_clrbits_32(pll_base, RCC_PLLNCR_PLLON);
+}
+
+static int _clk_stm32_pll_wait_ready_on(struct stm32_clk_priv *priv,
+					const struct stm32_clk_pll *pll)
+{
+	uintptr_t pll_base = priv->base + pll->reg_pllxcr;
+	uint64_t timeout = timeout_init_us(PLLRDY_TIMEOUT);
+
+	/* 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));
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int _clk_stm32_pll_wait_ready_off(struct stm32_clk_priv *priv,
+					 const struct stm32_clk_pll *pll)
+{
+	uintptr_t pll_base = priv->base + pll->reg_pllxcr;
+	uint64_t timeout = timeout_init_us(PLLRDY_TIMEOUT);
+
+	/* 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));
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int _clk_stm32_pll_enable(struct stm32_clk_priv *priv, const struct stm32_clk_pll *pll)
+{
+	if (_clk_stm32_pll_is_enabled(priv, pll)) {
+		return 0;
+	}
+
+	/* Preserve RCC_PLLNCR_SSCG_CTRL value */
+	_clk_stm32_pll_set_on(priv, pll);
+
+	/* Wait PLL lock */
+	return _clk_stm32_pll_wait_ready_on(priv, pll);
+}
+
+static void _clk_stm32_pll_disable(struct stm32_clk_priv *priv, const struct stm32_clk_pll *pll)
+{
+	if (!_clk_stm32_pll_is_enabled(priv, pll)) {
+		return;
+	}
+
+	/* Stop all outputs and the PLL */
+	_clk_stm32_pll_set_off(priv, pll);
+
+	/* Wait PLL stopped */
+	_clk_stm32_pll_wait_ready_off(priv, pll);
+}
+
+static int _clk_stm32_pll_init(struct stm32_clk_priv *priv, int pll_idx,
+			       struct stm32_pll_dt_cfg *pll_conf)
+{
+	const struct stm32_clk_pll *pll = clk_st32_pll_data(pll_idx);
+	uintptr_t pll_base = priv->base + pll->reg_pllxcr;
+	int ret = 0;
+
+	/* Configure PLLs source */
+	ret = stm32_clk_configure_mux(priv, pll_conf->vco.src);
+	if (ret != 0) {
+		return ret;
+	}
+
+#if STM32MP_USB_PROGRAMMER
+	if ((pll_idx == _PLL4) && pll4_bootrom) {
+		clk_stm32_pll_config_out(priv, pll, &pll_conf->output);
+
+		mmio_setbits_32(pll_base,
+				RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN | RCC_PLLNCR_DIVREN);
+
+		return 0;
+	}
+#endif
+	/* Stop the PLL before */
+	_clk_stm32_pll_disable(priv, pll);
+
+	clk_stm32_pll_config_vco(priv, pll, &pll_conf->vco);
+	clk_stm32_pll_config_out(priv, pll, &pll_conf->output);
+	clk_stm32_pll_config_csg(priv, pll, &pll_conf->vco);
+
+	ret = _clk_stm32_pll_enable(priv, pll);
+	if (ret != 0) {
+		return ret;
+	}
+
+	mmio_setbits_32(pll_base, RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN | RCC_PLLNCR_DIVREN);
+
+	return 0;
+}
+
+static int clk_stm32_pll_init(struct stm32_clk_priv *priv, int pll_idx)
+{
+	struct stm32_pll_dt_cfg *pll_conf = clk_stm32_pll_get_pdata(pll_idx);
+
+	if (pll_conf->vco.status != 0U) {
+		return _clk_stm32_pll_init(priv, pll_idx, pll_conf);
+	}
+
+	return 0;
+}
+
+static int stm32_clk_pll_configure(struct stm32_clk_priv *priv)
+{
+	int err = 0;
+
+	err = clk_stm32_pll_init(priv, _PLL1);
+	if (err != 0) {
+		return err;
+	}
+
+	err = clk_stm32_pll_init(priv, _PLL2);
+	if (err != 0) {
+		return err;
+	}
+
+	err = clk_stm32_pll_init(priv, _PLL3);
+	if (err != 0) {
+		return err;
+	}
+
+	err = clk_stm32_pll_init(priv, _PLL4);
+	if (err != 0) {
+		return err;
+	}
+
+	return 0;
+}
+
+static int stm32_clk_oscillators_wait_lse_ready(struct stm32_clk_priv *priv)
+{
+	int ret = 0;
+
+	if (_clk_stm32_get_rate(priv, _CK_LSE) != 0U) {
+		ret = clk_oscillator_wait_ready_on(priv, _CK_LSE);
+	}
+
+	return ret;
+}
+
+static void stm32_clk_oscillators_enable(struct stm32_clk_priv *priv)
+{
+	stm32_enable_oscillator_hse(priv);
+	stm32_enable_oscillator_lse(priv);
+	_clk_stm32_enable(priv, _CK_LSI);
+	_clk_stm32_enable(priv, _CK_CSI);
+}
+
+static int stm32_clk_hsidiv_configure(struct stm32_clk_priv *priv)
+{
+	return stm32mp1_hsidiv(_clk_stm32_get_rate(priv, _CK_HSI));
+}
+
+#if STM32MP_USB_PROGRAMMER
+static bool stm32mp1_clk_is_pll4_used_by_bootrom(struct stm32_clk_priv *priv, int usbphy_p)
+{
+	/* Don't initialize PLL4, when used by BOOTROM */
+	if ((stm32mp_get_boot_itf_selected() ==
+	     BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB) &&
+	    (usbphy_p == _PLL4R)) {
+		return true;
+	}
+
+	return false;
+}
+
+static int stm32mp1_clk_check_usb_conflict(struct stm32_clk_priv *priv, int usbphy_p, int usbo_p)
+{
+	int _usbo_p;
+	int _usbphy_p;
+
+	if (!pll4_bootrom) {
+		return 0;
+	}
+
+	_usbo_p = _clk_stm32_get_parent(priv, _USBO_K);
+	_usbphy_p = _clk_stm32_get_parent(priv, _USBPHY_K);
+
+	if ((_usbo_p != usbo_p) || (_usbphy_p != usbphy_p)) {
+		return -FDT_ERR_BADVALUE;
+	}
+
+	return 0;
+}
+#endif
+
+static struct clk_oscillator_data stm32mp13_osc_data[NB_OSCILLATOR] = {
+	OSCILLATOR(OSC_HSI, _CK_HSI, "clk-hsi", GATE_HSI, GATE_HSI_RDY,
+		   NULL, NULL, NULL),
+
+	OSCILLATOR(OSC_LSI, _CK_LSI, "clk-lsi", GATE_LSI, GATE_LSI_RDY,
+		   NULL, NULL, NULL),
+
+	OSCILLATOR(OSC_CSI, _CK_CSI, "clk-csi", GATE_CSI, GATE_CSI_RDY,
+		   NULL, NULL, NULL),
+
+	OSCILLATOR(OSC_LSE, _CK_LSE, "clk-lse", GATE_LSE, GATE_LSE_RDY,
+		   BYPASS(RCC_BDCR, 1, 3),
+		   CSS(RCC_BDCR, 8),
+		   DRIVE(RCC_BDCR, 4, 2, 2)),
+
+	OSCILLATOR(OSC_HSE, _CK_HSE, "clk-hse", GATE_HSE, GATE_HSE_RDY,
+		   BYPASS(RCC_OCENSETR, 10, 7),
+		   CSS(RCC_OCENSETR, 11),
+		   NULL),
+
+	OSCILLATOR(OSC_I2SCKIN, _I2SCKIN, "i2s_ckin", NO_GATE, NO_GATE,
+		   NULL, NULL, NULL),
+};
+
+static const char *clk_stm32_get_oscillator_name(enum stm32_osc id)
+{
+	if (id < NB_OSCILLATOR) {
+		return stm32mp13_osc_data[id].name;
+	}
+
+	return NULL;
+}
+
+#define CLK_PLL_CFG(_idx, _clk_id, _type, _reg)\
+	[(_idx)] = {\
+		.clk_id = (_clk_id),\
+		.plltype = (_type),\
+		.reg_pllxcr = (_reg),\
+	}
+
+static const struct stm32_clk_pll stm32_mp13_clk_pll[_PLL_NB] = {
+	CLK_PLL_CFG(_PLL1, _CK_PLL1, PLL_2000, RCC_PLL1CR),
+	CLK_PLL_CFG(_PLL2, _CK_PLL2, PLL_1600, RCC_PLL2CR),
+	CLK_PLL_CFG(_PLL3, _CK_PLL3, PLL_800, RCC_PLL3CR),
+	CLK_PLL_CFG(_PLL4, _CK_PLL4, PLL_800, RCC_PLL4CR),
+};
+
+static const struct stm32_clk_pll *clk_st32_pll_data(unsigned int idx)
+{
+	return &stm32_mp13_clk_pll[idx];
+}
+
+struct stm32_pll_cfg {
+	int pll_id;
+};
+
+static unsigned long clk_stm32_pll_recalc_rate(struct stm32_clk_priv *priv,  int id,
+					       unsigned long prate)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct stm32_pll_cfg *pll_cfg = clk->clock_cfg;
+	const struct stm32_clk_pll *pll = clk_st32_pll_data(pll_cfg->pll_id);
+	uintptr_t pll_base = priv->base + pll->reg_pllxcr;
+	uint32_t cfgr1, fracr, divm, divn;
+	unsigned long fvco;
+
+	cfgr1 = mmio_read_32(pll_base + RCC_OFFSET_PLLXCFGR1);
+	fracr = mmio_read_32(pll_base + RCC_OFFSET_PLLXFRACR);
+
+	divm = (cfgr1 & (RCC_PLLNCFGR1_DIVM_MASK)) >> RCC_PLLNCFGR1_DIVM_SHIFT;
+	divn = cfgr1 & RCC_PLLNCFGR1_DIVN_MASK;
+
+	/*
+	 * With FRACV :
+	 *   Fvco = Fck_ref * ((DIVN + 1) + FRACV / 2^13) / (DIVM + 1)
+	 * Without FRACV
+	 *   Fvco = Fck_ref * ((DIVN + 1) / (DIVM + 1)
+	 */
+	if ((fracr & RCC_PLLNFRACR_FRACLE) != 0U) {
+		uint32_t fracv = (fracr & RCC_PLLNFRACR_FRACV_MASK) >>
+				 RCC_PLLNFRACR_FRACV_SHIFT;
+		unsigned long long numerator, denominator;
+
+		numerator = (((unsigned long long)divn + 1U) << 13) + fracv;
+		numerator = prate * numerator;
+		denominator = ((unsigned long long)divm + 1U) << 13;
+		fvco = (unsigned long)(numerator / denominator);
+	} else {
+		fvco = (unsigned long)(prate * (divn + 1U) / (divm + 1U));
+	}
+
+	return fvco;
+};
+
+static bool clk_stm32_pll_is_enabled(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct stm32_pll_cfg *pll_cfg = clk->clock_cfg;
+	const struct stm32_clk_pll *pll = clk_st32_pll_data(pll_cfg->pll_id);
+
+	return _clk_stm32_pll_is_enabled(priv, pll);
+}
+
+static int clk_stm32_pll_enable(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct stm32_pll_cfg *pll_cfg = clk->clock_cfg;
+	const struct stm32_clk_pll *pll = clk_st32_pll_data(pll_cfg->pll_id);
+
+	return _clk_stm32_pll_enable(priv, pll);
+}
+
+static void clk_stm32_pll_disable(struct stm32_clk_priv *priv, int id)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, id);
+	struct stm32_pll_cfg *pll_cfg = clk->clock_cfg;
+	const struct stm32_clk_pll *pll = clk_st32_pll_data(pll_cfg->pll_id);
+
+	_clk_stm32_pll_disable(priv, pll);
+}
+
+static const struct stm32_clk_ops clk_stm32_pll_ops = {
+	.recalc_rate	= clk_stm32_pll_recalc_rate,
+	.enable		= clk_stm32_pll_enable,
+	.disable	= clk_stm32_pll_disable,
+	.is_enabled	= clk_stm32_pll_is_enabled,
+};
+
+#define CLK_PLL(idx, _idx, _parent, _gate, _pll_id, _flags)[idx] = {\
+	.binding = _idx,\
+	.parent = _parent,\
+	.flags = (_flags),\
+	.clock_cfg	= &(struct stm32_pll_cfg) {\
+		.pll_id = _pll_id,\
+	},\
+	.ops = &clk_stm32_pll_ops,\
+}
+
+struct clk_stm32_composite_cfg {
+	int gate_id;
+	int div_id;
+};
+
+static unsigned long clk_stm32_composite_recalc_rate(struct stm32_clk_priv *priv,
+						     int idx, unsigned long prate)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, idx);
+	struct clk_stm32_composite_cfg *composite_cfg = clk->clock_cfg;
+
+	return _clk_stm32_divider_recalc(priv, composite_cfg->div_id, prate);
+};
+
+static bool clk_stm32_composite_gate_is_enabled(struct stm32_clk_priv *priv, int idx)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, idx);
+	struct clk_stm32_composite_cfg *composite_cfg = clk->clock_cfg;
+
+	return _clk_stm32_gate_is_enabled(priv, composite_cfg->gate_id);
+}
+
+static int clk_stm32_composite_gate_enable(struct stm32_clk_priv *priv, int idx)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, idx);
+	struct clk_stm32_composite_cfg *composite_cfg = clk->clock_cfg;
+
+	return _clk_stm32_gate_enable(priv, composite_cfg->gate_id);
+}
+
+static void clk_stm32_composite_gate_disable(struct stm32_clk_priv *priv, int idx)
+{
+	const struct clk_stm32 *clk = _clk_get(priv, idx);
+	struct clk_stm32_composite_cfg *composite_cfg = clk->clock_cfg;
+
+	_clk_stm32_gate_disable(priv, composite_cfg->gate_id);
+}
+
+static const struct stm32_clk_ops clk_stm32_composite_ops = {
+	.recalc_rate = clk_stm32_composite_recalc_rate,
+	.is_enabled = clk_stm32_composite_gate_is_enabled,
+	.enable = clk_stm32_composite_gate_enable,
+	.disable = clk_stm32_composite_gate_disable,
+};
+
+#define STM32_COMPOSITE(idx, _binding, _parent, _flags, _gate_id,\
+			_div_id)[idx] = {\
+	.binding = (_binding),\
+	.parent =  (_parent),\
+	.flags = (_flags),\
+	.clock_cfg	= &(struct clk_stm32_composite_cfg) {\
+		.gate_id	= (_gate_id),\
+		.div_id	= (_div_id),\
+	},\
+	.ops = &clk_stm32_composite_ops,\
+}
+
+static const struct clk_stm32 stm32mp13_clk[CK_LAST] = {
+	/* ROOT CLOCKS */
+	CLK_FIXED_RATE(_CK_OFF, _NO_ID, 0),
+	CLK_OSC(_CK_HSE, CK_HSE, CLK_IS_ROOT, OSC_HSE),
+	CLK_OSC(_CK_HSI, CK_HSI, CLK_IS_ROOT, OSC_HSI),
+	CLK_OSC(_CK_CSI, CK_CSI, CLK_IS_ROOT, OSC_CSI),
+	CLK_OSC(_CK_LSI, CK_LSI, CLK_IS_ROOT, OSC_LSI),
+	CLK_OSC(_CK_LSE, CK_LSE, CLK_IS_ROOT, OSC_LSE),
+
+	CLK_OSC_FIXED(_I2SCKIN, _NO_ID, CLK_IS_ROOT, OSC_I2SCKIN),
+
+	CLK_FIXED_RATE(_USB_PHY_48, _NO_ID, USB_PHY_48_MHZ),
+
+	STM32_DIV(_HSE_DIV, _NO_ID, _CK_HSE, 0, DIV_RTC),
+
+	FIXED_FACTOR(_HSE_DIV2, CK_HSE_DIV2, _CK_HSE, 1, 2),
+	FIXED_FACTOR(_CSI_DIV122, _NO_ID, _CK_CSI, 1, 122),
+
+	CLK_PLL(_CK_PLL1, PLL1, MUX(MUX_PLL12), GATE_PLL1, _PLL1, 0),
+	CLK_PLL(_CK_PLL2, PLL2, MUX(MUX_PLL12), GATE_PLL2, _PLL2, 0),
+	CLK_PLL(_CK_PLL3, PLL3, MUX(MUX_PLL3), GATE_PLL3, _PLL3, 0),
+	CLK_PLL(_CK_PLL4, PLL4, MUX(MUX_PLL4), GATE_PLL4, _PLL4, 0),
+
+	STM32_COMPOSITE(_PLL1P, PLL1_P, _CK_PLL1, CLK_IS_CRITICAL, GATE_PLL1_DIVP, DIV_PLL1DIVP),
+	STM32_DIV(_PLL1P_DIV, _NO_ID, _CK_PLL1, 0, DIV_MPU),
+
+	STM32_COMPOSITE(_PLL2P, PLL2_P, _CK_PLL2, CLK_IS_CRITICAL, GATE_PLL2_DIVP, DIV_PLL2DIVP),
+	STM32_COMPOSITE(_PLL2Q, PLL2_Q, _CK_PLL2, 0, GATE_PLL2_DIVQ, DIV_PLL2DIVQ),
+	STM32_COMPOSITE(_PLL2R, PLL2_R, _CK_PLL2, CLK_IS_CRITICAL, GATE_PLL2_DIVR, DIV_PLL2DIVR),
+
+	STM32_COMPOSITE(_PLL3P, PLL3_P, _CK_PLL3, 0, GATE_PLL3_DIVP, DIV_PLL3DIVP),
+	STM32_COMPOSITE(_PLL3Q, PLL3_Q, _CK_PLL3, 0, GATE_PLL3_DIVQ, DIV_PLL3DIVQ),
+	STM32_COMPOSITE(_PLL3R, PLL3_R, _CK_PLL3, 0, GATE_PLL3_DIVR, DIV_PLL3DIVR),
+
+	STM32_COMPOSITE(_PLL4P, PLL4_P, _CK_PLL4, 0, GATE_PLL4_DIVP, DIV_PLL4DIVP),
+	STM32_COMPOSITE(_PLL4Q, PLL4_Q, _CK_PLL4, 0, GATE_PLL4_DIVQ, DIV_PLL4DIVQ),
+	STM32_COMPOSITE(_PLL4R, PLL4_R, _CK_PLL4, 0, GATE_PLL4_DIVR, DIV_PLL4DIVR),
+
+	STM32_MUX(_CKMPU, CK_MPU, MUX_MPU, 0),
+	STM32_DIV(_CKAXI, CK_AXI, MUX(MUX_AXI), 0, DIV_AXI),
+	STM32_DIV(_CKMLAHB, CK_MLAHB, MUX(MUX_MLAHB), CLK_IS_CRITICAL, DIV_MLAHB),
+	STM32_MUX(_CKPER, CK_PER, MUX(MUX_CKPER), 0),
+
+	STM32_DIV(_PCLK1, PCLK1, _CKMLAHB, 0, DIV_APB1),
+	STM32_DIV(_PCLK2, PCLK2, _CKMLAHB, 0, DIV_APB2),
+	STM32_DIV(_PCLK3, PCLK3, _CKMLAHB, 0, DIV_APB3),
+	STM32_DIV(_PCLK4, PCLK4, _CKAXI, 0, DIV_APB4),
+	STM32_DIV(_PCLK5, PCLK5, _CKAXI, 0, DIV_APB5),
+	STM32_DIV(_PCLK6, PCLK6, _CKMLAHB, 0, DIV_APB6),
+
+	CK_TIMER(_CKTIMG1, CK_TIMG1, _PCLK1, 0, RCC_APB1DIVR, RCC_TIMG1PRER),
+	CK_TIMER(_CKTIMG2, CK_TIMG2, _PCLK2, 0, RCC_APB2DIVR, RCC_TIMG2PRER),
+	CK_TIMER(_CKTIMG3, CK_TIMG3, _PCLK6, 0, RCC_APB6DIVR, RCC_TIMG3PRER),
+
+	/* END ROOT CLOCKS */
+
+	STM32_GATE(_DDRC1, DDRC1, _CKAXI, CLK_IS_CRITICAL, GATE_DDRC1),
+	STM32_GATE(_DDRC1LP, DDRC1LP, _CKAXI, CLK_IS_CRITICAL, GATE_DDRC1LP),
+	STM32_GATE(_DDRPHYC, DDRPHYC, _PLL2R, CLK_IS_CRITICAL, GATE_DDRPHYC),
+	STM32_GATE(_DDRPHYCLP, DDRPHYCLP, _PLL2R, CLK_IS_CRITICAL, GATE_DDRPHYCLP),
+	STM32_GATE(_DDRCAPB, DDRCAPB, _PCLK4, CLK_IS_CRITICAL, GATE_DDRCAPB),
+	STM32_GATE(_DDRCAPBLP, DDRCAPBLP, _PCLK4, CLK_IS_CRITICAL, GATE_DDRCAPBLP),
+	STM32_GATE(_AXIDCG, AXIDCG, _CKAXI, CLK_IS_CRITICAL, GATE_AXIDCG),
+	STM32_GATE(_DDRPHYCAPB, DDRPHYCAPB, _PCLK4, CLK_IS_CRITICAL, GATE_DDRPHYCAPB),
+	STM32_GATE(_DDRPHYCAPBLP, DDRPHYCAPBLP, _PCLK4, CLK_IS_CRITICAL,  GATE_DDRPHYCAPBLP),
+
+	STM32_GATE(_SYSCFG, SYSCFG, _PCLK3, 0, GATE_SYSCFG),
+	STM32_GATE(_DDRPERFM, DDRPERFM, _PCLK4, 0, GATE_DDRPERFM),
+	STM32_GATE(_IWDG2APB, IWDG2, _PCLK4, 0, GATE_IWDG2APB),
+	STM32_GATE(_USBPHY_K, USBPHY_K, MUX(MUX_USBPHY), 0, GATE_USBPHY),
+	STM32_GATE(_USBO_K, USBO_K, MUX(MUX_USBO), 0, GATE_USBO),
+
+	STM32_GATE(_RTCAPB, RTCAPB, _PCLK5, CLK_IS_CRITICAL, GATE_RTCAPB),
+	STM32_GATE(_TZC, TZC, _PCLK5, CLK_IS_CRITICAL, GATE_TZC),
+	STM32_GATE(_ETZPC, TZPC, _PCLK5, CLK_IS_CRITICAL, GATE_ETZPC),
+	STM32_GATE(_IWDG1APB, IWDG1, _PCLK5, 0, GATE_IWDG1APB),
+	STM32_GATE(_BSEC, BSEC, _PCLK5, CLK_IS_CRITICAL, GATE_BSEC),
+	STM32_GATE(_STGENC, STGEN_K, MUX(MUX_STGEN), CLK_IS_CRITICAL, GATE_STGENC),
+
+	STM32_GATE(_USART1_K, USART1_K, MUX(MUX_UART1), 0, GATE_USART1),
+	STM32_GATE(_USART2_K, USART2_K, MUX(MUX_UART2), 0, GATE_USART2),
+	STM32_GATE(_I2C3_K, I2C3_K, MUX(MUX_I2C3), 0, GATE_I2C3),
+	STM32_GATE(_I2C4_K, I2C4_K, MUX(MUX_I2C4), 0, GATE_I2C4),
+	STM32_GATE(_I2C5_K, I2C5_K, MUX(MUX_I2C5), 0, GATE_I2C5),
+	STM32_GATE(_TIM12, TIM12_K, _CKTIMG3, 0, GATE_TIM12),
+	STM32_GATE(_TIM15, TIM15_K, _CKTIMG3, 0, GATE_TIM15),
+
+	STM32_GATE(_RTCCK, RTC, MUX(MUX_RTC), 0, GATE_RTCCK),
+
+	STM32_GATE(_GPIOA, GPIOA, _CKMLAHB, 0, GATE_GPIOA),
+	STM32_GATE(_GPIOB, GPIOB, _CKMLAHB, 0, GATE_GPIOB),
+	STM32_GATE(_GPIOC, GPIOC, _CKMLAHB, 0, GATE_GPIOC),
+	STM32_GATE(_GPIOD, GPIOD, _CKMLAHB, 0, GATE_GPIOD),
+	STM32_GATE(_GPIOE, GPIOE, _CKMLAHB, 0, GATE_GPIOE),
+	STM32_GATE(_GPIOF, GPIOF, _CKMLAHB, 0, GATE_GPIOF),
+	STM32_GATE(_GPIOG, GPIOG, _CKMLAHB, 0, GATE_GPIOG),
+	STM32_GATE(_GPIOH, GPIOH, _CKMLAHB, 0, GATE_GPIOH),
+	STM32_GATE(_GPIOI, GPIOI, _CKMLAHB, 0, GATE_GPIOI),
+
+	STM32_GATE(_PKA, PKA, _CKAXI, 0, GATE_PKA),
+	STM32_GATE(_SAES_K, SAES_K, MUX(MUX_SAES), 0, GATE_SAES),
+	STM32_GATE(_CRYP1, CRYP1, _PCLK5, 0, GATE_CRYP1),
+	STM32_GATE(_HASH1, HASH1, _PCLK5, 0, GATE_HASH1),
+
+	STM32_GATE(_RNG1_K, RNG1_K, MUX(MUX_RNG1), 0, GATE_RNG1),
+	STM32_GATE(_BKPSRAM, BKPSRAM, _PCLK5, CLK_IS_CRITICAL, GATE_BKPSRAM),
+
+	STM32_GATE(_SDMMC1_K, SDMMC1_K, MUX(MUX_SDMMC1), 0, GATE_SDMMC1),
+	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),
+	STM32_GATE(_UART7_K, UART7_K, MUX(MUX_UART78), 0, GATE_UART7),
+	STM32_GATE(_UART8_K, UART8_K, MUX(MUX_UART78), 0, GATE_UART8),
+	STM32_GATE(_USART6_K, USART6_K, MUX(MUX_UART6), 0, GATE_USART6),
+	STM32_GATE(_MCE, MCE, _CKAXI, CLK_IS_CRITICAL, GATE_MCE),
+	STM32_GATE(_FMC_K, FMC_K, MUX(MUX_FMC), 0, GATE_FMC),
+	STM32_GATE(_QSPI_K, QSPI_K, MUX(MUX_QSPI), 0, GATE_QSPI),
+
+	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];
+
+static struct stm32_osci_dt_cfg mp13_osci[NB_OSCILLATOR];
+
+static uint32_t mp13_clksrc[MUX_MAX];
+
+static uint32_t mp13_clkdiv[DIV_MAX];
+
+static struct stm32_clk_platdata stm32mp13_clock_pdata = {
+	.osci		= mp13_osci,
+	.nosci		= NB_OSCILLATOR,
+	.pll		= mp13_pll,
+	.npll		= _PLL_NB,
+	.clksrc		= mp13_clksrc,
+	.nclksrc	= MUX_MAX,
+	.clkdiv		= mp13_clkdiv,
+	.nclkdiv	= DIV_MAX,
+};
+
+static struct stm32_clk_priv stm32mp13_clock_data = {
+	.base		= RCC_BASE,
+	.num		= ARRAY_SIZE(stm32mp13_clk),
+	.clks		= stm32mp13_clk,
+	.parents	= parent_mp13,
+	.nb_parents	= ARRAY_SIZE(parent_mp13),
+	.gates		= gates_mp13,
+	.nb_gates	= ARRAY_SIZE(gates_mp13),
+	.div		= dividers_mp13,
+	.nb_div		= ARRAY_SIZE(dividers_mp13),
+	.osci_data	= stm32mp13_osc_data,
+	.nb_osci_data	= ARRAY_SIZE(stm32mp13_osc_data),
+	.gate_refcounts	= refcounts_mp13,
+	.pdata		= &stm32mp13_clock_pdata,
+};
+
+static int stm32mp1_init_clock_tree(void)
+{
+	struct stm32_clk_priv *priv = clk_stm32_get_priv();
+	int ret;
+
+#if STM32MP_USB_PROGRAMMER
+	int usbphy_p = _clk_stm32_get_parent(priv, _USBPHY_K);
+	int usbo_p = _clk_stm32_get_parent(priv, _USBO_K);
+
+	/* Don't initialize PLL4, when used by BOOTROM */
+	pll4_bootrom = stm32mp1_clk_is_pll4_used_by_bootrom(priv, usbphy_p);
+#endif
+
+	/*
+	 * Switch ON oscillators found in device-tree.
+	 * Note: HSI already ON after BootROM stage.
+	 */
+	stm32_clk_oscillators_enable(priv);
+
+	/* Come back to HSI */
+	ret = stm32mp1_come_back_to_hsi();
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = stm32_clk_hsidiv_configure(priv);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = stm32_clk_stgen_configure(priv, _STGENC);
+	if (ret != 0) {
+		panic();
+	}
+
+	ret = stm32_clk_dividers_configure(priv);
+	if (ret != 0) {
+		panic();
+	}
+
+	ret = stm32_clk_pll_configure(priv);
+	if (ret != 0) {
+		panic();
+	}
+
+	/* Wait LSE ready before to use it */
+	ret = stm32_clk_oscillators_wait_lse_ready(priv);
+	if (ret != 0) {
+		panic();
+	}
+
+	/* Configure with expected clock source */
+	ret = stm32_clk_source_configure(priv);
+	if (ret != 0) {
+		panic();
+	}
+
+	/* Configure LSE css after RTC source configuration */
+	ret = stm32_clk_oscillators_lse_set_css(priv);
+	if (ret != 0) {
+		panic();
+	}
+
+#if STM32MP_USB_PROGRAMMER
+	ret = stm32mp1_clk_check_usb_conflict(priv, usbphy_p, usbo_p);
+	if (ret != 0) {
+		return ret;
+	}
+#endif
+	/* reconfigure STGEN with DT config */
+	ret = stm32_clk_stgen_configure(priv, _STGENC);
+	if (ret != 0) {
+		panic();
+	}
+
+	/* Software Self-Refresh mode (SSR) during DDR initilialization */
+	mmio_clrsetbits_32(priv->base + RCC_DDRITFCR,
+			   RCC_DDRITFCR_DDRCKMOD_MASK,
+			   RCC_DDRITFCR_DDRCKMOD_SSR <<
+			   RCC_DDRITFCR_DDRCKMOD_SHIFT);
+
+	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)
+{
+	int subnode = 0;
+
+	/* default value oscillator not found, freq=0 */
+	osci->freq = 0;
+
+	fdt_for_each_subnode(subnode, fdt, node) {
+		const char *cchar = NULL;
+		const fdt32_t *cuint = NULL;
+		int ret = 0;
+
+		cchar = fdt_get_name(fdt, subnode, &ret);
+		if (cchar == NULL) {
+			return ret;
+		}
+
+		if (strncmp(cchar, name, (size_t)ret) ||
+		    fdt_get_status(subnode) == DT_DISABLED) {
+			continue;
+		}
+
+		cuint = fdt_getprop(fdt, subnode, "clock-frequency", &ret);
+		if (cuint == NULL) {
+			return ret;
+		}
+
+		osci->freq = fdt32_to_cpu(*cuint);
+
+		if (fdt_getprop(fdt, subnode, "st,bypass", NULL) != NULL) {
+			osci->bypass = true;
+		}
+
+		if (fdt_getprop(fdt, subnode, "st,digbypass", NULL) != NULL) {
+			osci->digbyp = true;
+		}
+
+		if (fdt_getprop(fdt, subnode, "st,css", NULL) != NULL) {
+			osci->css = true;
+		}
+
+		osci->drive = fdt_read_uint32_default(fdt, subnode, "st,drive", LSEDRV_MEDIUM_HIGH);
+
+		return 0;
+	}
+
+	return 0;
+}
+
+static int stm32_clk_parse_fdt_all_oscillator(void *fdt, struct stm32_clk_platdata *pdata)
+{
+	int fdt_err = 0;
+	uint32_t i = 0;
+	int node = 0;
+
+	node = fdt_path_offset(fdt, "/clocks");
+	if (node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	for (i = 0; i < pdata->nosci; i++) {
+		const char *name = NULL;
+
+		name = clk_stm32_get_oscillator_name((enum stm32_osc)i);
+		if (name == NULL) {
+			continue;
+		}
+
+		fdt_err = clk_stm32_parse_oscillator_fdt(fdt, node, name, &pdata->osci[i]);
+		if (fdt_err < 0) {
+			panic();
+		}
+	}
+
+	return 0;
+}
+
+#define RCC_PLL_NAME_SIZE 12
+
+static int clk_stm32_load_vco_config(void *fdt, int subnode, struct stm32_pll_vco *vco)
+{
+	int err = 0;
+
+	err = fdt_read_uint32_array(fdt, subnode, "divmn", (int)PLL_DIV_MN_NB, vco->div_mn);
+	if (err != 0) {
+		return err;
+	}
+
+	err = fdt_read_uint32_array(fdt, subnode, "csg", (int)PLL_CSG_NB, vco->csg);
+
+	vco->csg_enabled = (err == 0);
+
+	if (err == -FDT_ERR_NOTFOUND) {
+		err = 0;
+	}
+
+	if (err != 0) {
+		return err;
+	}
+
+	vco->status = RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN | RCC_PLLNCR_DIVREN | RCC_PLLNCR_PLLON;
+
+	vco->frac = fdt_read_uint32_default(fdt, subnode, "frac", 0);
+
+	vco->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_output *output)
+{
+	int err = 0;
+
+	err = fdt_read_uint32_array(fdt, subnode, "st,pll_div_pqr", (int)PLL_DIV_PQR_NB,
+				    output->output);
+	if (err != 0) {
+		return err;
+	}
+
+	return 0;
+}
+
+static int clk_stm32_parse_pll_fdt(void *fdt, int subnode, struct stm32_pll_dt_cfg *pll)
+{
+	const fdt32_t *cuint = NULL;
+	int subnode_pll = 0;
+	int subnode_vco = 0;
+	int err = 0;
+
+	cuint = fdt_getprop(fdt, subnode, "st,pll", NULL);
+	if (!cuint) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	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) {
+		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->vco);
+	if (err != 0) {
+		return err;
+	}
+
+	err = clk_stm32_load_output_config(fdt, subnode_pll, &pll->output);
+	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 = 0;
+		int err = 0;
+
+		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_oscillator(fdt, pdata);
+	if (err != 0) {
+		return err;
+	}
+
+	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_init(void)
+{
+	return 0;
+}
+
+int stm32mp1_clk_probe(void)
+{
+	uintptr_t base = RCC_BASE;
+	int ret;
+
+	ret = stm32_clk_parse_fdt(&stm32mp13_clock_pdata);
+	if (ret != 0) {
+		return ret;
+	}
+
+	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;
+}
diff --git a/drivers/st/clk/stm32mp1_clk.c b/drivers/st/clk/stm32mp1_clk.c
new file mode 100644
index 0000000..c9c3c5f
--- /dev/null
+++ b/drivers/st/clk/stm32mp1_clk.c
@@ -0,0 +1,2373 @@
+/*
+ * Copyright (C) 2018-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdint.h>
+#include <stdio.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32mp_clkfunc.h>
+#include <drivers/st/stm32mp1_clk.h>
+#include <drivers/st/stm32mp1_rcc.h>
+#include <dt-bindings/clock/stm32mp1-clksrc.h>
+#include <lib/mmio.h>
+#include <lib/spinlock.h>
+#include <lib/utils_def.h>
+#include <libfdt.h>
+#include <plat/common/platform.h>
+
+#include <platform_def.h>
+
+#define MAX_HSI_HZ		64000000
+#define USB_PHY_48_MHZ		48000000
+
+#define TIMEOUT_US_200MS	U(200000)
+#define TIMEOUT_US_1S		U(1000000)
+
+#define PLLRDY_TIMEOUT		TIMEOUT_US_200MS
+#define CLKSRC_TIMEOUT		TIMEOUT_US_200MS
+#define CLKDIV_TIMEOUT		TIMEOUT_US_200MS
+#define HSIDIV_TIMEOUT		TIMEOUT_US_200MS
+#define OSCRDY_TIMEOUT		TIMEOUT_US_1S
+
+const char *stm32mp_osc_node_label[NB_OSC] = {
+	[_LSI] = "clk-lsi",
+	[_LSE] = "clk-lse",
+	[_HSI] = "clk-hsi",
+	[_HSE] = "clk-hse",
+	[_CSI] = "clk-csi",
+	[_I2S_CKIN] = "i2s_ckin",
+};
+
+enum stm32mp1_parent_id {
+/* Oscillators are defined in enum stm32mp_osc_id */
+
+/* Other parent source */
+	_HSI_KER = NB_OSC,
+	_HSE_KER,
+	_HSE_KER_DIV2,
+	_HSE_RTC,
+	_CSI_KER,
+	_PLL1_P,
+	_PLL1_Q,
+	_PLL1_R,
+	_PLL2_P,
+	_PLL2_Q,
+	_PLL2_R,
+	_PLL3_P,
+	_PLL3_Q,
+	_PLL3_R,
+	_PLL4_P,
+	_PLL4_Q,
+	_PLL4_R,
+	_ACLK,
+	_PCLK1,
+	_PCLK2,
+	_PCLK3,
+	_PCLK4,
+	_PCLK5,
+	_HCLK6,
+	_HCLK2,
+	_CK_PER,
+	_CK_MPU,
+	_CK_MCU,
+	_USB_PHY_48,
+	_PARENT_NB,
+	_UNKNOWN_ID = 0xff,
+};
+
+/* Lists only the parent clock we are interested in */
+enum stm32mp1_parent_sel {
+	_I2C12_SEL,
+	_I2C35_SEL,
+	_STGEN_SEL,
+	_I2C46_SEL,
+	_SPI6_SEL,
+	_UART1_SEL,
+	_RNG1_SEL,
+	_UART6_SEL,
+	_UART24_SEL,
+	_UART35_SEL,
+	_UART78_SEL,
+	_SDMMC12_SEL,
+	_SDMMC3_SEL,
+	_QSPI_SEL,
+	_FMC_SEL,
+	_AXIS_SEL,
+	_MCUS_SEL,
+	_USBPHY_SEL,
+	_USBO_SEL,
+	_MPU_SEL,
+	_CKPER_SEL,
+	_RTC_SEL,
+	_PARENT_SEL_NB,
+	_UNKNOWN_SEL = 0xff,
+};
+
+/* State the parent clock ID straight related to a clock */
+static const uint8_t parent_id_clock_id[_PARENT_NB] = {
+	[_HSE] = CK_HSE,
+	[_HSI] = CK_HSI,
+	[_CSI] = CK_CSI,
+	[_LSE] = CK_LSE,
+	[_LSI] = CK_LSI,
+	[_I2S_CKIN] = _UNKNOWN_ID,
+	[_USB_PHY_48] = _UNKNOWN_ID,
+	[_HSI_KER] = CK_HSI,
+	[_HSE_KER] = CK_HSE,
+	[_HSE_KER_DIV2] = CK_HSE_DIV2,
+	[_HSE_RTC] = _UNKNOWN_ID,
+	[_CSI_KER] = CK_CSI,
+	[_PLL1_P] = PLL1_P,
+	[_PLL1_Q] = PLL1_Q,
+	[_PLL1_R] = PLL1_R,
+	[_PLL2_P] = PLL2_P,
+	[_PLL2_Q] = PLL2_Q,
+	[_PLL2_R] = PLL2_R,
+	[_PLL3_P] = PLL3_P,
+	[_PLL3_Q] = PLL3_Q,
+	[_PLL3_R] = PLL3_R,
+	[_PLL4_P] = PLL4_P,
+	[_PLL4_Q] = PLL4_Q,
+	[_PLL4_R] = PLL4_R,
+	[_ACLK] = CK_AXI,
+	[_PCLK1] = CK_AXI,
+	[_PCLK2] = CK_AXI,
+	[_PCLK3] = CK_AXI,
+	[_PCLK4] = CK_AXI,
+	[_PCLK5] = CK_AXI,
+	[_CK_PER] = CK_PER,
+	[_CK_MPU] = CK_MPU,
+	[_CK_MCU] = CK_MCU,
+};
+
+static unsigned int clock_id2parent_id(unsigned long id)
+{
+	unsigned int n;
+
+	for (n = 0U; n < ARRAY_SIZE(parent_id_clock_id); n++) {
+		if (parent_id_clock_id[n] == id) {
+			return n;
+		}
+	}
+
+	return _UNKNOWN_ID;
+}
+
+enum stm32mp1_pll_id {
+	_PLL1,
+	_PLL2,
+	_PLL3,
+	_PLL4,
+	_PLL_NB
+};
+
+enum stm32mp1_div_id {
+	_DIV_P,
+	_DIV_Q,
+	_DIV_R,
+	_DIV_NB,
+};
+
+enum stm32mp1_clksrc_id {
+	CLKSRC_MPU,
+	CLKSRC_AXI,
+	CLKSRC_MCU,
+	CLKSRC_PLL12,
+	CLKSRC_PLL3,
+	CLKSRC_PLL4,
+	CLKSRC_RTC,
+	CLKSRC_MCO1,
+	CLKSRC_MCO2,
+	CLKSRC_NB
+};
+
+enum stm32mp1_clkdiv_id {
+	CLKDIV_MPU,
+	CLKDIV_AXI,
+	CLKDIV_MCU,
+	CLKDIV_APB1,
+	CLKDIV_APB2,
+	CLKDIV_APB3,
+	CLKDIV_APB4,
+	CLKDIV_APB5,
+	CLKDIV_RTC,
+	CLKDIV_MCO1,
+	CLKDIV_MCO2,
+	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,
+	PLL_TYPE_NB
+};
+
+struct stm32mp1_pll {
+	uint8_t refclk_min;
+	uint8_t refclk_max;
+};
+
+struct stm32mp1_clk_gate {
+	uint16_t offset;
+	uint8_t bit;
+	uint8_t index;
+	uint8_t set_clr;
+	uint8_t secure;
+	uint8_t sel; /* Relates to enum stm32mp1_parent_sel */
+	uint8_t fixed; /* Relates to enum stm32mp1_parent_id */
+};
+
+struct stm32mp1_clk_sel {
+	uint16_t offset;
+	uint8_t src;
+	uint8_t msk;
+	uint8_t nb_parent;
+	const uint8_t *parent;
+};
+
+#define REFCLK_SIZE 4
+struct stm32mp1_clk_pll {
+	enum stm32mp1_plltype plltype;
+	uint16_t rckxselr;
+	uint16_t pllxcfgr1;
+	uint16_t pllxcfgr2;
+	uint16_t pllxfracr;
+	uint16_t pllxcr;
+	uint16_t pllxcsgr;
+	enum stm32mp_osc_id refclk[REFCLK_SIZE];
+};
+
+/* Clocks with selectable source and non set/clr register access */
+#define _CLK_SELEC(sec, off, b, idx, s)			\
+	{						\
+		.offset = (off),			\
+		.bit = (b),				\
+		.index = (idx),				\
+		.set_clr = 0,				\
+		.secure = (sec),			\
+		.sel = (s),				\
+		.fixed = _UNKNOWN_ID,			\
+	}
+
+/* Clocks with fixed source and non set/clr register access */
+#define _CLK_FIXED(sec, off, b, idx, f)			\
+	{						\
+		.offset = (off),			\
+		.bit = (b),				\
+		.index = (idx),				\
+		.set_clr = 0,				\
+		.secure = (sec),			\
+		.sel = _UNKNOWN_SEL,			\
+		.fixed = (f),				\
+	}
+
+/* Clocks with selectable source and set/clr register access */
+#define _CLK_SC_SELEC(sec, off, b, idx, s)			\
+	{						\
+		.offset = (off),			\
+		.bit = (b),				\
+		.index = (idx),				\
+		.set_clr = 1,				\
+		.secure = (sec),			\
+		.sel = (s),				\
+		.fixed = _UNKNOWN_ID,			\
+	}
+
+/* Clocks with fixed source and set/clr register access */
+#define _CLK_SC_FIXED(sec, off, b, idx, f)			\
+	{						\
+		.offset = (off),			\
+		.bit = (b),				\
+		.index = (idx),				\
+		.set_clr = 1,				\
+		.secure = (sec),			\
+		.sel = _UNKNOWN_SEL,			\
+		.fixed = (f),				\
+	}
+
+#define _CLK_PARENT_SEL(_label, _rcc_selr, _parents)		\
+	[_ ## _label ## _SEL] = {				\
+		.offset = _rcc_selr,				\
+		.src = _rcc_selr ## _ ## _label ## SRC_SHIFT,	\
+		.msk = (_rcc_selr ## _ ## _label ## SRC_MASK) >> \
+		       (_rcc_selr ## _ ## _label ## SRC_SHIFT), \
+		.parent = (_parents),				\
+		.nb_parent = ARRAY_SIZE(_parents)		\
+	}
+
+#define _CLK_PLL(idx, type, off1, off2, off3,		\
+		 off4, off5, off6,			\
+		 p1, p2, p3, p4)			\
+	[(idx)] = {					\
+		.plltype = (type),			\
+		.rckxselr = (off1),			\
+		.pllxcfgr1 = (off2),			\
+		.pllxcfgr2 = (off3),			\
+		.pllxfracr = (off4),			\
+		.pllxcr = (off5),			\
+		.pllxcsgr = (off6),			\
+		.refclk[0] = (p1),			\
+		.refclk[1] = (p2),			\
+		.refclk[2] = (p3),			\
+		.refclk[3] = (p4),			\
+	}
+
+#define NB_GATES	ARRAY_SIZE(stm32mp1_clk_gate)
+
+#define SEC		1
+#define N_S		0
+
+static const struct stm32mp1_clk_gate stm32mp1_clk_gate[] = {
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 0, DDRC1, _ACLK),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 1, DDRC1LP, _ACLK),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 2, DDRC2, _ACLK),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 3, DDRC2LP, _ACLK),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 4, DDRPHYC, _PLL2_R),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 5, DDRPHYCLP, _PLL2_R),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 6, DDRCAPB, _PCLK4),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 7, DDRCAPBLP, _PCLK4),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 8, AXIDCG, _ACLK),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 9, DDRPHYCAPB, _PCLK4),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 10, DDRPHYCAPBLP, _PCLK4),
+
+#if defined(IMAGE_BL32)
+	_CLK_SC_FIXED(N_S, RCC_MP_APB1ENSETR, 6, TIM12_K, _PCLK1),
+#endif
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 14, USART2_K, _UART24_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 15, USART3_K, _UART35_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 16, UART4_K, _UART24_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 17, UART5_K, _UART35_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 18, UART7_K, _UART78_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 19, UART8_K, _UART78_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 21, I2C1_K, _I2C12_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 22, I2C2_K, _I2C12_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 23, I2C3_K, _I2C35_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 24, I2C5_K, _I2C35_SEL),
+
+#if defined(IMAGE_BL32)
+	_CLK_SC_FIXED(N_S, RCC_MP_APB2ENSETR, 2, TIM15_K, _PCLK2),
+#endif
+	_CLK_SC_SELEC(N_S, RCC_MP_APB2ENSETR, 13, USART6_K, _UART6_SEL),
+
+	_CLK_SC_FIXED(N_S, RCC_MP_APB3ENSETR, 11, SYSCFG, _UNKNOWN_ID),
+
+	_CLK_SC_SELEC(N_S, RCC_MP_APB4ENSETR, 8, DDRPERFM, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB4ENSETR, 15, IWDG2, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB4ENSETR, 16, USBPHY_K, _USBPHY_SEL),
+
+	_CLK_SC_SELEC(SEC, RCC_MP_APB5ENSETR, 0, SPI6_K, _SPI6_SEL),
+	_CLK_SC_SELEC(SEC, RCC_MP_APB5ENSETR, 2, I2C4_K, _I2C46_SEL),
+	_CLK_SC_SELEC(SEC, RCC_MP_APB5ENSETR, 3, I2C6_K, _I2C46_SEL),
+	_CLK_SC_SELEC(SEC, RCC_MP_APB5ENSETR, 4, USART1_K, _UART1_SEL),
+	_CLK_SC_FIXED(SEC, RCC_MP_APB5ENSETR, 8, RTCAPB, _PCLK5),
+	_CLK_SC_FIXED(SEC, RCC_MP_APB5ENSETR, 11, TZC1, _PCLK5),
+	_CLK_SC_FIXED(SEC, RCC_MP_APB5ENSETR, 12, TZC2, _PCLK5),
+	_CLK_SC_FIXED(SEC, RCC_MP_APB5ENSETR, 13, TZPC, _PCLK5),
+	_CLK_SC_FIXED(SEC, RCC_MP_APB5ENSETR, 15, IWDG1, _PCLK5),
+	_CLK_SC_FIXED(SEC, RCC_MP_APB5ENSETR, 16, BSEC, _PCLK5),
+	_CLK_SC_SELEC(SEC, RCC_MP_APB5ENSETR, 20, STGEN_K, _STGEN_SEL),
+
+#if defined(IMAGE_BL32)
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB2ENSETR, 8, USBO_K, _USBO_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB2ENSETR, 16, SDMMC3_K, _SDMMC3_SEL),
+#endif
+
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 0, GPIOA, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 1, GPIOB, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 2, GPIOC, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 3, GPIOD, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 4, GPIOE, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 5, GPIOF, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 6, GPIOG, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 7, GPIOH, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 8, GPIOI, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 9, GPIOJ, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 10, GPIOK, _UNKNOWN_SEL),
+
+	_CLK_SC_FIXED(SEC, RCC_MP_AHB5ENSETR, 0, GPIOZ, _PCLK5),
+	_CLK_SC_FIXED(SEC, RCC_MP_AHB5ENSETR, 4, CRYP1, _PCLK5),
+	_CLK_SC_FIXED(SEC, RCC_MP_AHB5ENSETR, 5, HASH1, _PCLK5),
+	_CLK_SC_SELEC(SEC, RCC_MP_AHB5ENSETR, 6, RNG1_K, _RNG1_SEL),
+	_CLK_SC_FIXED(SEC, RCC_MP_AHB5ENSETR, 8, BKPSRAM, _PCLK5),
+
+#if defined(IMAGE_BL2)
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB6ENSETR, 12, FMC_K, _FMC_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB6ENSETR, 14, QSPI_K, _QSPI_SEL),
+#endif
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB6ENSETR, 16, SDMMC1_K, _SDMMC12_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB6ENSETR, 17, SDMMC2_K, _SDMMC12_SEL),
+#if defined(IMAGE_BL32)
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB6ENSETR, 24, USBH, _UNKNOWN_SEL),
+#endif
+
+	_CLK_SELEC(SEC, RCC_BDCR, 20, RTC, _RTC_SEL),
+	_CLK_SELEC(N_S, RCC_DBGCFGR, 8, CK_DBG, _UNKNOWN_SEL),
+};
+
+static const uint8_t i2c12_parents[] = {
+	_PCLK1, _PLL4_R, _HSI_KER, _CSI_KER
+};
+
+static const uint8_t i2c35_parents[] = {
+	_PCLK1, _PLL4_R, _HSI_KER, _CSI_KER
+};
+
+static const uint8_t stgen_parents[] = {
+	_HSI_KER, _HSE_KER
+};
+
+static const uint8_t i2c46_parents[] = {
+	_PCLK5, _PLL3_Q, _HSI_KER, _CSI_KER
+};
+
+static const uint8_t spi6_parents[] = {
+	_PCLK5, _PLL4_Q, _HSI_KER, _CSI_KER, _HSE_KER, _PLL3_Q
+};
+
+static const uint8_t usart1_parents[] = {
+	_PCLK5, _PLL3_Q, _HSI_KER, _CSI_KER, _PLL4_Q, _HSE_KER
+};
+
+static const uint8_t rng1_parents[] = {
+	_CSI, _PLL4_R, _LSE, _LSI
+};
+
+static const uint8_t uart6_parents[] = {
+	_PCLK2, _PLL4_Q, _HSI_KER, _CSI_KER, _HSE_KER
+};
+
+static const uint8_t uart234578_parents[] = {
+	_PCLK1, _PLL4_Q, _HSI_KER, _CSI_KER, _HSE_KER
+};
+
+static const uint8_t sdmmc12_parents[] = {
+	_HCLK6, _PLL3_R, _PLL4_P, _HSI_KER
+};
+
+static const uint8_t sdmmc3_parents[] = {
+	_HCLK2, _PLL3_R, _PLL4_P, _HSI_KER
+};
+
+static const uint8_t qspi_parents[] = {
+	_ACLK, _PLL3_R, _PLL4_P, _CK_PER
+};
+
+static const uint8_t fmc_parents[] = {
+	_ACLK, _PLL3_R, _PLL4_P, _CK_PER
+};
+
+static const uint8_t axiss_parents[] = {
+	_HSI, _HSE, _PLL2_P
+};
+
+static const uint8_t mcuss_parents[] = {
+	_HSI, _HSE, _CSI, _PLL3_P
+};
+
+static const uint8_t usbphy_parents[] = {
+	_HSE_KER, _PLL4_R, _HSE_KER_DIV2
+};
+
+static const uint8_t usbo_parents[] = {
+	_PLL4_R, _USB_PHY_48
+};
+
+static const uint8_t mpu_parents[] = {
+	_HSI, _HSE, _PLL1_P, _PLL1_P /* specific div */
+};
+
+static const uint8_t per_parents[] = {
+	_HSI, _HSE, _CSI,
+};
+
+static const uint8_t rtc_parents[] = {
+	_UNKNOWN_ID, _LSE, _LSI, _HSE_RTC
+};
+
+static const struct stm32mp1_clk_sel stm32mp1_clk_sel[_PARENT_SEL_NB] = {
+	_CLK_PARENT_SEL(I2C12, RCC_I2C12CKSELR, i2c12_parents),
+	_CLK_PARENT_SEL(I2C35, RCC_I2C35CKSELR, i2c35_parents),
+	_CLK_PARENT_SEL(STGEN, RCC_STGENCKSELR, stgen_parents),
+	_CLK_PARENT_SEL(I2C46, RCC_I2C46CKSELR, i2c46_parents),
+	_CLK_PARENT_SEL(SPI6, RCC_SPI6CKSELR, spi6_parents),
+	_CLK_PARENT_SEL(UART1, RCC_UART1CKSELR, usart1_parents),
+	_CLK_PARENT_SEL(RNG1, RCC_RNG1CKSELR, rng1_parents),
+	_CLK_PARENT_SEL(MPU, RCC_MPCKSELR, mpu_parents),
+	_CLK_PARENT_SEL(CKPER, RCC_CPERCKSELR, per_parents),
+	_CLK_PARENT_SEL(RTC, RCC_BDCR, rtc_parents),
+	_CLK_PARENT_SEL(UART6, RCC_UART6CKSELR, uart6_parents),
+	_CLK_PARENT_SEL(UART24, RCC_UART24CKSELR, uart234578_parents),
+	_CLK_PARENT_SEL(UART35, RCC_UART35CKSELR, uart234578_parents),
+	_CLK_PARENT_SEL(UART78, RCC_UART78CKSELR, uart234578_parents),
+	_CLK_PARENT_SEL(SDMMC12, RCC_SDMMC12CKSELR, sdmmc12_parents),
+	_CLK_PARENT_SEL(SDMMC3, RCC_SDMMC3CKSELR, sdmmc3_parents),
+	_CLK_PARENT_SEL(QSPI, RCC_QSPICKSELR, qspi_parents),
+	_CLK_PARENT_SEL(FMC, RCC_FMCCKSELR, fmc_parents),
+	_CLK_PARENT_SEL(AXIS, RCC_ASSCKSELR, axiss_parents),
+	_CLK_PARENT_SEL(MCUS, RCC_MSSCKSELR, mcuss_parents),
+	_CLK_PARENT_SEL(USBPHY, RCC_USBCKSELR, usbphy_parents),
+	_CLK_PARENT_SEL(USBO, RCC_USBCKSELR, usbo_parents),
+};
+
+/* Define characteristic of PLL according type */
+#define DIVN_MIN	24
+static const struct stm32mp1_pll stm32mp1_pll[PLL_TYPE_NB] = {
+	[PLL_800] = {
+		.refclk_min = 4,
+		.refclk_max = 16,
+	},
+	[PLL_1600] = {
+		.refclk_min = 8,
+		.refclk_max = 16,
+	},
+};
+
+/* PLLNCFGR2 register divider by output */
+static const uint8_t pllncfgr2[_DIV_NB] = {
+	[_DIV_P] = RCC_PLLNCFGR2_DIVP_SHIFT,
+	[_DIV_Q] = RCC_PLLNCFGR2_DIVQ_SHIFT,
+	[_DIV_R] = RCC_PLLNCFGR2_DIVR_SHIFT,
+};
+
+static const struct stm32mp1_clk_pll stm32mp1_clk_pll[_PLL_NB] = {
+	_CLK_PLL(_PLL1, PLL_1600,
+		 RCC_RCK12SELR, RCC_PLL1CFGR1, RCC_PLL1CFGR2,
+		 RCC_PLL1FRACR, RCC_PLL1CR, RCC_PLL1CSGR,
+		 _HSI, _HSE, _UNKNOWN_OSC_ID, _UNKNOWN_OSC_ID),
+	_CLK_PLL(_PLL2, PLL_1600,
+		 RCC_RCK12SELR, RCC_PLL2CFGR1, RCC_PLL2CFGR2,
+		 RCC_PLL2FRACR, RCC_PLL2CR, RCC_PLL2CSGR,
+		 _HSI, _HSE, _UNKNOWN_OSC_ID, _UNKNOWN_OSC_ID),
+	_CLK_PLL(_PLL3, PLL_800,
+		 RCC_RCK3SELR, RCC_PLL3CFGR1, RCC_PLL3CFGR2,
+		 RCC_PLL3FRACR, RCC_PLL3CR, RCC_PLL3CSGR,
+		 _HSI, _HSE, _CSI, _UNKNOWN_OSC_ID),
+	_CLK_PLL(_PLL4, PLL_800,
+		 RCC_RCK4SELR, RCC_PLL4CFGR1, RCC_PLL4CFGR2,
+		 RCC_PLL4FRACR, RCC_PLL4CR, RCC_PLL4CSGR,
+		 _HSI, _HSE, _CSI, _I2S_CKIN),
+};
+
+/* Prescaler table lookups for clock computation */
+/* div = /1 /2 /4 /8 / 16 /64 /128 /512 */
+static const uint8_t stm32mp1_mcu_div[16] = {
+	0, 1, 2, 3, 4, 6, 7, 8, 9, 9, 9, 9, 9, 9, 9, 9
+};
+
+/* div = /1 /2 /4 /8 /16 : same divider for PMU and APBX */
+#define stm32mp1_mpu_div stm32mp1_mpu_apbx_div
+#define stm32mp1_apbx_div stm32mp1_mpu_apbx_div
+static const uint8_t stm32mp1_mpu_apbx_div[8] = {
+	0, 1, 2, 3, 4, 4, 4, 4
+};
+
+/* div = /1 /2 /3 /4 */
+static const uint8_t stm32mp1_axi_div[8] = {
+	1, 2, 3, 4, 4, 4, 4, 4
+};
+
+static const char * const stm32mp1_clk_parent_name[_PARENT_NB] __unused = {
+	[_HSI] = "HSI",
+	[_HSE] = "HSE",
+	[_CSI] = "CSI",
+	[_LSI] = "LSI",
+	[_LSE] = "LSE",
+	[_I2S_CKIN] = "I2S_CKIN",
+	[_HSI_KER] = "HSI_KER",
+	[_HSE_KER] = "HSE_KER",
+	[_HSE_KER_DIV2] = "HSE_KER_DIV2",
+	[_HSE_RTC] = "HSE_RTC",
+	[_CSI_KER] = "CSI_KER",
+	[_PLL1_P] = "PLL1_P",
+	[_PLL1_Q] = "PLL1_Q",
+	[_PLL1_R] = "PLL1_R",
+	[_PLL2_P] = "PLL2_P",
+	[_PLL2_Q] = "PLL2_Q",
+	[_PLL2_R] = "PLL2_R",
+	[_PLL3_P] = "PLL3_P",
+	[_PLL3_Q] = "PLL3_Q",
+	[_PLL3_R] = "PLL3_R",
+	[_PLL4_P] = "PLL4_P",
+	[_PLL4_Q] = "PLL4_Q",
+	[_PLL4_R] = "PLL4_R",
+	[_ACLK] = "ACLK",
+	[_PCLK1] = "PCLK1",
+	[_PCLK2] = "PCLK2",
+	[_PCLK3] = "PCLK3",
+	[_PCLK4] = "PCLK4",
+	[_PCLK5] = "PCLK5",
+	[_HCLK6] = "KCLK6",
+	[_HCLK2] = "HCLK2",
+	[_CK_PER] = "CK_PER",
+	[_CK_MPU] = "CK_MPU",
+	[_CK_MCU] = "CK_MCU",
+	[_USB_PHY_48] = "USB_PHY_48",
+};
+
+/* RCC clock device driver private */
+static unsigned long stm32mp1_osc[NB_OSC];
+static struct spinlock reg_lock;
+static unsigned int gate_refcounts[NB_GATES];
+static struct spinlock refcount_lock;
+
+static const struct stm32mp1_clk_gate *gate_ref(unsigned int idx)
+{
+	return &stm32mp1_clk_gate[idx];
+}
+
+#if defined(IMAGE_BL32)
+static bool gate_is_non_secure(const struct stm32mp1_clk_gate *gate)
+{
+	return gate->secure == N_S;
+}
+#endif
+
+static const struct stm32mp1_clk_sel *clk_sel_ref(unsigned int idx)
+{
+	return &stm32mp1_clk_sel[idx];
+}
+
+static const struct stm32mp1_clk_pll *pll_ref(unsigned int idx)
+{
+	return &stm32mp1_clk_pll[idx];
+}
+
+static void stm32mp1_clk_lock(struct spinlock *lock)
+{
+	if (stm32mp_lock_available()) {
+		/* Assume interrupts are masked */
+		spin_lock(lock);
+	}
+}
+
+static void stm32mp1_clk_unlock(struct spinlock *lock)
+{
+	if (stm32mp_lock_available()) {
+		spin_unlock(lock);
+	}
+}
+
+bool stm32mp1_rcc_is_secure(void)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	uint32_t mask = RCC_TZCR_TZEN;
+
+	return (mmio_read_32(rcc_base + RCC_TZCR) & mask) == mask;
+}
+
+bool stm32mp1_rcc_is_mckprot(void)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	uint32_t mask = RCC_TZCR_TZEN | RCC_TZCR_MCKPROT;
+
+	return (mmio_read_32(rcc_base + RCC_TZCR) & mask) == mask;
+}
+
+void stm32mp1_clk_rcc_regs_lock(void)
+{
+	stm32mp1_clk_lock(&reg_lock);
+}
+
+void stm32mp1_clk_rcc_regs_unlock(void)
+{
+	stm32mp1_clk_unlock(&reg_lock);
+}
+
+static unsigned long stm32mp1_clk_get_fixed(enum stm32mp_osc_id idx)
+{
+	if (idx >= NB_OSC) {
+		return 0;
+	}
+
+	return stm32mp1_osc[idx];
+}
+
+static int stm32mp1_clk_get_gated_id(unsigned long id)
+{
+	unsigned int i;
+
+	for (i = 0U; i < NB_GATES; i++) {
+		if (gate_ref(i)->index == id) {
+			return i;
+		}
+	}
+
+	ERROR("%s: clk id %lu not found\n", __func__, id);
+
+	return -EINVAL;
+}
+
+static enum stm32mp1_parent_sel stm32mp1_clk_get_sel(int i)
+{
+	return (enum stm32mp1_parent_sel)(gate_ref(i)->sel);
+}
+
+static enum stm32mp1_parent_id stm32mp1_clk_get_fixed_parent(int i)
+{
+	return (enum stm32mp1_parent_id)(gate_ref(i)->fixed);
+}
+
+static int stm32mp1_clk_get_parent(unsigned long id)
+{
+	const struct stm32mp1_clk_sel *sel;
+	uint32_t p_sel;
+	int i;
+	enum stm32mp1_parent_id p;
+	enum stm32mp1_parent_sel s;
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	/* Few non gateable clock have a static parent ID, find them */
+	i = (int)clock_id2parent_id(id);
+	if (i != _UNKNOWN_ID) {
+		return i;
+	}
+
+	i = stm32mp1_clk_get_gated_id(id);
+	if (i < 0) {
+		panic();
+	}
+
+	p = stm32mp1_clk_get_fixed_parent(i);
+	if (p < _PARENT_NB) {
+		return (int)p;
+	}
+
+	s = stm32mp1_clk_get_sel(i);
+	if (s == _UNKNOWN_SEL) {
+		return -EINVAL;
+	}
+	if (s >= _PARENT_SEL_NB) {
+		panic();
+	}
+
+	sel = clk_sel_ref(s);
+	p_sel = (mmio_read_32(rcc_base + sel->offset) &
+		 (sel->msk << sel->src)) >> sel->src;
+	if (p_sel < sel->nb_parent) {
+		return (int)sel->parent[p_sel];
+	}
+
+	return -EINVAL;
+}
+
+static unsigned long stm32mp1_pll_get_fref(const struct stm32mp1_clk_pll *pll)
+{
+	uint32_t selr = mmio_read_32(stm32mp_rcc_base() + pll->rckxselr);
+	uint32_t src = selr & RCC_SELR_REFCLK_SRC_MASK;
+
+	return stm32mp1_clk_get_fixed(pll->refclk[src]);
+}
+
+/*
+ * pll_get_fvco() : return the VCO or (VCO / 2) frequency for the requested PLL
+ * - PLL1 & PLL2 => return VCO / 2 with Fpll_y_ck = FVCO / 2 * (DIVy + 1)
+ * - PLL3 & PLL4 => return VCO     with Fpll_y_ck = FVCO / (DIVy + 1)
+ * => in all cases Fpll_y_ck = pll_get_fvco() / (DIVy + 1)
+ */
+static unsigned long stm32mp1_pll_get_fvco(const struct stm32mp1_clk_pll *pll)
+{
+	unsigned long refclk, fvco;
+	uint32_t cfgr1, fracr, divm, divn;
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	cfgr1 = mmio_read_32(rcc_base + pll->pllxcfgr1);
+	fracr = mmio_read_32(rcc_base + pll->pllxfracr);
+
+	divm = (cfgr1 & (RCC_PLLNCFGR1_DIVM_MASK)) >> RCC_PLLNCFGR1_DIVM_SHIFT;
+	divn = cfgr1 & RCC_PLLNCFGR1_DIVN_MASK;
+
+	refclk = stm32mp1_pll_get_fref(pll);
+
+	/*
+	 * With FRACV :
+	 *   Fvco = Fck_ref * ((DIVN + 1) + FRACV / 2^13) / (DIVM + 1)
+	 * Without FRACV
+	 *   Fvco = Fck_ref * ((DIVN + 1) / (DIVM + 1)
+	 */
+	if ((fracr & RCC_PLLNFRACR_FRACLE) != 0U) {
+		uint32_t fracv = (fracr & RCC_PLLNFRACR_FRACV_MASK) >>
+				 RCC_PLLNFRACR_FRACV_SHIFT;
+		unsigned long long numerator, denominator;
+
+		numerator = (((unsigned long long)divn + 1U) << 13) + fracv;
+		numerator = refclk * numerator;
+		denominator = ((unsigned long long)divm + 1U) << 13;
+		fvco = (unsigned long)(numerator / denominator);
+	} else {
+		fvco = (unsigned long)(refclk * (divn + 1U) / (divm + 1U));
+	}
+
+	return fvco;
+}
+
+static unsigned long stm32mp1_read_pll_freq(enum stm32mp1_pll_id pll_id,
+					    enum stm32mp1_div_id div_id)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	unsigned long dfout;
+	uint32_t cfgr2, divy;
+
+	if (div_id >= _DIV_NB) {
+		return 0;
+	}
+
+	cfgr2 = mmio_read_32(stm32mp_rcc_base() + pll->pllxcfgr2);
+	divy = (cfgr2 >> pllncfgr2[div_id]) & RCC_PLLNCFGR2_DIVX_MASK;
+
+	dfout = stm32mp1_pll_get_fvco(pll) / (divy + 1U);
+
+	return dfout;
+}
+
+static unsigned long get_clock_rate(int p)
+{
+	uint32_t reg, clkdiv;
+	unsigned long clock = 0;
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	switch (p) {
+	case _CK_MPU:
+	/* MPU sub system */
+		reg = mmio_read_32(rcc_base + RCC_MPCKSELR);
+		switch (reg & RCC_SELR_SRC_MASK) {
+		case RCC_MPCKSELR_HSI:
+			clock = stm32mp1_clk_get_fixed(_HSI);
+			break;
+		case RCC_MPCKSELR_HSE:
+			clock = stm32mp1_clk_get_fixed(_HSE);
+			break;
+		case RCC_MPCKSELR_PLL:
+			clock = stm32mp1_read_pll_freq(_PLL1, _DIV_P);
+			break;
+		case RCC_MPCKSELR_PLL_MPUDIV:
+			clock = stm32mp1_read_pll_freq(_PLL1, _DIV_P);
+
+			reg = mmio_read_32(rcc_base + RCC_MPCKDIVR);
+			clkdiv = reg & RCC_MPUDIV_MASK;
+			clock >>= stm32mp1_mpu_div[clkdiv];
+			break;
+		default:
+			break;
+		}
+		break;
+	/* AXI sub system */
+	case _ACLK:
+	case _HCLK2:
+	case _HCLK6:
+	case _PCLK4:
+	case _PCLK5:
+		reg = mmio_read_32(rcc_base + RCC_ASSCKSELR);
+		switch (reg & RCC_SELR_SRC_MASK) {
+		case RCC_ASSCKSELR_HSI:
+			clock = stm32mp1_clk_get_fixed(_HSI);
+			break;
+		case RCC_ASSCKSELR_HSE:
+			clock = stm32mp1_clk_get_fixed(_HSE);
+			break;
+		case RCC_ASSCKSELR_PLL:
+			clock = stm32mp1_read_pll_freq(_PLL2, _DIV_P);
+			break;
+		default:
+			break;
+		}
+
+		/* System clock divider */
+		reg = mmio_read_32(rcc_base + RCC_AXIDIVR);
+		clock /= stm32mp1_axi_div[reg & RCC_AXIDIV_MASK];
+
+		switch (p) {
+		case _PCLK4:
+			reg = mmio_read_32(rcc_base + RCC_APB4DIVR);
+			clock >>= stm32mp1_apbx_div[reg & RCC_APBXDIV_MASK];
+			break;
+		case _PCLK5:
+			reg = mmio_read_32(rcc_base + RCC_APB5DIVR);
+			clock >>= stm32mp1_apbx_div[reg & RCC_APBXDIV_MASK];
+			break;
+		default:
+			break;
+		}
+		break;
+	/* MCU sub system */
+	case _CK_MCU:
+	case _PCLK1:
+	case _PCLK2:
+	case _PCLK3:
+		reg = mmio_read_32(rcc_base + RCC_MSSCKSELR);
+		switch (reg & RCC_SELR_SRC_MASK) {
+		case RCC_MSSCKSELR_HSI:
+			clock = stm32mp1_clk_get_fixed(_HSI);
+			break;
+		case RCC_MSSCKSELR_HSE:
+			clock = stm32mp1_clk_get_fixed(_HSE);
+			break;
+		case RCC_MSSCKSELR_CSI:
+			clock = stm32mp1_clk_get_fixed(_CSI);
+			break;
+		case RCC_MSSCKSELR_PLL:
+			clock = stm32mp1_read_pll_freq(_PLL3, _DIV_P);
+			break;
+		default:
+			break;
+		}
+
+		/* MCU clock divider */
+		reg = mmio_read_32(rcc_base + RCC_MCUDIVR);
+		clock >>= stm32mp1_mcu_div[reg & RCC_MCUDIV_MASK];
+
+		switch (p) {
+		case _PCLK1:
+			reg = mmio_read_32(rcc_base + RCC_APB1DIVR);
+			clock >>= stm32mp1_apbx_div[reg & RCC_APBXDIV_MASK];
+			break;
+		case _PCLK2:
+			reg = mmio_read_32(rcc_base + RCC_APB2DIVR);
+			clock >>= stm32mp1_apbx_div[reg & RCC_APBXDIV_MASK];
+			break;
+		case _PCLK3:
+			reg = mmio_read_32(rcc_base + RCC_APB3DIVR);
+			clock >>= stm32mp1_apbx_div[reg & RCC_APBXDIV_MASK];
+			break;
+		case _CK_MCU:
+		default:
+			break;
+		}
+		break;
+	case _CK_PER:
+		reg = mmio_read_32(rcc_base + RCC_CPERCKSELR);
+		switch (reg & RCC_SELR_SRC_MASK) {
+		case RCC_CPERCKSELR_HSI:
+			clock = stm32mp1_clk_get_fixed(_HSI);
+			break;
+		case RCC_CPERCKSELR_HSE:
+			clock = stm32mp1_clk_get_fixed(_HSE);
+			break;
+		case RCC_CPERCKSELR_CSI:
+			clock = stm32mp1_clk_get_fixed(_CSI);
+			break;
+		default:
+			break;
+		}
+		break;
+	case _HSI:
+	case _HSI_KER:
+		clock = stm32mp1_clk_get_fixed(_HSI);
+		break;
+	case _CSI:
+	case _CSI_KER:
+		clock = stm32mp1_clk_get_fixed(_CSI);
+		break;
+	case _HSE:
+	case _HSE_KER:
+		clock = stm32mp1_clk_get_fixed(_HSE);
+		break;
+	case _HSE_KER_DIV2:
+		clock = stm32mp1_clk_get_fixed(_HSE) >> 1;
+		break;
+	case _HSE_RTC:
+		clock = stm32mp1_clk_get_fixed(_HSE);
+		clock /= (mmio_read_32(rcc_base + RCC_RTCDIVR) & RCC_DIVR_DIV_MASK) + 1U;
+		break;
+	case _LSI:
+		clock = stm32mp1_clk_get_fixed(_LSI);
+		break;
+	case _LSE:
+		clock = stm32mp1_clk_get_fixed(_LSE);
+		break;
+	/* PLL */
+	case _PLL1_P:
+		clock = stm32mp1_read_pll_freq(_PLL1, _DIV_P);
+		break;
+	case _PLL1_Q:
+		clock = stm32mp1_read_pll_freq(_PLL1, _DIV_Q);
+		break;
+	case _PLL1_R:
+		clock = stm32mp1_read_pll_freq(_PLL1, _DIV_R);
+		break;
+	case _PLL2_P:
+		clock = stm32mp1_read_pll_freq(_PLL2, _DIV_P);
+		break;
+	case _PLL2_Q:
+		clock = stm32mp1_read_pll_freq(_PLL2, _DIV_Q);
+		break;
+	case _PLL2_R:
+		clock = stm32mp1_read_pll_freq(_PLL2, _DIV_R);
+		break;
+	case _PLL3_P:
+		clock = stm32mp1_read_pll_freq(_PLL3, _DIV_P);
+		break;
+	case _PLL3_Q:
+		clock = stm32mp1_read_pll_freq(_PLL3, _DIV_Q);
+		break;
+	case _PLL3_R:
+		clock = stm32mp1_read_pll_freq(_PLL3, _DIV_R);
+		break;
+	case _PLL4_P:
+		clock = stm32mp1_read_pll_freq(_PLL4, _DIV_P);
+		break;
+	case _PLL4_Q:
+		clock = stm32mp1_read_pll_freq(_PLL4, _DIV_Q);
+		break;
+	case _PLL4_R:
+		clock = stm32mp1_read_pll_freq(_PLL4, _DIV_R);
+		break;
+	/* Other */
+	case _USB_PHY_48:
+		clock = USB_PHY_48_MHZ;
+		break;
+	default:
+		break;
+	}
+
+	return clock;
+}
+
+static void __clk_enable(struct stm32mp1_clk_gate const *gate)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	VERBOSE("Enable clock %u\n", gate->index);
+
+	if (gate->set_clr != 0U) {
+		mmio_write_32(rcc_base + gate->offset, BIT(gate->bit));
+	} else {
+		mmio_setbits_32(rcc_base + gate->offset, BIT(gate->bit));
+	}
+}
+
+static void __clk_disable(struct stm32mp1_clk_gate const *gate)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	VERBOSE("Disable clock %u\n", gate->index);
+
+	if (gate->set_clr != 0U) {
+		mmio_write_32(rcc_base + gate->offset + RCC_MP_ENCLRR_OFFSET,
+			      BIT(gate->bit));
+	} else {
+		mmio_clrbits_32(rcc_base + gate->offset, BIT(gate->bit));
+	}
+}
+
+static bool __clk_is_enabled(struct stm32mp1_clk_gate const *gate)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	return mmio_read_32(rcc_base + gate->offset) & BIT(gate->bit);
+}
+
+/* Oscillators and PLLs are not gated at runtime */
+static bool clock_is_always_on(unsigned long id)
+{
+	switch (id) {
+	case CK_HSE:
+	case CK_CSI:
+	case CK_LSI:
+	case CK_LSE:
+	case CK_HSI:
+	case CK_HSE_DIV2:
+	case PLL1_Q:
+	case PLL1_R:
+	case PLL2_P:
+	case PLL2_Q:
+	case PLL2_R:
+	case PLL3_P:
+	case PLL3_Q:
+	case PLL3_R:
+	case CK_AXI:
+	case CK_MPU:
+	case CK_MCU:
+	case RTC:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static void __stm32mp1_clk_enable(unsigned long id, bool with_refcnt)
+{
+	const struct stm32mp1_clk_gate *gate;
+	int i;
+
+	if (clock_is_always_on(id)) {
+		return;
+	}
+
+	i = stm32mp1_clk_get_gated_id(id);
+	if (i < 0) {
+		ERROR("Clock %lu can't be enabled\n", id);
+		panic();
+	}
+
+	gate = gate_ref(i);
+
+	if (!with_refcnt) {
+		__clk_enable(gate);
+		return;
+	}
+
+#if defined(IMAGE_BL32)
+	if (gate_is_non_secure(gate)) {
+		/* Enable non-secure clock w/o any refcounting */
+		__clk_enable(gate);
+		return;
+	}
+#endif
+
+	stm32mp1_clk_lock(&refcount_lock);
+
+	if (gate_refcounts[i] == 0U) {
+		__clk_enable(gate);
+	}
+
+	gate_refcounts[i]++;
+	if (gate_refcounts[i] == UINT_MAX) {
+		ERROR("Clock %lu refcount reached max value\n", id);
+		panic();
+	}
+
+	stm32mp1_clk_unlock(&refcount_lock);
+}
+
+static void __stm32mp1_clk_disable(unsigned long id, bool with_refcnt)
+{
+	const struct stm32mp1_clk_gate *gate;
+	int i;
+
+	if (clock_is_always_on(id)) {
+		return;
+	}
+
+	i = stm32mp1_clk_get_gated_id(id);
+	if (i < 0) {
+		ERROR("Clock %lu can't be disabled\n", id);
+		panic();
+	}
+
+	gate = gate_ref(i);
+
+	if (!with_refcnt) {
+		__clk_disable(gate);
+		return;
+	}
+
+#if defined(IMAGE_BL32)
+	if (gate_is_non_secure(gate)) {
+		/* Don't disable non-secure clocks */
+		return;
+	}
+#endif
+
+	stm32mp1_clk_lock(&refcount_lock);
+
+	if (gate_refcounts[i] == 0U) {
+		ERROR("Clock %lu refcount reached 0\n", id);
+		panic();
+	}
+	gate_refcounts[i]--;
+
+	if (gate_refcounts[i] == 0U) {
+		__clk_disable(gate);
+	}
+
+	stm32mp1_clk_unlock(&refcount_lock);
+}
+
+static int stm32mp_clk_enable(unsigned long id)
+{
+	__stm32mp1_clk_enable(id, true);
+
+	return 0;
+}
+
+static void stm32mp_clk_disable(unsigned long id)
+{
+	__stm32mp1_clk_disable(id, true);
+}
+
+static bool stm32mp_clk_is_enabled(unsigned long id)
+{
+	int i;
+
+	if (clock_is_always_on(id)) {
+		return true;
+	}
+
+	i = stm32mp1_clk_get_gated_id(id);
+	if (i < 0) {
+		panic();
+	}
+
+	return __clk_is_enabled(gate_ref(i));
+}
+
+static unsigned long stm32mp_clk_get_rate(unsigned long id)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	int p = stm32mp1_clk_get_parent(id);
+	uint32_t prescaler, timpre;
+	unsigned long parent_rate;
+
+	if (p < 0) {
+		return 0;
+	}
+
+	parent_rate = get_clock_rate(p);
+
+	switch (id) {
+	case TIM2_K:
+	case TIM3_K:
+	case TIM4_K:
+	case TIM5_K:
+	case TIM6_K:
+	case TIM7_K:
+	case TIM12_K:
+	case TIM13_K:
+	case TIM14_K:
+		prescaler = mmio_read_32(rcc_base + RCC_APB1DIVR) &
+			    RCC_APBXDIV_MASK;
+		timpre = mmio_read_32(rcc_base + RCC_TIMG1PRER) &
+			 RCC_TIMGXPRER_TIMGXPRE;
+		break;
+
+	case TIM1_K:
+	case TIM8_K:
+	case TIM15_K:
+	case TIM16_K:
+	case TIM17_K:
+		prescaler = mmio_read_32(rcc_base + RCC_APB2DIVR) &
+			    RCC_APBXDIV_MASK;
+		timpre = mmio_read_32(rcc_base + RCC_TIMG2PRER) &
+			 RCC_TIMGXPRER_TIMGXPRE;
+		break;
+
+	default:
+		return parent_rate;
+	}
+
+	if (prescaler == 0U) {
+		return parent_rate;
+	}
+
+	return parent_rate * (timpre + 1U) * 2U;
+}
+
+static void stm32mp1_ls_osc_set(bool enable, uint32_t offset, uint32_t mask_on)
+{
+	uintptr_t address = stm32mp_rcc_base() + offset;
+
+	if (enable) {
+		mmio_setbits_32(address, mask_on);
+	} else {
+		mmio_clrbits_32(address, mask_on);
+	}
+}
+
+static void stm32mp1_hs_ocs_set(bool enable, uint32_t mask_on)
+{
+	uint32_t offset = enable ? RCC_OCENSETR : RCC_OCENCLRR;
+	uintptr_t address = stm32mp_rcc_base() + offset;
+
+	mmio_write_32(address, mask_on);
+}
+
+static int stm32mp1_osc_wait(bool enable, uint32_t offset, uint32_t mask_rdy)
+{
+	uint64_t timeout;
+	uint32_t mask_test;
+	uintptr_t address = stm32mp_rcc_base() + offset;
+
+	if (enable) {
+		mask_test = mask_rdy;
+	} else {
+		mask_test = 0;
+	}
+
+	timeout = timeout_init_us(OSCRDY_TIMEOUT);
+	while ((mmio_read_32(address) & mask_rdy) != mask_test) {
+		if (timeout_elapsed(timeout)) {
+			ERROR("OSC %x @ %lx timeout for enable=%d : 0x%x\n",
+			      mask_rdy, address, enable, mmio_read_32(address));
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+static void stm32mp1_lse_enable(bool bypass, bool digbyp, uint32_t lsedrv)
+{
+	uint32_t value;
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	if (digbyp) {
+		mmio_setbits_32(rcc_base + RCC_BDCR, RCC_BDCR_DIGBYP);
+	}
+
+	if (bypass || digbyp) {
+		mmio_setbits_32(rcc_base + RCC_BDCR, RCC_BDCR_LSEBYP);
+	}
+
+	/*
+	 * Warning: not recommended to switch directly from "high drive"
+	 * to "medium low drive", and vice-versa.
+	 */
+	value = (mmio_read_32(rcc_base + RCC_BDCR) & RCC_BDCR_LSEDRV_MASK) >>
+		RCC_BDCR_LSEDRV_SHIFT;
+
+	while (value != lsedrv) {
+		if (value > lsedrv) {
+			value--;
+		} else {
+			value++;
+		}
+
+		mmio_clrsetbits_32(rcc_base + RCC_BDCR,
+				   RCC_BDCR_LSEDRV_MASK,
+				   value << RCC_BDCR_LSEDRV_SHIFT);
+	}
+
+	stm32mp1_ls_osc_set(true, RCC_BDCR, RCC_BDCR_LSEON);
+}
+
+static void stm32mp1_lse_wait(void)
+{
+	if (stm32mp1_osc_wait(true, RCC_BDCR, RCC_BDCR_LSERDY) != 0) {
+		VERBOSE("%s: failed\n", __func__);
+	}
+}
+
+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__);
+	}
+}
+
+static void stm32mp1_hse_enable(bool bypass, bool digbyp, bool css)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	if (digbyp) {
+		mmio_write_32(rcc_base + RCC_OCENSETR, RCC_OCENR_DIGBYP);
+	}
+
+	if (bypass || digbyp) {
+		mmio_write_32(rcc_base + RCC_OCENSETR, RCC_OCENR_HSEBYP);
+	}
+
+	stm32mp1_hs_ocs_set(true, RCC_OCENR_HSEON);
+	if (stm32mp1_osc_wait(true, RCC_OCRDYR, RCC_OCRDYR_HSERDY) != 0) {
+		VERBOSE("%s: failed\n", __func__);
+	}
+
+	if (css) {
+		mmio_write_32(rcc_base + RCC_OCENSETR, RCC_OCENR_HSECSSON);
+	}
+
+#if STM32MP_UART_PROGRAMMER || STM32MP_USB_PROGRAMMER
+	if ((mmio_read_32(rcc_base + RCC_OCENSETR) & RCC_OCENR_HSEBYP) &&
+	    (!(digbyp || bypass))) {
+		panic();
+	}
+#endif
+}
+
+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__);
+	}
+}
+
+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__);
+	}
+}
+
+static int stm32mp1_set_hsidiv(uint8_t hsidiv)
+{
+	uint64_t timeout;
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	uintptr_t address = rcc_base + RCC_OCRDYR;
+
+	mmio_clrsetbits_32(rcc_base + RCC_HSICFGR,
+			   RCC_HSICFGR_HSIDIV_MASK,
+			   RCC_HSICFGR_HSIDIV_MASK & (uint32_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));
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+static int stm32mp1_hsidiv(unsigned long hsifreq)
+{
+	uint8_t hsidiv;
+	uint32_t hsidivfreq = MAX_HSI_HZ;
+
+	for (hsidiv = 0; hsidiv < 4U; hsidiv++) {
+		if (hsidivfreq == hsifreq) {
+			break;
+		}
+
+		hsidivfreq /= 2U;
+	}
+
+	if (hsidiv == 4U) {
+		ERROR("Invalid clk-hsi frequency\n");
+		return -1;
+	}
+
+	if (hsidiv != 0U) {
+		return stm32mp1_set_hsidiv(hsidiv);
+	}
+
+	return 0;
+}
+
+static bool stm32mp1_check_pll_conf(enum stm32mp1_pll_id pll_id,
+				    unsigned int clksrc,
+				    uint32_t *pllcfg, int plloff)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	uintptr_t pllxcr = rcc_base + pll->pllxcr;
+	enum stm32mp1_plltype type = pll->plltype;
+	uintptr_t clksrc_address = rcc_base + (clksrc >> 4);
+	unsigned long refclk;
+	uint32_t ifrge = 0U;
+	uint32_t src, value, fracv = 0;
+	void *fdt;
+
+	/* Check PLL output */
+	if (mmio_read_32(pllxcr) != RCC_PLLNCR_PLLON) {
+		return false;
+	}
+
+	/* Check current clksrc */
+	src = mmio_read_32(clksrc_address) & RCC_SELR_SRC_MASK;
+	if (src != (clksrc & RCC_SELR_SRC_MASK)) {
+		return false;
+	}
+
+	/* Check Div */
+	src = mmio_read_32(rcc_base + pll->rckxselr) & RCC_SELR_REFCLK_SRC_MASK;
+
+	refclk = stm32mp1_clk_get_fixed(pll->refclk[src]) /
+		 (pllcfg[PLLCFG_M] + 1U);
+
+	if ((refclk < (stm32mp1_pll[type].refclk_min * 1000000U)) ||
+	    (refclk > (stm32mp1_pll[type].refclk_max * 1000000U))) {
+		return false;
+	}
+
+	if ((type == PLL_800) && (refclk >= 8000000U)) {
+		ifrge = 1U;
+	}
+
+	value = (pllcfg[PLLCFG_N] << RCC_PLLNCFGR1_DIVN_SHIFT) &
+		RCC_PLLNCFGR1_DIVN_MASK;
+	value |= (pllcfg[PLLCFG_M] << RCC_PLLNCFGR1_DIVM_SHIFT) &
+		 RCC_PLLNCFGR1_DIVM_MASK;
+	value |= (ifrge << RCC_PLLNCFGR1_IFRGE_SHIFT) &
+		 RCC_PLLNCFGR1_IFRGE_MASK;
+	if (mmio_read_32(rcc_base + pll->pllxcfgr1) != value) {
+		return false;
+	}
+
+	/* 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) {
+		return false;
+	}
+
+	/* Output config */
+	value = (pllcfg[PLLCFG_P] << RCC_PLLNCFGR2_DIVP_SHIFT) &
+		RCC_PLLNCFGR2_DIVP_MASK;
+	value |= (pllcfg[PLLCFG_Q] << RCC_PLLNCFGR2_DIVQ_SHIFT) &
+		 RCC_PLLNCFGR2_DIVQ_MASK;
+	value |= (pllcfg[PLLCFG_R] << RCC_PLLNCFGR2_DIVR_SHIFT) &
+		 RCC_PLLNCFGR2_DIVR_MASK;
+	if (mmio_read_32(rcc_base + pll->pllxcfgr2) != value) {
+		return false;
+	}
+
+	return true;
+}
+
+static void stm32mp1_pll_start(enum stm32mp1_pll_id pll_id)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	uintptr_t pllxcr = stm32mp_rcc_base() + pll->pllxcr;
+
+	/* Preserve RCC_PLLNCR_SSCG_CTRL value */
+	mmio_clrsetbits_32(pllxcr,
+			   RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN |
+			   RCC_PLLNCR_DIVREN,
+			   RCC_PLLNCR_PLLON);
+}
+
+static int stm32mp1_pll_output(enum stm32mp1_pll_id pll_id, uint32_t output)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	uintptr_t pllxcr = stm32mp_rcc_base() + pll->pllxcr;
+	uint64_t timeout = timeout_init_us(PLLRDY_TIMEOUT);
+
+	/* 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));
+			return -ETIMEDOUT;
+		}
+	}
+
+	/* Start the requested output */
+	mmio_setbits_32(pllxcr, output << RCC_PLLNCR_DIVEN_SHIFT);
+
+	return 0;
+}
+
+static int stm32mp1_pll_stop(enum stm32mp1_pll_id pll_id)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	uintptr_t pllxcr = stm32mp_rcc_base() + pll->pllxcr;
+	uint64_t timeout;
+
+	/* Stop all output */
+	mmio_clrbits_32(pllxcr, RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN |
+			RCC_PLLNCR_DIVREN);
+
+	/* Stop PLL */
+	mmio_clrbits_32(pllxcr, RCC_PLLNCR_PLLON);
+
+	timeout = timeout_init_us(PLLRDY_TIMEOUT);
+	/* 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));
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+static void stm32mp1_pll_config_output(enum stm32mp1_pll_id pll_id,
+				       uint32_t *pllcfg)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	uint32_t value;
+
+	value = (pllcfg[PLLCFG_P] << RCC_PLLNCFGR2_DIVP_SHIFT) &
+		RCC_PLLNCFGR2_DIVP_MASK;
+	value |= (pllcfg[PLLCFG_Q] << RCC_PLLNCFGR2_DIVQ_SHIFT) &
+		 RCC_PLLNCFGR2_DIVQ_MASK;
+	value |= (pllcfg[PLLCFG_R] << RCC_PLLNCFGR2_DIVR_SHIFT) &
+		 RCC_PLLNCFGR2_DIVR_MASK;
+	mmio_write_32(rcc_base + pll->pllxcfgr2, value);
+}
+
+static int stm32mp1_pll_config(enum stm32mp1_pll_id pll_id,
+			       uint32_t *pllcfg, uint32_t fracv)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	enum stm32mp1_plltype type = pll->plltype;
+	unsigned long refclk;
+	uint32_t ifrge = 0;
+	uint32_t src, value;
+
+	src = mmio_read_32(rcc_base + pll->rckxselr) &
+		RCC_SELR_REFCLK_SRC_MASK;
+
+	refclk = stm32mp1_clk_get_fixed(pll->refclk[src]) /
+		 (pllcfg[PLLCFG_M] + 1U);
+
+	if ((refclk < (stm32mp1_pll[type].refclk_min * 1000000U)) ||
+	    (refclk > (stm32mp1_pll[type].refclk_max * 1000000U))) {
+		return -EINVAL;
+	}
+
+	if ((type == PLL_800) && (refclk >= 8000000U)) {
+		ifrge = 1U;
+	}
+
+	value = (pllcfg[PLLCFG_N] << RCC_PLLNCFGR1_DIVN_SHIFT) &
+		RCC_PLLNCFGR1_DIVN_MASK;
+	value |= (pllcfg[PLLCFG_M] << RCC_PLLNCFGR1_DIVM_SHIFT) &
+		 RCC_PLLNCFGR1_DIVM_MASK;
+	value |= (ifrge << RCC_PLLNCFGR1_IFRGE_SHIFT) &
+		 RCC_PLLNCFGR1_IFRGE_MASK;
+	mmio_write_32(rcc_base + pll->pllxcfgr1, value);
+
+	/* Fractional configuration */
+	value = 0;
+	mmio_write_32(rcc_base + pll->pllxfracr, value);
+
+	value = fracv << RCC_PLLNFRACR_FRACV_SHIFT;
+	mmio_write_32(rcc_base + pll->pllxfracr, value);
+
+	value |= RCC_PLLNFRACR_FRACLE;
+	mmio_write_32(rcc_base + pll->pllxfracr, value);
+
+	stm32mp1_pll_config_output(pll_id, pllcfg);
+
+	return 0;
+}
+
+static void stm32mp1_pll_csg(enum stm32mp1_pll_id pll_id, uint32_t *csg)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	uint32_t pllxcsg = 0;
+
+	pllxcsg |= (csg[PLLCSG_MOD_PER] << RCC_PLLNCSGR_MOD_PER_SHIFT) &
+		    RCC_PLLNCSGR_MOD_PER_MASK;
+
+	pllxcsg |= (csg[PLLCSG_INC_STEP] << RCC_PLLNCSGR_INC_STEP_SHIFT) &
+		    RCC_PLLNCSGR_INC_STEP_MASK;
+
+	pllxcsg |= (csg[PLLCSG_SSCG_MODE] << RCC_PLLNCSGR_SSCG_MODE_SHIFT) &
+		    RCC_PLLNCSGR_SSCG_MODE_MASK;
+
+	mmio_write_32(stm32mp_rcc_base() + pll->pllxcsgr, pllxcsg);
+
+	mmio_setbits_32(stm32mp_rcc_base() + pll->pllxcr,
+			RCC_PLLNCR_SSCG_CTRL);
+}
+
+static int stm32mp1_set_clksrc(unsigned int clksrc)
+{
+	uintptr_t clksrc_address = stm32mp_rcc_base() + (clksrc >> 4);
+	uint64_t timeout;
+
+	mmio_clrsetbits_32(clksrc_address, RCC_SELR_SRC_MASK,
+			   clksrc & RCC_SELR_SRC_MASK);
+
+	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;
+		}
+	}
+
+	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]) {
+			continue;
+		}
+
+		ret = clk_get_pll_settings_from_dt(plloff[i], pllcfg[i],
+						   &pllfracv[i], pllcsg[i],
+						   &pllcsg_set[i]);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	stm32mp1_mco_csg(clksrc[CLKSRC_MCO1], clkdiv[CLKDIV_MCO1]);
+	stm32mp1_mco_csg(clksrc[CLKSRC_MCO2], clkdiv[CLKDIV_MCO2]);
+
+	/*
+	 * Switch ON oscillator found in device-tree.
+	 * Note: HSI already ON after BootROM stage.
+	 */
+	if (stm32mp1_osc[_LSI] != 0U) {
+		stm32mp1_lsi_set(true);
+	}
+	if (stm32mp1_osc[_LSE] != 0U) {
+		const char *name = stm32mp_osc_node_label[_LSE];
+		bool bypass, digbyp;
+		uint32_t lsedrv;
+
+		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");
+		lsedrv = fdt_clk_read_uint32_default(name, "st,drive",
+						     LSEDRV_MEDIUM_HIGH);
+		stm32mp1_lse_enable(bypass, digbyp, lsedrv);
+	}
+	if (stm32mp1_osc[_HSE] != 0U) {
+		const char *name = stm32mp_osc_node_label[_HSE];
+		bool bypass, digbyp, css;
+
+		bypass = fdt_clk_read_bool(name, "st,bypass");
+		digbyp = fdt_clk_read_bool(name, "st,digbypass");
+		css = fdt_clk_read_bool(name, "st,css");
+		stm32mp1_hse_enable(bypass, digbyp, css);
+	}
+	/*
+	 * CSI is mandatory for automatic I/O compensation (SYSCFG_CMPCR)
+	 * => switch on CSI even if node is not present in device tree
+	 */
+	stm32mp1_csi_set(true);
+
+	/* Come back to HSI */
+	ret = stm32mp1_set_clksrc(CLK_MPU_HSI);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = stm32mp1_set_clksrc(CLK_AXI_HSI);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = stm32mp1_set_clksrc(CLK_MCU_HSI);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if ((mmio_read_32(rcc_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]);
+		}
+	}
+	/* Don't initialize PLL4, when used by BOOTROM */
+	if ((stm32mp_get_boot_itf_selected() ==
+	     BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB) &&
+	    ((stgen_p == (int)_PLL4_R) || (usbphy_p == (int)_PLL4_R))) {
+		pll4_bootrom = true;
+		pll4_preserve = true;
+	}
+
+	for (i = (enum stm32mp1_pll_id)0; i < _PLL_NB; i++) {
+		if (((i == _PLL3) && pll3_preserve) ||
+		    ((i == _PLL4) && pll4_preserve)) {
+			continue;
+		}
+
+		ret = stm32mp1_pll_stop(i);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	/* Configure HSIDIV */
+	if (stm32mp1_osc[_HSI] != 0U) {
+		ret = stm32mp1_hsidiv(stm32mp1_osc[_HSI]);
+		if (ret != 0) {
+			return ret;
+		}
+
+		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);
+	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]);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (!pll3_preserve) {
+		ret = stm32mp1_set_clksrc(clksrc[CLKSRC_PLL3]);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	if (!pll4_preserve) {
+		ret = stm32mp1_set_clksrc(clksrc[CLKSRC_PLL4]);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	/* Configure and start PLLs */
+	for (i = (enum stm32mp1_pll_id)0; i < _PLL_NB; i++) {
+		if (((i == _PLL3) && pll3_preserve) ||
+		    ((i == _PLL4) && pll4_preserve && !pll4_bootrom)) {
+			continue;
+		}
+
+		if (!pllcfg_valid[i]) {
+			continue;
+		}
+
+		if ((i == _PLL4) && pll4_bootrom) {
+			/* Set output divider if not done by the Bootrom */
+			stm32mp1_pll_config_output(i, pllcfg[i]);
+			continue;
+		}
+
+		ret = stm32mp1_pll_config(i, pllcfg[i], pllfracv[i]);
+		if (ret != 0) {
+			return ret;
+		}
+
+		if (pllcsg_set[i]) {
+			stm32mp1_pll_csg(i, pllcsg[i]);
+		}
+
+		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]) {
+			continue;
+		}
+
+		ret = stm32mp1_pll_output(i, pllcfg[i][PLLCFG_O]);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+	/* Wait LSE ready before to use it */
+	if (stm32mp1_osc[_LSE] != 0U) {
+		stm32mp1_lse_wait();
+	}
+
+	/* Configure with expected clock source */
+	ret = stm32mp1_set_clksrc(clksrc[CLKSRC_MPU]);
+	if (ret != 0) {
+		return ret;
+	}
+	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) {
+			usbreg_bootrom = mmio_read_32(rcc_base + RCC_USBCKSELR);
+		}
+
+		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;
+			}
+		}
+	}
+
+	/* Switch OFF HSI if not found in device-tree */
+	if (stm32mp1_osc[_HSI] == 0U) {
+		stm32mp1_hsi_set(false);
+	}
+
+	stm32mp_stgen_config(stm32mp_clk_get_rate(STGEN_K));
+
+	/* Software Self-Refresh mode (SSR) during DDR initilialization */
+	mmio_clrsetbits_32(rcc_base + RCC_DDRITFCR,
+			   RCC_DDRITFCR_DDRCKMOD_MASK,
+			   RCC_DDRITFCR_DDRCKMOD_SSR <<
+			   RCC_DDRITFCR_DDRCKMOD_SHIFT);
+
+	return 0;
+}
+
+static void stm32mp1_osc_clk_init(const char *name,
+				  enum stm32mp_osc_id index)
+{
+	uint32_t frequency;
+
+	if (fdt_osc_read_freq(name, &frequency) == 0) {
+		stm32mp1_osc[index] = frequency;
+	}
+}
+
+static void stm32mp1_osc_init(void)
+{
+	enum stm32mp_osc_id i;
+
+	for (i = (enum stm32mp_osc_id)0 ; i < NB_OSC; i++) {
+		stm32mp1_osc_clk_init(stm32mp_osc_node_label[i], i);
+	}
+}
+
+#ifdef STM32MP_SHARED_RESOURCES
+/*
+ * Get the parent ID of the target parent clock, for tagging as secure
+ * shared clock dependencies.
+ */
+static int get_parent_id_parent(unsigned int parent_id)
+{
+	enum stm32mp1_parent_sel s = _UNKNOWN_SEL;
+	enum stm32mp1_pll_id pll_id;
+	uint32_t p_sel;
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	switch (parent_id) {
+	case _ACLK:
+	case _PCLK4:
+	case _PCLK5:
+		s = _AXIS_SEL;
+		break;
+	case _PLL1_P:
+	case _PLL1_Q:
+	case _PLL1_R:
+		pll_id = _PLL1;
+		break;
+	case _PLL2_P:
+	case _PLL2_Q:
+	case _PLL2_R:
+		pll_id = _PLL2;
+		break;
+	case _PLL3_P:
+	case _PLL3_Q:
+	case _PLL3_R:
+		pll_id = _PLL3;
+		break;
+	case _PLL4_P:
+	case _PLL4_Q:
+	case _PLL4_R:
+		pll_id = _PLL4;
+		break;
+	case _PCLK1:
+	case _PCLK2:
+	case _HCLK2:
+	case _HCLK6:
+	case _CK_PER:
+	case _CK_MPU:
+	case _CK_MCU:
+	case _USB_PHY_48:
+		/* We do not expect to access these */
+		panic();
+		break;
+	default:
+		/* Other parents have no parent */
+		return -1;
+	}
+
+	if (s != _UNKNOWN_SEL) {
+		const struct stm32mp1_clk_sel *sel = clk_sel_ref(s);
+
+		p_sel = (mmio_read_32(rcc_base + sel->offset) >> sel->src) &
+			sel->msk;
+
+		if (p_sel < sel->nb_parent) {
+			return (int)sel->parent[p_sel];
+		}
+	} else {
+		const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+
+		p_sel = mmio_read_32(rcc_base + pll->rckxselr) &
+			RCC_SELR_REFCLK_SRC_MASK;
+
+		if (pll->refclk[p_sel] != _UNKNOWN_OSC_ID) {
+			return (int)pll->refclk[p_sel];
+		}
+	}
+
+	VERBOSE("No parent selected for %s\n",
+		stm32mp1_clk_parent_name[parent_id]);
+
+	return -1;
+}
+
+static void secure_parent_clocks(unsigned long parent_id)
+{
+	int grandparent_id;
+
+	switch (parent_id) {
+	case _PLL3_P:
+	case _PLL3_Q:
+	case _PLL3_R:
+		stm32mp_register_secure_periph(STM32MP1_SHRES_PLL3);
+		break;
+
+	/* These clocks are always secure when RCC is secure */
+	case _ACLK:
+	case _HCLK2:
+	case _HCLK6:
+	case _PCLK4:
+	case _PCLK5:
+	case _PLL1_P:
+	case _PLL1_Q:
+	case _PLL1_R:
+	case _PLL2_P:
+	case _PLL2_Q:
+	case _PLL2_R:
+	case _HSI:
+	case _HSI_KER:
+	case _LSI:
+	case _CSI:
+	case _CSI_KER:
+	case _HSE:
+	case _HSE_KER:
+	case _HSE_KER_DIV2:
+	case _HSE_RTC:
+	case _LSE:
+		break;
+
+	default:
+		VERBOSE("Cannot secure parent clock %s\n",
+			stm32mp1_clk_parent_name[parent_id]);
+		panic();
+	}
+
+	grandparent_id = get_parent_id_parent(parent_id);
+	if (grandparent_id >= 0) {
+		secure_parent_clocks(grandparent_id);
+	}
+}
+
+void stm32mp1_register_clock_parents_secure(unsigned long clock_id)
+{
+	int parent_id;
+
+	if (!stm32mp1_rcc_is_secure()) {
+		return;
+	}
+
+	switch (clock_id) {
+	case PLL1:
+	case PLL2:
+		/* PLL1/PLL2 are always secure: nothing to do */
+		break;
+	case PLL3:
+		stm32mp_register_secure_periph(STM32MP1_SHRES_PLL3);
+		break;
+	case PLL4:
+		ERROR("PLL4 cannot be secured\n");
+		panic();
+		break;
+	default:
+		/* Others are expected gateable clock */
+		parent_id = stm32mp1_clk_get_parent(clock_id);
+		if (parent_id < 0) {
+			INFO("No parent found for clock %lu\n", clock_id);
+		} else {
+			secure_parent_clocks(parent_id);
+		}
+		break;
+	}
+}
+#endif /* STM32MP_SHARED_RESOURCES */
+
+static void sync_earlyboot_clocks_state(void)
+{
+	unsigned int idx;
+	const unsigned long secure_enable[] = {
+		AXIDCG,
+		BSEC,
+		DDRC1, DDRC1LP,
+		DDRC2, DDRC2LP,
+		DDRCAPB, DDRPHYCAPB, DDRPHYCAPBLP,
+		DDRPHYC, DDRPHYCLP,
+		RTCAPB,
+		TZC1, TZC2,
+		TZPC,
+		STGEN_K,
+	};
+
+	for (idx = 0U; idx < ARRAY_SIZE(secure_enable); idx++) {
+		stm32mp_clk_enable(secure_enable[idx]);
+	}
+}
+
+static const struct clk_ops stm32mp_clk_ops = {
+	.enable		= stm32mp_clk_enable,
+	.disable	= stm32mp_clk_disable,
+	.is_enabled	= stm32mp_clk_is_enabled,
+	.get_rate	= stm32mp_clk_get_rate,
+	.get_parent	= stm32mp1_clk_get_parent,
+};
+
+int stm32mp1_clk_probe(void)
+{
+#if defined(IMAGE_BL32)
+	if (!fdt_get_rcc_secure_state()) {
+		mmio_write_32(stm32mp_rcc_base() + RCC_TZCR, 0U);
+	}
+#endif
+
+	stm32mp1_osc_init();
+
+	sync_earlyboot_clocks_state();
+
+	clk_register(&stm32mp_clk_ops);
+
+	return 0;
+}
diff --git a/drivers/st/clk/stm32mp_clkfunc.c b/drivers/st/clk/stm32mp_clkfunc.c
new file mode 100644
index 0000000..379547f
--- /dev/null
+++ b/drivers/st/clk/stm32mp_clkfunc.c
@@ -0,0 +1,394 @@
+/*
+ * Copyright (c) 2017-2023, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+
+#include <arch_helpers.h>
+#include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
+#include <drivers/generic_delay_timer.h>
+#include <drivers/st/stm32_gpio.h>
+#include <drivers/st/stm32mp_clkfunc.h>
+#include <lib/mmio.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+/*
+ * Get the frequency of an oscillator from its name in device tree.
+ * @param name: oscillator name
+ * @param freq: stores the frequency of the oscillator
+ * @return: 0 on success, and a negative FDT/ERRNO error code on failure.
+ */
+int fdt_osc_read_freq(const char *name, uint32_t *freq)
+{
+	int node, subnode;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	node = fdt_path_offset(fdt, "/clocks");
+	if (node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	fdt_for_each_subnode(subnode, fdt, node) {
+		const char *cchar;
+		int ret;
+
+		cchar = fdt_get_name(fdt, subnode, &ret);
+		if (cchar == NULL) {
+			return ret;
+		}
+
+		if ((strncmp(cchar, name, (size_t)ret) == 0) &&
+		    (fdt_get_status(subnode) != DT_DISABLED)) {
+			const fdt32_t *cuint;
+
+			cuint = fdt_getprop(fdt, subnode, "clock-frequency",
+					    &ret);
+			if (cuint == NULL) {
+				return ret;
+			}
+
+			*freq = fdt32_to_cpu(*cuint);
+
+			return 0;
+		}
+	}
+
+	/* Oscillator not found, freq=0 */
+	*freq = 0;
+	return 0;
+}
+
+/*
+ * Check the presence of an oscillator property from its id.
+ * @param node_label: clock node name
+ * @param prop_name: property name
+ * @return: true/false regarding search result.
+ */
+bool fdt_clk_read_bool(const char *node_label, const char *prop_name)
+{
+	int node, subnode;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return false;
+	}
+
+	node = fdt_path_offset(fdt, "/clocks");
+	if (node < 0) {
+		return false;
+	}
+
+	fdt_for_each_subnode(subnode, fdt, node) {
+		const char *cchar;
+		int ret;
+
+		cchar = fdt_get_name(fdt, subnode, &ret);
+		if (cchar == NULL) {
+			return false;
+		}
+
+		if (strncmp(cchar, node_label, (size_t)ret) != 0) {
+			continue;
+		}
+
+		if (fdt_getprop(fdt, subnode, prop_name, NULL) != NULL) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+/*
+ * Get the value of a oscillator property from its name.
+ * @param node_label: oscillator name
+ * @param prop_name: property name
+ * @param dflt_value: default value
+ * @return oscillator value on success, default value if property not found.
+ */
+uint32_t fdt_clk_read_uint32_default(const char *node_label,
+				     const char *prop_name, uint32_t dflt_value)
+{
+	int node, subnode;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return dflt_value;
+	}
+
+	node = fdt_path_offset(fdt, "/clocks");
+	if (node < 0) {
+		return dflt_value;
+	}
+
+	fdt_for_each_subnode(subnode, fdt, node) {
+		const char *cchar;
+		int ret;
+
+		cchar = fdt_get_name(fdt, subnode, &ret);
+		if (cchar == NULL) {
+			return dflt_value;
+		}
+
+		if (strncmp(cchar, node_label, (size_t)ret) != 0) {
+			continue;
+		}
+
+		return fdt_read_uint32_default(fdt, subnode, prop_name,
+					       dflt_value);
+	}
+
+	return dflt_value;
+}
+
+/*
+ * Get the RCC node offset from the device tree
+ * @param fdt: Device tree reference
+ * @return: Node offset or a negative value on error
+ */
+static int fdt_get_rcc_node(void *fdt)
+{
+	static int node;
+
+	if (node <= 0) {
+		node = fdt_node_offset_by_compatible(fdt, -1, DT_RCC_CLK_COMPAT);
+	}
+
+	return node;
+}
+
+/*
+ * Read a series of parameters in rcc-clk section in device tree
+ * @param prop_name: Name of the RCC property to be read
+ * @param array: the array to store the property parameters
+ * @param count: number of parameters to be read
+ * @return: 0 on succes or a negative value on error
+ */
+int fdt_rcc_read_uint32_array(const char *prop_name, uint32_t count,
+			      uint32_t *array)
+{
+	int node;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	node = fdt_get_rcc_node(fdt);
+	if (node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	return fdt_read_uint32_array(fdt, node, prop_name, count, array);
+}
+
+/*
+ * Get the subnode offset in rcc-clk section from its name in device tree
+ * @param name: name of the RCC property
+ * @return: offset on success, and a negative FDT/ERRNO error code on failure.
+ */
+int fdt_rcc_subnode_offset(const char *name)
+{
+	int node, subnode;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	node = fdt_get_rcc_node(fdt);
+	if (node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	subnode = fdt_subnode_offset(fdt, node, name);
+	if (subnode <= 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	return subnode;
+}
+
+/*
+ * Get the pointer to a rcc-clk property from its name.
+ * @param name: name of the RCC property
+ * @param lenp: stores the length of the property.
+ * @return: pointer to the property on success, and NULL value on failure.
+ */
+const fdt32_t *fdt_rcc_read_prop(const char *prop_name, int *lenp)
+{
+	const fdt32_t *cuint;
+	int node, len;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return NULL;
+	}
+
+	node = fdt_get_rcc_node(fdt);
+	if (node < 0) {
+		return NULL;
+	}
+
+	cuint = fdt_getprop(fdt, node, prop_name, &len);
+	if (cuint == NULL) {
+		return NULL;
+	}
+
+	*lenp = len;
+	return cuint;
+}
+
+#if defined(IMAGE_BL32)
+/*
+ * Get the secure state for rcc node in device tree.
+ * @return: true if rcc is configured for secure world access, false if not.
+ */
+bool fdt_get_rcc_secure_state(void)
+{
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return false;
+	}
+
+	if (fdt_node_offset_by_compatible(fdt, -1, DT_RCC_SEC_CLK_COMPAT) < 0) {
+		return false;
+	}
+
+	return true;
+}
+#endif
+
+/*
+ * Get the clock ID of the given node in device tree.
+ * @param node: node offset
+ * @return: Clock ID on success, and a negative FDT/ERRNO error code on failure.
+ */
+int fdt_get_clock_id(int node)
+{
+	const fdt32_t *cuint;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	cuint = fdt_getprop(fdt, node, "clocks", NULL);
+	if (cuint == NULL) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	cuint++;
+	return (int)fdt32_to_cpu(*cuint);
+}
+
+/*
+ * Get the frequency of the specified UART instance.
+ * @param instance: UART interface registers base address.
+ * @return: clock frequency on success, 0 value on failure.
+ */
+unsigned long fdt_get_uart_clock_freq(uintptr_t instance)
+{
+	void *fdt;
+	int node;
+	int clk_id;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return 0UL;
+	}
+
+	/* Check for UART nodes */
+	node = dt_match_instance_by_compatible(DT_UART_COMPAT, instance);
+	if (node < 0) {
+		return 0UL;
+	}
+
+	clk_id = fdt_get_clock_id(node);
+	if (clk_id < 0) {
+		return 0UL;
+	}
+
+	return clk_get_rate((unsigned long)clk_id);
+}
+
+/*******************************************************************************
+ * This function sets the STGEN counter value.
+ ******************************************************************************/
+static void stgen_set_counter(unsigned long long counter)
+{
+#ifdef __aarch64__
+	mmio_write_64(STGEN_BASE + CNTCV_OFF, counter);
+#else
+	mmio_write_32(STGEN_BASE + CNTCVL_OFF, (uint32_t)counter);
+	mmio_write_32(STGEN_BASE + CNTCVU_OFF, (uint32_t)(counter >> 32));
+#endif
+}
+
+/*******************************************************************************
+ * This function configures and restores the STGEN counter depending on the
+ * connected clock.
+ ******************************************************************************/
+void stm32mp_stgen_config(unsigned long rate)
+{
+	uint32_t cntfid0;
+	unsigned long long counter;
+
+	cntfid0 = mmio_read_32(STGEN_BASE + CNTFID_OFF);
+
+	if (cntfid0 == rate) {
+		return;
+	}
+
+	mmio_clrbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN);
+	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);
+
+	write_cntfrq_el0(rate);
+
+	/* Need to update timer with new frequency */
+	generic_delay_timer_init();
+}
+
+/*******************************************************************************
+ * This function returns the STGEN counter value.
+ ******************************************************************************/
+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 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);
+}