summaryrefslogtreecommitdiffstats
path: root/drivers/st
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--drivers/st/bsec/bsec2.c961
-rw-r--r--drivers/st/clk/clk-stm32-core.c1096
-rw-r--r--drivers/st/clk/clk-stm32-core.h393
-rw-r--r--drivers/st/clk/clk-stm32mp13.c2332
-rw-r--r--drivers/st/clk/stm32mp1_clk.c2373
-rw-r--r--drivers/st/clk/stm32mp_clkfunc.c377
-rw-r--r--drivers/st/crypto/stm32_hash.c364
-rw-r--r--drivers/st/crypto/stm32_pka.c707
-rw-r--r--drivers/st/crypto/stm32_rng.c269
-rw-r--r--drivers/st/crypto/stm32_saes.c913
-rw-r--r--drivers/st/ddr/stm32mp1_ddr.c764
-rw-r--r--drivers/st/ddr/stm32mp1_ddr_helpers.c26
-rw-r--r--drivers/st/ddr/stm32mp1_ram.c151
-rw-r--r--drivers/st/ddr/stm32mp_ddr.c106
-rw-r--r--drivers/st/ddr/stm32mp_ddr_test.c148
-rw-r--r--drivers/st/ddr/stm32mp_ram.c60
-rw-r--r--drivers/st/etzpc/etzpc.c246
-rw-r--r--drivers/st/fmc/stm32_fmc2_nand.c934
-rw-r--r--drivers/st/gpio/stm32_gpio.c323
-rw-r--r--drivers/st/i2c/stm32_i2c.c982
-rw-r--r--drivers/st/io/io_mmc.c143
-rw-r--r--drivers/st/iwdg/stm32_iwdg.c157
-rw-r--r--drivers/st/mmc/stm32_sdmmc2.c798
-rw-r--r--drivers/st/pmic/stm32mp_pmic.c526
-rw-r--r--drivers/st/pmic/stpmic1.c937
-rw-r--r--drivers/st/regulator/regulator_core.c560
-rw-r--r--drivers/st/regulator/regulator_fixed.c87
-rw-r--r--drivers/st/reset/stm32mp1_reset.c69
-rw-r--r--drivers/st/spi/stm32_qspi.c508
-rw-r--r--drivers/st/uart/aarch32/stm32_console.S261
-rw-r--r--drivers/st/uart/stm32_uart.c439
-rw-r--r--drivers/st/usb/stm32mp1_usb.c1091
32 files changed, 19101 insertions, 0 deletions
diff --git a/drivers/st/bsec/bsec2.c b/drivers/st/bsec/bsec2.c
new file mode 100644
index 0000000..68d3a5b
--- /dev/null
+++ b/drivers/st/bsec/bsec2.c
@@ -0,0 +1,961 @@
+/*
+ * Copyright (c) 2017-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <limits.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/st/bsec.h>
+#include <drivers/st/bsec2_reg.h>
+#include <lib/mmio.h>
+#include <lib/spinlock.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+#define BSEC_IP_VERSION_1_1 U(0x11)
+#define BSEC_IP_VERSION_2_0 U(0x20)
+#define BSEC_IP_ID_2 U(0x100032)
+
+#define OTP_ACCESS_SIZE (round_up(OTP_MAX_SIZE, __WORD_BIT) / __WORD_BIT)
+
+static uint32_t otp_nsec_access[OTP_ACCESS_SIZE] __unused;
+
+static uint32_t bsec_power_safmem(bool power);
+
+/* BSEC access protection */
+static spinlock_t bsec_spinlock;
+static uintptr_t bsec_base;
+
+static void bsec_lock(void)
+{
+ if (stm32mp_lock_available()) {
+ spin_lock(&bsec_spinlock);
+ }
+}
+
+static void bsec_unlock(void)
+{
+ if (stm32mp_lock_available()) {
+ spin_unlock(&bsec_spinlock);
+ }
+}
+
+static bool is_otp_invalid_mode(void)
+{
+ bool ret = ((bsec_get_status() & BSEC_MODE_INVALID) == BSEC_MODE_INVALID);
+
+ if (ret) {
+ ERROR("OTP mode is OTP-INVALID\n");
+ }
+
+ return ret;
+}
+
+#if defined(IMAGE_BL32)
+static int bsec_get_dt_node(struct dt_node_info *info)
+{
+ int node;
+
+ node = dt_get_node(info, -1, DT_BSEC_COMPAT);
+ if (node < 0) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ return node;
+}
+
+static void enable_non_secure_access(uint32_t otp)
+{
+ otp_nsec_access[otp / __WORD_BIT] |= BIT(otp % __WORD_BIT);
+
+ if (bsec_shadow_register(otp) != BSEC_OK) {
+ panic();
+ }
+}
+
+static bool non_secure_can_access(uint32_t otp)
+{
+ return (otp_nsec_access[otp / __WORD_BIT] &
+ BIT(otp % __WORD_BIT)) != 0U;
+}
+
+static void bsec_dt_otp_nsec_access(void *fdt, int bsec_node)
+{
+ int bsec_subnode;
+
+ fdt_for_each_subnode(bsec_subnode, fdt, bsec_node) {
+ const fdt32_t *cuint;
+ uint32_t otp;
+ uint32_t i;
+ uint32_t size;
+ uint32_t offset;
+ uint32_t length;
+
+ cuint = fdt_getprop(fdt, bsec_subnode, "reg", NULL);
+ if (cuint == NULL) {
+ panic();
+ }
+
+ offset = fdt32_to_cpu(*cuint);
+ cuint++;
+ length = fdt32_to_cpu(*cuint);
+
+ otp = offset / sizeof(uint32_t);
+
+ if (otp < STM32MP1_UPPER_OTP_START) {
+ unsigned int otp_end = round_up(offset + length,
+ sizeof(uint32_t)) /
+ sizeof(uint32_t);
+
+ if (otp_end > STM32MP1_UPPER_OTP_START) {
+ /*
+ * OTP crosses Lower/Upper boundary, consider
+ * only the upper part.
+ */
+ otp = STM32MP1_UPPER_OTP_START;
+ length -= (STM32MP1_UPPER_OTP_START *
+ sizeof(uint32_t)) - offset;
+ offset = STM32MP1_UPPER_OTP_START *
+ sizeof(uint32_t);
+
+ WARN("OTP crosses Lower/Upper boundary\n");
+ } else {
+ continue;
+ }
+ }
+
+ if ((fdt_getprop(fdt, bsec_subnode,
+ "st,non-secure-otp", NULL)) == NULL) {
+ continue;
+ }
+
+ if (((offset % sizeof(uint32_t)) != 0U) ||
+ ((length % sizeof(uint32_t)) != 0U)) {
+ ERROR("Unaligned non-secure OTP\n");
+ panic();
+ }
+
+ size = length / sizeof(uint32_t);
+
+ for (i = otp; i < (otp + size); i++) {
+ enable_non_secure_access(i);
+ }
+ }
+}
+
+static void bsec_late_init(void)
+{
+ void *fdt;
+ int node;
+ struct dt_node_info bsec_info;
+
+ if (fdt_get_address(&fdt) == 0) {
+ panic();
+ }
+
+ node = bsec_get_dt_node(&bsec_info);
+ if (node < 0) {
+ panic();
+ }
+
+ assert(bsec_base == bsec_info.base);
+
+ bsec_dt_otp_nsec_access(fdt, node);
+}
+#endif
+
+static uint32_t otp_bank_offset(uint32_t otp)
+{
+ assert(otp <= STM32MP1_OTP_MAX_ID);
+
+ return ((otp & ~BSEC_OTP_MASK) >> BSEC_OTP_BANK_SHIFT) *
+ sizeof(uint32_t);
+}
+
+/*
+ * bsec_check_error: check BSEC error status.
+ * otp: OTP number.
+ * check_disturbed: check only error (false),
+ * or error and disturbed status (true).
+ * return value: BSEC_OK if no error.
+ */
+static uint32_t bsec_check_error(uint32_t otp, bool check_disturbed)
+{
+ uint32_t bit = BIT(otp & BSEC_OTP_MASK);
+ uint32_t bank = otp_bank_offset(otp);
+
+ if ((mmio_read_32(bsec_base + BSEC_ERROR_OFF + bank) & bit) != 0U) {
+ return BSEC_ERROR;
+ }
+
+ if (!check_disturbed) {
+ return BSEC_OK;
+ }
+
+ if ((mmio_read_32(bsec_base + BSEC_DISTURBED_OFF + bank) & bit) != 0U) {
+ return BSEC_DISTURBED;
+ }
+
+ return BSEC_OK;
+}
+
+/*
+ * bsec_probe: initialize BSEC driver.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_probe(void)
+{
+ bsec_base = BSEC_BASE;
+
+ if (is_otp_invalid_mode()) {
+ return BSEC_ERROR;
+ }
+
+ if ((((bsec_get_version() & BSEC_IPVR_MSK) != BSEC_IP_VERSION_1_1) &&
+ ((bsec_get_version() & BSEC_IPVR_MSK) != BSEC_IP_VERSION_2_0)) ||
+ (bsec_get_id() != BSEC_IP_ID_2)) {
+ panic();
+ }
+
+#if defined(IMAGE_BL32)
+ bsec_late_init();
+#endif
+ return BSEC_OK;
+}
+
+/*
+ * bsec_get_base: return BSEC base address.
+ */
+uint32_t bsec_get_base(void)
+{
+ return bsec_base;
+}
+
+/*
+ * bsec_set_config: enable and configure BSEC.
+ * cfg: pointer to param structure used to set register.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_set_config(struct bsec_config *cfg)
+{
+ uint32_t value;
+ uint32_t result;
+
+ if (is_otp_invalid_mode()) {
+ return BSEC_ERROR;
+ }
+
+ value = ((((uint32_t)cfg->freq << BSEC_CONF_FRQ_SHIFT) &
+ BSEC_CONF_FRQ_MASK) |
+ (((uint32_t)cfg->pulse_width << BSEC_CONF_PRG_WIDTH_SHIFT) &
+ BSEC_CONF_PRG_WIDTH_MASK) |
+ (((uint32_t)cfg->tread << BSEC_CONF_TREAD_SHIFT) &
+ BSEC_CONF_TREAD_MASK));
+
+ bsec_lock();
+
+ mmio_write_32(bsec_base + BSEC_OTP_CONF_OFF, value);
+
+ bsec_unlock();
+
+ result = bsec_power_safmem((bool)cfg->power &
+ BSEC_CONF_POWER_UP_MASK);
+ if (result != BSEC_OK) {
+ return result;
+ }
+
+ value = ((((uint32_t)cfg->upper_otp_lock << UPPER_OTP_LOCK_SHIFT) &
+ UPPER_OTP_LOCK_MASK) |
+ (((uint32_t)cfg->den_lock << DENREG_LOCK_SHIFT) &
+ DENREG_LOCK_MASK) |
+ (((uint32_t)cfg->prog_lock << GPLOCK_LOCK_SHIFT) &
+ GPLOCK_LOCK_MASK));
+
+ bsec_lock();
+
+ mmio_write_32(bsec_base + BSEC_OTP_LOCK_OFF, value);
+
+ bsec_unlock();
+
+ return BSEC_OK;
+}
+
+/*
+ * bsec_get_config: return config parameters set in BSEC registers.
+ * cfg: config param return.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_get_config(struct bsec_config *cfg)
+{
+ uint32_t value;
+
+ if (cfg == NULL) {
+ return BSEC_INVALID_PARAM;
+ }
+
+ value = mmio_read_32(bsec_base + BSEC_OTP_CONF_OFF);
+ cfg->power = (uint8_t)((value & BSEC_CONF_POWER_UP_MASK) >>
+ BSEC_CONF_POWER_UP_SHIFT);
+ cfg->freq = (uint8_t)((value & BSEC_CONF_FRQ_MASK) >>
+ BSEC_CONF_FRQ_SHIFT);
+ cfg->pulse_width = (uint8_t)((value & BSEC_CONF_PRG_WIDTH_MASK) >>
+ BSEC_CONF_PRG_WIDTH_SHIFT);
+ cfg->tread = (uint8_t)((value & BSEC_CONF_TREAD_MASK) >>
+ BSEC_CONF_TREAD_SHIFT);
+
+ value = mmio_read_32(bsec_base + BSEC_OTP_LOCK_OFF);
+ cfg->upper_otp_lock = (uint8_t)((value & UPPER_OTP_LOCK_MASK) >>
+ UPPER_OTP_LOCK_SHIFT);
+ cfg->den_lock = (uint8_t)((value & DENREG_LOCK_MASK) >>
+ DENREG_LOCK_SHIFT);
+ cfg->prog_lock = (uint8_t)((value & GPLOCK_LOCK_MASK) >>
+ GPLOCK_LOCK_SHIFT);
+
+ return BSEC_OK;
+}
+
+/*
+ * bsec_shadow_register: copy SAFMEM OTP to BSEC data.
+ * otp: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_shadow_register(uint32_t otp)
+{
+ uint32_t result;
+ bool value;
+ bool power_up = false;
+
+ if (is_otp_invalid_mode()) {
+ return BSEC_ERROR;
+ }
+
+ result = bsec_read_sr_lock(otp, &value);
+ if (result != BSEC_OK) {
+ ERROR("BSEC: %u Sticky-read bit read Error %u\n", otp, result);
+ return result;
+ }
+
+ if (value) {
+ VERBOSE("BSEC: OTP %u is locked and will not be refreshed\n",
+ otp);
+ }
+
+ if ((bsec_get_status() & BSEC_MODE_PWR_MASK) == 0U) {
+ result = bsec_power_safmem(true);
+
+ if (result != BSEC_OK) {
+ return result;
+ }
+
+ power_up = true;
+ }
+
+ bsec_lock();
+
+ mmio_write_32(bsec_base + BSEC_OTP_CTRL_OFF, otp | BSEC_READ);
+
+ while ((bsec_get_status() & BSEC_MODE_BUSY_MASK) != 0U) {
+ ;
+ }
+
+ result = bsec_check_error(otp, true);
+
+ bsec_unlock();
+
+ if (power_up) {
+ if (bsec_power_safmem(false) != BSEC_OK) {
+ panic();
+ }
+ }
+
+ return result;
+}
+
+/*
+ * bsec_read_otp: read an OTP data value.
+ * val: read value.
+ * otp: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_read_otp(uint32_t *val, uint32_t otp)
+{
+ if (is_otp_invalid_mode()) {
+ return BSEC_ERROR;
+ }
+
+ if (otp > STM32MP1_OTP_MAX_ID) {
+ return BSEC_INVALID_PARAM;
+ }
+
+ *val = mmio_read_32(bsec_base + BSEC_OTP_DATA_OFF +
+ (otp * sizeof(uint32_t)));
+
+ return BSEC_OK;
+}
+
+/*
+ * bsec_write_otp: write value in BSEC data register.
+ * val: value to write.
+ * otp: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_write_otp(uint32_t val, uint32_t otp)
+{
+ uint32_t result;
+ bool value;
+
+ if (is_otp_invalid_mode()) {
+ return BSEC_ERROR;
+ }
+
+ result = bsec_read_sw_lock(otp, &value);
+ if (result != BSEC_OK) {
+ ERROR("BSEC: %u Sticky-write bit read Error %u\n", otp, result);
+ return result;
+ }
+
+ if (value) {
+ VERBOSE("BSEC: OTP %u is locked and write will be ignored\n",
+ otp);
+ }
+
+ /* Ensure integrity of each register access sequence */
+ bsec_lock();
+
+ mmio_write_32(bsec_base + BSEC_OTP_DATA_OFF +
+ (otp * sizeof(uint32_t)), val);
+
+ bsec_unlock();
+
+ return result;
+}
+
+/*
+ * bsec_program_otp: program a bit in SAFMEM after the prog.
+ * The OTP data is not refreshed.
+ * val: value to program.
+ * otp: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_program_otp(uint32_t val, uint32_t otp)
+{
+ uint32_t result;
+ bool power_up = false;
+ bool sp_lock;
+ bool perm_lock;
+
+ if (is_otp_invalid_mode()) {
+ return BSEC_ERROR;
+ }
+
+ result = bsec_read_sp_lock(otp, &sp_lock);
+ if (result != BSEC_OK) {
+ ERROR("BSEC: %u Sticky-prog bit read Error %u\n", otp, result);
+ return result;
+ }
+
+ result = bsec_read_permanent_lock(otp, &perm_lock);
+ if (result != BSEC_OK) {
+ ERROR("BSEC: %u permanent bit read Error %u\n", otp, result);
+ return result;
+ }
+
+ if (sp_lock || perm_lock) {
+ WARN("BSEC: OTP locked, prog will be ignored\n");
+ return BSEC_PROG_FAIL;
+ }
+
+ if ((mmio_read_32(bsec_base + BSEC_OTP_LOCK_OFF) &
+ BIT(BSEC_LOCK_PROGRAM)) != 0U) {
+ WARN("BSEC: GPLOCK activated, prog will be ignored\n");
+ }
+
+ if ((bsec_get_status() & BSEC_MODE_PWR_MASK) == 0U) {
+ result = bsec_power_safmem(true);
+
+ if (result != BSEC_OK) {
+ return result;
+ }
+
+ power_up = true;
+ }
+
+ bsec_lock();
+
+ mmio_write_32(bsec_base + BSEC_OTP_WRDATA_OFF, val);
+
+ mmio_write_32(bsec_base + BSEC_OTP_CTRL_OFF, otp | BSEC_WRITE);
+
+ while ((bsec_get_status() & BSEC_MODE_BUSY_MASK) != 0U) {
+ ;
+ }
+
+ if ((bsec_get_status() & BSEC_MODE_PROGFAIL_MASK) != 0U) {
+ result = BSEC_PROG_FAIL;
+ } else {
+ result = bsec_check_error(otp, true);
+ }
+
+ bsec_unlock();
+
+ if (power_up) {
+ if (bsec_power_safmem(false) != BSEC_OK) {
+ panic();
+ }
+ }
+
+ return result;
+}
+
+/*
+ * bsec_permanent_lock_otp: permanent lock of OTP in SAFMEM.
+ * otp: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_permanent_lock_otp(uint32_t otp)
+{
+ uint32_t result;
+ bool power_up = false;
+ uint32_t data;
+ uint32_t addr;
+
+ if (is_otp_invalid_mode()) {
+ return BSEC_ERROR;
+ }
+
+ if (otp > STM32MP1_OTP_MAX_ID) {
+ return BSEC_INVALID_PARAM;
+ }
+
+ if ((bsec_get_status() & BSEC_MODE_PWR_MASK) == 0U) {
+ result = bsec_power_safmem(true);
+
+ if (result != BSEC_OK) {
+ return result;
+ }
+
+ power_up = true;
+ }
+
+ if (otp < STM32MP1_UPPER_OTP_START) {
+ addr = otp >> ADDR_LOWER_OTP_PERLOCK_SHIFT;
+ data = DATA_LOWER_OTP_PERLOCK_BIT <<
+ ((otp & DATA_LOWER_OTP_PERLOCK_MASK) << 1U);
+ } else {
+ addr = (otp >> ADDR_UPPER_OTP_PERLOCK_SHIFT) + 2U;
+ data = DATA_UPPER_OTP_PERLOCK_BIT <<
+ (otp & DATA_UPPER_OTP_PERLOCK_MASK);
+ }
+
+ bsec_lock();
+
+ mmio_write_32(bsec_base + BSEC_OTP_WRDATA_OFF, data);
+
+ mmio_write_32(bsec_base + BSEC_OTP_CTRL_OFF,
+ addr | BSEC_WRITE | BSEC_LOCK);
+
+ while ((bsec_get_status() & BSEC_MODE_BUSY_MASK) != 0U) {
+ ;
+ }
+
+ if ((bsec_get_status() & BSEC_MODE_PROGFAIL_MASK) != 0U) {
+ result = BSEC_PROG_FAIL;
+ } else {
+ result = bsec_check_error(otp, false);
+ }
+
+ bsec_unlock();
+
+ if (power_up) {
+ if (bsec_power_safmem(false) != BSEC_OK) {
+ panic();
+ }
+ }
+
+ return result;
+}
+
+/*
+ * bsec_write_debug_conf: write value in debug feature.
+ * to enable/disable debug service.
+ * val: value to write.
+ * return value: none.
+ */
+void bsec_write_debug_conf(uint32_t val)
+{
+ if (is_otp_invalid_mode()) {
+ return;
+ }
+
+ bsec_lock();
+ mmio_write_32(bsec_base + BSEC_DEN_OFF, val & BSEC_DEN_ALL_MSK);
+ bsec_unlock();
+}
+
+/*
+ * bsec_read_debug_conf: return debug configuration register value.
+ */
+uint32_t bsec_read_debug_conf(void)
+{
+ return mmio_read_32(bsec_base + BSEC_DEN_OFF);
+}
+
+/*
+ * bsec_write_scratch: write value in scratch register.
+ * val: value to write.
+ * return value: none.
+ */
+void bsec_write_scratch(uint32_t val)
+{
+#if defined(IMAGE_BL32)
+ if (is_otp_invalid_mode()) {
+ return;
+ }
+
+ bsec_lock();
+ mmio_write_32(bsec_base + BSEC_SCRATCH_OFF, val);
+ bsec_unlock();
+#else
+ mmio_write_32(BSEC_BASE + BSEC_SCRATCH_OFF, val);
+#endif
+}
+
+/*
+ * bsec_read_scratch: return scratch register value.
+ */
+uint32_t bsec_read_scratch(void)
+{
+ return mmio_read_32(bsec_base + BSEC_SCRATCH_OFF);
+}
+
+/*
+ * bsec_get_status: return status register value.
+ */
+uint32_t bsec_get_status(void)
+{
+ return mmio_read_32(bsec_base + BSEC_OTP_STATUS_OFF);
+}
+
+/*
+ * bsec_get_hw_conf: return hardware configuration register value.
+ */
+uint32_t bsec_get_hw_conf(void)
+{
+ return mmio_read_32(bsec_base + BSEC_IPHW_CFG_OFF);
+}
+
+/*
+ * bsec_get_version: return BSEC version register value.
+ */
+uint32_t bsec_get_version(void)
+{
+ return mmio_read_32(bsec_base + BSEC_IPVR_OFF);
+}
+
+/*
+ * bsec_get_id: return BSEC ID register value.
+ */
+uint32_t bsec_get_id(void)
+{
+ return mmio_read_32(bsec_base + BSEC_IP_ID_OFF);
+}
+
+/*
+ * bsec_get_magic_id: return BSEC magic number register value.
+ */
+uint32_t bsec_get_magic_id(void)
+{
+ return mmio_read_32(bsec_base + BSEC_IP_MAGIC_ID_OFF);
+}
+
+/*
+ * bsec_set_sr_lock: set shadow-read lock.
+ * otp: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_set_sr_lock(uint32_t otp)
+{
+ uint32_t bank = otp_bank_offset(otp);
+ uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
+
+ if (is_otp_invalid_mode()) {
+ return BSEC_ERROR;
+ }
+
+ if (otp > STM32MP1_OTP_MAX_ID) {
+ return BSEC_INVALID_PARAM;
+ }
+
+ bsec_lock();
+ mmio_write_32(bsec_base + BSEC_SRLOCK_OFF + bank, otp_mask);
+ bsec_unlock();
+
+ return BSEC_OK;
+}
+
+/*
+ * bsec_read_sr_lock: read shadow-read lock.
+ * otp: OTP number.
+ * value: read value (true or false).
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_read_sr_lock(uint32_t otp, bool *value)
+{
+ uint32_t bank = otp_bank_offset(otp);
+ uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
+ uint32_t bank_value;
+
+ if (otp > STM32MP1_OTP_MAX_ID) {
+ return BSEC_INVALID_PARAM;
+ }
+
+ bank_value = mmio_read_32(bsec_base + BSEC_SRLOCK_OFF + bank);
+
+ *value = ((bank_value & otp_mask) != 0U);
+
+ return BSEC_OK;
+}
+
+/*
+ * bsec_set_sw_lock: set shadow-write lock.
+ * otp: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_set_sw_lock(uint32_t otp)
+{
+ uint32_t bank = otp_bank_offset(otp);
+ uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
+
+ if (is_otp_invalid_mode()) {
+ return BSEC_ERROR;
+ }
+
+ if (otp > STM32MP1_OTP_MAX_ID) {
+ return BSEC_INVALID_PARAM;
+ }
+
+ bsec_lock();
+ mmio_write_32(bsec_base + BSEC_SWLOCK_OFF + bank, otp_mask);
+ bsec_unlock();
+
+ return BSEC_OK;
+}
+
+/*
+ * bsec_read_sw_lock: read shadow-write lock.
+ * otp: OTP number.
+ * value: read value (true or false).
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_read_sw_lock(uint32_t otp, bool *value)
+{
+ uint32_t bank = otp_bank_offset(otp);
+ uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
+ uint32_t bank_value;
+
+ if (otp > STM32MP1_OTP_MAX_ID) {
+ return BSEC_INVALID_PARAM;
+ }
+
+ bank_value = mmio_read_32(bsec_base + BSEC_SWLOCK_OFF + bank);
+
+ *value = ((bank_value & otp_mask) != 0U);
+
+ return BSEC_OK;
+}
+
+/*
+ * bsec_set_sp_lock: set shadow-program lock.
+ * otp: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_set_sp_lock(uint32_t otp)
+{
+ uint32_t bank = otp_bank_offset(otp);
+ uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
+
+ if (is_otp_invalid_mode()) {
+ return BSEC_ERROR;
+ }
+
+ if (otp > STM32MP1_OTP_MAX_ID) {
+ return BSEC_INVALID_PARAM;
+ }
+
+ bsec_lock();
+ mmio_write_32(bsec_base + BSEC_SPLOCK_OFF + bank, otp_mask);
+ bsec_unlock();
+
+ return BSEC_OK;
+}
+
+/*
+ * bsec_read_sp_lock: read shadow-program lock.
+ * otp: OTP number.
+ * value: read value (true or false).
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_read_sp_lock(uint32_t otp, bool *value)
+{
+ uint32_t bank = otp_bank_offset(otp);
+ uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
+ uint32_t bank_value;
+
+ if (otp > STM32MP1_OTP_MAX_ID) {
+ return BSEC_INVALID_PARAM;
+ }
+
+ bank_value = mmio_read_32(bsec_base + BSEC_SPLOCK_OFF + bank);
+
+ *value = ((bank_value & otp_mask) != 0U);
+
+ return BSEC_OK;
+}
+
+/*
+ * bsec_read_permanent_lock: Read permanent lock status.
+ * otp: OTP number.
+ * value: read value (true or false).
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_read_permanent_lock(uint32_t otp, bool *value)
+{
+ uint32_t bank = otp_bank_offset(otp);
+ uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
+ uint32_t bank_value;
+
+ if (otp > STM32MP1_OTP_MAX_ID) {
+ return BSEC_INVALID_PARAM;
+ }
+
+ bank_value = mmio_read_32(bsec_base + BSEC_WRLOCK_OFF + bank);
+
+ *value = ((bank_value & otp_mask) != 0U);
+
+ return BSEC_OK;
+}
+
+/*
+ * bsec_otp_lock: Lock Upper OTP or Global Programming or Debug Enable.
+ * service: Service to lock, see header file.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_otp_lock(uint32_t service)
+{
+ uintptr_t reg = bsec_base + BSEC_OTP_LOCK_OFF;
+
+ if (is_otp_invalid_mode()) {
+ return BSEC_ERROR;
+ }
+
+ switch (service) {
+ case BSEC_LOCK_UPPER_OTP:
+ mmio_write_32(reg, BIT(BSEC_LOCK_UPPER_OTP));
+ break;
+ case BSEC_LOCK_DEBUG:
+ mmio_write_32(reg, BIT(BSEC_LOCK_DEBUG));
+ break;
+ case BSEC_LOCK_PROGRAM:
+ mmio_write_32(reg, BIT(BSEC_LOCK_PROGRAM));
+ break;
+ default:
+ return BSEC_INVALID_PARAM;
+ }
+
+ return BSEC_OK;
+}
+
+/*
+ * bsec_power_safmem: Activate or deactivate SAFMEM power.
+ * power: true to power up, false to power down.
+ * return value: BSEC_OK if no error.
+ */
+static uint32_t bsec_power_safmem(bool power)
+{
+ uint32_t register_val;
+ uint32_t timeout = BSEC_TIMEOUT_VALUE;
+
+ bsec_lock();
+
+ register_val = mmio_read_32(bsec_base + BSEC_OTP_CONF_OFF);
+
+ if (power) {
+ register_val |= BSEC_CONF_POWER_UP_MASK;
+ } else {
+ register_val &= ~BSEC_CONF_POWER_UP_MASK;
+ }
+
+ mmio_write_32(bsec_base + BSEC_OTP_CONF_OFF, register_val);
+
+ if (power) {
+ while (((bsec_get_status() & BSEC_MODE_PWR_MASK) == 0U) &&
+ (timeout != 0U)) {
+ timeout--;
+ }
+ } else {
+ while (((bsec_get_status() & BSEC_MODE_PWR_MASK) != 0U) &&
+ (timeout != 0U)) {
+ timeout--;
+ }
+ }
+
+ bsec_unlock();
+
+ if (timeout == 0U) {
+ return BSEC_TIMEOUT;
+ }
+
+ return BSEC_OK;
+}
+
+/*
+ * bsec_shadow_read_otp: Load OTP from SAFMEM and provide its value.
+ * otp_value: read value.
+ * word: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_shadow_read_otp(uint32_t *otp_value, uint32_t word)
+{
+ uint32_t result;
+
+ result = bsec_shadow_register(word);
+ if (result != BSEC_OK) {
+ ERROR("BSEC: %u Shadowing Error %u\n", word, result);
+ return result;
+ }
+
+ result = bsec_read_otp(otp_value, word);
+ if (result != BSEC_OK) {
+ ERROR("BSEC: %u Read Error %u\n", word, result);
+ }
+
+ return result;
+}
+
+/*
+ * bsec_check_nsec_access_rights: check non-secure access rights to target OTP.
+ * otp: OTP number.
+ * return value: BSEC_OK if authorized access.
+ */
+uint32_t bsec_check_nsec_access_rights(uint32_t otp)
+{
+#if defined(IMAGE_BL32)
+ if (otp > STM32MP1_OTP_MAX_ID) {
+ return BSEC_INVALID_PARAM;
+ }
+
+ if (otp >= STM32MP1_UPPER_OTP_START) {
+ if (!non_secure_can_access(otp)) {
+ return BSEC_ERROR;
+ }
+ }
+#endif
+
+ return BSEC_OK;
+}
+
diff --git a/drivers/st/clk/clk-stm32-core.c b/drivers/st/clk/clk-stm32-core.c
new file mode 100644
index 0000000..bb03125
--- /dev/null
+++ b/drivers/st/clk/clk-stm32-core.c
@@ -0,0 +1,1096 @@
+/*
+ * 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;
+ unsigned long rate = 0UL;
+
+ if ((unsigned int)id >= priv->num) {
+ return rate;
+ }
+
+ 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);
+ }
+
+ rate = clk->ops->recalc_rate(priv, id, prate);
+
+ return rate;
+ }
+
+ switch (parent) {
+ case CLK_IS_ROOT:
+ panic();
+
+ default:
+ rate = _clk_stm32_get_rate(priv, parent);
+ break;
+ }
+ return rate;
+
+}
+
+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) {
+ 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) {
+ 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..c960928
--- /dev/null
+++ b/drivers/st/clk/clk-stm32mp13.c
@@ -0,0 +1,2332 @@
+/*
+ * Copyright (C) 2022, 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_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_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) {
+ 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 & CMD_MASK);
+ 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) {
+ 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) {
+ 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) {
+ return err;
+ }
+
+ err = clk_stm32_pll_init(priv, _PLL2);
+ if (err) {
+ return err;
+ }
+
+ err = clk_stm32_pll_init(priv, _PLL3);
+ if (err) {
+ return err;
+ }
+
+ err = clk_stm32_pll_init(priv, _PLL4);
+ if (err) {
+ 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..aa5db6f
--- /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 ouptut 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..01d1420
--- /dev/null
+++ b/drivers/st/clk/stm32mp_clkfunc.c
@@ -0,0 +1,377 @@
+/*
+ * Copyright (c) 2017-2022, 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;
+}
+
+/*
+ * 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;
+}
+
+/*
+ * 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 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;
+
+ mmio_write_32(STGEN_BASE + CNTCVL_OFF, (uint32_t)counter);
+ mmio_write_32(STGEN_BASE + CNTCVU_OFF, (uint32_t)(counter >> 32));
+ 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)
+{
+ return (((unsigned long long)mmio_read_32(STGEN_BASE + CNTCVU_OFF) << 32) |
+ mmio_read_32(STGEN_BASE + CNTCVL_OFF));
+}
+
+/*******************************************************************************
+ * 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);
+ mmio_write_32(STGEN_BASE + CNTCVL_OFF, (uint32_t)cnt);
+ mmio_write_32(STGEN_BASE + CNTCVU_OFF, (uint32_t)(cnt >> 32));
+ mmio_setbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN);
+}
diff --git a/drivers/st/crypto/stm32_hash.c b/drivers/st/crypto/stm32_hash.c
new file mode 100644
index 0000000..e92f980
--- /dev/null
+++ b/drivers/st/crypto/stm32_hash.c
@@ -0,0 +1,364 @@
+/*
+ * Copyright (c) 2019-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdint.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32_hash.h>
+#include <drivers/st/stm32mp_reset.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+#include <libfdt.h>
+#include <plat/common/platform.h>
+
+#include <platform_def.h>
+
+#if STM32_HASH_VER == 2
+#define DT_HASH_COMPAT "st,stm32f756-hash"
+#endif
+#if STM32_HASH_VER == 4
+#define DT_HASH_COMPAT "st,stm32mp13-hash"
+#endif
+
+#define HASH_CR 0x00U
+#define HASH_DIN 0x04U
+#define HASH_STR 0x08U
+#define HASH_SR 0x24U
+#define HASH_HREG(x) (0x310U + ((x) * 0x04U))
+
+/* Control Register */
+#define HASH_CR_INIT BIT(2)
+#define HASH_CR_DATATYPE_SHIFT U(4)
+#if STM32_HASH_VER == 2
+#define HASH_CR_ALGO_SHA1 0x0U
+#define HASH_CR_ALGO_MD5 BIT(7)
+#define HASH_CR_ALGO_SHA224 BIT(18)
+#define HASH_CR_ALGO_SHA256 (BIT(18) | BIT(7))
+#endif
+#if STM32_HASH_VER == 4
+#define HASH_CR_ALGO_SHIFT U(17)
+#define HASH_CR_ALGO_SHA1 (0x0U << HASH_CR_ALGO_SHIFT)
+#define HASH_CR_ALGO_SHA224 (0x2U << HASH_CR_ALGO_SHIFT)
+#define HASH_CR_ALGO_SHA256 (0x3U << HASH_CR_ALGO_SHIFT)
+#define HASH_CR_ALGO_SHA384 (0xCU << HASH_CR_ALGO_SHIFT)
+#define HASH_CR_ALGO_SHA512_224 (0xDU << HASH_CR_ALGO_SHIFT)
+#define HASH_CR_ALGO_SHA512_256 (0xEU << HASH_CR_ALGO_SHIFT)
+#define HASH_CR_ALGO_SHA512 (0xFU << HASH_CR_ALGO_SHIFT)
+#endif
+
+/* Status Flags */
+#define HASH_SR_DCIS BIT(1)
+#define HASH_SR_BUSY BIT(3)
+
+/* STR Register */
+#define HASH_STR_NBLW_MASK GENMASK(4, 0)
+#define HASH_STR_DCAL BIT(8)
+
+#define MD5_DIGEST_SIZE 16U
+#define SHA1_DIGEST_SIZE 20U
+#define SHA224_DIGEST_SIZE 28U
+#define SHA256_DIGEST_SIZE 32U
+#define SHA384_DIGEST_SIZE 48U
+#define SHA512_224_DIGEST_SIZE 28U
+#define SHA512_256_DIGEST_SIZE 32U
+#define SHA512_DIGEST_SIZE 64U
+
+#define RESET_TIMEOUT_US_1MS 1000U
+#define HASH_TIMEOUT_US 10000U
+
+enum stm32_hash_data_format {
+ HASH_DATA_32_BITS,
+ HASH_DATA_16_BITS,
+ HASH_DATA_8_BITS,
+ HASH_DATA_1_BIT
+};
+
+struct stm32_hash_instance {
+ uintptr_t base;
+ unsigned int clock;
+ size_t digest_size;
+};
+
+struct stm32_hash_remain {
+ uint32_t buffer;
+ size_t length;
+};
+
+/* Expect a single HASH peripheral */
+static struct stm32_hash_instance stm32_hash;
+static struct stm32_hash_remain stm32_remain;
+
+static uintptr_t hash_base(void)
+{
+ return stm32_hash.base;
+}
+
+static int hash_wait_busy(void)
+{
+ uint64_t timeout = timeout_init_us(HASH_TIMEOUT_US);
+
+ while ((mmio_read_32(hash_base() + HASH_SR) & HASH_SR_BUSY) != 0U) {
+ if (timeout_elapsed(timeout)) {
+ ERROR("%s: busy timeout\n", __func__);
+ return -ETIMEDOUT;
+ }
+ }
+
+ return 0;
+}
+
+static int hash_wait_computation(void)
+{
+ uint64_t timeout = timeout_init_us(HASH_TIMEOUT_US);
+
+ while ((mmio_read_32(hash_base() + HASH_SR) & HASH_SR_DCIS) == 0U) {
+ if (timeout_elapsed(timeout)) {
+ ERROR("%s: busy timeout\n", __func__);
+ return -ETIMEDOUT;
+ }
+ }
+
+ return 0;
+}
+
+static int hash_write_data(uint32_t data)
+{
+ int ret;
+
+ ret = hash_wait_busy();
+ if (ret != 0) {
+ return ret;
+ }
+
+ mmio_write_32(hash_base() + HASH_DIN, data);
+
+ return 0;
+}
+
+static void hash_hw_init(enum stm32_hash_algo_mode mode)
+{
+ uint32_t reg;
+
+ reg = HASH_CR_INIT | (HASH_DATA_8_BITS << HASH_CR_DATATYPE_SHIFT);
+
+ switch (mode) {
+#if STM32_HASH_VER == 2
+ case HASH_MD5SUM:
+ reg |= HASH_CR_ALGO_MD5;
+ stm32_hash.digest_size = MD5_DIGEST_SIZE;
+ break;
+#endif
+ case HASH_SHA1:
+ reg |= HASH_CR_ALGO_SHA1;
+ stm32_hash.digest_size = SHA1_DIGEST_SIZE;
+ break;
+ case HASH_SHA224:
+ reg |= HASH_CR_ALGO_SHA224;
+ stm32_hash.digest_size = SHA224_DIGEST_SIZE;
+ break;
+#if STM32_HASH_VER == 4
+ case HASH_SHA384:
+ reg |= HASH_CR_ALGO_SHA384;
+ stm32_hash.digest_size = SHA384_DIGEST_SIZE;
+ break;
+ case HASH_SHA512:
+ reg |= HASH_CR_ALGO_SHA512;
+ stm32_hash.digest_size = SHA512_DIGEST_SIZE;
+ break;
+#endif
+ /* Default selected algo is SHA256 */
+ case HASH_SHA256:
+ default:
+ reg |= HASH_CR_ALGO_SHA256;
+ stm32_hash.digest_size = SHA256_DIGEST_SIZE;
+ break;
+ }
+
+ mmio_write_32(hash_base() + HASH_CR, reg);
+}
+
+static int hash_get_digest(uint8_t *digest)
+{
+ int ret;
+ uint32_t i;
+ uint32_t dsg;
+
+ ret = hash_wait_computation();
+ if (ret != 0) {
+ return ret;
+ }
+
+ for (i = 0U; i < (stm32_hash.digest_size / sizeof(uint32_t)); i++) {
+ dsg = __builtin_bswap32(mmio_read_32(hash_base() +
+ HASH_HREG(i)));
+ memcpy(digest + (i * sizeof(uint32_t)), &dsg, sizeof(uint32_t));
+ }
+
+ /*
+ * Clean hardware context as HASH could be used later
+ * by non-secure software
+ */
+ hash_hw_init(HASH_SHA256);
+
+ return 0;
+}
+
+int stm32_hash_update(const uint8_t *buffer, size_t length)
+{
+ size_t remain_length = length;
+ int ret = 0;
+
+ if ((length == 0U) || (buffer == NULL)) {
+ return 0;
+ }
+
+ clk_enable(stm32_hash.clock);
+
+ if (stm32_remain.length != 0U) {
+ uint32_t copysize;
+
+ copysize = MIN((sizeof(uint32_t) - stm32_remain.length),
+ length);
+ memcpy(((uint8_t *)&stm32_remain.buffer) + stm32_remain.length,
+ buffer, copysize);
+ remain_length -= copysize;
+ buffer += copysize;
+ if (stm32_remain.length == sizeof(uint32_t)) {
+ ret = hash_write_data(stm32_remain.buffer);
+ if (ret != 0) {
+ goto exit;
+ }
+
+ zeromem(&stm32_remain, sizeof(stm32_remain));
+ }
+ }
+
+ while (remain_length / sizeof(uint32_t) != 0U) {
+ uint32_t tmp_buf;
+
+ memcpy(&tmp_buf, buffer, sizeof(uint32_t));
+ ret = hash_write_data(tmp_buf);
+ if (ret != 0) {
+ goto exit;
+ }
+
+ buffer += sizeof(uint32_t);
+ remain_length -= sizeof(uint32_t);
+ }
+
+ if (remain_length != 0U) {
+ assert(stm32_remain.length == 0U);
+
+ memcpy((uint8_t *)&stm32_remain.buffer, buffer, remain_length);
+ stm32_remain.length = remain_length;
+ }
+
+exit:
+ clk_disable(stm32_hash.clock);
+
+ return ret;
+}
+
+int stm32_hash_final(uint8_t *digest)
+{
+ int ret;
+
+ clk_enable(stm32_hash.clock);
+
+ if (stm32_remain.length != 0U) {
+ ret = hash_write_data(stm32_remain.buffer);
+ if (ret != 0) {
+ clk_disable(stm32_hash.clock);
+ return ret;
+ }
+
+ mmio_clrsetbits_32(hash_base() + HASH_STR, HASH_STR_NBLW_MASK,
+ 8U * stm32_remain.length);
+ zeromem(&stm32_remain, sizeof(stm32_remain));
+ } else {
+ mmio_clrbits_32(hash_base() + HASH_STR, HASH_STR_NBLW_MASK);
+ }
+
+ mmio_setbits_32(hash_base() + HASH_STR, HASH_STR_DCAL);
+
+ ret = hash_get_digest(digest);
+
+ clk_disable(stm32_hash.clock);
+
+ return ret;
+}
+
+int stm32_hash_final_update(const uint8_t *buffer, uint32_t length,
+ uint8_t *digest)
+{
+ int ret;
+
+ ret = stm32_hash_update(buffer, length);
+ if (ret != 0) {
+ return ret;
+ }
+
+ return stm32_hash_final(digest);
+}
+
+void stm32_hash_init(enum stm32_hash_algo_mode mode)
+{
+ clk_enable(stm32_hash.clock);
+
+ hash_hw_init(mode);
+
+ clk_disable(stm32_hash.clock);
+
+ zeromem(&stm32_remain, sizeof(stm32_remain));
+}
+
+int stm32_hash_register(void)
+{
+ struct dt_node_info hash_info;
+ int node;
+
+ for (node = dt_get_node(&hash_info, -1, DT_HASH_COMPAT);
+ node != -FDT_ERR_NOTFOUND;
+ node = dt_get_node(&hash_info, node, DT_HASH_COMPAT)) {
+ if (hash_info.status != DT_DISABLED) {
+ break;
+ }
+ }
+
+ if (node == -FDT_ERR_NOTFOUND) {
+ return -ENODEV;
+ }
+
+ if (hash_info.clock < 0) {
+ return -EINVAL;
+ }
+
+ stm32_hash.base = hash_info.base;
+ stm32_hash.clock = hash_info.clock;
+
+ clk_enable(stm32_hash.clock);
+
+ if (hash_info.reset >= 0) {
+ uint32_t id = (uint32_t)hash_info.reset;
+
+ if (stm32mp_reset_assert(id, RESET_TIMEOUT_US_1MS) != 0) {
+ panic();
+ }
+ udelay(20);
+ if (stm32mp_reset_deassert(id, RESET_TIMEOUT_US_1MS) != 0) {
+ panic();
+ }
+ }
+
+ clk_disable(stm32_hash.clock);
+
+ return 0;
+}
diff --git a/drivers/st/crypto/stm32_pka.c b/drivers/st/crypto/stm32_pka.c
new file mode 100644
index 0000000..e03cf0f
--- /dev/null
+++ b/drivers/st/crypto/stm32_pka.c
@@ -0,0 +1,707 @@
+/*
+ * Copyright (c) 2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdint.h>
+
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32_pka.h>
+#include <drivers/st/stm32mp_reset.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+#include <libfdt.h>
+#include <plat/common/platform.h>
+
+#include <platform_def.h>
+
+/*
+ * For our comprehension in this file
+ * _len are in BITs
+ * _size are in BYTEs
+ * _nbw are in number of PKA_word (PKA_word = u64)
+ */
+
+#define UINT8_LEN 8U
+#define UINT64_LEN (UINT8_LEN * sizeof(uint64_t))
+#define WORD_SIZE (sizeof(uint64_t))
+#define OP_NBW_FROM_LEN(len) (DIV_ROUND_UP_2EVAL((len), UINT64_LEN) + 1)
+#define OP_NBW_FROM_SIZE(s) OP_NBW_FROM_LEN((s) * UINT8_LEN)
+#define OP_SIZE_FROM_SIZE(s) (OP_NBW_FROM_SIZE(s) * WORD_SIZE)
+
+#define DT_PKA_COMPAT "st,stm32-pka64"
+
+#define MAX_ECC_SIZE_LEN 640U
+#define MAX_EO_NBW OP_NBW_FROM_LEN(MAX_ECC_SIZE_LEN)
+
+/* PKA registers */
+/* PKA control register */
+#define _PKA_CR 0x0U
+/* PKA status register */
+#define _PKA_SR 0x4U
+/* PKA clear flag register */
+#define _PKA_CLRFR 0x8U
+/* PKA version register */
+#define _PKA_VERR 0x1FF4U
+/* PKA identification register */
+#define _PKA_IPIDR 0x1FF8U
+
+/* PKA control register fields */
+#define _PKA_CR_MODE_MASK GENMASK(13, 8)
+#define _PKA_CR_MODE_SHIFT 8U
+#define _PKA_CR_MODE_ADD 0x9U
+#define _PKA_CR_MODE_ECDSA_VERIF 0x26U
+#define _PKA_CR_START BIT(1)
+#define _PKA_CR_EN BIT(0)
+
+/* PKA status register fields */
+#define _PKA_SR_BUSY BIT(16)
+#define _PKA_SR_LMF BIT(1)
+#define _PKA_SR_INITOK BIT(0)
+
+/* PKA it flag fields (used in CR, SR and CLRFR) */
+#define _PKA_IT_MASK (GENMASK(21, 19) | BIT(17))
+#define _PKA_IT_SHIFT 17U
+#define _PKA_IT_OPERR BIT(21)
+#define _PKA_IT_ADDRERR BIT(20)
+#define _PKA_IT_RAMERR BIT(19)
+#define _PKA_IT_PROCEND BIT(17)
+
+/* PKA version register fields */
+#define _PKA_VERR_MAJREV_MASK GENMASK(7, 4)
+#define _PKA_VERR_MAJREV_SHIFT 4U
+#define _PKA_VERR_MINREV_MASK GENMASK(3, 0)
+#define _PKA_VERR_MINREV_SHIFT 0U
+
+/* RAM magic offset */
+#define _PKA_RAM_START 0x400U
+#define _PKA_RAM_SIZE 5336U
+
+/* ECDSA verification */
+#define _PKA_RAM_N_LEN 0x408U /* 64 */
+#define _PKA_RAM_P_LEN 0x4C8U /* 64 */
+#define _PKA_RAM_A_SIGN 0x468U /* 64 */
+#define _PKA_RAM_A 0x470U /* EOS */
+#define _PKA_RAM_P 0x4D0U /* EOS */
+#define _PKA_RAM_XG 0x678U /* EOS */
+#define _PKA_RAM_YG 0x6D0U /* EOS */
+#define _PKA_RAM_XQ 0x12F8U /* EOS */
+#define _PKA_RAM_YQ 0x1350U /* EOS */
+#define _PKA_RAM_SIGN_R 0x10E0U /* EOS */
+#define _PKA_RAM_SIGN_S 0xC68U /* EOS */
+#define _PKA_RAM_HASH_Z 0x13A8U /* EOS */
+#define _PKA_RAM_PRIME_N 0x1088U /* EOS */
+#define _PKA_RAM_ECDSA_VERIFY 0x5D0U /* 64 */
+#define _PKA_RAM_ECDSA_VERIFY_VALID 0xD60DULL
+#define _PKA_RAM_ECDSA_VERIFY_INVALID 0xA3B7ULL
+
+#define PKA_TIMEOUT_US 1000000U
+#define TIMEOUT_US_1MS 1000U
+#define PKA_RESET_DELAY 20U
+
+struct curve_parameters {
+ uint32_t a_sign; /* 0 positive, 1 negative */
+ uint8_t *a; /* Curve coefficient |a| */
+ size_t a_size;
+ uint8_t *p; /* Curve modulus value */
+ uint32_t p_len;
+ uint8_t *xg; /* Curve base point G coordinate x */
+ size_t xg_size;
+ uint8_t *yg; /* Curve base point G coordinate y */
+ size_t yg_size;
+ uint8_t *n; /* Curve prime order n */
+ uint32_t n_len;
+};
+
+static const struct curve_parameters curve_def[] = {
+#if PKA_USE_NIST_P256
+ [PKA_NIST_P256] = {
+ .p_len = 256U,
+ .n_len = 256U,
+ .p = (uint8_t[]){0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x01,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF,
+ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF},
+ .n = (uint8_t[]){0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00,
+ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+ 0xBC, 0xE6, 0xFA, 0xAD, 0xA7, 0x17, 0x9E, 0x84,
+ 0xF3, 0xB9, 0xCA, 0xC2, 0xFC, 0x63, 0x25, 0x51},
+ .a_sign = 1U,
+ .a = (uint8_t[]){0x03},
+ .a_size = 1U,
+ .xg = (uint8_t[]){0x6B, 0x17, 0xD1, 0xF2, 0xE1, 0x2C, 0x42, 0x47,
+ 0xF8, 0xBC, 0xE6, 0xE5, 0x63, 0xA4, 0x40, 0xF2,
+ 0x77, 0x03, 0x7D, 0x81, 0x2D, 0xEB, 0x33, 0xA0,
+ 0xF4, 0xA1, 0x39, 0x45, 0xD8, 0x98, 0xC2, 0x96},
+ .xg_size = 32U,
+ .yg = (uint8_t[]){0x4F, 0xE3, 0x42, 0xE2, 0xFE, 0x1A, 0x7F, 0x9B,
+ 0x8E, 0xE7, 0xEB, 0x4A, 0x7C, 0x0F, 0x9E, 0x16,
+ 0x2B, 0xCE, 0x33, 0x57, 0x6B, 0x31, 0x5E, 0xCE,
+ 0xCB, 0xB6, 0x40, 0x68, 0x37, 0xBF, 0x51, 0xF5},
+ .yg_size = 32U,
+ },
+#endif
+#if PKA_USE_BRAINPOOL_P256R1
+ [PKA_BRAINPOOL_P256R1] = {
+ .p_len = 256,
+ .n_len = 256,
+ .p = (uint8_t[]){0xA9, 0xFB, 0x57, 0xDB, 0xA1, 0xEE, 0xA9, 0xBC,
+ 0x3E, 0x66, 0x0A, 0x90, 0x9D, 0x83, 0x8D, 0x72,
+ 0x6E, 0x3B, 0xF6, 0x23, 0xD5, 0x26, 0x20, 0x28,
+ 0x20, 0x13, 0x48, 0x1D, 0x1F, 0x6E, 0x53, 0x77},
+ .n = (uint8_t[]){0xA9, 0xFB, 0x57, 0xDB, 0xA1, 0xEE, 0xA9, 0xBC,
+ 0x3E, 0x66, 0x0A, 0x90, 0x9D, 0x83, 0x8D, 0x71,
+ 0x8C, 0x39, 0x7A, 0xA3, 0xB5, 0x61, 0xA6, 0xF7,
+ 0x90, 0x1E, 0x0E, 0x82, 0x97, 0x48, 0x56, 0xA7},
+ .a = (uint8_t[]){0x7D, 0x5A, 0x09, 0x75, 0xFC, 0x2C, 0x30, 0x57,
+ 0xEE, 0xF6, 0x75, 0x30, 0x41, 0x7A, 0xFF, 0xE7,
+ 0xFB, 0x80, 0x55, 0xC1, 0x26, 0xDC, 0x5C, 0x6C,
+ 0xE9, 0x4A, 0x4B, 0x44, 0xF3, 0x30, 0xB5, 0xD9},
+ .a_size = 32U,
+ .xg = (uint8_t[]){0x8B, 0xD2, 0xAE, 0xB9, 0xCB, 0x7E, 0x57, 0xCB,
+ 0x2C, 0x4B, 0x48, 0x2F, 0xFC, 0x81, 0xB7, 0xAF,
+ 0xB9, 0xDE, 0x27, 0xE1, 0xE3, 0xBD, 0x23, 0xC2,
+ 0x3A, 0x44, 0x53, 0xBD, 0x9A, 0xCE, 0x32, 0x62},
+ .xg_size = 32U,
+ .yg = (uint8_t[]){0x54, 0x7E, 0xF8, 0x35, 0xC3, 0xDA, 0xC4, 0xFD,
+ 0x97, 0xF8, 0x46, 0x1A, 0x14, 0x61, 0x1D, 0xC9,
+ 0xC2, 0x77, 0x45, 0x13, 0x2D, 0xED, 0x8E, 0x54,
+ 0x5C, 0x1D, 0x54, 0xC7, 0x2F, 0x04, 0x69, 0x97},
+ .yg_size = 32U,
+ },
+#endif
+#if PKA_USE_BRAINPOOL_P256T1
+ [PKA_BRAINPOOL_P256T1] = {
+ .p_len = 256,
+ .n_len = 256,
+ .p = (uint8_t[]){0xA9, 0xFB, 0x57, 0xDB, 0xA1, 0xEE, 0xA9, 0xBC,
+ 0x3E, 0x66, 0x0A, 0x90, 0x9D, 0x83, 0x8D, 0x72,
+ 0x6E, 0x3B, 0xF6, 0x23, 0xD5, 0x26, 0x20, 0x28,
+ 0x20, 0x13, 0x48, 0x1D, 0x1F, 0x6E, 0x53, 0x77},
+ .n = (uint8_t[]){0xA9, 0xFB, 0x57, 0xDB, 0xA1, 0xEE, 0xA9, 0xBC,
+ 0x3E, 0x66, 0x0A, 0x90, 0x9D, 0x83, 0x8D, 0x71,
+ 0x8C, 0x39, 0x7A, 0xA3, 0xB5, 0x61, 0xA6, 0xF7,
+ 0x90, 0x1E, 0x0E, 0x82, 0x97, 0x48, 0x56, 0xA7},
+ .a = (uint8_t[]){0xA9, 0xFB, 0x57, 0xDB, 0xA1, 0xEE, 0xA9, 0xBC,
+ 0x3E, 0x66, 0x0A, 0x90, 0x9D, 0x83, 0x8D, 0x72,
+ 0x6E, 0x3B, 0xF6, 0x23, 0xD5, 0x26, 0x20, 0x28,
+ 0x20, 0x13, 0x48, 0x1D, 0x1F, 0x6E, 0x53, 0x74},
+ .a_size = 32U,
+ .xg = (uint8_t[]){0xA3, 0xE8, 0xEB, 0x3C, 0xC1, 0xCF, 0xE7, 0xB7,
+ 0x73, 0x22, 0x13, 0xB2, 0x3A, 0x65, 0x61, 0x49,
+ 0xAF, 0xA1, 0x42, 0xC4, 0x7A, 0xAF, 0xBC, 0x2B,
+ 0x79, 0xA1, 0x91, 0x56, 0x2E, 0x13, 0x05, 0xF4},
+ .xg_size = 32U,
+ .yg = (uint8_t[]){0x2D, 0x99, 0x6C, 0x82, 0x34, 0x39, 0xC5, 0x6D,
+ 0x7F, 0x7B, 0x22, 0xE1, 0x46, 0x44, 0x41, 0x7E,
+ 0x69, 0xBC, 0xB6, 0xDE, 0x39, 0xD0, 0x27, 0x00,
+ 0x1D, 0xAB, 0xE8, 0xF3, 0x5B, 0x25, 0xC9, 0xBE},
+ .yg_size = 32U,
+ },
+#endif
+#if PKA_USE_NIST_P521
+ [PKA_NIST_P521] = {
+ .p_len = 521,
+ .n_len = 521,
+ .p = (uint8_t[]){ 0x01, 0xff,
+ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff},
+ .n = (uint8_t[]){ 0x01, 0xff,
+ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfa,
+ 0x51, 0x86, 0x87, 0x83, 0xbf, 0x2f, 0x96, 0x6b,
+ 0x7f, 0xcc, 0x01, 0x48, 0xf7, 0x09, 0xa5, 0xd0,
+ 0x3b, 0xb5, 0xc9, 0xb8, 0x89, 0x9c, 0x47, 0xae,
+ 0xbb, 0x6f, 0xb7, 0x1e, 0x91, 0x38, 0x64, 0x09},
+ .a_sign = 1,
+ .a = (uint8_t[]){0x03},
+ .a_size = 1U,
+ .xg = (uint8_t[]){ 0xc6,
+ 0x85, 0x8e, 0x06, 0xb7, 0x04, 0x04, 0xe9, 0xcd,
+ 0x9e, 0x3e, 0xcb, 0x66, 0x23, 0x95, 0xb4, 0x42,
+ 0x9c, 0x64, 0x81, 0x39, 0x05, 0x3f, 0xb5, 0x21,
+ 0xf8, 0x28, 0xaf, 0x60, 0x6b, 0x4d, 0x3d, 0xba,
+ 0xa1, 0x4b, 0x5e, 0x77, 0xef, 0xe7, 0x59, 0x28,
+ 0xfe, 0x1d, 0xc1, 0x27, 0xa2, 0xff, 0xa8, 0xde,
+ 0x33, 0x48, 0xb3, 0xc1, 0x85, 0x6a, 0x42, 0x9b,
+ 0xf9, 0x7e, 0x7e, 0x31, 0xc2, 0xe5, 0xbd, 0x66},
+ .xg_size = 65U,
+ .yg = (uint8_t[]){ 0x01, 0x18,
+ 0x39, 0x29, 0x6a, 0x78, 0x9a, 0x3b, 0xc0, 0x04,
+ 0x5c, 0x8a, 0x5f, 0xb4, 0x2c, 0x7d, 0x1b, 0xd9,
+ 0x98, 0xf5, 0x44, 0x49, 0x57, 0x9b, 0x44, 0x68,
+ 0x17, 0xaf, 0xbd, 0x17, 0x27, 0x3e, 0x66, 0x2c,
+ 0x97, 0xee, 0x72, 0x99, 0x5e, 0xf4, 0x26, 0x40,
+ 0xc5, 0x50, 0xb9, 0x01, 0x3f, 0xad, 0x07, 0x61,
+ 0x35, 0x3c, 0x70, 0x86, 0xa2, 0x72, 0xc2, 0x40,
+ 0x88, 0xbe, 0x94, 0x76, 0x9f, 0xd1, 0x66, 0x50},
+ .yg_size = 66U,
+ },
+#endif
+};
+
+static struct stm32_pka_platdata pka_pdata;
+
+#pragma weak stm32_pka_get_platdata
+
+int stm32_pka_get_platdata(struct stm32_pka_platdata *pdata)
+{
+ return -ENODEV;
+}
+
+static int stm32_pka_parse_fdt(void)
+{
+ int node;
+ struct dt_node_info info;
+ void *fdt;
+
+ if (fdt_get_address(&fdt) == 0) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ node = dt_get_node(&info, -1, DT_PKA_COMPAT);
+ if (node < 0) {
+ ERROR("No PKA entry in DT\n");
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ if (info.status == DT_DISABLED) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ if ((info.base == 0) || (info.clock < 0) || (info.reset < 0)) {
+ return -FDT_ERR_BADVALUE;
+ }
+
+ pka_pdata.base = (uintptr_t)info.base;
+ pka_pdata.clock_id = (unsigned long)info.clock;
+ pka_pdata.reset_id = (unsigned int)info.reset;
+
+ return 0;
+}
+
+static int pka_wait_bit(uintptr_t base, uint32_t bit)
+{
+ uint64_t timeout = timeout_init_us(PKA_TIMEOUT_US);
+
+ while ((mmio_read_32(base + _PKA_SR) & bit) != bit) {
+ if (timeout_elapsed(timeout)) {
+ WARN("timeout waiting %x\n", bit);
+ return -ETIMEDOUT;
+ }
+ }
+
+ return 0;
+
+}
+
+static void pka_disable(uintptr_t base)
+{
+ mmio_clrbits_32(base + _PKA_CR, _PKA_CR_EN);
+}
+
+static int pka_enable(uintptr_t base, uint32_t mode)
+{
+ /* Set mode and disable interrupts */
+ mmio_clrsetbits_32(base + _PKA_CR, _PKA_IT_MASK | _PKA_CR_MODE_MASK,
+ _PKA_CR_MODE_MASK & (mode << _PKA_CR_MODE_SHIFT));
+
+ mmio_setbits_32(base + _PKA_CR, _PKA_CR_EN);
+
+ return pka_wait_bit(base, _PKA_SR_INITOK);
+}
+
+/*
+ * Data are already loaded in PKA internal RAM
+ * MODE is set
+ * We start process, and wait for its end.
+ */
+static int stm32_pka_process(uintptr_t base)
+{
+ mmio_setbits_32(base + _PKA_CR, _PKA_CR_START);
+
+ return pka_wait_bit(base, _PKA_IT_PROCEND);
+}
+
+/**
+ * @brief Write ECC operand to PKA RAM.
+ * @note PKA expect to write u64 word, each u64 are: the least significant bit is
+ * bit 0; the most significant bit is bit 63.
+ * We write eo_nbw (ECC operand Size) u64, value that depends of the chosen
+ * prime modulus length in bits.
+ * First less signicant u64 is written to low address
+ * Most significant u64 to higher address.
+ * And at last address we write a u64(0x0)
+ * @note This function doesn't only manage endianness (as bswap64 do), but also
+ * complete most significant incomplete u64 with 0 (if data is not a u64
+ * multiple), and fill u64 last address with 0.
+ * @param addr: PKA_RAM address to write the buffer 'data'
+ * @param data: is a BYTE list with most significant bytes first
+ * @param data_size: nb of byte in data
+ * @param eo_nbw: is ECC Operand size in 64bits word (including the extra 0)
+ * (note it depends of the prime modulus length, not the data size)
+ * @retval 0 if OK.
+ * -EINVAL if data_size and eo_nbw are inconsistent, ie data doesn't
+ * fit in defined eo_nbw, or eo_nbw bigger than hardware limit.
+ */
+static int write_eo_data(uintptr_t addr, uint8_t *data, unsigned int data_size,
+ unsigned int eo_nbw)
+{
+ uint32_t word_index;
+ int data_index;
+
+ if ((eo_nbw < OP_NBW_FROM_SIZE(data_size)) || (eo_nbw > MAX_EO_NBW)) {
+ return -EINVAL;
+ }
+
+ /* Fill value */
+ data_index = (int)data_size - 1;
+ for (word_index = 0U; word_index < eo_nbw; word_index++) {
+ uint64_t tmp = 0ULL;
+ unsigned int i = 0U; /* index in the tmp U64 word */
+
+ /* Stop if end of tmp or end of data */
+ while ((i < sizeof(tmp)) && (data_index >= 0)) {
+ tmp |= (uint64_t)(data[data_index]) << (UINT8_LEN * i);
+ i++; /* Move byte index in current (u64)tmp */
+ data_index--; /* Move to just next most significat byte */
+ }
+
+ mmio_write_64(addr + word_index * sizeof(tmp), tmp);
+ }
+
+ return 0;
+}
+
+static unsigned int get_ecc_op_nbword(enum stm32_pka_ecdsa_curve_id cid)
+{
+ if (cid >= ARRAY_SIZE(curve_def)) {
+ ERROR("CID %u is out of boundaries\n", cid);
+ panic();
+ }
+
+ return OP_NBW_FROM_LEN(curve_def[cid].n_len);
+}
+
+static int stm32_pka_ecdsa_verif_configure_curve(uintptr_t base, enum stm32_pka_ecdsa_curve_id cid)
+{
+ int ret;
+ unsigned int eo_nbw = get_ecc_op_nbword(cid);
+
+ mmio_write_64(base + _PKA_RAM_N_LEN, curve_def[cid].n_len);
+ mmio_write_64(base + _PKA_RAM_P_LEN, curve_def[cid].p_len);
+ mmio_write_64(base + _PKA_RAM_A_SIGN, curve_def[cid].a_sign);
+
+ ret = write_eo_data(base + _PKA_RAM_A, curve_def[cid].a, curve_def[cid].a_size, eo_nbw);
+ if (ret < 0) {
+ return ret;
+ }
+
+ ret = write_eo_data(base + _PKA_RAM_PRIME_N,
+ curve_def[cid].n, div_round_up(curve_def[cid].n_len, UINT8_LEN),
+ eo_nbw);
+ if (ret < 0) {
+ return ret;
+ }
+
+ ret = write_eo_data(base + _PKA_RAM_P, curve_def[cid].p,
+ div_round_up(curve_def[cid].p_len, UINT8_LEN), eo_nbw);
+ if (ret < 0) {
+ return ret;
+ }
+
+ ret = write_eo_data(base + _PKA_RAM_XG, curve_def[cid].xg, curve_def[cid].xg_size, eo_nbw);
+ if (ret < 0) {
+ return ret;
+ }
+
+ ret = write_eo_data(base + _PKA_RAM_YG, curve_def[cid].yg, curve_def[cid].yg_size, eo_nbw);
+ if (ret < 0) {
+ return ret;
+ }
+
+ return 0;
+}
+
+static int stm32_pka_ecdsa_verif_check_return(uintptr_t base)
+{
+ uint64_t value;
+ uint32_t sr;
+
+ sr = mmio_read_32(base + _PKA_SR);
+ if ((sr & (_PKA_IT_OPERR | _PKA_IT_ADDRERR | _PKA_IT_RAMERR)) != 0) {
+ WARN("Detected error(s): %s%s%s\n",
+ (sr & _PKA_IT_OPERR) ? "Operation " : "",
+ (sr & _PKA_IT_ADDRERR) ? "Address " : "",
+ (sr & _PKA_IT_RAMERR) ? "RAM" : "");
+ return -EINVAL;
+ }
+
+ value = mmio_read_64(base + _PKA_RAM_ECDSA_VERIFY);
+ if (value == _PKA_RAM_ECDSA_VERIFY_VALID) {
+ return 0;
+ }
+
+ if (value == _PKA_RAM_ECDSA_VERIFY_INVALID) {
+ return -EAUTH;
+ }
+
+ return -EINVAL;
+}
+
+/**
+ * @brief Check if BigInt stored in data is 0
+ *
+ * @param data: a BYTE array with most significant bytes first
+ * @param size: data size
+ *
+ * @retval: true: if data represents a 0 value (ie all bytes == 0)
+ * false: if data represents a non-zero value.
+ */
+static bool is_zero(uint8_t *data, unsigned int size)
+{
+ unsigned int i;
+
+ for (i = 0U; i < size; i++) {
+ if (data[i] != 0U) {
+ return false;
+ }
+ }
+
+ return true;
+}
+
+/**
+ * @brief Compare two BigInt:
+ * @param xdata_a: a BYTE array with most significant bytes first
+ * @param size_a: nb of Byte of 'a'
+ * @param data_b: a BYTE array with most significant bytes first
+ * @param size_b: nb of Byte of 'b'
+ *
+ * @retval: true if data_a < data_b
+ * false if data_a >= data_b
+ */
+static bool is_smaller(uint8_t *data_a, unsigned int size_a,
+ uint8_t *data_b, unsigned int size_b)
+{
+ unsigned int i;
+
+ i = MAX(size_a, size_b) + 1U;
+ do {
+ uint8_t a, b;
+
+ i--;
+ if (size_a < i) {
+ a = 0U;
+ } else {
+ a = data_a[size_a - i];
+ }
+
+ if (size_b < i) {
+ b = 0U;
+ } else {
+ b = data_b[size_b - i];
+ }
+
+ if (a < b) {
+ return true;
+ }
+
+ if (a > b) {
+ return false;
+ }
+ } while (i != 0U);
+
+ return false;
+}
+
+static int stm32_pka_ecdsa_check_param(void *sig_r_ptr, unsigned int sig_r_size,
+ void *sig_s_ptr, unsigned int sig_s_size,
+ void *pk_x_ptr, unsigned int pk_x_size,
+ void *pk_y_ptr, unsigned int pk_y_size,
+ enum stm32_pka_ecdsa_curve_id cid)
+{
+ /* Public Key check */
+ /* Check Xq < p */
+ if (!is_smaller(pk_x_ptr, pk_x_size,
+ curve_def[cid].p, div_round_up(curve_def[cid].p_len, UINT8_LEN))) {
+ WARN("%s Xq < p inval\n", __func__);
+ return -EINVAL;
+ }
+
+ /* Check Yq < p */
+ if (!is_smaller(pk_y_ptr, pk_y_size,
+ curve_def[cid].p, div_round_up(curve_def[cid].p_len, UINT8_LEN))) {
+ WARN("%s Yq < p inval\n", __func__);
+ return -EINVAL;
+ }
+
+ /* Signature check */
+ /* Check 0 < r < n */
+ if (!is_smaller(sig_r_ptr, sig_r_size,
+ curve_def[cid].n, div_round_up(curve_def[cid].n_len, UINT8_LEN)) &&
+ !is_zero(sig_r_ptr, sig_r_size)) {
+ WARN("%s 0< r < n inval\n", __func__);
+ return -EINVAL;
+ }
+
+ /* Check 0 < s < n */
+ if (!is_smaller(sig_s_ptr, sig_s_size,
+ curve_def[cid].n, div_round_up(curve_def[cid].n_len, UINT8_LEN)) &&
+ !is_zero(sig_s_ptr, sig_s_size)) {
+ WARN("%s 0< s < n inval\n", __func__);
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+/*
+ * @brief Initialize the PKA driver.
+ * @param None.
+ * @retval 0 if OK, negative value else.
+ */
+int stm32_pka_init(void)
+{
+ int err;
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
+ uint32_t ver;
+ uint32_t id;
+#endif
+
+ err = stm32_pka_parse_fdt();
+ if (err != 0) {
+ err = stm32_pka_get_platdata(&pka_pdata);
+ if (err != 0) {
+ return err;
+ }
+ }
+
+ clk_enable(pka_pdata.clock_id);
+
+ if (stm32mp_reset_assert((unsigned long)pka_pdata.reset_id, TIMEOUT_US_1MS) != 0) {
+ panic();
+ }
+
+ udelay(PKA_RESET_DELAY);
+ if (stm32mp_reset_deassert((unsigned long)pka_pdata.reset_id, TIMEOUT_US_1MS) != 0) {
+ panic();
+ }
+
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
+ id = mmio_read_32(pka_pdata.base + _PKA_IPIDR);
+ ver = mmio_read_32(pka_pdata.base + _PKA_VERR);
+
+ VERBOSE("STM32 PKA[%x] V%u.%u\n", id,
+ (ver & _PKA_VERR_MAJREV_MASK) >> _PKA_VERR_MAJREV_SHIFT,
+ (ver & _PKA_VERR_MINREV_MASK) >> _PKA_VERR_MINREV_SHIFT);
+#endif
+ return 0;
+}
+
+int stm32_pka_ecdsa_verif(void *hash, unsigned int hash_size,
+ void *sig_r_ptr, unsigned int sig_r_size,
+ void *sig_s_ptr, unsigned int sig_s_size,
+ void *pk_x_ptr, unsigned int pk_x_size,
+ void *pk_y_ptr, unsigned int pk_y_size,
+ enum stm32_pka_ecdsa_curve_id cid)
+{
+ int ret;
+ uintptr_t base = pka_pdata.base;
+ unsigned int eo_nbw = get_ecc_op_nbword(cid);
+
+ if ((hash == NULL) || (sig_r_ptr == NULL) || (sig_s_ptr == NULL) ||
+ (pk_x_ptr == NULL) || (pk_y_ptr == NULL)) {
+ INFO("%s invalid input param\n", __func__);
+ return -EINVAL;
+ }
+
+ ret = stm32_pka_ecdsa_check_param(sig_r_ptr, sig_r_size,
+ sig_s_ptr, sig_s_size,
+ pk_x_ptr, pk_x_size,
+ pk_y_ptr, pk_y_size,
+ cid);
+ if (ret < 0) {
+ INFO("%s check param error %d\n", __func__, ret);
+ goto out;
+ }
+
+ if ((mmio_read_32(base + _PKA_SR) & _PKA_SR_BUSY) == _PKA_SR_BUSY) {
+ INFO("%s busy\n", __func__);
+ ret = -EBUSY;
+ goto out;
+ }
+
+ /* Fill PKA RAM */
+ /* With curve id values */
+ ret = stm32_pka_ecdsa_verif_configure_curve(base, cid);
+ if (ret < 0) {
+ goto out;
+ }
+
+ /* With pubkey */
+ ret = write_eo_data(base + _PKA_RAM_XQ, pk_x_ptr, pk_x_size, eo_nbw);
+ if (ret < 0) {
+ goto out;
+ }
+
+ ret = write_eo_data(base + _PKA_RAM_YQ, pk_y_ptr, pk_y_size, eo_nbw);
+ if (ret < 0) {
+ goto out;
+ }
+
+ /* With hash */
+ ret = write_eo_data(base + _PKA_RAM_HASH_Z, hash, hash_size, eo_nbw);
+ if (ret < 0) {
+ goto out;
+ }
+
+ /* With signature */
+ ret = write_eo_data(base + _PKA_RAM_SIGN_R, sig_r_ptr, sig_r_size, eo_nbw);
+ if (ret < 0) {
+ goto out;
+ }
+
+ ret = write_eo_data(base + _PKA_RAM_SIGN_S, sig_s_ptr, sig_s_size, eo_nbw);
+ if (ret < 0) {
+ goto out;
+ }
+
+ /* Set mode to ecdsa signature verification */
+ ret = pka_enable(base, _PKA_CR_MODE_ECDSA_VERIF);
+ if (ret < 0) {
+ WARN("%s set mode pka error %d\n", __func__, ret);
+ goto out;
+ }
+
+ /* Start processing and wait end */
+ ret = stm32_pka_process(base);
+ if (ret < 0) {
+ WARN("%s process error %d\n", __func__, ret);
+ goto out;
+ }
+
+ /* Check return status */
+ ret = stm32_pka_ecdsa_verif_check_return(base);
+
+ /* Unset end proc */
+ mmio_setbits_32(base + _PKA_CLRFR, _PKA_IT_PROCEND);
+
+out:
+ /* Disable PKA (will stop all pending proccess and reset RAM) */
+ pka_disable(base);
+
+ return ret;
+}
diff --git a/drivers/st/crypto/stm32_rng.c b/drivers/st/crypto/stm32_rng.c
new file mode 100644
index 0000000..a9dc43f
--- /dev/null
+++ b/drivers/st/crypto/stm32_rng.c
@@ -0,0 +1,269 @@
+/*
+ * Copyright (c) 2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdbool.h>
+
+#include <arch_helpers.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32_rng.h>
+#include <drivers/st/stm32mp_reset.h>
+#include <lib/mmio.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+#if STM32_RNG_VER == 2
+#define DT_RNG_COMPAT "st,stm32-rng"
+#endif
+#if STM32_RNG_VER == 4
+#define DT_RNG_COMPAT "st,stm32mp13-rng"
+#endif
+#define RNG_CR 0x00U
+#define RNG_SR 0x04U
+#define RNG_DR 0x08U
+
+#define RNG_CR_RNGEN BIT(2)
+#define RNG_CR_IE BIT(3)
+#define RNG_CR_CED BIT(5)
+#define RNG_CR_CLKDIV GENMASK(19, 16)
+#define RNG_CR_CLKDIV_SHIFT 16U
+#define RNG_CR_CONDRST BIT(30)
+
+#define RNG_SR_DRDY BIT(0)
+#define RNG_SR_CECS BIT(1)
+#define RNG_SR_SECS BIT(2)
+#define RNG_SR_CEIS BIT(5)
+#define RNG_SR_SEIS BIT(6)
+
+#define RNG_TIMEOUT_US 100000U
+#define RNG_TIMEOUT_STEP_US 10U
+
+#define TIMEOUT_US_1MS 1000U
+
+#define RNG_NIST_CONFIG_A 0x00F40F00U
+#define RNG_NIST_CONFIG_B 0x01801000U
+#define RNG_NIST_CONFIG_C 0x00F00D00U
+#define RNG_NIST_CONFIG_MASK GENMASK(25, 8)
+
+#define RNG_MAX_NOISE_CLK_FREQ 48000000U
+
+struct stm32_rng_instance {
+ uintptr_t base;
+ unsigned long clock;
+};
+
+static struct stm32_rng_instance stm32_rng;
+
+static void seed_error_recovery(void)
+{
+ uint8_t i __maybe_unused;
+
+ /* Recommended by the SoC reference manual */
+ mmio_clrbits_32(stm32_rng.base + RNG_SR, RNG_SR_SEIS);
+ dmbsy();
+
+#if STM32_RNG_VER == 2
+ /* No Auto-reset on version 2, need to clean FIFO */
+ for (i = 12U; i != 0U; i--) {
+ (void)mmio_read_32(stm32_rng.base + RNG_DR);
+ }
+
+ dmbsy();
+#endif
+
+ if ((mmio_read_32(stm32_rng.base + RNG_SR) & RNG_SR_SEIS) != 0U) {
+ ERROR("RNG noise\n");
+ panic();
+ }
+}
+
+static uint32_t stm32_rng_clock_freq_restrain(void)
+{
+ unsigned long clock_rate;
+ uint32_t clock_div = 0U;
+
+ clock_rate = clk_get_rate(stm32_rng.clock);
+
+ /*
+ * Get the exponent to apply on the CLKDIV field in RNG_CR register
+ * No need to handle the case when clock-div > 0xF as it is physically
+ * impossible
+ */
+ while ((clock_rate >> clock_div) > RNG_MAX_NOISE_CLK_FREQ) {
+ clock_div++;
+ }
+
+ VERBOSE("RNG clk rate : %lu\n", clk_get_rate(stm32_rng.clock) >> clock_div);
+
+ return clock_div;
+}
+
+static int stm32_rng_enable(void)
+{
+ uint32_t sr;
+ uint64_t timeout;
+ uint32_t clock_div __maybe_unused;
+
+#if STM32_RNG_VER == 2
+ mmio_write_32(stm32_rng.base + RNG_CR, RNG_CR_RNGEN | RNG_CR_CED);
+#endif
+#if STM32_RNG_VER == 4
+ /* Reset internal block and disable CED bit */
+ clock_div = stm32_rng_clock_freq_restrain();
+
+ /* Update configuration fields */
+ mmio_clrsetbits_32(stm32_rng.base + RNG_CR, RNG_NIST_CONFIG_MASK,
+ RNG_NIST_CONFIG_A | RNG_CR_CONDRST | RNG_CR_CED);
+
+ mmio_clrsetbits_32(stm32_rng.base + RNG_CR, RNG_CR_CLKDIV,
+ (clock_div << RNG_CR_CLKDIV_SHIFT));
+
+ mmio_clrsetbits_32(stm32_rng.base + RNG_CR, RNG_CR_CONDRST, RNG_CR_RNGEN);
+#endif
+ timeout = timeout_init_us(RNG_TIMEOUT_US);
+ sr = mmio_read_32(stm32_rng.base + RNG_SR);
+ while ((sr & RNG_SR_DRDY) == 0U) {
+ if (timeout_elapsed(timeout)) {
+ WARN("Timeout waiting\n");
+ return -ETIMEDOUT;
+ }
+
+ if ((sr & (RNG_SR_SECS | RNG_SR_SEIS)) != 0U) {
+ seed_error_recovery();
+ timeout = timeout_init_us(RNG_TIMEOUT_US);
+ }
+
+ udelay(RNG_TIMEOUT_STEP_US);
+ sr = mmio_read_32(stm32_rng.base + RNG_SR);
+ }
+
+ VERBOSE("Init RNG done\n");
+
+ return 0;
+}
+
+/*
+ * stm32_rng_read - Read a number of random bytes from RNG
+ * out: pointer to the output buffer
+ * size: number of bytes to be read
+ * Return 0 on success, non-0 on failure
+ */
+int stm32_rng_read(uint8_t *out, uint32_t size)
+{
+ uint8_t *buf = out;
+ size_t len = size;
+ int nb_tries;
+ uint32_t data32;
+ int rc = 0;
+ unsigned int count;
+
+ if (stm32_rng.base == 0U) {
+ return -EPERM;
+ }
+
+ while (len != 0U) {
+ nb_tries = RNG_TIMEOUT_US / RNG_TIMEOUT_STEP_US;
+ do {
+ uint32_t status = mmio_read_32(stm32_rng.base + RNG_SR);
+
+ if ((status & (RNG_SR_SECS | RNG_SR_SEIS)) != 0U) {
+ seed_error_recovery();
+ }
+
+ udelay(RNG_TIMEOUT_STEP_US);
+ nb_tries--;
+ if (nb_tries == 0) {
+ rc = -ETIMEDOUT;
+ goto bail;
+ }
+ } while ((mmio_read_32(stm32_rng.base + RNG_SR) &
+ RNG_SR_DRDY) == 0U);
+
+ count = 4U;
+ while (len != 0U) {
+ data32 = mmio_read_32(stm32_rng.base + RNG_DR);
+ count--;
+
+ memcpy(buf, &data32, MIN(len, sizeof(uint32_t)));
+ buf += MIN(len, sizeof(uint32_t));
+ len -= MIN(len, sizeof(uint32_t));
+
+ if (count == 0U) {
+ break;
+ }
+ }
+ }
+
+bail:
+ if (rc != 0) {
+ memset(out, 0, buf - out);
+ }
+
+ return rc;
+}
+
+/*
+ * stm32_rng_init: Initialize rng from DT
+ * return 0 on success, negative value on failure
+ */
+int stm32_rng_init(void)
+{
+ void *fdt;
+ struct dt_node_info dt_rng;
+ int node;
+
+ if (stm32_rng.base != 0U) {
+ /* Driver is already initialized */
+ return 0;
+ }
+
+ if (fdt_get_address(&fdt) == 0) {
+ panic();
+ }
+
+ node = dt_get_node(&dt_rng, -1, DT_RNG_COMPAT);
+ if (node < 0) {
+ return 0;
+ }
+
+ if (dt_rng.status == DT_DISABLED) {
+ return 0;
+ }
+
+ assert(dt_rng.base != 0U);
+
+ stm32_rng.base = dt_rng.base;
+
+ if (dt_rng.clock < 0) {
+ panic();
+ }
+
+ stm32_rng.clock = (unsigned long)dt_rng.clock;
+ clk_enable(stm32_rng.clock);
+
+ if (dt_rng.reset >= 0) {
+ int ret;
+
+ ret = stm32mp_reset_assert((unsigned long)dt_rng.reset,
+ TIMEOUT_US_1MS);
+ if (ret != 0) {
+ panic();
+ }
+
+ udelay(20);
+
+ ret = stm32mp_reset_deassert((unsigned long)dt_rng.reset,
+ TIMEOUT_US_1MS);
+ if (ret != 0) {
+ panic();
+ }
+ }
+
+ return stm32_rng_enable();
+}
diff --git a/drivers/st/crypto/stm32_saes.c b/drivers/st/crypto/stm32_saes.c
new file mode 100644
index 0000000..02baf21
--- /dev/null
+++ b/drivers/st/crypto/stm32_saes.c
@@ -0,0 +1,913 @@
+/*
+ * Copyright (c) 2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <assert.h>
+#include <endian.h>
+#include <errno.h>
+#include <stdint.h>
+
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32_saes.h>
+#include <drivers/st/stm32mp_reset.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+#define UINT8_BIT 8U
+#define AES_BLOCK_SIZE_BIT 128U
+#define AES_BLOCK_SIZE (AES_BLOCK_SIZE_BIT / UINT8_BIT)
+
+#define AES_KEYSIZE_128 16U
+#define AES_KEYSIZE_256 32U
+#define AES_IVSIZE 16U
+
+/* SAES control register */
+#define _SAES_CR 0x0U
+/* SAES status register */
+#define _SAES_SR 0x04U
+/* SAES data input register */
+#define _SAES_DINR 0x08U
+/* SAES data output register */
+#define _SAES_DOUTR 0x0CU
+/* SAES key registers [0-3] */
+#define _SAES_KEYR0 0x10U
+#define _SAES_KEYR1 0x14U
+#define _SAES_KEYR2 0x18U
+#define _SAES_KEYR3 0x1CU
+/* SAES initialization vector registers [0-3] */
+#define _SAES_IVR0 0x20U
+#define _SAES_IVR1 0x24U
+#define _SAES_IVR2 0x28U
+#define _SAES_IVR3 0x2CU
+/* SAES key registers [4-7] */
+#define _SAES_KEYR4 0x30U
+#define _SAES_KEYR5 0x34U
+#define _SAES_KEYR6 0x38U
+#define _SAES_KEYR7 0x3CU
+/* SAES suspend registers [0-7] */
+#define _SAES_SUSPR0 0x40U
+#define _SAES_SUSPR1 0x44U
+#define _SAES_SUSPR2 0x48U
+#define _SAES_SUSPR3 0x4CU
+#define _SAES_SUSPR4 0x50U
+#define _SAES_SUSPR5 0x54U
+#define _SAES_SUSPR6 0x58U
+#define _SAES_SUSPR7 0x5CU
+/* SAES Interrupt Enable Register */
+#define _SAES_IER 0x300U
+/* SAES Interrupt Status Register */
+#define _SAES_ISR 0x304U
+/* SAES Interrupt Clear Register */
+#define _SAES_ICR 0x308U
+
+/* SAES control register fields */
+#define _SAES_CR_RESET_VALUE 0x0U
+#define _SAES_CR_IPRST BIT(31)
+#define _SAES_CR_KEYSEL_MASK GENMASK(30, 28)
+#define _SAES_CR_KEYSEL_SHIFT 28U
+#define _SAES_CR_KEYSEL_SOFT 0x0U
+#define _SAES_CR_KEYSEL_DHUK 0x1U
+#define _SAES_CR_KEYSEL_BHK 0x2U
+#define _SAES_CR_KEYSEL_BHU_XOR_BH_K 0x4U
+#define _SAES_CR_KEYSEL_TEST 0x7U
+#define _SAES_CR_KSHAREID_MASK GENMASK(27, 26)
+#define _SAES_CR_KSHAREID_SHIFT 26U
+#define _SAES_CR_KSHAREID_CRYP 0x0U
+#define _SAES_CR_KEYMOD_MASK GENMASK(25, 24)
+#define _SAES_CR_KEYMOD_SHIFT 24U
+#define _SAES_CR_KEYMOD_NORMAL 0x0U
+#define _SAES_CR_KEYMOD_WRAPPED 0x1U
+#define _SAES_CR_KEYMOD_SHARED 0x2U
+#define _SAES_CR_NPBLB_MASK GENMASK(23, 20)
+#define _SAES_CR_NPBLB_SHIFT 20U
+#define _SAES_CR_KEYPROT BIT(19)
+#define _SAES_CR_KEYSIZE BIT(18)
+#define _SAES_CR_GCMPH_MASK GENMASK(14, 13)
+#define _SAES_CR_GCMPH_SHIFT 13U
+#define _SAES_CR_GCMPH_INIT 0U
+#define _SAES_CR_GCMPH_HEADER 1U
+#define _SAES_CR_GCMPH_PAYLOAD 2U
+#define _SAES_CR_GCMPH_FINAL 3U
+#define _SAES_CR_DMAOUTEN BIT(12)
+#define _SAES_CR_DMAINEN BIT(11)
+#define _SAES_CR_CHMOD_MASK (BIT(16) | GENMASK(6, 5))
+#define _SAES_CR_CHMOD_SHIFT 5U
+#define _SAES_CR_CHMOD_ECB 0x0U
+#define _SAES_CR_CHMOD_CBC 0x1U
+#define _SAES_CR_CHMOD_CTR 0x2U
+#define _SAES_CR_CHMOD_GCM 0x3U
+#define _SAES_CR_CHMOD_GMAC 0x3U
+#define _SAES_CR_CHMOD_CCM 0x800U
+#define _SAES_CR_MODE_MASK GENMASK(4, 3)
+#define _SAES_CR_MODE_SHIFT 3U
+#define _SAES_CR_MODE_ENC 0U
+#define _SAES_CR_MODE_KEYPREP 1U
+#define _SAES_CR_MODE_DEC 2U
+#define _SAES_CR_DATATYPE_MASK GENMASK(2, 1)
+#define _SAES_CR_DATATYPE_SHIFT 1U
+#define _SAES_CR_DATATYPE_NONE 0U
+#define _SAES_CR_DATATYPE_HALF_WORD 1U
+#define _SAES_CR_DATATYPE_BYTE 2U
+#define _SAES_CR_DATATYPE_BIT 3U
+#define _SAES_CR_EN BIT(0)
+
+/* SAES status register fields */
+#define _SAES_SR_KEYVALID BIT(7)
+#define _SAES_SR_BUSY BIT(3)
+#define _SAES_SR_WRERR BIT(2)
+#define _SAES_SR_RDERR BIT(1)
+#define _SAES_SR_CCF BIT(0)
+
+/* SAES interrupt registers fields */
+#define _SAES_I_RNG_ERR BIT(3)
+#define _SAES_I_KEY_ERR BIT(2)
+#define _SAES_I_RW_ERR BIT(1)
+#define _SAES_I_CC BIT(0)
+
+#define SAES_TIMEOUT_US 100000U
+#define TIMEOUT_US_1MS 1000U
+#define SAES_RESET_DELAY 20U
+
+#define IS_CHAINING_MODE(mod, cr) \
+ (((cr) & _SAES_CR_CHMOD_MASK) == (_SAES_CR_CHMOD_##mod << _SAES_CR_CHMOD_SHIFT))
+
+#define SET_CHAINING_MODE(mod, cr) \
+ mmio_clrsetbits_32((cr), _SAES_CR_CHMOD_MASK, _SAES_CR_CHMOD_##mod << _SAES_CR_CHMOD_SHIFT)
+
+#define pragma weak stm32_saes_get_platdata
+
+static struct stm32_saes_platdata saes_pdata;
+
+int stm32_saes_get_platdata(struct stm32_saes_platdata *pdata)
+{
+ return -ENODEV;
+}
+
+static int stm32_saes_parse_fdt(struct stm32_saes_platdata *pdata)
+{
+ int node;
+ struct dt_node_info info;
+ void *fdt;
+
+ if (fdt_get_address(&fdt) == 0) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ node = dt_get_node(&info, -1, DT_SAES_COMPAT);
+ if (node < 0) {
+ ERROR("No SAES entry in DT\n");
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ if (info.status == DT_DISABLED) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ if ((info.base == 0U) || (info.clock < 0) || (info.reset < 0)) {
+ return -FDT_ERR_BADVALUE;
+ }
+
+ pdata->base = (uintptr_t)info.base;
+ pdata->clock_id = (unsigned long)info.clock;
+ pdata->reset_id = (unsigned int)info.reset;
+
+ return 0;
+}
+
+static bool does_chaining_mode_need_iv(uint32_t cr)
+{
+ return !(IS_CHAINING_MODE(ECB, cr));
+}
+
+static bool is_encrypt(uint32_t cr)
+{
+ return (cr & _SAES_CR_MODE_MASK) == (_SAES_CR_MODE_ENC << _SAES_CR_MODE_SHIFT);
+}
+
+static bool is_decrypt(uint32_t cr)
+{
+ return (cr & _SAES_CR_MODE_MASK) == (_SAES_CR_MODE_DEC << _SAES_CR_MODE_SHIFT);
+}
+
+static int wait_computation_completed(uintptr_t base)
+{
+ uint64_t timeout = timeout_init_us(SAES_TIMEOUT_US);
+
+ while ((mmio_read_32(base + _SAES_SR) & _SAES_SR_CCF) != _SAES_SR_CCF) {
+ if (timeout_elapsed(timeout)) {
+ WARN("%s: timeout\n", __func__);
+ return -ETIMEDOUT;
+ }
+ }
+
+ return 0;
+}
+
+static void clear_computation_completed(uintptr_t base)
+{
+ mmio_setbits_32(base + _SAES_ICR, _SAES_I_CC);
+}
+
+static int saes_start(struct stm32_saes_context *ctx)
+{
+ uint64_t timeout;
+
+ /* Reset IP */
+ mmio_setbits_32(ctx->base + _SAES_CR, _SAES_CR_IPRST);
+ udelay(SAES_RESET_DELAY);
+ mmio_clrbits_32(ctx->base + _SAES_CR, _SAES_CR_IPRST);
+
+ timeout = timeout_init_us(SAES_TIMEOUT_US);
+ while ((mmio_read_32(ctx->base + _SAES_SR) & _SAES_SR_BUSY) == _SAES_SR_BUSY) {
+ if (timeout_elapsed(timeout)) {
+ WARN("%s: timeout\n", __func__);
+ return -ETIMEDOUT;
+ }
+ }
+
+ return 0;
+}
+
+static void saes_end(struct stm32_saes_context *ctx, int prev_error)
+{
+ if (prev_error != 0) {
+ /* Reset IP */
+ mmio_setbits_32(ctx->base + _SAES_CR, _SAES_CR_IPRST);
+ udelay(SAES_RESET_DELAY);
+ mmio_clrbits_32(ctx->base + _SAES_CR, _SAES_CR_IPRST);
+ }
+
+ /* Disable the SAES peripheral */
+ mmio_clrbits_32(ctx->base + _SAES_CR, _SAES_CR_EN);
+}
+
+static void saes_write_iv(struct stm32_saes_context *ctx)
+{
+ /* If chaining mode need to restore IV */
+ if (does_chaining_mode_need_iv(ctx->cr)) {
+ uint8_t i;
+
+ /* Restore the _SAES_IVRx */
+ for (i = 0U; i < AES_IVSIZE / sizeof(uint32_t); i++) {
+ mmio_write_32(ctx->base + _SAES_IVR0 + i * sizeof(uint32_t), ctx->iv[i]);
+ }
+ }
+
+}
+
+static void saes_write_key(struct stm32_saes_context *ctx)
+{
+ /* Restore the _SAES_KEYRx if SOFTWARE key */
+ if ((ctx->cr & _SAES_CR_KEYSEL_MASK) == (_SAES_CR_KEYSEL_SOFT << _SAES_CR_KEYSEL_SHIFT)) {
+ uint8_t i;
+
+ for (i = 0U; i < AES_KEYSIZE_128 / sizeof(uint32_t); i++) {
+ mmio_write_32(ctx->base + _SAES_KEYR0 + i * sizeof(uint32_t), ctx->key[i]);
+ }
+
+ if ((ctx->cr & _SAES_CR_KEYSIZE) == _SAES_CR_KEYSIZE) {
+ for (i = 0U; i < (AES_KEYSIZE_256 / 2U) / sizeof(uint32_t); i++) {
+ mmio_write_32(ctx->base + _SAES_KEYR4 + i * sizeof(uint32_t),
+ ctx->key[i + 4U]);
+ }
+ }
+ }
+}
+
+static int saes_prepare_key(struct stm32_saes_context *ctx)
+{
+ /* Disable the SAES peripheral */
+ mmio_clrbits_32(ctx->base + _SAES_CR, _SAES_CR_EN);
+
+ /* Set key size */
+ if ((ctx->cr & _SAES_CR_KEYSIZE) != 0U) {
+ mmio_setbits_32(ctx->base + _SAES_CR, _SAES_CR_KEYSIZE);
+ } else {
+ mmio_clrbits_32(ctx->base + _SAES_CR, _SAES_CR_KEYSIZE);
+ }
+
+ saes_write_key(ctx);
+
+ /* For ECB/CBC decryption, key preparation mode must be selected to populate the key */
+ if ((IS_CHAINING_MODE(ECB, ctx->cr) || IS_CHAINING_MODE(CBC, ctx->cr)) &&
+ is_decrypt(ctx->cr)) {
+ int ret;
+
+ /* Select Mode 2 */
+ mmio_clrsetbits_32(ctx->base + _SAES_CR, _SAES_CR_MODE_MASK,
+ _SAES_CR_MODE_KEYPREP << _SAES_CR_MODE_SHIFT);
+
+ /* Enable SAES */
+ mmio_setbits_32(ctx->base + _SAES_CR, _SAES_CR_EN);
+
+ /* Wait Computation completed */
+ ret = wait_computation_completed(ctx->base);
+ if (ret != 0) {
+ return ret;
+ }
+
+ clear_computation_completed(ctx->base);
+
+ /* Set Mode 3 */
+ mmio_clrsetbits_32(ctx->base + _SAES_CR, _SAES_CR_MODE_MASK,
+ _SAES_CR_MODE_DEC << _SAES_CR_MODE_SHIFT);
+ }
+
+ return 0;
+}
+
+static int save_context(struct stm32_saes_context *ctx)
+{
+ if ((mmio_read_32(ctx->base + _SAES_SR) & _SAES_SR_CCF) != 0U) {
+ /* Device should not be in a processing phase */
+ return -EINVAL;
+ }
+
+ /* Save CR */
+ ctx->cr = mmio_read_32(ctx->base + _SAES_CR);
+
+ /* If chaining mode need to save current IV */
+ if (does_chaining_mode_need_iv(ctx->cr)) {
+ uint8_t i;
+
+ /* Save IV */
+ for (i = 0U; i < AES_IVSIZE / sizeof(uint32_t); i++) {
+ ctx->iv[i] = mmio_read_32(ctx->base + _SAES_IVR0 + i * sizeof(uint32_t));
+ }
+ }
+
+ /* Disable the SAES peripheral */
+ mmio_clrbits_32(ctx->base + _SAES_CR, _SAES_CR_EN);
+
+ return 0;
+}
+
+/* To resume the processing of a message */
+static int restore_context(struct stm32_saes_context *ctx)
+{
+ int ret;
+
+ /* IP should be disabled */
+ if ((mmio_read_32(ctx->base + _SAES_CR) & _SAES_CR_EN) != 0U) {
+ VERBOSE("%s: Device is still enabled\n", __func__);
+ return -EINVAL;
+ }
+
+ /* Reset internal state */
+ mmio_setbits_32(ctx->base + _SAES_CR, _SAES_CR_IPRST);
+
+ /* Restore the _SAES_CR */
+ mmio_write_32(ctx->base + _SAES_CR, ctx->cr);
+
+ /* Preparation decrypt key */
+ ret = saes_prepare_key(ctx);
+ if (ret != 0) {
+ return ret;
+ }
+
+ saes_write_iv(ctx);
+
+ /* Enable the SAES peripheral */
+ mmio_setbits_32(ctx->base + _SAES_CR, _SAES_CR_EN);
+
+ return 0;
+}
+
+/**
+ * @brief Initialize SAES driver.
+ * @param None.
+ * @retval 0 if OK; negative value else.
+ */
+int stm32_saes_driver_init(void)
+{
+ int err;
+
+ err = stm32_saes_parse_fdt(&saes_pdata);
+ if (err != 0) {
+ err = stm32_saes_get_platdata(&saes_pdata);
+ if (err != 0) {
+ return err;
+ }
+ }
+
+ clk_enable(saes_pdata.clock_id);
+ if (stm32mp_reset_assert(saes_pdata.reset_id, TIMEOUT_US_1MS) != 0) {
+ panic();
+ }
+
+ udelay(SAES_RESET_DELAY);
+ if (stm32mp_reset_deassert(saes_pdata.reset_id, TIMEOUT_US_1MS) != 0) {
+ panic();
+ }
+
+ return 0;
+}
+
+/**
+ * @brief Start a AES computation.
+ * @param ctx: SAES process context
+ * @param is_dec: true if decryption, false if encryption
+ * @param ch_mode: define the chaining mode
+ * @param key_select: define where the key comes from.
+ * @param key: pointer to key (if key_select is KEY_SOFT, else unused)
+ * @param key_size: key size
+ * @param iv: pointer to initialization vectore (unsed if ch_mode is ECB)
+ * @param iv_size: iv size
+ * @note this function doesn't access to hardware but store in ctx the values
+ *
+ * @retval 0 if OK; negative value else.
+ */
+int stm32_saes_init(struct stm32_saes_context *ctx, bool is_dec,
+ enum stm32_saes_chaining_mode ch_mode, enum stm32_saes_key_selection key_select,
+ const void *key, size_t key_size, const void *iv, size_t iv_size)
+{
+ unsigned int i;
+ const uint32_t *iv_u32;
+ const uint32_t *key_u32;
+
+ ctx->assoc_len = 0U;
+ ctx->load_len = 0U;
+
+ ctx->base = saes_pdata.base;
+ ctx->cr = _SAES_CR_RESET_VALUE;
+
+ /* We want buffer to be u32 aligned */
+ assert((uintptr_t)key % __alignof__(uint32_t) == 0);
+ assert((uintptr_t)iv % __alignof__(uint32_t) == 0);
+
+ iv_u32 = iv;
+ key_u32 = key;
+
+ if (is_dec) {
+ /* Save Mode 3 = decrypt */
+ mmio_clrsetbits_32((uintptr_t)&(ctx->cr), _SAES_CR_MODE_MASK,
+ _SAES_CR_MODE_DEC << _SAES_CR_MODE_SHIFT);
+ } else {
+ /* Save Mode 1 = crypt */
+ mmio_clrsetbits_32((uintptr_t)&(ctx->cr), _SAES_CR_MODE_MASK,
+ _SAES_CR_MODE_ENC << _SAES_CR_MODE_SHIFT);
+ }
+
+ /* Save chaining mode */
+ switch (ch_mode) {
+ case STM32_SAES_MODE_ECB:
+ SET_CHAINING_MODE(ECB, (uintptr_t)&(ctx->cr));
+ break;
+ case STM32_SAES_MODE_CBC:
+ SET_CHAINING_MODE(CBC, (uintptr_t)&(ctx->cr));
+ break;
+ case STM32_SAES_MODE_CTR:
+ SET_CHAINING_MODE(CTR, (uintptr_t)&(ctx->cr));
+ break;
+ case STM32_SAES_MODE_GCM:
+ SET_CHAINING_MODE(GCM, (uintptr_t)&(ctx->cr));
+ break;
+ case STM32_SAES_MODE_CCM:
+ SET_CHAINING_MODE(CCM, (uintptr_t)&(ctx->cr));
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ /* We will use HW Byte swap (_SAES_CR_DATATYPE_BYTE) for data.
+ * so we won't need to
+ * htobe32(data) before write to DINR
+ * nor
+ * be32toh after reading from DOUTR
+ *
+ * But note that wrap key only accept _SAES_CR_DATATYPE_NONE
+ */
+ mmio_clrsetbits_32((uintptr_t)&(ctx->cr), _SAES_CR_DATATYPE_MASK,
+ _SAES_CR_DATATYPE_BYTE << _SAES_CR_DATATYPE_SHIFT);
+
+ /* Configure keysize */
+ switch (key_size) {
+ case AES_KEYSIZE_128:
+ mmio_clrbits_32((uintptr_t)&(ctx->cr), _SAES_CR_KEYSIZE);
+ break;
+ case AES_KEYSIZE_256:
+ mmio_setbits_32((uintptr_t)&(ctx->cr), _SAES_CR_KEYSIZE);
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ /* Configure key */
+ switch (key_select) {
+ case STM32_SAES_KEY_SOFT:
+ mmio_clrsetbits_32((uintptr_t)&(ctx->cr), _SAES_CR_KEYSEL_MASK,
+ _SAES_CR_KEYSEL_SOFT << _SAES_CR_KEYSEL_SHIFT);
+ /* Save key */
+ switch (key_size) {
+ case AES_KEYSIZE_128:
+ /* First 16 bytes == 4 u32 */
+ for (i = 0U; i < AES_KEYSIZE_128 / sizeof(uint32_t); i++) {
+ mmio_write_32((uintptr_t)(ctx->key + i), htobe32(key_u32[3 - i]));
+ /* /!\ we save the key in HW byte order
+ * and word order : key[i] is for _SAES_KEYRi
+ */
+ }
+ break;
+ case AES_KEYSIZE_256:
+ for (i = 0U; i < AES_KEYSIZE_256 / sizeof(uint32_t); i++) {
+ mmio_write_32((uintptr_t)(ctx->key + i), htobe32(key_u32[7 - i]));
+ /* /!\ we save the key in HW byte order
+ * and word order : key[i] is for _SAES_KEYRi
+ */
+ }
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ break;
+ case STM32_SAES_KEY_DHU:
+ mmio_clrsetbits_32((uintptr_t)&(ctx->cr), _SAES_CR_KEYSEL_MASK,
+ _SAES_CR_KEYSEL_DHUK << _SAES_CR_KEYSEL_SHIFT);
+ break;
+ case STM32_SAES_KEY_BH:
+ mmio_clrsetbits_32((uintptr_t)&(ctx->cr), _SAES_CR_KEYSEL_MASK,
+ _SAES_CR_KEYSEL_BHK << _SAES_CR_KEYSEL_SHIFT);
+ break;
+ case STM32_SAES_KEY_BHU_XOR_BH:
+ mmio_clrsetbits_32((uintptr_t)&(ctx->cr), _SAES_CR_KEYSEL_MASK,
+ _SAES_CR_KEYSEL_BHU_XOR_BH_K << _SAES_CR_KEYSEL_SHIFT);
+ break;
+ case STM32_SAES_KEY_WRAPPED:
+ mmio_clrsetbits_32((uintptr_t)&(ctx->cr), _SAES_CR_KEYSEL_MASK,
+ _SAES_CR_KEYSEL_SOFT << _SAES_CR_KEYSEL_SHIFT);
+ break;
+
+ default:
+ return -EINVAL;
+ }
+
+ /* Save IV */
+ if (ch_mode != STM32_SAES_MODE_ECB) {
+ if ((iv == NULL) || (iv_size != AES_IVSIZE)) {
+ return -EINVAL;
+ }
+
+ for (i = 0U; i < AES_IVSIZE / sizeof(uint32_t); i++) {
+ mmio_write_32((uintptr_t)(ctx->iv + i), htobe32(iv_u32[3 - i]));
+ /* /!\ We save the iv in HW byte order */
+ }
+ }
+
+ return saes_start(ctx);
+}
+
+/**
+ * @brief Update (or start) a AES authentificate process of associated data (CCM or GCM).
+ * @param ctx: SAES process context
+ * @param last_block: true if last assoc data block
+ * @param data: pointer to associated data
+ * @param data_size: data size
+ *
+ * @retval 0 if OK; negative value else.
+ */
+int stm32_saes_update_assodata(struct stm32_saes_context *ctx, bool last_block,
+ uint8_t *data, size_t data_size)
+{
+ int ret;
+ uint32_t *data_u32;
+ unsigned int i = 0U;
+
+ /* We want buffers to be u32 aligned */
+ assert((uintptr_t)data % __alignof__(uint32_t) == 0);
+ data_u32 = (uint32_t *)data;
+
+ /* Init phase */
+ ret = restore_context(ctx);
+ if (ret != 0) {
+ goto out;
+ }
+
+ ret = wait_computation_completed(ctx->base);
+ if (ret != 0) {
+ return ret;
+ }
+
+ clear_computation_completed(ctx->base);
+
+ if ((data == NULL) || (data_size == 0U)) {
+ /* No associated data */
+ /* ret already = 0 */
+ goto out;
+ }
+
+ /* There is an header/associated data phase */
+ mmio_clrsetbits_32(ctx->base + _SAES_CR, _SAES_CR_GCMPH_MASK,
+ _SAES_CR_GCMPH_HEADER << _SAES_CR_GCMPH_SHIFT);
+
+ /* Enable the SAES peripheral */
+ mmio_setbits_32(ctx->base + _SAES_CR, _SAES_CR_EN);
+
+ while (i < round_down(data_size, AES_BLOCK_SIZE)) {
+ unsigned int w; /* Word index */
+
+ w = i / sizeof(uint32_t);
+ /* No need to htobe() as we configure the HW to swap bytes */
+ mmio_write_32(ctx->base + _SAES_DINR, data_u32[w + 0U]);
+ mmio_write_32(ctx->base + _SAES_DINR, data_u32[w + 1U]);
+ mmio_write_32(ctx->base + _SAES_DINR, data_u32[w + 2U]);
+ mmio_write_32(ctx->base + _SAES_DINR, data_u32[w + 3U]);
+
+ ret = wait_computation_completed(ctx->base);
+ if (ret != 0) {
+ goto out;
+ }
+
+ clear_computation_completed(ctx->base);
+
+ /* Process next block */
+ i += AES_BLOCK_SIZE;
+ ctx->assoc_len += AES_BLOCK_SIZE_BIT;
+ }
+
+ /* Manage last block if not a block size multiple */
+ if ((last_block) && (i < data_size)) {
+ /* We don't manage unaligned last block yet */
+ ret = -ENODEV;
+ goto out;
+ }
+
+out:
+ if (ret != 0) {
+ saes_end(ctx, ret);
+ }
+
+ return ret;
+}
+
+/**
+ * @brief Update (or start) a AES authenticate and de/encrypt with payload data (CCM or GCM).
+ * @param ctx: SAES process context
+ * @param last_block: true if last payload data block
+ * @param data_in: pointer to payload
+ * @param data_out: pointer where to save de/encrypted payload
+ * @param data_size: payload size
+ *
+ * @retval 0 if OK; negative value else.
+ */
+int stm32_saes_update_load(struct stm32_saes_context *ctx, bool last_block,
+ uint8_t *data_in, uint8_t *data_out, size_t data_size)
+{
+ int ret = 0;
+ uint32_t *data_in_u32;
+ uint32_t *data_out_u32;
+ unsigned int i = 0U;
+ uint32_t prev_cr;
+
+ /* We want buffers to be u32 aligned */
+ assert((uintptr_t)data_in % __alignof__(uint32_t) == 0);
+ assert((uintptr_t)data_out % __alignof__(uint32_t) == 0);
+ data_in_u32 = (uint32_t *)data_in;
+ data_out_u32 = (uint32_t *)data_out;
+
+ prev_cr = mmio_read_32(ctx->base + _SAES_CR);
+
+ if ((data_in == NULL) || (data_size == 0U)) {
+ /* there is no data */
+ goto out;
+ }
+
+ /* There is a load phase */
+ mmio_clrsetbits_32(ctx->base + _SAES_CR, _SAES_CR_GCMPH_MASK,
+ _SAES_CR_GCMPH_PAYLOAD << _SAES_CR_GCMPH_SHIFT);
+
+ if ((prev_cr & _SAES_CR_GCMPH_MASK) ==
+ (_SAES_CR_GCMPH_INIT << _SAES_CR_GCMPH_SHIFT)) {
+ /* Still in initialization phase, no header
+ * We need to enable the SAES peripheral
+ */
+ mmio_setbits_32(ctx->base + _SAES_CR, _SAES_CR_EN);
+ }
+
+ while (i < round_down(data_size, AES_BLOCK_SIZE)) {
+ unsigned int w; /* Word index */
+
+ w = i / sizeof(uint32_t);
+ /* No need to htobe() as we configure the HW to swap bytes */
+ mmio_write_32(ctx->base + _SAES_DINR, data_in_u32[w + 0U]);
+ mmio_write_32(ctx->base + _SAES_DINR, data_in_u32[w + 1U]);
+ mmio_write_32(ctx->base + _SAES_DINR, data_in_u32[w + 2U]);
+ mmio_write_32(ctx->base + _SAES_DINR, data_in_u32[w + 3U]);
+
+ ret = wait_computation_completed(ctx->base);
+ if (ret != 0) {
+ goto out;
+ }
+
+ /* No need to htobe() as we configure the HW to swap bytes */
+ data_out_u32[w + 0U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+ data_out_u32[w + 1U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+ data_out_u32[w + 2U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+ data_out_u32[w + 3U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+
+ clear_computation_completed(ctx->base);
+
+ /* Process next block */
+ i += AES_BLOCK_SIZE;
+ ctx->load_len += AES_BLOCK_SIZE_BIT;
+ }
+ /* Manage last block if not a block size multiple */
+ if ((last_block) && (i < data_size)) {
+ uint32_t block_in[AES_BLOCK_SIZE / sizeof(uint32_t)] = {0};
+ uint32_t block_out[AES_BLOCK_SIZE / sizeof(uint32_t)] = {0};
+
+ memcpy(block_in, data_in + i, data_size - i);
+
+ /* No need to htobe() as we configure the HW to swap bytes */
+ mmio_write_32(ctx->base + _SAES_DINR, block_in[0U]);
+ mmio_write_32(ctx->base + _SAES_DINR, block_in[1U]);
+ mmio_write_32(ctx->base + _SAES_DINR, block_in[2U]);
+ mmio_write_32(ctx->base + _SAES_DINR, block_in[3U]);
+
+ ret = wait_computation_completed(ctx->base);
+ if (ret != 0) {
+ VERBOSE("%s %d\n", __func__, __LINE__);
+ goto out;
+ }
+
+ /* No need to htobe() as we configure the HW to swap bytes */
+ block_out[0U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+ block_out[1U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+ block_out[2U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+ block_out[3U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+
+ clear_computation_completed(ctx->base);
+
+ memcpy(data_out + i, block_out, data_size - i);
+
+ ctx->load_len += (data_size - i) * UINT8_BIT;
+ }
+
+out:
+ if (ret != 0) {
+ saes_end(ctx, ret);
+ }
+
+ return ret;
+}
+
+/**
+ * @brief Get authentication tag for AES authenticated algorithms (CCM or GCM).
+ * @param ctx: SAES process context
+ * @param tag: pointer where to save the tag
+ * @param data_size: tag size
+ *
+ * @retval 0 if OK; negative value else.
+ */
+int stm32_saes_final(struct stm32_saes_context *ctx, uint8_t *tag,
+ size_t tag_size)
+{
+ int ret;
+ uint32_t tag_u32[4];
+ uint32_t prev_cr;
+
+ prev_cr = mmio_read_32(ctx->base + _SAES_CR);
+
+ mmio_clrsetbits_32(ctx->base + _SAES_CR, _SAES_CR_GCMPH_MASK,
+ _SAES_CR_GCMPH_FINAL << _SAES_CR_GCMPH_SHIFT);
+
+ if ((prev_cr & _SAES_CR_GCMPH_MASK) == (_SAES_CR_GCMPH_INIT << _SAES_CR_GCMPH_SHIFT)) {
+ /* Still in initialization phase, no header
+ * We need to enable the SAES peripheral
+ */
+ mmio_setbits_32(ctx->base + _SAES_CR, _SAES_CR_EN);
+ }
+
+ /* No need to htobe() as we configure the HW to swap bytes */
+ mmio_write_32(ctx->base + _SAES_DINR, 0);
+ mmio_write_32(ctx->base + _SAES_DINR, ctx->assoc_len);
+ mmio_write_32(ctx->base + _SAES_DINR, 0);
+ mmio_write_32(ctx->base + _SAES_DINR, ctx->load_len);
+
+ ret = wait_computation_completed(ctx->base);
+ if (ret != 0) {
+ goto out;
+ }
+
+ /* No need to htobe() as we configure the HW to swap bytes */
+ tag_u32[0] = mmio_read_32(ctx->base + _SAES_DOUTR);
+ tag_u32[1] = mmio_read_32(ctx->base + _SAES_DOUTR);
+ tag_u32[2] = mmio_read_32(ctx->base + _SAES_DOUTR);
+ tag_u32[3] = mmio_read_32(ctx->base + _SAES_DOUTR);
+
+ clear_computation_completed(ctx->base);
+
+ memcpy(tag, tag_u32, MIN(sizeof(tag_u32), tag_size));
+
+out:
+ saes_end(ctx, ret);
+
+ return ret;
+}
+
+/**
+ * @brief Update (or start) a AES de/encrypt process (ECB, CBC or CTR).
+ * @param ctx: SAES process context
+ * @param last_block: true if last payload data block
+ * @param data_in: pointer to payload
+ * @param data_out: pointer where to save de/encrypted payload
+ * @param data_size: payload size
+ *
+ * @retval 0 if OK; negative value else.
+ */
+int stm32_saes_update(struct stm32_saes_context *ctx, bool last_block,
+ uint8_t *data_in, uint8_t *data_out, size_t data_size)
+{
+ int ret;
+ uint32_t *data_in_u32;
+ uint32_t *data_out_u32;
+ unsigned int i = 0U;
+
+ /* We want buffers to be u32 aligned */
+ assert((uintptr_t)data_in % __alignof__(uint32_t) == 0);
+ assert((uintptr_t)data_out % __alignof__(uint32_t) == 0);
+ data_in_u32 = (uint32_t *)data_in;
+ data_out_u32 = (uint32_t *)data_out;
+
+ if ((!last_block) &&
+ (round_down(data_size, AES_BLOCK_SIZE) != data_size)) {
+ ERROR("%s: non last block must be multiple of 128 bits\n",
+ __func__);
+ ret = -EINVAL;
+ goto out;
+ }
+
+ /* In CBC encryption we need to manage specifically last 2 128bits
+ * blocks if total size in not a block size aligned
+ * work TODO. Currently return ENODEV.
+ * Morevoer as we need to know last 2 block, if unaligned and
+ * call with less than two block, return -EINVAL.
+ */
+ if (last_block && IS_CHAINING_MODE(CBC, ctx->cr) && is_encrypt(ctx->cr) &&
+ (round_down(data_size, AES_BLOCK_SIZE) != data_size)) {
+ if (data_size < AES_BLOCK_SIZE * 2U) {
+ ERROR("if CBC, last part size should be at least 2 * AES_BLOCK_SIZE\n");
+ ret = -EINVAL;
+ goto out;
+ }
+ /* Moreover the CBC specific padding for encrypt is not yet implemented */
+ ret = -ENODEV;
+ goto out;
+ }
+
+ ret = restore_context(ctx);
+ if (ret != 0) {
+ goto out;
+ }
+
+ while (i < round_down(data_size, AES_BLOCK_SIZE)) {
+ unsigned int w; /* Word index */
+
+ w = i / sizeof(uint32_t);
+ /* No need to htobe() as we configure the HW to swap bytes */
+ mmio_write_32(ctx->base + _SAES_DINR, data_in_u32[w + 0U]);
+ mmio_write_32(ctx->base + _SAES_DINR, data_in_u32[w + 1U]);
+ mmio_write_32(ctx->base + _SAES_DINR, data_in_u32[w + 2U]);
+ mmio_write_32(ctx->base + _SAES_DINR, data_in_u32[w + 3U]);
+
+ ret = wait_computation_completed(ctx->base);
+ if (ret != 0) {
+ goto out;
+ }
+
+ /* No need to htobe() as we configure the HW to swap bytes */
+ data_out_u32[w + 0U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+ data_out_u32[w + 1U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+ data_out_u32[w + 2U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+ data_out_u32[w + 3U] = mmio_read_32(ctx->base + _SAES_DOUTR);
+
+ clear_computation_completed(ctx->base);
+
+ /* Process next block */
+ i += AES_BLOCK_SIZE;
+ }
+ /* Manage last block if not a block size multiple */
+
+ if ((last_block) && (i < data_size)) {
+ /* In and out buffer have same size so should be AES_BLOCK_SIZE multiple */
+ ret = -ENODEV;
+ goto out;
+ }
+
+ if (!last_block) {
+ ret = save_context(ctx);
+ }
+
+out:
+ /* If last block or error, end of SAES process */
+ if (last_block || (ret != 0)) {
+ saes_end(ctx, ret);
+ }
+
+ return ret;
+}
diff --git a/drivers/st/ddr/stm32mp1_ddr.c b/drivers/st/ddr/stm32mp1_ddr.c
new file mode 100644
index 0000000..4719e1e
--- /dev/null
+++ b/drivers/st/ddr/stm32mp1_ddr.c
@@ -0,0 +1,764 @@
+/*
+ * Copyright (C) 2018-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+ */
+
+#include <errno.h>
+#include <stddef.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32mp1_ddr.h>
+#include <drivers/st/stm32mp1_ddr_regs.h>
+#include <drivers/st/stm32mp1_pwr.h>
+#include <drivers/st/stm32mp1_ram.h>
+#include <drivers/st/stm32mp_ddr.h>
+#include <lib/mmio.h>
+#include <plat/common/platform.h>
+
+#include <platform_def.h>
+
+#define DDRCTL_REG(x, y) \
+ { \
+ .name = #x, \
+ .offset = offsetof(struct stm32mp_ddrctl, x), \
+ .par_offset = offsetof(struct y, x) \
+ }
+
+#define DDRPHY_REG(x, y) \
+ { \
+ .name = #x, \
+ .offset = offsetof(struct stm32mp_ddrphy, x), \
+ .par_offset = offsetof(struct y, x) \
+ }
+
+/*
+ * PARAMETERS: value get from device tree :
+ * size / order need to be aligned with binding
+ * modification NOT ALLOWED !!!
+ */
+#define DDRCTL_REG_REG_SIZE 25 /* st,ctl-reg */
+#define DDRCTL_REG_TIMING_SIZE 12 /* st,ctl-timing */
+#define DDRCTL_REG_MAP_SIZE 9 /* st,ctl-map */
+#if STM32MP_DDR_DUAL_AXI_PORT
+#define DDRCTL_REG_PERF_SIZE 17 /* st,ctl-perf */
+#else
+#define DDRCTL_REG_PERF_SIZE 11 /* st,ctl-perf */
+#endif
+
+#if STM32MP_DDR_32BIT_INTERFACE
+#define DDRPHY_REG_REG_SIZE 11 /* st,phy-reg */
+#else
+#define DDRPHY_REG_REG_SIZE 9 /* st,phy-reg */
+#endif
+#define DDRPHY_REG_TIMING_SIZE 10 /* st,phy-timing */
+
+#define DDRCTL_REG_REG(x) DDRCTL_REG(x, stm32mp1_ddrctrl_reg)
+static const struct stm32mp_ddr_reg_desc ddr_reg[DDRCTL_REG_REG_SIZE] = {
+ DDRCTL_REG_REG(mstr),
+ DDRCTL_REG_REG(mrctrl0),
+ DDRCTL_REG_REG(mrctrl1),
+ DDRCTL_REG_REG(derateen),
+ DDRCTL_REG_REG(derateint),
+ DDRCTL_REG_REG(pwrctl),
+ DDRCTL_REG_REG(pwrtmg),
+ DDRCTL_REG_REG(hwlpctl),
+ DDRCTL_REG_REG(rfshctl0),
+ DDRCTL_REG_REG(rfshctl3),
+ DDRCTL_REG_REG(crcparctl0),
+ DDRCTL_REG_REG(zqctl0),
+ DDRCTL_REG_REG(dfitmg0),
+ DDRCTL_REG_REG(dfitmg1),
+ DDRCTL_REG_REG(dfilpcfg0),
+ DDRCTL_REG_REG(dfiupd0),
+ DDRCTL_REG_REG(dfiupd1),
+ DDRCTL_REG_REG(dfiupd2),
+ DDRCTL_REG_REG(dfiphymstr),
+ DDRCTL_REG_REG(odtmap),
+ DDRCTL_REG_REG(dbg0),
+ DDRCTL_REG_REG(dbg1),
+ DDRCTL_REG_REG(dbgcmd),
+ DDRCTL_REG_REG(poisoncfg),
+ DDRCTL_REG_REG(pccfg),
+};
+
+#define DDRCTL_REG_TIMING(x) DDRCTL_REG(x, stm32mp1_ddrctrl_timing)
+static const struct stm32mp_ddr_reg_desc ddr_timing[DDRCTL_REG_TIMING_SIZE] = {
+ DDRCTL_REG_TIMING(rfshtmg),
+ DDRCTL_REG_TIMING(dramtmg0),
+ DDRCTL_REG_TIMING(dramtmg1),
+ DDRCTL_REG_TIMING(dramtmg2),
+ DDRCTL_REG_TIMING(dramtmg3),
+ DDRCTL_REG_TIMING(dramtmg4),
+ DDRCTL_REG_TIMING(dramtmg5),
+ DDRCTL_REG_TIMING(dramtmg6),
+ DDRCTL_REG_TIMING(dramtmg7),
+ DDRCTL_REG_TIMING(dramtmg8),
+ DDRCTL_REG_TIMING(dramtmg14),
+ DDRCTL_REG_TIMING(odtcfg),
+};
+
+#define DDRCTL_REG_MAP(x) DDRCTL_REG(x, stm32mp1_ddrctrl_map)
+static const struct stm32mp_ddr_reg_desc ddr_map[DDRCTL_REG_MAP_SIZE] = {
+ DDRCTL_REG_MAP(addrmap1),
+ DDRCTL_REG_MAP(addrmap2),
+ DDRCTL_REG_MAP(addrmap3),
+ DDRCTL_REG_MAP(addrmap4),
+ DDRCTL_REG_MAP(addrmap5),
+ DDRCTL_REG_MAP(addrmap6),
+ DDRCTL_REG_MAP(addrmap9),
+ DDRCTL_REG_MAP(addrmap10),
+ DDRCTL_REG_MAP(addrmap11),
+};
+
+#define DDRCTL_REG_PERF(x) DDRCTL_REG(x, stm32mp1_ddrctrl_perf)
+static const struct stm32mp_ddr_reg_desc ddr_perf[DDRCTL_REG_PERF_SIZE] = {
+ DDRCTL_REG_PERF(sched),
+ DDRCTL_REG_PERF(sched1),
+ DDRCTL_REG_PERF(perfhpr1),
+ DDRCTL_REG_PERF(perflpr1),
+ DDRCTL_REG_PERF(perfwr1),
+ DDRCTL_REG_PERF(pcfgr_0),
+ DDRCTL_REG_PERF(pcfgw_0),
+ DDRCTL_REG_PERF(pcfgqos0_0),
+ DDRCTL_REG_PERF(pcfgqos1_0),
+ DDRCTL_REG_PERF(pcfgwqos0_0),
+ DDRCTL_REG_PERF(pcfgwqos1_0),
+#if STM32MP_DDR_DUAL_AXI_PORT
+ DDRCTL_REG_PERF(pcfgr_1),
+ DDRCTL_REG_PERF(pcfgw_1),
+ DDRCTL_REG_PERF(pcfgqos0_1),
+ DDRCTL_REG_PERF(pcfgqos1_1),
+ DDRCTL_REG_PERF(pcfgwqos0_1),
+ DDRCTL_REG_PERF(pcfgwqos1_1),
+#endif
+};
+
+#define DDRPHY_REG_REG(x) DDRPHY_REG(x, stm32mp1_ddrphy_reg)
+static const struct stm32mp_ddr_reg_desc ddrphy_reg[DDRPHY_REG_REG_SIZE] = {
+ DDRPHY_REG_REG(pgcr),
+ DDRPHY_REG_REG(aciocr),
+ DDRPHY_REG_REG(dxccr),
+ DDRPHY_REG_REG(dsgcr),
+ DDRPHY_REG_REG(dcr),
+ DDRPHY_REG_REG(odtcr),
+ DDRPHY_REG_REG(zq0cr1),
+ DDRPHY_REG_REG(dx0gcr),
+ DDRPHY_REG_REG(dx1gcr),
+#if STM32MP_DDR_32BIT_INTERFACE
+ DDRPHY_REG_REG(dx2gcr),
+ DDRPHY_REG_REG(dx3gcr),
+#endif
+};
+
+#define DDRPHY_REG_TIMING(x) DDRPHY_REG(x, stm32mp1_ddrphy_timing)
+static const struct stm32mp_ddr_reg_desc ddrphy_timing[DDRPHY_REG_TIMING_SIZE] = {
+ DDRPHY_REG_TIMING(ptr0),
+ DDRPHY_REG_TIMING(ptr1),
+ DDRPHY_REG_TIMING(ptr2),
+ DDRPHY_REG_TIMING(dtpr0),
+ DDRPHY_REG_TIMING(dtpr1),
+ DDRPHY_REG_TIMING(dtpr2),
+ DDRPHY_REG_TIMING(mr0),
+ DDRPHY_REG_TIMING(mr1),
+ DDRPHY_REG_TIMING(mr2),
+ DDRPHY_REG_TIMING(mr3),
+};
+
+/*
+ * REGISTERS ARRAY: used to parse device tree and interactive mode
+ */
+static const struct stm32mp_ddr_reg_info ddr_registers[REG_TYPE_NB] = {
+ [REG_REG] = {
+ .name = "static",
+ .desc = ddr_reg,
+ .size = DDRCTL_REG_REG_SIZE,
+ .base = DDR_BASE
+ },
+ [REG_TIMING] = {
+ .name = "timing",
+ .desc = ddr_timing,
+ .size = DDRCTL_REG_TIMING_SIZE,
+ .base = DDR_BASE
+ },
+ [REG_PERF] = {
+ .name = "perf",
+ .desc = ddr_perf,
+ .size = DDRCTL_REG_PERF_SIZE,
+ .base = DDR_BASE
+ },
+ [REG_MAP] = {
+ .name = "map",
+ .desc = ddr_map,
+ .size = DDRCTL_REG_MAP_SIZE,
+ .base = DDR_BASE
+ },
+ [REGPHY_REG] = {
+ .name = "static",
+ .desc = ddrphy_reg,
+ .size = DDRPHY_REG_REG_SIZE,
+ .base = DDRPHY_BASE
+ },
+ [REGPHY_TIMING] = {
+ .name = "timing",
+ .desc = ddrphy_timing,
+ .size = DDRPHY_REG_TIMING_SIZE,
+ .base = DDRPHY_BASE
+ },
+};
+
+static void stm32mp1_ddrphy_idone_wait(struct stm32mp_ddrphy *phy)
+{
+ uint32_t pgsr;
+ int error = 0;
+ uint64_t timeout = timeout_init_us(TIMEOUT_US_1S);
+
+ do {
+ pgsr = mmio_read_32((uintptr_t)&phy->pgsr);
+
+ VERBOSE(" > [0x%lx] pgsr = 0x%x &\n",
+ (uintptr_t)&phy->pgsr, pgsr);
+
+ if (timeout_elapsed(timeout)) {
+ panic();
+ }
+
+ if ((pgsr & DDRPHYC_PGSR_DTERR) != 0U) {
+ VERBOSE("DQS Gate Trainig Error\n");
+ error++;
+ }
+
+ if ((pgsr & DDRPHYC_PGSR_DTIERR) != 0U) {
+ VERBOSE("DQS Gate Trainig Intermittent Error\n");
+ error++;
+ }
+
+ if ((pgsr & DDRPHYC_PGSR_DFTERR) != 0U) {
+ VERBOSE("DQS Drift Error\n");
+ error++;
+ }
+
+ if ((pgsr & DDRPHYC_PGSR_RVERR) != 0U) {
+ VERBOSE("Read Valid Training Error\n");
+ error++;
+ }
+
+ if ((pgsr & DDRPHYC_PGSR_RVEIRR) != 0U) {
+ VERBOSE("Read Valid Training Intermittent Error\n");
+ error++;
+ }
+ } while (((pgsr & DDRPHYC_PGSR_IDONE) == 0U) && (error == 0));
+ VERBOSE("\n[0x%lx] pgsr = 0x%x\n",
+ (uintptr_t)&phy->pgsr, pgsr);
+}
+
+static void stm32mp1_ddrphy_init(struct stm32mp_ddrphy *phy, uint32_t pir)
+{
+ uint32_t pir_init = pir | DDRPHYC_PIR_INIT;
+
+ mmio_write_32((uintptr_t)&phy->pir, pir_init);
+ VERBOSE("[0x%lx] pir = 0x%x -> 0x%x\n",
+ (uintptr_t)&phy->pir, pir_init,
+ mmio_read_32((uintptr_t)&phy->pir));
+
+ /* Need to wait 10 configuration clock before start polling */
+ udelay(10);
+
+ /* Wait DRAM initialization and Gate Training Evaluation complete */
+ stm32mp1_ddrphy_idone_wait(phy);
+}
+
+/* Wait quasi dynamic register update */
+static void stm32mp1_wait_operating_mode(struct stm32mp_ddr_priv *priv, uint32_t mode)
+{
+ uint64_t timeout;
+ uint32_t stat;
+ int break_loop = 0;
+
+ timeout = timeout_init_us(TIMEOUT_US_1S);
+ for ( ; ; ) {
+ uint32_t operating_mode;
+ uint32_t selref_type;
+
+ stat = mmio_read_32((uintptr_t)&priv->ctl->stat);
+ operating_mode = stat & DDRCTRL_STAT_OPERATING_MODE_MASK;
+ selref_type = stat & DDRCTRL_STAT_SELFREF_TYPE_MASK;
+ VERBOSE("[0x%lx] stat = 0x%x\n",
+ (uintptr_t)&priv->ctl->stat, stat);
+ if (timeout_elapsed(timeout)) {
+ panic();
+ }
+
+ if (mode == DDRCTRL_STAT_OPERATING_MODE_SR) {
+ /*
+ * Self-refresh due to software
+ * => checking also STAT.selfref_type.
+ */
+ if ((operating_mode ==
+ DDRCTRL_STAT_OPERATING_MODE_SR) &&
+ (selref_type == DDRCTRL_STAT_SELFREF_TYPE_SR)) {
+ break_loop = 1;
+ }
+ } else if (operating_mode == mode) {
+ break_loop = 1;
+ } else if ((mode == DDRCTRL_STAT_OPERATING_MODE_NORMAL) &&
+ (operating_mode == DDRCTRL_STAT_OPERATING_MODE_SR) &&
+ (selref_type == DDRCTRL_STAT_SELFREF_TYPE_ASR)) {
+ /* Normal mode: handle also automatic self refresh */
+ break_loop = 1;
+ }
+
+ if (break_loop == 1) {
+ break;
+ }
+ }
+
+ VERBOSE("[0x%lx] stat = 0x%x\n",
+ (uintptr_t)&priv->ctl->stat, stat);
+}
+
+/* Mode Register Writes (MRW or MRS) */
+static void stm32mp1_mode_register_write(struct stm32mp_ddr_priv *priv, uint8_t addr,
+ uint32_t data)
+{
+ uint32_t mrctrl0;
+
+ VERBOSE("MRS: %d = %x\n", addr, data);
+
+ /*
+ * 1. Poll MRSTAT.mr_wr_busy until it is '0'.
+ * This checks that there is no outstanding MR transaction.
+ * No write should be performed to MRCTRL0 and MRCTRL1
+ * if MRSTAT.mr_wr_busy = 1.
+ */
+ while ((mmio_read_32((uintptr_t)&priv->ctl->mrstat) &
+ DDRCTRL_MRSTAT_MR_WR_BUSY) != 0U) {
+ ;
+ }
+
+ /*
+ * 2. Write the MRCTRL0.mr_type, MRCTRL0.mr_addr, MRCTRL0.mr_rank
+ * and (for MRWs) MRCTRL1.mr_data to define the MR transaction.
+ */
+ mrctrl0 = DDRCTRL_MRCTRL0_MR_TYPE_WRITE |
+ DDRCTRL_MRCTRL0_MR_RANK_ALL |
+ (((uint32_t)addr << DDRCTRL_MRCTRL0_MR_ADDR_SHIFT) &
+ DDRCTRL_MRCTRL0_MR_ADDR_MASK);
+ mmio_write_32((uintptr_t)&priv->ctl->mrctrl0, mrctrl0);
+ VERBOSE("[0x%lx] mrctrl0 = 0x%x (0x%x)\n",
+ (uintptr_t)&priv->ctl->mrctrl0,
+ mmio_read_32((uintptr_t)&priv->ctl->mrctrl0), mrctrl0);
+ mmio_write_32((uintptr_t)&priv->ctl->mrctrl1, data);
+ VERBOSE("[0x%lx] mrctrl1 = 0x%x\n",
+ (uintptr_t)&priv->ctl->mrctrl1,
+ mmio_read_32((uintptr_t)&priv->ctl->mrctrl1));
+
+ /*
+ * 3. In a separate APB transaction, write the MRCTRL0.mr_wr to 1. This
+ * bit is self-clearing, and triggers the MR transaction.
+ * The uMCTL2 then asserts the MRSTAT.mr_wr_busy while it performs
+ * the MR transaction to SDRAM, and no further access can be
+ * initiated until it is deasserted.
+ */
+ mrctrl0 |= DDRCTRL_MRCTRL0_MR_WR;
+ mmio_write_32((uintptr_t)&priv->ctl->mrctrl0, mrctrl0);
+
+ while ((mmio_read_32((uintptr_t)&priv->ctl->mrstat) &
+ DDRCTRL_MRSTAT_MR_WR_BUSY) != 0U) {
+ ;
+ }
+
+ VERBOSE("[0x%lx] mrctrl0 = 0x%x\n",
+ (uintptr_t)&priv->ctl->mrctrl0, mrctrl0);
+}
+
+/* Switch DDR3 from DLL-on to DLL-off */
+static void stm32mp1_ddr3_dll_off(struct stm32mp_ddr_priv *priv)
+{
+ uint32_t mr1 = mmio_read_32((uintptr_t)&priv->phy->mr1);
+ uint32_t mr2 = mmio_read_32((uintptr_t)&priv->phy->mr2);
+ uint32_t dbgcam;
+
+ VERBOSE("mr1: 0x%x\n", mr1);
+ VERBOSE("mr2: 0x%x\n", mr2);
+
+ /*
+ * 1. Set the DBG1.dis_hif = 1.
+ * This prevents further reads/writes being received on the HIF.
+ */
+ mmio_setbits_32((uintptr_t)&priv->ctl->dbg1, DDRCTRL_DBG1_DIS_HIF);
+ VERBOSE("[0x%lx] dbg1 = 0x%x\n",
+ (uintptr_t)&priv->ctl->dbg1,
+ mmio_read_32((uintptr_t)&priv->ctl->dbg1));
+
+ /*
+ * 2. Ensure all commands have been flushed from the uMCTL2 by polling
+ * DBGCAM.wr_data_pipeline_empty = 1,
+ * DBGCAM.rd_data_pipeline_empty = 1,
+ * DBGCAM.dbg_wr_q_depth = 0 ,
+ * DBGCAM.dbg_lpr_q_depth = 0, and
+ * DBGCAM.dbg_hpr_q_depth = 0.
+ */
+ do {
+ dbgcam = mmio_read_32((uintptr_t)&priv->ctl->dbgcam);
+ VERBOSE("[0x%lx] dbgcam = 0x%x\n",
+ (uintptr_t)&priv->ctl->dbgcam, dbgcam);
+ } while ((((dbgcam & DDRCTRL_DBGCAM_DATA_PIPELINE_EMPTY) ==
+ DDRCTRL_DBGCAM_DATA_PIPELINE_EMPTY)) &&
+ ((dbgcam & DDRCTRL_DBGCAM_DBG_Q_DEPTH) == 0U));
+
+ /*
+ * 3. Perform an MRS command (using MRCTRL0 and MRCTRL1 registers)
+ * to disable RTT_NOM:
+ * a. DDR3: Write to MR1[9], MR1[6] and MR1[2]
+ * b. DDR4: Write to MR1[10:8]
+ */
+ mr1 &= ~(BIT(9) | BIT(6) | BIT(2));
+ stm32mp1_mode_register_write(priv, 1, mr1);
+
+ /*
+ * 4. For DDR4 only: Perform an MRS command
+ * (using MRCTRL0 and MRCTRL1 registers) to write to MR5[8:6]
+ * to disable RTT_PARK
+ */
+
+ /*
+ * 5. Perform an MRS command (using MRCTRL0 and MRCTRL1 registers)
+ * to write to MR2[10:9], to disable RTT_WR
+ * (and therefore disable dynamic ODT).
+ * This applies for both DDR3 and DDR4.
+ */
+ mr2 &= ~GENMASK(10, 9);
+ stm32mp1_mode_register_write(priv, 2, mr2);
+
+ /*
+ * 6. Perform an MRS command (using MRCTRL0 and MRCTRL1 registers)
+ * to disable the DLL. The timing of this MRS is automatically
+ * handled by the uMCTL2.
+ * a. DDR3: Write to MR1[0]
+ * b. DDR4: Write to MR1[0]
+ */
+ mr1 |= BIT(0);
+ stm32mp1_mode_register_write(priv, 1, mr1);
+
+ /*
+ * 7. Put the SDRAM into self-refresh mode by setting
+ * PWRCTL.selfref_sw = 1, and polling STAT.operating_mode to ensure
+ * the DDRC has entered self-refresh.
+ */
+ mmio_setbits_32((uintptr_t)&priv->ctl->pwrctl,
+ DDRCTRL_PWRCTL_SELFREF_SW);
+ VERBOSE("[0x%lx] pwrctl = 0x%x\n",
+ (uintptr_t)&priv->ctl->pwrctl,
+ mmio_read_32((uintptr_t)&priv->ctl->pwrctl));
+
+ /*
+ * 8. Wait until STAT.operating_mode[1:0]==11 indicating that the
+ * DWC_ddr_umctl2 core is in self-refresh mode.
+ * Ensure transition to self-refresh was due to software
+ * by checking that STAT.selfref_type[1:0]=2.
+ */
+ stm32mp1_wait_operating_mode(priv, DDRCTRL_STAT_OPERATING_MODE_SR);
+
+ /*
+ * 9. Set the MSTR.dll_off_mode = 1.
+ * warning: MSTR.dll_off_mode is a quasi-dynamic type 2 field
+ */
+ stm32mp_ddr_start_sw_done(priv->ctl);
+
+ mmio_setbits_32((uintptr_t)&priv->ctl->mstr, DDRCTRL_MSTR_DLL_OFF_MODE);
+ VERBOSE("[0x%lx] mstr = 0x%x\n",
+ (uintptr_t)&priv->ctl->mstr,
+ mmio_read_32((uintptr_t)&priv->ctl->mstr));
+
+ stm32mp_ddr_wait_sw_done_ack(priv->ctl);
+
+ /* 10. Change the clock frequency to the desired value. */
+
+ /*
+ * 11. Update any registers which may be required to change for the new
+ * frequency. This includes static and dynamic registers.
+ * This includes both uMCTL2 registers and PHY registers.
+ */
+
+ /* Change Bypass Mode Frequency Range */
+ if (clk_get_rate(DDRPHYC) < 100000000U) {
+ mmio_clrbits_32((uintptr_t)&priv->phy->dllgcr,
+ DDRPHYC_DLLGCR_BPS200);
+ } else {
+ mmio_setbits_32((uintptr_t)&priv->phy->dllgcr,
+ DDRPHYC_DLLGCR_BPS200);
+ }
+
+ mmio_setbits_32((uintptr_t)&priv->phy->acdllcr, DDRPHYC_ACDLLCR_DLLDIS);
+
+ mmio_setbits_32((uintptr_t)&priv->phy->dx0dllcr,
+ DDRPHYC_DXNDLLCR_DLLDIS);
+ mmio_setbits_32((uintptr_t)&priv->phy->dx1dllcr,
+ DDRPHYC_DXNDLLCR_DLLDIS);
+#if STM32MP_DDR_32BIT_INTERFACE
+ mmio_setbits_32((uintptr_t)&priv->phy->dx2dllcr,
+ DDRPHYC_DXNDLLCR_DLLDIS);
+ mmio_setbits_32((uintptr_t)&priv->phy->dx3dllcr,
+ DDRPHYC_DXNDLLCR_DLLDIS);
+#endif
+
+ /* 12. Exit the self-refresh state by setting PWRCTL.selfref_sw = 0. */
+ mmio_clrbits_32((uintptr_t)&priv->ctl->pwrctl,
+ DDRCTRL_PWRCTL_SELFREF_SW);
+ stm32mp1_wait_operating_mode(priv, DDRCTRL_STAT_OPERATING_MODE_NORMAL);
+
+ /*
+ * 13. If ZQCTL0.dis_srx_zqcl = 0, the uMCTL2 performs a ZQCL command
+ * at this point.
+ */
+
+ /*
+ * 14. Perform MRS commands as required to re-program timing registers
+ * in the SDRAM for the new frequency
+ * (in particular, CL, CWL and WR may need to be changed).
+ */
+
+ /* 15. Write DBG1.dis_hif = 0 to re-enable reads and writes. */
+ mmio_clrbits_32((uintptr_t)&priv->ctl->dbg1, DDRCTRL_DBG1_DIS_HIF);
+ VERBOSE("[0x%lx] dbg1 = 0x%x\n",
+ (uintptr_t)&priv->ctl->dbg1,
+ mmio_read_32((uintptr_t)&priv->ctl->dbg1));
+}
+
+static void stm32mp1_refresh_disable(struct stm32mp_ddrctl *ctl)
+{
+ stm32mp_ddr_start_sw_done(ctl);
+ /* Quasi-dynamic register update*/
+ mmio_setbits_32((uintptr_t)&ctl->rfshctl3,
+ DDRCTRL_RFSHCTL3_DIS_AUTO_REFRESH);
+ mmio_clrbits_32((uintptr_t)&ctl->pwrctl, DDRCTRL_PWRCTL_POWERDOWN_EN);
+ mmio_clrbits_32((uintptr_t)&ctl->dfimisc,
+ DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+ stm32mp_ddr_wait_sw_done_ack(ctl);
+}
+
+static void stm32mp1_refresh_restore(struct stm32mp_ddrctl *ctl,
+ uint32_t rfshctl3, uint32_t pwrctl)
+{
+ stm32mp_ddr_start_sw_done(ctl);
+ if ((rfshctl3 & DDRCTRL_RFSHCTL3_DIS_AUTO_REFRESH) == 0U) {
+ mmio_clrbits_32((uintptr_t)&ctl->rfshctl3,
+ DDRCTRL_RFSHCTL3_DIS_AUTO_REFRESH);
+ }
+ if ((pwrctl & DDRCTRL_PWRCTL_POWERDOWN_EN) != 0U) {
+ mmio_setbits_32((uintptr_t)&ctl->pwrctl,
+ DDRCTRL_PWRCTL_POWERDOWN_EN);
+ }
+ mmio_setbits_32((uintptr_t)&ctl->dfimisc,
+ DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+ stm32mp_ddr_wait_sw_done_ack(ctl);
+}
+
+void stm32mp1_ddr_init(struct stm32mp_ddr_priv *priv,
+ struct stm32mp_ddr_config *config)
+{
+ uint32_t pir;
+ int ret = -EINVAL;
+
+ if ((config->c_reg.mstr & DDRCTRL_MSTR_DDR3) != 0U) {
+ ret = stm32mp_board_ddr_power_init(STM32MP_DDR3);
+ } else if ((config->c_reg.mstr & DDRCTRL_MSTR_LPDDR2) != 0U) {
+ ret = stm32mp_board_ddr_power_init(STM32MP_LPDDR2);
+ } else if ((config->c_reg.mstr & DDRCTRL_MSTR_LPDDR3) != 0U) {
+ ret = stm32mp_board_ddr_power_init(STM32MP_LPDDR3);
+ } else {
+ ERROR("DDR type not supported\n");
+ }
+
+ if (ret != 0) {
+ panic();
+ }
+
+ VERBOSE("name = %s\n", config->info.name);
+ VERBOSE("speed = %u kHz\n", config->info.speed);
+ VERBOSE("size = 0x%x\n", config->info.size);
+
+ /* DDR INIT SEQUENCE */
+
+ /*
+ * 1. Program the DWC_ddr_umctl2 registers
+ * nota: check DFIMISC.dfi_init_complete = 0
+ */
+
+ /* 1.1 RESETS: presetn, core_ddrc_rstn, aresetn */
+ mmio_setbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DDRCAPBRST);
+ mmio_setbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DDRCAXIRST);
+ mmio_setbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DDRCORERST);
+ mmio_setbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DPHYAPBRST);
+ mmio_setbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DPHYRST);
+ mmio_setbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DPHYCTLRST);
+
+ /* 1.2. start CLOCK */
+ if (stm32mp1_ddr_clk_enable(priv, config->info.speed) != 0) {
+ panic();
+ }
+
+ /* 1.3. deassert reset */
+ /* De-assert PHY rstn and ctl_rstn via DPHYRST and DPHYCTLRST. */
+ mmio_clrbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DPHYRST);
+ mmio_clrbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DPHYCTLRST);
+ /*
+ * De-assert presetn once the clocks are active
+ * and stable via DDRCAPBRST bit.
+ */
+ mmio_clrbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DDRCAPBRST);
+
+ /* 1.4. wait 128 cycles to permit initialization of end logic */
+ udelay(2);
+ /* For PCLK = 133MHz => 1 us is enough, 2 to allow lower frequency */
+
+ /* 1.5. initialize registers ddr_umctl2 */
+ /* Stop uMCTL2 before PHY is ready */
+ mmio_clrbits_32((uintptr_t)&priv->ctl->dfimisc,
+ DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+ VERBOSE("[0x%lx] dfimisc = 0x%x\n",
+ (uintptr_t)&priv->ctl->dfimisc,
+ mmio_read_32((uintptr_t)&priv->ctl->dfimisc));
+
+ stm32mp_ddr_set_reg(priv, REG_REG, &config->c_reg, ddr_registers);
+
+ /* DDR3 = don't set DLLOFF for init mode */
+ if ((config->c_reg.mstr &
+ (DDRCTRL_MSTR_DDR3 | DDRCTRL_MSTR_DLL_OFF_MODE))
+ == (DDRCTRL_MSTR_DDR3 | DDRCTRL_MSTR_DLL_OFF_MODE)) {
+ VERBOSE("deactivate DLL OFF in mstr\n");
+ mmio_clrbits_32((uintptr_t)&priv->ctl->mstr,
+ DDRCTRL_MSTR_DLL_OFF_MODE);
+ VERBOSE("[0x%lx] mstr = 0x%x\n",
+ (uintptr_t)&priv->ctl->mstr,
+ mmio_read_32((uintptr_t)&priv->ctl->mstr));
+ }
+
+ stm32mp_ddr_set_reg(priv, REG_TIMING, &config->c_timing, ddr_registers);
+ stm32mp_ddr_set_reg(priv, REG_MAP, &config->c_map, ddr_registers);
+
+ /* Skip CTRL init, SDRAM init is done by PHY PUBL */
+ mmio_clrsetbits_32((uintptr_t)&priv->ctl->init0,
+ DDRCTRL_INIT0_SKIP_DRAM_INIT_MASK,
+ DDRCTRL_INIT0_SKIP_DRAM_INIT_NORMAL);
+ VERBOSE("[0x%lx] init0 = 0x%x\n",
+ (uintptr_t)&priv->ctl->init0,
+ mmio_read_32((uintptr_t)&priv->ctl->init0));
+
+ stm32mp_ddr_set_reg(priv, REG_PERF, &config->c_perf, ddr_registers);
+
+ /* 2. deassert reset signal core_ddrc_rstn, aresetn and presetn */
+ mmio_clrbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DDRCORERST);
+ mmio_clrbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DDRCAXIRST);
+ mmio_clrbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_DPHYAPBRST);
+
+ /*
+ * 3. start PHY init by accessing relevant PUBL registers
+ * (DXGCR, DCR, PTR*, MR*, DTPR*)
+ */
+ stm32mp_ddr_set_reg(priv, REGPHY_REG, &config->p_reg, ddr_registers);
+ stm32mp_ddr_set_reg(priv, REGPHY_TIMING, &config->p_timing, ddr_registers);
+
+ /* DDR3 = don't set DLLOFF for init mode */
+ if ((config->c_reg.mstr &
+ (DDRCTRL_MSTR_DDR3 | DDRCTRL_MSTR_DLL_OFF_MODE))
+ == (DDRCTRL_MSTR_DDR3 | DDRCTRL_MSTR_DLL_OFF_MODE)) {
+ VERBOSE("deactivate DLL OFF in mr1\n");
+ mmio_clrbits_32((uintptr_t)&priv->phy->mr1, BIT(0));
+ VERBOSE("[0x%lx] mr1 = 0x%x\n",
+ (uintptr_t)&priv->phy->mr1,
+ mmio_read_32((uintptr_t)&priv->phy->mr1));
+ }
+
+ /*
+ * 4. Monitor PHY init status by polling PUBL register PGSR.IDONE
+ * Perform DDR PHY DRAM initialization and Gate Training Evaluation
+ */
+ stm32mp1_ddrphy_idone_wait(priv->phy);
+
+ /*
+ * 5. Indicate to PUBL that controller performs SDRAM initialization
+ * by setting PIR.INIT and PIR CTLDINIT and pool PGSR.IDONE
+ * DRAM init is done by PHY, init0.skip_dram.init = 1
+ */
+
+ pir = DDRPHYC_PIR_DLLSRST | DDRPHYC_PIR_DLLLOCK | DDRPHYC_PIR_ZCAL |
+ DDRPHYC_PIR_ITMSRST | DDRPHYC_PIR_DRAMINIT | DDRPHYC_PIR_ICPC;
+
+ if ((config->c_reg.mstr & DDRCTRL_MSTR_DDR3) != 0U) {
+ pir |= DDRPHYC_PIR_DRAMRST; /* Only for DDR3 */
+ }
+
+ stm32mp1_ddrphy_init(priv->phy, pir);
+
+ /*
+ * 6. SET DFIMISC.dfi_init_complete_en to 1
+ * Enable quasi-dynamic register programming.
+ */
+ stm32mp_ddr_start_sw_done(priv->ctl);
+
+ mmio_setbits_32((uintptr_t)&priv->ctl->dfimisc,
+ DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+ VERBOSE("[0x%lx] dfimisc = 0x%x\n",
+ (uintptr_t)&priv->ctl->dfimisc,
+ mmio_read_32((uintptr_t)&priv->ctl->dfimisc));
+
+ stm32mp_ddr_wait_sw_done_ack(priv->ctl);
+
+ /*
+ * 7. Wait for DWC_ddr_umctl2 to move to normal operation mode
+ * by monitoring STAT.operating_mode signal
+ */
+
+ /* Wait uMCTL2 ready */
+ stm32mp1_wait_operating_mode(priv, DDRCTRL_STAT_OPERATING_MODE_NORMAL);
+
+ /* Switch to DLL OFF mode */
+ if ((config->c_reg.mstr & DDRCTRL_MSTR_DLL_OFF_MODE) != 0U) {
+ stm32mp1_ddr3_dll_off(priv);
+ }
+
+ VERBOSE("DDR DQS training : ");
+
+ /*
+ * 8. Disable Auto refresh and power down by setting
+ * - RFSHCTL3.dis_au_refresh = 1
+ * - PWRCTL.powerdown_en = 0
+ * - DFIMISC.dfiinit_complete_en = 0
+ */
+ stm32mp1_refresh_disable(priv->ctl);
+
+ /*
+ * 9. Program PUBL PGCR to enable refresh during training
+ * and rank to train
+ * not done => keep the programed value in PGCR
+ */
+
+ /*
+ * 10. configure PUBL PIR register to specify which training step
+ * to run
+ * RVTRN is executed only on LPDDR2/LPDDR3
+ */
+ pir = DDRPHYC_PIR_QSTRN;
+ if ((config->c_reg.mstr & DDRCTRL_MSTR_DDR3) == 0U) {
+ pir |= DDRPHYC_PIR_RVTRN;
+ }
+
+ stm32mp1_ddrphy_init(priv->phy, pir);
+
+ /* 11. monitor PUB PGSR.IDONE to poll cpmpletion of training sequence */
+ stm32mp1_ddrphy_idone_wait(priv->phy);
+
+ /*
+ * 12. set back registers in step 8 to the orginal values if desidered
+ */
+ stm32mp1_refresh_restore(priv->ctl, config->c_reg.rfshctl3,
+ config->c_reg.pwrctl);
+
+ stm32mp_ddr_enable_axi_port(priv->ctl);
+}
diff --git a/drivers/st/ddr/stm32mp1_ddr_helpers.c b/drivers/st/ddr/stm32mp1_ddr_helpers.c
new file mode 100644
index 0000000..e0621b5
--- /dev/null
+++ b/drivers/st/ddr/stm32mp1_ddr_helpers.c
@@ -0,0 +1,26 @@
+/*
+ * Copyright (c) 2017-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <drivers/st/stm32mp1_ddr_helpers.h>
+#include <lib/mmio.h>
+
+#include <platform_def.h>
+
+void ddr_enable_clock(void)
+{
+ stm32mp1_clk_rcc_regs_lock();
+
+ mmio_setbits_32(stm32mp_rcc_base() + RCC_DDRITFCR,
+ RCC_DDRITFCR_DDRC1EN |
+#if STM32MP_DDR_DUAL_AXI_PORT
+ RCC_DDRITFCR_DDRC2EN |
+#endif
+ RCC_DDRITFCR_DDRPHYCEN |
+ RCC_DDRITFCR_DDRPHYCAPBEN |
+ RCC_DDRITFCR_DDRCAPBEN);
+
+ stm32mp1_clk_rcc_regs_unlock();
+}
diff --git a/drivers/st/ddr/stm32mp1_ram.c b/drivers/st/ddr/stm32mp1_ram.c
new file mode 100644
index 0000000..b510c8f
--- /dev/null
+++ b/drivers/st/ddr/stm32mp1_ram.c
@@ -0,0 +1,151 @@
+/*
+ * Copyright (C) 2018-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+ */
+
+#include <errno.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
+#include <drivers/st/stm32mp1_ddr.h>
+#include <drivers/st/stm32mp1_ddr_helpers.h>
+#include <drivers/st/stm32mp1_ram.h>
+#include <drivers/st/stm32mp_ddr.h>
+#include <drivers/st/stm32mp_ddr_test.h>
+#include <drivers/st/stm32mp_ram.h>
+#include <lib/mmio.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+static struct stm32mp_ddr_priv ddr_priv_data;
+
+int stm32mp1_ddr_clk_enable(struct stm32mp_ddr_priv *priv, uint32_t mem_speed)
+{
+ unsigned long ddrphy_clk, ddr_clk, mem_speed_hz;
+
+ ddr_enable_clock();
+
+ ddrphy_clk = clk_get_rate(DDRPHYC);
+
+ VERBOSE("DDR: mem_speed (%u kHz), RCC %lu kHz\n",
+ mem_speed, ddrphy_clk / 1000U);
+
+ mem_speed_hz = mem_speed * 1000U;
+
+ /* Max 10% frequency delta */
+ if (ddrphy_clk > mem_speed_hz) {
+ ddr_clk = ddrphy_clk - mem_speed_hz;
+ } else {
+ ddr_clk = mem_speed_hz - ddrphy_clk;
+ }
+ if (ddr_clk > (mem_speed_hz / 10)) {
+ ERROR("DDR expected freq %u kHz, current is %lu kHz\n",
+ mem_speed, ddrphy_clk / 1000U);
+ return -1;
+ }
+ return 0;
+}
+
+static int stm32mp1_ddr_setup(void)
+{
+ struct stm32mp_ddr_priv *priv = &ddr_priv_data;
+ int ret;
+ struct stm32mp_ddr_config config;
+ int node;
+ uint32_t uret;
+ void *fdt;
+
+ const struct stm32mp_ddr_param param[] = {
+ CTL_PARAM(reg),
+ CTL_PARAM(timing),
+ CTL_PARAM(map),
+ CTL_PARAM(perf),
+ PHY_PARAM(reg),
+ PHY_PARAM(timing),
+ };
+
+ if (fdt_get_address(&fdt) == 0) {
+ return -ENOENT;
+ }
+
+ node = fdt_node_offset_by_compatible(fdt, -1, DT_DDR_COMPAT);
+ if (node < 0) {
+ ERROR("%s: Cannot read DDR node in DT\n", __func__);
+ return -EINVAL;
+ }
+
+ ret = stm32mp_ddr_dt_get_info(fdt, node, &config.info);
+ if (ret < 0) {
+ return ret;
+ }
+
+ ret = stm32mp_ddr_dt_get_param(fdt, node, param, ARRAY_SIZE(param), (uintptr_t)&config);
+ if (ret < 0) {
+ return ret;
+ }
+
+ /* Disable axidcg clock gating during init */
+ mmio_clrbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_AXIDCGEN);
+
+ stm32mp1_ddr_init(priv, &config);
+
+ /* Enable axidcg clock gating */
+ mmio_setbits_32(priv->rcc + RCC_DDRITFCR, RCC_DDRITFCR_AXIDCGEN);
+
+ priv->info.size = config.info.size;
+
+ VERBOSE("%s : ram size(%x, %x)\n", __func__,
+ (uint32_t)priv->info.base, (uint32_t)priv->info.size);
+
+ if (stm32mp_map_ddr_non_cacheable() != 0) {
+ panic();
+ }
+
+ uret = stm32mp_ddr_test_data_bus();
+ if (uret != 0U) {
+ ERROR("DDR data bus test: can't access memory @ 0x%x\n",
+ uret);
+ panic();
+ }
+
+ uret = stm32mp_ddr_test_addr_bus(config.info.size);
+ if (uret != 0U) {
+ ERROR("DDR addr bus test: can't access memory @ 0x%x\n",
+ uret);
+ panic();
+ }
+
+ uret = stm32mp_ddr_check_size();
+ if (uret < config.info.size) {
+ ERROR("DDR size: 0x%x does not match DT config: 0x%x\n",
+ uret, config.info.size);
+ panic();
+ }
+
+ if (stm32mp_unmap_ddr() != 0) {
+ panic();
+ }
+
+ return 0;
+}
+
+int stm32mp1_ddr_probe(void)
+{
+ struct stm32mp_ddr_priv *priv = &ddr_priv_data;
+
+ VERBOSE("STM32MP DDR probe\n");
+
+ priv->ctl = (struct stm32mp_ddrctl *)stm32mp_ddrctrl_base();
+ priv->phy = (struct stm32mp_ddrphy *)stm32mp_ddrphyc_base();
+ priv->pwr = stm32mp_pwr_base();
+ priv->rcc = stm32mp_rcc_base();
+
+ priv->info.base = STM32MP_DDR_BASE;
+ priv->info.size = 0;
+
+ return stm32mp1_ddr_setup();
+}
diff --git a/drivers/st/ddr/stm32mp_ddr.c b/drivers/st/ddr/stm32mp_ddr.c
new file mode 100644
index 0000000..6776e3b
--- /dev/null
+++ b/drivers/st/ddr/stm32mp_ddr.c
@@ -0,0 +1,106 @@
+/*
+ * Copyright (C) 2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32mp_ddr.h>
+#include <drivers/st/stm32mp_ddrctrl_regs.h>
+#include <drivers/st/stm32mp_pmic.h>
+#include <lib/mmio.h>
+
+#include <platform_def.h>
+
+#define INVALID_OFFSET 0xFFU
+
+static uintptr_t get_base_addr(const struct stm32mp_ddr_priv *priv, enum stm32mp_ddr_base_type base)
+{
+ if (base == DDRPHY_BASE) {
+ return (uintptr_t)priv->phy;
+ } else {
+ return (uintptr_t)priv->ctl;
+ }
+}
+
+void stm32mp_ddr_set_reg(const struct stm32mp_ddr_priv *priv, enum stm32mp_ddr_reg_type type,
+ const void *param, const struct stm32mp_ddr_reg_info *ddr_registers)
+{
+ unsigned int i;
+ unsigned int value;
+ enum stm32mp_ddr_base_type base = ddr_registers[type].base;
+ uintptr_t base_addr = get_base_addr(priv, base);
+ const struct stm32mp_ddr_reg_desc *desc = ddr_registers[type].desc;
+
+ VERBOSE("init %s\n", ddr_registers[type].name);
+ for (i = 0; i < ddr_registers[type].size; i++) {
+ uintptr_t ptr = base_addr + desc[i].offset;
+
+ if (desc[i].par_offset == INVALID_OFFSET) {
+ ERROR("invalid parameter offset for %s", desc[i].name);
+ panic();
+ } else {
+ value = *((uint32_t *)((uintptr_t)param +
+ desc[i].par_offset));
+ mmio_write_32(ptr, value);
+ }
+ }
+}
+
+/* Start quasi dynamic register update */
+void stm32mp_ddr_start_sw_done(struct stm32mp_ddrctl *ctl)
+{
+ mmio_clrbits_32((uintptr_t)&ctl->swctl, DDRCTRL_SWCTL_SW_DONE);
+ VERBOSE("[0x%lx] swctl = 0x%x\n",
+ (uintptr_t)&ctl->swctl, mmio_read_32((uintptr_t)&ctl->swctl));
+}
+
+/* Wait quasi dynamic register update */
+void stm32mp_ddr_wait_sw_done_ack(struct stm32mp_ddrctl *ctl)
+{
+ uint64_t timeout;
+ uint32_t swstat;
+
+ mmio_setbits_32((uintptr_t)&ctl->swctl, DDRCTRL_SWCTL_SW_DONE);
+ VERBOSE("[0x%lx] swctl = 0x%x\n",
+ (uintptr_t)&ctl->swctl, mmio_read_32((uintptr_t)&ctl->swctl));
+
+ timeout = timeout_init_us(TIMEOUT_US_1S);
+ do {
+ swstat = mmio_read_32((uintptr_t)&ctl->swstat);
+ VERBOSE("[0x%lx] swstat = 0x%x ",
+ (uintptr_t)&ctl->swstat, swstat);
+ if (timeout_elapsed(timeout)) {
+ panic();
+ }
+ } while ((swstat & DDRCTRL_SWSTAT_SW_DONE_ACK) == 0U);
+
+ VERBOSE("[0x%lx] swstat = 0x%x\n",
+ (uintptr_t)&ctl->swstat, swstat);
+}
+
+void stm32mp_ddr_enable_axi_port(struct stm32mp_ddrctl *ctl)
+{
+ /* Enable uMCTL2 AXI port 0 */
+ mmio_setbits_32((uintptr_t)&ctl->pctrl_0, DDRCTRL_PCTRL_N_PORT_EN);
+ VERBOSE("[0x%lx] pctrl_0 = 0x%x\n", (uintptr_t)&ctl->pctrl_0,
+ mmio_read_32((uintptr_t)&ctl->pctrl_0));
+
+#if STM32MP_DDR_DUAL_AXI_PORT
+ /* Enable uMCTL2 AXI port 1 */
+ mmio_setbits_32((uintptr_t)&ctl->pctrl_1, DDRCTRL_PCTRL_N_PORT_EN);
+ VERBOSE("[0x%lx] pctrl_1 = 0x%x\n", (uintptr_t)&ctl->pctrl_1,
+ mmio_read_32((uintptr_t)&ctl->pctrl_1));
+#endif
+
+}
+
+int stm32mp_board_ddr_power_init(enum ddr_type ddr_type)
+{
+ if (dt_pmic_status() > 0) {
+ return pmic_ddr_power_init(ddr_type);
+ }
+
+ return 0;
+}
diff --git a/drivers/st/ddr/stm32mp_ddr_test.c b/drivers/st/ddr/stm32mp_ddr_test.c
new file mode 100644
index 0000000..6733cc6
--- /dev/null
+++ b/drivers/st/ddr/stm32mp_ddr_test.c
@@ -0,0 +1,148 @@
+/*
+ * Copyright (C) 2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <drivers/st/stm32mp_ddr_test.h>
+#include <lib/mmio.h>
+
+#include <platform_def.h>
+
+#define DDR_PATTERN 0xAAAAAAAAU
+#define DDR_ANTIPATTERN 0x55555555U
+
+/*******************************************************************************
+ * This function tests a simple read/write access to the DDR.
+ * Note that the previous content is restored after test.
+ * Returns 0 if success, and address value else.
+ ******************************************************************************/
+uint32_t stm32mp_ddr_test_rw_access(void)
+{
+ uint32_t saved_value = mmio_read_32(STM32MP_DDR_BASE);
+
+ mmio_write_32(STM32MP_DDR_BASE, DDR_PATTERN);
+
+ if (mmio_read_32(STM32MP_DDR_BASE) != DDR_PATTERN) {
+ return (uint32_t)STM32MP_DDR_BASE;
+ }
+
+ mmio_write_32(STM32MP_DDR_BASE, saved_value);
+
+ return 0U;
+}
+
+/*******************************************************************************
+ * This function tests the DDR data bus wiring.
+ * This is inspired from the Data Bus Test algorithm written by Michael Barr
+ * in "Programming Embedded Systems in C and C++" book.
+ * resources.oreilly.com/examples/9781565923546/blob/master/Chapter6/
+ * File: memtest.c - This source code belongs to Public Domain.
+ * Returns 0 if success, and address value else.
+ ******************************************************************************/
+uint32_t stm32mp_ddr_test_data_bus(void)
+{
+ uint32_t pattern;
+
+ for (pattern = 1U; pattern != 0U; pattern <<= 1U) {
+ mmio_write_32(STM32MP_DDR_BASE, pattern);
+
+ if (mmio_read_32(STM32MP_DDR_BASE) != pattern) {
+ return (uint32_t)STM32MP_DDR_BASE;
+ }
+ }
+
+ return 0;
+}
+
+/*******************************************************************************
+ * This function tests the DDR address bus wiring.
+ * This is inspired from the Data Bus Test algorithm written by Michael Barr
+ * in "Programming Embedded Systems in C and C++" book.
+ * resources.oreilly.com/examples/9781565923546/blob/master/Chapter6/
+ * File: memtest.c - This source code belongs to Public Domain.
+ * size: size in bytes of the DDR memory device.
+ * Returns 0 if success, and address value else.
+ ******************************************************************************/
+uint32_t stm32mp_ddr_test_addr_bus(uint64_t size)
+{
+ uint64_t addressmask = size - 1U;
+ uint64_t offset;
+ uint64_t testoffset = 0U;
+
+ /* Write the default pattern at each of the power-of-two offsets. */
+ for (offset = sizeof(uint32_t); (offset & addressmask) != 0U;
+ offset <<= 1U) {
+ mmio_write_32(STM32MP_DDR_BASE + (uint32_t)offset,
+ DDR_PATTERN);
+ }
+
+ /* Check for address bits stuck high. */
+ mmio_write_32(STM32MP_DDR_BASE + (uint32_t)testoffset,
+ DDR_ANTIPATTERN);
+
+ for (offset = sizeof(uint32_t); (offset & addressmask) != 0U;
+ offset <<= 1U) {
+ if (mmio_read_32(STM32MP_DDR_BASE + (uint32_t)offset) !=
+ DDR_PATTERN) {
+ return (uint32_t)(STM32MP_DDR_BASE + offset);
+ }
+ }
+
+ mmio_write_32(STM32MP_DDR_BASE + (uint32_t)testoffset, DDR_PATTERN);
+
+ /* Check for address bits stuck low or shorted. */
+ for (testoffset = sizeof(uint32_t); (testoffset & addressmask) != 0U;
+ testoffset <<= 1U) {
+ mmio_write_32(STM32MP_DDR_BASE + (uint32_t)testoffset,
+ DDR_ANTIPATTERN);
+
+ if (mmio_read_32(STM32MP_DDR_BASE) != DDR_PATTERN) {
+ return STM32MP_DDR_BASE;
+ }
+
+ for (offset = sizeof(uint32_t); (offset & addressmask) != 0U;
+ offset <<= 1) {
+ if ((mmio_read_32(STM32MP_DDR_BASE +
+ (uint32_t)offset) != DDR_PATTERN) &&
+ (offset != testoffset)) {
+ return (uint32_t)(STM32MP_DDR_BASE + offset);
+ }
+ }
+
+ mmio_write_32(STM32MP_DDR_BASE + (uint32_t)testoffset,
+ DDR_PATTERN);
+ }
+
+ return 0U;
+}
+
+/*******************************************************************************
+ * This function checks the DDR size. It has to be run with Data Cache off.
+ * This test is run before data have been put in DDR, and is only done for
+ * cold boot. The DDR data can then be overwritten, and it is not useful to
+ * restore its content.
+ * Returns DDR computed size.
+ ******************************************************************************/
+uint32_t stm32mp_ddr_check_size(void)
+{
+ uint32_t offset = sizeof(uint32_t);
+
+ mmio_write_32(STM32MP_DDR_BASE, DDR_PATTERN);
+
+ while (offset < STM32MP_DDR_MAX_SIZE) {
+ mmio_write_32(STM32MP_DDR_BASE + offset, DDR_ANTIPATTERN);
+ dsb();
+
+ if (mmio_read_32(STM32MP_DDR_BASE) != DDR_PATTERN) {
+ break;
+ }
+
+ offset <<= 1U;
+ }
+
+ INFO("Memory size = 0x%x (%u MB)\n", offset, offset / (1024U * 1024U));
+
+ return offset;
+}
diff --git a/drivers/st/ddr/stm32mp_ram.c b/drivers/st/ddr/stm32mp_ram.c
new file mode 100644
index 0000000..0804568
--- /dev/null
+++ b/drivers/st/ddr/stm32mp_ram.c
@@ -0,0 +1,60 @@
+/*
+ * Copyright (C) 2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+#include <stdbool.h>
+
+#include <common/debug.h>
+#include <common/fdt_wrappers.h>
+#include <drivers/st/stm32mp_ram.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+int stm32mp_ddr_dt_get_info(void *fdt, int node, struct stm32mp_ddr_info *info)
+{
+ int ret;
+
+ ret = fdt_read_uint32(fdt, node, "st,mem-speed", &info->speed);
+ if (ret < 0) {
+ VERBOSE("%s: no st,mem-speed\n", __func__);
+ return -EINVAL;
+ }
+ ret = fdt_read_uint32(fdt, node, "st,mem-size", &info->size);
+ if (ret < 0) {
+ VERBOSE("%s: no st,mem-size\n", __func__);
+ return -EINVAL;
+ }
+ info->name = fdt_getprop(fdt, node, "st,mem-name", NULL);
+ if (info->name == NULL) {
+ VERBOSE("%s: no st,mem-name\n", __func__);
+ return -EINVAL;
+ }
+
+ INFO("RAM: %s\n", info->name);
+
+ return 0;
+}
+
+int stm32mp_ddr_dt_get_param(void *fdt, int node, const struct stm32mp_ddr_param *param,
+ uint32_t param_size, uintptr_t config)
+{
+ int ret;
+ uint32_t idx;
+
+ for (idx = 0U; idx < param_size; idx++) {
+ ret = fdt_read_uint32_array(fdt, node, param[idx].name, param[idx].size,
+ (void *)(config + param[idx].offset));
+
+ VERBOSE("%s: %s[0x%x] = %d\n", __func__, param[idx].name, param[idx].size, ret);
+ if (ret != 0) {
+ ERROR("%s: Cannot read %s, error=%d\n", __func__, param[idx].name, ret);
+ return -EINVAL;
+ }
+ }
+
+ return 0;
+}
diff --git a/drivers/st/etzpc/etzpc.c b/drivers/st/etzpc/etzpc.c
new file mode 100644
index 0000000..4c3c26d
--- /dev/null
+++ b/drivers/st/etzpc/etzpc.c
@@ -0,0 +1,246 @@
+/*
+ * Copyright (c) 2017-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdint.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/st/etzpc.h>
+#include <dt-bindings/soc/st,stm32-etzpc.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+/* Device Tree related definitions */
+#define ETZPC_COMPAT "st,stm32-etzpc"
+#define ETZPC_LOCK_MASK 0x1U
+#define ETZPC_MODE_SHIFT 8
+#define ETZPC_MODE_MASK GENMASK(1, 0)
+#define ETZPC_ID_SHIFT 16
+#define ETZPC_ID_MASK GENMASK(7, 0)
+
+/* ID Registers */
+#define ETZPC_TZMA0_SIZE 0x000U
+#define ETZPC_DECPROT0 0x010U
+#define ETZPC_DECPROT_LOCK0 0x030U
+#define ETZPC_HWCFGR 0x3F0U
+#define ETZPC_VERR 0x3F4U
+
+/* ID Registers fields */
+#define ETZPC_TZMA0_SIZE_LOCK BIT(31)
+#define ETZPC_DECPROT0_MASK GENMASK(1, 0)
+#define ETZPC_HWCFGR_NUM_TZMA_SHIFT 0
+#define ETZPC_HWCFGR_NUM_PER_SEC_SHIFT 8
+#define ETZPC_HWCFGR_NUM_AHB_SEC_SHIFT 16
+#define ETZPC_HWCFGR_CHUNCKS1N4_SHIFT 24
+
+#define DECPROT_SHIFT 1
+#define IDS_PER_DECPROT_REGS 16U
+#define IDS_PER_DECPROT_LOCK_REGS 32U
+
+/*
+ * etzpc_instance.
+ * base : register base address set during init given by user
+ * chunk_size : supported TZMA size steps
+ * num_tzma: number of TZMA zone read from register at init
+ * num_ahb_sec : number of securable AHB master zone read from register
+ * num_per_sec : number of securable AHB & APB Peripherals read from register
+ * revision : IP revision read from register at init
+ */
+struct etzpc_instance {
+ uintptr_t base;
+ uint8_t chunck_size;
+ uint8_t num_tzma;
+ uint8_t num_per_sec;
+ uint8_t num_ahb_sec;
+ uint8_t revision;
+};
+
+/* Only 1 instance of the ETZPC is expected per platform */
+static struct etzpc_instance etzpc_dev;
+
+/*
+ * Implementation uses uint8_t to store each securable DECPROT configuration.
+ * When resuming from deep suspend, the DECPROT configurations are restored.
+ */
+#define PERIPH_LOCK_BIT BIT(7)
+#define PERIPH_ATTR_MASK GENMASK(2, 0)
+
+#if ENABLE_ASSERTIONS
+static bool valid_decprot_id(unsigned int id)
+{
+ return id < (unsigned int)etzpc_dev.num_per_sec;
+}
+
+static bool valid_tzma_id(unsigned int id)
+{
+ return id < (unsigned int)etzpc_dev.num_tzma;
+}
+#endif
+
+/*
+ * etzpc_configure_decprot : Load a DECPROT configuration
+ * decprot_id : ID of the IP
+ * decprot_attr : Restriction access attribute
+ */
+void etzpc_configure_decprot(uint32_t decprot_id,
+ enum etzpc_decprot_attributes decprot_attr)
+{
+ uintptr_t offset = 4U * (decprot_id / IDS_PER_DECPROT_REGS);
+ uint32_t shift = (decprot_id % IDS_PER_DECPROT_REGS) << DECPROT_SHIFT;
+ uint32_t masked_decprot = (uint32_t)decprot_attr & ETZPC_DECPROT0_MASK;
+
+ assert(valid_decprot_id(decprot_id));
+
+ mmio_clrsetbits_32(etzpc_dev.base + ETZPC_DECPROT0 + offset,
+ (uint32_t)ETZPC_DECPROT0_MASK << shift,
+ masked_decprot << shift);
+}
+
+/*
+ * etzpc_get_decprot : Get the DECPROT attribute
+ * decprot_id : ID of the IP
+ * return : Attribute of this DECPROT
+ */
+enum etzpc_decprot_attributes etzpc_get_decprot(uint32_t decprot_id)
+{
+ uintptr_t offset = 4U * (decprot_id / IDS_PER_DECPROT_REGS);
+ uint32_t shift = (decprot_id % IDS_PER_DECPROT_REGS) << DECPROT_SHIFT;
+ uintptr_t base_decprot = etzpc_dev.base + offset;
+ uint32_t value;
+
+ assert(valid_decprot_id(decprot_id));
+
+ value = (mmio_read_32(base_decprot + ETZPC_DECPROT0) >> shift) &
+ ETZPC_DECPROT0_MASK;
+
+ return (enum etzpc_decprot_attributes)value;
+}
+
+/*
+ * etzpc_lock_decprot : Lock access to the DECPROT attribute
+ * decprot_id : ID of the IP
+ */
+void etzpc_lock_decprot(uint32_t decprot_id)
+{
+ uintptr_t offset = 4U * (decprot_id / IDS_PER_DECPROT_LOCK_REGS);
+ uint32_t shift = BIT(decprot_id % IDS_PER_DECPROT_LOCK_REGS);
+ uintptr_t base_decprot = etzpc_dev.base + offset;
+
+ assert(valid_decprot_id(decprot_id));
+
+ mmio_write_32(base_decprot + ETZPC_DECPROT_LOCK0, shift);
+}
+
+/*
+ * etzpc_configure_tzma : Configure the target TZMA read only size
+ * tzma_id : ID of the memory
+ * tzma_value : read-only size
+ */
+void etzpc_configure_tzma(uint32_t tzma_id, uint16_t tzma_value)
+{
+ assert(valid_tzma_id(tzma_id));
+
+ mmio_write_32(etzpc_dev.base + ETZPC_TZMA0_SIZE +
+ (sizeof(uint32_t) * tzma_id), tzma_value);
+}
+
+/*
+ * etzpc_get_tzma : Get the target TZMA read only size
+ * tzma_id : TZMA ID
+ * return : Size of read only size
+ */
+uint16_t etzpc_get_tzma(uint32_t tzma_id)
+{
+ assert(valid_tzma_id(tzma_id));
+
+ return (uint16_t)mmio_read_32(etzpc_dev.base + ETZPC_TZMA0_SIZE +
+ (sizeof(uint32_t) * tzma_id));
+}
+
+/*
+ * etzpc_lock_tzma : Lock the target TZMA
+ * tzma_id : TZMA ID
+ */
+void etzpc_lock_tzma(uint32_t tzma_id)
+{
+ assert(valid_tzma_id(tzma_id));
+
+ mmio_setbits_32(etzpc_dev.base + ETZPC_TZMA0_SIZE +
+ (sizeof(uint32_t) * tzma_id), ETZPC_TZMA0_SIZE_LOCK);
+}
+
+/*
+ * etzpc_get_lock_tzma : Return the lock status of the target TZMA
+ * tzma_id : TZMA ID
+ * return : True if TZMA is locked, false otherwise
+ */
+bool etzpc_get_lock_tzma(uint32_t tzma_id)
+{
+ uint32_t tzma_size;
+
+ assert(valid_tzma_id(tzma_id));
+
+ tzma_size = mmio_read_32(etzpc_dev.base + ETZPC_TZMA0_SIZE +
+ (sizeof(uint32_t) * tzma_id));
+
+ return (tzma_size & ETZPC_TZMA0_SIZE_LOCK) != 0;
+}
+
+/*
+ * etzpc_get_num_per_sec : Return the DECPROT ID limit value
+ */
+uint8_t etzpc_get_num_per_sec(void)
+{
+ return etzpc_dev.num_per_sec;
+}
+
+/*
+ * etzpc_get_revision : Return the ETZPC IP revision
+ */
+uint8_t etzpc_get_revision(void)
+{
+ return etzpc_dev.revision;
+}
+
+/*
+ * etzpc_get_base_address : Return the ETZPC IP base address
+ */
+uintptr_t etzpc_get_base_address(void)
+{
+ return etzpc_dev.base;
+}
+
+/*
+ * etzpc_init : Initialize the ETZPC driver
+ * Return 0 on success and a negative errno on failure
+ */
+int etzpc_init(void)
+{
+ uint32_t hwcfg;
+
+ etzpc_dev.base = STM32MP1_ETZPC_BASE;
+
+ hwcfg = mmio_read_32(etzpc_dev.base + ETZPC_HWCFGR);
+
+ etzpc_dev.num_tzma = (uint8_t)(hwcfg >> ETZPC_HWCFGR_NUM_TZMA_SHIFT);
+ etzpc_dev.num_per_sec = (uint8_t)(hwcfg >>
+ ETZPC_HWCFGR_NUM_PER_SEC_SHIFT);
+ etzpc_dev.num_ahb_sec = (uint8_t)(hwcfg >>
+ ETZPC_HWCFGR_NUM_AHB_SEC_SHIFT);
+ etzpc_dev.chunck_size = (uint8_t)(hwcfg >>
+ ETZPC_HWCFGR_CHUNCKS1N4_SHIFT);
+
+ etzpc_dev.revision = mmio_read_8(etzpc_dev.base + ETZPC_VERR);
+
+ VERBOSE("ETZPC version 0x%x", etzpc_dev.revision);
+
+ return 0;
+}
diff --git a/drivers/st/fmc/stm32_fmc2_nand.c b/drivers/st/fmc/stm32_fmc2_nand.c
new file mode 100644
index 0000000..9bdc854
--- /dev/null
+++ b/drivers/st/fmc/stm32_fmc2_nand.c
@@ -0,0 +1,934 @@
+/*
+ * Copyright (c) 2019-2022, 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 <common/debug.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/raw_nand.h>
+#include <drivers/st/stm32_fmc2_nand.h>
+#include <drivers/st/stm32_gpio.h>
+#include <drivers/st/stm32mp_reset.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+/* Timeout for device interface reset */
+#define TIMEOUT_US_1_MS 1000U
+
+/* FMC2 Compatibility */
+#define DT_FMC2_EBI_COMPAT "st,stm32mp1-fmc2-ebi"
+#define DT_FMC2_NFC_COMPAT "st,stm32mp1-fmc2-nfc"
+#define MAX_CS 2U
+#define MAX_BANK 5U
+
+/* FMC2 Controller Registers */
+#define FMC2_BCR1 0x00U
+#define FMC2_PCR 0x80U
+#define FMC2_SR 0x84U
+#define FMC2_PMEM 0x88U
+#define FMC2_PATT 0x8CU
+#define FMC2_HECCR 0x94U
+#define FMC2_BCHISR 0x254U
+#define FMC2_BCHICR 0x258U
+#define FMC2_BCHDSR0 0x27CU
+#define FMC2_BCHDSR1 0x280U
+#define FMC2_BCHDSR2 0x284U
+#define FMC2_BCHDSR3 0x288U
+#define FMC2_BCHDSR4 0x28CU
+
+/* FMC2_BCR1 register */
+#define FMC2_BCR1_FMC2EN BIT(31)
+/* FMC2_PCR register */
+#define FMC2_PCR_PWAITEN BIT(1)
+#define FMC2_PCR_PBKEN BIT(2)
+#define FMC2_PCR_PWID_MASK GENMASK_32(5, 4)
+#define FMC2_PCR_PWID(x) (((x) << 4) & FMC2_PCR_PWID_MASK)
+#define FMC2_PCR_PWID_8 0x0U
+#define FMC2_PCR_PWID_16 0x1U
+#define FMC2_PCR_ECCEN BIT(6)
+#define FMC2_PCR_ECCALG BIT(8)
+#define FMC2_PCR_TCLR_MASK GENMASK_32(12, 9)
+#define FMC2_PCR_TCLR(x) (((x) << 9) & FMC2_PCR_TCLR_MASK)
+#define FMC2_PCR_TCLR_DEFAULT 0xFU
+#define FMC2_PCR_TAR_MASK GENMASK_32(16, 13)
+#define FMC2_PCR_TAR(x) (((x) << 13) & FMC2_PCR_TAR_MASK)
+#define FMC2_PCR_TAR_DEFAULT 0xFU
+#define FMC2_PCR_ECCSS_MASK GENMASK_32(19, 17)
+#define FMC2_PCR_ECCSS(x) (((x) << 17) & FMC2_PCR_ECCSS_MASK)
+#define FMC2_PCR_ECCSS_512 0x1U
+#define FMC2_PCR_ECCSS_2048 0x3U
+#define FMC2_PCR_BCHECC BIT(24)
+#define FMC2_PCR_WEN BIT(25)
+/* FMC2_SR register */
+#define FMC2_SR_NWRF BIT(6)
+/* FMC2_PMEM register*/
+#define FMC2_PMEM_MEMSET(x) (((x) & GENMASK_32(7, 0)) << 0)
+#define FMC2_PMEM_MEMWAIT(x) (((x) & GENMASK_32(7, 0)) << 8)
+#define FMC2_PMEM_MEMHOLD(x) (((x) & GENMASK_32(7, 0)) << 16)
+#define FMC2_PMEM_MEMHIZ(x) (((x) & GENMASK_32(7, 0)) << 24)
+#define FMC2_PMEM_DEFAULT 0x0A0A0A0AU
+/* FMC2_PATT register */
+#define FMC2_PATT_ATTSET(x) (((x) & GENMASK_32(7, 0)) << 0)
+#define FMC2_PATT_ATTWAIT(x) (((x) & GENMASK_32(7, 0)) << 8)
+#define FMC2_PATT_ATTHOLD(x) (((x) & GENMASK_32(7, 0)) << 16)
+#define FMC2_PATT_ATTHIZ(x) (((x) & GENMASK_32(7, 0)) << 24)
+#define FMC2_PATT_DEFAULT 0x0A0A0A0AU
+/* FMC2_BCHISR register */
+#define FMC2_BCHISR_DERF BIT(1)
+/* FMC2_BCHICR register */
+#define FMC2_BCHICR_CLEAR_IRQ GENMASK_32(4, 0)
+/* FMC2_BCHDSR0 register */
+#define FMC2_BCHDSR0_DUE BIT(0)
+#define FMC2_BCHDSR0_DEF BIT(1)
+#define FMC2_BCHDSR0_DEN_MASK GENMASK_32(7, 4)
+#define FMC2_BCHDSR0_DEN_SHIFT 4U
+/* FMC2_BCHDSR1 register */
+#define FMC2_BCHDSR1_EBP1_MASK GENMASK_32(12, 0)
+#define FMC2_BCHDSR1_EBP2_MASK GENMASK_32(28, 16)
+#define FMC2_BCHDSR1_EBP2_SHIFT 16U
+/* FMC2_BCHDSR2 register */
+#define FMC2_BCHDSR2_EBP3_MASK GENMASK_32(12, 0)
+#define FMC2_BCHDSR2_EBP4_MASK GENMASK_32(28, 16)
+#define FMC2_BCHDSR2_EBP4_SHIFT 16U
+/* FMC2_BCHDSR3 register */
+#define FMC2_BCHDSR3_EBP5_MASK GENMASK_32(12, 0)
+#define FMC2_BCHDSR3_EBP6_MASK GENMASK_32(28, 16)
+#define FMC2_BCHDSR3_EBP6_SHIFT 16U
+/* FMC2_BCHDSR4 register */
+#define FMC2_BCHDSR4_EBP7_MASK GENMASK_32(12, 0)
+#define FMC2_BCHDSR4_EBP8_MASK GENMASK_32(28, 16)
+#define FMC2_BCHDSR4_EBP8_SHIFT 16U
+
+/* Timings */
+#define FMC2_THIZ 0x01U
+#define FMC2_TIO 8000U
+#define FMC2_TSYNC 3000U
+#define FMC2_PCR_TIMING_MASK GENMASK_32(3, 0)
+#define FMC2_PMEM_PATT_TIMING_MASK GENMASK_32(7, 0)
+
+#define FMC2_BBM_LEN 2U
+#define FMC2_MAX_ECC_BYTES 14U
+#define TIMEOUT_US_10_MS 10000U
+#define FMC2_PSEC_PER_MSEC (1000UL * 1000UL * 1000UL)
+
+enum stm32_fmc2_ecc {
+ FMC2_ECC_HAM = 1U,
+ FMC2_ECC_BCH4 = 4U,
+ FMC2_ECC_BCH8 = 8U
+};
+
+struct stm32_fmc2_cs_reg {
+ uintptr_t data_base;
+ uintptr_t cmd_base;
+ uintptr_t addr_base;
+};
+
+struct stm32_fmc2_nand_timings {
+ uint8_t tclr;
+ uint8_t tar;
+ uint8_t thiz;
+ uint8_t twait;
+ uint8_t thold_mem;
+ uint8_t tset_mem;
+ uint8_t thold_att;
+ uint8_t tset_att;
+};
+
+struct stm32_fmc2_nfc {
+ uintptr_t reg_base;
+ struct stm32_fmc2_cs_reg cs[MAX_CS];
+ unsigned long clock_id;
+ unsigned int reset_id;
+ uint8_t cs_sel;
+};
+
+static struct stm32_fmc2_nfc stm32_fmc2;
+
+static uintptr_t fmc2_base(void)
+{
+ return stm32_fmc2.reg_base;
+}
+
+static void stm32_fmc2_nand_setup_timing(void)
+{
+ struct stm32_fmc2_nand_timings tims;
+ unsigned long hclk = clk_get_rate(stm32_fmc2.clock_id);
+ unsigned long hclkp = FMC2_PSEC_PER_MSEC / (hclk / 1000U);
+ unsigned long timing, tar, tclr, thiz, twait;
+ unsigned long tset_mem, tset_att, thold_mem, thold_att;
+ uint32_t pcr, pmem, patt;
+
+ tar = MAX(hclkp, NAND_TAR_MIN);
+ timing = div_round_up(tar, hclkp) - 1U;
+ tims.tar = MIN(timing, (unsigned long)FMC2_PCR_TIMING_MASK);
+
+ tclr = MAX(hclkp, NAND_TCLR_MIN);
+ timing = div_round_up(tclr, hclkp) - 1U;
+ tims.tclr = MIN(timing, (unsigned long)FMC2_PCR_TIMING_MASK);
+
+ tims.thiz = FMC2_THIZ;
+ thiz = (tims.thiz + 1U) * hclkp;
+
+ /*
+ * tWAIT > tRP
+ * tWAIT > tWP
+ * tWAIT > tREA + tIO
+ */
+ twait = MAX(hclkp, NAND_TRP_MIN);
+ twait = MAX(twait, NAND_TWP_MIN);
+ twait = MAX(twait, NAND_TREA_MAX + FMC2_TIO);
+ timing = div_round_up(twait, hclkp);
+ tims.twait = CLAMP(timing, 1UL,
+ (unsigned long)FMC2_PMEM_PATT_TIMING_MASK);
+
+ /*
+ * tSETUP_MEM > tCS - tWAIT
+ * tSETUP_MEM > tALS - tWAIT
+ * tSETUP_MEM > tDS - (tWAIT - tHIZ)
+ */
+ tset_mem = hclkp;
+ if ((twait < NAND_TCS_MIN) && (tset_mem < (NAND_TCS_MIN - twait))) {
+ tset_mem = NAND_TCS_MIN - twait;
+ }
+ if ((twait > thiz) && ((twait - thiz) < NAND_TDS_MIN) &&
+ (tset_mem < (NAND_TDS_MIN - (twait - thiz)))) {
+ tset_mem = NAND_TDS_MIN - (twait - thiz);
+ }
+ timing = div_round_up(tset_mem, hclkp);
+ tims.tset_mem = CLAMP(timing, 1UL,
+ (unsigned long)FMC2_PMEM_PATT_TIMING_MASK);
+
+ /*
+ * tHOLD_MEM > tCH
+ * tHOLD_MEM > tREH - tSETUP_MEM
+ * tHOLD_MEM > max(tRC, tWC) - (tSETUP_MEM + tWAIT)
+ */
+ thold_mem = MAX(hclkp, NAND_TCH_MIN);
+ if ((tset_mem < NAND_TREH_MIN) &&
+ (thold_mem < (NAND_TREH_MIN - tset_mem))) {
+ thold_mem = NAND_TREH_MIN - tset_mem;
+ }
+ if (((tset_mem + twait) < NAND_TRC_MIN) &&
+ (thold_mem < (NAND_TRC_MIN - (tset_mem + twait)))) {
+ thold_mem = NAND_TRC_MIN - (tset_mem + twait);
+ }
+ if (((tset_mem + twait) < NAND_TWC_MIN) &&
+ (thold_mem < (NAND_TWC_MIN - (tset_mem + twait)))) {
+ thold_mem = NAND_TWC_MIN - (tset_mem + twait);
+ }
+ timing = div_round_up(thold_mem, hclkp);
+ tims.thold_mem = CLAMP(timing, 1UL,
+ (unsigned long)FMC2_PMEM_PATT_TIMING_MASK);
+
+ /*
+ * tSETUP_ATT > tCS - tWAIT
+ * tSETUP_ATT > tCLS - tWAIT
+ * tSETUP_ATT > tALS - tWAIT
+ * tSETUP_ATT > tRHW - tHOLD_MEM
+ * tSETUP_ATT > tDS - (tWAIT - tHIZ)
+ */
+ tset_att = hclkp;
+ if ((twait < NAND_TCS_MIN) && (tset_att < (NAND_TCS_MIN - twait))) {
+ tset_att = NAND_TCS_MIN - twait;
+ }
+ if ((thold_mem < NAND_TRHW_MIN) &&
+ (tset_att < (NAND_TRHW_MIN - thold_mem))) {
+ tset_att = NAND_TRHW_MIN - thold_mem;
+ }
+ if ((twait > thiz) && ((twait - thiz) < NAND_TDS_MIN) &&
+ (tset_att < (NAND_TDS_MIN - (twait - thiz)))) {
+ tset_att = NAND_TDS_MIN - (twait - thiz);
+ }
+ timing = div_round_up(tset_att, hclkp);
+ tims.tset_att = CLAMP(timing, 1UL,
+ (unsigned long)FMC2_PMEM_PATT_TIMING_MASK);
+
+ /*
+ * tHOLD_ATT > tALH
+ * tHOLD_ATT > tCH
+ * tHOLD_ATT > tCLH
+ * tHOLD_ATT > tCOH
+ * tHOLD_ATT > tDH
+ * tHOLD_ATT > tWB + tIO + tSYNC - tSETUP_MEM
+ * tHOLD_ATT > tADL - tSETUP_MEM
+ * tHOLD_ATT > tWH - tSETUP_MEM
+ * tHOLD_ATT > tWHR - tSETUP_MEM
+ * tHOLD_ATT > tRC - (tSETUP_ATT + tWAIT)
+ * tHOLD_ATT > tWC - (tSETUP_ATT + tWAIT)
+ */
+ thold_att = MAX(hclkp, NAND_TALH_MIN);
+ thold_att = MAX(thold_att, NAND_TCH_MIN);
+ thold_att = MAX(thold_att, NAND_TCLH_MIN);
+ thold_att = MAX(thold_att, NAND_TCOH_MIN);
+ thold_att = MAX(thold_att, NAND_TDH_MIN);
+ if (((NAND_TWB_MAX + FMC2_TIO + FMC2_TSYNC) > tset_mem) &&
+ (thold_att < (NAND_TWB_MAX + FMC2_TIO + FMC2_TSYNC - tset_mem))) {
+ thold_att = NAND_TWB_MAX + FMC2_TIO + FMC2_TSYNC - tset_mem;
+ }
+ if ((tset_mem < NAND_TADL_MIN) &&
+ (thold_att < (NAND_TADL_MIN - tset_mem))) {
+ thold_att = NAND_TADL_MIN - tset_mem;
+ }
+ if ((tset_mem < NAND_TWH_MIN) &&
+ (thold_att < (NAND_TWH_MIN - tset_mem))) {
+ thold_att = NAND_TWH_MIN - tset_mem;
+ }
+ if ((tset_mem < NAND_TWHR_MIN) &&
+ (thold_att < (NAND_TWHR_MIN - tset_mem))) {
+ thold_att = NAND_TWHR_MIN - tset_mem;
+ }
+ if (((tset_att + twait) < NAND_TRC_MIN) &&
+ (thold_att < (NAND_TRC_MIN - (tset_att + twait)))) {
+ thold_att = NAND_TRC_MIN - (tset_att + twait);
+ }
+ if (((tset_att + twait) < NAND_TWC_MIN) &&
+ (thold_att < (NAND_TWC_MIN - (tset_att + twait)))) {
+ thold_att = NAND_TWC_MIN - (tset_att + twait);
+ }
+ timing = div_round_up(thold_att, hclkp);
+ tims.thold_att = CLAMP(timing, 1UL,
+ (unsigned long)FMC2_PMEM_PATT_TIMING_MASK);
+
+ VERBOSE("NAND timings: %u - %u - %u - %u - %u - %u - %u - %u\n",
+ tims.tclr, tims.tar, tims.thiz, tims.twait,
+ tims.thold_mem, tims.tset_mem,
+ tims.thold_att, tims.tset_att);
+
+ /* Set tclr/tar timings */
+ pcr = mmio_read_32(fmc2_base() + FMC2_PCR);
+ pcr &= ~FMC2_PCR_TCLR_MASK;
+ pcr |= FMC2_PCR_TCLR(tims.tclr);
+ pcr &= ~FMC2_PCR_TAR_MASK;
+ pcr |= FMC2_PCR_TAR(tims.tar);
+
+ /* Set tset/twait/thold/thiz timings in common bank */
+ pmem = FMC2_PMEM_MEMSET(tims.tset_mem);
+ pmem |= FMC2_PMEM_MEMWAIT(tims.twait);
+ pmem |= FMC2_PMEM_MEMHOLD(tims.thold_mem);
+ pmem |= FMC2_PMEM_MEMHIZ(tims.thiz);
+
+ /* Set tset/twait/thold/thiz timings in attribute bank */
+ patt = FMC2_PATT_ATTSET(tims.tset_att);
+ patt |= FMC2_PATT_ATTWAIT(tims.twait);
+ patt |= FMC2_PATT_ATTHOLD(tims.thold_att);
+ patt |= FMC2_PATT_ATTHIZ(tims.thiz);
+
+ mmio_write_32(fmc2_base() + FMC2_PCR, pcr);
+ mmio_write_32(fmc2_base() + FMC2_PMEM, pmem);
+ mmio_write_32(fmc2_base() + FMC2_PATT, patt);
+}
+
+static void stm32_fmc2_set_buswidth_16(bool set)
+{
+ mmio_clrsetbits_32(fmc2_base() + FMC2_PCR, FMC2_PCR_PWID_MASK,
+ (set ? FMC2_PCR_PWID(FMC2_PCR_PWID_16) : 0U));
+}
+
+static void stm32_fmc2_set_ecc(bool enable)
+{
+ mmio_clrsetbits_32(fmc2_base() + FMC2_PCR, FMC2_PCR_ECCEN,
+ (enable ? FMC2_PCR_ECCEN : 0U));
+}
+
+static int stm32_fmc2_ham_correct(uint8_t *buffer, uint8_t *eccbuffer,
+ uint8_t *ecc)
+{
+ uint8_t xor_ecc_ones;
+ uint16_t xor_ecc_1b, xor_ecc_2b, xor_ecc_3b;
+ union {
+ uint32_t val;
+ uint8_t bytes[4];
+ } xor_ecc;
+
+ /* Page size--------ECC_Code Size
+ * 256---------------22 bits LSB (ECC_CODE & 0x003FFFFF)
+ * 512---------------24 bits (ECC_CODE & 0x00FFFFFF)
+ * 1024--------------26 bits (ECC_CODE & 0x03FFFFFF)
+ * 2048--------------28 bits (ECC_CODE & 0x0FFFFFFF)
+ * 4096--------------30 bits (ECC_CODE & 0x3FFFFFFF)
+ * 8192--------------32 bits (ECC_CODE & 0xFFFFFFFF)
+ */
+
+ /* For Page size 512, ECC_Code size 24 bits */
+ xor_ecc_1b = ecc[0] ^ eccbuffer[0];
+ xor_ecc_2b = ecc[1] ^ eccbuffer[1];
+ xor_ecc_3b = ecc[2] ^ eccbuffer[2];
+
+ xor_ecc.val = 0U;
+ xor_ecc.bytes[2] = xor_ecc_3b;
+ xor_ecc.bytes[1] = xor_ecc_2b;
+ xor_ecc.bytes[0] = xor_ecc_1b;
+
+ if (xor_ecc.val == 0U) {
+ return 0; /* No Error */
+ }
+
+ xor_ecc_ones = __builtin_popcount(xor_ecc.val);
+ if (xor_ecc_ones < 23U) {
+ if (xor_ecc_ones == 12U) {
+ uint16_t bit_address, byte_address;
+
+ /* Correctable ERROR */
+ bit_address = ((xor_ecc_1b >> 1) & BIT(0)) |
+ ((xor_ecc_1b >> 2) & BIT(1)) |
+ ((xor_ecc_1b >> 3) & BIT(2));
+
+ byte_address = ((xor_ecc_1b >> 7) & BIT(0)) |
+ ((xor_ecc_2b) & BIT(1)) |
+ ((xor_ecc_2b >> 1) & BIT(2)) |
+ ((xor_ecc_2b >> 2) & BIT(3)) |
+ ((xor_ecc_2b >> 3) & BIT(4)) |
+ ((xor_ecc_3b << 4) & BIT(5)) |
+ ((xor_ecc_3b << 3) & BIT(6)) |
+ ((xor_ecc_3b << 2) & BIT(7)) |
+ ((xor_ecc_3b << 1) & BIT(8));
+
+ /* Correct bit error in the data */
+ buffer[byte_address] =
+ buffer[byte_address] ^ BIT(bit_address);
+ VERBOSE("Hamming: 1 ECC error corrected\n");
+
+ return 0;
+ }
+
+ /* Non Correctable ERROR */
+ ERROR("%s: Uncorrectable ECC Errors\n", __func__);
+ return -1;
+ }
+
+ /* ECC ERROR */
+ ERROR("%s: Hamming correction error\n", __func__);
+ return -1;
+}
+
+
+static int stm32_fmc2_ham_calculate(uint8_t *buffer, uint8_t *ecc)
+{
+ uint32_t heccr;
+ uint64_t timeout = timeout_init_us(TIMEOUT_US_10_MS);
+
+ while ((mmio_read_32(fmc2_base() + FMC2_SR) & FMC2_SR_NWRF) == 0U) {
+ if (timeout_elapsed(timeout)) {
+ return -ETIMEDOUT;
+ }
+ }
+
+ heccr = mmio_read_32(fmc2_base() + FMC2_HECCR);
+
+ ecc[0] = heccr;
+ ecc[1] = heccr >> 8;
+ ecc[2] = heccr >> 16;
+
+ /* Disable ECC */
+ stm32_fmc2_set_ecc(false);
+
+ return 0;
+}
+
+static int stm32_fmc2_bch_correct(uint8_t *buffer, unsigned int eccsize)
+{
+ uint32_t bchdsr0, bchdsr1, bchdsr2, bchdsr3, bchdsr4;
+ uint16_t pos[8];
+ int i, den;
+ uint64_t timeout = timeout_init_us(TIMEOUT_US_10_MS);
+
+ while ((mmio_read_32(fmc2_base() + FMC2_BCHISR) &
+ FMC2_BCHISR_DERF) == 0U) {
+ if (timeout_elapsed(timeout)) {
+ return -ETIMEDOUT;
+ }
+ }
+
+ bchdsr0 = mmio_read_32(fmc2_base() + FMC2_BCHDSR0);
+ bchdsr1 = mmio_read_32(fmc2_base() + FMC2_BCHDSR1);
+ bchdsr2 = mmio_read_32(fmc2_base() + FMC2_BCHDSR2);
+ bchdsr3 = mmio_read_32(fmc2_base() + FMC2_BCHDSR3);
+ bchdsr4 = mmio_read_32(fmc2_base() + FMC2_BCHDSR4);
+
+ /* Disable ECC */
+ stm32_fmc2_set_ecc(false);
+
+ /* No error found */
+ if ((bchdsr0 & FMC2_BCHDSR0_DEF) == 0U) {
+ return 0;
+ }
+
+ /* Too many errors detected */
+ if ((bchdsr0 & FMC2_BCHDSR0_DUE) != 0U) {
+ return -EBADMSG;
+ }
+
+ pos[0] = bchdsr1 & FMC2_BCHDSR1_EBP1_MASK;
+ pos[1] = (bchdsr1 & FMC2_BCHDSR1_EBP2_MASK) >> FMC2_BCHDSR1_EBP2_SHIFT;
+ pos[2] = bchdsr2 & FMC2_BCHDSR2_EBP3_MASK;
+ pos[3] = (bchdsr2 & FMC2_BCHDSR2_EBP4_MASK) >> FMC2_BCHDSR2_EBP4_SHIFT;
+ pos[4] = bchdsr3 & FMC2_BCHDSR3_EBP5_MASK;
+ pos[5] = (bchdsr3 & FMC2_BCHDSR3_EBP6_MASK) >> FMC2_BCHDSR3_EBP6_SHIFT;
+ pos[6] = bchdsr4 & FMC2_BCHDSR4_EBP7_MASK;
+ pos[7] = (bchdsr4 & FMC2_BCHDSR4_EBP8_MASK) >> FMC2_BCHDSR4_EBP8_SHIFT;
+
+ den = (bchdsr0 & FMC2_BCHDSR0_DEN_MASK) >> FMC2_BCHDSR0_DEN_SHIFT;
+ for (i = 0; i < den; i++) {
+ if (pos[i] < (eccsize * 8U)) {
+ uint8_t bitmask = BIT(pos[i] % 8U);
+ uint32_t offset = pos[i] / 8U;
+
+ *(buffer + offset) ^= bitmask;
+ }
+ }
+
+ return 0;
+}
+
+static void stm32_fmc2_hwctl(struct nand_device *nand)
+{
+ stm32_fmc2_set_ecc(false);
+
+ if (nand->ecc.max_bit_corr != FMC2_ECC_HAM) {
+ mmio_clrbits_32(fmc2_base() + FMC2_PCR, FMC2_PCR_WEN);
+ mmio_write_32(fmc2_base() + FMC2_BCHICR, FMC2_BCHICR_CLEAR_IRQ);
+ }
+
+ stm32_fmc2_set_ecc(true);
+}
+
+static int stm32_fmc2_read_page(struct nand_device *nand,
+ unsigned int page, uintptr_t buffer)
+{
+ unsigned int eccsize = nand->ecc.size;
+ unsigned int eccbytes = nand->ecc.bytes;
+ unsigned int eccsteps = nand->page_size / eccsize;
+ uint8_t ecc_corr[FMC2_MAX_ECC_BYTES];
+ uint8_t ecc_cal[FMC2_MAX_ECC_BYTES] = {0U};
+ uint8_t *p;
+ unsigned int i;
+ unsigned int s;
+ int ret;
+
+ VERBOSE(">%s page %u buffer %lx\n", __func__, page, buffer);
+
+ ret = nand_read_page_cmd(page, 0U, 0U, 0U);
+ if (ret != 0) {
+ return ret;
+ }
+
+ for (s = 0U, i = nand->page_size + FMC2_BBM_LEN, p = (uint8_t *)buffer;
+ s < eccsteps;
+ s++, i += eccbytes, p += eccsize) {
+ stm32_fmc2_hwctl(nand);
+
+ /* Read the NAND page sector (512 bytes) */
+ ret = nand_change_read_column_cmd(s * eccsize, (uintptr_t)p,
+ eccsize);
+ if (ret != 0) {
+ return ret;
+ }
+
+ if (nand->ecc.max_bit_corr == FMC2_ECC_HAM) {
+ ret = stm32_fmc2_ham_calculate(p, ecc_cal);
+ if (ret != 0) {
+ return ret;
+ }
+ }
+
+ /* Read the corresponding ECC bytes */
+ ret = nand_change_read_column_cmd(i, (uintptr_t)ecc_corr,
+ eccbytes);
+ if (ret != 0) {
+ return ret;
+ }
+
+ /* Correct the data */
+ if (nand->ecc.max_bit_corr == FMC2_ECC_HAM) {
+ ret = stm32_fmc2_ham_correct(p, ecc_corr, ecc_cal);
+ } else {
+ ret = stm32_fmc2_bch_correct(p, eccsize);
+ }
+
+ if (ret != 0) {
+ return ret;
+ }
+ }
+
+ return 0;
+}
+
+static void stm32_fmc2_read_data(struct nand_device *nand,
+ uint8_t *buff, unsigned int length,
+ bool use_bus8)
+{
+ uintptr_t data_base = stm32_fmc2.cs[stm32_fmc2.cs_sel].data_base;
+
+ if (use_bus8 && (nand->buswidth == NAND_BUS_WIDTH_16)) {
+ stm32_fmc2_set_buswidth_16(false);
+ }
+
+ if ((((uintptr_t)buff & BIT(0)) != 0U) && (length != 0U)) {
+ *buff = mmio_read_8(data_base);
+ buff += sizeof(uint8_t);
+ length -= sizeof(uint8_t);
+ }
+
+ if ((((uintptr_t)buff & GENMASK_32(1, 0)) != 0U) &&
+ (length >= sizeof(uint16_t))) {
+ *(uint16_t *)buff = mmio_read_16(data_base);
+ buff += sizeof(uint16_t);
+ length -= sizeof(uint16_t);
+ }
+
+ /* 32bit aligned */
+ while (length >= sizeof(uint32_t)) {
+ *(uint32_t *)buff = mmio_read_32(data_base);
+ buff += sizeof(uint32_t);
+ length -= sizeof(uint32_t);
+ }
+
+ /* Read remaining bytes */
+ if (length >= sizeof(uint16_t)) {
+ *(uint16_t *)buff = mmio_read_16(data_base);
+ buff += sizeof(uint16_t);
+ length -= sizeof(uint16_t);
+ }
+
+ if (length != 0U) {
+ *buff = mmio_read_8(data_base);
+ }
+
+ if (use_bus8 && (nand->buswidth == NAND_BUS_WIDTH_16)) {
+ /* Reconfigure bus width to 16-bit */
+ stm32_fmc2_set_buswidth_16(true);
+ }
+}
+
+static void stm32_fmc2_write_data(struct nand_device *nand,
+ uint8_t *buff, unsigned int length,
+ bool use_bus8)
+{
+ uintptr_t data_base = stm32_fmc2.cs[stm32_fmc2.cs_sel].data_base;
+
+ if (use_bus8 && (nand->buswidth == NAND_BUS_WIDTH_16)) {
+ /* Reconfigure bus width to 8-bit */
+ stm32_fmc2_set_buswidth_16(false);
+ }
+
+ if ((((uintptr_t)buff & BIT(0)) != 0U) && (length != 0U)) {
+ mmio_write_8(data_base, *buff);
+ buff += sizeof(uint8_t);
+ length -= sizeof(uint8_t);
+ }
+
+ if ((((uintptr_t)buff & GENMASK_32(1, 0)) != 0U) &&
+ (length >= sizeof(uint16_t))) {
+ mmio_write_16(data_base, *(uint16_t *)buff);
+ buff += sizeof(uint16_t);
+ length -= sizeof(uint16_t);
+ }
+
+ /* 32bits aligned */
+ while (length >= sizeof(uint32_t)) {
+ mmio_write_32(data_base, *(uint32_t *)buff);
+ buff += sizeof(uint32_t);
+ length -= sizeof(uint32_t);
+ }
+
+ /* Read remaining bytes */
+ if (length >= sizeof(uint16_t)) {
+ mmio_write_16(data_base, *(uint16_t *)buff);
+ buff += sizeof(uint16_t);
+ length -= sizeof(uint16_t);
+ }
+
+ if (length != 0U) {
+ mmio_write_8(data_base, *buff);
+ }
+
+ if (use_bus8 && (nand->buswidth == NAND_BUS_WIDTH_16)) {
+ /* Reconfigure bus width to 16-bit */
+ stm32_fmc2_set_buswidth_16(true);
+ }
+}
+
+static void stm32_fmc2_ctrl_init(void)
+{
+ uint32_t pcr = mmio_read_32(fmc2_base() + FMC2_PCR);
+ uint32_t bcr1 = mmio_read_32(fmc2_base() + FMC2_BCR1);
+
+ /* Enable wait feature and NAND flash memory bank */
+ pcr |= FMC2_PCR_PWAITEN;
+ pcr |= FMC2_PCR_PBKEN;
+
+ /* Set buswidth to 8 bits mode for identification */
+ pcr &= ~FMC2_PCR_PWID_MASK;
+
+ /* ECC logic is disabled */
+ pcr &= ~FMC2_PCR_ECCEN;
+
+ /* Default mode */
+ pcr &= ~FMC2_PCR_ECCALG;
+ pcr &= ~FMC2_PCR_BCHECC;
+ pcr &= ~FMC2_PCR_WEN;
+
+ /* Set default ECC sector size */
+ pcr &= ~FMC2_PCR_ECCSS_MASK;
+ pcr |= FMC2_PCR_ECCSS(FMC2_PCR_ECCSS_2048);
+
+ /* Set default TCLR/TAR timings */
+ pcr &= ~FMC2_PCR_TCLR_MASK;
+ pcr |= FMC2_PCR_TCLR(FMC2_PCR_TCLR_DEFAULT);
+ pcr &= ~FMC2_PCR_TAR_MASK;
+ pcr |= FMC2_PCR_TAR(FMC2_PCR_TAR_DEFAULT);
+
+ /* Enable FMC2 controller */
+ bcr1 |= FMC2_BCR1_FMC2EN;
+
+ mmio_write_32(fmc2_base() + FMC2_BCR1, bcr1);
+ mmio_write_32(fmc2_base() + FMC2_PCR, pcr);
+ mmio_write_32(fmc2_base() + FMC2_PMEM, FMC2_PMEM_DEFAULT);
+ mmio_write_32(fmc2_base() + FMC2_PATT, FMC2_PATT_DEFAULT);
+}
+
+static int stm32_fmc2_exec(struct nand_req *req)
+{
+ int ret = 0;
+
+ switch (req->type & NAND_REQ_MASK) {
+ case NAND_REQ_CMD:
+ VERBOSE("Write CMD %x\n", (uint8_t)req->type);
+ mmio_write_8(stm32_fmc2.cs[stm32_fmc2.cs_sel].cmd_base,
+ (uint8_t)req->type);
+ break;
+ case NAND_REQ_ADDR:
+ VERBOSE("Write ADDR %x\n", *(req->addr));
+ mmio_write_8(stm32_fmc2.cs[stm32_fmc2.cs_sel].addr_base,
+ *(req->addr));
+ break;
+ case NAND_REQ_DATAIN:
+ VERBOSE("Read data\n");
+ stm32_fmc2_read_data(req->nand, req->addr, req->length,
+ ((req->type & NAND_REQ_BUS_WIDTH_8) !=
+ 0U));
+ break;
+ case NAND_REQ_DATAOUT:
+ VERBOSE("Write data\n");
+ stm32_fmc2_write_data(req->nand, req->addr, req->length,
+ ((req->type & NAND_REQ_BUS_WIDTH_8) !=
+ 0U));
+ break;
+ case NAND_REQ_WAIT:
+ VERBOSE("WAIT Ready\n");
+ ret = nand_wait_ready(req->delay_ms);
+ break;
+ default:
+ ret = -EINVAL;
+ break;
+ };
+
+ return ret;
+}
+
+static void stm32_fmc2_setup(struct nand_device *nand)
+{
+ uint32_t pcr = mmio_read_32(fmc2_base() + FMC2_PCR);
+
+ /* Set buswidth */
+ pcr &= ~FMC2_PCR_PWID_MASK;
+ if (nand->buswidth == NAND_BUS_WIDTH_16) {
+ pcr |= FMC2_PCR_PWID(FMC2_PCR_PWID_16);
+ }
+
+ if (nand->ecc.mode == NAND_ECC_HW) {
+ nand->mtd_read_page = stm32_fmc2_read_page;
+
+ pcr &= ~FMC2_PCR_ECCALG;
+ pcr &= ~FMC2_PCR_BCHECC;
+
+ pcr &= ~FMC2_PCR_ECCSS_MASK;
+ pcr |= FMC2_PCR_ECCSS(FMC2_PCR_ECCSS_512);
+
+ switch (nand->ecc.max_bit_corr) {
+ case FMC2_ECC_HAM:
+ nand->ecc.bytes = 3;
+ break;
+ case FMC2_ECC_BCH8:
+ pcr |= FMC2_PCR_ECCALG;
+ pcr |= FMC2_PCR_BCHECC;
+ nand->ecc.bytes = 13;
+ break;
+ default:
+ /* Use FMC2 ECC BCH4 */
+ pcr |= FMC2_PCR_ECCALG;
+ nand->ecc.bytes = 7;
+ break;
+ }
+
+ if ((nand->buswidth & NAND_BUS_WIDTH_16) != 0) {
+ nand->ecc.bytes++;
+ }
+ }
+
+ mmio_write_32(stm32_fmc2.reg_base + FMC2_PCR, pcr);
+}
+
+static const struct nand_ctrl_ops ctrl_ops = {
+ .setup = stm32_fmc2_setup,
+ .exec = stm32_fmc2_exec
+};
+
+int stm32_fmc2_init(void)
+{
+ int fmc_ebi_node;
+ int fmc_nfc_node;
+ int fmc_flash_node = 0;
+ int nchips = 0;
+ unsigned int i;
+ void *fdt = NULL;
+ const fdt32_t *cuint;
+ struct dt_node_info info;
+ uintptr_t bank_address[MAX_BANK] = { 0, 0, 0, 0, 0 };
+ uint8_t bank_assigned = 0;
+ uint8_t bank;
+ int ret;
+
+ if (fdt_get_address(&fdt) == 0) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ fmc_ebi_node = dt_get_node(&info, -1, DT_FMC2_EBI_COMPAT);
+ if (fmc_ebi_node < 0) {
+ return fmc_ebi_node;
+ }
+
+ if (info.status == DT_DISABLED) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ stm32_fmc2.reg_base = info.base;
+
+ if ((info.clock < 0) || (info.reset < 0)) {
+ return -FDT_ERR_BADVALUE;
+ }
+
+ stm32_fmc2.clock_id = (unsigned long)info.clock;
+ stm32_fmc2.reset_id = (unsigned int)info.reset;
+
+ cuint = fdt_getprop(fdt, fmc_ebi_node, "ranges", NULL);
+ if (cuint == NULL) {
+ return -FDT_ERR_BADVALUE;
+ }
+
+ for (i = 0U; i < MAX_BANK; i++) {
+ bank = fdt32_to_cpu(*cuint);
+ if ((bank >= MAX_BANK) || ((bank_assigned & BIT(bank)) != 0U)) {
+ return -FDT_ERR_BADVALUE;
+ }
+ bank_assigned |= BIT(bank);
+ bank_address[bank] = fdt32_to_cpu(*(cuint + 2));
+ cuint += 4;
+ }
+
+ /* Pinctrl initialization */
+ if (dt_set_pinctrl_config(fmc_ebi_node) != 0) {
+ return -FDT_ERR_BADVALUE;
+ }
+
+ /* Parse NFC controller node */
+ fmc_nfc_node = fdt_node_offset_by_compatible(fdt, fmc_ebi_node,
+ DT_FMC2_NFC_COMPAT);
+ if (fmc_nfc_node < 0) {
+ return fmc_nfc_node;
+ }
+
+ if (fdt_get_status(fmc_nfc_node) == DT_DISABLED) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ cuint = fdt_getprop(fdt, fmc_nfc_node, "reg", NULL);
+ if (cuint == NULL) {
+ return -FDT_ERR_BADVALUE;
+ }
+
+ for (i = 0U; i < MAX_CS; i++) {
+ bank = fdt32_to_cpu(*cuint);
+ if (bank >= MAX_BANK) {
+ return -FDT_ERR_BADVALUE;
+ }
+ stm32_fmc2.cs[i].data_base = fdt32_to_cpu(*(cuint + 1)) +
+ bank_address[bank];
+
+ bank = fdt32_to_cpu(*(cuint + 3));
+ if (bank >= MAX_BANK) {
+ return -FDT_ERR_BADVALUE;
+ }
+ stm32_fmc2.cs[i].cmd_base = fdt32_to_cpu(*(cuint + 4)) +
+ bank_address[bank];
+
+ bank = fdt32_to_cpu(*(cuint + 6));
+ if (bank >= MAX_BANK) {
+ return -FDT_ERR_BADVALUE;
+ }
+ stm32_fmc2.cs[i].addr_base = fdt32_to_cpu(*(cuint + 7)) +
+ bank_address[bank];
+
+ cuint += 9;
+ }
+
+ /* Parse flash nodes */
+ fdt_for_each_subnode(fmc_flash_node, fdt, fmc_nfc_node) {
+ nchips++;
+ }
+
+ if (nchips != 1) {
+ WARN("Only one SLC NAND device supported\n");
+ return -FDT_ERR_BADVALUE;
+ }
+
+ fdt_for_each_subnode(fmc_flash_node, fdt, fmc_nfc_node) {
+ /* Get chip select */
+ cuint = fdt_getprop(fdt, fmc_flash_node, "reg", NULL);
+ if (cuint == NULL) {
+ WARN("Chip select not well defined\n");
+ return -FDT_ERR_BADVALUE;
+ }
+
+ stm32_fmc2.cs_sel = fdt32_to_cpu(*cuint);
+ if (stm32_fmc2.cs_sel >= MAX_CS) {
+ return -FDT_ERR_BADVALUE;
+ }
+
+ VERBOSE("NAND CS %i\n", stm32_fmc2.cs_sel);
+ }
+
+ /* Enable Clock */
+ clk_enable(stm32_fmc2.clock_id);
+
+ /* Reset IP */
+ ret = stm32mp_reset_assert(stm32_fmc2.reset_id, TIMEOUT_US_1_MS);
+ if (ret != 0) {
+ panic();
+ }
+ ret = stm32mp_reset_deassert(stm32_fmc2.reset_id, TIMEOUT_US_1_MS);
+ if (ret != 0) {
+ panic();
+ }
+
+ /* Setup default IP registers */
+ stm32_fmc2_ctrl_init();
+
+ /* Setup default timings */
+ stm32_fmc2_nand_setup_timing();
+
+ /* Init NAND RAW framework */
+ nand_raw_ctrl_init(&ctrl_ops);
+
+ return 0;
+}
diff --git a/drivers/st/gpio/stm32_gpio.c b/drivers/st/gpio/stm32_gpio.c
new file mode 100644
index 0000000..708989f
--- /dev/null
+++ b/drivers/st/gpio/stm32_gpio.c
@@ -0,0 +1,323 @@
+/*
+ * Copyright (c) 2016-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdbool.h>
+
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <drivers/clk.h>
+#include <drivers/st/stm32_gpio.h>
+#include <drivers/st/stm32mp_clkfunc.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+#define DT_GPIO_BANK_SHIFT 12
+#define DT_GPIO_BANK_MASK GENMASK(16, 12)
+#define DT_GPIO_PIN_SHIFT 8
+#define DT_GPIO_PIN_MASK GENMASK(11, 8)
+#define DT_GPIO_MODE_MASK GENMASK(7, 0)
+
+static void set_gpio(uint32_t bank, uint32_t pin, uint32_t mode, uint32_t type,
+ uint32_t speed, uint32_t pull, uint32_t od,
+ uint32_t alternate, uint8_t status);
+
+/*******************************************************************************
+ * This function gets GPIO bank node in DT.
+ * Returns node offset if status is okay in DT, else return 0
+ ******************************************************************************/
+static int ckeck_gpio_bank(void *fdt, uint32_t bank, int pinctrl_node)
+{
+ int pinctrl_subnode;
+ uint32_t bank_offset = stm32_get_gpio_bank_offset(bank);
+
+ fdt_for_each_subnode(pinctrl_subnode, fdt, pinctrl_node) {
+ const fdt32_t *cuint;
+
+ if (fdt_getprop(fdt, pinctrl_subnode,
+ "gpio-controller", NULL) == NULL) {
+ continue;
+ }
+
+ cuint = fdt_getprop(fdt, pinctrl_subnode, "reg", NULL);
+ if (cuint == NULL) {
+ continue;
+ }
+
+ if ((fdt32_to_cpu(*cuint) == bank_offset) &&
+ (fdt_get_status(pinctrl_subnode) != DT_DISABLED)) {
+ return pinctrl_subnode;
+ }
+ }
+
+ return 0;
+}
+
+/*******************************************************************************
+ * This function gets the pin settings from DT information.
+ * When analyze and parsing is done, set the GPIO registers.
+ * Returns 0 on success and a negative FDT error code on failure.
+ ******************************************************************************/
+static int dt_set_gpio_config(void *fdt, int node, uint8_t status)
+{
+ const fdt32_t *cuint, *slewrate;
+ int len;
+ int pinctrl_node;
+ uint32_t i;
+ uint32_t speed = GPIO_SPEED_LOW;
+ uint32_t pull = GPIO_NO_PULL;
+
+ cuint = fdt_getprop(fdt, node, "pinmux", &len);
+ if (cuint == NULL) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ pinctrl_node = fdt_parent_offset(fdt, fdt_parent_offset(fdt, node));
+ if (pinctrl_node < 0) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ slewrate = fdt_getprop(fdt, node, "slew-rate", NULL);
+ if (slewrate != NULL) {
+ speed = fdt32_to_cpu(*slewrate);
+ }
+
+ if (fdt_getprop(fdt, node, "bias-pull-up", NULL) != NULL) {
+ pull = GPIO_PULL_UP;
+ } else if (fdt_getprop(fdt, node, "bias-pull-down", NULL) != NULL) {
+ pull = GPIO_PULL_DOWN;
+ } else {
+ VERBOSE("No bias configured in node %d\n", node);
+ }
+
+ for (i = 0U; i < ((uint32_t)len / sizeof(uint32_t)); i++) {
+ uint32_t pincfg;
+ uint32_t bank;
+ uint32_t pin;
+ uint32_t mode;
+ uint32_t alternate = GPIO_ALTERNATE_(0);
+ uint32_t type;
+ uint32_t od = GPIO_OD_OUTPUT_LOW;
+ int bank_node;
+ int clk;
+
+ pincfg = fdt32_to_cpu(*cuint);
+ cuint++;
+
+ bank = (pincfg & DT_GPIO_BANK_MASK) >> DT_GPIO_BANK_SHIFT;
+
+ pin = (pincfg & DT_GPIO_PIN_MASK) >> DT_GPIO_PIN_SHIFT;
+
+ mode = pincfg & DT_GPIO_MODE_MASK;
+
+ switch (mode) {
+ case 0:
+ mode = GPIO_MODE_INPUT;
+ break;
+ case 1 ... 16:
+ alternate = mode - 1U;
+ mode = GPIO_MODE_ALTERNATE;
+ break;
+ case 17:
+ mode = GPIO_MODE_ANALOG;
+ break;
+ default:
+ mode = GPIO_MODE_OUTPUT;
+ break;
+ }
+
+ if (fdt_getprop(fdt, node, "drive-open-drain", NULL) != NULL) {
+ type = GPIO_TYPE_OPEN_DRAIN;
+ } else {
+ type = GPIO_TYPE_PUSH_PULL;
+ }
+
+ if (fdt_getprop(fdt, node, "output-high", NULL) != NULL) {
+ if (mode == GPIO_MODE_INPUT) {
+ mode = GPIO_MODE_OUTPUT;
+ od = GPIO_OD_OUTPUT_HIGH;
+ }
+ }
+
+ if (fdt_getprop(fdt, node, "output-low", NULL) != NULL) {
+ if (mode == GPIO_MODE_INPUT) {
+ mode = GPIO_MODE_OUTPUT;
+ od = GPIO_OD_OUTPUT_LOW;
+ }
+ }
+
+ bank_node = ckeck_gpio_bank(fdt, bank, pinctrl_node);
+ if (bank_node == 0) {
+ ERROR("PINCTRL inconsistent in DT\n");
+ panic();
+ }
+
+ clk = fdt_get_clock_id(bank_node);
+ if (clk < 0) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ /* Platform knows the clock: assert it is okay */
+ assert((unsigned long)clk == stm32_get_gpio_bank_clock(bank));
+
+ set_gpio(bank, pin, mode, type, speed, pull, od, alternate, status);
+ }
+
+ return 0;
+}
+
+/*******************************************************************************
+ * This function gets the pin settings from DT information.
+ * When analyze and parsing is done, set the GPIO registers.
+ * Returns 0 on success and a negative FDT/ERRNO error code on failure.
+ ******************************************************************************/
+int dt_set_pinctrl_config(int node)
+{
+ const fdt32_t *cuint;
+ int lenp;
+ uint32_t i;
+ uint8_t status;
+ void *fdt;
+
+ if (fdt_get_address(&fdt) == 0) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ status = fdt_get_status(node);
+ if (status == DT_DISABLED) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ cuint = fdt_getprop(fdt, node, "pinctrl-0", &lenp);
+ if (cuint == NULL) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ for (i = 0; i < ((uint32_t)lenp / 4U); i++) {
+ int p_node, p_subnode;
+
+ p_node = fdt_node_offset_by_phandle(fdt, fdt32_to_cpu(*cuint));
+ if (p_node < 0) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ fdt_for_each_subnode(p_subnode, fdt, p_node) {
+ int ret = dt_set_gpio_config(fdt, p_subnode, status);
+
+ if (ret < 0) {
+ return ret;
+ }
+ }
+
+ cuint++;
+ }
+
+ return 0;
+}
+
+static void set_gpio(uint32_t bank, uint32_t pin, uint32_t mode, uint32_t type,
+ uint32_t speed, uint32_t pull, uint32_t od,
+ uint32_t alternate, uint8_t status)
+{
+ uintptr_t base = stm32_get_gpio_bank_base(bank);
+ unsigned long clock = stm32_get_gpio_bank_clock(bank);
+
+ assert(pin <= GPIO_PIN_MAX);
+
+ clk_enable(clock);
+
+ mmio_clrsetbits_32(base + GPIO_MODE_OFFSET,
+ (uint32_t)GPIO_MODE_MASK << (pin << 1),
+ mode << (pin << 1));
+
+ mmio_clrsetbits_32(base + GPIO_TYPE_OFFSET,
+ (uint32_t)GPIO_TYPE_MASK << pin,
+ type << pin);
+
+ mmio_clrsetbits_32(base + GPIO_SPEED_OFFSET,
+ (uint32_t)GPIO_SPEED_MASK << (pin << 1),
+ speed << (pin << 1));
+
+ mmio_clrsetbits_32(base + GPIO_PUPD_OFFSET,
+ (uint32_t)GPIO_PULL_MASK << (pin << 1),
+ pull << (pin << 1));
+
+ if (pin < GPIO_ALT_LOWER_LIMIT) {
+ mmio_clrsetbits_32(base + GPIO_AFRL_OFFSET,
+ (uint32_t)GPIO_ALTERNATE_MASK << (pin << 2),
+ alternate << (pin << 2));
+ } else {
+ size_t shift = (pin - GPIO_ALT_LOWER_LIMIT) << 2;
+
+ mmio_clrsetbits_32(base + GPIO_AFRH_OFFSET,
+ (uint32_t)GPIO_ALTERNATE_MASK << shift,
+ alternate << shift);
+ }
+
+ mmio_clrsetbits_32(base + GPIO_OD_OFFSET,
+ (uint32_t)GPIO_OD_MASK << pin,
+ od << pin);
+
+ VERBOSE("GPIO %u mode set to 0x%x\n", bank,
+ mmio_read_32(base + GPIO_MODE_OFFSET));
+ VERBOSE("GPIO %u type set to 0x%x\n", bank,
+ mmio_read_32(base + GPIO_TYPE_OFFSET));
+ VERBOSE("GPIO %u speed set to 0x%x\n", bank,
+ mmio_read_32(base + GPIO_SPEED_OFFSET));
+ VERBOSE("GPIO %u mode pull to 0x%x\n", bank,
+ mmio_read_32(base + GPIO_PUPD_OFFSET));
+ VERBOSE("GPIO %u mode alternate low to 0x%x\n", bank,
+ mmio_read_32(base + GPIO_AFRL_OFFSET));
+ VERBOSE("GPIO %u mode alternate high to 0x%x\n", bank,
+ mmio_read_32(base + GPIO_AFRH_OFFSET));
+ VERBOSE("GPIO %u output data set to 0x%x\n", bank,
+ mmio_read_32(base + GPIO_OD_OFFSET));
+
+ clk_disable(clock);
+
+ if (status == DT_SECURE) {
+ stm32mp_register_secure_gpio(bank, pin);
+#if !IMAGE_BL2
+ set_gpio_secure_cfg(bank, pin, true);
+#endif
+
+ } else {
+ stm32mp_register_non_secure_gpio(bank, pin);
+#if !IMAGE_BL2
+ set_gpio_secure_cfg(bank, pin, false);
+#endif
+ }
+}
+
+void set_gpio_secure_cfg(uint32_t bank, uint32_t pin, bool secure)
+{
+ uintptr_t base = stm32_get_gpio_bank_base(bank);
+ unsigned long clock = stm32_get_gpio_bank_clock(bank);
+
+ assert(pin <= GPIO_PIN_MAX);
+
+ clk_enable(clock);
+
+ if (secure) {
+ mmio_setbits_32(base + GPIO_SECR_OFFSET, BIT(pin));
+ } else {
+ mmio_clrbits_32(base + GPIO_SECR_OFFSET, BIT(pin));
+ }
+
+ clk_disable(clock);
+}
+
+void set_gpio_reset_cfg(uint32_t bank, uint32_t pin)
+{
+ set_gpio(bank, pin, GPIO_MODE_ANALOG, GPIO_TYPE_PUSH_PULL,
+ GPIO_SPEED_LOW, GPIO_NO_PULL, GPIO_OD_OUTPUT_LOW,
+ GPIO_ALTERNATE_(0), DT_DISABLED);
+ set_gpio_secure_cfg(bank, pin, stm32_gpio_is_secure_at_reset(bank));
+}
diff --git a/drivers/st/i2c/stm32_i2c.c b/drivers/st/i2c/stm32_i2c.c
new file mode 100644
index 0000000..bf6c3ee
--- /dev/null
+++ b/drivers/st/i2c/stm32_i2c.c
@@ -0,0 +1,982 @@
+/*
+ * Copyright (c) 2016-2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+#include <stdbool.h>
+#include <stdlib.h>
+
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+#include <common/debug.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32_gpio.h>
+#include <drivers/st/stm32_i2c.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+
+/* STM32 I2C registers offsets */
+#define I2C_CR1 0x00U
+#define I2C_CR2 0x04U
+#define I2C_OAR1 0x08U
+#define I2C_OAR2 0x0CU
+#define I2C_TIMINGR 0x10U
+#define I2C_TIMEOUTR 0x14U
+#define I2C_ISR 0x18U
+#define I2C_ICR 0x1CU
+#define I2C_PECR 0x20U
+#define I2C_RXDR 0x24U
+#define I2C_TXDR 0x28U
+
+#define TIMINGR_CLEAR_MASK 0xF0FFFFFFU
+
+#define MAX_NBYTE_SIZE 255U
+
+#define I2C_NSEC_PER_SEC 1000000000L
+
+/* I2C Timing hard-coded value, for I2C clock source is HSI at 64MHz */
+#define I2C_TIMING 0x10D07DB5
+
+static void notif_i2c_timeout(struct i2c_handle_s *hi2c)
+{
+ hi2c->i2c_err |= I2C_ERROR_TIMEOUT;
+ hi2c->i2c_mode = I2C_MODE_NONE;
+ hi2c->i2c_state = I2C_STATE_READY;
+}
+
+/*
+ * @brief Configure I2C Analog noise filter.
+ * @param hi2c: Pointer to a struct i2c_handle_s structure that contains
+ * the configuration information for the specified I2C peripheral.
+ * @param analog_filter: New state of the Analog filter
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_config_analog_filter(struct i2c_handle_s *hi2c,
+ uint32_t analog_filter)
+{
+ if ((hi2c->i2c_state != I2C_STATE_READY) || (hi2c->lock != 0U)) {
+ return -EBUSY;
+ }
+
+ hi2c->lock = 1;
+
+ hi2c->i2c_state = I2C_STATE_BUSY;
+
+ /* Disable the selected I2C peripheral */
+ mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR1, I2C_CR1_PE);
+
+ /* Reset I2Cx ANOFF bit */
+ mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR1, I2C_CR1_ANFOFF);
+
+ /* Set analog filter bit*/
+ mmio_setbits_32(hi2c->i2c_base_addr + I2C_CR1, analog_filter);
+
+ /* Enable the selected I2C peripheral */
+ mmio_setbits_32(hi2c->i2c_base_addr + I2C_CR1, I2C_CR1_PE);
+
+ hi2c->i2c_state = I2C_STATE_READY;
+
+ hi2c->lock = 0;
+
+ return 0;
+}
+
+/*
+ * @brief Get I2C setup information from the device tree and set pinctrl
+ * configuration.
+ * @param fdt: Pointer to the device tree
+ * @param node: I2C node offset
+ * @param init: Ref to the initialization configuration structure
+ * @retval 0 if OK, negative value else
+ */
+int stm32_i2c_get_setup_from_fdt(void *fdt, int node,
+ struct stm32_i2c_init_s *init)
+{
+ const fdt32_t *cuint;
+
+ cuint = fdt_getprop(fdt, node, "i2c-scl-rising-time-ns", NULL);
+ if (cuint == NULL) {
+ init->rise_time = STM32_I2C_RISE_TIME_DEFAULT;
+ } else {
+ init->rise_time = fdt32_to_cpu(*cuint);
+ }
+
+ cuint = fdt_getprop(fdt, node, "i2c-scl-falling-time-ns", NULL);
+ if (cuint == NULL) {
+ init->fall_time = STM32_I2C_FALL_TIME_DEFAULT;
+ } else {
+ init->fall_time = fdt32_to_cpu(*cuint);
+ }
+
+ cuint = fdt_getprop(fdt, node, "clock-frequency", NULL);
+ if (cuint == NULL) {
+ init->speed_mode = STM32_I2C_SPEED_DEFAULT;
+ } else {
+ switch (fdt32_to_cpu(*cuint)) {
+ case STANDARD_RATE:
+ init->speed_mode = I2C_SPEED_STANDARD;
+ break;
+ case FAST_RATE:
+ init->speed_mode = I2C_SPEED_FAST;
+ break;
+ case FAST_PLUS_RATE:
+ init->speed_mode = I2C_SPEED_FAST_PLUS;
+ break;
+ default:
+ init->speed_mode = STM32_I2C_SPEED_DEFAULT;
+ break;
+ }
+ }
+
+ return dt_set_pinctrl_config(node);
+}
+
+/*
+ * @brief Initialize the I2C device.
+ * @param hi2c: Pointer to a struct i2c_handle_s structure that contains
+ * the configuration information for the specified I2C.
+ * @param init_data: Initialization configuration structure
+ * @retval 0 if OK, negative value else
+ */
+int stm32_i2c_init(struct i2c_handle_s *hi2c,
+ struct stm32_i2c_init_s *init_data)
+{
+ int rc = 0;
+ uint32_t timing = I2C_TIMING;
+
+ if (hi2c == NULL) {
+ return -ENOENT;
+ }
+
+ if (hi2c->i2c_state == I2C_STATE_RESET) {
+ hi2c->lock = 0;
+ }
+
+ hi2c->i2c_state = I2C_STATE_BUSY;
+
+ clk_enable(hi2c->clock);
+
+ /* Disable the selected I2C peripheral */
+ mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR1, I2C_CR1_PE);
+
+ /* Configure I2Cx: Frequency range */
+ mmio_write_32(hi2c->i2c_base_addr + I2C_TIMINGR,
+ timing & TIMINGR_CLEAR_MASK);
+
+ /* Disable Own Address1 before set the Own Address1 configuration */
+ mmio_clrbits_32(hi2c->i2c_base_addr + I2C_OAR1, I2C_OAR1_OA1EN);
+
+ /* Configure I2Cx: Own Address1 and ack own address1 mode */
+ if (init_data->addressing_mode == I2C_ADDRESSINGMODE_7BIT) {
+ mmio_write_32(hi2c->i2c_base_addr + I2C_OAR1,
+ I2C_OAR1_OA1EN | init_data->own_address1);
+ } else { /* I2C_ADDRESSINGMODE_10BIT */
+ mmio_write_32(hi2c->i2c_base_addr + I2C_OAR1,
+ I2C_OAR1_OA1EN | I2C_OAR1_OA1MODE |
+ init_data->own_address1);
+ }
+
+ mmio_write_32(hi2c->i2c_base_addr + I2C_CR2, 0);
+
+ /* Configure I2Cx: Addressing Master mode */
+ if (init_data->addressing_mode == I2C_ADDRESSINGMODE_10BIT) {
+ mmio_setbits_32(hi2c->i2c_base_addr + I2C_CR2, I2C_CR2_ADD10);
+ }
+
+ /*
+ * Enable the AUTOEND by default, and enable NACK
+ * (should be disabled only during Slave process).
+ */
+ mmio_setbits_32(hi2c->i2c_base_addr + I2C_CR2,
+ I2C_CR2_AUTOEND | I2C_CR2_NACK);
+
+ /* Disable Own Address2 before set the Own Address2 configuration */
+ mmio_clrbits_32(hi2c->i2c_base_addr + I2C_OAR2, I2C_DUALADDRESS_ENABLE);
+
+ /* Configure I2Cx: Dual mode and Own Address2 */
+ mmio_write_32(hi2c->i2c_base_addr + I2C_OAR2,
+ init_data->dual_address_mode |
+ init_data->own_address2 |
+ (init_data->own_address2_masks << 8));
+
+ /* Configure I2Cx: Generalcall and NoStretch mode */
+ mmio_write_32(hi2c->i2c_base_addr + I2C_CR1,
+ init_data->general_call_mode |
+ init_data->no_stretch_mode);
+
+ /* Enable the selected I2C peripheral */
+ mmio_setbits_32(hi2c->i2c_base_addr + I2C_CR1, I2C_CR1_PE);
+
+ hi2c->i2c_err = I2C_ERROR_NONE;
+ hi2c->i2c_state = I2C_STATE_READY;
+ hi2c->i2c_mode = I2C_MODE_NONE;
+
+ rc = i2c_config_analog_filter(hi2c, init_data->analog_filter ?
+ I2C_ANALOGFILTER_ENABLE :
+ I2C_ANALOGFILTER_DISABLE);
+ if (rc != 0) {
+ ERROR("Cannot initialize I2C analog filter (%d)\n", rc);
+ clk_disable(hi2c->clock);
+ return rc;
+ }
+
+ clk_disable(hi2c->clock);
+
+ return rc;
+}
+
+/*
+ * @brief I2C Tx data register flush process.
+ * @param hi2c: I2C handle
+ * @retval None
+ */
+static void i2c_flush_txdr(struct i2c_handle_s *hi2c)
+{
+ /*
+ * If a pending TXIS flag is set,
+ * write a dummy data in TXDR to clear it.
+ */
+ if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) & I2C_FLAG_TXIS) !=
+ 0U) {
+ mmio_write_32(hi2c->i2c_base_addr + I2C_TXDR, 0);
+ }
+
+ /* Flush TX register if not empty */
+ if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) & I2C_FLAG_TXE) ==
+ 0U) {
+ mmio_setbits_32(hi2c->i2c_base_addr + I2C_ISR,
+ I2C_FLAG_TXE);
+ }
+}
+
+/*
+ * @brief This function handles I2C Communication timeout.
+ * @param hi2c: Pointer to a struct i2c_handle_s structure that contains
+ * the configuration information for the specified I2C.
+ * @param flag: Specifies the I2C flag to check
+ * @param awaited_value: The awaited bit value for the flag (0 or 1)
+ * @param timeout_ref: Reference to target timeout
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_wait_flag(struct i2c_handle_s *hi2c, uint32_t flag,
+ uint8_t awaited_value, uint64_t timeout_ref)
+{
+ for ( ; ; ) {
+ uint32_t isr = mmio_read_32(hi2c->i2c_base_addr + I2C_ISR);
+
+ if (!!(isr & flag) != !!awaited_value) {
+ return 0;
+ }
+
+ if (timeout_elapsed(timeout_ref)) {
+ notif_i2c_timeout(hi2c);
+ hi2c->lock = 0;
+
+ return -EIO;
+ }
+ }
+}
+
+/*
+ * @brief This function handles Acknowledge failed detection during
+ * an I2C Communication.
+ * @param hi2c: Pointer to a struct i2c_handle_s structure that contains
+ * the configuration information for the specified I2C.
+ * @param timeout_ref: Reference to target timeout
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_ack_failed(struct i2c_handle_s *hi2c, uint64_t timeout_ref)
+{
+ if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) & I2C_FLAG_AF) == 0U) {
+ return 0;
+ }
+
+ /*
+ * Wait until STOP Flag is reset.
+ * AutoEnd should be initiate after AF.
+ */
+ while ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
+ I2C_FLAG_STOPF) == 0U) {
+ if (timeout_elapsed(timeout_ref)) {
+ notif_i2c_timeout(hi2c);
+ hi2c->lock = 0;
+
+ return -EIO;
+ }
+ }
+
+ mmio_write_32(hi2c->i2c_base_addr + I2C_ICR, I2C_FLAG_AF);
+
+ mmio_write_32(hi2c->i2c_base_addr + I2C_ICR, I2C_FLAG_STOPF);
+
+ i2c_flush_txdr(hi2c);
+
+ mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR2, I2C_RESET_CR2);
+
+ hi2c->i2c_err |= I2C_ERROR_AF;
+ hi2c->i2c_state = I2C_STATE_READY;
+ hi2c->i2c_mode = I2C_MODE_NONE;
+
+ hi2c->lock = 0;
+
+ return -EIO;
+}
+
+/*
+ * @brief This function handles I2C Communication timeout for specific usage
+ * of TXIS flag.
+ * @param hi2c: Pointer to a struct i2c_handle_s structure that contains
+ * the configuration information for the specified I2C.
+ * @param timeout_ref: Reference to target timeout
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_wait_txis(struct i2c_handle_s *hi2c, uint64_t timeout_ref)
+{
+ while ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
+ I2C_FLAG_TXIS) == 0U) {
+ if (i2c_ack_failed(hi2c, timeout_ref) != 0) {
+ return -EIO;
+ }
+
+ if (timeout_elapsed(timeout_ref)) {
+ notif_i2c_timeout(hi2c);
+ hi2c->lock = 0;
+
+ return -EIO;
+ }
+ }
+
+ return 0;
+}
+
+/*
+ * @brief This function handles I2C Communication timeout for specific
+ * usage of STOP flag.
+ * @param hi2c: Pointer to a struct i2c_handle_s structure that contains
+ * the configuration information for the specified I2C.
+ * @param timeout_ref: Reference to target timeout
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_wait_stop(struct i2c_handle_s *hi2c, uint64_t timeout_ref)
+{
+ while ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
+ I2C_FLAG_STOPF) == 0U) {
+ if (i2c_ack_failed(hi2c, timeout_ref) != 0) {
+ return -EIO;
+ }
+
+ if (timeout_elapsed(timeout_ref)) {
+ notif_i2c_timeout(hi2c);
+ hi2c->lock = 0;
+
+ return -EIO;
+ }
+ }
+
+ return 0;
+}
+
+/*
+ * @brief Handles I2Cx communication when starting transfer or during transfer
+ * (TC or TCR flag are set).
+ * @param hi2c: I2C handle
+ * @param dev_addr: Specifies the slave address to be programmed
+ * @param size: Specifies the number of bytes to be programmed.
+ * This parameter must be a value between 0 and 255.
+ * @param i2c_mode: New state of the I2C START condition generation.
+ * This parameter can be one of the following values:
+ * @arg @ref I2C_RELOAD_MODE: Enable Reload mode.
+ * @arg @ref I2C_AUTOEND_MODE: Enable Automatic end mode.
+ * @arg @ref I2C_SOFTEND_MODE: Enable Software end mode.
+ * @param request: New state of the I2C START condition generation.
+ * This parameter can be one of the following values:
+ * @arg @ref I2C_NO_STARTSTOP: Don't Generate stop and start condition.
+ * @arg @ref I2C_GENERATE_STOP: Generate stop condition
+ * (size should be set to 0).
+ * @arg @ref I2C_GENERATE_START_READ: Generate Restart for read request.
+ * @arg @ref I2C_GENERATE_START_WRITE: Generate Restart for write request.
+ * @retval None
+ */
+static void i2c_transfer_config(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+ uint16_t size, uint32_t i2c_mode,
+ uint32_t request)
+{
+ uint32_t clr_value, set_value;
+
+ clr_value = (I2C_CR2_SADD | I2C_CR2_NBYTES | I2C_CR2_RELOAD |
+ I2C_CR2_AUTOEND | I2C_CR2_START | I2C_CR2_STOP) |
+ (I2C_CR2_RD_WRN & (request >> (31U - I2C_CR2_RD_WRN_OFFSET)));
+
+ set_value = ((uint32_t)dev_addr & I2C_CR2_SADD) |
+ (((uint32_t)size << I2C_CR2_NBYTES_OFFSET) & I2C_CR2_NBYTES) |
+ i2c_mode | request;
+
+ mmio_clrsetbits_32(hi2c->i2c_base_addr + I2C_CR2, clr_value, set_value);
+}
+
+/*
+ * @brief Master sends target device address followed by internal memory
+ * address for write request.
+ * @param hi2c: Pointer to a struct i2c_handle_s structure that contains
+ * the configuration information for the specified I2C.
+ * @param dev_addr: Target device address
+ * @param mem_addr: Internal memory address
+ * @param mem_add_size: Size of internal memory address
+ * @param timeout_ref: Reference to target timeout
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_request_memory_write(struct i2c_handle_s *hi2c,
+ uint16_t dev_addr, uint16_t mem_addr,
+ uint16_t mem_add_size, uint64_t timeout_ref)
+{
+ i2c_transfer_config(hi2c, dev_addr, mem_add_size, I2C_RELOAD_MODE,
+ I2C_GENERATE_START_WRITE);
+
+ if (i2c_wait_txis(hi2c, timeout_ref) != 0) {
+ return -EIO;
+ }
+
+ if (mem_add_size == I2C_MEMADD_SIZE_8BIT) {
+ /* Send Memory Address */
+ mmio_write_8(hi2c->i2c_base_addr + I2C_TXDR,
+ (uint8_t)(mem_addr & 0x00FFU));
+ } else {
+ /* Send MSB of Memory Address */
+ mmio_write_8(hi2c->i2c_base_addr + I2C_TXDR,
+ (uint8_t)((mem_addr & 0xFF00U) >> 8));
+
+ if (i2c_wait_txis(hi2c, timeout_ref) != 0) {
+ return -EIO;
+ }
+
+ /* Send LSB of Memory Address */
+ mmio_write_8(hi2c->i2c_base_addr + I2C_TXDR,
+ (uint8_t)(mem_addr & 0x00FFU));
+ }
+
+ if (i2c_wait_flag(hi2c, I2C_FLAG_TCR, 0, timeout_ref) != 0) {
+ return -EIO;
+ }
+
+ return 0;
+}
+
+/*
+ * @brief Master sends target device address followed by internal memory
+ * address for read request.
+ * @param hi2c: Pointer to a struct i2c_handle_s structure that contains
+ * the configuration information for the specified I2C.
+ * @param dev_addr: Target device address
+ * @param mem_addr: Internal memory address
+ * @param mem_add_size: Size of internal memory address
+ * @param timeout_ref: Reference to target timeout
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_request_memory_read(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+ uint16_t mem_addr, uint16_t mem_add_size,
+ uint64_t timeout_ref)
+{
+ i2c_transfer_config(hi2c, dev_addr, mem_add_size, I2C_SOFTEND_MODE,
+ I2C_GENERATE_START_WRITE);
+
+ if (i2c_wait_txis(hi2c, timeout_ref) != 0) {
+ return -EIO;
+ }
+
+ if (mem_add_size == I2C_MEMADD_SIZE_8BIT) {
+ /* Send Memory Address */
+ mmio_write_8(hi2c->i2c_base_addr + I2C_TXDR,
+ (uint8_t)(mem_addr & 0x00FFU));
+ } else {
+ /* Send MSB of Memory Address */
+ mmio_write_8(hi2c->i2c_base_addr + I2C_TXDR,
+ (uint8_t)((mem_addr & 0xFF00U) >> 8));
+
+ if (i2c_wait_txis(hi2c, timeout_ref) != 0) {
+ return -EIO;
+ }
+
+ /* Send LSB of Memory Address */
+ mmio_write_8(hi2c->i2c_base_addr + I2C_TXDR,
+ (uint8_t)(mem_addr & 0x00FFU));
+ }
+
+ if (i2c_wait_flag(hi2c, I2C_FLAG_TC, 0, timeout_ref) != 0) {
+ return -EIO;
+ }
+
+ return 0;
+}
+/*
+ * @brief Generic function to write an amount of data in blocking mode
+ * (for Memory Mode and Master Mode)
+ * @param hi2c: Pointer to a struct i2c_handle_s structure that contains
+ * the configuration information for the specified I2C.
+ * @param dev_addr: Target device address
+ * @param mem_addr: Internal memory address (if Memory Mode)
+ * @param mem_add_size: Size of internal memory address (if Memory Mode)
+ * @param p_data: Pointer to data buffer
+ * @param size: Amount of data to be sent
+ * @param timeout_ms: Timeout duration in milliseconds
+ * @param mode: Communication mode
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_write(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+ uint16_t mem_addr, uint16_t mem_add_size,
+ uint8_t *p_data, uint16_t size, uint32_t timeout_ms,
+ enum i2c_mode_e mode)
+{
+ uint64_t timeout_ref;
+ int rc = -EIO;
+ uint8_t *p_buff = p_data;
+ uint32_t xfer_size;
+ uint32_t xfer_count = size;
+
+ if ((mode != I2C_MODE_MASTER) && (mode != I2C_MODE_MEM)) {
+ return -1;
+ }
+
+ if ((hi2c->i2c_state != I2C_STATE_READY) || (hi2c->lock != 0U)) {
+ return -EBUSY;
+ }
+
+ if ((p_data == NULL) || (size == 0U)) {
+ return -EINVAL;
+ }
+
+ clk_enable(hi2c->clock);
+
+ hi2c->lock = 1;
+
+ timeout_ref = timeout_init_us(I2C_TIMEOUT_BUSY_MS * 1000);
+ if (i2c_wait_flag(hi2c, I2C_FLAG_BUSY, 1, timeout_ref) != 0) {
+ goto bail;
+ }
+
+ hi2c->i2c_state = I2C_STATE_BUSY_TX;
+ hi2c->i2c_mode = mode;
+ hi2c->i2c_err = I2C_ERROR_NONE;
+
+ timeout_ref = timeout_init_us(timeout_ms * 1000);
+
+ if (mode == I2C_MODE_MEM) {
+ /* In Memory Mode, Send Slave Address and Memory Address */
+ if (i2c_request_memory_write(hi2c, dev_addr, mem_addr,
+ mem_add_size, timeout_ref) != 0) {
+ goto bail;
+ }
+
+ if (xfer_count > MAX_NBYTE_SIZE) {
+ xfer_size = MAX_NBYTE_SIZE;
+ i2c_transfer_config(hi2c, dev_addr, xfer_size,
+ I2C_RELOAD_MODE, I2C_NO_STARTSTOP);
+ } else {
+ xfer_size = xfer_count;
+ i2c_transfer_config(hi2c, dev_addr, xfer_size,
+ I2C_AUTOEND_MODE, I2C_NO_STARTSTOP);
+ }
+ } else {
+ /* In Master Mode, Send Slave Address */
+ if (xfer_count > MAX_NBYTE_SIZE) {
+ xfer_size = MAX_NBYTE_SIZE;
+ i2c_transfer_config(hi2c, dev_addr, xfer_size,
+ I2C_RELOAD_MODE,
+ I2C_GENERATE_START_WRITE);
+ } else {
+ xfer_size = xfer_count;
+ i2c_transfer_config(hi2c, dev_addr, xfer_size,
+ I2C_AUTOEND_MODE,
+ I2C_GENERATE_START_WRITE);
+ }
+ }
+
+ do {
+ if (i2c_wait_txis(hi2c, timeout_ref) != 0) {
+ goto bail;
+ }
+
+ mmio_write_8(hi2c->i2c_base_addr + I2C_TXDR, *p_buff);
+ p_buff++;
+ xfer_count--;
+ xfer_size--;
+
+ if ((xfer_count != 0U) && (xfer_size == 0U)) {
+ /* Wait until TCR flag is set */
+ if (i2c_wait_flag(hi2c, I2C_FLAG_TCR, 0,
+ timeout_ref) != 0) {
+ goto bail;
+ }
+
+ if (xfer_count > MAX_NBYTE_SIZE) {
+ xfer_size = MAX_NBYTE_SIZE;
+ i2c_transfer_config(hi2c, dev_addr,
+ xfer_size,
+ I2C_RELOAD_MODE,
+ I2C_NO_STARTSTOP);
+ } else {
+ xfer_size = xfer_count;
+ i2c_transfer_config(hi2c, dev_addr,
+ xfer_size,
+ I2C_AUTOEND_MODE,
+ I2C_NO_STARTSTOP);
+ }
+ }
+
+ } while (xfer_count > 0U);
+
+ /*
+ * No need to Check TC flag, with AUTOEND mode the stop
+ * is automatically generated.
+ * Wait until STOPF flag is reset.
+ */
+ if (i2c_wait_stop(hi2c, timeout_ref) != 0) {
+ goto bail;
+ }
+
+ mmio_write_32(hi2c->i2c_base_addr + I2C_ICR, I2C_FLAG_STOPF);
+
+ mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR2, I2C_RESET_CR2);
+
+ hi2c->i2c_state = I2C_STATE_READY;
+ hi2c->i2c_mode = I2C_MODE_NONE;
+
+ rc = 0;
+
+bail:
+ hi2c->lock = 0;
+ clk_disable(hi2c->clock);
+
+ return rc;
+}
+
+/*
+ * @brief Write an amount of data in blocking mode to a specific memory
+ * address.
+ * @param hi2c: Pointer to a struct i2c_handle_s structure that contains
+ * the configuration information for the specified I2C.
+ * @param dev_addr: Target device address
+ * @param mem_addr: Internal memory address
+ * @param mem_add_size: Size of internal memory address
+ * @param p_data: Pointer to data buffer
+ * @param size: Amount of data to be sent
+ * @param timeout_ms: Timeout duration in milliseconds
+ * @retval 0 if OK, negative value else
+ */
+int stm32_i2c_mem_write(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+ uint16_t mem_addr, uint16_t mem_add_size,
+ uint8_t *p_data, uint16_t size, uint32_t timeout_ms)
+{
+ return i2c_write(hi2c, dev_addr, mem_addr, mem_add_size,
+ p_data, size, timeout_ms, I2C_MODE_MEM);
+}
+
+/*
+ * @brief Transmits in master mode an amount of data in blocking mode.
+ * @param hi2c: Pointer to a struct i2c_handle_s structure that contains
+ * the configuration information for the specified I2C.
+ * @param dev_addr: Target device address
+ * @param p_data: Pointer to data buffer
+ * @param size: Amount of data to be sent
+ * @param timeout_ms: Timeout duration in milliseconds
+ * @retval 0 if OK, negative value else
+ */
+int stm32_i2c_master_transmit(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+ uint8_t *p_data, uint16_t size,
+ uint32_t timeout_ms)
+{
+ return i2c_write(hi2c, dev_addr, 0, 0,
+ p_data, size, timeout_ms, I2C_MODE_MASTER);
+}
+
+/*
+ * @brief Generic function to read an amount of data in blocking mode
+ * (for Memory Mode and Master Mode)
+ * @param hi2c: Pointer to a struct i2c_handle_s structure that contains
+ * the configuration information for the specified I2C.
+ * @param dev_addr: Target device address
+ * @param mem_addr: Internal memory address (if Memory Mode)
+ * @param mem_add_size: Size of internal memory address (if Memory Mode)
+ * @param p_data: Pointer to data buffer
+ * @param size: Amount of data to be sent
+ * @param timeout_ms: Timeout duration in milliseconds
+ * @param mode: Communication mode
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_read(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+ uint16_t mem_addr, uint16_t mem_add_size,
+ uint8_t *p_data, uint16_t size, uint32_t timeout_ms,
+ enum i2c_mode_e mode)
+{
+ uint64_t timeout_ref;
+ int rc = -EIO;
+ uint8_t *p_buff = p_data;
+ uint32_t xfer_count = size;
+ uint32_t xfer_size;
+
+ if ((mode != I2C_MODE_MASTER) && (mode != I2C_MODE_MEM)) {
+ return -1;
+ }
+
+ if ((hi2c->i2c_state != I2C_STATE_READY) || (hi2c->lock != 0U)) {
+ return -EBUSY;
+ }
+
+ if ((p_data == NULL) || (size == 0U)) {
+ return -EINVAL;
+ }
+
+ clk_enable(hi2c->clock);
+
+ hi2c->lock = 1;
+
+ timeout_ref = timeout_init_us(I2C_TIMEOUT_BUSY_MS * 1000);
+ if (i2c_wait_flag(hi2c, I2C_FLAG_BUSY, 1, timeout_ref) != 0) {
+ goto bail;
+ }
+
+ hi2c->i2c_state = I2C_STATE_BUSY_RX;
+ hi2c->i2c_mode = mode;
+ hi2c->i2c_err = I2C_ERROR_NONE;
+
+ if (mode == I2C_MODE_MEM) {
+ /* Send Memory Address */
+ if (i2c_request_memory_read(hi2c, dev_addr, mem_addr,
+ mem_add_size, timeout_ref) != 0) {
+ goto bail;
+ }
+ }
+
+ /*
+ * Send Slave Address.
+ * Set NBYTES to write and reload if xfer_count > MAX_NBYTE_SIZE
+ * and generate RESTART.
+ */
+ if (xfer_count > MAX_NBYTE_SIZE) {
+ xfer_size = MAX_NBYTE_SIZE;
+ i2c_transfer_config(hi2c, dev_addr, xfer_size,
+ I2C_RELOAD_MODE, I2C_GENERATE_START_READ);
+ } else {
+ xfer_size = xfer_count;
+ i2c_transfer_config(hi2c, dev_addr, xfer_size,
+ I2C_AUTOEND_MODE, I2C_GENERATE_START_READ);
+ }
+
+ do {
+ if (i2c_wait_flag(hi2c, I2C_FLAG_RXNE, 0, timeout_ref) != 0) {
+ goto bail;
+ }
+
+ *p_buff = mmio_read_8(hi2c->i2c_base_addr + I2C_RXDR);
+ p_buff++;
+ xfer_size--;
+ xfer_count--;
+
+ if ((xfer_count != 0U) && (xfer_size == 0U)) {
+ if (i2c_wait_flag(hi2c, I2C_FLAG_TCR, 0,
+ timeout_ref) != 0) {
+ goto bail;
+ }
+
+ if (xfer_count > MAX_NBYTE_SIZE) {
+ xfer_size = MAX_NBYTE_SIZE;
+ i2c_transfer_config(hi2c, dev_addr,
+ xfer_size,
+ I2C_RELOAD_MODE,
+ I2C_NO_STARTSTOP);
+ } else {
+ xfer_size = xfer_count;
+ i2c_transfer_config(hi2c, dev_addr,
+ xfer_size,
+ I2C_AUTOEND_MODE,
+ I2C_NO_STARTSTOP);
+ }
+ }
+ } while (xfer_count > 0U);
+
+ /*
+ * No need to Check TC flag, with AUTOEND mode the stop
+ * is automatically generated.
+ * Wait until STOPF flag is reset.
+ */
+ if (i2c_wait_stop(hi2c, timeout_ref) != 0) {
+ goto bail;
+ }
+
+ mmio_write_32(hi2c->i2c_base_addr + I2C_ICR, I2C_FLAG_STOPF);
+
+ mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR2, I2C_RESET_CR2);
+
+ hi2c->i2c_state = I2C_STATE_READY;
+ hi2c->i2c_mode = I2C_MODE_NONE;
+
+ rc = 0;
+
+bail:
+ hi2c->lock = 0;
+ clk_disable(hi2c->clock);
+
+ return rc;
+}
+
+/*
+ * @brief Read an amount of data in blocking mode from a specific memory
+ * address.
+ * @param hi2c: Pointer to a struct i2c_handle_s structure that contains
+ * the configuration information for the specified I2C.
+ * @param dev_addr: Target device address
+ * @param mem_addr: Internal memory address
+ * @param mem_add_size: Size of internal memory address
+ * @param p_data: Pointer to data buffer
+ * @param size: Amount of data to be sent
+ * @param timeout_ms: Timeout duration in milliseconds
+ * @retval 0 if OK, negative value else
+ */
+int stm32_i2c_mem_read(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+ uint16_t mem_addr, uint16_t mem_add_size,
+ uint8_t *p_data, uint16_t size, uint32_t timeout_ms)
+{
+ return i2c_read(hi2c, dev_addr, mem_addr, mem_add_size,
+ p_data, size, timeout_ms, I2C_MODE_MEM);
+}
+
+/*
+ * @brief Receives in master mode an amount of data in blocking mode.
+ * @param hi2c: Pointer to a struct i2c_handle_s structure that contains
+ * the configuration information for the specified I2C.
+ * @param dev_addr: Target device address
+ * @param p_data: Pointer to data buffer
+ * @param size: Amount of data to be sent
+ * @param timeout_ms: Timeout duration in milliseconds
+ * @retval 0 if OK, negative value else
+ */
+int stm32_i2c_master_receive(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+ uint8_t *p_data, uint16_t size,
+ uint32_t timeout_ms)
+{
+ return i2c_read(hi2c, dev_addr, 0, 0,
+ p_data, size, timeout_ms, I2C_MODE_MASTER);
+}
+
+/*
+ * @brief Checks if target device is ready for communication.
+ * @note This function is used with Memory devices
+ * @param hi2c: Pointer to a struct i2c_handle_s structure that contains
+ * the configuration information for the specified I2C.
+ * @param dev_addr: Target device address
+ * @param trials: Number of trials
+ * @param timeout_ms: Timeout duration in milliseconds
+ * @retval True if device is ready, false else
+ */
+bool stm32_i2c_is_device_ready(struct i2c_handle_s *hi2c,
+ uint16_t dev_addr, uint32_t trials,
+ uint32_t timeout_ms)
+{
+ uint32_t i2c_trials = 0U;
+ bool rc = false;
+
+ if ((hi2c->i2c_state != I2C_STATE_READY) || (hi2c->lock != 0U)) {
+ return rc;
+ }
+
+ clk_enable(hi2c->clock);
+
+ hi2c->lock = 1;
+ hi2c->i2c_mode = I2C_MODE_NONE;
+
+ if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) & I2C_FLAG_BUSY) !=
+ 0U) {
+ goto bail;
+ }
+
+ hi2c->i2c_state = I2C_STATE_BUSY;
+ hi2c->i2c_err = I2C_ERROR_NONE;
+
+ do {
+ uint64_t timeout_ref;
+
+ /* Generate Start */
+ if ((mmio_read_32(hi2c->i2c_base_addr + I2C_OAR1) &
+ I2C_OAR1_OA1MODE) == 0) {
+ mmio_write_32(hi2c->i2c_base_addr + I2C_CR2,
+ (((uint32_t)dev_addr & I2C_CR2_SADD) |
+ I2C_CR2_START | I2C_CR2_AUTOEND) &
+ ~I2C_CR2_RD_WRN);
+ } else {
+ mmio_write_32(hi2c->i2c_base_addr + I2C_CR2,
+ (((uint32_t)dev_addr & I2C_CR2_SADD) |
+ I2C_CR2_START | I2C_CR2_ADD10) &
+ ~I2C_CR2_RD_WRN);
+ }
+
+ /*
+ * No need to Check TC flag, with AUTOEND mode the stop
+ * is automatically generated.
+ * Wait until STOPF flag is set or a NACK flag is set.
+ */
+ timeout_ref = timeout_init_us(timeout_ms * 1000);
+ do {
+ if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
+ (I2C_FLAG_STOPF | I2C_FLAG_AF)) != 0U) {
+ break;
+ }
+
+ if (timeout_elapsed(timeout_ref)) {
+ notif_i2c_timeout(hi2c);
+ goto bail;
+ }
+ } while (true);
+
+ if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
+ I2C_FLAG_AF) == 0U) {
+ if (i2c_wait_flag(hi2c, I2C_FLAG_STOPF, 0,
+ timeout_ref) != 0) {
+ goto bail;
+ }
+
+ mmio_write_32(hi2c->i2c_base_addr + I2C_ICR,
+ I2C_FLAG_STOPF);
+
+ hi2c->i2c_state = I2C_STATE_READY;
+
+ rc = true;
+ goto bail;
+ }
+
+ if (i2c_wait_flag(hi2c, I2C_FLAG_STOPF, 0, timeout_ref) != 0) {
+ goto bail;
+ }
+
+ mmio_write_32(hi2c->i2c_base_addr + I2C_ICR, I2C_FLAG_AF);
+
+ mmio_write_32(hi2c->i2c_base_addr + I2C_ICR, I2C_FLAG_STOPF);
+
+ if (i2c_trials == trials) {
+ mmio_setbits_32(hi2c->i2c_base_addr + I2C_CR2,
+ I2C_CR2_STOP);
+
+ if (i2c_wait_flag(hi2c, I2C_FLAG_STOPF, 0,
+ timeout_ref) != 0) {
+ goto bail;
+ }
+
+ mmio_write_32(hi2c->i2c_base_addr + I2C_ICR,
+ I2C_FLAG_STOPF);
+ }
+
+ i2c_trials++;
+ } while (i2c_trials < trials);
+
+ notif_i2c_timeout(hi2c);
+
+bail:
+ hi2c->lock = 0;
+ clk_disable(hi2c->clock);
+
+ return rc;
+}
+
diff --git a/drivers/st/io/io_mmc.c b/drivers/st/io/io_mmc.c
new file mode 100644
index 0000000..2bf88e6
--- /dev/null
+++ b/drivers/st/io/io_mmc.c
@@ -0,0 +1,143 @@
+/*
+ * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <drivers/io/io_driver.h>
+#include <drivers/io/io_storage.h>
+#include <drivers/mmc.h>
+#include <drivers/st/io_mmc.h>
+#include <drivers/st/stm32_sdmmc2.h>
+
+/* SDMMC device functions */
+static int mmc_dev_open(const uintptr_t init_params, io_dev_info_t **dev_info);
+static int mmc_block_open(io_dev_info_t *dev_info, const uintptr_t spec,
+ io_entity_t *entity);
+static int mmc_dev_init(io_dev_info_t *dev_info, const uintptr_t init_params);
+static int mmc_block_seek(io_entity_t *entity, int mode,
+ signed long long offset);
+static int mmc_block_read(io_entity_t *entity, uintptr_t buffer, size_t length,
+ size_t *length_read);
+static int mmc_block_close(io_entity_t *entity);
+static int mmc_dev_close(io_dev_info_t *dev_info);
+static io_type_t device_type_mmc(void);
+
+static signed long long seek_offset;
+static size_t (*_read_blocks)(int lba, uintptr_t buf, size_t size);
+
+static const io_dev_connector_t mmc_dev_connector = {
+ .dev_open = mmc_dev_open
+};
+
+static const io_dev_funcs_t mmc_dev_funcs = {
+ .type = device_type_mmc,
+ .open = mmc_block_open,
+ .seek = mmc_block_seek,
+ .size = NULL,
+ .read = mmc_block_read,
+ .write = NULL,
+ .close = mmc_block_close,
+ .dev_init = mmc_dev_init,
+ .dev_close = mmc_dev_close,
+};
+
+static const io_dev_info_t mmc_dev_info = {
+ .funcs = &mmc_dev_funcs,
+ .info = 0,
+};
+
+/* Identify the device type as mmc device */
+static io_type_t device_type_mmc(void)
+{
+ return IO_TYPE_MMC;
+}
+
+/* Open a connection to the mmc device */
+static int mmc_dev_open(const uintptr_t init_params, io_dev_info_t **dev_info)
+{
+ struct io_mmc_dev_spec *device_spec =
+ (struct io_mmc_dev_spec *)init_params;
+
+ assert(dev_info != NULL);
+ *dev_info = (io_dev_info_t *)&mmc_dev_info;
+
+ _read_blocks = !device_spec->use_boot_part ?
+ mmc_read_blocks : mmc_boot_part_read_blocks;
+
+ return 0;
+}
+
+static int mmc_dev_init(io_dev_info_t *dev_info, const uintptr_t init_params)
+{
+ return 0;
+}
+
+/* Close a connection to the mmc device */
+static int mmc_dev_close(io_dev_info_t *dev_info)
+{
+ return 0;
+}
+
+/* Open a file on the mmc device */
+static int mmc_block_open(io_dev_info_t *dev_info, const uintptr_t spec,
+ io_entity_t *entity)
+{
+ seek_offset = 0;
+ return 0;
+}
+
+/* Seek to a particular file offset on the mmc device */
+static int mmc_block_seek(io_entity_t *entity, int mode,
+ signed long long offset)
+{
+ seek_offset = offset;
+ return 0;
+}
+
+/* Read data from a file on the mmc device */
+static int mmc_block_read(io_entity_t *entity, uintptr_t buffer,
+ size_t length, size_t *length_read)
+{
+ uint8_t retries;
+
+ for (retries = 0U; retries < 3U; retries++) {
+ *length_read = _read_blocks(seek_offset / MMC_BLOCK_SIZE,
+ buffer, length);
+
+ if (*length_read == length) {
+ return 0;
+ }
+ WARN("%s: length_read = %lu (!= %lu), retry %u\n", __func__,
+ (unsigned long)*length_read, (unsigned long)length,
+ retries + 1U);
+ }
+
+ return -EIO;
+}
+
+/* Close a file on the mmc device */
+static int mmc_block_close(io_entity_t *entity)
+{
+ return 0;
+}
+
+/* Register the mmc driver with the IO abstraction */
+int register_io_dev_mmc(const io_dev_connector_t **dev_con)
+{
+ int result;
+
+ assert(dev_con != NULL);
+
+ result = io_register_device(&mmc_dev_info);
+ if (result == 0) {
+ *dev_con = &mmc_dev_connector;
+ }
+
+ return result;
+}
diff --git a/drivers/st/iwdg/stm32_iwdg.c b/drivers/st/iwdg/stm32_iwdg.c
new file mode 100644
index 0000000..74451d7
--- /dev/null
+++ b/drivers/st/iwdg/stm32_iwdg.c
@@ -0,0 +1,157 @@
+/*
+ * Copyright (c) 2017-2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/arm/gicv2.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32_iwdg.h>
+#include <drivers/st/stm32mp_clkfunc.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+#include <plat/common/platform.h>
+
+/* IWDG registers offsets */
+#define IWDG_KR_OFFSET 0x00U
+
+/* Registers values */
+#define IWDG_KR_RELOAD_KEY 0xAAAA
+
+struct stm32_iwdg_instance {
+ uintptr_t base;
+ unsigned long clock;
+ uint8_t flags;
+ int num_irq;
+};
+
+static struct stm32_iwdg_instance stm32_iwdg[IWDG_MAX_INSTANCE];
+
+static int stm32_iwdg_get_dt_node(struct dt_node_info *info, int offset)
+{
+ int node;
+
+ node = dt_get_node(info, offset, DT_IWDG_COMPAT);
+ if (node < 0) {
+ if (offset == -1) {
+ VERBOSE("%s: No IDWG found\n", __func__);
+ }
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ return node;
+}
+
+void stm32_iwdg_refresh(void)
+{
+ uint8_t i;
+
+ for (i = 0U; i < IWDG_MAX_INSTANCE; i++) {
+ struct stm32_iwdg_instance *iwdg = &stm32_iwdg[i];
+
+ /* 0x00000000 is not a valid address for IWDG peripherals */
+ if (iwdg->base != 0U) {
+ clk_enable(iwdg->clock);
+
+ mmio_write_32(iwdg->base + IWDG_KR_OFFSET,
+ IWDG_KR_RELOAD_KEY);
+
+ clk_disable(iwdg->clock);
+ }
+ }
+}
+
+int stm32_iwdg_init(void)
+{
+ int node = -1;
+ struct dt_node_info dt_info;
+ void *fdt;
+ uint32_t __unused count = 0;
+
+ if (fdt_get_address(&fdt) == 0) {
+ panic();
+ }
+
+ for (node = stm32_iwdg_get_dt_node(&dt_info, node);
+ node != -FDT_ERR_NOTFOUND;
+ node = stm32_iwdg_get_dt_node(&dt_info, node)) {
+ struct stm32_iwdg_instance *iwdg;
+ uint32_t hw_init;
+ uint32_t idx;
+
+ count++;
+
+ idx = stm32_iwdg_get_instance(dt_info.base);
+ iwdg = &stm32_iwdg[idx];
+ iwdg->base = dt_info.base;
+ iwdg->clock = (unsigned long)dt_info.clock;
+
+ /* DT can specify low power cases */
+ if (fdt_getprop(fdt, node, "stm32,enable-on-stop", NULL) ==
+ NULL) {
+ iwdg->flags |= IWDG_DISABLE_ON_STOP;
+ }
+
+ if (fdt_getprop(fdt, node, "stm32,enable-on-standby", NULL) ==
+ NULL) {
+ iwdg->flags |= IWDG_DISABLE_ON_STANDBY;
+ }
+
+ /* Explicit list of supported bit flags */
+ hw_init = stm32_iwdg_get_otp_config(idx);
+
+ if ((hw_init & IWDG_HW_ENABLED) != 0) {
+ if (dt_info.status == DT_DISABLED) {
+ ERROR("OTP enabled but iwdg%u DT-disabled\n",
+ idx + 1U);
+ panic();
+ }
+ iwdg->flags |= IWDG_HW_ENABLED;
+ }
+
+ if (dt_info.status == DT_DISABLED) {
+ zeromem((void *)iwdg,
+ sizeof(struct stm32_iwdg_instance));
+ continue;
+ }
+
+ if ((hw_init & IWDG_DISABLE_ON_STOP) != 0) {
+ iwdg->flags |= IWDG_DISABLE_ON_STOP;
+ }
+
+ if ((hw_init & IWDG_DISABLE_ON_STANDBY) != 0) {
+ iwdg->flags |= IWDG_DISABLE_ON_STANDBY;
+ }
+
+ VERBOSE("IWDG%u found, %ssecure\n", idx + 1U,
+ ((dt_info.status & DT_NON_SECURE) != 0) ?
+ "non-" : "");
+
+ if ((dt_info.status & DT_NON_SECURE) != 0) {
+ stm32mp_register_non_secure_periph_iomem(iwdg->base);
+ } else {
+ stm32mp_register_secure_periph_iomem(iwdg->base);
+ }
+
+#if defined(IMAGE_BL2)
+ if (stm32_iwdg_shadow_update(idx, iwdg->flags) != BSEC_OK) {
+ return -1;
+ }
+#endif
+ }
+
+ VERBOSE("%u IWDG instance%s found\n", count, (count > 1U) ? "s" : "");
+
+ return 0;
+}
diff --git a/drivers/st/mmc/stm32_sdmmc2.c b/drivers/st/mmc/stm32_sdmmc2.c
new file mode 100644
index 0000000..6bdd782
--- /dev/null
+++ b/drivers/st/mmc/stm32_sdmmc2.c
@@ -0,0 +1,798 @@
+/*
+ * Copyright (c) 2018-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/mmc.h>
+#include <drivers/st/stm32_gpio.h>
+#include <drivers/st/stm32_sdmmc2.h>
+#include <drivers/st/stm32mp_reset.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+#include <libfdt.h>
+#include <plat/common/platform.h>
+
+#include <platform_def.h>
+
+/* Registers offsets */
+#define SDMMC_POWER 0x00U
+#define SDMMC_CLKCR 0x04U
+#define SDMMC_ARGR 0x08U
+#define SDMMC_CMDR 0x0CU
+#define SDMMC_RESPCMDR 0x10U
+#define SDMMC_RESP1R 0x14U
+#define SDMMC_RESP2R 0x18U
+#define SDMMC_RESP3R 0x1CU
+#define SDMMC_RESP4R 0x20U
+#define SDMMC_DTIMER 0x24U
+#define SDMMC_DLENR 0x28U
+#define SDMMC_DCTRLR 0x2CU
+#define SDMMC_DCNTR 0x30U
+#define SDMMC_STAR 0x34U
+#define SDMMC_ICR 0x38U
+#define SDMMC_MASKR 0x3CU
+#define SDMMC_ACKTIMER 0x40U
+#define SDMMC_IDMACTRLR 0x50U
+#define SDMMC_IDMABSIZER 0x54U
+#define SDMMC_IDMABASE0R 0x58U
+#define SDMMC_IDMABASE1R 0x5CU
+#define SDMMC_FIFOR 0x80U
+
+/* SDMMC power control register */
+#define SDMMC_POWER_PWRCTRL GENMASK(1, 0)
+#define SDMMC_POWER_PWRCTRL_PWR_CYCLE BIT(1)
+#define SDMMC_POWER_DIRPOL BIT(4)
+
+/* SDMMC clock control register */
+#define SDMMC_CLKCR_WIDBUS_4 BIT(14)
+#define SDMMC_CLKCR_WIDBUS_8 BIT(15)
+#define SDMMC_CLKCR_NEGEDGE BIT(16)
+#define SDMMC_CLKCR_HWFC_EN BIT(17)
+#define SDMMC_CLKCR_SELCLKRX_0 BIT(20)
+
+/* SDMMC command register */
+#define SDMMC_CMDR_CMDTRANS BIT(6)
+#define SDMMC_CMDR_CMDSTOP BIT(7)
+#define SDMMC_CMDR_WAITRESP GENMASK(9, 8)
+#define SDMMC_CMDR_WAITRESP_SHORT BIT(8)
+#define SDMMC_CMDR_WAITRESP_SHORT_NOCRC BIT(9)
+#define SDMMC_CMDR_CPSMEN BIT(12)
+
+/* SDMMC data control register */
+#define SDMMC_DCTRLR_DTEN BIT(0)
+#define SDMMC_DCTRLR_DTDIR BIT(1)
+#define SDMMC_DCTRLR_DTMODE GENMASK(3, 2)
+#define SDMMC_DCTRLR_DBLOCKSIZE GENMASK(7, 4)
+#define SDMMC_DCTRLR_DBLOCKSIZE_SHIFT 4
+#define SDMMC_DCTRLR_FIFORST BIT(13)
+
+#define SDMMC_DCTRLR_CLEAR_MASK (SDMMC_DCTRLR_DTEN | \
+ SDMMC_DCTRLR_DTDIR | \
+ SDMMC_DCTRLR_DTMODE | \
+ SDMMC_DCTRLR_DBLOCKSIZE)
+
+/* SDMMC status register */
+#define SDMMC_STAR_CCRCFAIL BIT(0)
+#define SDMMC_STAR_DCRCFAIL BIT(1)
+#define SDMMC_STAR_CTIMEOUT BIT(2)
+#define SDMMC_STAR_DTIMEOUT BIT(3)
+#define SDMMC_STAR_TXUNDERR BIT(4)
+#define SDMMC_STAR_RXOVERR BIT(5)
+#define SDMMC_STAR_CMDREND BIT(6)
+#define SDMMC_STAR_CMDSENT BIT(7)
+#define SDMMC_STAR_DATAEND BIT(8)
+#define SDMMC_STAR_DBCKEND BIT(10)
+#define SDMMC_STAR_DPSMACT BIT(12)
+#define SDMMC_STAR_RXFIFOHF BIT(15)
+#define SDMMC_STAR_RXFIFOE BIT(19)
+#define SDMMC_STAR_IDMATE BIT(27)
+#define SDMMC_STAR_IDMABTC BIT(28)
+
+/* SDMMC DMA control register */
+#define SDMMC_IDMACTRLR_IDMAEN BIT(0)
+
+#define SDMMC_STATIC_FLAGS (SDMMC_STAR_CCRCFAIL | \
+ SDMMC_STAR_DCRCFAIL | \
+ SDMMC_STAR_CTIMEOUT | \
+ SDMMC_STAR_DTIMEOUT | \
+ SDMMC_STAR_TXUNDERR | \
+ SDMMC_STAR_RXOVERR | \
+ SDMMC_STAR_CMDREND | \
+ SDMMC_STAR_CMDSENT | \
+ SDMMC_STAR_DATAEND | \
+ SDMMC_STAR_DBCKEND | \
+ SDMMC_STAR_IDMATE | \
+ SDMMC_STAR_IDMABTC)
+
+#define TIMEOUT_US_1_MS 1000U
+#define TIMEOUT_US_10_MS 10000U
+#define TIMEOUT_US_1_S 1000000U
+
+/* Power cycle delays in ms */
+#define VCC_POWER_OFF_DELAY 2
+#define VCC_POWER_ON_DELAY 2
+#define POWER_CYCLE_DELAY 2
+#define POWER_OFF_DELAY 2
+#define POWER_ON_DELAY 1
+
+#ifndef DT_SDMMC2_COMPAT
+#define DT_SDMMC2_COMPAT "st,stm32-sdmmc2"
+#endif
+
+#define SDMMC_FIFO_SIZE 64U
+
+static void stm32_sdmmc2_init(void);
+static int stm32_sdmmc2_send_cmd_req(struct mmc_cmd *cmd);
+static int stm32_sdmmc2_send_cmd(struct mmc_cmd *cmd);
+static int stm32_sdmmc2_set_ios(unsigned int clk, unsigned int width);
+static int stm32_sdmmc2_prepare(int lba, uintptr_t buf, size_t size);
+static int stm32_sdmmc2_read(int lba, uintptr_t buf, size_t size);
+static int stm32_sdmmc2_write(int lba, uintptr_t buf, size_t size);
+
+static const struct mmc_ops stm32_sdmmc2_ops = {
+ .init = stm32_sdmmc2_init,
+ .send_cmd = stm32_sdmmc2_send_cmd,
+ .set_ios = stm32_sdmmc2_set_ios,
+ .prepare = stm32_sdmmc2_prepare,
+ .read = stm32_sdmmc2_read,
+ .write = stm32_sdmmc2_write,
+};
+
+static struct stm32_sdmmc2_params sdmmc2_params;
+
+static bool next_cmd_is_acmd;
+
+#pragma weak plat_sdmmc2_use_dma
+bool plat_sdmmc2_use_dma(unsigned int instance, unsigned int memory)
+{
+ return false;
+}
+
+static void stm32_sdmmc2_init(void)
+{
+ uint32_t clock_div;
+ uint32_t freq = STM32MP_MMC_INIT_FREQ;
+ uintptr_t base = sdmmc2_params.reg_base;
+ int ret;
+
+ if (sdmmc2_params.max_freq != 0U) {
+ freq = MIN(sdmmc2_params.max_freq, freq);
+ }
+
+ if (sdmmc2_params.vmmc_regu != NULL) {
+ ret = regulator_disable(sdmmc2_params.vmmc_regu);
+ if (ret < 0) {
+ panic();
+ }
+ }
+
+ mdelay(VCC_POWER_OFF_DELAY);
+
+ mmio_write_32(base + SDMMC_POWER,
+ SDMMC_POWER_PWRCTRL_PWR_CYCLE | sdmmc2_params.dirpol);
+ mdelay(POWER_CYCLE_DELAY);
+
+ if (sdmmc2_params.vmmc_regu != NULL) {
+ ret = regulator_enable(sdmmc2_params.vmmc_regu);
+ if (ret < 0) {
+ panic();
+ }
+ }
+
+ mdelay(VCC_POWER_ON_DELAY);
+
+ mmio_write_32(base + SDMMC_POWER, sdmmc2_params.dirpol);
+ mdelay(POWER_OFF_DELAY);
+
+ clock_div = div_round_up(sdmmc2_params.clk_rate, freq * 2U);
+
+ mmio_write_32(base + SDMMC_CLKCR, SDMMC_CLKCR_HWFC_EN | clock_div |
+ sdmmc2_params.negedge |
+ sdmmc2_params.pin_ckin);
+
+ mmio_write_32(base + SDMMC_POWER,
+ SDMMC_POWER_PWRCTRL | sdmmc2_params.dirpol);
+
+ mdelay(POWER_ON_DELAY);
+}
+
+static int stm32_sdmmc2_stop_transfer(void)
+{
+ struct mmc_cmd cmd_stop;
+
+ zeromem(&cmd_stop, sizeof(struct mmc_cmd));
+
+ cmd_stop.cmd_idx = MMC_CMD(12);
+ cmd_stop.resp_type = MMC_RESPONSE_R1B;
+
+ return stm32_sdmmc2_send_cmd(&cmd_stop);
+}
+
+static int stm32_sdmmc2_send_cmd_req(struct mmc_cmd *cmd)
+{
+ uint64_t timeout;
+ uint32_t flags_cmd, status;
+ uint32_t flags_data = 0;
+ int err = 0;
+ uintptr_t base = sdmmc2_params.reg_base;
+ unsigned int cmd_reg, arg_reg;
+
+ if (cmd == NULL) {
+ return -EINVAL;
+ }
+
+ flags_cmd = SDMMC_STAR_CTIMEOUT;
+ arg_reg = cmd->cmd_arg;
+
+ if ((mmio_read_32(base + SDMMC_CMDR) & SDMMC_CMDR_CPSMEN) != 0U) {
+ mmio_write_32(base + SDMMC_CMDR, 0);
+ }
+
+ cmd_reg = cmd->cmd_idx | SDMMC_CMDR_CPSMEN;
+
+ if (cmd->resp_type == 0U) {
+ flags_cmd |= SDMMC_STAR_CMDSENT;
+ }
+
+ if ((cmd->resp_type & MMC_RSP_48) != 0U) {
+ if ((cmd->resp_type & MMC_RSP_136) != 0U) {
+ flags_cmd |= SDMMC_STAR_CMDREND;
+ cmd_reg |= SDMMC_CMDR_WAITRESP;
+ } else if ((cmd->resp_type & MMC_RSP_CRC) != 0U) {
+ flags_cmd |= SDMMC_STAR_CMDREND | SDMMC_STAR_CCRCFAIL;
+ cmd_reg |= SDMMC_CMDR_WAITRESP_SHORT;
+ } else {
+ flags_cmd |= SDMMC_STAR_CMDREND;
+ cmd_reg |= SDMMC_CMDR_WAITRESP_SHORT_NOCRC;
+ }
+ }
+
+ switch (cmd->cmd_idx) {
+ case MMC_CMD(1):
+ arg_reg |= OCR_POWERUP;
+ break;
+ case MMC_CMD(6):
+ if ((sdmmc2_params.device_info->mmc_dev_type == MMC_IS_SD_HC) &&
+ (!next_cmd_is_acmd)) {
+ cmd_reg |= SDMMC_CMDR_CMDTRANS;
+ if (sdmmc2_params.use_dma) {
+ flags_data |= SDMMC_STAR_DCRCFAIL |
+ SDMMC_STAR_DTIMEOUT |
+ SDMMC_STAR_DATAEND |
+ SDMMC_STAR_RXOVERR |
+ SDMMC_STAR_IDMATE |
+ SDMMC_STAR_DBCKEND;
+ }
+ }
+ break;
+ case MMC_CMD(8):
+ if (sdmmc2_params.device_info->mmc_dev_type == MMC_IS_EMMC) {
+ cmd_reg |= SDMMC_CMDR_CMDTRANS;
+ }
+ break;
+ case MMC_CMD(12):
+ cmd_reg |= SDMMC_CMDR_CMDSTOP;
+ break;
+ case MMC_CMD(17):
+ case MMC_CMD(18):
+ cmd_reg |= SDMMC_CMDR_CMDTRANS;
+ if (sdmmc2_params.use_dma) {
+ flags_data |= SDMMC_STAR_DCRCFAIL |
+ SDMMC_STAR_DTIMEOUT |
+ SDMMC_STAR_DATAEND |
+ SDMMC_STAR_RXOVERR |
+ SDMMC_STAR_IDMATE;
+ }
+ break;
+ case MMC_ACMD(41):
+ arg_reg |= OCR_3_2_3_3 | OCR_3_3_3_4;
+ break;
+ case MMC_ACMD(51):
+ cmd_reg |= SDMMC_CMDR_CMDTRANS;
+ if (sdmmc2_params.use_dma) {
+ flags_data |= SDMMC_STAR_DCRCFAIL |
+ SDMMC_STAR_DTIMEOUT |
+ SDMMC_STAR_DATAEND |
+ SDMMC_STAR_RXOVERR |
+ SDMMC_STAR_IDMATE |
+ SDMMC_STAR_DBCKEND;
+ }
+ break;
+ default:
+ break;
+ }
+
+ next_cmd_is_acmd = (cmd->cmd_idx == MMC_CMD(55));
+
+ mmio_write_32(base + SDMMC_ICR, SDMMC_STATIC_FLAGS);
+
+ /*
+ * Clear the SDMMC_DCTRLR if the command does not await data.
+ * Skip CMD55 as the next command could be data related, and
+ * the register could have been set in prepare function.
+ */
+ if (((cmd_reg & SDMMC_CMDR_CMDTRANS) == 0U) && !next_cmd_is_acmd) {
+ mmio_write_32(base + SDMMC_DCTRLR, 0U);
+ }
+
+ if ((cmd->resp_type & MMC_RSP_BUSY) != 0U) {
+ mmio_write_32(base + SDMMC_DTIMER, UINT32_MAX);
+ }
+
+ mmio_write_32(base + SDMMC_ARGR, arg_reg);
+
+ mmio_write_32(base + SDMMC_CMDR, cmd_reg);
+
+ status = mmio_read_32(base + SDMMC_STAR);
+
+ timeout = timeout_init_us(TIMEOUT_US_10_MS);
+
+ while ((status & flags_cmd) == 0U) {
+ if (timeout_elapsed(timeout)) {
+ err = -ETIMEDOUT;
+ ERROR("%s: timeout 10ms (cmd = %u,status = %x)\n",
+ __func__, cmd->cmd_idx, status);
+ goto err_exit;
+ }
+
+ status = mmio_read_32(base + SDMMC_STAR);
+ }
+
+ if ((status & (SDMMC_STAR_CTIMEOUT | SDMMC_STAR_CCRCFAIL)) != 0U) {
+ if ((status & SDMMC_STAR_CTIMEOUT) != 0U) {
+ err = -ETIMEDOUT;
+ /*
+ * Those timeouts can occur, and framework will handle
+ * the retries. CMD8 is expected to return this timeout
+ * for eMMC
+ */
+ if (!((cmd->cmd_idx == MMC_CMD(1)) ||
+ (cmd->cmd_idx == MMC_CMD(13)) ||
+ ((cmd->cmd_idx == MMC_CMD(8)) &&
+ (cmd->resp_type == MMC_RESPONSE_R7)))) {
+ ERROR("%s: CTIMEOUT (cmd = %u,status = %x)\n",
+ __func__, cmd->cmd_idx, status);
+ }
+ } else {
+ err = -EIO;
+ ERROR("%s: CRCFAIL (cmd = %u,status = %x)\n",
+ __func__, cmd->cmd_idx, status);
+ }
+
+ goto err_exit;
+ }
+
+ if ((cmd_reg & SDMMC_CMDR_WAITRESP) != 0U) {
+ if ((cmd->cmd_idx == MMC_CMD(9)) &&
+ ((cmd_reg & SDMMC_CMDR_WAITRESP) == SDMMC_CMDR_WAITRESP)) {
+ /* Need to invert response to match CSD structure */
+ cmd->resp_data[0] = mmio_read_32(base + SDMMC_RESP4R);
+ cmd->resp_data[1] = mmio_read_32(base + SDMMC_RESP3R);
+ cmd->resp_data[2] = mmio_read_32(base + SDMMC_RESP2R);
+ cmd->resp_data[3] = mmio_read_32(base + SDMMC_RESP1R);
+ } else {
+ cmd->resp_data[0] = mmio_read_32(base + SDMMC_RESP1R);
+ if ((cmd_reg & SDMMC_CMDR_WAITRESP) ==
+ SDMMC_CMDR_WAITRESP) {
+ cmd->resp_data[1] = mmio_read_32(base +
+ SDMMC_RESP2R);
+ cmd->resp_data[2] = mmio_read_32(base +
+ SDMMC_RESP3R);
+ cmd->resp_data[3] = mmio_read_32(base +
+ SDMMC_RESP4R);
+ }
+ }
+ }
+
+ if (flags_data == 0U) {
+ mmio_write_32(base + SDMMC_ICR, SDMMC_STATIC_FLAGS);
+
+ return 0;
+ }
+
+ status = mmio_read_32(base + SDMMC_STAR);
+
+ timeout = timeout_init_us(TIMEOUT_US_10_MS);
+
+ while ((status & flags_data) == 0U) {
+ if (timeout_elapsed(timeout)) {
+ ERROR("%s: timeout 10ms (cmd = %u,status = %x)\n",
+ __func__, cmd->cmd_idx, status);
+ err = -ETIMEDOUT;
+ goto err_exit;
+ }
+
+ status = mmio_read_32(base + SDMMC_STAR);
+ };
+
+ if ((status & (SDMMC_STAR_DTIMEOUT | SDMMC_STAR_DCRCFAIL |
+ SDMMC_STAR_TXUNDERR | SDMMC_STAR_RXOVERR |
+ SDMMC_STAR_IDMATE)) != 0U) {
+ ERROR("%s: Error flag (cmd = %u,status = %x)\n", __func__,
+ cmd->cmd_idx, status);
+ err = -EIO;
+ }
+
+err_exit:
+ mmio_write_32(base + SDMMC_ICR, SDMMC_STATIC_FLAGS);
+ mmio_clrbits_32(base + SDMMC_CMDR, SDMMC_CMDR_CMDTRANS);
+
+ if ((err != 0) && ((status & SDMMC_STAR_DPSMACT) != 0U)) {
+ int ret_stop = stm32_sdmmc2_stop_transfer();
+
+ if (ret_stop != 0) {
+ return ret_stop;
+ }
+ }
+
+ return err;
+}
+
+static int stm32_sdmmc2_send_cmd(struct mmc_cmd *cmd)
+{
+ uint8_t retry;
+ int err;
+
+ assert(cmd != NULL);
+
+ for (retry = 0U; retry < 3U; retry++) {
+ err = stm32_sdmmc2_send_cmd_req(cmd);
+ if (err == 0) {
+ return 0;
+ }
+
+ if ((cmd->cmd_idx == MMC_CMD(1)) ||
+ (cmd->cmd_idx == MMC_CMD(13))) {
+ return 0; /* Retry managed by framework */
+ }
+
+ /* Command 8 is expected to fail for eMMC */
+ if (cmd->cmd_idx != MMC_CMD(8)) {
+ WARN(" CMD%u, Retry: %u, Error: %d\n",
+ cmd->cmd_idx, retry + 1U, err);
+ }
+
+ udelay(10U);
+ }
+
+ return err;
+}
+
+static int stm32_sdmmc2_set_ios(unsigned int clk, unsigned int width)
+{
+ uintptr_t base = sdmmc2_params.reg_base;
+ uint32_t bus_cfg = 0;
+ uint32_t clock_div, max_freq, freq;
+ uint32_t clk_rate = sdmmc2_params.clk_rate;
+ uint32_t max_bus_freq = sdmmc2_params.device_info->max_bus_freq;
+
+ switch (width) {
+ case MMC_BUS_WIDTH_1:
+ break;
+ case MMC_BUS_WIDTH_4:
+ bus_cfg |= SDMMC_CLKCR_WIDBUS_4;
+ break;
+ case MMC_BUS_WIDTH_8:
+ bus_cfg |= SDMMC_CLKCR_WIDBUS_8;
+ break;
+ default:
+ panic();
+ break;
+ }
+
+ if (sdmmc2_params.device_info->mmc_dev_type == MMC_IS_EMMC) {
+ if (max_bus_freq >= 52000000U) {
+ max_freq = STM32MP_EMMC_HIGH_SPEED_MAX_FREQ;
+ } else {
+ max_freq = STM32MP_EMMC_NORMAL_SPEED_MAX_FREQ;
+ }
+ } else {
+ if (max_bus_freq >= 50000000U) {
+ max_freq = STM32MP_SD_HIGH_SPEED_MAX_FREQ;
+ } else {
+ max_freq = STM32MP_SD_NORMAL_SPEED_MAX_FREQ;
+ }
+ }
+
+ if (sdmmc2_params.max_freq != 0U) {
+ freq = MIN(sdmmc2_params.max_freq, max_freq);
+ } else {
+ freq = max_freq;
+ }
+
+ clock_div = div_round_up(clk_rate, freq * 2U);
+
+ mmio_write_32(base + SDMMC_CLKCR,
+ SDMMC_CLKCR_HWFC_EN | clock_div | bus_cfg |
+ sdmmc2_params.negedge |
+ sdmmc2_params.pin_ckin);
+
+ return 0;
+}
+
+static int stm32_sdmmc2_prepare(int lba, uintptr_t buf, size_t size)
+{
+ struct mmc_cmd cmd;
+ int ret;
+ uintptr_t base = sdmmc2_params.reg_base;
+ uint32_t data_ctrl = SDMMC_DCTRLR_DTDIR;
+ uint32_t arg_size;
+
+ assert(size != 0U);
+
+ if (size > MMC_BLOCK_SIZE) {
+ arg_size = MMC_BLOCK_SIZE;
+ } else {
+ arg_size = size;
+ }
+
+ sdmmc2_params.use_dma = plat_sdmmc2_use_dma(base, buf);
+
+ if (sdmmc2_params.use_dma) {
+ inv_dcache_range(buf, size);
+ }
+
+ /* Prepare CMD 16*/
+ mmio_write_32(base + SDMMC_DTIMER, 0);
+
+ mmio_write_32(base + SDMMC_DLENR, 0);
+
+ mmio_write_32(base + SDMMC_DCTRLR, 0);
+
+ zeromem(&cmd, sizeof(struct mmc_cmd));
+
+ cmd.cmd_idx = MMC_CMD(16);
+ cmd.cmd_arg = arg_size;
+ cmd.resp_type = MMC_RESPONSE_R1;
+
+ ret = stm32_sdmmc2_send_cmd(&cmd);
+ if (ret != 0) {
+ ERROR("CMD16 failed\n");
+ return ret;
+ }
+
+ /* Prepare data command */
+ mmio_write_32(base + SDMMC_DTIMER, UINT32_MAX);
+
+ mmio_write_32(base + SDMMC_DLENR, size);
+
+ if (sdmmc2_params.use_dma) {
+ mmio_write_32(base + SDMMC_IDMACTRLR,
+ SDMMC_IDMACTRLR_IDMAEN);
+ mmio_write_32(base + SDMMC_IDMABASE0R, buf);
+
+ flush_dcache_range(buf, size);
+ }
+
+ data_ctrl |= __builtin_ctz(arg_size) << SDMMC_DCTRLR_DBLOCKSIZE_SHIFT;
+
+ mmio_clrsetbits_32(base + SDMMC_DCTRLR,
+ SDMMC_DCTRLR_CLEAR_MASK,
+ data_ctrl);
+
+ return 0;
+}
+
+static int stm32_sdmmc2_read(int lba, uintptr_t buf, size_t size)
+{
+ uint32_t error_flags = SDMMC_STAR_RXOVERR | SDMMC_STAR_DCRCFAIL |
+ SDMMC_STAR_DTIMEOUT;
+ uint32_t flags = error_flags | SDMMC_STAR_DATAEND;
+ uint32_t status;
+ uint32_t *buffer;
+ uintptr_t base = sdmmc2_params.reg_base;
+ uintptr_t fifo_reg = base + SDMMC_FIFOR;
+ uint64_t timeout;
+ int ret;
+
+ /* Assert buf is 4 bytes aligned */
+ assert((buf & GENMASK(1, 0)) == 0U);
+
+ buffer = (uint32_t *)buf;
+
+ if (sdmmc2_params.use_dma) {
+ inv_dcache_range(buf, size);
+
+ return 0;
+ }
+
+ if (size <= MMC_BLOCK_SIZE) {
+ flags |= SDMMC_STAR_DBCKEND;
+ }
+
+ timeout = timeout_init_us(TIMEOUT_US_1_S);
+
+ do {
+ status = mmio_read_32(base + SDMMC_STAR);
+
+ if ((status & error_flags) != 0U) {
+ ERROR("%s: Read error (status = %x)\n", __func__,
+ status);
+ mmio_write_32(base + SDMMC_DCTRLR,
+ SDMMC_DCTRLR_FIFORST);
+
+ mmio_write_32(base + SDMMC_ICR,
+ SDMMC_STATIC_FLAGS);
+
+ ret = stm32_sdmmc2_stop_transfer();
+ if (ret != 0) {
+ return ret;
+ }
+
+ return -EIO;
+ }
+
+ if (timeout_elapsed(timeout)) {
+ ERROR("%s: timeout 1s (status = %x)\n",
+ __func__, status);
+ mmio_write_32(base + SDMMC_ICR,
+ SDMMC_STATIC_FLAGS);
+
+ ret = stm32_sdmmc2_stop_transfer();
+ if (ret != 0) {
+ return ret;
+ }
+
+ return -ETIMEDOUT;
+ }
+
+ if (size < (SDMMC_FIFO_SIZE / 2U)) {
+ if ((mmio_read_32(base + SDMMC_DCNTR) > 0U) &&
+ ((status & SDMMC_STAR_RXFIFOE) == 0U)) {
+ *buffer = mmio_read_32(fifo_reg);
+ buffer++;
+ }
+ } else if ((status & SDMMC_STAR_RXFIFOHF) != 0U) {
+ uint32_t count;
+
+ /* Read data from SDMMC Rx FIFO */
+ for (count = 0; count < (SDMMC_FIFO_SIZE / 2U);
+ count += sizeof(uint32_t)) {
+ *buffer = mmio_read_32(fifo_reg);
+ buffer++;
+ }
+ }
+ } while ((status & flags) == 0U);
+
+ mmio_write_32(base + SDMMC_ICR, SDMMC_STATIC_FLAGS);
+
+ if ((status & SDMMC_STAR_DPSMACT) != 0U) {
+ WARN("%s: DPSMACT=1, send stop\n", __func__);
+ return stm32_sdmmc2_stop_transfer();
+ }
+
+ return 0;
+}
+
+static int stm32_sdmmc2_write(int lba, uintptr_t buf, size_t size)
+{
+ return 0;
+}
+
+static int stm32_sdmmc2_dt_get_config(void)
+{
+ int sdmmc_node;
+ void *fdt = NULL;
+ const fdt32_t *cuint;
+ struct dt_node_info dt_info;
+
+ if (fdt_get_address(&fdt) == 0) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ if (fdt == NULL) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ sdmmc_node = dt_match_instance_by_compatible(DT_SDMMC2_COMPAT,
+ sdmmc2_params.reg_base);
+ if (sdmmc_node == -FDT_ERR_NOTFOUND) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ dt_fill_device_info(&dt_info, sdmmc_node);
+ if (dt_info.status == DT_DISABLED) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ if (dt_set_pinctrl_config(sdmmc_node) != 0) {
+ return -FDT_ERR_BADVALUE;
+ }
+
+ sdmmc2_params.clock_id = dt_info.clock;
+ sdmmc2_params.reset_id = dt_info.reset;
+
+ if ((fdt_getprop(fdt, sdmmc_node, "st,use-ckin", NULL)) != NULL) {
+ sdmmc2_params.pin_ckin = SDMMC_CLKCR_SELCLKRX_0;
+ }
+
+ if ((fdt_getprop(fdt, sdmmc_node, "st,sig-dir", NULL)) != NULL) {
+ sdmmc2_params.dirpol = SDMMC_POWER_DIRPOL;
+ }
+
+ if ((fdt_getprop(fdt, sdmmc_node, "st,neg-edge", NULL)) != NULL) {
+ sdmmc2_params.negedge = SDMMC_CLKCR_NEGEDGE;
+ }
+
+ cuint = fdt_getprop(fdt, sdmmc_node, "bus-width", NULL);
+ if (cuint != NULL) {
+ switch (fdt32_to_cpu(*cuint)) {
+ case 4:
+ sdmmc2_params.bus_width = MMC_BUS_WIDTH_4;
+ break;
+
+ case 8:
+ sdmmc2_params.bus_width = MMC_BUS_WIDTH_8;
+ break;
+
+ default:
+ break;
+ }
+ }
+
+ cuint = fdt_getprop(fdt, sdmmc_node, "max-frequency", NULL);
+ if (cuint != NULL) {
+ sdmmc2_params.max_freq = fdt32_to_cpu(*cuint);
+ }
+
+ sdmmc2_params.vmmc_regu = regulator_get_by_supply_name(fdt, sdmmc_node, "vmmc");
+
+ return 0;
+}
+
+unsigned long long stm32_sdmmc2_mmc_get_device_size(void)
+{
+ return sdmmc2_params.device_info->device_size;
+}
+
+int stm32_sdmmc2_mmc_init(struct stm32_sdmmc2_params *params)
+{
+ assert((params != NULL) &&
+ ((params->reg_base & MMC_BLOCK_MASK) == 0U) &&
+ ((params->bus_width == MMC_BUS_WIDTH_1) ||
+ (params->bus_width == MMC_BUS_WIDTH_4) ||
+ (params->bus_width == MMC_BUS_WIDTH_8)));
+
+ memcpy(&sdmmc2_params, params, sizeof(struct stm32_sdmmc2_params));
+
+ sdmmc2_params.vmmc_regu = NULL;
+
+ if (stm32_sdmmc2_dt_get_config() != 0) {
+ ERROR("%s: DT error\n", __func__);
+ return -ENOMEM;
+ }
+
+ clk_enable(sdmmc2_params.clock_id);
+
+ if ((int)sdmmc2_params.reset_id >= 0) {
+ int rc;
+
+ rc = stm32mp_reset_assert(sdmmc2_params.reset_id, TIMEOUT_US_1_MS);
+ if (rc != 0) {
+ panic();
+ }
+ udelay(2);
+ rc = stm32mp_reset_deassert(sdmmc2_params.reset_id, TIMEOUT_US_1_MS);
+ if (rc != 0) {
+ panic();
+ }
+ mdelay(1);
+ }
+
+ sdmmc2_params.clk_rate = clk_get_rate(sdmmc2_params.clock_id);
+ sdmmc2_params.device_info->ocr_voltage = OCR_3_2_3_3 | OCR_3_3_3_4;
+
+ return mmc_init(&stm32_sdmmc2_ops, sdmmc2_params.clk_rate,
+ sdmmc2_params.bus_width, sdmmc2_params.flags,
+ sdmmc2_params.device_info);
+}
diff --git a/drivers/st/pmic/stm32mp_pmic.c b/drivers/st/pmic/stm32mp_pmic.c
new file mode 100644
index 0000000..5b43760
--- /dev/null
+++ b/drivers/st/pmic/stm32mp_pmic.c
@@ -0,0 +1,526 @@
+/*
+ * Copyright (c) 2017-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/regulator.h>
+#include <drivers/st/stm32_i2c.h>
+#include <drivers/st/stm32mp_pmic.h>
+#include <drivers/st/stpmic1.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+#define PMIC_NODE_NOT_FOUND 1
+
+static struct i2c_handle_s i2c_handle;
+static uint32_t pmic_i2c_addr;
+
+static int register_pmic(void);
+
+static int dt_get_pmic_node(void *fdt)
+{
+ static int node = -FDT_ERR_BADOFFSET;
+
+ if (node == -FDT_ERR_BADOFFSET) {
+ node = fdt_node_offset_by_compatible(fdt, -1, "st,stpmic1");
+ }
+
+ return node;
+}
+
+int dt_pmic_status(void)
+{
+ static int status = -FDT_ERR_BADVALUE;
+ int node;
+ void *fdt;
+
+ if (status != -FDT_ERR_BADVALUE) {
+ return status;
+ }
+
+ if (fdt_get_address(&fdt) == 0) {
+ return -ENOENT;
+ }
+
+ node = dt_get_pmic_node(fdt);
+ if (node <= 0) {
+ status = -FDT_ERR_NOTFOUND;
+
+ return status;
+ }
+
+ status = (int)fdt_get_status(node);
+
+ return status;
+}
+
+static bool dt_pmic_is_secure(void)
+{
+ int status = dt_pmic_status();
+
+ return (status >= 0) &&
+ (status == DT_SECURE) &&
+ (i2c_handle.dt_status == DT_SECURE);
+}
+
+/*
+ * Get PMIC and its I2C bus configuration from the device tree.
+ * Return 0 on success, negative on error, 1 if no PMIC node is defined.
+ */
+static int dt_pmic_i2c_config(struct dt_node_info *i2c_info,
+ struct stm32_i2c_init_s *init)
+{
+ static int i2c_node = -FDT_ERR_NOTFOUND;
+ void *fdt;
+
+ if (fdt_get_address(&fdt) == 0) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ if (i2c_node == -FDT_ERR_NOTFOUND) {
+ int pmic_node;
+ const fdt32_t *cuint;
+
+ pmic_node = dt_get_pmic_node(fdt);
+ if (pmic_node < 0) {
+ return PMIC_NODE_NOT_FOUND;
+ }
+
+ cuint = fdt_getprop(fdt, pmic_node, "reg", NULL);
+ if (cuint == NULL) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ pmic_i2c_addr = fdt32_to_cpu(*cuint) << 1;
+ if (pmic_i2c_addr > UINT16_MAX) {
+ return -FDT_ERR_BADVALUE;
+ }
+
+ i2c_node = fdt_parent_offset(fdt, pmic_node);
+ if (i2c_node < 0) {
+ return -FDT_ERR_NOTFOUND;
+ }
+ }
+
+ dt_fill_device_info(i2c_info, i2c_node);
+ if (i2c_info->base == 0U) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ return stm32_i2c_get_setup_from_fdt(fdt, i2c_node, init);
+}
+
+bool initialize_pmic_i2c(void)
+{
+ int ret;
+ struct dt_node_info i2c_info;
+ struct i2c_handle_s *i2c = &i2c_handle;
+ struct stm32_i2c_init_s i2c_init;
+
+ ret = dt_pmic_i2c_config(&i2c_info, &i2c_init);
+ if (ret < 0) {
+ ERROR("I2C configuration failed %d\n", ret);
+ panic();
+ }
+
+ if (ret != 0) {
+ return false;
+ }
+
+ /* Initialize PMIC I2C */
+ i2c->i2c_base_addr = i2c_info.base;
+ i2c->dt_status = i2c_info.status;
+ i2c->clock = i2c_info.clock;
+ i2c->i2c_state = I2C_STATE_RESET;
+ i2c_init.own_address1 = pmic_i2c_addr;
+ i2c_init.addressing_mode = I2C_ADDRESSINGMODE_7BIT;
+ i2c_init.dual_address_mode = I2C_DUALADDRESS_DISABLE;
+ i2c_init.own_address2 = 0;
+ i2c_init.own_address2_masks = I2C_OAR2_OA2NOMASK;
+ i2c_init.general_call_mode = I2C_GENERALCALL_DISABLE;
+ i2c_init.no_stretch_mode = I2C_NOSTRETCH_DISABLE;
+ i2c_init.analog_filter = 1;
+ i2c_init.digital_filter_coef = 0;
+
+ ret = stm32_i2c_init(i2c, &i2c_init);
+ if (ret != 0) {
+ ERROR("Cannot initialize I2C %x (%d)\n",
+ i2c->i2c_base_addr, ret);
+ panic();
+ }
+
+ if (!stm32_i2c_is_device_ready(i2c, pmic_i2c_addr, 1,
+ I2C_TIMEOUT_BUSY_MS)) {
+ ERROR("I2C device not ready\n");
+ panic();
+ }
+
+ stpmic1_bind_i2c(i2c, (uint16_t)pmic_i2c_addr);
+
+ return true;
+}
+
+static void register_pmic_shared_peripherals(void)
+{
+ uintptr_t i2c_base = i2c_handle.i2c_base_addr;
+
+ if (dt_pmic_is_secure()) {
+ stm32mp_register_secure_periph_iomem(i2c_base);
+ } else {
+ if (i2c_base != 0U) {
+ stm32mp_register_non_secure_periph_iomem(i2c_base);
+ }
+ }
+}
+
+void initialize_pmic(void)
+{
+ if (!initialize_pmic_i2c()) {
+ VERBOSE("No PMIC\n");
+ return;
+ }
+
+ register_pmic_shared_peripherals();
+
+ if (register_pmic() < 0) {
+ panic();
+ }
+
+ if (stpmic1_powerctrl_on() < 0) {
+ panic();
+ }
+
+}
+
+#if DEBUG
+void print_pmic_info_and_debug(void)
+{
+ unsigned long pmic_version;
+
+ if (stpmic1_get_version(&pmic_version) != 0) {
+ ERROR("Failed to access PMIC\n");
+ panic();
+ }
+
+ INFO("PMIC version = 0x%02lx\n", pmic_version);
+}
+#endif
+
+int pmic_ddr_power_init(enum ddr_type ddr_type)
+{
+ int status;
+ uint16_t buck3_min_mv;
+ struct rdev *buck2, *buck3, *vref;
+ struct rdev *ldo3 __unused;
+
+ buck2 = regulator_get_by_name("buck2");
+ if (buck2 == NULL) {
+ return -ENOENT;
+ }
+
+#if STM32MP15
+ ldo3 = regulator_get_by_name("ldo3");
+ if (ldo3 == NULL) {
+ return -ENOENT;
+ }
+#endif
+
+ vref = regulator_get_by_name("vref_ddr");
+ if (vref == NULL) {
+ return -ENOENT;
+ }
+
+ switch (ddr_type) {
+ case STM32MP_DDR3:
+#if STM32MP15
+ status = regulator_set_flag(ldo3, REGUL_SINK_SOURCE);
+ if (status != 0) {
+ return status;
+ }
+#endif
+
+ status = regulator_set_min_voltage(buck2);
+ if (status != 0) {
+ return status;
+ }
+
+ status = regulator_enable(buck2);
+ if (status != 0) {
+ return status;
+ }
+
+ status = regulator_enable(vref);
+ if (status != 0) {
+ return status;
+ }
+
+#if STM32MP15
+ status = regulator_enable(ldo3);
+ if (status != 0) {
+ return status;
+ }
+#endif
+ break;
+
+ case STM32MP_LPDDR2:
+ case STM32MP_LPDDR3:
+ /*
+ * Set LDO3 to 1.8V
+ * Set LDO3 to bypass mode if BUCK3 = 1.8V
+ * Set LDO3 to normal mode if BUCK3 != 1.8V
+ */
+ buck3 = regulator_get_by_name("buck3");
+ if (buck3 == NULL) {
+ return -ENOENT;
+ }
+
+ regulator_get_range(buck3, &buck3_min_mv, NULL);
+
+#if STM32MP15
+ if (buck3_min_mv != 1800) {
+ status = regulator_set_min_voltage(ldo3);
+ if (status != 0) {
+ return status;
+ }
+ } else {
+ status = regulator_set_flag(ldo3, REGUL_ENABLE_BYPASS);
+ if (status != 0) {
+ return status;
+ }
+ }
+#endif
+
+ status = regulator_set_min_voltage(buck2);
+ if (status != 0) {
+ return status;
+ }
+
+#if STM32MP15
+ status = regulator_enable(ldo3);
+ if (status != 0) {
+ return status;
+ }
+#endif
+
+ status = regulator_enable(buck2);
+ if (status != 0) {
+ return status;
+ }
+
+ status = regulator_enable(vref);
+ if (status != 0) {
+ return status;
+ }
+ break;
+
+ default:
+ break;
+ };
+
+ return 0;
+}
+
+int pmic_voltages_init(void)
+{
+#if STM32MP13
+ struct rdev *buck1, *buck4;
+ int status;
+
+ buck1 = regulator_get_by_name("buck1");
+ if (buck1 == NULL) {
+ return -ENOENT;
+ }
+
+ buck4 = regulator_get_by_name("buck4");
+ if (buck4 == NULL) {
+ return -ENOENT;
+ }
+
+ status = regulator_set_min_voltage(buck1);
+ if (status != 0) {
+ return status;
+ }
+
+ status = regulator_set_min_voltage(buck4);
+ if (status != 0) {
+ return status;
+ }
+#endif
+
+ return 0;
+}
+
+enum {
+ STPMIC1_BUCK1 = 0,
+ STPMIC1_BUCK2,
+ STPMIC1_BUCK3,
+ STPMIC1_BUCK4,
+ STPMIC1_LDO1,
+ STPMIC1_LDO2,
+ STPMIC1_LDO3,
+ STPMIC1_LDO4,
+ STPMIC1_LDO5,
+ STPMIC1_LDO6,
+ STPMIC1_VREF_DDR,
+ STPMIC1_BOOST,
+ STPMIC1_VBUS_OTG,
+ STPMIC1_SW_OUT,
+};
+
+static int pmic_set_state(const struct regul_description *desc, bool enable)
+{
+ VERBOSE("%s: set state to %d\n", desc->node_name, enable);
+
+ if (enable == STATE_ENABLE) {
+ return stpmic1_regulator_enable(desc->node_name);
+ } else {
+ return stpmic1_regulator_disable(desc->node_name);
+ }
+}
+
+static int pmic_get_state(const struct regul_description *desc)
+{
+ VERBOSE("%s: get state\n", desc->node_name);
+
+ return stpmic1_is_regulator_enabled(desc->node_name);
+}
+
+static int pmic_get_voltage(const struct regul_description *desc)
+{
+ VERBOSE("%s: get volt\n", desc->node_name);
+
+ return stpmic1_regulator_voltage_get(desc->node_name);
+}
+
+static int pmic_set_voltage(const struct regul_description *desc, uint16_t mv)
+{
+ VERBOSE("%s: get volt\n", desc->node_name);
+
+ return stpmic1_regulator_voltage_set(desc->node_name, mv);
+}
+
+static int pmic_list_voltages(const struct regul_description *desc,
+ const uint16_t **levels, size_t *count)
+{
+ VERBOSE("%s: list volt\n", desc->node_name);
+
+ return stpmic1_regulator_levels_mv(desc->node_name, levels, count);
+}
+
+static int pmic_set_flag(const struct regul_description *desc, uint16_t flag)
+{
+ VERBOSE("%s: set_flag 0x%x\n", desc->node_name, flag);
+
+ switch (flag) {
+ case REGUL_OCP:
+ return stpmic1_regulator_icc_set(desc->node_name);
+
+ case REGUL_ACTIVE_DISCHARGE:
+ return stpmic1_active_discharge_mode_set(desc->node_name);
+
+ case REGUL_PULL_DOWN:
+ return stpmic1_regulator_pull_down_set(desc->node_name);
+
+ case REGUL_MASK_RESET:
+ return stpmic1_regulator_mask_reset_set(desc->node_name);
+
+ case REGUL_SINK_SOURCE:
+ return stpmic1_regulator_sink_mode_set(desc->node_name);
+
+ case REGUL_ENABLE_BYPASS:
+ return stpmic1_regulator_bypass_mode_set(desc->node_name);
+
+ default:
+ return -EINVAL;
+ }
+}
+
+static const struct regul_ops pmic_ops = {
+ .set_state = pmic_set_state,
+ .get_state = pmic_get_state,
+ .set_voltage = pmic_set_voltage,
+ .get_voltage = pmic_get_voltage,
+ .list_voltages = pmic_list_voltages,
+ .set_flag = pmic_set_flag,
+};
+
+#define DEFINE_REGU(name) { \
+ .node_name = name, \
+ .ops = &pmic_ops, \
+ .driver_data = NULL, \
+ .enable_ramp_delay = 1000, \
+}
+
+static const struct regul_description pmic_regs[] = {
+ [STPMIC1_BUCK1] = DEFINE_REGU("buck1"),
+ [STPMIC1_BUCK2] = DEFINE_REGU("buck2"),
+ [STPMIC1_BUCK3] = DEFINE_REGU("buck3"),
+ [STPMIC1_BUCK4] = DEFINE_REGU("buck4"),
+ [STPMIC1_LDO1] = DEFINE_REGU("ldo1"),
+ [STPMIC1_LDO2] = DEFINE_REGU("ldo2"),
+ [STPMIC1_LDO3] = DEFINE_REGU("ldo3"),
+ [STPMIC1_LDO4] = DEFINE_REGU("ldo4"),
+ [STPMIC1_LDO5] = DEFINE_REGU("ldo5"),
+ [STPMIC1_LDO6] = DEFINE_REGU("ldo6"),
+ [STPMIC1_VREF_DDR] = DEFINE_REGU("vref_ddr"),
+ [STPMIC1_BOOST] = DEFINE_REGU("boost"),
+ [STPMIC1_VBUS_OTG] = DEFINE_REGU("pwr_sw1"),
+ [STPMIC1_SW_OUT] = DEFINE_REGU("pwr_sw2"),
+};
+
+#define NB_REG ARRAY_SIZE(pmic_regs)
+
+static int register_pmic(void)
+{
+ void *fdt;
+ int pmic_node, regulators_node, subnode;
+
+ VERBOSE("Register pmic\n");
+
+ if (fdt_get_address(&fdt) == 0) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ pmic_node = dt_get_pmic_node(fdt);
+ if (pmic_node < 0) {
+ return pmic_node;
+ }
+
+ regulators_node = fdt_subnode_offset(fdt, pmic_node, "regulators");
+ if (regulators_node < 0) {
+ return -ENOENT;
+ }
+
+ fdt_for_each_subnode(subnode, fdt, regulators_node) {
+ const char *reg_name = fdt_get_name(fdt, subnode, NULL);
+ const struct regul_description *desc;
+ unsigned int i;
+ int ret;
+
+ for (i = 0; i < NB_REG; i++) {
+ desc = &pmic_regs[i];
+ if (strcmp(desc->node_name, reg_name) == 0) {
+ break;
+ }
+ }
+ assert(i < NB_REG);
+
+ ret = regulator_register(desc, subnode);
+ if (ret != 0) {
+ WARN("%s:%d failed to register %s\n", __func__,
+ __LINE__, reg_name);
+ return ret;
+ }
+ }
+
+ return 0;
+}
diff --git a/drivers/st/pmic/stpmic1.c b/drivers/st/pmic/stpmic1.c
new file mode 100644
index 0000000..37eb50b
--- /dev/null
+++ b/drivers/st/pmic/stpmic1.c
@@ -0,0 +1,937 @@
+/*
+ * Copyright (c) 2016-2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <drivers/st/stpmic1.h>
+
+#define I2C_TIMEOUT_MS 25
+
+struct regul_struct {
+ const char *dt_node_name;
+ const uint16_t *voltage_table;
+ uint8_t voltage_table_size;
+ uint8_t control_reg;
+ uint8_t enable_mask;
+ uint8_t low_power_reg;
+ uint8_t pull_down_reg;
+ uint8_t pull_down;
+ uint8_t mask_reset_reg;
+ uint8_t mask_reset;
+ uint8_t icc_reg;
+ uint8_t icc_mask;
+};
+
+static struct i2c_handle_s *pmic_i2c_handle;
+static uint16_t pmic_i2c_addr;
+/*
+ * Special mode corresponds to LDO3 in sink source mode or in bypass mode.
+ * LDO3 doesn't switch back from special to normal mode.
+ */
+static bool ldo3_special_mode;
+
+/* Voltage tables in mV */
+static const uint16_t buck1_voltage_table[] = {
+ 725,
+ 725,
+ 725,
+ 725,
+ 725,
+ 725,
+ 750,
+ 775,
+ 800,
+ 825,
+ 850,
+ 875,
+ 900,
+ 925,
+ 950,
+ 975,
+ 1000,
+ 1025,
+ 1050,
+ 1075,
+ 1100,
+ 1125,
+ 1150,
+ 1175,
+ 1200,
+ 1225,
+ 1250,
+ 1275,
+ 1300,
+ 1325,
+ 1350,
+ 1375,
+ 1400,
+ 1425,
+ 1450,
+ 1475,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+ 1500,
+};
+
+static const uint16_t buck2_voltage_table[] = {
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1050,
+ 1050,
+ 1100,
+ 1100,
+ 1150,
+ 1150,
+ 1200,
+ 1200,
+ 1250,
+ 1250,
+ 1300,
+ 1300,
+ 1350,
+ 1350,
+ 1400,
+ 1400,
+ 1450,
+ 1450,
+ 1500,
+};
+
+static const uint16_t buck3_voltage_table[] = {
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1000,
+ 1100,
+ 1100,
+ 1100,
+ 1100,
+ 1200,
+ 1200,
+ 1200,
+ 1200,
+ 1300,
+ 1300,
+ 1300,
+ 1300,
+ 1400,
+ 1400,
+ 1400,
+ 1400,
+ 1500,
+ 1600,
+ 1700,
+ 1800,
+ 1900,
+ 2000,
+ 2100,
+ 2200,
+ 2300,
+ 2400,
+ 2500,
+ 2600,
+ 2700,
+ 2800,
+ 2900,
+ 3000,
+ 3100,
+ 3200,
+ 3300,
+ 3400,
+};
+
+static const uint16_t buck4_voltage_table[] = {
+ 600,
+ 625,
+ 650,
+ 675,
+ 700,
+ 725,
+ 750,
+ 775,
+ 800,
+ 825,
+ 850,
+ 875,
+ 900,
+ 925,
+ 950,
+ 975,
+ 1000,
+ 1025,
+ 1050,
+ 1075,
+ 1100,
+ 1125,
+ 1150,
+ 1175,
+ 1200,
+ 1225,
+ 1250,
+ 1275,
+ 1300,
+ 1300,
+ 1350,
+ 1350,
+ 1400,
+ 1400,
+ 1450,
+ 1450,
+ 1500,
+ 1600,
+ 1700,
+ 1800,
+ 1900,
+ 2000,
+ 2100,
+ 2200,
+ 2300,
+ 2400,
+ 2500,
+ 2600,
+ 2700,
+ 2800,
+ 2900,
+ 3000,
+ 3100,
+ 3200,
+ 3300,
+ 3400,
+ 3500,
+ 3600,
+ 3700,
+ 3800,
+ 3900,
+};
+
+static const uint16_t ldo1_voltage_table[] = {
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1800,
+ 1900,
+ 2000,
+ 2100,
+ 2200,
+ 2300,
+ 2400,
+ 2500,
+ 2600,
+ 2700,
+ 2800,
+ 2900,
+ 3000,
+ 3100,
+ 3200,
+ 3300,
+};
+
+static const uint16_t ldo2_voltage_table[] = {
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1800,
+ 1900,
+ 2000,
+ 2100,
+ 2200,
+ 2300,
+ 2400,
+ 2500,
+ 2600,
+ 2700,
+ 2800,
+ 2900,
+ 3000,
+ 3100,
+ 3200,
+ 3300,
+};
+
+static const uint16_t ldo3_voltage_table[] = {
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1800,
+ 1900,
+ 2000,
+ 2100,
+ 2200,
+ 2300,
+ 2400,
+ 2500,
+ 2600,
+ 2700,
+ 2800,
+ 2900,
+ 3000,
+ 3100,
+ 3200,
+ 3300,
+ 3300,
+ 3300,
+ 3300,
+ 3300,
+ 3300,
+ 3300,
+};
+
+/* Special mode table is used for sink source OR bypass mode */
+static const uint16_t ldo3_special_mode_table[] = {
+ 0,
+};
+
+static const uint16_t ldo5_voltage_table[] = {
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1700,
+ 1800,
+ 1900,
+ 2000,
+ 2100,
+ 2200,
+ 2300,
+ 2400,
+ 2500,
+ 2600,
+ 2700,
+ 2800,
+ 2900,
+ 3000,
+ 3100,
+ 3200,
+ 3300,
+ 3400,
+ 3500,
+ 3600,
+ 3700,
+ 3800,
+ 3900,
+};
+
+static const uint16_t ldo6_voltage_table[] = {
+ 900,
+ 1000,
+ 1100,
+ 1200,
+ 1300,
+ 1400,
+ 1500,
+ 1600,
+ 1700,
+ 1800,
+ 1900,
+ 2000,
+ 2100,
+ 2200,
+ 2300,
+ 2400,
+ 2500,
+ 2600,
+ 2700,
+ 2800,
+ 2900,
+ 3000,
+ 3100,
+ 3200,
+ 3300,
+};
+
+static const uint16_t ldo4_voltage_table[] = {
+ 3300,
+};
+
+static const uint16_t vref_ddr_voltage_table[] = {
+ 3300,
+};
+
+static const uint16_t fixed_5v_voltage_table[] = {
+ 5000,
+};
+
+/* Table of Regulators in PMIC SoC */
+static const struct regul_struct regulators_table[] = {
+ {
+ .dt_node_name = "buck1",
+ .voltage_table = buck1_voltage_table,
+ .voltage_table_size = ARRAY_SIZE(buck1_voltage_table),
+ .control_reg = BUCK1_CONTROL_REG,
+ .enable_mask = LDO_BUCK_ENABLE_MASK,
+ .low_power_reg = BUCK1_PWRCTRL_REG,
+ .pull_down_reg = BUCK_PULL_DOWN_REG,
+ .pull_down = BUCK1_PULL_DOWN_SHIFT,
+ .mask_reset_reg = MASK_RESET_BUCK_REG,
+ .mask_reset = BUCK1_MASK_RESET,
+ .icc_reg = BUCK_ICC_TURNOFF_REG,
+ .icc_mask = BUCK1_ICC_SHIFT,
+ },
+ {
+ .dt_node_name = "buck2",
+ .voltage_table = buck2_voltage_table,
+ .voltage_table_size = ARRAY_SIZE(buck2_voltage_table),
+ .control_reg = BUCK2_CONTROL_REG,
+ .enable_mask = LDO_BUCK_ENABLE_MASK,
+ .low_power_reg = BUCK2_PWRCTRL_REG,
+ .pull_down_reg = BUCK_PULL_DOWN_REG,
+ .pull_down = BUCK2_PULL_DOWN_SHIFT,
+ .mask_reset_reg = MASK_RESET_BUCK_REG,
+ .mask_reset = BUCK2_MASK_RESET,
+ .icc_reg = BUCK_ICC_TURNOFF_REG,
+ .icc_mask = BUCK2_ICC_SHIFT,
+ },
+ {
+ .dt_node_name = "buck3",
+ .voltage_table = buck3_voltage_table,
+ .voltage_table_size = ARRAY_SIZE(buck3_voltage_table),
+ .control_reg = BUCK3_CONTROL_REG,
+ .enable_mask = LDO_BUCK_ENABLE_MASK,
+ .low_power_reg = BUCK3_PWRCTRL_REG,
+ .pull_down_reg = BUCK_PULL_DOWN_REG,
+ .pull_down = BUCK3_PULL_DOWN_SHIFT,
+ .mask_reset_reg = MASK_RESET_BUCK_REG,
+ .mask_reset = BUCK3_MASK_RESET,
+ .icc_reg = BUCK_ICC_TURNOFF_REG,
+ .icc_mask = BUCK3_ICC_SHIFT,
+ },
+ {
+ .dt_node_name = "buck4",
+ .voltage_table = buck4_voltage_table,
+ .voltage_table_size = ARRAY_SIZE(buck4_voltage_table),
+ .control_reg = BUCK4_CONTROL_REG,
+ .enable_mask = LDO_BUCK_ENABLE_MASK,
+ .low_power_reg = BUCK4_PWRCTRL_REG,
+ .pull_down_reg = BUCK_PULL_DOWN_REG,
+ .pull_down = BUCK4_PULL_DOWN_SHIFT,
+ .mask_reset_reg = MASK_RESET_BUCK_REG,
+ .mask_reset = BUCK4_MASK_RESET,
+ .icc_reg = BUCK_ICC_TURNOFF_REG,
+ .icc_mask = BUCK4_ICC_SHIFT,
+ },
+ {
+ .dt_node_name = "ldo1",
+ .voltage_table = ldo1_voltage_table,
+ .voltage_table_size = ARRAY_SIZE(ldo1_voltage_table),
+ .control_reg = LDO1_CONTROL_REG,
+ .enable_mask = LDO_BUCK_ENABLE_MASK,
+ .low_power_reg = LDO1_PWRCTRL_REG,
+ .mask_reset_reg = MASK_RESET_LDO_REG,
+ .mask_reset = LDO1_MASK_RESET,
+ .icc_reg = LDO_ICC_TURNOFF_REG,
+ .icc_mask = LDO1_ICC_SHIFT,
+ },
+ {
+ .dt_node_name = "ldo2",
+ .voltage_table = ldo2_voltage_table,
+ .voltage_table_size = ARRAY_SIZE(ldo2_voltage_table),
+ .control_reg = LDO2_CONTROL_REG,
+ .enable_mask = LDO_BUCK_ENABLE_MASK,
+ .low_power_reg = LDO2_PWRCTRL_REG,
+ .mask_reset_reg = MASK_RESET_LDO_REG,
+ .mask_reset = LDO2_MASK_RESET,
+ .icc_reg = LDO_ICC_TURNOFF_REG,
+ .icc_mask = LDO2_ICC_SHIFT,
+ },
+ {
+ .dt_node_name = "ldo3",
+ .voltage_table = ldo3_voltage_table,
+ .voltage_table_size = ARRAY_SIZE(ldo3_voltage_table),
+ .control_reg = LDO3_CONTROL_REG,
+ .enable_mask = LDO_BUCK_ENABLE_MASK,
+ .low_power_reg = LDO3_PWRCTRL_REG,
+ .mask_reset_reg = MASK_RESET_LDO_REG,
+ .mask_reset = LDO3_MASK_RESET,
+ .icc_reg = LDO_ICC_TURNOFF_REG,
+ .icc_mask = LDO3_ICC_SHIFT,
+ },
+ {
+ .dt_node_name = "ldo4",
+ .voltage_table = ldo4_voltage_table,
+ .voltage_table_size = ARRAY_SIZE(ldo4_voltage_table),
+ .control_reg = LDO4_CONTROL_REG,
+ .enable_mask = LDO_BUCK_ENABLE_MASK,
+ .low_power_reg = LDO4_PWRCTRL_REG,
+ .mask_reset_reg = MASK_RESET_LDO_REG,
+ .mask_reset = LDO4_MASK_RESET,
+ .icc_reg = LDO_ICC_TURNOFF_REG,
+ .icc_mask = LDO4_ICC_SHIFT,
+ },
+ {
+ .dt_node_name = "ldo5",
+ .voltage_table = ldo5_voltage_table,
+ .voltage_table_size = ARRAY_SIZE(ldo5_voltage_table),
+ .control_reg = LDO5_CONTROL_REG,
+ .enable_mask = LDO_BUCK_ENABLE_MASK,
+ .low_power_reg = LDO5_PWRCTRL_REG,
+ .mask_reset_reg = MASK_RESET_LDO_REG,
+ .mask_reset = LDO5_MASK_RESET,
+ .icc_reg = LDO_ICC_TURNOFF_REG,
+ .icc_mask = LDO5_ICC_SHIFT,
+ },
+ {
+ .dt_node_name = "ldo6",
+ .voltage_table = ldo6_voltage_table,
+ .voltage_table_size = ARRAY_SIZE(ldo6_voltage_table),
+ .control_reg = LDO6_CONTROL_REG,
+ .enable_mask = LDO_BUCK_ENABLE_MASK,
+ .low_power_reg = LDO6_PWRCTRL_REG,
+ .mask_reset_reg = MASK_RESET_LDO_REG,
+ .mask_reset = LDO6_MASK_RESET,
+ .icc_reg = LDO_ICC_TURNOFF_REG,
+ .icc_mask = LDO6_ICC_SHIFT,
+ },
+ {
+ .dt_node_name = "vref_ddr",
+ .voltage_table = vref_ddr_voltage_table,
+ .voltage_table_size = ARRAY_SIZE(vref_ddr_voltage_table),
+ .control_reg = VREF_DDR_CONTROL_REG,
+ .enable_mask = LDO_BUCK_ENABLE_MASK,
+ .low_power_reg = VREF_DDR_PWRCTRL_REG,
+ .mask_reset_reg = MASK_RESET_LDO_REG,
+ .mask_reset = VREF_DDR_MASK_RESET,
+ },
+ {
+ .dt_node_name = "boost",
+ .voltage_table = fixed_5v_voltage_table,
+ .voltage_table_size = ARRAY_SIZE(fixed_5v_voltage_table),
+ .control_reg = USB_CONTROL_REG,
+ .enable_mask = BOOST_ENABLED,
+ .icc_reg = BUCK_ICC_TURNOFF_REG,
+ .icc_mask = BOOST_ICC_SHIFT,
+ },
+ {
+ .dt_node_name = "pwr_sw1",
+ .voltage_table = fixed_5v_voltage_table,
+ .voltage_table_size = ARRAY_SIZE(fixed_5v_voltage_table),
+ .control_reg = USB_CONTROL_REG,
+ .enable_mask = USBSW_OTG_SWITCH_ENABLED,
+ .icc_reg = BUCK_ICC_TURNOFF_REG,
+ .icc_mask = PWR_SW1_ICC_SHIFT,
+ },
+ {
+ .dt_node_name = "pwr_sw2",
+ .voltage_table = fixed_5v_voltage_table,
+ .voltage_table_size = ARRAY_SIZE(fixed_5v_voltage_table),
+ .control_reg = USB_CONTROL_REG,
+ .enable_mask = SWIN_SWOUT_ENABLED,
+ .icc_reg = BUCK_ICC_TURNOFF_REG,
+ .icc_mask = PWR_SW2_ICC_SHIFT,
+ },
+};
+
+#define MAX_REGUL ARRAY_SIZE(regulators_table)
+
+static const struct regul_struct *get_regulator_data(const char *name)
+{
+ uint8_t i;
+
+ for (i = 0 ; i < MAX_REGUL ; i++) {
+ if (strncmp(name, regulators_table[i].dt_node_name,
+ strlen(regulators_table[i].dt_node_name)) == 0) {
+ return &regulators_table[i];
+ }
+ }
+
+ /* Regulator not found */
+ panic();
+ return NULL;
+}
+
+static uint8_t voltage_to_index(const char *name, uint16_t millivolts)
+{
+ const struct regul_struct *regul = get_regulator_data(name);
+ uint8_t i;
+
+ for (i = 0 ; i < regul->voltage_table_size ; i++) {
+ if (regul->voltage_table[i] == millivolts) {
+ return i;
+ }
+ }
+
+ /* Voltage not found */
+ panic();
+
+ return 0;
+}
+
+int stpmic1_powerctrl_on(void)
+{
+ return stpmic1_register_update(MAIN_CONTROL_REG, PWRCTRL_PIN_VALID,
+ PWRCTRL_PIN_VALID);
+}
+
+int stpmic1_switch_off(void)
+{
+ return stpmic1_register_update(MAIN_CONTROL_REG, 1,
+ SOFTWARE_SWITCH_OFF_ENABLED);
+}
+
+int stpmic1_regulator_enable(const char *name)
+{
+ const struct regul_struct *regul = get_regulator_data(name);
+
+ return stpmic1_register_update(regul->control_reg, regul->enable_mask,
+ regul->enable_mask);
+}
+
+int stpmic1_regulator_disable(const char *name)
+{
+ const struct regul_struct *regul = get_regulator_data(name);
+
+ return stpmic1_register_update(regul->control_reg, 0,
+ regul->enable_mask);
+}
+
+bool stpmic1_is_regulator_enabled(const char *name)
+{
+ uint8_t val;
+ const struct regul_struct *regul = get_regulator_data(name);
+
+ if (stpmic1_register_read(regul->control_reg, &val) != 0) {
+ panic();
+ }
+
+ return (val & regul->enable_mask) == regul->enable_mask;
+}
+
+int stpmic1_regulator_voltage_set(const char *name, uint16_t millivolts)
+{
+ uint8_t voltage_index = voltage_to_index(name, millivolts);
+ const struct regul_struct *regul = get_regulator_data(name);
+ uint8_t mask;
+
+ if ((strncmp(name, "ldo3", 5) == 0) && ldo3_special_mode) {
+ /*
+ * when the LDO3 is in special mode, we do not change voltage,
+ * because by setting voltage, the LDO would leaves sink-source
+ * mode. There is obviously no reason to leave sink-source mode
+ * at runtime.
+ */
+ return 0;
+ }
+
+ /* Voltage can be set for buck<N> or ldo<N> (except ldo4) regulators */
+ if (strncmp(name, "buck", 4) == 0) {
+ mask = BUCK_VOLTAGE_MASK;
+ } else if ((strncmp(name, "ldo", 3) == 0) &&
+ (strncmp(name, "ldo4", 5) != 0)) {
+ mask = LDO_VOLTAGE_MASK;
+ } else {
+ return 0;
+ }
+
+ return stpmic1_register_update(regul->control_reg,
+ voltage_index << LDO_BUCK_VOLTAGE_SHIFT,
+ mask);
+}
+
+int stpmic1_regulator_pull_down_set(const char *name)
+{
+ const struct regul_struct *regul = get_regulator_data(name);
+
+ if (regul->pull_down_reg != 0) {
+ return stpmic1_register_update(regul->pull_down_reg,
+ BIT(regul->pull_down),
+ LDO_BUCK_PULL_DOWN_MASK <<
+ regul->pull_down);
+ }
+
+ return 0;
+}
+
+int stpmic1_regulator_mask_reset_set(const char *name)
+{
+ const struct regul_struct *regul = get_regulator_data(name);
+
+ if (regul->mask_reset_reg == 0U) {
+ return -EPERM;
+ }
+
+ return stpmic1_register_update(regul->mask_reset_reg,
+ BIT(regul->mask_reset),
+ LDO_BUCK_RESET_MASK <<
+ regul->mask_reset);
+}
+
+int stpmic1_regulator_icc_set(const char *name)
+{
+ const struct regul_struct *regul = get_regulator_data(name);
+
+ if (regul->mask_reset_reg == 0U) {
+ return -EPERM;
+ }
+
+ return stpmic1_register_update(regul->icc_reg,
+ BIT(regul->icc_mask),
+ BIT(regul->icc_mask));
+}
+
+int stpmic1_regulator_sink_mode_set(const char *name)
+{
+ if (strncmp(name, "ldo3", 5) != 0) {
+ return -EPERM;
+ }
+
+ ldo3_special_mode = true;
+
+ /* disable bypass mode, enable sink mode */
+ return stpmic1_register_update(LDO3_CONTROL_REG,
+ LDO3_DDR_SEL << LDO_BUCK_VOLTAGE_SHIFT,
+ LDO3_BYPASS | LDO_VOLTAGE_MASK);
+}
+
+int stpmic1_regulator_bypass_mode_set(const char *name)
+{
+ if (strncmp(name, "ldo3", 5) != 0) {
+ return -EPERM;
+ }
+
+ ldo3_special_mode = true;
+
+ /* enable bypass mode, disable sink mode */
+ return stpmic1_register_update(LDO3_CONTROL_REG,
+ LDO3_BYPASS,
+ LDO3_BYPASS | LDO_VOLTAGE_MASK);
+}
+
+int stpmic1_active_discharge_mode_set(const char *name)
+{
+ if (strncmp(name, "pwr_sw1", 8) == 0) {
+ return stpmic1_register_update(USB_CONTROL_REG,
+ VBUS_OTG_DISCHARGE,
+ VBUS_OTG_DISCHARGE);
+ }
+
+ if (strncmp(name, "pwr_sw2", 8) == 0) {
+ return stpmic1_register_update(USB_CONTROL_REG,
+ SW_OUT_DISCHARGE,
+ SW_OUT_DISCHARGE);
+ }
+
+ return -EPERM;
+}
+
+int stpmic1_regulator_levels_mv(const char *name, const uint16_t **levels,
+ size_t *levels_count)
+{
+ const struct regul_struct *regul = get_regulator_data(name);
+
+ if ((strncmp(name, "ldo3", 5) == 0) && ldo3_special_mode) {
+ *levels_count = ARRAY_SIZE(ldo3_special_mode_table);
+ *levels = ldo3_special_mode_table;
+ } else {
+ *levels_count = regul->voltage_table_size;
+ *levels = regul->voltage_table;
+ }
+
+ return 0;
+}
+
+int stpmic1_regulator_voltage_get(const char *name)
+{
+ const struct regul_struct *regul = get_regulator_data(name);
+ uint8_t value;
+ uint8_t mask;
+ int status;
+
+ if ((strncmp(name, "ldo3", 5) == 0) && ldo3_special_mode) {
+ return 0;
+ }
+
+ /* Voltage can be set for buck<N> or ldo<N> (except ldo4) regulators */
+ if (strncmp(name, "buck", 4) == 0) {
+ mask = BUCK_VOLTAGE_MASK;
+ } else if ((strncmp(name, "ldo", 3) == 0) &&
+ (strncmp(name, "ldo4", 5) != 0)) {
+ mask = LDO_VOLTAGE_MASK;
+ } else {
+ return 0;
+ }
+
+ status = stpmic1_register_read(regul->control_reg, &value);
+ if (status < 0) {
+ return status;
+ }
+
+ value = (value & mask) >> LDO_BUCK_VOLTAGE_SHIFT;
+
+ if (value > regul->voltage_table_size) {
+ return -ERANGE;
+ }
+
+ return (int)regul->voltage_table[value];
+}
+
+int stpmic1_register_read(uint8_t register_id, uint8_t *value)
+{
+ return stm32_i2c_mem_read(pmic_i2c_handle, pmic_i2c_addr,
+ (uint16_t)register_id,
+ I2C_MEMADD_SIZE_8BIT, value,
+ 1, I2C_TIMEOUT_MS);
+}
+
+int stpmic1_register_write(uint8_t register_id, uint8_t value)
+{
+ int status;
+
+ status = stm32_i2c_mem_write(pmic_i2c_handle, pmic_i2c_addr,
+ (uint16_t)register_id,
+ I2C_MEMADD_SIZE_8BIT, &value,
+ 1, I2C_TIMEOUT_MS);
+
+#if ENABLE_ASSERTIONS
+ if (status != 0) {
+ return status;
+ }
+
+ if ((register_id != WATCHDOG_CONTROL_REG) && (register_id <= 0x40U)) {
+ uint8_t readval;
+
+ status = stpmic1_register_read(register_id, &readval);
+ if (status != 0) {
+ return status;
+ }
+
+ if (readval != value) {
+ return -EIO;
+ }
+ }
+#endif
+
+ return status;
+}
+
+int stpmic1_register_update(uint8_t register_id, uint8_t value, uint8_t mask)
+{
+ int status;
+ uint8_t val;
+
+ status = stpmic1_register_read(register_id, &val);
+ if (status != 0) {
+ return status;
+ }
+
+ val = (val & ~mask) | (value & mask);
+
+ return stpmic1_register_write(register_id, val);
+}
+
+void stpmic1_bind_i2c(struct i2c_handle_s *i2c_handle, uint16_t i2c_addr)
+{
+ pmic_i2c_handle = i2c_handle;
+ pmic_i2c_addr = i2c_addr;
+}
+
+void stpmic1_dump_regulators(void)
+{
+ uint32_t i;
+
+ for (i = 0U; i < MAX_REGUL; i++) {
+ const char *name __unused = regulators_table[i].dt_node_name;
+
+ VERBOSE("PMIC regul %s: %sable, %dmV",
+ name,
+ stpmic1_is_regulator_enabled(name) ? "en" : "dis",
+ stpmic1_regulator_voltage_get(name));
+ }
+}
+
+int stpmic1_get_version(unsigned long *version)
+{
+ uint8_t read_val;
+ int status;
+
+ status = stpmic1_register_read(VERSION_STATUS_REG, &read_val);
+ if (status < 0) {
+ return status;
+ }
+
+ *version = (unsigned long)read_val;
+
+ return 0;
+}
diff --git a/drivers/st/regulator/regulator_core.c b/drivers/st/regulator/regulator_core.c
new file mode 100644
index 0000000..5cc8329
--- /dev/null
+++ b/drivers/st/regulator/regulator_core.c
@@ -0,0 +1,560 @@
+/*
+ * Copyright (c) 2021-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <limits.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/regulator.h>
+#include <libfdt.h>
+
+#define MAX_PROPERTY_LEN 64
+
+static struct rdev rdev_array[PLAT_NB_RDEVS];
+
+#define for_each_rdev(rdev) \
+ for (rdev = rdev_array; rdev < (rdev_array + PLAT_NB_RDEVS); rdev++)
+
+#define for_each_registered_rdev(rdev) \
+ for (rdev = rdev_array; \
+ (rdev < (rdev_array + PLAT_NB_RDEVS)) && (rdev->desc != NULL); rdev++)
+
+static void lock_driver(const struct rdev *rdev)
+{
+ if (rdev->desc->ops->lock != NULL) {
+ rdev->desc->ops->lock(rdev->desc);
+ }
+}
+
+static void unlock_driver(const struct rdev *rdev)
+{
+ if (rdev->desc->ops->unlock != NULL) {
+ rdev->desc->ops->unlock(rdev->desc);
+ }
+}
+
+static struct rdev *regulator_get_by_phandle(int32_t phandle)
+{
+ struct rdev *rdev;
+
+ for_each_registered_rdev(rdev) {
+ if (rdev->phandle == phandle) {
+ return rdev;
+ }
+ }
+
+ WARN("%s: phandle %d not found\n", __func__, phandle);
+ return NULL;
+}
+
+/*
+ * Get a regulator from its node name
+ *
+ * @fdt - pointer to device tree memory
+ * @node_name - name of the node "ldo1"
+ * Return pointer to rdev if succeed, NULL else.
+ */
+struct rdev *regulator_get_by_name(const char *node_name)
+{
+ struct rdev *rdev;
+
+ assert(node_name != NULL);
+ VERBOSE("get %s\n", node_name);
+
+ for_each_registered_rdev(rdev) {
+ if (strcmp(rdev->desc->node_name, node_name) == 0) {
+ return rdev;
+ }
+ }
+
+ WARN("%s: %s not found\n", __func__, node_name);
+ return NULL;
+}
+
+static int32_t get_supply_phandle(const void *fdt, int node, const char *name)
+{
+ const fdt32_t *cuint;
+ int len __unused;
+ int supply_phandle = -FDT_ERR_NOTFOUND;
+ char prop_name[MAX_PROPERTY_LEN];
+
+ len = snprintf(prop_name, MAX_PROPERTY_LEN - 1, "%s-supply", name);
+ assert((len >= 0) && (len < MAX_PROPERTY_LEN - 1));
+
+ cuint = fdt_getprop(fdt, node, prop_name, NULL);
+ if (cuint != NULL) {
+ supply_phandle = fdt32_to_cpu(*cuint);
+ VERBOSE("%s: supplied by %d\n", name, supply_phandle);
+ }
+
+ return supply_phandle;
+}
+
+/*
+ * Get a regulator from a supply name
+ *
+ * @fdt - pointer to device tree memory
+ * @node - offset of the node that contains the supply description
+ * @name - name of the supply "vdd" for "vdd-supply'
+ * Return pointer to rdev if succeed, NULL else.
+ */
+struct rdev *regulator_get_by_supply_name(const void *fdt, int node, const char *name)
+{
+ const int p = get_supply_phandle(fdt, node, name);
+
+ if (p < 0) {
+ return NULL;
+ }
+
+ return regulator_get_by_phandle(p);
+}
+
+static int __regulator_set_state(struct rdev *rdev, bool state)
+{
+ if (rdev->desc->ops->set_state == NULL) {
+ return -ENODEV;
+ }
+
+ return rdev->desc->ops->set_state(rdev->desc, state);
+}
+
+/*
+ * Enable regulator
+ *
+ * @rdev - pointer to rdev struct
+ * Return 0 if succeed, non 0 else.
+ */
+int regulator_enable(struct rdev *rdev)
+{
+ int ret;
+
+ assert(rdev != NULL);
+
+ ret = __regulator_set_state(rdev, STATE_ENABLE);
+
+ udelay(rdev->enable_ramp_delay);
+
+ return ret;
+}
+
+/*
+ * Disable regulator
+ *
+ * @rdev - pointer to rdev struct
+ * Return 0 if succeed, non 0 else.
+ */
+int regulator_disable(struct rdev *rdev)
+{
+ int ret;
+
+ assert(rdev != NULL);
+
+ if (rdev->flags & REGUL_ALWAYS_ON) {
+ return 0;
+ }
+
+ ret = __regulator_set_state(rdev, STATE_DISABLE);
+
+ udelay(rdev->enable_ramp_delay);
+
+ return ret;
+}
+
+/*
+ * Regulator enabled query
+ *
+ * @rdev - pointer to rdev struct
+ * Return 0 if disabled, 1 if enabled, <0 else.
+ */
+int regulator_is_enabled(const struct rdev *rdev)
+{
+ int ret;
+
+ assert(rdev != NULL);
+
+ VERBOSE("%s: is en\n", rdev->desc->node_name);
+
+ if (rdev->desc->ops->get_state == NULL) {
+ return -ENODEV;
+ }
+
+ lock_driver(rdev);
+
+ ret = rdev->desc->ops->get_state(rdev->desc);
+ if (ret < 0) {
+ ERROR("regul %s get state failed: err:%d\n",
+ rdev->desc->node_name, ret);
+ }
+
+ unlock_driver(rdev);
+
+ return ret;
+}
+
+/*
+ * Set regulator voltage
+ *
+ * @rdev - pointer to rdev struct
+ * @mvolt - Target voltage level in millivolt
+ * Return 0 if succeed, non 0 else.
+ */
+int regulator_set_voltage(struct rdev *rdev, uint16_t mvolt)
+{
+ int ret;
+
+ assert(rdev != NULL);
+
+ VERBOSE("%s: set mvolt\n", rdev->desc->node_name);
+
+ if (rdev->desc->ops->set_voltage == NULL) {
+ return -ENODEV;
+ }
+
+ if ((mvolt < rdev->min_mv) || (mvolt > rdev->max_mv)) {
+ return -EPERM;
+ }
+
+ lock_driver(rdev);
+
+ ret = rdev->desc->ops->set_voltage(rdev->desc, mvolt);
+ if (ret < 0) {
+ ERROR("regul %s set volt failed: err:%d\n",
+ rdev->desc->node_name, ret);
+ }
+
+ unlock_driver(rdev);
+
+ return ret;
+}
+
+/*
+ * Set regulator min voltage
+ *
+ * @rdev - pointer to rdev struct
+ * Return 0 if succeed, non 0 else.
+ */
+int regulator_set_min_voltage(struct rdev *rdev)
+{
+ return regulator_set_voltage(rdev, rdev->min_mv);
+}
+
+/*
+ * Get regulator voltage
+ *
+ * @rdev - pointer to rdev struct
+ * Return milli volts if succeed, <0 else.
+ */
+int regulator_get_voltage(const struct rdev *rdev)
+{
+ int ret;
+
+ assert(rdev != NULL);
+
+ VERBOSE("%s: get volt\n", rdev->desc->node_name);
+
+ if (rdev->desc->ops->get_voltage == NULL) {
+ return rdev->min_mv;
+ }
+
+ lock_driver(rdev);
+
+ ret = rdev->desc->ops->get_voltage(rdev->desc);
+ if (ret < 0) {
+ ERROR("regul %s get voltage failed: err:%d\n",
+ rdev->desc->node_name, ret);
+ }
+
+ unlock_driver(rdev);
+
+ return ret;
+}
+
+/*
+ * List regulator voltages
+ *
+ * @rdev - pointer to rdev struct
+ * @levels - out: array of supported millitvolt levels from min to max value
+ * @count - out: number of possible millivolt values
+ * Return 0 if succeed, non 0 else.
+ */
+int regulator_list_voltages(const struct rdev *rdev, const uint16_t **levels, size_t *count)
+{
+ int ret;
+ size_t n;
+
+ assert(rdev != NULL);
+ assert(levels != NULL);
+ assert(count != NULL);
+
+ VERBOSE("%s: list volt\n", rdev->desc->node_name);
+
+ if (rdev->desc->ops->list_voltages == NULL) {
+ return -ENODEV;
+ }
+
+ lock_driver(rdev);
+
+ ret = rdev->desc->ops->list_voltages(rdev->desc, levels, count);
+
+ unlock_driver(rdev);
+
+ if (ret < 0) {
+ ERROR("regul %s list_voltages failed: err: %d\n",
+ rdev->desc->node_name, ret);
+ return ret;
+ }
+
+ /*
+ * Reduce the possible values depending on min and max from device-tree
+ */
+ n = *count;
+ while ((n > 1U) && ((*levels)[n - 1U] > rdev->max_mv)) {
+ n--;
+ }
+
+ /* Verify that max val is a valid value */
+ if (rdev->max_mv != (*levels)[n - 1]) {
+ ERROR("regul %s: max value %u is invalid\n",
+ rdev->desc->node_name, rdev->max_mv);
+ return -EINVAL;
+ }
+
+ while ((n > 1U) && ((*levels[0U]) < rdev->min_mv)) {
+ (*levels)++;
+ n--;
+ }
+
+ /* Verify that min is not too high */
+ if (n == 0U) {
+ ERROR("regul %s set min voltage is too high\n",
+ rdev->desc->node_name);
+ return -EINVAL;
+ }
+
+ /* Verify that min val is a valid vlue */
+ if (rdev->min_mv != (*levels)[0U]) {
+ ERROR("regul %s: min value %u is invalid\n",
+ rdev->desc->node_name, rdev->min_mv);
+ return -EINVAL;
+ }
+
+ *count = n;
+
+ VERBOSE("rdev->min_mv=%u rdev->max_mv=%u\n", rdev->min_mv, rdev->max_mv);
+
+ return 0;
+}
+
+/*
+ * Get regulator voltages range
+ *
+ * @rdev - pointer to rdev struct
+ * @min_mv - out: min possible millivolt value
+ * @max_mv - out: max possible millivolt value
+ * Return 0 if succeed, non 0 else.
+ */
+void regulator_get_range(const struct rdev *rdev, uint16_t *min_mv, uint16_t *max_mv)
+{
+ assert(rdev != NULL);
+
+ if (min_mv != NULL) {
+ *min_mv = rdev->min_mv;
+ }
+ if (max_mv != NULL) {
+ *max_mv = rdev->max_mv;
+ }
+}
+
+/*
+ * Set regulator flag
+ *
+ * @rdev - pointer to rdev struct
+ * @flag - flag value to set (eg: REGUL_OCP)
+ * Return 0 if succeed, non 0 else.
+ */
+int regulator_set_flag(struct rdev *rdev, uint16_t flag)
+{
+ int ret;
+
+ /* check that only one bit is set on flag */
+ if (__builtin_popcount(flag) != 1) {
+ return -EINVAL;
+ }
+
+ /* REGUL_ALWAYS_ON and REGUL_BOOT_ON are internal properties of the core */
+ if ((flag == REGUL_ALWAYS_ON) || (flag == REGUL_BOOT_ON)) {
+ rdev->flags |= flag;
+ return 0;
+ }
+
+ if (rdev->desc->ops->set_flag == NULL) {
+ ERROR("%s can not set any flag\n", rdev->desc->node_name);
+ return -ENODEV;
+ }
+
+ lock_driver(rdev);
+
+ ret = rdev->desc->ops->set_flag(rdev->desc, flag);
+
+ unlock_driver(rdev);
+
+ if (ret != 0) {
+ ERROR("%s: could not set flag %d ret=%d\n",
+ rdev->desc->node_name, flag, ret);
+ return ret;
+ }
+
+ rdev->flags |= flag;
+
+ return 0;
+}
+
+static int parse_properties(const void *fdt, struct rdev *rdev, int node)
+{
+ int ret;
+
+ if (fdt_getprop(fdt, node, "regulator-always-on", NULL) != NULL) {
+ VERBOSE("%s: set regulator-always-on\n", rdev->desc->node_name);
+ ret = regulator_set_flag(rdev, REGUL_ALWAYS_ON);
+ if (ret != 0) {
+ return ret;
+ }
+ }
+
+ return 0;
+}
+
+/*
+ * Parse the device-tree for a regulator
+ *
+ * Read min/max voltage from dt and check its validity
+ * Read the properties, and call the driver to set flags
+ * Read power supply phandle
+ * Read and store low power mode states
+ *
+ * @rdev - pointer to rdev struct
+ * @node - device-tree node offset of the regulator
+ * Return 0 if disabled, 1 if enabled, <0 else.
+ */
+static int parse_dt(struct rdev *rdev, int node)
+{
+ void *fdt;
+ const fdt32_t *cuint;
+ const uint16_t *levels;
+ size_t size;
+ int ret;
+
+ VERBOSE("%s: parse dt\n", rdev->desc->node_name);
+
+ if (fdt_get_address(&fdt) == 0) {
+ return -ENOENT;
+ }
+
+ rdev->phandle = fdt_get_phandle(fdt, node);
+
+ cuint = fdt_getprop(fdt, node, "regulator-min-microvolt", NULL);
+ if (cuint != NULL) {
+ uint16_t min_mv;
+
+ min_mv = (uint16_t)(fdt32_to_cpu(*cuint) / 1000U);
+ VERBOSE("%s: min_mv=%d\n", rdev->desc->node_name, (int)min_mv);
+ if (min_mv <= rdev->max_mv) {
+ rdev->min_mv = min_mv;
+ } else {
+ ERROR("%s: min_mv=%d is too high\n",
+ rdev->desc->node_name, (int)min_mv);
+ return -EINVAL;
+ }
+ }
+
+ cuint = fdt_getprop(fdt, node, "regulator-max-microvolt", NULL);
+ if (cuint != NULL) {
+ uint16_t max_mv;
+
+ max_mv = (uint16_t)(fdt32_to_cpu(*cuint) / 1000U);
+ VERBOSE("%s: max_mv=%d\n", rdev->desc->node_name, (int)max_mv);
+ if (max_mv >= rdev->min_mv) {
+ rdev->max_mv = max_mv;
+ } else {
+ ERROR("%s: max_mv=%d is too low\n",
+ rdev->desc->node_name, (int)max_mv);
+ return -EINVAL;
+ }
+ }
+
+ /* validate that min and max values can be used */
+ ret = regulator_list_voltages(rdev, &levels, &size);
+ if ((ret != 0) && (ret != -ENODEV)) {
+ return ret;
+ }
+
+ ret = parse_properties(fdt, rdev, node);
+ if (ret != 0) {
+ return ret;
+ }
+
+ return 0;
+}
+
+/*
+ * Register a regulator driver in regulator framework.
+ * Initialize voltage range from driver description
+ *
+ * @desc - pointer to the regulator description
+ * @node - device-tree node offset of the regulator
+ * Return 0 if succeed, non 0 else.
+ */
+int regulator_register(const struct regul_description *desc, int node)
+{
+ struct rdev *rdev;
+
+ assert(desc != NULL);
+
+ VERBOSE("register %s\n", desc->node_name);
+
+ for_each_rdev(rdev) {
+ if (rdev->desc == NULL) {
+ break;
+ }
+ }
+
+ if (rdev == rdev_array + PLAT_NB_RDEVS) {
+ WARN("Not enough place for regulators, PLAT_NB_RDEVS should be increased.\n");
+ return -ENOMEM;
+ }
+
+ rdev->desc = desc;
+ rdev->enable_ramp_delay = rdev->desc->enable_ramp_delay;
+
+ if (rdev->desc->ops->list_voltages != NULL) {
+ int ret;
+ const uint16_t *levels;
+ size_t count;
+
+ lock_driver(rdev);
+
+ ret = rdev->desc->ops->list_voltages(rdev->desc, &levels, &count);
+
+ unlock_driver(rdev);
+
+ if (ret < 0) {
+ ERROR("regul %s set state failed: err:%d\n",
+ rdev->desc->node_name, ret);
+ return ret;
+ }
+
+ rdev->min_mv = levels[0];
+ rdev->max_mv = levels[count - 1U];
+ } else {
+ rdev->max_mv = UINT16_MAX;
+ }
+
+ return parse_dt(rdev, node);
+}
diff --git a/drivers/st/regulator/regulator_fixed.c b/drivers/st/regulator/regulator_fixed.c
new file mode 100644
index 0000000..f1c224e
--- /dev/null
+++ b/drivers/st/regulator/regulator_fixed.c
@@ -0,0 +1,87 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+
+#include <common/debug.h>
+#include <common/fdt_wrappers.h>
+#include <drivers/st/regulator.h>
+#include <drivers/st/regulator_fixed.h>
+#include <libfdt.h>
+
+#ifndef PLAT_NB_FIXED_REGS
+#error "Missing PLAT_NB_FIXED_REGS"
+#endif
+
+#define FIXED_NAME_LEN 32
+
+struct fixed_data {
+ char name[FIXED_NAME_LEN];
+ uint16_t volt;
+ struct regul_description desc;
+};
+
+static struct fixed_data data[PLAT_NB_FIXED_REGS];
+
+static int fixed_set_state(const struct regul_description *desc, bool state)
+{
+ return 0;
+}
+
+static int fixed_get_state(const struct regul_description *desc)
+{
+ return 1;
+}
+
+static struct regul_ops fixed_ops = {
+ .set_state = fixed_set_state,
+ .get_state = fixed_get_state,
+};
+
+int fixed_regulator_register(void)
+{
+ uint32_t count = 0;
+ void *fdt;
+ int node;
+
+ VERBOSE("fixed reg init!\n");
+
+ if (fdt_get_address(&fdt) == 0) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ fdt_for_each_compatible_node(fdt, node, "regulator-fixed") {
+ int len __unused;
+ int ret;
+ struct fixed_data *d = &data[count];
+ const char *reg_name;
+
+ reg_name = fdt_get_name(fdt, node, NULL);
+
+ VERBOSE("register fixed reg %s!\n", reg_name);
+
+ len = snprintf(d->name, FIXED_NAME_LEN - 1, "%s", reg_name);
+ assert((len > 0) && (len < (FIXED_NAME_LEN - 1)));
+
+ d->desc.node_name = d->name;
+ d->desc.driver_data = d;
+ d->desc.ops = &fixed_ops;
+
+ ret = regulator_register(&d->desc, node);
+ if (ret != 0) {
+ WARN("%s:%d failed to register %s\n", __func__,
+ __LINE__, reg_name);
+ return ret;
+ }
+
+ count++;
+ assert(count <= PLAT_NB_FIXED_REGS);
+
+ }
+
+ return 0;
+}
diff --git a/drivers/st/reset/stm32mp1_reset.c b/drivers/st/reset/stm32mp1_reset.c
new file mode 100644
index 0000000..98c8dcf
--- /dev/null
+++ b/drivers/st/reset/stm32mp1_reset.c
@@ -0,0 +1,69 @@
+/*
+ * Copyright (c) 2018-2019, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+#include <limits.h>
+
+#include <platform_def.h>
+
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32mp_reset.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+
+static uint32_t id2reg_offset(unsigned int reset_id)
+{
+ return ((reset_id & GENMASK(31, 5)) >> 5) * sizeof(uint32_t);
+}
+
+static uint8_t id2reg_bit_pos(unsigned int reset_id)
+{
+ return (uint8_t)(reset_id & GENMASK(4, 0));
+}
+
+int stm32mp_reset_assert(uint32_t id, unsigned int to_us)
+{
+ uint32_t offset = id2reg_offset(id);
+ uint32_t bitmsk = BIT(id2reg_bit_pos(id));
+ uintptr_t rcc_base = stm32mp_rcc_base();
+
+ mmio_write_32(rcc_base + offset, bitmsk);
+
+ if (to_us != 0U) {
+ uint64_t timeout_ref = timeout_init_us(to_us);
+
+ while ((mmio_read_32(rcc_base + offset) & bitmsk) == 0U) {
+ if (timeout_elapsed(timeout_ref)) {
+ return -ETIMEDOUT;
+ }
+ }
+ }
+
+ return 0;
+}
+
+int stm32mp_reset_deassert(uint32_t id, unsigned int to_us)
+{
+ uint32_t offset = id2reg_offset(id) + RCC_RSTCLRR_OFFSET;
+ uint32_t bitmsk = BIT(id2reg_bit_pos(id));
+ uintptr_t rcc_base = stm32mp_rcc_base();
+
+ mmio_write_32(rcc_base + offset, bitmsk);
+
+ if (to_us != 0U) {
+ uint64_t timeout_ref = timeout_init_us(to_us);
+
+ while ((mmio_read_32(rcc_base + offset) & bitmsk) != 0U) {
+ if (timeout_elapsed(timeout_ref)) {
+ return -ETIMEDOUT;
+ }
+ }
+ }
+
+ return 0;
+}
diff --git a/drivers/st/spi/stm32_qspi.c b/drivers/st/spi/stm32_qspi.c
new file mode 100644
index 0000000..73aa9ac
--- /dev/null
+++ b/drivers/st/spi/stm32_qspi.c
@@ -0,0 +1,508 @@
+/*
+ * Copyright (c) 2019-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+ */
+
+#include <inttypes.h>
+
+#include <common/debug.h>
+#include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/spi_mem.h>
+#include <drivers/st/stm32_gpio.h>
+#include <drivers/st/stm32_qspi.h>
+#include <drivers/st/stm32mp_reset.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+/* Timeout for device interface reset */
+#define TIMEOUT_US_1_MS 1000U
+
+/* QUADSPI registers */
+#define QSPI_CR 0x00U
+#define QSPI_DCR 0x04U
+#define QSPI_SR 0x08U
+#define QSPI_FCR 0x0CU
+#define QSPI_DLR 0x10U
+#define QSPI_CCR 0x14U
+#define QSPI_AR 0x18U
+#define QSPI_ABR 0x1CU
+#define QSPI_DR 0x20U
+#define QSPI_PSMKR 0x24U
+#define QSPI_PSMAR 0x28U
+#define QSPI_PIR 0x2CU
+#define QSPI_LPTR 0x30U
+
+/* QUADSPI control register */
+#define QSPI_CR_EN BIT(0)
+#define QSPI_CR_ABORT BIT(1)
+#define QSPI_CR_DMAEN BIT(2)
+#define QSPI_CR_TCEN BIT(3)
+#define QSPI_CR_SSHIFT BIT(4)
+#define QSPI_CR_DFM BIT(6)
+#define QSPI_CR_FSEL BIT(7)
+#define QSPI_CR_FTHRES_SHIFT 8U
+#define QSPI_CR_TEIE BIT(16)
+#define QSPI_CR_TCIE BIT(17)
+#define QSPI_CR_FTIE BIT(18)
+#define QSPI_CR_SMIE BIT(19)
+#define QSPI_CR_TOIE BIT(20)
+#define QSPI_CR_APMS BIT(22)
+#define QSPI_CR_PMM BIT(23)
+#define QSPI_CR_PRESCALER_MASK GENMASK_32(31, 24)
+#define QSPI_CR_PRESCALER_SHIFT 24U
+
+/* QUADSPI device configuration register */
+#define QSPI_DCR_CKMODE BIT(0)
+#define QSPI_DCR_CSHT_MASK GENMASK_32(10, 8)
+#define QSPI_DCR_CSHT_SHIFT 8U
+#define QSPI_DCR_FSIZE_MASK GENMASK_32(20, 16)
+#define QSPI_DCR_FSIZE_SHIFT 16U
+
+/* QUADSPI status register */
+#define QSPI_SR_TEF BIT(0)
+#define QSPI_SR_TCF BIT(1)
+#define QSPI_SR_FTF BIT(2)
+#define QSPI_SR_SMF BIT(3)
+#define QSPI_SR_TOF BIT(4)
+#define QSPI_SR_BUSY BIT(5)
+
+/* QUADSPI flag clear register */
+#define QSPI_FCR_CTEF BIT(0)
+#define QSPI_FCR_CTCF BIT(1)
+#define QSPI_FCR_CSMF BIT(3)
+#define QSPI_FCR_CTOF BIT(4)
+
+/* QUADSPI communication configuration register */
+#define QSPI_CCR_DDRM BIT(31)
+#define QSPI_CCR_DHHC BIT(30)
+#define QSPI_CCR_SIOO BIT(28)
+#define QSPI_CCR_FMODE_SHIFT 26U
+#define QSPI_CCR_DMODE_SHIFT 24U
+#define QSPI_CCR_DCYC_SHIFT 18U
+#define QSPI_CCR_ABSIZE_SHIFT 16U
+#define QSPI_CCR_ABMODE_SHIFT 14U
+#define QSPI_CCR_ADSIZE_SHIFT 12U
+#define QSPI_CCR_ADMODE_SHIFT 10U
+#define QSPI_CCR_IMODE_SHIFT 8U
+#define QSPI_CCR_IND_WRITE 0U
+#define QSPI_CCR_IND_READ 1U
+#define QSPI_CCR_MEM_MAP 3U
+
+#define QSPI_MAX_CHIP 2U
+
+#define QSPI_FIFO_TIMEOUT_US 30U
+#define QSPI_CMD_TIMEOUT_US 1000U
+#define QSPI_BUSY_TIMEOUT_US 100U
+#define QSPI_ABT_TIMEOUT_US 100U
+
+#define DT_QSPI_COMPAT "st,stm32f469-qspi"
+
+#define FREQ_100MHZ 100000000U
+
+struct stm32_qspi_ctrl {
+ uintptr_t reg_base;
+ uintptr_t mm_base;
+ size_t mm_size;
+ unsigned long clock_id;
+ unsigned int reset_id;
+};
+
+static struct stm32_qspi_ctrl stm32_qspi;
+
+static uintptr_t qspi_base(void)
+{
+ return stm32_qspi.reg_base;
+}
+
+static int stm32_qspi_wait_for_not_busy(void)
+{
+ uint64_t timeout = timeout_init_us(QSPI_BUSY_TIMEOUT_US);
+
+ while ((mmio_read_32(qspi_base() + QSPI_SR) & QSPI_SR_BUSY) != 0U) {
+ if (timeout_elapsed(timeout)) {
+ ERROR("%s: busy timeout\n", __func__);
+ return -ETIMEDOUT;
+ }
+ }
+
+ return 0;
+}
+
+static int stm32_qspi_wait_cmd(const struct spi_mem_op *op)
+{
+ int ret = 0;
+ uint64_t timeout;
+
+ timeout = timeout_init_us(QSPI_CMD_TIMEOUT_US);
+ while ((mmio_read_32(qspi_base() + QSPI_SR) & QSPI_SR_TCF) == 0U) {
+ if (timeout_elapsed(timeout)) {
+ ret = -ETIMEDOUT;
+ break;
+ }
+ }
+
+ if (ret == 0) {
+ if ((mmio_read_32(qspi_base() + QSPI_SR) & QSPI_SR_TEF) != 0U) {
+ ERROR("%s: transfer error\n", __func__);
+ ret = -EIO;
+ }
+ } else {
+ ERROR("%s: cmd timeout\n", __func__);
+ }
+
+ /* Clear flags */
+ mmio_write_32(qspi_base() + QSPI_FCR, QSPI_FCR_CTCF | QSPI_FCR_CTEF);
+
+ if (ret == 0) {
+ ret = stm32_qspi_wait_for_not_busy();
+ }
+
+ return ret;
+}
+
+static void stm32_qspi_read_fifo(uint8_t *val, uintptr_t addr)
+{
+ *val = mmio_read_8(addr);
+}
+
+static void stm32_qspi_write_fifo(uint8_t *val, uintptr_t addr)
+{
+ mmio_write_8(addr, *val);
+}
+
+static int stm32_qspi_poll(const struct spi_mem_op *op)
+{
+ void (*fifo)(uint8_t *val, uintptr_t addr);
+ uint32_t len;
+ uint8_t *buf;
+
+ if (op->data.dir == SPI_MEM_DATA_IN) {
+ fifo = stm32_qspi_read_fifo;
+ } else {
+ fifo = stm32_qspi_write_fifo;
+ }
+
+ buf = (uint8_t *)op->data.buf;
+
+ for (len = op->data.nbytes; len != 0U; len--) {
+ uint64_t timeout = timeout_init_us(QSPI_FIFO_TIMEOUT_US);
+
+ while ((mmio_read_32(qspi_base() + QSPI_SR) &
+ QSPI_SR_FTF) == 0U) {
+ if (timeout_elapsed(timeout)) {
+ ERROR("%s: fifo timeout\n", __func__);
+ return -ETIMEDOUT;
+ }
+ }
+
+ fifo(buf++, qspi_base() + QSPI_DR);
+ }
+
+ return 0;
+}
+
+static int stm32_qspi_mm(const struct spi_mem_op *op)
+{
+ memcpy(op->data.buf,
+ (void *)(stm32_qspi.mm_base + (size_t)op->addr.val),
+ op->data.nbytes);
+
+ return 0;
+}
+
+static int stm32_qspi_tx(const struct spi_mem_op *op, uint8_t mode)
+{
+ if (op->data.nbytes == 0U) {
+ return 0;
+ }
+
+ if (mode == QSPI_CCR_MEM_MAP) {
+ return stm32_qspi_mm(op);
+ }
+
+ return stm32_qspi_poll(op);
+}
+
+static unsigned int stm32_qspi_get_mode(uint8_t buswidth)
+{
+ if (buswidth == 4U) {
+ return 3U;
+ }
+
+ return buswidth;
+}
+
+static int stm32_qspi_exec_op(const struct spi_mem_op *op)
+{
+ uint64_t timeout;
+ uint32_t ccr;
+ size_t addr_max;
+ uint8_t mode = QSPI_CCR_IND_WRITE;
+ int ret;
+
+ VERBOSE("%s: cmd:%x mode:%d.%d.%d.%d addr:%" PRIx64 " len:%x\n",
+ __func__, op->cmd.opcode, op->cmd.buswidth, op->addr.buswidth,
+ op->dummy.buswidth, op->data.buswidth,
+ op->addr.val, op->data.nbytes);
+
+ addr_max = op->addr.val + op->data.nbytes + 1U;
+
+ if ((op->data.dir == SPI_MEM_DATA_IN) && (op->data.nbytes != 0U)) {
+ if ((addr_max < stm32_qspi.mm_size) &&
+ (op->addr.buswidth != 0U)) {
+ mode = QSPI_CCR_MEM_MAP;
+ } else {
+ mode = QSPI_CCR_IND_READ;
+ }
+ }
+
+ if (op->data.nbytes != 0U) {
+ mmio_write_32(qspi_base() + QSPI_DLR, op->data.nbytes - 1U);
+ }
+
+ ccr = mode << QSPI_CCR_FMODE_SHIFT;
+ ccr |= op->cmd.opcode;
+ ccr |= stm32_qspi_get_mode(op->cmd.buswidth) << QSPI_CCR_IMODE_SHIFT;
+
+ if (op->addr.nbytes != 0U) {
+ ccr |= (op->addr.nbytes - 1U) << QSPI_CCR_ADSIZE_SHIFT;
+ ccr |= stm32_qspi_get_mode(op->addr.buswidth) <<
+ QSPI_CCR_ADMODE_SHIFT;
+ }
+
+ if ((op->dummy.buswidth != 0U) && (op->dummy.nbytes != 0U)) {
+ ccr |= (op->dummy.nbytes * 8U / op->dummy.buswidth) <<
+ QSPI_CCR_DCYC_SHIFT;
+ }
+
+ if (op->data.nbytes != 0U) {
+ ccr |= stm32_qspi_get_mode(op->data.buswidth) <<
+ QSPI_CCR_DMODE_SHIFT;
+ }
+
+ mmio_write_32(qspi_base() + QSPI_CCR, ccr);
+
+ if ((op->addr.nbytes != 0U) && (mode != QSPI_CCR_MEM_MAP)) {
+ mmio_write_32(qspi_base() + QSPI_AR, op->addr.val);
+ }
+
+ ret = stm32_qspi_tx(op, mode);
+
+ /*
+ * Abort in:
+ * - Error case.
+ * - Memory mapped read: prefetching must be stopped if we read the last
+ * byte of device (device size - fifo size). If device size is not
+ * known then prefetching is always stopped.
+ */
+ if ((ret != 0) || (mode == QSPI_CCR_MEM_MAP)) {
+ goto abort;
+ }
+
+ /* Wait end of TX in indirect mode */
+ ret = stm32_qspi_wait_cmd(op);
+ if (ret != 0) {
+ goto abort;
+ }
+
+ return 0;
+
+abort:
+ mmio_setbits_32(qspi_base() + QSPI_CR, QSPI_CR_ABORT);
+
+ /* Wait clear of abort bit by hardware */
+ timeout = timeout_init_us(QSPI_ABT_TIMEOUT_US);
+ while ((mmio_read_32(qspi_base() + QSPI_CR) & QSPI_CR_ABORT) != 0U) {
+ if (timeout_elapsed(timeout)) {
+ ret = -ETIMEDOUT;
+ break;
+ }
+ }
+
+ mmio_write_32(qspi_base() + QSPI_FCR, QSPI_FCR_CTCF);
+
+ if (ret != 0) {
+ ERROR("%s: exec op error\n", __func__);
+ }
+
+ return ret;
+}
+
+static int stm32_qspi_claim_bus(unsigned int cs)
+{
+ uint32_t cr;
+
+ if (cs >= QSPI_MAX_CHIP) {
+ return -ENODEV;
+ }
+
+ /* Set chip select and enable the controller */
+ cr = QSPI_CR_EN;
+ if (cs == 1U) {
+ cr |= QSPI_CR_FSEL;
+ }
+
+ mmio_clrsetbits_32(qspi_base() + QSPI_CR, QSPI_CR_FSEL, cr);
+
+ return 0;
+}
+
+static void stm32_qspi_release_bus(void)
+{
+ mmio_clrbits_32(qspi_base() + QSPI_CR, QSPI_CR_EN);
+}
+
+static int stm32_qspi_set_speed(unsigned int hz)
+{
+ unsigned long qspi_clk = clk_get_rate(stm32_qspi.clock_id);
+ uint32_t prescaler = UINT8_MAX;
+ uint32_t csht;
+ int ret;
+
+ if (qspi_clk == 0U) {
+ return -EINVAL;
+ }
+
+ if (hz > 0U) {
+ prescaler = div_round_up(qspi_clk, hz) - 1U;
+ if (prescaler > UINT8_MAX) {
+ prescaler = UINT8_MAX;
+ }
+ }
+
+ csht = div_round_up((5U * qspi_clk) / (prescaler + 1U), FREQ_100MHZ);
+ csht = ((csht - 1U) << QSPI_DCR_CSHT_SHIFT) & QSPI_DCR_CSHT_MASK;
+
+ ret = stm32_qspi_wait_for_not_busy();
+ if (ret != 0) {
+ return ret;
+ }
+
+ mmio_clrsetbits_32(qspi_base() + QSPI_CR, QSPI_CR_PRESCALER_MASK,
+ prescaler << QSPI_CR_PRESCALER_SHIFT);
+
+ mmio_clrsetbits_32(qspi_base() + QSPI_DCR, QSPI_DCR_CSHT_MASK, csht);
+
+ VERBOSE("%s: speed=%lu\n", __func__, qspi_clk / (prescaler + 1U));
+
+ return 0;
+}
+
+static int stm32_qspi_set_mode(unsigned int mode)
+{
+ int ret;
+
+ ret = stm32_qspi_wait_for_not_busy();
+ if (ret != 0) {
+ return ret;
+ }
+
+ if ((mode & SPI_CS_HIGH) != 0U) {
+ return -ENODEV;
+ }
+
+ if (((mode & SPI_CPHA) != 0U) && ((mode & SPI_CPOL) != 0U)) {
+ mmio_setbits_32(qspi_base() + QSPI_DCR, QSPI_DCR_CKMODE);
+ } else if (((mode & SPI_CPHA) == 0U) && ((mode & SPI_CPOL) == 0U)) {
+ mmio_clrbits_32(qspi_base() + QSPI_DCR, QSPI_DCR_CKMODE);
+ } else {
+ return -ENODEV;
+ }
+
+ VERBOSE("%s: mode=0x%x\n", __func__, mode);
+
+ if ((mode & SPI_RX_QUAD) != 0U) {
+ VERBOSE("rx: quad\n");
+ } else if ((mode & SPI_RX_DUAL) != 0U) {
+ VERBOSE("rx: dual\n");
+ } else {
+ VERBOSE("rx: single\n");
+ }
+
+ if ((mode & SPI_TX_QUAD) != 0U) {
+ VERBOSE("tx: quad\n");
+ } else if ((mode & SPI_TX_DUAL) != 0U) {
+ VERBOSE("tx: dual\n");
+ } else {
+ VERBOSE("tx: single\n");
+ }
+
+ return 0;
+}
+
+static const struct spi_bus_ops stm32_qspi_bus_ops = {
+ .claim_bus = stm32_qspi_claim_bus,
+ .release_bus = stm32_qspi_release_bus,
+ .set_speed = stm32_qspi_set_speed,
+ .set_mode = stm32_qspi_set_mode,
+ .exec_op = stm32_qspi_exec_op,
+};
+
+int stm32_qspi_init(void)
+{
+ size_t size;
+ int qspi_node;
+ struct dt_node_info info;
+ void *fdt = NULL;
+ int ret;
+
+ if (fdt_get_address(&fdt) == 0) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ qspi_node = dt_get_node(&info, -1, DT_QSPI_COMPAT);
+ if (qspi_node < 0) {
+ ERROR("No QSPI ctrl found\n");
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ if (info.status == DT_DISABLED) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ ret = fdt_get_reg_props_by_name(fdt, qspi_node, "qspi",
+ &stm32_qspi.reg_base, &size);
+ if (ret != 0) {
+ return ret;
+ }
+
+ ret = fdt_get_reg_props_by_name(fdt, qspi_node, "qspi_mm",
+ &stm32_qspi.mm_base,
+ &stm32_qspi.mm_size);
+ if (ret != 0) {
+ return ret;
+ }
+
+ if (dt_set_pinctrl_config(qspi_node) != 0) {
+ return -FDT_ERR_BADVALUE;
+ }
+
+ if ((info.clock < 0) || (info.reset < 0)) {
+ return -FDT_ERR_BADVALUE;
+ }
+
+ stm32_qspi.clock_id = (unsigned long)info.clock;
+ stm32_qspi.reset_id = (unsigned int)info.reset;
+
+ clk_enable(stm32_qspi.clock_id);
+
+ ret = stm32mp_reset_assert(stm32_qspi.reset_id, TIMEOUT_US_1_MS);
+ if (ret != 0) {
+ panic();
+ }
+ ret = stm32mp_reset_deassert(stm32_qspi.reset_id, TIMEOUT_US_1_MS);
+ if (ret != 0) {
+ panic();
+ }
+
+ mmio_write_32(qspi_base() + QSPI_CR, QSPI_CR_SSHIFT);
+ mmio_write_32(qspi_base() + QSPI_DCR, QSPI_DCR_FSIZE_MASK);
+
+ return spi_mem_init_slave(fdt, qspi_node, &stm32_qspi_bus_ops);
+};
diff --git a/drivers/st/uart/aarch32/stm32_console.S b/drivers/st/uart/aarch32/stm32_console.S
new file mode 100644
index 0000000..e3e0e67
--- /dev/null
+++ b/drivers/st/uart/aarch32/stm32_console.S
@@ -0,0 +1,261 @@
+/*
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <asm_macros.S>
+#include <assert_macros.S>
+#include <console_macros.S>
+#include <drivers/st/stm32_console.h>
+#include <drivers/st/stm32_uart_regs.h>
+
+#define USART_TIMEOUT 0x1000
+
+ /*
+ * "core" functions are low-level implementations that don't require
+ * writeable memory and are thus safe to call in BL1 crash context.
+ */
+ .globl console_stm32_core_init
+ .globl console_stm32_core_putc
+ .globl console_stm32_core_getc
+ .globl console_stm32_core_flush
+
+ .globl console_stm32_putc
+ .globl console_stm32_flush
+
+
+
+ /* -----------------------------------------------------------------
+ * int console_core_init(uintptr_t base_addr,
+ * unsigned int uart_clk,
+ * unsigned int baud_rate)
+ *
+ * Function to initialize the console without a C Runtime to print
+ * debug information. This function will be accessed by console_init
+ * and crash reporting.
+ *
+ * In: r0 - console base address
+ * r1 - Uart clock in Hz
+ * r2 - Baud rate
+ * Out: return 1 on success else 0 on error
+ * Clobber list : r1, r2, r3
+ * -----------------------------------------------------------------
+ */
+func console_stm32_core_init
+ /* Check the input base address */
+ cmp r0, #0
+ beq core_init_fail
+#if !defined(IMAGE_BL2)
+#if STM32MP_RECONFIGURE_CONSOLE
+ /* UART clock rate is set to 0 in BL32, skip init in that case */
+ cmp r1, #0
+ beq 1f
+#else /* STM32MP_RECONFIGURE_CONSOLE */
+ /* Skip UART initialization if it is already enabled */
+ ldr r3, [r0, #USART_CR1]
+ ands r3, r3, #USART_CR1_UE
+ bne 1f
+#endif /* STM32MP_RECONFIGURE_CONSOLE */
+#endif /* IMAGE_BL2 */
+ /* Check baud rate and uart clock for sanity */
+ cmp r1, #0
+ beq core_init_fail
+ cmp r2, #0
+ beq core_init_fail
+ /* Disable UART */
+ ldr r3, [r0, #USART_CR1]
+ bic r3, r3, #USART_CR1_UE
+ str r3, [r0, #USART_CR1]
+ /* Configure UART */
+ orr r3, r3, #(USART_CR1_TE | USART_CR1_FIFOEN)
+ str r3, [r0, #USART_CR1]
+ ldr r3, [r0, #USART_CR2]
+ bic r3, r3, #USART_CR2_STOP
+ str r3, [r0, #USART_CR2]
+ /* Divisor = (Uart clock + (baudrate / 2)) / baudrate */
+ lsr r3, r2, #1
+ add r3, r1, r3
+ udiv r3, r3, r2
+ cmp r3, #16
+ bhi 2f
+ /* Oversampling 8 */
+ /* Divisor = (2 * Uart clock + (baudrate / 2)) / baudrate */
+ lsr r3, r2, #1
+ add r3, r3, r1, lsl #1
+ udiv r3, r3, r2
+ and r1, r3, #USART_BRR_DIV_FRACTION
+ lsr r1, r1, #1
+ bic r3, r3, #USART_BRR_DIV_FRACTION
+ orr r3, r3, r1
+ ldr r1, [r0, #USART_CR1]
+ orr r1, r1, #USART_CR1_OVER8
+ str r1, [r0, #USART_CR1]
+2:
+ str r3, [r0, #USART_BRR]
+ /* Enable UART */
+ ldr r3, [r0, #USART_CR1]
+ orr r3, r3, #USART_CR1_UE
+ str r3, [r0, #USART_CR1]
+ /* Check TEACK bit */
+ mov r2, #USART_TIMEOUT
+teack_loop:
+ subs r2, r2, #1
+ beq core_init_fail
+ ldr r3, [r0, #USART_ISR]
+ tst r3, #USART_ISR_TEACK
+ beq teack_loop
+1:
+ mov r0, #1
+ bx lr
+core_init_fail:
+ mov r0, #0
+ bx lr
+endfunc console_stm32_core_init
+
+ .globl console_stm32_register
+
+ /* -------------------------------------------------------
+ * int console_stm32_register(uintptr_t baseaddr,
+ * uint32_t clock, uint32_t baud,
+ * console_t *console);
+ * Function to initialize and register a new STM32
+ * console. Storage passed in for the console struct
+ * *must* be persistent (i.e. not from the stack).
+ * In: r0 - UART register base address
+ * r1 - UART clock in Hz
+ * r2 - Baud rate
+ * r3 - pointer to empty console_t struct
+ * Out: return 1 on success, 0 on error
+ * Clobber list : r0, r1, r2
+ * -------------------------------------------------------
+ */
+func console_stm32_register
+ push {r4, lr}
+ mov r4, r3
+ cmp r4, #0
+ beq register_fail
+ str r0, [r4, #CONSOLE_T_BASE]
+
+ bl console_stm32_core_init
+ cmp r0, #0
+ beq register_fail
+
+ mov r0, r4
+ pop {r4, lr}
+ finish_console_register stm32 putc=1, getc=0, flush=1
+
+register_fail:
+ pop {r4, pc}
+endfunc console_stm32_register
+
+ /* ---------------------------------------------------------------
+ * int console_core_putc(int c, uintptr_t base_addr)
+ *
+ * Function to output a character over the console. It returns the
+ * character printed on success or -1 on error.
+ *
+ * In : r0 - character to be printed
+ * r1 - console base address
+ * Out : return -1 on error else return character.
+ * Clobber list : r2
+ * ---------------------------------------------------------------
+ */
+func console_stm32_core_putc
+ /* Check the input parameter */
+ cmp r1, #0
+ beq putc_error
+
+ /* Check Transmit Data Register Empty */
+txe_loop:
+ ldr r2, [r1, #USART_ISR]
+ tst r2, #USART_ISR_TXE
+ beq txe_loop
+ str r0, [r1, #USART_TDR]
+ /* Check transmit complete flag */
+tc_loop:
+ ldr r2, [r1, #USART_ISR]
+ tst r2, #USART_ISR_TC
+ beq tc_loop
+ bx lr
+putc_error:
+ mov r0, #-1
+ bx lr
+endfunc console_stm32_core_putc
+
+ /* ------------------------------------------------------------
+ * int console_stm32_putc(int c, console_t *console)
+ * Function to output a character over the console. It
+ * returns the character printed on success or -1 on error.
+ * In: r0 - character to be printed
+ * r1 - pointer to console_t structure
+ * Out : return -1 on error else return character.
+ * Clobber list: r2
+ * ------------------------------------------------------------
+ */
+func console_stm32_putc
+#if ENABLE_ASSERTIONS
+ cmp r1, #0
+ ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+ ldr r1, [r1, #CONSOLE_T_BASE]
+ b console_stm32_core_putc
+endfunc console_stm32_putc
+
+ /* -----------------------------------------------------------
+ * int console_core_getc(uintptr_t base_addr)
+ *
+ * Function to get a character from the console.
+ * It returns the character grabbed on success or -1 on error.
+ *
+ * In : r0 - console base address
+ * Out : return -1.
+ * Clobber list : r0, r1
+ * -----------------------------------------------------------
+ */
+func console_stm32_core_getc
+ /* Not supported */
+ mov r0, #-1
+ bx lr
+endfunc console_stm32_core_getc
+
+ /* ---------------------------------------------------------------
+ * void console_core_flush(uintptr_t base_addr)
+ *
+ * Function to force a write of all buffered data that hasn't been
+ * output.
+ *
+ * In : r0 - console base address
+ * Out : void.
+ * Clobber list : r0, r1
+ * ---------------------------------------------------------------
+ */
+func console_stm32_core_flush
+#if ENABLE_ASSERTIONS
+ cmp r0, #0
+ ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+ /* Check Transmit Data Register Empty */
+txe_loop_3:
+ ldr r1, [r0, #USART_ISR]
+ tst r1, #USART_ISR_TXE
+ beq txe_loop_3
+ bx lr
+endfunc console_stm32_core_flush
+
+ /* ------------------------------------------------------
+ * void console_stm32_flush(console_t *console)
+ * Function to force a write of all buffered
+ * data that hasn't been output.
+ * In : r0 - pointer to console_t structure
+ * Out : void.
+ * Clobber list: r0, r1
+ * ------------------------------------------------------
+ */
+func console_stm32_flush
+#if ENABLE_ASSERTIONS
+ cmp r0, #0
+ ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+ ldr r0, [r0, #CONSOLE_T_BASE]
+ b console_stm32_core_flush
+endfunc console_stm32_flush
diff --git a/drivers/st/uart/stm32_uart.c b/drivers/st/uart/stm32_uart.c
new file mode 100644
index 0000000..63970c7
--- /dev/null
+++ b/drivers/st/uart/stm32_uart.c
@@ -0,0 +1,439 @@
+/*
+ * Copyright (c) 2021-2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+
+#include <common/bl_common.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32_gpio.h>
+#include <drivers/st/stm32_uart.h>
+#include <drivers/st/stm32_uart_regs.h>
+#include <drivers/st/stm32mp_clkfunc.h>
+#include <lib/mmio.h>
+
+#include <platform_def.h>
+
+/* UART time-out value */
+#define STM32_UART_TIMEOUT_US 20000U
+
+/* Mask to clear ALL the configuration registers */
+
+#define STM32_UART_CR1_FIELDS \
+ (USART_CR1_M | USART_CR1_PCE | USART_CR1_PS | USART_CR1_TE | \
+ USART_CR1_RE | USART_CR1_OVER8 | USART_CR1_FIFOEN)
+
+#define STM32_UART_CR2_FIELDS \
+ (USART_CR2_SLVEN | USART_CR2_DIS_NSS | USART_CR2_ADDM7 | \
+ USART_CR2_LBDL | USART_CR2_LBDIE | USART_CR2_LBCL | \
+ USART_CR2_CPHA | USART_CR2_CPOL | USART_CR2_CLKEN | \
+ USART_CR2_STOP | USART_CR2_LINEN | USART_CR2_SWAP | \
+ USART_CR2_RXINV | USART_CR2_TXINV | USART_CR2_DATAINV | \
+ USART_CR2_MSBFIRST | USART_CR2_ABREN | USART_CR2_ABRMODE | \
+ USART_CR2_RTOEN | USART_CR2_ADD)
+
+#define STM32_UART_CR3_FIELDS \
+ (USART_CR3_EIE | USART_CR3_IREN | USART_CR3_IRLP | \
+ USART_CR3_HDSEL | USART_CR3_NACK | USART_CR3_SCEN | \
+ USART_CR3_DMAR | USART_CR3_DMAT | USART_CR3_RTSE | \
+ USART_CR3_CTSE | USART_CR3_CTSIE | USART_CR3_ONEBIT | \
+ USART_CR3_OVRDIS | USART_CR3_DDRE | USART_CR3_DEM | \
+ USART_CR3_DEP | USART_CR3_SCARCNT | USART_CR3_WUS | \
+ USART_CR3_WUFIE | USART_CR3_TXFTIE | USART_CR3_TCBGTIE | \
+ USART_CR3_RXFTCFG | USART_CR3_RXFTIE | USART_CR3_TXFTCFG)
+
+#define STM32_UART_ISR_ERRORS \
+ (USART_ISR_ORE | USART_ISR_NE | USART_ISR_FE | USART_ISR_PE)
+
+static const uint16_t presc_table[STM32_UART_PRESCALER_NB] = {
+ 1U, 2U, 4U, 6U, 8U, 10U, 12U, 16U, 32U, 64U, 128U, 256U
+};
+
+/* @brief BRR division operation to set BRR register in 8-bit oversampling
+ * mode.
+ * @param clockfreq: UART clock.
+ * @param baud_rate: Baud rate set by the user.
+ * @param prescaler: UART prescaler value.
+ * @retval Division result.
+ */
+static uint32_t uart_div_sampling8(unsigned long clockfreq,
+ uint32_t baud_rate,
+ uint32_t prescaler)
+{
+ uint32_t scaled_freq = clockfreq / presc_table[prescaler];
+
+ return ((scaled_freq * 2) + (baud_rate / 2)) / baud_rate;
+
+}
+
+/* @brief BRR division operation to set BRR register in 16-bit oversampling
+ * mode.
+ * @param clockfreq: UART clock.
+ * @param baud_rate: Baud rate set by the user.
+ * @param prescaler: UART prescaler value.
+ * @retval Division result.
+ */
+static uint32_t uart_div_sampling16(unsigned long clockfreq,
+ uint32_t baud_rate,
+ uint32_t prescaler)
+{
+ uint32_t scaled_freq = clockfreq / presc_table[prescaler];
+
+ return (scaled_freq + (baud_rate / 2)) / baud_rate;
+
+}
+
+/*
+ * @brief Return the UART clock frequency.
+ * @param huart: UART handle.
+ * @retval Frequency value in Hz.
+ */
+static unsigned long uart_get_clock_freq(struct stm32_uart_handle_s *huart)
+{
+ return fdt_get_uart_clock_freq((uintptr_t)huart->base);
+}
+
+/*
+ * @brief Configure the UART peripheral.
+ * @param huart: UART handle.
+ * @retval UART status.
+ */
+static int uart_set_config(struct stm32_uart_handle_s *huart,
+ const struct stm32_uart_init_s *init)
+{
+ uint32_t tmpreg;
+ unsigned long clockfreq;
+ unsigned long int_div;
+ uint32_t brrtemp;
+ uint32_t over_sampling;
+
+ /*---------------------- USART BRR configuration --------------------*/
+ clockfreq = uart_get_clock_freq(huart);
+ if (clockfreq == 0UL) {
+ return -ENODEV;
+ }
+
+ int_div = clockfreq / init->baud_rate;
+ if (int_div < 16U) {
+ uint32_t usartdiv = uart_div_sampling8(clockfreq,
+ init->baud_rate,
+ init->prescaler);
+
+ brrtemp = (usartdiv & USART_BRR_DIV_MANTISSA) |
+ ((usartdiv & USART_BRR_DIV_FRACTION) >> 1);
+ over_sampling = USART_CR1_OVER8;
+ } else {
+ brrtemp = uart_div_sampling16(clockfreq,
+ init->baud_rate,
+ init->prescaler) &
+ (USART_BRR_DIV_FRACTION | USART_BRR_DIV_MANTISSA);
+ over_sampling = 0x0U;
+ }
+ mmio_write_32(huart->base + USART_BRR, brrtemp);
+
+ /*
+ * ---------------------- USART CR1 Configuration --------------------
+ * Clear M, PCE, PS, TE, RE and OVER8 bits and configure
+ * the UART word length, parity, mode and oversampling:
+ * - set the M bits according to init->word_length value,
+ * - set PCE and PS bits according to init->parity value,
+ * - set TE and RE bits according to init->mode value,
+ * - set OVER8 bit according baudrate and clock.
+ */
+ tmpreg = init->word_length |
+ init->parity |
+ init->mode |
+ over_sampling |
+ init->fifo_mode;
+ mmio_clrsetbits_32(huart->base + USART_CR1, STM32_UART_CR1_FIELDS, tmpreg);
+
+ /*
+ * --------------------- USART CR2 Configuration ---------------------
+ * Configure the UART Stop Bits: Set STOP[13:12] bits according
+ * to init->stop_bits value.
+ */
+ mmio_clrsetbits_32(huart->base + USART_CR2, STM32_UART_CR2_FIELDS,
+ init->stop_bits);
+
+ /*
+ * --------------------- USART CR3 Configuration ---------------------
+ * Configure:
+ * - UART HardWare Flow Control: set CTSE and RTSE bits according
+ * to init->hw_flow_control value,
+ * - one-bit sampling method versus three samples' majority rule
+ * according to init->one_bit_sampling (not applicable to
+ * LPUART),
+ * - set TXFTCFG bit according to init->tx_fifo_threshold value,
+ * - set RXFTCFG bit according to init->rx_fifo_threshold value.
+ */
+ tmpreg = init->hw_flow_control | init->one_bit_sampling;
+
+ if (init->fifo_mode == USART_CR1_FIFOEN) {
+ tmpreg |= init->tx_fifo_threshold |
+ init->rx_fifo_threshold;
+ }
+
+ mmio_clrsetbits_32(huart->base + USART_CR3, STM32_UART_CR3_FIELDS, tmpreg);
+
+ /*
+ * --------------------- USART PRESC Configuration -------------------
+ * Configure UART Clock Prescaler : set PRESCALER according to
+ * init->prescaler value.
+ */
+ assert(init->prescaler < STM32_UART_PRESCALER_NB);
+ mmio_clrsetbits_32(huart->base + USART_PRESC, USART_PRESC_PRESCALER,
+ init->prescaler);
+
+ return 0;
+}
+
+/*
+ * @brief Handle UART communication timeout.
+ * @param huart: UART handle.
+ * @param flag: Specifies the UART flag to check.
+ * @retval UART status.
+ */
+static int stm32_uart_wait_flag(struct stm32_uart_handle_s *huart, uint32_t flag)
+{
+ uint64_t timeout_ref = timeout_init_us(STM32_UART_TIMEOUT_US);
+
+ while ((mmio_read_32(huart->base + USART_ISR) & flag) == 0U) {
+ if (timeout_elapsed(timeout_ref)) {
+ return -ETIMEDOUT;
+ }
+ }
+
+ return 0;
+}
+
+/*
+ * @brief Check the UART idle State.
+ * @param huart: UART handle.
+ * @retval UART status.
+ */
+static int stm32_uart_check_idle(struct stm32_uart_handle_s *huart)
+{
+ int ret;
+
+ /* Check if the transmitter is enabled */
+ if ((mmio_read_32(huart->base + USART_CR1) & USART_CR1_TE) == USART_CR1_TE) {
+ ret = stm32_uart_wait_flag(huart, USART_ISR_TEACK);
+ if (ret != 0) {
+ return ret;
+ }
+ }
+
+ /* Check if the receiver is enabled */
+ if ((mmio_read_32(huart->base + USART_CR1) & USART_CR1_RE) == USART_CR1_RE) {
+ ret = stm32_uart_wait_flag(huart, USART_ISR_REACK);
+ if (ret != 0) {
+ return ret;
+ }
+ }
+
+ return 0;
+}
+
+/*
+ * @brief Compute RDR register mask depending on word length.
+ * @param huart: UART handle.
+ * @retval Mask value.
+ */
+static unsigned int stm32_uart_rdr_mask(const struct stm32_uart_init_s *init)
+{
+ unsigned int mask = 0U;
+
+ switch (init->word_length) {
+ case STM32_UART_WORDLENGTH_9B:
+ mask = GENMASK(8, 0);
+ break;
+ case STM32_UART_WORDLENGTH_8B:
+ mask = GENMASK(7, 0);
+ break;
+ case STM32_UART_WORDLENGTH_7B:
+ mask = GENMASK(6, 0);
+ break;
+ default:
+ break; /* not reached */
+ }
+
+ if (init->parity != STM32_UART_PARITY_NONE) {
+ mask >>= 1;
+ }
+
+ return mask;
+}
+
+/*
+ * @brief Check interrupt and status errors.
+ * @retval True if error detected, false otherwise.
+ */
+static bool stm32_uart_error_detected(struct stm32_uart_handle_s *huart)
+{
+ return (mmio_read_32(huart->base + USART_ISR) & STM32_UART_ISR_ERRORS) != 0U;
+}
+
+/*
+ * @brief Clear status errors.
+ */
+static void stm32_uart_error_clear(struct stm32_uart_handle_s *huart)
+{
+ mmio_write_32(huart->base + USART_ICR, STM32_UART_ISR_ERRORS);
+}
+
+/*
+ * @brief Stop the UART.
+ * @param base: UART base address.
+ */
+void stm32_uart_stop(uintptr_t base)
+{
+ mmio_clrbits_32(base + USART_CR1, USART_CR1_UE);
+}
+
+/*
+ * @brief Initialize UART.
+ * @param huart: UART handle.
+ * @param base_addr: base address of UART.
+ * @param init: UART initialization parameter.
+ * @retval UART status.
+ */
+int stm32_uart_init(struct stm32_uart_handle_s *huart,
+ uintptr_t base_addr,
+ const struct stm32_uart_init_s *init)
+{
+ int ret;
+ int uart_node;
+ int clk;
+ void *fdt = NULL;
+
+ if (huart == NULL || init == NULL || base_addr == 0U) {
+ return -EINVAL;
+ }
+
+ huart->base = base_addr;
+
+ /* Search UART instance in DT */
+ if (fdt_get_address(&fdt) == 0) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ if (fdt == NULL) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ uart_node = dt_match_instance_by_compatible(DT_UART_COMPAT, base_addr);
+ if (uart_node == -FDT_ERR_NOTFOUND) {
+ return -FDT_ERR_NOTFOUND;
+ }
+
+ /* Pinctrl initialization */
+ if (dt_set_pinctrl_config(uart_node) != 0) {
+ return -FDT_ERR_BADVALUE;
+ }
+
+ /* Clock initialization */
+ clk = fdt_get_clock_id(uart_node);
+ if (clk < 0) {
+ return -FDT_ERR_NOTFOUND;
+ }
+ clk_enable(clk);
+
+ /* Disable the peripheral */
+ stm32_uart_stop(huart->base);
+
+ /* Computation of UART mask to apply to RDR register */
+ huart->rdr_mask = stm32_uart_rdr_mask(init);
+
+ /* Init the peripheral */
+ ret = uart_set_config(huart, init);
+ if (ret != 0) {
+ return ret;
+ }
+
+ /* Enable the peripheral */
+ mmio_setbits_32(huart->base + USART_CR1, USART_CR1_UE);
+
+ /* TEACK and/or REACK to check */
+ return stm32_uart_check_idle(huart);
+}
+
+/*
+ * @brief Transmit one data in no blocking mode.
+ * @param huart: UART handle.
+ * @param c: data to sent.
+ * @retval UART status.
+ */
+int stm32_uart_putc(struct stm32_uart_handle_s *huart, int c)
+{
+ int ret;
+
+ if (huart == NULL) {
+ return -EINVAL;
+ }
+
+ ret = stm32_uart_wait_flag(huart, USART_ISR_TXE);
+ if (ret != 0) {
+ return ret;
+ }
+
+ mmio_write_32(huart->base + USART_TDR, c);
+ if (stm32_uart_error_detected(huart)) {
+ stm32_uart_error_clear(huart);
+ return -EFAULT;
+ }
+
+ return 0;
+}
+
+/*
+ * @brief Flush TX Transmit fifo
+ * @param huart: UART handle.
+ * @retval UART status.
+ */
+int stm32_uart_flush(struct stm32_uart_handle_s *huart)
+{
+ int ret;
+
+ if (huart == NULL) {
+ return -EINVAL;
+ }
+
+ ret = stm32_uart_wait_flag(huart, USART_ISR_TXE);
+ if (ret != 0) {
+ return ret;
+ }
+
+ return stm32_uart_wait_flag(huart, USART_ISR_TC);
+}
+
+/*
+ * @brief Receive a data in no blocking mode.
+ * @retval value if >0 or UART status.
+ */
+int stm32_uart_getc(struct stm32_uart_handle_s *huart)
+{
+ uint32_t data;
+
+ if (huart == NULL) {
+ return -EINVAL;
+ }
+
+ /* Check if data is available */
+ if ((mmio_read_32(huart->base + USART_ISR) & USART_ISR_RXNE) == 0U) {
+ return -EAGAIN;
+ }
+
+ data = mmio_read_32(huart->base + USART_RDR) & huart->rdr_mask;
+
+ if (stm32_uart_error_detected(huart)) {
+ stm32_uart_error_clear(huart);
+ return -EFAULT;
+ }
+
+ return (int)data;
+}
diff --git a/drivers/st/usb/stm32mp1_usb.c b/drivers/st/usb/stm32mp1_usb.c
new file mode 100644
index 0000000..9a49690
--- /dev/null
+++ b/drivers/st/usb/stm32mp1_usb.c
@@ -0,0 +1,1091 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32mp1_usb.h>
+#include <lib/mmio.h>
+
+#include <platform_def.h>
+
+#define USB_OTG_MODE_DEVICE 0U
+#define USB_OTG_MODE_HOST 1U
+#define USB_OTG_MODE_DRD 2U
+
+#define EP_TYPE_CTRL 0U
+#define EP_TYPE_ISOC 1U
+#define EP_TYPE_BULK 2U
+#define EP_TYPE_INTR 3U
+
+#define USBD_FIFO_FLUSH_TIMEOUT_US 1000U
+#define EP0_FIFO_SIZE 64U
+
+/* OTG registers offsets */
+#define OTG_GOTGINT 0x004U
+#define OTG_GAHBCFG 0x008U
+#define OTG_GUSBCFG 0x00CU
+#define OTG_GRSTCTL 0x010U
+#define OTG_GINTSTS 0x014U
+#define OTG_GINTMSK 0x018U
+#define OTG_GRXSTSP 0x020U
+#define OTG_GLPMCFG 0x054U
+#define OTG_DCFG 0x800U
+#define OTG_DCTL 0x804U
+#define OTG_DSTS 0x808U
+#define OTG_DIEPMSK 0x810U
+#define OTG_DOEPMSK 0x814U
+#define OTG_DAINT 0x818U
+#define OTG_DAINTMSK 0x81CU
+#define OTG_DIEPEMPMSK 0x834U
+
+/* Definitions for OTG_DIEPx registers */
+#define OTG_DIEP_BASE 0x900U
+#define OTG_DIEP_SIZE 0x20U
+#define OTG_DIEPCTL 0x00U
+#define OTG_DIEPINT 0x08U
+#define OTG_DIEPTSIZ 0x10U
+#define OTG_DIEPDMA 0x14U
+#define OTG_DTXFSTS 0x18U
+#define OTG_DIEP_MAX_NB 9U
+
+/* Definitions for OTG_DOEPx registers */
+#define OTG_DOEP_BASE 0xB00U
+#define OTG_DOEP_SIZE 0x20U
+#define OTG_DOEPCTL 0x00U
+#define OTG_DOEPINT 0x08U
+#define OTG_DOEPTSIZ 0x10U
+#define OTG_DOEPDMA 0x14U
+#define OTG_D0EP_MAX_NB 9U
+
+/* Definitions for OTG_DAINT registers */
+#define OTG_DAINT_OUT_MASK GENMASK(31, 16)
+#define OTG_DAINT_OUT_SHIFT 16U
+#define OTG_DAINT_IN_MASK GENMASK(15, 0)
+#define OTG_DAINT_IN_SHIFT 0U
+
+#define OTG_DAINT_EP0_IN BIT(16)
+#define OTG_DAINT_EP0_OUT BIT(0)
+
+/* Definitions for FIFOs */
+#define OTG_FIFO_BASE 0x1000U
+#define OTG_FIFO_SIZE 0x1000U
+
+/* Bit definitions for OTG_GOTGINT register */
+#define OTG_GOTGINT_SEDET BIT(2)
+
+/* Bit definitions for OTG_GAHBCFG register */
+#define OTG_GAHBCFG_GINT BIT(0)
+
+/* Bit definitions for OTG_GUSBCFG register */
+#define OTG_GUSBCFG_TRDT GENMASK(13, 10)
+#define OTG_GUSBCFG_TRDT_SHIFT 10U
+
+#define USBD_HS_TRDT_VALUE 9U
+
+/* Bit definitions for OTG_GRSTCTL register */
+#define OTG_GRSTCTL_RXFFLSH BIT(4)
+#define OTG_GRSTCTL_TXFFLSH BIT(5)
+#define OTG_GRSTCTL_TXFNUM_SHIFT 6U
+
+/* Bit definitions for OTG_GINTSTS register */
+#define OTG_GINTSTS_CMOD BIT(0)
+#define OTG_GINTSTS_MMIS BIT(1)
+#define OTG_GINTSTS_OTGINT BIT(2)
+#define OTG_GINTSTS_SOF BIT(3)
+#define OTG_GINTSTS_RXFLVL BIT(4)
+#define OTG_GINTSTS_USBSUSP BIT(11)
+#define OTG_GINTSTS_USBRST BIT(12)
+#define OTG_GINTSTS_ENUMDNE BIT(13)
+#define OTG_GINTSTS_IEPINT BIT(18)
+#define OTG_GINTSTS_OEPINT BIT(19)
+#define OTG_GINTSTS_IISOIXFR BIT(20)
+#define OTG_GINTSTS_IPXFR_INCOMPISOOUT BIT(21)
+#define OTG_GINTSTS_LPMINT BIT(27)
+#define OTG_GINTSTS_SRQINT BIT(30)
+#define OTG_GINTSTS_WKUPINT BIT(31)
+
+/* Bit definitions for OTG_GRXSTSP register */
+#define OTG_GRXSTSP_EPNUM GENMASK(3, 0)
+#define OTG_GRXSTSP_BCNT GENMASK(14, 4)
+#define OTG_GRXSTSP_BCNT_SHIFT 4U
+#define OTG_GRXSTSP_PKTSTS GENMASK(20, 17)
+#define OTG_GRXSTSP_PKTSTS_SHIFT 17U
+
+#define STS_GOUT_NAK 1U
+#define STS_DATA_UPDT 2U
+#define STS_XFER_COMP 3U
+#define STS_SETUP_COMP 4U
+#define STS_SETUP_UPDT 6U
+
+/* Bit definitions for OTG_GLPMCFG register */
+#define OTG_GLPMCFG_BESL GENMASK(5, 2)
+
+/* Bit definitions for OTG_DCFG register */
+#define OTG_DCFG_DAD GENMASK(10, 4)
+#define OTG_DCFG_DAD_SHIFT 4U
+
+/* Bit definitions for OTG_DCTL register */
+#define OTG_DCTL_RWUSIG BIT(0)
+#define OTG_DCTL_SDIS BIT(1)
+#define OTG_DCTL_CGINAK BIT(8)
+
+/* Bit definitions for OTG_DSTS register */
+#define OTG_DSTS_SUSPSTS BIT(0)
+#define OTG_DSTS_ENUMSPD_MASK GENMASK(2, 1)
+#define OTG_DSTS_FNSOF0 BIT(8)
+
+#define OTG_DSTS_ENUMSPD(val) ((val) << 1)
+#define OTG_DSTS_ENUMSPD_HS_PHY_30MHZ_OR_60MHZ OTG_DSTS_ENUMSPD(0U)
+#define OTG_DSTS_ENUMSPD_FS_PHY_30MHZ_OR_60MHZ OTG_DSTS_ENUMSPD(1U)
+#define OTG_DSTS_ENUMSPD_LS_PHY_6MHZ OTG_DSTS_ENUMSPD(2U)
+#define OTG_DSTS_ENUMSPD_FS_PHY_48MHZ OTG_DSTS_ENUMSPD(3U)
+
+/* Bit definitions for OTG_DIEPMSK register */
+#define OTG_DIEPMSK_XFRCM BIT(0)
+#define OTG_DIEPMSK_EPDM BIT(1)
+#define OTG_DIEPMSK_TOM BIT(3)
+
+/* Bit definitions for OTG_DOEPMSK register */
+#define OTG_DOEPMSK_XFRCM BIT(0)
+#define OTG_DOEPMSK_EPDM BIT(1)
+#define OTG_DOEPMSK_STUPM BIT(3)
+
+/* Bit definitions for OTG_DIEPCTLx registers */
+#define OTG_DIEPCTL_MPSIZ GENMASK(10, 0)
+#define OTG_DIEPCTL_STALL BIT(21)
+#define OTG_DIEPCTL_CNAK BIT(26)
+#define OTG_DIEPCTL_SD0PID_SEVNFRM BIT(28)
+#define OTG_DIEPCTL_SODDFRM BIT(29)
+#define OTG_DIEPCTL_EPDIS BIT(30)
+#define OTG_DIEPCTL_EPENA BIT(31)
+
+/* Bit definitions for OTG_DIEPINTx registers */
+#define OTG_DIEPINT_XFRC BIT(0)
+#define OTG_DIEPINT_EPDISD BIT(1)
+#define OTG_DIEPINT_TOC BIT(3)
+#define OTG_DIEPINT_ITTXFE BIT(4)
+#define OTG_DIEPINT_INEPNE BIT(6)
+#define OTG_DIEPINT_TXFE BIT(7)
+#define OTG_DIEPINT_TXFE_SHIFT 7U
+
+#define OTG_DIEPINT_MASK (BIT(13) | BIT(11) | GENMASK(9, 0))
+
+/* Bit definitions for OTG_DIEPTSIZx registers */
+#define OTG_DIEPTSIZ_XFRSIZ GENMASK(18, 0)
+#define OTG_DIEPTSIZ_PKTCNT GENMASK(28, 19)
+#define OTG_DIEPTSIZ_PKTCNT_SHIFT 19U
+#define OTG_DIEPTSIZ_MCNT_MASK GENMASK(30, 29)
+#define OTG_DIEPTSIZ_MCNT_DATA0 BIT(29)
+
+#define OTG_DIEPTSIZ_PKTCNT_1 BIT(19)
+
+/* Bit definitions for OTG_DTXFSTSx registers */
+#define OTG_DTXFSTS_INEPTFSAV GENMASK(15, 0)
+
+/* Bit definitions for OTG_DOEPCTLx registers */
+#define OTG_DOEPCTL_STALL BIT(21)
+#define OTG_DOEPCTL_CNAK BIT(26)
+#define OTG_DOEPCTL_SD0PID_SEVNFRM BIT(28) /* other than endpoint 0 */
+#define OTG_DOEPCTL_SD1PID_SODDFRM BIT(29) /* other than endpoint 0 */
+#define OTG_DOEPCTL_EPDIS BIT(30)
+#define OTG_DOEPCTL_EPENA BIT(31)
+
+/* Bit definitions for OTG_DOEPTSIZx registers */
+#define OTG_DOEPTSIZ_XFRSIZ GENMASK(18, 0)
+#define OTG_DOEPTSIZ_PKTCNT GENMASK(28, 19)
+#define OTG_DOEPTSIZ_RXDPID_STUPCNT GENMASK(30, 29)
+
+/* Bit definitions for OTG_DOEPINTx registers */
+#define OTG_DOEPINT_XFRC BIT(0)
+#define OTG_DOEPINT_STUP BIT(3)
+#define OTG_DOEPINT_OTEPDIS BIT(4)
+
+#define OTG_DOEPINT_MASK (GENMASK(15, 12) | GENMASK(9, 8) | GENMASK(6, 0))
+
+#define EP_NB 15U
+#define EP_ALL 0x10U
+
+/*
+ * Flush TX FIFO.
+ * handle: PCD handle.
+ * num: FIFO number.
+ * This parameter can be a value from 1 to 15 or EP_ALL.
+ * EP_ALL= 0x10 means Flush all TX FIFOs
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_flush_tx_fifo(void *handle, uint32_t num)
+{
+ uintptr_t usb_base_addr = (uintptr_t)handle;
+ uint64_t timeout = timeout_init_us(USBD_FIFO_FLUSH_TIMEOUT_US);
+
+ mmio_write_32(usb_base_addr + OTG_GRSTCTL,
+ OTG_GRSTCTL_TXFFLSH | (uint32_t)(num << OTG_GRSTCTL_TXFNUM_SHIFT));
+
+ while ((mmio_read_32(usb_base_addr + OTG_GRSTCTL) &
+ OTG_GRSTCTL_TXFFLSH) == OTG_GRSTCTL_TXFFLSH) {
+ if (timeout_elapsed(timeout)) {
+ return USBD_TIMEOUT;
+ }
+ }
+
+ return USBD_OK;
+}
+
+/*
+ * Flush RX FIFO.
+ * handle: PCD handle.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_flush_rx_fifo(void *handle)
+{
+ uintptr_t usb_base_addr = (uintptr_t)handle;
+ uint64_t timeout = timeout_init_us(USBD_FIFO_FLUSH_TIMEOUT_US);
+
+ mmio_write_32(usb_base_addr + OTG_GRSTCTL, OTG_GRSTCTL_RXFFLSH);
+
+ while ((mmio_read_32(usb_base_addr + OTG_GRSTCTL) &
+ OTG_GRSTCTL_RXFFLSH) == OTG_GRSTCTL_RXFFLSH) {
+ if (timeout_elapsed(timeout)) {
+ return USBD_TIMEOUT;
+ }
+ }
+
+ return USBD_OK;
+}
+
+/*
+ * Return the global USB interrupt status.
+ * handle: PCD handle.
+ * return: Interrupt register value.
+ */
+static uint32_t usb_dwc2_read_int(void *handle)
+{
+ uintptr_t usb_base_addr = (uintptr_t)handle;
+
+ return mmio_read_32(usb_base_addr + OTG_GINTSTS) &
+ mmio_read_32(usb_base_addr + OTG_GINTMSK);
+}
+
+/*
+ * Return the USB device OUT endpoints interrupt.
+ * handle: PCD handle.
+ * return: Device OUT endpoint interrupts.
+ */
+static uint32_t usb_dwc2_all_out_ep_int(void *handle)
+{
+ uintptr_t usb_base_addr = (uintptr_t)handle;
+
+ return ((mmio_read_32(usb_base_addr + OTG_DAINT) &
+ mmio_read_32(usb_base_addr + OTG_DAINTMSK)) &
+ OTG_DAINT_OUT_MASK) >> OTG_DAINT_OUT_SHIFT;
+}
+
+/*
+ * Return the USB device IN endpoints interrupt.
+ * handle: PCD handle.
+ * return: Device IN endpoint interrupts.
+ */
+static uint32_t usb_dwc2_all_in_ep_int(void *handle)
+{
+ uintptr_t usb_base_addr = (uintptr_t)handle;
+
+ return ((mmio_read_32(usb_base_addr + OTG_DAINT) &
+ mmio_read_32(usb_base_addr + OTG_DAINTMSK)) &
+ OTG_DAINT_IN_MASK) >> OTG_DAINT_IN_SHIFT;
+}
+
+/*
+ * Return Device OUT EP interrupt register.
+ * handle: PCD handle.
+ * epnum: Endpoint number.
+ * This parameter can be a value from 0 to 15.
+ * return: Device OUT EP Interrupt register.
+ */
+static uint32_t usb_dwc2_out_ep_int(void *handle, uint8_t epnum)
+{
+ uintptr_t usb_base_addr = (uintptr_t)handle;
+
+ return mmio_read_32(usb_base_addr + OTG_DOEP_BASE +
+ (epnum * OTG_DOEP_SIZE) + OTG_DOEPINT) &
+ mmio_read_32(usb_base_addr + OTG_DOEPMSK);
+}
+
+/*
+ * Return Device IN EP interrupt register.
+ * handle: PCD handle.
+ * epnum: Endpoint number.
+ * This parameter can be a value from 0 to 15.
+ * return: Device IN EP Interrupt register.
+ */
+static uint32_t usb_dwc2_in_ep_int(void *handle, uint8_t epnum)
+{
+ uintptr_t usb_base_addr = (uintptr_t)handle;
+ uint32_t msk;
+ uint32_t emp;
+
+ msk = mmio_read_32(usb_base_addr + OTG_DIEPMSK);
+ emp = mmio_read_32(usb_base_addr + OTG_DIEPEMPMSK);
+ msk |= ((emp >> epnum) << OTG_DIEPINT_TXFE_SHIFT) & OTG_DIEPINT_TXFE;
+
+ return mmio_read_32(usb_base_addr + OTG_DIEP_BASE +
+ (epnum * OTG_DIEP_SIZE) + OTG_DIEPINT) & msk;
+}
+
+/*
+ * Return USB core mode.
+ * handle: PCD handle.
+ * return: Core mode.
+ * This parameter can be 0 (host) or 1 (device).
+ */
+static uint32_t usb_dwc2_get_mode(void *handle)
+{
+ uintptr_t usb_base_addr = (uintptr_t)handle;
+
+ return mmio_read_32(usb_base_addr + OTG_GINTSTS) & OTG_GINTSTS_CMOD;
+}
+
+/*
+ * Activate EP0 for detup transactions.
+ * handle: PCD handle.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_activate_setup(void *handle)
+{
+ uintptr_t usb_base_addr = (uintptr_t)handle;
+ uintptr_t reg_offset = usb_base_addr + OTG_DIEP_BASE;
+
+ /* Set the MPS of the IN EP based on the enumeration speed */
+ mmio_clrbits_32(reg_offset + OTG_DIEPCTL, OTG_DIEPCTL_MPSIZ);
+
+ if ((mmio_read_32(usb_base_addr + OTG_DSTS) & OTG_DSTS_ENUMSPD_MASK) ==
+ OTG_DSTS_ENUMSPD_LS_PHY_6MHZ) {
+ mmio_setbits_32(reg_offset + OTG_DIEPCTL, 3U);
+ }
+
+ mmio_setbits_32(usb_base_addr + OTG_DCTL, OTG_DCTL_CGINAK);
+
+ return USBD_OK;
+}
+
+/*
+ * Prepare the EP0 to start the first control setup.
+ * handle: Selected device.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_ep0_out_start(void *handle)
+{
+ uintptr_t usb_base_addr = (uintptr_t)handle;
+ uintptr_t reg_offset = usb_base_addr + OTG_DIEP_BASE + OTG_DIEPTSIZ;
+ uint32_t reg_value = 0U;
+
+ /* PKTCNT = 1 and XFRSIZ = 24 bytes for endpoint 0 */
+ reg_value |= OTG_DIEPTSIZ_PKTCNT_1;
+ reg_value |= (EP0_FIFO_SIZE & OTG_DIEPTSIZ_XFRSIZ);
+ reg_value |= OTG_DOEPTSIZ_RXDPID_STUPCNT;
+
+ mmio_write_32(reg_offset, reg_value);
+
+ return USBD_OK;
+}
+
+/*
+ * Write a packet into the TX FIFO associated with the EP/channel.
+ * handle: Selected device.
+ * src: Pointer to source buffer.
+ * ch_ep_num: Endpoint or host channel number.
+ * len: Number of bytes to write.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_write_packet(void *handle, uint8_t *src,
+ uint8_t ch_ep_num, uint16_t len)
+{
+ uint32_t reg_offset;
+ uint32_t count32b = (len + 3U) / 4U;
+ uint32_t i;
+
+ reg_offset = (uintptr_t)handle + OTG_FIFO_BASE +
+ (ch_ep_num * OTG_FIFO_SIZE);
+
+ for (i = 0U; i < count32b; i++) {
+ uint32_t src_copy = 0U;
+ uint32_t j;
+
+ /* Data written to FIFO need to be 4 bytes aligned */
+ for (j = 0U; j < 4U; j++) {
+ src_copy += (*(src + j)) << (8U * j);
+ }
+
+ mmio_write_32(reg_offset, src_copy);
+ src += 4U;
+ }
+
+ return USBD_OK;
+}
+
+/*
+ * Read a packet from the RX FIFO associated with the EP/channel.
+ * handle: Selected device.
+ * dst: Destination pointer.
+ * len: Number of bytes to read.
+ * return: Pointer to destination buffer.
+ */
+static void *usb_dwc2_read_packet(void *handle, uint8_t *dest, uint16_t len)
+{
+ uint32_t reg_offset;
+ uint32_t count32b = (len + 3U) / 4U;
+ uint32_t i;
+
+ VERBOSE("read packet length %i to 0x%lx\n", len, (uintptr_t)dest);
+
+ reg_offset = (uintptr_t)handle + OTG_FIFO_BASE;
+
+ for (i = 0U; i < count32b; i++) {
+ *(uint32_t *)dest = mmio_read_32(reg_offset);
+ dest += 4U;
+ dsb();
+ }
+
+ return (void *)dest;
+}
+
+/*
+ * Setup and start a transfer over an EP.
+ * handle: Selected device
+ * ep: Pointer to endpoint structure.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_ep_start_xfer(void *handle, struct usbd_ep *ep)
+{
+ uintptr_t usb_base_addr = (uintptr_t)handle;
+ uint32_t reg_offset;
+ uint32_t reg_value;
+ uint32_t clear_value;
+
+ if (ep->is_in) {
+ reg_offset = usb_base_addr + OTG_DIEP_BASE + (ep->num * OTG_DIEP_SIZE);
+ clear_value = OTG_DIEPTSIZ_PKTCNT | OTG_DIEPTSIZ_XFRSIZ;
+ if (ep->xfer_len == 0U) {
+ reg_value = OTG_DIEPTSIZ_PKTCNT_1;
+ } else {
+ /*
+ * Program the transfer size and packet count
+ * as follows:
+ * xfersize = N * maxpacket + short_packet
+ * pktcnt = N + (short_packet exist ? 1 : 0)
+ */
+ reg_value = (OTG_DIEPTSIZ_PKTCNT &
+ (((ep->xfer_len + ep->maxpacket - 1U) /
+ ep->maxpacket) << OTG_DIEPTSIZ_PKTCNT_SHIFT))
+ | ep->xfer_len;
+
+ if (ep->type == EP_TYPE_ISOC) {
+ clear_value |= OTG_DIEPTSIZ_MCNT_MASK;
+ reg_value |= OTG_DIEPTSIZ_MCNT_DATA0;
+ }
+ }
+
+ mmio_clrsetbits_32(reg_offset + OTG_DIEPTSIZ, clear_value, reg_value);
+
+ if ((ep->type != EP_TYPE_ISOC) && (ep->xfer_len > 0U)) {
+ /* Enable the TX FIFO empty interrupt for this EP */
+ mmio_setbits_32(usb_base_addr + OTG_DIEPEMPMSK, BIT(ep->num));
+ }
+
+ /* EP enable, IN data in FIFO */
+ reg_value = OTG_DIEPCTL_CNAK | OTG_DIEPCTL_EPENA;
+
+ if (ep->type == EP_TYPE_ISOC) {
+ if ((mmio_read_32(usb_base_addr + OTG_DSTS) & OTG_DSTS_FNSOF0) == 0U) {
+ reg_value |= OTG_DIEPCTL_SODDFRM;
+ } else {
+ reg_value |= OTG_DIEPCTL_SD0PID_SEVNFRM;
+ }
+ }
+
+ mmio_setbits_32(reg_offset + OTG_DIEPCTL, reg_value);
+
+ if (ep->type == EP_TYPE_ISOC) {
+ usb_dwc2_write_packet(handle, ep->xfer_buff, ep->num, ep->xfer_len);
+ }
+ } else {
+ reg_offset = usb_base_addr + OTG_DOEP_BASE + (ep->num * OTG_DOEP_SIZE);
+ /*
+ * Program the transfer size and packet count as follows:
+ * pktcnt = N
+ * xfersize = N * maxpacket
+ */
+ if (ep->xfer_len == 0U) {
+ reg_value = ep->maxpacket | OTG_DIEPTSIZ_PKTCNT_1;
+ } else {
+ uint16_t pktcnt = (ep->xfer_len + ep->maxpacket - 1U) / ep->maxpacket;
+
+ reg_value = (pktcnt << OTG_DIEPTSIZ_PKTCNT_SHIFT) |
+ (ep->maxpacket * pktcnt);
+ }
+
+ mmio_clrsetbits_32(reg_offset + OTG_DOEPTSIZ,
+ OTG_DOEPTSIZ_XFRSIZ & OTG_DOEPTSIZ_PKTCNT,
+ reg_value);
+
+ /* EP enable */
+ reg_value = OTG_DOEPCTL_CNAK | OTG_DOEPCTL_EPENA;
+
+ if (ep->type == EP_TYPE_ISOC) {
+ if ((mmio_read_32(usb_base_addr + OTG_DSTS) & OTG_DSTS_FNSOF0) == 0U) {
+ reg_value |= OTG_DOEPCTL_SD1PID_SODDFRM;
+ } else {
+ reg_value |= OTG_DOEPCTL_SD0PID_SEVNFRM;
+ }
+ }
+
+ mmio_setbits_32(reg_offset + OTG_DOEPCTL, reg_value);
+ }
+
+ return USBD_OK;
+}
+
+/*
+ * Setup and start a transfer over the EP0.
+ * handle: Selected device.
+ * ep: Pointer to endpoint structure.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_ep0_start_xfer(void *handle, struct usbd_ep *ep)
+{
+ uintptr_t usb_base_addr = (uintptr_t)handle;
+ uint32_t reg_offset;
+ uint32_t reg_value;
+
+ if (ep->is_in) {
+ reg_offset = usb_base_addr + OTG_DIEP_BASE +
+ (ep->num * OTG_DIEP_SIZE);
+
+ if (ep->xfer_len == 0U) {
+ reg_value = OTG_DIEPTSIZ_PKTCNT_1;
+ } else {
+ /*
+ * Program the transfer size and packet count
+ * as follows:
+ * xfersize = N * maxpacket + short_packet
+ * pktcnt = N + (short_packet exist ? 1 : 0)
+ */
+
+ if (ep->xfer_len > ep->maxpacket) {
+ ep->xfer_len = ep->maxpacket;
+ }
+
+ reg_value = OTG_DIEPTSIZ_PKTCNT_1 | ep->xfer_len;
+ }
+
+ mmio_clrsetbits_32(reg_offset + OTG_DIEPTSIZ,
+ OTG_DIEPTSIZ_XFRSIZ | OTG_DIEPTSIZ_PKTCNT,
+ reg_value);
+
+ /* Enable the TX FIFO empty interrupt for this EP */
+ if (ep->xfer_len > 0U) {
+ mmio_setbits_32(usb_base_addr + OTG_DIEPEMPMSK,
+ BIT(ep->num));
+ }
+
+ /* EP enable, IN data in FIFO */
+ mmio_setbits_32(reg_offset + OTG_DIEPCTL,
+ OTG_DIEPCTL_CNAK | OTG_DIEPCTL_EPENA);
+ } else {
+ reg_offset = usb_base_addr + OTG_DOEP_BASE +
+ (ep->num * OTG_DOEP_SIZE);
+
+ /*
+ * Program the transfer size and packet count as follows:
+ * pktcnt = N
+ * xfersize = N * maxpacket
+ */
+ if (ep->xfer_len > 0U) {
+ ep->xfer_len = ep->maxpacket;
+ }
+
+ reg_value = OTG_DIEPTSIZ_PKTCNT_1 | ep->maxpacket;
+
+ mmio_clrsetbits_32(reg_offset + OTG_DIEPTSIZ,
+ OTG_DIEPTSIZ_XFRSIZ | OTG_DIEPTSIZ_PKTCNT,
+ reg_value);
+
+ /* EP enable */
+ mmio_setbits_32(reg_offset + OTG_DOEPCTL,
+ OTG_DOEPCTL_CNAK | OTG_DOEPCTL_EPENA);
+ }
+
+ return USBD_OK;
+}
+
+/*
+ * Set a stall condition over an EP.
+ * handle: Selected device.
+ * ep: Pointer to endpoint structure.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_ep_set_stall(void *handle, struct usbd_ep *ep)
+{
+ uintptr_t usb_base_addr = (uintptr_t)handle;
+ uint32_t reg_offset;
+ uint32_t reg_value;
+
+ if (ep->is_in) {
+ reg_offset = usb_base_addr + OTG_DIEP_BASE +
+ (ep->num * OTG_DIEP_SIZE);
+ reg_value = mmio_read_32(reg_offset + OTG_DIEPCTL);
+
+ if ((reg_value & OTG_DIEPCTL_EPENA) == 0U) {
+ reg_value &= ~OTG_DIEPCTL_EPDIS;
+ }
+
+ reg_value |= OTG_DIEPCTL_STALL;
+
+ mmio_write_32(reg_offset + OTG_DIEPCTL, reg_value);
+ } else {
+ reg_offset = usb_base_addr + OTG_DOEP_BASE +
+ (ep->num * OTG_DOEP_SIZE);
+ reg_value = mmio_read_32(reg_offset + OTG_DOEPCTL);
+
+ if ((reg_value & OTG_DOEPCTL_EPENA) == 0U) {
+ reg_value &= ~OTG_DOEPCTL_EPDIS;
+ }
+
+ reg_value |= OTG_DOEPCTL_STALL;
+
+ mmio_write_32(reg_offset + OTG_DOEPCTL, reg_value);
+ }
+
+ return USBD_OK;
+}
+
+/*
+ * Stop the USB device mode.
+ * handle: Selected device.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_stop_device(void *handle)
+{
+ uintptr_t usb_base_addr = (uintptr_t)handle;
+ uint32_t i;
+
+ /* Disable Int */
+ mmio_clrbits_32(usb_base_addr + OTG_GAHBCFG, OTG_GAHBCFG_GINT);
+
+ /* Clear pending interrupts */
+ for (i = 0U; i < EP_NB; i++) {
+ mmio_write_32(usb_base_addr + OTG_DIEP_BASE + (i * OTG_DIEP_SIZE) + OTG_DIEPINT,
+ OTG_DIEPINT_MASK);
+ mmio_write_32(usb_base_addr + OTG_DOEP_BASE + (i * OTG_DOEP_SIZE) + OTG_DOEPINT,
+ OTG_DOEPINT_MASK);
+ }
+
+ mmio_write_32(usb_base_addr + OTG_DAINT, OTG_DAINT_IN_MASK | OTG_DAINT_OUT_MASK);
+
+ /* Clear interrupt masks */
+ mmio_write_32(usb_base_addr + OTG_DIEPMSK, 0U);
+ mmio_write_32(usb_base_addr + OTG_DOEPMSK, 0U);
+ mmio_write_32(usb_base_addr + OTG_DAINTMSK, 0U);
+
+ /* Flush the FIFO */
+ usb_dwc2_flush_rx_fifo(handle);
+ usb_dwc2_flush_tx_fifo(handle, EP_ALL);
+
+ /* Disconnect the USB device by disabling the pull-up/pull-down */
+ mmio_setbits_32((uintptr_t)handle + OTG_DCTL, OTG_DCTL_SDIS);
+
+ return USBD_OK;
+}
+
+/*
+ * Stop the USB device mode.
+ * handle: Selected device.
+ * address: New device address to be assigned.
+ * This parameter can be a value from 0 to 255.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_set_address(void *handle, uint8_t address)
+{
+ uintptr_t usb_base_addr = (uintptr_t)handle;
+
+ mmio_clrsetbits_32(usb_base_addr + OTG_DCFG,
+ OTG_DCFG_DAD,
+ address << OTG_DCFG_DAD_SHIFT);
+
+ return USBD_OK;
+}
+
+/*
+ * Check FIFO for the next packet to be loaded.
+ * handle: Selected device.
+ * epnum : Endpoint number.
+ * xfer_len: Block length.
+ * xfer_count: Number of blocks.
+ * maxpacket: Max packet length.
+ * xfer_buff: Buffer pointer.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_write_empty_tx_fifo(void *handle,
+ uint32_t epnum,
+ uint32_t xfer_len,
+ uint32_t *xfer_count,
+ uint32_t maxpacket,
+ uint8_t **xfer_buff)
+{
+ uintptr_t usb_base_addr = (uintptr_t)handle;
+ uint32_t reg_offset;
+ int32_t len;
+ uint32_t len32b;
+ enum usb_status ret;
+
+ len = xfer_len - *xfer_count;
+
+ if ((len > 0) && ((uint32_t)len > maxpacket)) {
+ len = maxpacket;
+ }
+
+ len32b = (len + 3U) / 4U;
+
+ reg_offset = usb_base_addr + OTG_DIEP_BASE + (epnum * OTG_DIEP_SIZE);
+
+ while (((mmio_read_32(reg_offset + OTG_DTXFSTS) &
+ OTG_DTXFSTS_INEPTFSAV) > len32b) &&
+ (*xfer_count < xfer_len) && (xfer_len != 0U)) {
+ /* Write the FIFO */
+ len = xfer_len - *xfer_count;
+
+ if ((len > 0) && ((uint32_t)len > maxpacket)) {
+ len = maxpacket;
+ }
+
+ len32b = (len + 3U) / 4U;
+
+ ret = usb_dwc2_write_packet(handle, *xfer_buff, epnum, len);
+ if (ret != USBD_OK) {
+ return ret;
+ }
+
+ *xfer_buff += len;
+ *xfer_count += len;
+ }
+
+ if (len <= 0) {
+ mmio_clrbits_32(usb_base_addr + OTG_DIEPEMPMSK, BIT(epnum));
+ }
+
+ return USBD_OK;
+}
+
+/*
+ * Handle PCD interrupt request.
+ * handle: PCD handle.
+ * param: Pointer to information updated by the IT handling.
+ * return: Action to do after IT handling.
+ */
+static enum usb_action usb_dwc2_it_handler(void *handle, uint32_t *param)
+{
+ uintptr_t usb_base_addr = (uintptr_t)handle;
+ uint32_t ep_intr;
+ uint32_t epint;
+ uint32_t epnum;
+ uint32_t temp;
+ enum usb_status ret;
+
+ if (usb_dwc2_get_mode(handle) != USB_OTG_MODE_DEVICE) {
+ return USB_NOTHING;
+ }
+
+ /* Avoid spurious interrupt */
+ if (usb_dwc2_read_int(handle) == 0U) {
+ return USB_NOTHING;
+ }
+
+ if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_MMIS) != 0U) {
+ /* Incorrect mode, acknowledge the interrupt */
+ mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_MMIS);
+ }
+
+ if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_OEPINT) != 0U) {
+ uint32_t reg_offset;
+
+ /* Read in the device interrupt bits */
+ ep_intr = usb_dwc2_all_out_ep_int(handle);
+ epnum = 0U;
+ while ((ep_intr & BIT(0)) != BIT(0)) {
+ epnum++;
+ ep_intr >>= 1;
+ }
+
+ reg_offset = usb_base_addr + OTG_DOEP_BASE + (epnum * OTG_DOEP_SIZE) + OTG_DOEPINT;
+
+ epint = usb_dwc2_out_ep_int(handle, epnum);
+
+ if ((epint & OTG_DOEPINT_XFRC) == OTG_DOEPINT_XFRC) {
+ mmio_write_32(reg_offset, OTG_DOEPINT_XFRC);
+ *param = epnum;
+
+ return USB_DATA_OUT;
+ }
+
+ if ((epint & OTG_DOEPINT_STUP) == OTG_DOEPINT_STUP) {
+ /* Inform that a setup packet is available */
+ mmio_write_32(reg_offset, OTG_DOEPINT_STUP);
+
+ return USB_SETUP;
+ }
+
+ if ((epint & OTG_DOEPINT_OTEPDIS) == OTG_DOEPINT_OTEPDIS) {
+ mmio_write_32(reg_offset, OTG_DOEPINT_OTEPDIS);
+ }
+ }
+
+ if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_IEPINT) != 0U) {
+ uint32_t reg_offset;
+
+ /* Read in the device interrupt bits */
+ ep_intr = usb_dwc2_all_in_ep_int(handle);
+ epnum = 0U;
+ while ((ep_intr & BIT(0)) != BIT(0)) {
+ epnum++;
+ ep_intr >>= 1;
+ }
+
+ reg_offset = usb_base_addr + OTG_DIEP_BASE + (epnum * OTG_DIEP_SIZE) + OTG_DIEPINT;
+
+ epint = usb_dwc2_in_ep_int(handle, epnum);
+
+ if ((epint & OTG_DIEPINT_XFRC) == OTG_DIEPINT_XFRC) {
+ mmio_clrbits_32(usb_base_addr + OTG_DIEPEMPMSK, BIT(epnum));
+ mmio_write_32(reg_offset, OTG_DIEPINT_XFRC);
+ *param = epnum;
+
+ return USB_DATA_IN;
+ }
+
+ if ((epint & OTG_DIEPINT_TOC) == OTG_DIEPINT_TOC) {
+ mmio_write_32(reg_offset, OTG_DIEPINT_TOC);
+ }
+
+ if ((epint & OTG_DIEPINT_ITTXFE) == OTG_DIEPINT_ITTXFE) {
+ mmio_write_32(reg_offset, OTG_DIEPINT_ITTXFE);
+ }
+
+ if ((epint & OTG_DIEPINT_INEPNE) == OTG_DIEPINT_INEPNE) {
+ mmio_write_32(reg_offset, OTG_DIEPINT_INEPNE);
+ }
+
+ if ((epint & OTG_DIEPINT_EPDISD) == OTG_DIEPINT_EPDISD) {
+ mmio_write_32(reg_offset, OTG_DIEPINT_EPDISD);
+ }
+
+ if ((epint & OTG_DIEPINT_TXFE) == OTG_DIEPINT_TXFE) {
+ *param = epnum;
+
+ return USB_WRITE_EMPTY;
+ }
+ }
+
+ /* Handle resume interrupt */
+ if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_WKUPINT) != 0U) {
+ INFO("handle USB : Resume\n");
+
+ /* Clear the remote wake-up signaling */
+ mmio_clrbits_32(usb_base_addr + OTG_DCTL, OTG_DCTL_RWUSIG);
+ mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_WKUPINT);
+
+ return USB_RESUME;
+ }
+
+ /* Handle suspend interrupt */
+ if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_USBSUSP) != 0U) {
+ INFO("handle USB : Suspend int\n");
+
+ mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_USBSUSP);
+
+ if ((mmio_read_32(usb_base_addr + OTG_DSTS) &
+ OTG_DSTS_SUSPSTS) == OTG_DSTS_SUSPSTS) {
+ return USB_SUSPEND;
+ }
+ }
+
+ /* Handle LPM interrupt */
+ if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_LPMINT) != 0U) {
+ INFO("handle USB : LPM int enter in suspend\n");
+
+ mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_LPMINT);
+ *param = (mmio_read_32(usb_base_addr + OTG_GLPMCFG) &
+ OTG_GLPMCFG_BESL) >> 2;
+
+ return USB_LPM;
+ }
+
+ /* Handle reset interrupt */
+ if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_USBRST) != 0U) {
+ INFO("handle USB : Reset\n");
+
+ mmio_clrbits_32(usb_base_addr + OTG_DCTL, OTG_DCTL_RWUSIG);
+
+ usb_dwc2_flush_tx_fifo(handle, 0U);
+
+ mmio_write_32(usb_base_addr + OTG_DAINT, OTG_DAINT_IN_MASK | OTG_DAINT_OUT_MASK);
+ mmio_setbits_32(usb_base_addr + OTG_DAINTMSK, OTG_DAINT_EP0_IN | OTG_DAINT_EP0_OUT);
+
+ mmio_setbits_32(usb_base_addr + OTG_DOEPMSK, OTG_DOEPMSK_STUPM |
+ OTG_DOEPMSK_XFRCM |
+ OTG_DOEPMSK_EPDM);
+ mmio_setbits_32(usb_base_addr + OTG_DIEPMSK, OTG_DIEPMSK_TOM |
+ OTG_DIEPMSK_XFRCM |
+ OTG_DIEPMSK_EPDM);
+
+ /* Set default address to 0 */
+ mmio_clrbits_32(usb_base_addr + OTG_DCFG, OTG_DCFG_DAD);
+
+ /* Setup EP0 to receive SETUP packets */
+ ret = usb_dwc2_ep0_out_start(handle);
+ if (ret != USBD_OK) {
+ return ret;
+ }
+
+ mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_USBRST);
+
+ return USB_RESET;
+ }
+
+ /* Handle enumeration done interrupt */
+ if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_ENUMDNE) != 0U) {
+ ret = usb_dwc2_activate_setup(handle);
+ if (ret != USBD_OK) {
+ return ret;
+ }
+
+ mmio_clrbits_32(usb_base_addr + OTG_GUSBCFG, OTG_GUSBCFG_TRDT);
+
+ mmio_setbits_32(usb_base_addr + OTG_GUSBCFG,
+ (USBD_HS_TRDT_VALUE << OTG_GUSBCFG_TRDT_SHIFT) & OTG_GUSBCFG_TRDT);
+
+ mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_ENUMDNE);
+
+ return USB_ENUM_DONE;
+ }
+
+ /* Handle RXQLevel interrupt */
+ if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_RXFLVL) != 0U) {
+ mmio_clrbits_32(usb_base_addr + OTG_GINTMSK,
+ OTG_GINTSTS_RXFLVL);
+
+ temp = mmio_read_32(usb_base_addr + OTG_GRXSTSP);
+
+ *param = temp & OTG_GRXSTSP_EPNUM;
+ *param |= (temp & OTG_GRXSTSP_BCNT) << (USBD_OUT_COUNT_SHIFT -
+ OTG_GRXSTSP_BCNT_SHIFT);
+
+ if (((temp & OTG_GRXSTSP_PKTSTS) >> OTG_GRXSTSP_PKTSTS_SHIFT) == STS_DATA_UPDT) {
+ if ((temp & OTG_GRXSTSP_BCNT) != 0U) {
+ mmio_setbits_32(usb_base_addr + OTG_GINTMSK, OTG_GINTSTS_RXFLVL);
+
+ return USB_READ_DATA_PACKET;
+ }
+ } else if (((temp & OTG_GRXSTSP_PKTSTS) >> OTG_GRXSTSP_PKTSTS_SHIFT) ==
+ STS_SETUP_UPDT) {
+ mmio_setbits_32(usb_base_addr + OTG_GINTMSK, OTG_GINTSTS_RXFLVL);
+
+ return USB_READ_SETUP_PACKET;
+ }
+
+ mmio_setbits_32(usb_base_addr + OTG_GINTMSK, OTG_GINTSTS_RXFLVL);
+ }
+
+ /* Handle SOF interrupt */
+ if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_SOF) != 0U) {
+ INFO("handle USB : SOF\n");
+
+ mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_SOF);
+
+ return USB_SOF;
+ }
+
+ /* Handle incomplete ISO IN interrupt */
+ if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_IISOIXFR) != 0U) {
+ INFO("handle USB : ISO IN\n");
+
+ mmio_write_32(usb_base_addr + OTG_GINTSTS,
+ OTG_GINTSTS_IISOIXFR);
+ }
+
+ /* Handle incomplete ISO OUT interrupt */
+ if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_IPXFR_INCOMPISOOUT) !=
+ 0U) {
+ INFO("handle USB : ISO OUT\n");
+
+ mmio_write_32(usb_base_addr + OTG_GINTSTS,
+ OTG_GINTSTS_IPXFR_INCOMPISOOUT);
+ }
+
+ /* Handle connection event interrupt */
+ if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_SRQINT) != 0U) {
+ INFO("handle USB : Connect\n");
+
+ mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_SRQINT);
+ }
+
+ /* Handle disconnection event interrupt */
+ if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_OTGINT) != 0U) {
+ INFO("handle USB : Disconnect\n");
+
+ temp = mmio_read_32(usb_base_addr + OTG_GOTGINT);
+
+ if ((temp & OTG_GOTGINT_SEDET) == OTG_GOTGINT_SEDET) {
+ return USB_DISCONNECT;
+ }
+ }
+
+ return USB_NOTHING;
+}
+
+/*
+ * Start the usb device mode
+ * usb_core_handle: USB core driver handle.
+ * return USB status.
+ */
+static enum usb_status usb_dwc2_start_device(void *handle)
+{
+ uintptr_t usb_base_addr = (uintptr_t)handle;
+
+ mmio_clrbits_32(usb_base_addr + OTG_DCTL, OTG_DCTL_SDIS);
+ mmio_setbits_32(usb_base_addr + OTG_GAHBCFG, OTG_GAHBCFG_GINT);
+
+ return USBD_OK;
+}
+
+static const struct usb_driver usb_dwc2driver = {
+ .ep0_out_start = usb_dwc2_ep0_out_start,
+ .ep_start_xfer = usb_dwc2_ep_start_xfer,
+ .ep0_start_xfer = usb_dwc2_ep0_start_xfer,
+ .write_packet = usb_dwc2_write_packet,
+ .read_packet = usb_dwc2_read_packet,
+ .ep_set_stall = usb_dwc2_ep_set_stall,
+ .start_device = usb_dwc2_start_device,
+ .stop_device = usb_dwc2_stop_device,
+ .set_address = usb_dwc2_set_address,
+ .write_empty_tx_fifo = usb_dwc2_write_empty_tx_fifo,
+ .it_handler = usb_dwc2_it_handler
+};
+
+/*
+ * Initialize USB DWC2 driver.
+ * usb_core_handle: USB core driver handle.
+ * pcd_handle: PCD handle.
+ * base_register: USB global register base address.
+ */
+void stm32mp1_usb_init_driver(struct usb_handle *usb_core_handle,
+ struct pcd_handle *pcd_handle,
+ void *base_register)
+{
+ register_usb_driver(usb_core_handle, pcd_handle, &usb_dwc2driver,
+ base_register);
+}