Merge pull request #1466 from Yann-lms/stm32mp1

Add STMicroelectronics STM32MP1 platform support

.gitignore

@@ -19,6 +19,9 @@ tools/cert_create/src/**/*.o
 tools/cert_create/cert_create
 tools/cert_create/cert_create.exe
 tools/doimage/doimage
+tools/stm32image/*.o
+tools/stm32image/stm32image
+tools/stm32image/stm32image.exe
 
 # GNU GLOBAL files
 GPATH

acknowledgements.rst

@@ -16,5 +16,7 @@ NXP Semiconductors
 
 Marvell International Ltd.
 
+STMicroelectronics
+
 Individuals
 -----------

docs/plat/stm32mp1.rst

+Trusted Firmware-A for STM32MP1
+===============================
+
+STM32MP1 is a microprocessor designed by STMicroelectronics
+based on a dual Arm Cortex-A7.
+It is an Armv7-A platform, using dedicated code from TF-A.
+
+
+Design
+------
+The STM32MP1 resets in the ROM code of the Cortex-A7.
+The primary boot core (core 0) executes the boot sequence while
+secondary boot core (core 1) is kept in a holding pen loop.
+The ROM code boot sequence loads the TF-A binary image from boot device
+to embedded SRAM.
+
+The TF-A image must be properly formatted with a STM32 header structure
+for ROM code is able to load this image.
+Tool stm32image can be used to prepend this header to the generated TF-A binary.
+
+At compilation step, BL2, BL32 and DTB file are linked together in a single
+binary. The stm32image tool is also generated and the header is added to TF-A
+binary. This binary file with header is named tf-a-stm32mp157c-ev1.stm32.
+It can then be copied in the first partition of the boot device.
+
+
+Memory mapping
+~~~~~~~~~~~~~~
+
+::
+
+    0x00000000 +-----------------+
+               |                 |   ROM
+    0x00020000 +-----------------+
+               |                 |
+               |       ...       |
+               |                 |
+    0x2FFC0000 +-----------------+ \
+               |                 | |
+               |       ...       | |
+               |                 | |
+    0x2FFD8000 +-----------------+ |
+               |    TF-A DTB     | | Embedded SRAM
+    0x2FFDC000 +-----------------+ |
+               |       BL2       | |
+    0x2FFEF000 +-----------------+ |
+               |       BL32      | |
+    0x30000000 +-----------------+ /
+               |                 |
+               |       ...       |
+               |                 |
+    0x40000000 +-----------------+
+               |                 |
+               |                 |   Devices
+               |                 |
+    0xC0000000 +-----------------+ \
+               |                 | |
+    0xC0100000 +-----------------+ |
+               |       BL33      | | Non-secure RAM (DDR)
+               |       ...       | |
+               |                 | |
+    0xFFFFFFFF +-----------------+ /
+
+
+Boot sequence
+~~~~~~~~~~~~~
+
+ROM code -> BL2 (compiled with BL2_AT_EL3) -> BL32 (SP_min) -> BL33 (U-Boot)
+
+
+Build Instructions
+------------------
+
+To build:
+
+.. code:: bash
+
+    make CROSS_COMPILE=arm-linux-gnueabihf- PLAT=stm32mp1 ARCH=aarch32 ARM_ARCH_MAJOR=7 AARCH32_SP=sp_min
+
+The following build options are supported:
+
+- ``ENABLE_STACK_PROTECTOR``: To enable the stack protection.

drivers/st/clk/stm32mp1_clk.c

+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <assert.h>
+#include <debug.h>
+#include <delay_timer.h>
+#include <dt-bindings/clock/stm32mp1-clks.h>
+#include <dt-bindings/clock/stm32mp1-clksrc.h>
+#include <errno.h>
+#include <generic_delay_timer.h>
+#include <libfdt.h>
+#include <mmio.h>
+#include <platform.h>
+#include <stdint.h>
+#include <stm32mp1_clk.h>
+#include <stm32mp1_clkfunc.h>
+#include <stm32mp1_dt.h>
+#include <stm32mp1_private.h>
+#include <stm32mp1_rcc.h>
+#include <utils_def.h>
+
+#define MAX_HSI_HZ	64000000
+
+#define TIMEOUT_200MS	(plat_get_syscnt_freq2() / 5U)
+#define TIMEOUT_1S	plat_get_syscnt_freq2()
+
+#define PLLRDY_TIMEOUT	TIMEOUT_200MS
+#define CLKSRC_TIMEOUT	TIMEOUT_200MS
+#define CLKDIV_TIMEOUT	TIMEOUT_200MS
+#define HSIDIV_TIMEOUT	TIMEOUT_200MS
+#define OSCRDY_TIMEOUT	TIMEOUT_1S
+
+enum stm32mp1_parent_id {
+/* Oscillators are defined in enum stm32mp_osc_id */
+
+/* Other parent source */
+	_HSI_KER = NB_OSC,
+	_HSE_KER,
+	_HSE_KER_DIV2,
+	_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,
+	_PARENT_NB,
+	_UNKNOWN_ID = 0xff,
+};
+
+enum stm32mp1_parent_sel {
+	_I2C46_SEL,
+	_UART6_SEL,
+	_UART24_SEL,
+	_UART35_SEL,
+	_UART78_SEL,
+	_SDMMC12_SEL,
+	_SDMMC3_SEL,
+	_QSPI_SEL,
+	_FMC_SEL,
+	_USBPHY_SEL,
+	_USBO_SEL,
+	_STGEN_SEL,
+	_PARENT_SEL_NB,
+	_UNKNOWN_SEL = 0xff,
+};
+
+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_PLL12,
+	CLKSRC_PLL3,
+	CLKSRC_PLL4,
+	CLKSRC_RTC,
+	CLKSRC_MCO1,
+	CLKSRC_MCO2,
+	CLKSRC_NB
+};
+
+enum stm32mp1_clkdiv_id {
+	CLKDIV_MPU,
+	CLKDIV_AXI,
+	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;
+	uint8_t divn_max;
+};
+
+struct stm32mp1_clk_gate {
+	uint16_t offset;
+	uint8_t bit;
+	uint8_t index;
+	uint8_t set_clr;
+	enum stm32mp1_parent_sel sel;
+	enum stm32mp1_parent_id fixed;
+	bool secure;
+};
+
+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];
+};
+
+struct stm32mp1_clk_data {
+	const struct stm32mp1_clk_gate *gate;
+	const struct stm32mp1_clk_sel *sel;
+	const struct stm32mp1_clk_pll *pll;
+	const int nb_gate;
+};
+
+struct stm32mp1_clk_priv {
+	uint32_t base;
+	const struct stm32mp1_clk_data *data;
+	unsigned long osc[NB_OSC];
+	uint32_t pkcs_usb_value;
+};
+
+#define STM32MP1_CLK(off, b, idx, s)			\
+	{						\
+		.offset = (off),			\
+		.bit = (b),				\
+		.index = (idx),				\
+		.set_clr = 0,				\
+		.sel = (s),				\
+		.fixed = _UNKNOWN_ID,			\
+		.secure = 0,				\
+	}
+
+#define STM32MP1_CLK_F(off, b, idx, f)			\
+	{						\
+		.offset = (off),			\
+		.bit = (b),				\
+		.index = (idx),				\
+		.set_clr = 0,				\
+		.sel = _UNKNOWN_SEL,			\
+		.fixed = (f),				\
+		.secure = 0,				\
+	}
+
+#define STM32MP1_CLK_SET_CLR(off, b, idx, s)		\
+	{						\
+		.offset = (off),			\
+		.bit = (b),				\
+		.index = (idx),				\
+		.set_clr = 1,				\
+		.sel = (s),				\
+		.fixed = _UNKNOWN_ID,			\
+		.secure = 0,				\
+	}
+
+#define STM32MP1_CLK_SET_CLR_F(off, b, idx, f)		\
+	{						\
+		.offset = (off),			\
+		.bit = (b),				\
+		.index = (idx),				\
+		.set_clr = 1,				\
+		.sel = _UNKNOWN_SEL,			\
+		.fixed = (f),				\
+		.secure = 0,				\
+	}
+
+#define STM32MP1_CLK_SEC_SET_CLR(off, b, idx, s)	\
+	{						\
+		.offset = (off),			\
+		.bit = (b),				\
+		.index = (idx),				\
+		.set_clr = 1,				\
+		.sel = (s),				\
+		.fixed = _UNKNOWN_ID,			\
+		.secure = 1,				\
+	}
+
+#define STM32MP1_CLK_PARENT(idx, off, s, m, p)		\
+	[(idx)] = {					\
+		.offset = (off),			\
+		.src = (s),				\
+		.msk = (m),				\
+		.parent = (p),				\
+		.nb_parent = ARRAY_SIZE((p))		\
+	}
+
+#define STM32MP1_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),			\
+	}
+
+static const uint8_t stm32mp1_clks[][2] = {
+	{CK_PER, _CK_PER},
+	{CK_MPU, _CK_MPU},
+	{CK_AXI, _ACLK},
+	{CK_HSE, _HSE},
+	{CK_CSI, _CSI},
+	{CK_LSI, _LSI},
+	{CK_LSE, _LSE},
+	{CK_HSI, _HSI},
+	{CK_HSE_DIV2, _HSE_KER_DIV2},
+};
+
+static const struct stm32mp1_clk_gate stm32mp1_clk_gate[] = {
+	STM32MP1_CLK(RCC_DDRITFCR, 0, DDRC1, _UNKNOWN_SEL),
+	STM32MP1_CLK(RCC_DDRITFCR, 1, DDRC1LP, _UNKNOWN_SEL),
+	STM32MP1_CLK(RCC_DDRITFCR, 2, DDRC2, _UNKNOWN_SEL),
+	STM32MP1_CLK(RCC_DDRITFCR, 3, DDRC2LP, _UNKNOWN_SEL),
+	STM32MP1_CLK_F(RCC_DDRITFCR, 4, DDRPHYC, _PLL2_R),
+	STM32MP1_CLK(RCC_DDRITFCR, 5, DDRPHYCLP, _UNKNOWN_SEL),
+	STM32MP1_CLK(RCC_DDRITFCR, 6, DDRCAPB, _UNKNOWN_SEL),
+	STM32MP1_CLK(RCC_DDRITFCR, 7, DDRCAPBLP, _UNKNOWN_SEL),
+	STM32MP1_CLK(RCC_DDRITFCR, 8, AXIDCG, _UNKNOWN_SEL),
+	STM32MP1_CLK(RCC_DDRITFCR, 9, DDRPHYCAPB, _UNKNOWN_SEL),
+	STM32MP1_CLK(RCC_DDRITFCR, 10, DDRPHYCAPBLP, _UNKNOWN_SEL),
+
+	STM32MP1_CLK_SET_CLR(RCC_MP_APB1ENSETR, 14, USART2_K, _UART24_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_APB1ENSETR, 15, USART3_K, _UART35_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_APB1ENSETR, 16, UART4_K, _UART24_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_APB1ENSETR, 17, UART5_K, _UART35_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_APB1ENSETR, 18, UART7_K, _UART78_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_APB1ENSETR, 19, UART8_K, _UART78_SEL),
+
+	STM32MP1_CLK_SET_CLR(RCC_MP_APB2ENSETR, 13, USART6_K, _UART6_SEL),
+
+	STM32MP1_CLK_SET_CLR(RCC_MP_APB4ENSETR, 8, DDRPERFM, _UNKNOWN_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_APB4ENSETR, 15, IWDG2, _UNKNOWN_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_APB4ENSETR, 16, USBPHY_K, _USBPHY_SEL),
+
+	STM32MP1_CLK_SEC_SET_CLR(RCC_MP_APB5ENSETR, 2, I2C4_K, _I2C46_SEL),
+	STM32MP1_CLK_SEC_SET_CLR(RCC_MP_APB5ENSETR, 8, RTCAPB, _PCLK5),
+	STM32MP1_CLK_SEC_SET_CLR(RCC_MP_APB5ENSETR, 11, TZC1, _UNKNOWN_SEL),
+	STM32MP1_CLK_SEC_SET_CLR(RCC_MP_APB5ENSETR, 12, TZC2, _UNKNOWN_SEL),
+	STM32MP1_CLK_SEC_SET_CLR(RCC_MP_APB5ENSETR, 20, STGEN_K, _STGEN_SEL),
+
+	STM32MP1_CLK_SET_CLR(RCC_MP_AHB2ENSETR, 8, USBO_K, _USBO_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_AHB2ENSETR, 16, SDMMC3_K, _SDMMC3_SEL),
+
+	STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 0, GPIOA, _UNKNOWN_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 1, GPIOB, _UNKNOWN_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 2, GPIOC, _UNKNOWN_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 3, GPIOD, _UNKNOWN_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 4, GPIOE, _UNKNOWN_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 5, GPIOF, _UNKNOWN_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 6, GPIOG, _UNKNOWN_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 7, GPIOH, _UNKNOWN_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 8, GPIOI, _UNKNOWN_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 9, GPIOJ, _UNKNOWN_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_AHB4ENSETR, 10, GPIOK, _UNKNOWN_SEL),
+
+	STM32MP1_CLK_SEC_SET_CLR(RCC_MP_AHB5ENSETR, 0, GPIOZ, _UNKNOWN_SEL),
+	STM32MP1_CLK_SEC_SET_CLR(RCC_MP_AHB5ENSETR, 5, HASH1, _UNKNOWN_SEL),
+	STM32MP1_CLK_SEC_SET_CLR(RCC_MP_AHB5ENSETR, 6, RNG1_K, _CSI_KER),
+	STM32MP1_CLK_SEC_SET_CLR(RCC_MP_AHB5ENSETR, 8, BKPSRAM, _UNKNOWN_SEL),
+
+	STM32MP1_CLK_SET_CLR(RCC_MP_AHB6ENSETR, 12, FMC_K, _FMC_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_AHB6ENSETR, 14, QSPI_K, _QSPI_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_AHB6ENSETR, 16, SDMMC1_K, _SDMMC12_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_AHB6ENSETR, 17, SDMMC2_K, _SDMMC12_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_AHB6ENSETR, 24, USBH, _UNKNOWN_SEL),
+
+	STM32MP1_CLK(RCC_DBGCFGR, 8, CK_DBG, _UNKNOWN_SEL),
+};
+
+static const uint8_t i2c46_parents[] = {_PCLK5, _PLL3_Q, _HSI_KER, _CSI_KER};
+static const uint8_t uart6_parents[] = {_PCLK2, _PLL4_Q, _HSI_KER, _CSI_KER,
+					_HSE_KER};
+static const uint8_t uart24_parents[] = {_PCLK1, _PLL4_Q, _HSI_KER, _CSI_KER,
+					 _HSE_KER};
+static const uint8_t uart35_parents[] = {_PCLK1, _PLL4_Q, _HSI_KER, _CSI_KER,
+					 _HSE_KER};
+static const uint8_t uart78_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 usbphy_parents[] = {_HSE_KER, _PLL4_R, _HSE_KER_DIV2};
+static const uint8_t usbo_parents[] = {_PLL4_R, _USB_PHY_48};
+static const uint8_t stgen_parents[] = {_HSI_KER, _HSE_KER};
+
+static const struct stm32mp1_clk_sel stm32mp1_clk_sel[_PARENT_SEL_NB] = {
+	STM32MP1_CLK_PARENT(_I2C46_SEL, RCC_I2C46CKSELR, 0, 0x7, i2c46_parents),
+	STM32MP1_CLK_PARENT(_UART6_SEL, RCC_UART6CKSELR, 0, 0x7, uart6_parents),
+	STM32MP1_CLK_PARENT(_UART24_SEL, RCC_UART24CKSELR, 0, 0x7,
+			    uart24_parents),
+	STM32MP1_CLK_PARENT(_UART35_SEL, RCC_UART35CKSELR, 0, 0x7,
+			    uart35_parents),
+	STM32MP1_CLK_PARENT(_UART78_SEL, RCC_UART78CKSELR, 0, 0x7,
+			    uart78_parents),
+	STM32MP1_CLK_PARENT(_SDMMC12_SEL, RCC_SDMMC12CKSELR, 0, 0x7,
+			    sdmmc12_parents),
+	STM32MP1_CLK_PARENT(_SDMMC3_SEL, RCC_SDMMC3CKSELR, 0, 0x7,
+			    sdmmc3_parents),
+	STM32MP1_CLK_PARENT(_QSPI_SEL, RCC_QSPICKSELR, 0, 0xf, qspi_parents),
+	STM32MP1_CLK_PARENT(_FMC_SEL, RCC_FMCCKSELR, 0, 0xf, fmc_parents),
+	STM32MP1_CLK_PARENT(_USBPHY_SEL, RCC_USBCKSELR, 0, 0x3, usbphy_parents),
+	STM32MP1_CLK_PARENT(_USBO_SEL, RCC_USBCKSELR, 4, 0x1, usbo_parents),
+	STM32MP1_CLK_PARENT(_STGEN_SEL, RCC_STGENCKSELR, 0, 0x3, stgen_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,
+		.divn_max = 99,
+	},
+	[PLL_1600] = {
+		.refclk_min = 8,
+		.refclk_max = 16,
+		.divn_max = 199,
+	},
+};
+
+/* 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] = {
+	STM32MP1_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),
+	STM32MP1_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),
+	STM32MP1_CLK_PLL(_PLL3, PLL_800,
+			 RCC_RCK3SELR, RCC_PLL3CFGR1, RCC_PLL3CFGR2,
+			 RCC_PLL3FRACR, RCC_PLL3CR, RCC_PLL3CSGR,
+			 _HSI, _HSE, _CSI, _UNKNOWN_OSC_ID),
+	STM32MP1_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 : 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 struct stm32mp1_clk_data stm32mp1_data = {
+	.gate = stm32mp1_clk_gate,
+	.sel = stm32mp1_clk_sel,
+	.pll = stm32mp1_clk_pll,
+	.nb_gate = ARRAY_SIZE(stm32mp1_clk_gate),
+};
+
+static struct stm32mp1_clk_priv stm32mp1_clk_priv_data;
+
+static unsigned long stm32mp1_clk_get_fixed(struct stm32mp1_clk_priv *priv,
+					    enum stm32mp_osc_id idx)
+{
+	if (idx >= NB_OSC) {
+		return 0;
+	}
+
+	return priv->osc[idx];
+}
+
+static int stm32mp1_clk_get_id(struct stm32mp1_clk_priv *priv, unsigned long id)
+{
+	const struct stm32mp1_clk_gate *gate = priv->data->gate;
+	int i;
+	int nb_clks = priv->data->nb_gate;
+
+	for (i = 0; i < nb_clks; i++) {
+		if (gate[i].index == id) {
+			return i;
+		}
+	}
+
+	ERROR("%s: clk id %d not found\n", __func__, (uint32_t)id);
+
+	return -EINVAL;
+}
+
+static enum stm32mp1_parent_sel
+stm32mp1_clk_get_sel(struct stm32mp1_clk_priv *priv, int i)
+{
+	const struct stm32mp1_clk_gate *gate = priv->data->gate;
+
+	return gate[i].sel;
+}
+
+static enum stm32mp1_parent_id
+stm32mp1_clk_get_fixed_parent(struct stm32mp1_clk_priv *priv, int i)
+{
+	const struct stm32mp1_clk_gate *gate = priv->data->gate;
+
+	return gate[i].fixed;
+}
+
+static int stm32mp1_clk_get_parent(struct stm32mp1_clk_priv *priv,
+				   unsigned long id)
+{
+	const struct stm32mp1_clk_sel *sel = priv->data->sel;
+	uint32_t j, p_sel;
+	int i;
+	enum stm32mp1_parent_id p;
+	enum stm32mp1_parent_sel s;
+
+	for (j = 0; j < ARRAY_SIZE(stm32mp1_clks); j++) {
+		if (stm32mp1_clks[j][0] == id) {
+			return (int)stm32mp1_clks[j][1];
+		}
+	}
+
+	i = stm32mp1_clk_get_id(priv, id);
+	if (i < 0) {
+		return i;
+	}
+
+	p = stm32mp1_clk_get_fixed_parent(priv, i);
+	if (p < _PARENT_NB) {
+		return (int)p;
+	}
+
+	s = stm32mp1_clk_get_sel(priv, i);
+	if (s >= _PARENT_SEL_NB) {
+		return -EINVAL;
+	}
+
+	p_sel = (mmio_read_32(priv->base + sel[s].offset) >> sel[s].src) &
+		sel[s].msk;
+
+	if (p_sel < sel[s].nb_parent) {
+		return (int)sel[s].parent[p_sel];
+	}
+
+	ERROR("%s: no parents defined for clk id %ld\n", __func__, id);
+
+	return -EINVAL;
+}
+
+static unsigned long stm32mp1_pll_get_fref_ck(struct stm32mp1_clk_priv *priv,
+					      enum stm32mp1_pll_id pll_id)
+{
+	const struct stm32mp1_clk_pll *pll = priv->data->pll;
+	uint32_t selr, src;
+	unsigned long refclk;
+
+	selr = mmio_read_32(priv->base + pll[pll_id].rckxselr);
+	src = selr & RCC_SELR_REFCLK_SRC_MASK;
+
+	refclk = stm32mp1_clk_get_fixed(priv, pll[pll_id].refclk[src]);
+
+	return refclk;
+}
+
+/*
+ * 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(struct stm32mp1_clk_priv *priv,
+					   enum stm32mp1_pll_id pll_id)
+{
+	const struct stm32mp1_clk_pll *pll = priv->data->pll;
+	unsigned long refclk, fvco;
+	uint32_t cfgr1, fracr, divm, divn;
+
+	cfgr1 = mmio_read_32(priv->base + pll[pll_id].pllxcfgr1);
+	fracr = mmio_read_32(priv->base + pll[pll_id].pllxfracr);
+
+	divm = (cfgr1 & (RCC_PLLNCFGR1_DIVM_MASK)) >> RCC_PLLNCFGR1_DIVM_SHIFT;
+	divn = cfgr1 & RCC_PLLNCFGR1_DIVN_MASK;
+
+	refclk = stm32mp1_pll_get_fref_ck(priv, pll_id);
+
+	/*
+	 * 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;
+		numerator = (refclk * numerator) + fracv;
+		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(struct stm32mp1_clk_priv *priv,
+					    enum stm32mp1_pll_id pll_id,
+					    enum stm32mp1_div_id div_id)
+{
+	const struct stm32mp1_clk_pll *pll = priv->data->pll;
+	unsigned long dfout;
+	uint32_t cfgr2, divy;
+
+	if (div_id >= _DIV_NB) {
+		return 0;
+	}
+
+	cfgr2 = mmio_read_32(priv->base + pll[pll_id].pllxcfgr2);
+	divy = (cfgr2 >> pllncfgr2[div_id]) & RCC_PLLNCFGR2_DIVX_MASK;
+
+	dfout = stm32mp1_pll_get_fvco(priv, pll_id) / (divy + 1U);
+
+	return dfout;
+}
+
+static unsigned long stm32mp1_clk_get(struct stm32mp1_clk_priv *priv, int p)
+{
+	uint32_t reg, clkdiv;
+	unsigned long clock = 0;
+
+	switch (p) {
+	case _CK_MPU:
+	/* MPU sub system */
+		reg = mmio_read_32(priv->base + RCC_MPCKSELR);
+		switch (reg & RCC_SELR_SRC_MASK) {
+		case RCC_MPCKSELR_HSI:
+			clock = stm32mp1_clk_get_fixed(priv, _HSI);
+			break;
+		case RCC_MPCKSELR_HSE:
+			clock = stm32mp1_clk_get_fixed(priv, _HSE);
+			break;
+		case RCC_MPCKSELR_PLL:
+			clock = stm32mp1_read_pll_freq(priv, _PLL1, _DIV_P);
+			break;
+		case RCC_MPCKSELR_PLL_MPUDIV:
+			clock = stm32mp1_read_pll_freq(priv, _PLL1, _DIV_P);
+
+			reg = mmio_read_32(priv->base + RCC_MPCKDIVR);
+			clkdiv = reg & RCC_MPUDIV_MASK;
+			if (clkdiv != 0U) {
+				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(priv->base + RCC_ASSCKSELR);
+		switch (reg & RCC_SELR_SRC_MASK) {
+		case RCC_ASSCKSELR_HSI:
+			clock = stm32mp1_clk_get_fixed(priv, _HSI);
+			break;
+		case RCC_ASSCKSELR_HSE:
+			clock = stm32mp1_clk_get_fixed(priv, _HSE);
+			break;
+		case RCC_ASSCKSELR_PLL:
+			clock = stm32mp1_read_pll_freq(priv, _PLL2, _DIV_P);
+			break;
+		default:
+			break;
+		}
+
+		/* System clock divider */
+		reg = mmio_read_32(priv->base + RCC_AXIDIVR);
+		clock /= stm32mp1_axi_div[reg & RCC_AXIDIV_MASK];
+
+		switch (p) {
+		case _PCLK4:
+			reg = mmio_read_32(priv->base + RCC_APB4DIVR);
+			clock >>= stm32mp1_apbx_div[reg & RCC_APBXDIV_MASK];
+			break;
+		case _PCLK5:
+			reg = mmio_read_32(priv->base + RCC_APB5DIVR);
+			clock >>= stm32mp1_apbx_div[reg & RCC_APBXDIV_MASK];
+			break;
+		default:
+			break;
+		}
+		break;
+	case _CK_PER:
+		reg = mmio_read_32(priv->base + RCC_CPERCKSELR);
+		switch (reg & RCC_SELR_SRC_MASK) {
+		case RCC_CPERCKSELR_HSI:
+			clock = stm32mp1_clk_get_fixed(priv, _HSI);
+			break;
+		case RCC_CPERCKSELR_HSE:
+			clock = stm32mp1_clk_get_fixed(priv, _HSE);
+			break;
+		case RCC_CPERCKSELR_CSI:
+			clock = stm32mp1_clk_get_fixed(priv, _CSI);
+			break;
+		default:
+			break;
+		}
+		break;
+	case _HSI:
+	case _HSI_KER:
+		clock = stm32mp1_clk_get_fixed(priv, _HSI);
+		break;
+	case _CSI:
+	case _CSI_KER:
+		clock = stm32mp1_clk_get_fixed(priv, _CSI);
+		break;
+	case _HSE:
+	case _HSE_KER:
+		clock = stm32mp1_clk_get_fixed(priv, _HSE);
+		break;
+	case _HSE_KER_DIV2:
+		clock = stm32mp1_clk_get_fixed(priv, _HSE) >> 1;
+		break;
+	case _LSI:
+		clock = stm32mp1_clk_get_fixed(priv, _LSI);
+		break;
+	case _LSE:
+		clock = stm32mp1_clk_get_fixed(priv, _LSE);
+		break;
+	/* PLL */
+	case _PLL1_P:
+		clock = stm32mp1_read_pll_freq(priv, _PLL1, _DIV_P);
+		break;
+	case _PLL1_Q:
+		clock = stm32mp1_read_pll_freq(priv, _PLL1, _DIV_Q);
+		break;
+	case _PLL1_R:
+		clock = stm32mp1_read_pll_freq(priv, _PLL1, _DIV_R);
+		break;
+	case _PLL2_P:
+		clock = stm32mp1_read_pll_freq(priv, _PLL2, _DIV_P);
+		break;
+	case _PLL2_Q:
+		clock = stm32mp1_read_pll_freq(priv, _PLL2, _DIV_Q);
+		break;
+	case _PLL2_R:
+		clock = stm32mp1_read_pll_freq(priv, _PLL2, _DIV_R);
+		break;
+	case _PLL3_P:
+		clock = stm32mp1_read_pll_freq(priv, _PLL3, _DIV_P);
+		break;
+	case _PLL3_Q:
+		clock = stm32mp1_read_pll_freq(priv, _PLL3, _DIV_Q);
+		break;
+	case _PLL3_R:
+		clock = stm32mp1_read_pll_freq(priv, _PLL3, _DIV_R);
+		break;
+	case _PLL4_P:
+		clock = stm32mp1_read_pll_freq(priv, _PLL4, _DIV_P);
+		break;
+	case _PLL4_Q:
+		clock = stm32mp1_read_pll_freq(priv, _PLL4, _DIV_Q);
+		break;
+	case _PLL4_R:
+		clock = stm32mp1_read_pll_freq(priv, _PLL4, _DIV_R);
+		break;
+	/* Other */
+	case _USB_PHY_48:
+		clock = stm32mp1_clk_get_fixed(priv, _USB_PHY_48);
+		break;
+	default:
+		break;
+	}
+
+	return clock;
+}
+
+bool stm32mp1_clk_is_enabled(unsigned long id)
+{
+	struct stm32mp1_clk_priv *priv = &stm32mp1_clk_priv_data;
+	const struct stm32mp1_clk_gate *gate = priv->data->gate;
+	int i = stm32mp1_clk_get_id(priv, id);
+
+	if (i < 0) {
+		return false;
+	}
+
+	return ((mmio_read_32(priv->base + gate[i].offset) &
+		 BIT(gate[i].bit)) != 0U);
+}
+
+int stm32mp1_clk_enable(unsigned long id)
+{
+	struct stm32mp1_clk_priv *priv = &stm32mp1_clk_priv_data;
+	const struct stm32mp1_clk_gate *gate = priv->data->gate;
+	int i = stm32mp1_clk_get_id(priv, id);
+
+	if (i < 0) {
+		return i;
+	}
+
+	if (gate[i].set_clr != 0U) {
+		mmio_write_32(priv->base + gate[i].offset, BIT(gate[i].bit));
+	} else {
+		mmio_setbits_32(priv->base + gate[i].offset, BIT(gate[i].bit));
+	}
+
+	return 0;
+}
+
+int stm32mp1_clk_disable(unsigned long id)
+{
+	struct stm32mp1_clk_priv *priv = &stm32mp1_clk_priv_data;
+	const struct stm32mp1_clk_gate *gate = priv->data->gate;
+	int i = stm32mp1_clk_get_id(priv, id);
+
+	if (i < 0) {
+		return i;
+	}
+
+	if (gate[i].set_clr != 0U) {
+		mmio_write_32(priv->base + gate[i].offset
+			      + RCC_MP_ENCLRR_OFFSET,
+			      BIT(gate[i].bit));
+	} else {
+		mmio_clrbits_32(priv->base + gate[i].offset, BIT(gate[i].bit));
+	}
+
+	return 0;
+}
+
+unsigned long stm32mp1_clk_get_rate(unsigned long id)
+{
+	struct stm32mp1_clk_priv *priv = &stm32mp1_clk_priv_data;
+	int p = stm32mp1_clk_get_parent(priv, id);
+	unsigned long rate;
+
+	if (p < 0) {
+		return 0;
+	}
+
+	rate = stm32mp1_clk_get(priv, p);
+
+	return rate;
+}
+
+static void stm32mp1_ls_osc_set(int enable, uint32_t rcc, uint32_t offset,
+				uint32_t mask_on)
+{
+	uint32_t address = rcc + offset;
+
+	if (enable != 0) {
+		mmio_setbits_32(address, mask_on);
+	} else {
+		mmio_clrbits_32(address, mask_on);
+	}
+}
+
+static void stm32mp1_hs_ocs_set(int enable, uint32_t rcc, uint32_t mask_on)
+{
+	if (enable != 0) {
+		mmio_setbits_32(rcc + RCC_OCENSETR, mask_on);
+	} else {
+		mmio_setbits_32(rcc + RCC_OCENCLRR, mask_on);
+	}
+}
+
+static int stm32mp1_osc_wait(int enable, uint32_t rcc, uint32_t offset,
+			     uint32_t mask_rdy)
+{
+	unsigned long start;
+	uint32_t mask_test;
+	uint32_t address = rcc + offset;
+
+	if (enable != 0) {
+		mask_test = mask_rdy;
+	} else {
+		mask_test = 0;
+	}
+
+	start = get_timer(0);
+	while ((mmio_read_32(address) & mask_rdy) != mask_test) {
+		if (get_timer(start) > OSCRDY_TIMEOUT) {
+			ERROR("OSC %x @ %x timeout for enable=%d : 0x%x\n",
+			      mask_rdy, address, enable, mmio_read_32(address));
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+static void stm32mp1_lse_enable(uint32_t rcc, bool bypass, uint32_t lsedrv)
+{
+	uint32_t value;
+
+	if (bypass) {
+		mmio_setbits_32(rcc + 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 + RCC_BDCR) & RCC_BDCR_LSEDRV_MASK) >>
+		RCC_BDCR_LSEDRV_SHIFT;
+
+	while (value != lsedrv) {
+		if (value > lsedrv) {
+			value--;
+		} else {
+			value++;
+		}
+
+		mmio_clrsetbits_32(rcc + RCC_BDCR,
+				   RCC_BDCR_LSEDRV_MASK,
+				   value << RCC_BDCR_LSEDRV_SHIFT);
+	}
+
+	stm32mp1_ls_osc_set(1, rcc, RCC_BDCR, RCC_BDCR_LSEON);
+}
+
+static void stm32mp1_lse_wait(uint32_t rcc)
+{
+	if (stm32mp1_osc_wait(1, rcc, RCC_BDCR, RCC_BDCR_LSERDY) != 0) {
+		VERBOSE("%s: failed\n", __func__);
+	}
+}
+
+static void stm32mp1_lsi_set(uint32_t rcc, int enable)
+{
+	stm32mp1_ls_osc_set(enable, rcc, RCC_RDLSICR, RCC_RDLSICR_LSION);
+	if (stm32mp1_osc_wait(enable, rcc, RCC_RDLSICR, RCC_RDLSICR_LSIRDY) !=
+	    0) {
+		VERBOSE("%s: failed\n", __func__);
+	}
+}
+
+static void stm32mp1_hse_enable(uint32_t rcc, bool bypass, bool css)
+{
+	if (bypass) {
+		mmio_setbits_32(rcc + RCC_OCENSETR, RCC_OCENR_HSEBYP);
+	}
+
+	stm32mp1_hs_ocs_set(1, rcc, RCC_OCENR_HSEON);
+	if (stm32mp1_osc_wait(1, rcc, RCC_OCRDYR, RCC_OCRDYR_HSERDY) !=
+	    0) {
+		VERBOSE("%s: failed\n", __func__);
+	}
+
+	if (css) {
+		mmio_setbits_32(rcc + RCC_OCENSETR, RCC_OCENR_HSECSSON);
+	}
+}
+
+static void stm32mp1_csi_set(uint32_t rcc, int enable)
+{
+	stm32mp1_ls_osc_set(enable, rcc, RCC_OCENSETR, RCC_OCENR_CSION);
+	if (stm32mp1_osc_wait(enable, rcc, RCC_OCRDYR, RCC_OCRDYR_CSIRDY) !=
+	    0) {
+		VERBOSE("%s: failed\n", __func__);
+	}
+}
+
+static void stm32mp1_hsi_set(uint32_t rcc, int enable)
+{
+	stm32mp1_hs_ocs_set(enable, rcc, RCC_OCENR_HSION);
+	if (stm32mp1_osc_wait(enable, rcc, RCC_OCRDYR, RCC_OCRDYR_HSIRDY) !=
+	    0) {
+		VERBOSE("%s: failed\n", __func__);
+	}
+}
+
+static int stm32mp1_set_hsidiv(uint32_t rcc, uint8_t hsidiv)
+{
+	unsigned long start;
+	uint32_t address = rcc + RCC_OCRDYR;
+
+	mmio_clrsetbits_32(rcc + RCC_HSICFGR,
+			   RCC_HSICFGR_HSIDIV_MASK,
+			   RCC_HSICFGR_HSIDIV_MASK & (uint32_t)hsidiv);
+
+	start = get_timer(0);
+	while ((mmio_read_32(address) & RCC_OCRDYR_HSIDIVRDY) == 0U) {
+		if (get_timer(start) > HSIDIV_TIMEOUT) {
+			ERROR("HSIDIV failed @ 0x%x: 0x%x\n",
+			      address, mmio_read_32(address));
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+static int stm32mp1_hsidiv(uint32_t rcc, 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(rcc, hsidiv);
+	}
+
+	return 0;
+}
+
+static void stm32mp1_pll_start(struct stm32mp1_clk_priv *priv,
+			       enum stm32mp1_pll_id pll_id)
+{
+	const struct stm32mp1_clk_pll *pll = priv->data->pll;
+
+	mmio_write_32(priv->base + pll[pll_id].pllxcr, RCC_PLLNCR_PLLON);
+}
+
+static int stm32mp1_pll_output(struct stm32mp1_clk_priv *priv,
+			       enum stm32mp1_pll_id pll_id, uint32_t output)
+{
+	const struct stm32mp1_clk_pll *pll = priv->data->pll;
+	uint32_t pllxcr = priv->base + pll[pll_id].pllxcr;
+	unsigned long start;
+
+	start = get_timer(0);
+	/* Wait PLL lock */
+	while ((mmio_read_32(pllxcr) & RCC_PLLNCR_PLLRDY) == 0U) {
+		if (get_timer(start) > PLLRDY_TIMEOUT) {
+			ERROR("PLL%d start failed @ 0x%x: 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(struct stm32mp1_clk_priv *priv,
+			     enum stm32mp1_pll_id pll_id)
+{
+	const struct stm32mp1_clk_pll *pll = priv->data->pll;
+	uint32_t pllxcr = priv->base + pll[pll_id].pllxcr;
+	unsigned long start;
+
+	/* 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);
+
+	start = get_timer(0);
+	/* Wait PLL stopped */
+	while ((mmio_read_32(pllxcr) & RCC_PLLNCR_PLLRDY) != 0U) {
+		if (get_timer(start) > PLLRDY_TIMEOUT) {
+			ERROR("PLL%d stop failed @ 0x%x: 0x%x\n",
+			      pll_id, pllxcr, mmio_read_32(pllxcr));
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+static void stm32mp1_pll_config_output(struct stm32mp1_clk_priv *priv,
+				       enum stm32mp1_pll_id pll_id,
+				       uint32_t *pllcfg)
+{
+	const struct stm32mp1_clk_pll *pll = priv->data->pll;
+	uint32_t rcc = priv->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 + pll[pll_id].pllxcfgr2, value);
+}
+
+static int stm32mp1_pll_config(struct stm32mp1_clk_priv *priv,
+			       enum stm32mp1_pll_id pll_id,
+			       uint32_t *pllcfg, uint32_t fracv)
+{
+	const struct stm32mp1_clk_pll *pll = priv->data->pll;
+	uint32_t rcc = priv->base;
+	enum stm32mp1_plltype type = pll[pll_id].plltype;
+	unsigned long refclk;
+	uint32_t ifrge = 0;
+	uint32_t src, value;
+
+	src = mmio_read_32(priv->base + pll[pll_id].rckxselr) &
+		RCC_SELR_REFCLK_SRC_MASK;
+
+	refclk = stm32mp1_clk_get_fixed(priv, pll[pll_id].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 + pll[pll_id].pllxcfgr1, value);
+
+	/* Fractional configuration */
+	value = 0;
+	mmio_write_32(rcc + pll[pll_id].pllxfracr, value);
+
+	value = fracv << RCC_PLLNFRACR_FRACV_SHIFT;
+	mmio_write_32(rcc + pll[pll_id].pllxfracr, value);
+
+	value |= RCC_PLLNFRACR_FRACLE;
+	mmio_write_32(rcc + pll[pll_id].pllxfracr, value);
+
+	stm32mp1_pll_config_output(priv, pll_id, pllcfg);
+
+	return 0;
+}
+
+static void stm32mp1_pll_csg(struct stm32mp1_clk_priv *priv,
+			     enum stm32mp1_pll_id pll_id,
+			     uint32_t *csg)
+{
+	const struct stm32mp1_clk_pll *pll = priv->data->pll;
+	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(priv->base + pll[pll_id].pllxcsgr, pllxcsg);
+}
+
+static int stm32mp1_set_clksrc(struct stm32mp1_clk_priv *priv,
+			       unsigned int clksrc)
+{
+	uint32_t address = priv->base + (clksrc >> 4);
+	unsigned long start;
+
+	mmio_clrsetbits_32(address, RCC_SELR_SRC_MASK,
+			   clksrc & RCC_SELR_SRC_MASK);
+
+	start = get_timer(0);
+	while ((mmio_read_32(address) & RCC_SELR_SRCRDY) == 0U) {
+		if (get_timer(start) > CLKSRC_TIMEOUT) {
+			ERROR("CLKSRC %x start failed @ 0x%x: 0x%x\n",
+			      clksrc, address, mmio_read_32(address));
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+static int stm32mp1_set_clkdiv(unsigned int clkdiv, uint32_t address)
+{
+	unsigned long start;
+
+	mmio_clrsetbits_32(address, RCC_DIVR_DIV_MASK,
+			   clkdiv & RCC_DIVR_DIV_MASK);
+
+	start = get_timer(0);
+	while ((mmio_read_32(address) & RCC_DIVR_DIVRDY) == 0U) {
+		if (get_timer(start) > CLKDIV_TIMEOUT) {
+			ERROR("CLKDIV %x start failed @ 0x%x: 0x%x\n",
+			      clkdiv, address, mmio_read_32(address));
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+static void stm32mp1_mco_csg(struct stm32mp1_clk_priv *priv,
+			     uint32_t clksrc, uint32_t clkdiv)
+{
+	uint32_t address = priv->base + (clksrc >> 4);
+
+	/*
+	 * Binding clksrc :
+	 *      bit15-4 offset
+	 *      bit3:   disable
+	 *      bit2-0: MCOSEL[2:0]
+	 */
+	if ((clksrc & 0x8U) != 0U) {
+		mmio_clrbits_32(address, RCC_MCOCFG_MCOON);
+	} else {
+		mmio_clrsetbits_32(address,
+				   RCC_MCOCFG_MCOSRC_MASK,
+				   clksrc & RCC_MCOCFG_MCOSRC_MASK);
+		mmio_clrsetbits_32(address,
+				   RCC_MCOCFG_MCODIV_MASK,
+				   clkdiv << RCC_MCOCFG_MCODIV_SHIFT);
+		mmio_setbits_32(address, RCC_MCOCFG_MCOON);
+	}
+}
+
+static void stm32mp1_set_rtcsrc(struct stm32mp1_clk_priv *priv,
+				unsigned int clksrc, bool lse_css)
+{
+	uint32_t address = priv->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_BDCR_RTCSRC_SHIFT);
+
+		mmio_setbits_32(address, RCC_BDCR_RTCCKEN);
+	}
+
+	if (lse_css) {
+		mmio_setbits_32(address, RCC_BDCR_LSECSSON);
+	}
+}
+
+#define CNTCVL_OFF	0x008
+#define CNTCVU_OFF	0x00C
+
+static void stm32mp1_stgen_config(struct stm32mp1_clk_priv *priv)
+{
+	uintptr_t stgen;
+	int p;
+	uint32_t cntfid0;
+	unsigned long rate;
+
+	stgen = fdt_get_stgen_base();
+
+	cntfid0 = mmio_read_32(stgen + CNTFID_OFF);
+	p = stm32mp1_clk_get_parent(priv, STGEN_K);
+	rate = stm32mp1_clk_get(priv, p);
+
+	if (cntfid0 != rate) {
+		unsigned long long counter;
+
+		mmio_clrbits_32(stgen + CNTCR_OFF, CNTCR_EN);
+		counter = (unsigned long long)
+			mmio_read_32(stgen + CNTCVL_OFF);
+		counter |= ((unsigned long long)
+			    (mmio_read_32(stgen + CNTCVU_OFF))) << 32;
+		counter = (counter * rate / cntfid0);
+		mmio_write_32(stgen + CNTCVL_OFF, (uint32_t)counter);
+		mmio_write_32(stgen + CNTCVU_OFF, (uint32_t)(counter >> 32));
+		mmio_write_32(stgen + CNTFID_OFF, rate);
+		mmio_setbits_32(stgen + CNTCR_OFF, CNTCR_EN);
+
+		write_cntfrq((u_register_t)rate);
+
+		/* Need to update timer with new frequency */
+		generic_delay_timer_init();
+	}
+}
+
+void stm32mp1_stgen_increment(unsigned long long offset_in_ms)
+{
+	uintptr_t stgen;
+	unsigned long long cnt;
+
+	stgen = fdt_get_stgen_base();
+
+	cnt = ((unsigned long long)mmio_read_32(stgen + CNTCVU_OFF) << 32) |
+		mmio_read_32(stgen + CNTCVL_OFF);
+
+	cnt += (offset_in_ms * mmio_read_32(stgen + CNTFID_OFF)) / 1000U;
+
+	mmio_clrbits_32(stgen + CNTCR_OFF, CNTCR_EN);
+	mmio_write_32(stgen + CNTCVL_OFF, (uint32_t)cnt);
+	mmio_write_32(stgen + CNTCVU_OFF, (uint32_t)(cnt >> 32));
+	mmio_setbits_32(stgen + CNTCR_OFF, CNTCR_EN);
+}
+
+static void stm32mp1_pkcs_config(struct stm32mp1_clk_priv *priv, uint32_t pkcs)
+{
+	uint32_t address = priv->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);
+}
+
+int stm32mp1_clk_init(void)
+{
+	struct stm32mp1_clk_priv *priv = &stm32mp1_clk_priv_data;
+	uint32_t rcc = priv->base;
+	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 lse_css = false;
+	const uint32_t *pkcs_cell;
+
+	/* Check status field to disable security */
+	if (!fdt_get_rcc_secure_status()) {
+		mmio_write_32(rcc + RCC_TZCR, 0);
+	}
+
+	ret = fdt_rcc_read_uint32_array("st,clksrc", clksrc,
+					(uint32_t)CLKSRC_NB);
+	if (ret < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	ret = fdt_rcc_read_uint32_array("st,clkdiv", clkdiv,
+					(uint32_t)CLKDIV_NB);
+	if (ret < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	for (i = (enum stm32mp1_pll_id)0; i < _PLL_NB; i++) {
+		char name[12];
+
+		sprintf(name, "st,pll@%d", i);
+		plloff[i] = fdt_rcc_subnode_offset(name);
+
+		if (!fdt_check_node(plloff[i])) {
+			continue;
+		}
+
+		ret = fdt_read_uint32_array(plloff[i], "cfg",
+					    pllcfg[i], (int)PLLCFG_NB);
+		if (ret < 0) {
+			return -FDT_ERR_NOTFOUND;
+		}
+	}
+
+	stm32mp1_mco_csg(priv, clksrc[CLKSRC_MCO1], clkdiv[CLKDIV_MCO1]);
+	stm32mp1_mco_csg(priv, clksrc[CLKSRC_MCO2], clkdiv[CLKDIV_MCO2]);
+
+	/*
+	 * Switch ON oscillator found in device-tree.
+	 * Note: HSI already ON after BootROM stage.
+	 */
+	if (priv->osc[_LSI] != 0U) {
+		stm32mp1_lsi_set(rcc, 1);
+	}
+	if (priv->osc[_LSE] != 0U) {
+		bool bypass;
+		uint32_t lsedrv;
+
+		bypass = fdt_osc_read_bool(_LSE, "st,bypass");
+		lse_css = fdt_osc_read_bool(_LSE, "st,css");
+		lsedrv = fdt_osc_read_uint32_default(_LSE, "st,drive",
+						     LSEDRV_MEDIUM_HIGH);
+		stm32mp1_lse_enable(rcc, bypass, lsedrv);
+	}
+	if (priv->osc[_HSE] != 0U) {
+		bool bypass, css;
+
+		bypass = fdt_osc_read_bool(_LSE, "st,bypass");
+		css = fdt_osc_read_bool(_LSE, "st,css");
+		stm32mp1_hse_enable(rcc, bypass, 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(rcc, 1);
+
+	/* Come back to HSI */
+	ret = stm32mp1_set_clksrc(priv, CLK_MPU_HSI);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = stm32mp1_set_clksrc(priv, CLK_AXI_HSI);
+	if (ret != 0) {
+		return ret;
+	}
+
+	for (i = (enum stm32mp1_pll_id)0; i < _PLL_NB; i++) {
+		if (i == _PLL4)
+			continue;
+		ret = stm32mp1_pll_stop(priv, i);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	/* Configure HSIDIV */
+	if (priv->osc[_HSI] != 0U) {
+		ret = stm32mp1_hsidiv(rcc, priv->osc[_HSI]);
+		if (ret != 0) {
+			return ret;
+		}
+		stm32mp1_stgen_config(priv);
+	}
+
+	/* Select DIV */
+	/* No ready bit when MPUSRC != CLK_MPU_PLL1P_DIV, MPUDIV is disabled */
+	mmio_write_32(rcc + RCC_MPCKDIVR,
+		      clkdiv[CLKDIV_MPU] & RCC_DIVR_DIV_MASK);
+	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_AXI], rcc + RCC_AXIDIVR);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_APB4], rcc + RCC_APB4DIVR);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_APB5], rcc + RCC_APB5DIVR);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_APB1], rcc + RCC_APB1DIVR);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_APB2], rcc + RCC_APB2DIVR);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_APB3], rcc + RCC_APB3DIVR);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* No ready bit for RTC */
+	mmio_write_32(rcc + RCC_RTCDIVR,
+		      clkdiv[CLKDIV_RTC] & RCC_DIVR_DIV_MASK);
+
+	/* Configure PLLs source */
+	ret = stm32mp1_set_clksrc(priv, clksrc[CLKSRC_PLL12]);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = stm32mp1_set_clksrc(priv, clksrc[CLKSRC_PLL3]);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = stm32mp1_set_clksrc(priv, clksrc[CLKSRC_PLL4]);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* Configure and start PLLs */
+	for (i = (enum stm32mp1_pll_id)0; i < _PLL_NB; i++) {
+		uint32_t fracv;
+		uint32_t csg[PLLCSG_NB];
+
+		if (!fdt_check_node(plloff[i])) {
+			continue;
+		}
+
+		fracv = fdt_read_uint32_default(plloff[i], "frac", 0);
+
+		ret = stm32mp1_pll_config(priv, i, pllcfg[i], fracv);
+		if (ret != 0) {
+			return ret;
+		}
+		ret = fdt_read_uint32_array(plloff[i], "csg", csg,
+					    (uint32_t)PLLCSG_NB);
+		if (ret == 0) {
+			stm32mp1_pll_csg(priv, i, csg);
+		} else if (ret != -FDT_ERR_NOTFOUND) {
+			return ret;
+		}
+
+		stm32mp1_pll_start(priv, i);
+	}
+	/* Wait and start PLLs ouptut when ready */
+	for (i = (enum stm32mp1_pll_id)0; i < _PLL_NB; i++) {
+		if (!fdt_check_node(plloff[i])) {
+			continue;
+		}
+
+		ret = stm32mp1_pll_output(priv, i, pllcfg[i][PLLCFG_O]);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+	/* Wait LSE ready before to use it */
+	if (priv->osc[_LSE] != 0U) {
+		stm32mp1_lse_wait(rcc);
+	}
+
+	/* Configure with expected clock source */
+	ret = stm32mp1_set_clksrc(priv, clksrc[CLKSRC_MPU]);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = stm32mp1_set_clksrc(priv, clksrc[CLKSRC_AXI]);
+	if (ret != 0) {
+		return ret;
+	}
+	stm32mp1_set_rtcsrc(priv, 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;
+
+		priv->pkcs_usb_value = 0;
+
+		for (j = 0; j < ((uint32_t)len / sizeof(uint32_t)); j++) {
+			uint32_t pkcs = (uint32_t)fdt32_to_cpu(pkcs_cell[j]);
+
+			if (pkcs == (uint32_t)CLK_CKPER_DISABLED) {
+				ckper_disabled = true;
+				continue;
+			}
+			stm32mp1_pkcs_config(priv, 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(priv, CLK_CKPER_DISABLED);
+		}
+	}
+
+	/* Switch OFF HSI if not found in device-tree */
+	if (priv->osc[_HSI] == 0U) {
+		stm32mp1_hsi_set(rcc, 0);
+	}
+	stm32mp1_stgen_config(priv);
+
+	/* 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;
+}
+
+static void stm32mp1_osc_clk_init(const char *name,
+				  struct stm32mp1_clk_priv *priv,
+				  enum stm32mp_osc_id index)
+{
+	uint32_t frequency;
+
+	priv->osc[index] = 0;
+
+	if (fdt_osc_read_freq(name, &frequency) != 0) {
+		ERROR("%s frequency request failed\n", name);
+		panic();
+	} else {
+		priv->osc[index] = frequency;
+	}
+}
+
+static void stm32mp1_osc_init(void)
+{
+	struct stm32mp1_clk_priv *priv = &stm32mp1_clk_priv_data;
+	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], priv, i);
+	}
+}
+
+int stm32mp1_clk_probe(void)
+{
+	struct stm32mp1_clk_priv *priv = &stm32mp1_clk_priv_data;
+
+	priv->base = fdt_rcc_read_addr();
+	if (priv->base == 0U) {
+		return -EINVAL;
+	}
+
+	priv->data = &stm32mp1_data;
+
+	if ((priv->data->gate == NULL) || (priv->data->sel == NULL) ||
+	    (priv->data->pll == NULL)) {
+		return -EINVAL;
+	}
+
+	stm32mp1_osc_init();
+
+	return 0;
+}

drivers/st/clk/stm32mp1_clkfunc.c

+/*
+ * Copyright (c) 2017-2018, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <dt-bindings/clock/stm32mp1-clksrc.h>
+#include <errno.h>
+#include <libfdt.h>
+#include <stm32mp1_clk.h>
+#include <stm32mp1_clkfunc.h>
+#include <stm32mp1_dt.h>
+
+#define DT_RCC_NODE_NAME	"rcc@50000000"
+#define DT_RCC_CLK_COMPAT	"st,stm32mp1-rcc"
+#define DT_RCC_COMPAT		"syscon"
+#define DT_STGEN_COMPAT		"st,stm32-stgen"
+#define DT_UART_COMPAT		"st,stm32h7-uart"
+#define DT_USART_COMPAT		"st,stm32h7-usart"
+
+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",
+	[_USB_PHY_48] = "ck_usbo_48m"
+};
+
+/*******************************************************************************
+ * This function reads the frequency of an oscillator from its name.
+ * It reads the value indicated inside the device tree.
+ * Returns 0 if success, and a negative value else.
+ * If success, value is stored in the second parameter.
+ ******************************************************************************/
+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) {
+			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;
+}
+
+/*******************************************************************************
+ * This function checks the presence of an oscillator property from its id.
+ * The search is done inside the device tree.
+ * Returns true/false regarding search result.
+ ******************************************************************************/
+bool fdt_osc_read_bool(enum stm32mp_osc_id osc_id, const char *prop_name)
+{
+	int node, subnode;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return false;
+	}
+
+	if (osc_id >= NB_OSC) {
+		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, stm32mp_osc_node_label[osc_id],
+			    (size_t)ret) != 0) {
+			continue;
+		}
+
+		if (fdt_getprop(fdt, subnode, prop_name, NULL) != NULL) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+/*******************************************************************************
+ * This function reads a value of a oscillator property from its id.
+ * Returns value if success, and a default value if property not found.
+ * Default value is passed as parameter.
+ ******************************************************************************/
+uint32_t fdt_osc_read_uint32_default(enum stm32mp_osc_id osc_id,
+				     const char *prop_name, uint32_t dflt_value)
+{
+	int node, subnode;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return dflt_value;
+	}
+
+	if (osc_id >= NB_OSC) {
+		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, stm32mp_osc_node_label[osc_id],
+			    (size_t)ret) != 0) {
+			continue;
+		}
+
+		return fdt_read_uint32_default(subnode, prop_name, dflt_value);
+	}
+
+	return dflt_value;
+}
+
+/*******************************************************************************
+ * This function reads the rcc base address.
+ * It reads the value indicated inside the device tree.
+ * Returns address if success, and 0 value else.
+ ******************************************************************************/
+uint32_t fdt_rcc_read_addr(void)
+{
+	int node, subnode;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return 0;
+	}
+
+	node = fdt_path_offset(fdt, "/soc");
+	if (node < 0) {
+		return 0;
+	}
+
+	fdt_for_each_subnode(subnode, fdt, node) {
+		const char *cchar;
+		int ret;
+
+		cchar = fdt_get_name(fdt, subnode, &ret);
+		if (cchar == NULL) {
+			return 0;
+		}
+
+		if (strncmp(cchar, DT_RCC_NODE_NAME, (size_t)ret) == 0) {
+			const fdt32_t *cuint;
+
+			cuint = fdt_getprop(fdt, subnode, "reg", NULL);
+			if (cuint == NULL) {
+				return 0;
+			}
+
+			return fdt32_to_cpu(*cuint);
+		}
+	}
+
+	return 0;
+}
+
+/*******************************************************************************
+ * This function reads a series of parameters in rcc-clk section.
+ * It reads the values indicated inside the device tree, from property name.
+ * The number of parameters is also indicated as entry parameter.
+ * Returns 0 if success, and a negative value else.
+ * If success, values are stored at the second parameter address.
+ ******************************************************************************/
+int fdt_rcc_read_uint32_array(const char *prop_name,
+			      uint32_t *array, uint32_t count)
+{
+	int node;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	node = fdt_node_offset_by_compatible(fdt, -1, DT_RCC_CLK_COMPAT);
+	if (node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	return fdt_read_uint32_array(node, prop_name, array, count);
+}
+
+/*******************************************************************************
+ * This function gets the subnode offset in rcc-clk section from its name.
+ * It reads the values indicated inside the device tree.
+ * Returns offset if success, and a negative value else.
+ ******************************************************************************/
+int fdt_rcc_subnode_offset(const char *name)
+{
+	int node, subnode;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	node = fdt_node_offset_by_compatible(fdt, -1, DT_RCC_CLK_COMPAT);
+	if (node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	subnode = fdt_subnode_offset(fdt, node, name);
+	if (subnode <= 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	return subnode;
+}
+
+/*******************************************************************************
+ * This function gets the pointer to a rcc-clk property from its name.
+ * It reads the values indicated inside the device tree.
+ * Length of the property is stored in the second parameter.
+ * Returns pointer if success, and NULL value else.
+ ******************************************************************************/
+const uint32_t *fdt_rcc_read_prop(const char *prop_name, int *lenp)
+{
+	const uint32_t *cuint;
+	int node, len;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return NULL;
+	}
+
+	node = fdt_node_offset_by_compatible(fdt, -1, DT_RCC_CLK_COMPAT);
+	if (node < 0) {
+		return NULL;
+	}
+
+	cuint = fdt_getprop(fdt, node, prop_name, &len);
+	if (cuint == NULL) {
+		return NULL;
+	}
+
+	*lenp = len;
+	return cuint;
+}
+
+/*******************************************************************************
+ * This function gets the secure status for rcc node.
+ * It reads secure-status in device tree.
+ * Returns 1 if rcc is available from secure world, 0 else.
+ ******************************************************************************/
+bool fdt_get_rcc_secure_status(void)
+{
+	int node;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return false;
+	}
+
+	node = fdt_node_offset_by_compatible(fdt, -1, DT_RCC_COMPAT);
+	if (node < 0) {
+		return false;
+	}
+
+	return fdt_check_secure_status(node);
+}
+
+/*******************************************************************************
+ * This function reads the stgen base address.
+ * It reads the value indicated inside the device tree.
+ * Returns address if success, and NULL value else.
+ ******************************************************************************/
+uintptr_t fdt_get_stgen_base(void)
+{
+	int node;
+	const fdt32_t *cuint;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return 0;
+	}
+
+	node = fdt_node_offset_by_compatible(fdt, -1, DT_STGEN_COMPAT);
+	if (node < 0) {
+		return 0;
+	}
+
+	cuint = fdt_getprop(fdt, node, "reg", NULL);
+	if (cuint == NULL) {
+		return 0;
+	}
+
+	return fdt32_to_cpu(*cuint);
+}
+
+/*******************************************************************************
+ * This function gets the clock ID of the given node.
+ * It reads the value indicated inside the device tree.
+ * Returns ID if success, and a negative value else.
+ ******************************************************************************/
+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);
+}

drivers/st/ddr/stm32mp1_ddr.c

+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <debug.h>
+#include <delay_timer.h>
+#include <dt-bindings/clock/stm32mp1-clks.h>
+#include <mmio.h>
+#include <platform.h>
+#include <stddef.h>
+#include <stm32mp1_clk.h>
+#include <stm32mp1_ddr.h>
+#include <stm32mp1_ddr_regs.h>
+#include <stm32mp1_dt.h>
+#include <stm32mp1_pmic.h>
+#include <stm32mp1_pwr.h>
+#include <stm32mp1_ram.h>
+#include <stm32mp1_rcc.h>
+
+struct reg_desc {
+	const char *name;
+	uint16_t offset;	/* Offset for base address */
+	uint8_t par_offset;	/* Offset for parameter array */
+};
+
+#define INVALID_OFFSET	0xFFU
+
+#define TIMESLOT_1US	(plat_get_syscnt_freq2() / 1000000U)
+
+#define DDRCTL_REG(x, y)					\
+	{							\
+		.name = #x,					\
+		.offset = offsetof(struct stm32mp1_ddrctl, x),	\
+		.par_offset = offsetof(struct y, x)		\
+	}
+
+#define DDRPHY_REG(x, y)					\
+	{							\
+		.name = #x,					\
+		.offset = offsetof(struct stm32mp1_ddrphy, x),	\
+		.par_offset = offsetof(struct y, x)		\
+	}
+
+#define DDRCTL_REG_REG(x)	DDRCTL_REG(x, stm32mp1_ddrctrl_reg)
+static const struct reg_desc ddr_reg[] = {
+	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 reg_desc ddr_timing[] = {
+	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 reg_desc ddr_map[] = {
+	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 reg_desc ddr_perf[] = {
+	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),
+	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),
+};
+
+#define DDRPHY_REG_REG(x)	DDRPHY_REG(x, stm32mp1_ddrphy_reg)
+static const struct reg_desc ddrphy_reg[] = {
+	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),
+	DDRPHY_REG_REG(dx2gcr),
+	DDRPHY_REG_REG(dx3gcr),
+};
+
+#define DDRPHY_REG_TIMING(x)	DDRPHY_REG(x, stm32mp1_ddrphy_timing)
+static const struct reg_desc ddrphy_timing[] = {
+	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),
+};
+
+#define DDRPHY_REG_CAL(x)	DDRPHY_REG(x, stm32mp1_ddrphy_cal)
+static const struct reg_desc ddrphy_cal[] = {
+	DDRPHY_REG_CAL(dx0dllcr),
+	DDRPHY_REG_CAL(dx0dqtr),
+	DDRPHY_REG_CAL(dx0dqstr),
+	DDRPHY_REG_CAL(dx1dllcr),
+	DDRPHY_REG_CAL(dx1dqtr),
+	DDRPHY_REG_CAL(dx1dqstr),
+	DDRPHY_REG_CAL(dx2dllcr),
+	DDRPHY_REG_CAL(dx2dqtr),
+	DDRPHY_REG_CAL(dx2dqstr),
+	DDRPHY_REG_CAL(dx3dllcr),
+	DDRPHY_REG_CAL(dx3dqtr),
+	DDRPHY_REG_CAL(dx3dqstr),
+};
+
+#define DDR_REG_DYN(x)						\
+	{							\
+		.name = #x,					\
+		.offset = offsetof(struct stm32mp1_ddrctl, x),	\
+		.par_offset = INVALID_OFFSET \
+	}
+
+static const struct reg_desc ddr_dyn[] = {
+	DDR_REG_DYN(stat),
+	DDR_REG_DYN(init0),
+	DDR_REG_DYN(dfimisc),
+	DDR_REG_DYN(dfistat),
+	DDR_REG_DYN(swctl),
+	DDR_REG_DYN(swstat),
+	DDR_REG_DYN(pctrl_0),
+	DDR_REG_DYN(pctrl_1),
+};
+
+#define DDRPHY_REG_DYN(x)					\
+	{							\
+		.name = #x,					\
+		.offset = offsetof(struct stm32mp1_ddrphy, x),	\
+		.par_offset = INVALID_OFFSET			\
+	}
+
+static const struct reg_desc ddrphy_dyn[] = {
+	DDRPHY_REG_DYN(pir),
+	DDRPHY_REG_DYN(pgsr),
+};
+
+enum reg_type {
+	REG_REG,
+	REG_TIMING,
+	REG_PERF,
+	REG_MAP,
+	REGPHY_REG,
+	REGPHY_TIMING,
+	REGPHY_CAL,
+/*
+ * Dynamic registers => managed in driver or not changed,
+ * can be dumped in interactive mode.
+ */
+	REG_DYN,
+	REGPHY_DYN,
+	REG_TYPE_NB
+};
+
+enum base_type {
+	DDR_BASE,
+	DDRPHY_BASE,
+	NONE_BASE
+};
+
+struct ddr_reg_info {
+	const char *name;
+	const struct reg_desc *desc;
+	uint8_t size;
+	enum base_type base;
+};
+
+static const struct ddr_reg_info ddr_registers[REG_TYPE_NB] = {
+	[REG_REG] = {
+		"static", ddr_reg, ARRAY_SIZE(ddr_reg), DDR_BASE
+	},
+	[REG_TIMING] = {
+		"timing", ddr_timing, ARRAY_SIZE(ddr_timing), DDR_BASE
+	},
+	[REG_PERF] = {
+		"perf", ddr_perf, ARRAY_SIZE(ddr_perf), DDR_BASE
+	},
+	[REG_MAP] = {
+		"map", ddr_map, ARRAY_SIZE(ddr_map), DDR_BASE
+	},
+	[REGPHY_REG] = {
+		"static", ddrphy_reg, ARRAY_SIZE(ddrphy_reg), DDRPHY_BASE
+	},
+	[REGPHY_TIMING] = {
+		"timing", ddrphy_timing, ARRAY_SIZE(ddrphy_timing), DDRPHY_BASE
+	},
+	[REGPHY_CAL] = {
+		"cal", ddrphy_cal, ARRAY_SIZE(ddrphy_cal), DDRPHY_BASE
+	},
+	[REG_DYN] = {
+		"dyn", ddr_dyn, ARRAY_SIZE(ddr_dyn), DDR_BASE
+	},
+	[REGPHY_DYN] = {
+		"dyn", ddrphy_dyn, ARRAY_SIZE(ddrphy_dyn), DDRPHY_BASE
+	},
+};
+
+static uint32_t get_base_addr(const struct ddr_info *priv, enum base_type base)
+{
+	if (base == DDRPHY_BASE) {
+		return (uint32_t)priv->phy;
+	} else {
+		return (uint32_t)priv->ctl;
+	}
+}
+
+static void set_reg(const struct ddr_info *priv,
+		    enum reg_type type,
+		    const void *param)
+{
+	unsigned int i;
+	unsigned int *ptr, value;
+	enum base_type base = ddr_registers[type].base;
+	uint32_t base_addr = get_base_addr(priv, base);
+	const struct reg_desc *desc = ddr_registers[type].desc;
+
+	VERBOSE("init %s\n", ddr_registers[type].name);
+	for (i = 0; i < ddr_registers[type].size; i++) {
+		ptr = (unsigned int *)(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 *)((uint32_t)param +
+					       desc[i].par_offset));
+			mmio_write_32((uint32_t)ptr, value);
+		}
+	}
+}
+
+static void stm32mp1_ddrphy_idone_wait(struct stm32mp1_ddrphy *phy)
+{
+	uint32_t pgsr;
+	int error = 0;
+	unsigned long start;
+	unsigned long time0, time;
+
+	start = get_timer(0);
+	time0 = start;
+
+	do {
+		pgsr = mmio_read_32((uint32_t)&phy->pgsr);
+		time = get_timer(start);
+		if (time != time0) {
+			VERBOSE("  > [0x%x] pgsr = 0x%x &\n",
+				(uint32_t)&phy->pgsr, pgsr);
+			VERBOSE("    [0x%x] pir = 0x%x (time=%x)\n",
+				(uint32_t)&phy->pir,
+				mmio_read_32((uint32_t)&phy->pir),
+				(uint32_t)time);
+		}
+
+		time0 = time;
+		if (time > plat_get_syscnt_freq2()) {
+			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%x] pgsr = 0x%x\n",
+		(uint32_t)&phy->pgsr, pgsr);
+}
+
+static void stm32mp1_ddrphy_init(struct stm32mp1_ddrphy *phy, uint32_t pir)
+{
+	uint32_t pir_init = pir | DDRPHYC_PIR_INIT;
+
+	mmio_write_32((uint32_t)&phy->pir, pir_init);
+	VERBOSE("[0x%x] pir = 0x%x -> 0x%x\n",
+		(uint32_t)&phy->pir, pir_init,
+		mmio_read_32((uint32_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);
+}
+
+/* Start quasi dynamic register update */
+static void stm32mp1_start_sw_done(struct stm32mp1_ddrctl *ctl)
+{
+	mmio_clrbits_32((uint32_t)&ctl->swctl, DDRCTRL_SWCTL_SW_DONE);
+	VERBOSE("[0x%x] swctl = 0x%x\n",
+		(uint32_t)&ctl->swctl,  mmio_read_32((uint32_t)&ctl->swctl));
+}
+
+/* Wait quasi dynamic register update */
+static void stm32mp1_wait_sw_done_ack(struct stm32mp1_ddrctl *ctl)
+{
+	unsigned long start;
+	uint32_t swstat;
+
+	mmio_setbits_32((uint32_t)&ctl->swctl, DDRCTRL_SWCTL_SW_DONE);
+	VERBOSE("[0x%x] swctl = 0x%x\n",
+		(uint32_t)&ctl->swctl, mmio_read_32((uint32_t)&ctl->swctl));
+
+	start = get_timer(0);
+	do {
+		swstat = mmio_read_32((uint32_t)&ctl->swstat);
+		VERBOSE("[0x%x] swstat = 0x%x ",
+			(uint32_t)&ctl->swstat, swstat);
+		VERBOSE("timer in ms 0x%x = start 0x%lx\r",
+			get_timer(0), start);
+		if (get_timer(start) > plat_get_syscnt_freq2()) {
+			panic();
+		}
+	} while ((swstat & DDRCTRL_SWSTAT_SW_DONE_ACK) == 0U);
+
+	VERBOSE("[0x%x] swstat = 0x%x\n",
+		(uint32_t)&ctl->swstat, swstat);
+}
+
+/* Wait quasi dynamic register update */
+static void stm32mp1_wait_operating_mode(struct ddr_info *priv, uint32_t mode)
+{
+	unsigned long start;
+	uint32_t stat;
+	uint32_t operating_mode;
+	uint32_t selref_type;
+	int break_loop = 0;
+
+	start = get_timer(0);
+	for ( ; ; ) {
+		stat = mmio_read_32((uint32_t)&priv->ctl->stat);
+		operating_mode = stat & DDRCTRL_STAT_OPERATING_MODE_MASK;
+		selref_type = stat & DDRCTRL_STAT_SELFREF_TYPE_MASK;
+		VERBOSE("[0x%x] stat = 0x%x\n",
+			(uint32_t)&priv->ctl->stat, stat);
+		VERBOSE("timer in ms 0x%x = start 0x%lx\r",
+			get_timer(0), start);
+		if (get_timer(start) > plat_get_syscnt_freq2()) {
+			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%x] stat = 0x%x\n",
+		(uint32_t)&priv->ctl->stat, stat);
+}
+
+/* Mode Register Writes (MRW or MRS) */
+static void stm32mp1_mode_register_write(struct ddr_info *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((uint32_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((uint32_t)&priv->ctl->mrctrl0, mrctrl0);
+	VERBOSE("[0x%x] mrctrl0 = 0x%x (0x%x)\n",
+		(uint32_t)&priv->ctl->mrctrl0,
+		mmio_read_32((uint32_t)&priv->ctl->mrctrl0), mrctrl0);
+	mmio_write_32((uint32_t)&priv->ctl->mrctrl1, data);
+	VERBOSE("[0x%x] mrctrl1 = 0x%x\n",
+		(uint32_t)&priv->ctl->mrctrl1,
+		mmio_read_32((uint32_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((uint32_t)&priv->ctl->mrctrl0, mrctrl0);
+
+	while ((mmio_read_32((uint32_t)&priv->ctl->mrstat) &
+	       DDRCTRL_MRSTAT_MR_WR_BUSY) != 0U) {
+		;
+	}
+
+	VERBOSE("[0x%x] mrctrl0 = 0x%x\n",
+		(uint32_t)&priv->ctl->mrctrl0, mrctrl0);
+}
+
+/* Switch DDR3 from DLL-on to DLL-off */
+static void stm32mp1_ddr3_dll_off(struct ddr_info *priv)
+{
+	uint32_t mr1 = mmio_read_32((uint32_t)&priv->phy->mr1);
+	uint32_t mr2 = mmio_read_32((uint32_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((uint32_t)&priv->ctl->dbg1, DDRCTRL_DBG1_DIS_HIF);
+	VERBOSE("[0x%x] dbg1 = 0x%x\n",
+		(uint32_t)&priv->ctl->dbg1,
+		mmio_read_32((uint32_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((uint32_t)&priv->ctl->dbgcam);
+		VERBOSE("[0x%x] dbgcam = 0x%x\n",
+			(uint32_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((uint32_t)&priv->ctl->pwrctl,
+			DDRCTRL_PWRCTL_SELFREF_SW);
+	VERBOSE("[0x%x] pwrctl = 0x%x\n",
+		(uint32_t)&priv->ctl->pwrctl,
+		mmio_read_32((uint32_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
+	 */
+	stm32mp1_start_sw_done(priv->ctl);
+
+	mmio_setbits_32((uint32_t)&priv->ctl->mstr, DDRCTRL_MSTR_DLL_OFF_MODE);
+	VERBOSE("[0x%x] mstr = 0x%x\n",
+		(uint32_t)&priv->ctl->mstr,
+		mmio_read_32((uint32_t)&priv->ctl->mstr));
+
+	stm32mp1_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 (stm32mp1_clk_get_rate(DDRPHYC) < 100000000U) {
+		mmio_clrbits_32((uint32_t)&priv->phy->dllgcr,
+				DDRPHYC_DLLGCR_BPS200);
+	} else {
+		mmio_setbits_32((uint32_t)&priv->phy->dllgcr,
+				DDRPHYC_DLLGCR_BPS200);
+	}
+
+	mmio_setbits_32((uint32_t)&priv->phy->acdllcr, DDRPHYC_ACDLLCR_DLLDIS);
+
+	mmio_setbits_32((uint32_t)&priv->phy->dx0dllcr,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+	mmio_setbits_32((uint32_t)&priv->phy->dx1dllcr,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+	mmio_setbits_32((uint32_t)&priv->phy->dx2dllcr,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+	mmio_setbits_32((uint32_t)&priv->phy->dx3dllcr,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+
+	/* 12. Exit the self-refresh state by setting PWRCTL.selfref_sw = 0. */
+	mmio_clrbits_32((uint32_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((uint32_t)&priv->ctl->dbg1, DDRCTRL_DBG1_DIS_HIF);
+	VERBOSE("[0x%x] dbg1 = 0x%x\n",
+		(uint32_t)&priv->ctl->dbg1,
+		mmio_read_32((uint32_t)&priv->ctl->dbg1));
+}
+
+static void stm32mp1_refresh_disable(struct stm32mp1_ddrctl *ctl)
+{
+	stm32mp1_start_sw_done(ctl);
+	/* Quasi-dynamic register update*/
+	mmio_setbits_32((uint32_t)&ctl->rfshctl3,
+			DDRCTRL_RFSHCTL3_DIS_AUTO_REFRESH);
+	mmio_clrbits_32((uint32_t)&ctl->pwrctl, DDRCTRL_PWRCTL_POWERDOWN_EN);
+	mmio_clrbits_32((uint32_t)&ctl->dfimisc,
+			DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+	stm32mp1_wait_sw_done_ack(ctl);
+}
+
+static void stm32mp1_refresh_restore(struct stm32mp1_ddrctl *ctl,
+				     uint32_t rfshctl3, uint32_t pwrctl)
+{
+	stm32mp1_start_sw_done(ctl);
+	if ((rfshctl3 & DDRCTRL_RFSHCTL3_DIS_AUTO_REFRESH) == 0U) {
+		mmio_clrbits_32((uint32_t)&ctl->rfshctl3,
+				DDRCTRL_RFSHCTL3_DIS_AUTO_REFRESH);
+	}
+	if ((pwrctl & DDRCTRL_PWRCTL_POWERDOWN_EN) != 0U) {
+		mmio_setbits_32((uint32_t)&ctl->pwrctl,
+				DDRCTRL_PWRCTL_POWERDOWN_EN);
+	}
+	mmio_setbits_32((uint32_t)&ctl->dfimisc,
+			DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+	stm32mp1_wait_sw_done_ack(ctl);
+}
+
+static int board_ddr_power_init(enum ddr_type ddr_type)
+{
+	if (dt_check_pmic()) {
+		return pmic_ddr_power_init(ddr_type);
+	}
+
+	return 0;
+}
+
+void stm32mp1_ddr_init(struct ddr_info *priv,
+		       struct stm32mp1_ddr_config *config)
+{
+	uint32_t pir;
+	int ret;
+
+	if ((config->c_reg.mstr & DDRCTRL_MSTR_DDR3) != 0U) {
+		ret = board_ddr_power_init(STM32MP_DDR3);
+	} else {
+		ret = board_ddr_power_init(STM32MP_LPDDR2);
+	}
+
+	if (ret != 0) {
+		panic();
+	}
+
+	VERBOSE("name = %s\n", config->info.name);
+	VERBOSE("speed = %d MHz\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((uint32_t)&priv->ctl->dfimisc,
+			DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+	VERBOSE("[0x%x] dfimisc = 0x%x\n",
+		(uint32_t)&priv->ctl->dfimisc,
+		mmio_read_32((uint32_t)&priv->ctl->dfimisc));
+
+	set_reg(priv, REG_REG, &config->c_reg);
+
+	/* 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((uint32_t)&priv->ctl->mstr,
+				DDRCTRL_MSTR_DLL_OFF_MODE);
+		VERBOSE("[0x%x] mstr = 0x%x\n",
+			(uint32_t)&priv->ctl->mstr,
+			mmio_read_32((uint32_t)&priv->ctl->mstr));
+	}
+
+	set_reg(priv, REG_TIMING, &config->c_timing);
+	set_reg(priv, REG_MAP, &config->c_map);
+
+	/* Skip CTRL init, SDRAM init is done by PHY PUBL */
+	mmio_clrsetbits_32((uint32_t)&priv->ctl->init0,
+			   DDRCTRL_INIT0_SKIP_DRAM_INIT_MASK,
+			   DDRCTRL_INIT0_SKIP_DRAM_INIT_NORMAL);
+	VERBOSE("[0x%x] init0 = 0x%x\n",
+		(uint32_t)&priv->ctl->init0,
+		mmio_read_32((uint32_t)&priv->ctl->init0));
+
+	set_reg(priv, REG_PERF, &config->c_perf);
+
+	/*  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*)
+	 */
+	set_reg(priv, REGPHY_REG, &config->p_reg);
+	set_reg(priv, REGPHY_TIMING, &config->p_timing);
+	set_reg(priv, REGPHY_CAL, &config->p_cal);
+
+	/* 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((uint32_t)&priv->phy->mr1, BIT(0));
+		VERBOSE("[0x%x] mr1 = 0x%x\n",
+			(uint32_t)&priv->phy->mr1,
+			mmio_read_32((uint32_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.
+	 */
+	stm32mp1_start_sw_done(priv->ctl);
+
+	mmio_setbits_32((uint32_t)&priv->ctl->dfimisc,
+			DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+	VERBOSE("[0x%x] dfimisc = 0x%x\n",
+		(uint32_t)&priv->ctl->dfimisc,
+		mmio_read_32((uint32_t)&priv->ctl->dfimisc));
+
+	stm32mp1_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
+	 * Warning : RVTRN  is not supported by this PUBL
+	 */
+	stm32mp1_ddrphy_init(priv->phy, DDRPHYC_PIR_QSTRN);
+
+	/* 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);
+
+	/* Enable uMCTL2 AXI port 0 */
+	mmio_setbits_32((uint32_t)&priv->ctl->pctrl_0, DDRCTRL_PCTRL_N_PORT_EN);
+	VERBOSE("[0x%x] pctrl_0 = 0x%x\n",
+		(uint32_t)&priv->ctl->pctrl_0,
+		mmio_read_32((uint32_t)&priv->ctl->pctrl_0));
+
+	/* Enable uMCTL2 AXI port 1 */
+	mmio_setbits_32((uint32_t)&priv->ctl->pctrl_1, DDRCTRL_PCTRL_N_PORT_EN);
+	VERBOSE("[0x%x] pctrl_1 = 0x%x\n",
+		(uint32_t)&priv->ctl->pctrl_1,
+		mmio_read_32((uint32_t)&priv->ctl->pctrl_1));
+}

drivers/st/ddr/stm32mp1_ddr_helpers.c

+/*
+ * Copyright (c) 2017-2018, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <mmio.h>
+#include <platform_def.h>
+#include <stm32mp1_ddr_helpers.h>
+#include <stm32mp1_rcc.h>
+
+void ddr_enable_clock(void)
+{
+	mmio_setbits_32(RCC_BASE + RCC_DDRITFCR,
+			RCC_DDRITFCR_DDRC1EN |
+			RCC_DDRITFCR_DDRC2EN |
+			RCC_DDRITFCR_DDRPHYCEN |
+			RCC_DDRITFCR_DDRPHYCAPBEN |
+			RCC_DDRITFCR_DDRCAPBEN);
+}

drivers/st/ddr/stm32mp1_ram.c

+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+ */
+
+#include <arch_helpers.h>
+#include <boot_api.h>
+#include <debug.h>
+#include <dt-bindings/clock/stm32mp1-clks.h>
+#include <errno.h>
+#include <libfdt.h>
+#include <mmio.h>
+#include <platform_def.h>
+#include <stm32mp1_clk.h>
+#include <stm32mp1_ddr.h>
+#include <stm32mp1_ddr_helpers.h>
+#include <stm32mp1_dt.h>
+#include <stm32mp1_private.h>
+#include <stm32mp1_ram.h>
+#include <stm32mp1_rcc.h>
+
+#define DDR_PATTERN	0xAAAAAAAAU
+#define DDR_ANTIPATTERN	0x55555555U
+
+static struct ddr_info ddr_priv_data;
+
+int stm32mp1_ddr_clk_enable(struct ddr_info *priv, uint16_t mem_speed)
+{
+	unsigned long ddrphy_clk, ddr_clk, mem_speed_hz;
+
+	ddr_enable_clock();
+
+	ddrphy_clk = stm32mp1_clk_get_rate(DDRPHYC);
+
+	VERBOSE("DDR: mem_speed (%d MHz), RCC %ld MHz\n",
+		mem_speed, ddrphy_clk / 1000U / 1000U);
+
+	mem_speed_hz = (uint32_t)mem_speed * 1000U * 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) {
+		ERROR("DDR expected freq %d MHz, current is %ld MHz\n",
+		      mem_speed, ddrphy_clk / 1000U / 1000U);
+		return -1;
+	}
+	return 0;
+}
+
+/*******************************************************************************
+ * 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.
+ ******************************************************************************/
+static uint32_t ddr_test_data_bus(void)
+{
+	uint32_t pattern;
+
+	for (pattern = 1U; pattern != 0U; pattern <<= 1) {
+		mmio_write_32(STM32MP1_DDR_BASE, pattern);
+
+		if (mmio_read_32(STM32MP1_DDR_BASE) != pattern) {
+			return (uint32_t)STM32MP1_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.
+ * Returns 0 if success, and address value else.
+ ******************************************************************************/
+static uint32_t ddr_test_addr_bus(void)
+{
+	uint64_t addressmask = (ddr_priv_data.info.size - 1U);
+	uint64_t offset;
+	uint64_t testoffset = 0;
+
+	/* Write the default pattern at each of the power-of-two offsets. */
+	for (offset = sizeof(uint32_t); (offset & addressmask) != 0U;
+	     offset <<= 1) {
+		mmio_write_32(STM32MP1_DDR_BASE + (uint32_t)offset,
+			      DDR_PATTERN);
+	}
+
+	/* Check for address bits stuck high. */
+	mmio_write_32(STM32MP1_DDR_BASE + (uint32_t)testoffset,
+		      DDR_ANTIPATTERN);
+
+	for (offset = sizeof(uint32_t); (offset & addressmask) != 0U;
+	     offset <<= 1) {
+		if (mmio_read_32(STM32MP1_DDR_BASE + (uint32_t)offset) !=
+		    DDR_PATTERN) {
+			return (uint32_t)(STM32MP1_DDR_BASE + offset);
+		}
+	}
+
+	mmio_write_32(STM32MP1_DDR_BASE + (uint32_t)testoffset, DDR_PATTERN);
+
+	/* Check for address bits stuck low or shorted. */
+	for (testoffset = sizeof(uint32_t); (testoffset & addressmask) != 0U;
+	     testoffset <<= 1) {
+		mmio_write_32(STM32MP1_DDR_BASE + (uint32_t)testoffset,
+			      DDR_ANTIPATTERN);
+
+		if (mmio_read_32(STM32MP1_DDR_BASE) != DDR_PATTERN) {
+			return STM32MP1_DDR_BASE;
+		}
+
+		for (offset = sizeof(uint32_t); (offset & addressmask) != 0U;
+		     offset <<= 1) {
+			if ((mmio_read_32(STM32MP1_DDR_BASE +
+					  (uint32_t)offset) != DDR_PATTERN) &&
+			    (offset != testoffset)) {
+				return (uint32_t)(STM32MP1_DDR_BASE + offset);
+			}
+		}
+
+		mmio_write_32(STM32MP1_DDR_BASE + (uint32_t)testoffset,
+			      DDR_PATTERN);
+	}
+
+	return 0;
+}
+
+/*******************************************************************************
+ * 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.
+ ******************************************************************************/
+static uint32_t ddr_check_size(void)
+{
+	uint32_t offset = sizeof(uint32_t);
+
+	mmio_write_32(STM32MP1_DDR_BASE, DDR_PATTERN);
+
+	while (offset < STM32MP1_DDR_MAX_SIZE) {
+		mmio_write_32(STM32MP1_DDR_BASE + offset, DDR_ANTIPATTERN);
+		dsb();
+
+		if (mmio_read_32(STM32MP1_DDR_BASE) != DDR_PATTERN) {
+			break;
+		}
+
+		offset <<= 1;
+	}
+
+	INFO("Memory size = 0x%x (%d MB)\n", offset, offset / (1024U * 1024U));
+
+	return offset;
+}
+
+static int stm32mp1_ddr_setup(void)
+{
+	struct ddr_info *priv = &ddr_priv_data;
+	int ret;
+	struct stm32mp1_ddr_config config;
+	int node, len;
+	uint32_t tamp_clk_off = 0, uret, idx;
+	void *fdt;
+
+#define PARAM(x, y)							\
+	{								\
+		.name = x,						\
+		.offset = offsetof(struct stm32mp1_ddr_config, y),	\
+		.size = sizeof(config.y) / sizeof(uint32_t)		\
+	}
+
+#define CTL_PARAM(x) PARAM("st,ctl-"#x, c_##x)
+#define PHY_PARAM(x) PARAM("st,phy-"#x, p_##x)
+
+	const struct {
+		const char *name; /* Name in DT */
+		const uint32_t offset; /* Offset in config struct */
+		const uint32_t size;   /* Size of parameters */
+	} param[] = {
+		CTL_PARAM(reg),
+		CTL_PARAM(timing),
+		CTL_PARAM(map),
+		CTL_PARAM(perf),
+		PHY_PARAM(reg),
+		PHY_PARAM(timing),
+		PHY_PARAM(cal)
+	};
+
+	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;
+	}
+
+	config.info.speed =
+		(uint16_t)fdt_read_uint32_default(node, "st,mem-speed",
+						  STM32MP1_DDR_SPEED_DFLT);
+	config.info.size = fdt_read_uint32_default(node, "st,mem-size",
+						   STM32MP1_DDR_SIZE_DFLT);
+	config.info.name = fdt_getprop(fdt, node, "st,mem-name", &len);
+	if (config.info.name == NULL) {
+		VERBOSE("%s: no st,mem-name\n", __func__);
+		return -EINVAL;
+	}
+	INFO("RAM: %s\n", config.info.name);
+
+	for (idx = 0; idx < ARRAY_SIZE(param); idx++) {
+		ret = fdt_read_uint32_array(node, param[idx].name,
+					    (void *)((uint32_t)&config +
+						     param[idx].offset),
+					    param[idx].size);
+
+		VERBOSE("%s: %s[0x%x] = %d\n", __func__,
+			param[idx].name, param[idx].size, ret);
+		if (ret != 0) {
+			ERROR("%s: Cannot read %s\n",
+			      __func__, param[idx].name);
+			return -EINVAL;
+		}
+	}
+
+	if (!stm32mp1_clk_is_enabled(RTCAPB)) {
+		tamp_clk_off = 1;
+		if (stm32mp1_clk_enable(RTCAPB) != 0) {
+			return -EINVAL;
+		}
+	}
+
+	if (tamp_clk_off != 0U) {
+		if (stm32mp1_clk_disable(RTCAPB) != 0) {
+			return -EINVAL;
+		}
+	}
+
+	/* 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);
+
+	dcsw_op_all(DC_OP_CISW);
+	write_sctlr(read_sctlr() & ~SCTLR_C_BIT);
+
+	uret = ddr_test_data_bus();
+	if (uret != 0U) {
+		ERROR("DDR data bus test: can't access memory @ 0x%x\n",
+		      uret);
+		panic();
+	}
+
+	uret = ddr_test_addr_bus();
+	if (uret != 0U) {
+		ERROR("DDR addr bus test: can't access memory @ 0x%x\n",
+		      uret);
+		panic();
+	}
+
+	uret = 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();
+	}
+
+	write_sctlr(read_sctlr() | SCTLR_C_BIT);
+
+	return 0;
+}
+
+int stm32mp1_ddr_probe(void)
+{
+	struct ddr_info *priv = &ddr_priv_data;
+
+	VERBOSE("STM32MP DDR probe\n");
+
+	priv->ctl = (struct stm32mp1_ddrctl *)DDRCTRL_BASE;
+	priv->phy = (struct stm32mp1_ddrphy *)DDRPHYC_BASE;
+	priv->pwr = PWR_BASE;
+	priv->rcc = RCC_BASE;
+
+	priv->info.base = STM32MP1_DDR_BASE;
+	priv->info.size = 0;
+
+	return stm32mp1_ddr_setup();
+}

drivers/st/gpio/stm32_gpio.c

+/*
+ * Copyright (c) 2016-2018, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <bl_common.h>
+#include <debug.h>
+#include <mmio.h>
+#include <stdbool.h>
+#include <stm32_gpio.h>
+
+static bool check_gpio(uint32_t bank, uint32_t pin)
+{
+	if (pin > GPIO_PIN_MAX) {
+		ERROR("%s: wrong pin number (%d)\n", __func__, pin);
+		return false;
+	}
+
+	if ((bank > GPIO_BANK_K) && (bank != GPIO_BANK_Z)) {
+		ERROR("%s: wrong GPIO bank number (%d)\n", __func__, bank);
+		return false;
+	}
+
+	return true;
+}
+
+void set_gpio(uint32_t bank, uint32_t pin, uint32_t mode, uint32_t speed,
+	      uint32_t pull, uint32_t alternate)
+{
+	volatile uint32_t bank_address;
+
+	if (!check_gpio(bank, pin)) {
+		return;
+	}
+
+	if (bank == GPIO_BANK_Z) {
+		bank_address = STM32_GPIOZ_BANK;
+	} else {
+		bank_address = STM32_GPIOA_BANK +
+			(bank * STM32_GPIO_BANK_OFFSET);
+	}
+
+	mmio_clrbits_32(bank_address + GPIO_MODE_OFFSET,
+			((uint32_t)GPIO_MODE_MASK << (pin << 1)));
+	mmio_setbits_32(bank_address + GPIO_MODE_OFFSET,
+			(mode & ~GPIO_OPEN_DRAIN) << (pin << 1));
+
+	if ((mode & GPIO_OPEN_DRAIN) != 0U) {
+		mmio_setbits_32(bank_address + GPIO_TYPE_OFFSET,
+				BIT(pin));
+	}
+
+	mmio_clrbits_32(bank_address + GPIO_SPEED_OFFSET,
+			((uint32_t)GPIO_SPEED_MASK << (pin << 1)));
+	mmio_setbits_32(bank_address + GPIO_SPEED_OFFSET, speed << (pin << 1));
+
+	mmio_clrbits_32(bank_address + GPIO_PUPD_OFFSET,
+			((uint32_t)GPIO_PULL_MASK << (pin << 1)));
+	mmio_setbits_32(bank_address + GPIO_PUPD_OFFSET, pull << (pin << 1));
+
+	if (pin < GPIO_ALT_LOWER_LIMIT) {
+		mmio_clrbits_32(bank_address + GPIO_AFRL_OFFSET,
+				((uint32_t)GPIO_ALTERNATE_MASK << (pin << 2)));
+		mmio_setbits_32(bank_address + GPIO_AFRL_OFFSET,
+				alternate << (pin << 2));
+	} else {
+		mmio_clrbits_32(bank_address + GPIO_AFRH_OFFSET,
+				((uint32_t)GPIO_ALTERNATE_MASK <<
+				 ((pin - GPIO_ALT_LOWER_LIMIT) << 2)));
+		mmio_setbits_32(bank_address + GPIO_AFRH_OFFSET,
+				alternate << ((pin - GPIO_ALT_LOWER_LIMIT) <<
+					      2));
+	}
+
+	VERBOSE("GPIO %u mode set to 0x%x\n", bank,
+		mmio_read_32(bank_address + GPIO_MODE_OFFSET));
+	VERBOSE("GPIO %u speed set to 0x%x\n", bank,
+		mmio_read_32(bank_address + GPIO_SPEED_OFFSET));
+	VERBOSE("GPIO %u mode pull to 0x%x\n", bank,
+		mmio_read_32(bank_address + GPIO_PUPD_OFFSET));
+	VERBOSE("GPIO %u mode alternate low to 0x%x\n", bank,
+		mmio_read_32(bank_address + GPIO_AFRL_OFFSET));
+	VERBOSE("GPIO %u mode alternate high to 0x%x\n", bank,
+		mmio_read_32(bank_address + GPIO_AFRH_OFFSET));
+}

drivers/st/pmic/stm32_i2c.c

+/*
+ * Copyright (c) 2016-2018, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch_helpers.h>
+#include <delay_timer.h>
+#include <errno.h>
+#include <mmio.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <stm32_i2c.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 MAX_DELAY		0xFFFFFFFFU
+
+/* I2C TIMING clear register Mask */
+#define TIMING_CLEAR_MASK	0xF0FFFFFFU
+/* Timeout 25 ms */
+#define I2C_TIMEOUT_BUSY	25U
+
+#define MAX_NBYTE_SIZE		255U
+
+static int i2c_request_memory_write(struct i2c_handle_s *hi2c,
+				    uint16_t dev_addr, uint16_t mem_addr,
+				    uint16_t mem_add_size, uint32_t timeout,
+				    uint32_t tick_start);
+static int i2c_request_memory_read(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+				   uint16_t mem_addr, uint16_t mem_add_size,
+				   uint32_t timeout, uint32_t tick_start);
+
+/* Private functions to handle flags during polling transfer */
+static int i2c_wait_flag(struct i2c_handle_s *hi2c, uint32_t flag,
+			 uint8_t awaited_value, uint32_t timeout,
+			 uint32_t tick_start);
+static int i2c_wait_txis(struct i2c_handle_s *hi2c, uint32_t timeout,
+			 uint32_t tick_start);
+static int i2c_wait_stop(struct i2c_handle_s *hi2c, uint32_t timeout,
+			 uint32_t tick_start);
+static int i2c_ack_failed(struct i2c_handle_s *hi2c, uint32_t timeout,
+			  uint32_t tick_start);
+
+/* Private function to flush TXDR register */
+static void i2c_flush_txdr(struct i2c_handle_s *hi2c);
+
+/* Private function to start, restart or stop a transfer */
+static void i2c_transfer_config(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+				uint16_t size, uint32_t i2c_mode,
+				uint32_t request);
+
+/*
+ * @brief  Initialize the I2C device.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @retval 0 if OK, negative value else
+ */
+int stm32_i2c_init(struct i2c_handle_s *hi2c)
+{
+	if (hi2c == NULL) {
+		return -ENOENT;
+	}
+
+	if (hi2c->i2c_state == I2C_STATE_RESET) {
+		hi2c->lock = 0;
+	}
+
+	hi2c->i2c_state = I2C_STATE_BUSY;
+
+	/* 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,
+		      hi2c->i2c_init.timing & TIMING_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 (hi2c->i2c_init.addressing_mode == I2C_ADDRESSINGMODE_7BIT) {
+		mmio_write_32(hi2c->i2c_base_addr + I2C_OAR1,
+			      I2C_OAR1_OA1EN | hi2c->i2c_init.own_address1);
+	} else { /* I2C_ADDRESSINGMODE_10BIT */
+		mmio_write_32(hi2c->i2c_base_addr + I2C_OAR1,
+			      I2C_OAR1_OA1EN | I2C_OAR1_OA1MODE |
+			      hi2c->i2c_init.own_address1);
+	}
+
+	/* Configure I2Cx: Addressing Master mode */
+	if (hi2c->i2c_init.addressing_mode == I2C_ADDRESSINGMODE_10BIT) {
+		mmio_write_32(hi2c->i2c_base_addr + I2C_CR2, I2C_CR2_ADD10);
+	}
+
+	/*
+	 * Enable the AUTOEND by default, and enable NACK
+	 * (should be disable 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,
+		      hi2c->i2c_init.dual_address_mode |
+		      hi2c->i2c_init.own_address2 |
+		      (hi2c->i2c_init.own_address2_masks << 8));
+
+	/* Configure I2Cx: Generalcall and NoStretch mode */
+	mmio_write_32(hi2c->i2c_base_addr + I2C_CR1,
+		      hi2c->i2c_init.general_call_mode |
+		      hi2c->i2c_init.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;
+
+	return 0;
+}
+
+/*
+ * @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: timeout duration
+ * @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)
+{
+	uint32_t tickstart;
+
+	if ((hi2c->i2c_state != I2C_STATE_READY) || (hi2c->lock != 0U)) {
+		return -EBUSY;
+	}
+
+	if ((p_data == NULL) || (size == 0U)) {
+		return -EINVAL;
+	}
+
+	hi2c->lock = 1;
+
+	tickstart = (uint32_t)read_cntpct_el0();
+
+	if (i2c_wait_flag(hi2c, I2C_FLAG_BUSY, 1, I2C_TIMEOUT_BUSY,
+			  tickstart) != 0) {
+		return -EIO;
+	}
+
+	hi2c->i2c_state     = I2C_STATE_BUSY_TX;
+	hi2c->i2c_mode      = I2C_MODE_MEM;
+	hi2c->i2c_err = I2C_ERROR_NONE;
+
+	hi2c->p_buff  = p_data;
+	hi2c->xfer_count = size;
+
+	/* Send Slave Address and Memory Address */
+	if (i2c_request_memory_write(hi2c, dev_addr, mem_addr, mem_add_size,
+				     timeout, tickstart) != 0) {
+		hi2c->lock = 0;
+		return -EIO;
+	}
+
+	/*
+	 * Set NBYTES to write and reload
+	 * if hi2c->xfer_count > MAX_NBYTE_SIZE
+	 */
+	if (hi2c->xfer_count > MAX_NBYTE_SIZE) {
+		hi2c->xfer_size = MAX_NBYTE_SIZE;
+		i2c_transfer_config(hi2c, dev_addr, hi2c->xfer_size,
+				    I2C_RELOAD_MODE, I2C_NO_STARTSTOP);
+	} else {
+		hi2c->xfer_size = hi2c->xfer_count;
+		i2c_transfer_config(hi2c, dev_addr, hi2c->xfer_size,
+				    I2C_AUTOEND_MODE, I2C_NO_STARTSTOP);
+	}
+
+	do {
+		if (i2c_wait_txis(hi2c, timeout, tickstart) != 0) {
+			return -EIO;
+		}
+
+		mmio_write_8(hi2c->i2c_base_addr + I2C_TXDR, *hi2c->p_buff);
+		hi2c->p_buff++;
+		hi2c->xfer_count--;
+		hi2c->xfer_size--;
+
+		if ((hi2c->xfer_count != 0U) && (hi2c->xfer_size == 0U)) {
+			/* Wait until TCR flag is set */
+			if (i2c_wait_flag(hi2c, I2C_FLAG_TCR, 0, timeout,
+					  tickstart) != 0) {
+				return -EIO;
+		}
+
+			if (hi2c->xfer_count > MAX_NBYTE_SIZE) {
+				hi2c->xfer_size = MAX_NBYTE_SIZE;
+				i2c_transfer_config(hi2c, dev_addr,
+						    hi2c->xfer_size,
+						    I2C_RELOAD_MODE,
+						    I2C_NO_STARTSTOP);
+			} else {
+				hi2c->xfer_size = hi2c->xfer_count;
+				i2c_transfer_config(hi2c, dev_addr,
+						    hi2c->xfer_size,
+						    I2C_AUTOEND_MODE,
+						    I2C_NO_STARTSTOP);
+			}
+		}
+
+	} while (hi2c->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, tickstart) != 0) {
+		return -EIO;
+	}
+
+	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;
+
+	hi2c->lock = 0;
+
+	return 0;
+}
+
+/*
+ * @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: timeout duration
+ * @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)
+{
+	uint32_t tickstart;
+
+	if ((hi2c->i2c_state != I2C_STATE_READY) || (hi2c->lock != 0U)) {
+		return -EBUSY;
+	}
+
+	if ((p_data == NULL) || (size == 0U)) {
+		return  -EINVAL;
+	}
+
+	hi2c->lock = 1;
+
+	tickstart = (uint32_t)read_cntpct_el0();
+
+	if (i2c_wait_flag(hi2c, I2C_FLAG_BUSY, 1, I2C_TIMEOUT_BUSY,
+			  tickstart) != 0) {
+		return -EIO;
+	}
+
+	hi2c->i2c_state     = I2C_STATE_BUSY_RX;
+	hi2c->i2c_mode      = I2C_MODE_MEM;
+	hi2c->i2c_err = I2C_ERROR_NONE;
+
+	hi2c->p_buff  = p_data;
+	hi2c->xfer_count = size;
+
+	/* Send Slave Address and Memory Address */
+	if (i2c_request_memory_read(hi2c, dev_addr, mem_addr, mem_add_size,
+				    timeout, tickstart) != 0) {
+		hi2c->lock = 0;
+		return -EIO;
+	}
+
+	/*
+	 * Send Slave Address.
+	 * Set NBYTES to write and reload if hi2c->xfer_count > MAX_NBYTE_SIZE
+	 * and generate RESTART.
+	 */
+	if (hi2c->xfer_count > MAX_NBYTE_SIZE) {
+		hi2c->xfer_size = MAX_NBYTE_SIZE;
+		i2c_transfer_config(hi2c, dev_addr, hi2c->xfer_size,
+				    I2C_RELOAD_MODE, I2C_GENERATE_START_READ);
+	} else {
+		hi2c->xfer_size = hi2c->xfer_count;
+		i2c_transfer_config(hi2c, dev_addr, hi2c->xfer_size,
+				    I2C_AUTOEND_MODE, I2C_GENERATE_START_READ);
+	}
+
+	do {
+		if (i2c_wait_flag(hi2c, I2C_FLAG_RXNE, 0, timeout,
+				  tickstart) != 0) {
+			return -EIO;
+		}
+
+		*hi2c->p_buff = mmio_read_8(hi2c->i2c_base_addr + I2C_RXDR);
+		hi2c->p_buff++;
+		hi2c->xfer_size--;
+		hi2c->xfer_count--;
+
+		if ((hi2c->xfer_count != 0U) && (hi2c->xfer_size == 0U)) {
+			if (i2c_wait_flag(hi2c, I2C_FLAG_TCR, 0, timeout,
+					  tickstart) != 0) {
+				return -EIO;
+			}
+
+			if (hi2c->xfer_count > MAX_NBYTE_SIZE) {
+				hi2c->xfer_size = MAX_NBYTE_SIZE;
+				i2c_transfer_config(hi2c, dev_addr,
+						    hi2c->xfer_size,
+						    I2C_RELOAD_MODE,
+						    I2C_NO_STARTSTOP);
+			} else {
+				hi2c->xfer_size = hi2c->xfer_count;
+				i2c_transfer_config(hi2c, dev_addr,
+						    hi2c->xfer_size,
+						    I2C_AUTOEND_MODE,
+						    I2C_NO_STARTSTOP);
+			}
+		}
+	} while (hi2c->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, tickstart) != 0) {
+		return -EIO;
+	}
+
+	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;
+
+	hi2c->lock = 0;
+
+	return 0;
+}
+
+/*
+ * @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: timeout duration
+ * @retval 0 if OK, negative value else
+ */
+int stm32_i2c_is_device_ready(struct i2c_handle_s *hi2c,
+			      uint16_t dev_addr, uint32_t trials,
+			      uint32_t timeout)
+{
+	uint32_t i2c_trials = 0U;
+
+	if ((hi2c->i2c_state != I2C_STATE_READY) || (hi2c->lock != 0U)) {
+		return -EBUSY;
+	}
+
+	if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) & I2C_FLAG_BUSY) !=
+	    0U) {
+		return -EBUSY;
+	}
+
+	hi2c->lock = 1;
+
+	hi2c->i2c_state = I2C_STATE_BUSY;
+	hi2c->i2c_err = I2C_ERROR_NONE;
+
+	do {
+		uint32_t tickstart;
+
+		/* Generate Start */
+		if (hi2c->i2c_init.addressing_mode == I2C_ADDRESSINGMODE_7BIT) {
+			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
+		 */
+		tickstart = (uint32_t)read_cntpct_el0();
+		while (((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
+			 (I2C_FLAG_STOPF | I2C_FLAG_AF)) == 0U) &&
+		       (hi2c->i2c_state != I2C_STATE_TIMEOUT)) {
+			if (timeout != MAX_DELAY) {
+				if ((((uint32_t)read_cntpct_el0() - tickstart) >
+				     timeout) || (timeout == 0U)) {
+					hi2c->i2c_state = I2C_STATE_READY;
+
+					hi2c->i2c_err |=
+						I2C_ERROR_TIMEOUT;
+
+					hi2c->lock = 0;
+
+					return -EIO;
+				}
+			}
+		}
+
+		/* Check if the NACKF flag has not been set */
+		if ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
+		     I2C_FLAG_AF) == 0U) {
+			if (i2c_wait_flag(hi2c, I2C_FLAG_STOPF, 0, timeout,
+					  tickstart) != 0) {
+				return -EIO;
+			}
+
+			mmio_write_32(hi2c->i2c_base_addr + I2C_ICR,
+				      I2C_FLAG_STOPF);
+
+			hi2c->i2c_state = I2C_STATE_READY;
+
+			hi2c->lock = 0;
+
+			return 0;
+		}
+
+		if (i2c_wait_flag(hi2c, I2C_FLAG_STOPF, 0, timeout,
+				  tickstart) != 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);
+
+		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,
+					  tickstart) != 0) {
+				return -EIO;
+			}
+
+			mmio_write_32(hi2c->i2c_base_addr + I2C_ICR,
+				      I2C_FLAG_STOPF);
+		}
+
+		i2c_trials++;
+	} while (i2c_trials < trials);
+
+	hi2c->i2c_state = I2C_STATE_READY;
+
+	hi2c->i2c_err |= I2C_ERROR_TIMEOUT;
+
+	hi2c->lock = 0;
+
+	return -EIO;
+}
+
+/*
+ * @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: timeout duration
+ * @param  tick_start Tick start value
+ * @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, uint32_t timeout,
+				    uint32_t tick_start)
+{
+	i2c_transfer_config(hi2c, dev_addr, mem_add_size, I2C_RELOAD_MODE,
+			    I2C_GENERATE_START_WRITE);
+
+	if (i2c_wait_txis(hi2c, timeout, tick_start) != 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));
+
+		/* Wait until TXIS flag is set */
+		if (i2c_wait_txis(hi2c, timeout, tick_start) != 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, tick_start) !=
+	    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: timeout duration
+ * @param  tick_start Tick start value
+ * @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,
+				   uint32_t timeout, uint32_t tick_start)
+{
+	i2c_transfer_config(hi2c, dev_addr, mem_add_size, I2C_SOFTEND_MODE,
+			    I2C_GENERATE_START_WRITE);
+
+	if (i2c_wait_txis(hi2c, timeout, tick_start) != 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));
+
+		/* Wait until TXIS flag is set */
+		if (i2c_wait_txis(hi2c, timeout, tick_start) != 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, tick_start) != 0) {
+		return -EIO;
+	}
+
+	return 0;
+}
+
+/*
+ * @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: timeout duration
+ * @param  tick_start: Tick start value
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_wait_flag(struct i2c_handle_s *hi2c, uint32_t flag,
+			 uint8_t awaited_value, uint32_t timeout,
+			 uint32_t tick_start)
+{
+	uint8_t flag_check;
+
+	do {
+		flag_check = ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
+			       flag) == flag) ? 1U : 0U;
+
+		if (timeout != MAX_DELAY) {
+			if ((((uint32_t)read_cntpct_el0() - tick_start) >
+			     timeout) || (timeout == 0U)) {
+				hi2c->i2c_err |= I2C_ERROR_TIMEOUT;
+				hi2c->i2c_state = I2C_STATE_READY;
+				hi2c->i2c_mode = I2C_MODE_NONE;
+
+				hi2c->lock = 0;
+				return -EIO;
+			}
+		}
+	} while (flag_check == awaited_value);
+
+	return 0;
+}
+
+/*
+ * @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: timeout duration
+ * @param  tick_start: Tick start value
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_wait_txis(struct i2c_handle_s *hi2c, uint32_t timeout,
+			 uint32_t tick_start)
+{
+	while ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
+		I2C_FLAG_TXIS) == 0U) {
+		if (i2c_ack_failed(hi2c, timeout, tick_start) != 0) {
+			return -EIO;
+		}
+
+		if (timeout != MAX_DELAY) {
+			if ((((uint32_t)read_cntpct_el0() - tick_start) >
+			     timeout) || (timeout == 0U)) {
+				hi2c->i2c_err |= I2C_ERROR_TIMEOUT;
+				hi2c->i2c_state = I2C_STATE_READY;
+				hi2c->i2c_mode = I2C_MODE_NONE;
+
+				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: timeout duration
+ * @param  tick_start: Tick start value
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_wait_stop(struct i2c_handle_s *hi2c, uint32_t timeout,
+			 uint32_t tick_start)
+{
+	while ((mmio_read_32(hi2c->i2c_base_addr + I2C_ISR) &
+		 I2C_FLAG_STOPF) == 0U) {
+		if (i2c_ack_failed(hi2c, timeout, tick_start) != 0) {
+			return -EIO;
+		}
+
+		if ((((uint32_t)read_cntpct_el0() - tick_start) > timeout) ||
+		    (timeout == 0U)) {
+			hi2c->i2c_err |= I2C_ERROR_TIMEOUT;
+			hi2c->i2c_state = I2C_STATE_READY;
+			hi2c->i2c_mode = I2C_MODE_NONE;
+
+			hi2c->lock = 0;
+
+			return -EIO;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * @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: timeout duration
+ * @param  tick_start: Tick start value
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_ack_failed(struct i2c_handle_s *hi2c, uint32_t timeout,
+			  uint32_t tick_start)
+{
+	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 != MAX_DELAY) {
+			if ((((uint32_t)read_cntpct_el0() - tick_start) >
+			     timeout) || (timeout == 0U)) {
+				hi2c->i2c_err |= I2C_ERROR_TIMEOUT;
+				hi2c->i2c_state = I2C_STATE_READY;
+				hi2c->i2c_mode = I2C_MODE_NONE;
+
+				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  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  Configure I2C Analog noise filter.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2Cx peripheral
+ * @param  analog_filter: New state of the Analog filter.
+ * @retval 0 if OK, negative value else
+ */
+int stm32_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;
+}

drivers/st/pmic/stm32mp1_pmic.c

+/*
+ * Copyright (c) 2017-2018, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <debug.h>
+#include <delay_timer.h>
+#include <errno.h>
+#include <libfdt.h>
+#include <mmio.h>
+#include <mmio.h>
+#include <platform_def.h>
+#include <stdbool.h>
+#include <stm32_gpio.h>
+#include <stm32mp1_clk.h>
+#include <stm32mp1_dt.h>
+#include <stm32mp1_pmic.h>
+#include <stpmu1.h>
+#include <utils_def.h>
+
+/* I2C Timing hard-coded value, for I2C clock source is HSI at 64MHz */
+#define I2C_TIMING			0x10D07DB5
+
+#define I2C_TIMEOUT			0xFFFFF
+
+#define MASK_RESET_BUCK3		BIT(2)
+
+#define STPMU1_LDO12356_OUTPUT_MASK	(uint8_t)(GENMASK(6, 2))
+#define STPMU1_LDO12356_OUTPUT_SHIFT	2
+#define STPMU1_LDO3_MODE		(uint8_t)(BIT(7))
+#define STPMU1_LDO3_DDR_SEL		31U
+#define STPMU1_LDO3_1800000		(9U << STPMU1_LDO12356_OUTPUT_SHIFT)
+
+#define STPMU1_BUCK_OUTPUT_SHIFT	2
+#define STPMU1_BUCK3_1V8		(39U << STPMU1_BUCK_OUTPUT_SHIFT)
+
+#define STPMU1_DEFAULT_START_UP_DELAY_MS	1
+
+static struct i2c_handle_s i2c_handle;
+static uint32_t pmic_i2c_addr;
+
+static int dt_get_pmic_node(void *fdt)
+{
+	return fdt_node_offset_by_compatible(fdt, -1, "st,stpmu1");
+}
+
+bool dt_check_pmic(void)
+{
+	int node;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return false;
+	}
+
+	node = dt_get_pmic_node(fdt);
+	if (node < 0) {
+		VERBOSE("%s: No PMIC node found in DT\n", __func__);
+		return false;
+	}
+
+	return fdt_check_status(node);
+}
+
+static int dt_pmic_i2c_config(struct dt_node_info *i2c_info)
+{
+	int pmic_node, i2c_node;
+	void *fdt;
+	const fdt32_t *cuint;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	pmic_node = dt_get_pmic_node(fdt);
+	if (pmic_node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	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 -EINVAL;
+	}
+
+	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 dt_set_pinctrl_config(i2c_node);
+}
+
+int dt_pmic_enable_boot_on_regulators(void)
+{
+	int pmic_node, regulators_node, regulator_node;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	pmic_node = dt_get_pmic_node(fdt);
+	if (pmic_node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	regulators_node = fdt_subnode_offset(fdt, pmic_node, "regulators");
+
+	fdt_for_each_subnode(regulator_node, fdt, regulators_node) {
+		const fdt32_t *cuint;
+		const char *node_name;
+		uint16_t voltage;
+
+		if (fdt_getprop(fdt, regulator_node, "regulator-boot-on",
+				NULL) == NULL) {
+			continue;
+		}
+
+		cuint = fdt_getprop(fdt, regulator_node,
+				    "regulator-min-microvolt", NULL);
+		if (cuint == NULL) {
+			continue;
+		}
+
+		/* DT uses microvolts, whereas driver awaits millivolts */
+		voltage = (uint16_t)(fdt32_to_cpu(*cuint) / 1000U);
+		node_name = fdt_get_name(fdt, regulator_node, NULL);
+
+		if (stpmu1_is_regulator_enabled(node_name) == 0U) {
+			int status;
+
+			status = stpmu1_regulator_voltage_set(node_name,
+							      voltage);
+			if (status != 0) {
+				return status;
+			}
+
+			status = stpmu1_regulator_enable(node_name);
+			if (status != 0) {
+				return status;
+			}
+		}
+	}
+
+	return 0;
+}
+
+void initialize_pmic_i2c(void)
+{
+	int ret;
+	struct dt_node_info i2c_info;
+
+	if (dt_pmic_i2c_config(&i2c_info) != 0) {
+		ERROR("I2C configuration failed\n");
+		panic();
+	}
+
+	if (stm32mp1_clk_enable((uint32_t)i2c_info.clock) < 0) {
+		ERROR("I2C clock enable failed\n");
+		panic();
+	}
+
+	/* Initialize PMIC I2C */
+	i2c_handle.i2c_base_addr		= i2c_info.base;
+	i2c_handle.i2c_init.timing		= I2C_TIMING;
+	i2c_handle.i2c_init.own_address1	= pmic_i2c_addr;
+	i2c_handle.i2c_init.addressing_mode	= I2C_ADDRESSINGMODE_7BIT;
+	i2c_handle.i2c_init.dual_address_mode	= I2C_DUALADDRESS_DISABLE;
+	i2c_handle.i2c_init.own_address2	= 0;
+	i2c_handle.i2c_init.own_address2_masks	= I2C_OAR2_OA2NOMASK;
+	i2c_handle.i2c_init.general_call_mode	= I2C_GENERALCALL_DISABLE;
+	i2c_handle.i2c_init.no_stretch_mode	= I2C_NOSTRETCH_DISABLE;
+
+	ret = stm32_i2c_init(&i2c_handle);
+	if (ret != 0) {
+		ERROR("Cannot initialize I2C %x (%d)\n",
+		      i2c_handle.i2c_base_addr, ret);
+		panic();
+	}
+
+	ret = stm32_i2c_config_analog_filter(&i2c_handle,
+					     I2C_ANALOGFILTER_ENABLE);
+	if (ret != 0) {
+		ERROR("Cannot initialize I2C analog filter (%d)\n", ret);
+		panic();
+	}
+
+	ret = stm32_i2c_is_device_ready(&i2c_handle, (uint16_t)pmic_i2c_addr, 1,
+					I2C_TIMEOUT);
+	if (ret != 0) {
+		ERROR("I2C device not ready (%d)\n", ret);
+		panic();
+	}
+
+	stpmu1_bind_i2c(&i2c_handle, (uint16_t)pmic_i2c_addr);
+}
+
+void initialize_pmic(void)
+{
+	int status;
+	uint8_t read_val;
+
+	initialize_pmic_i2c();
+
+	status = stpmu1_register_read(VERSION_STATUS_REG, &read_val);
+	if (status != 0) {
+		panic();
+	}
+
+	INFO("PMIC version = 0x%x\n", read_val);
+
+	/* Keep VDD on during the reset cycle */
+	status = stpmu1_register_update(MASK_RESET_BUCK_REG,
+					MASK_RESET_BUCK3,
+					MASK_RESET_BUCK3);
+	if (status != 0) {
+		panic();
+	}
+}
+
+int pmic_ddr_power_init(enum ddr_type ddr_type)
+{
+	bool buck3_at_1v8 = false;
+	uint8_t read_val;
+	int status;
+
+	switch (ddr_type) {
+	case STM32MP_DDR3:
+		/* Set LDO3 to sync mode */
+		status = stpmu1_register_read(LDO3_CONTROL_REG, &read_val);
+		if (status != 0) {
+			return status;
+		}
+
+		read_val &= ~STPMU1_LDO3_MODE;
+		read_val &= ~STPMU1_LDO12356_OUTPUT_MASK;
+		read_val |= STPMU1_LDO3_DDR_SEL << STPMU1_LDO12356_OUTPUT_SHIFT;
+
+		status = stpmu1_register_write(LDO3_CONTROL_REG, read_val);
+		if (status != 0) {
+			return status;
+		}
+
+		status = stpmu1_regulator_voltage_set("buck2", 1350);
+		if (status != 0) {
+			return status;
+		}
+
+		status = stpmu1_regulator_enable("buck2");
+		if (status != 0) {
+			return status;
+		}
+
+		mdelay(STPMU1_DEFAULT_START_UP_DELAY_MS);
+
+		status = stpmu1_regulator_enable("vref_ddr");
+		if (status != 0) {
+			return status;
+		}
+
+		mdelay(STPMU1_DEFAULT_START_UP_DELAY_MS);
+
+		status = stpmu1_regulator_enable("ldo3");
+		if (status != 0) {
+			return status;
+		}
+
+		mdelay(STPMU1_DEFAULT_START_UP_DELAY_MS);
+		break;
+
+	case STM32MP_LPDDR2:
+		/*
+		 * Set LDO3 to 1.8V
+		 * Set LDO3 to bypass mode if BUCK3 = 1.8V
+		 * Set LDO3 to normal mode if BUCK3 != 1.8V
+		 */
+		status = stpmu1_register_read(BUCK3_CONTROL_REG, &read_val);
+		if (status != 0) {
+			return status;
+		}
+
+		if ((read_val & STPMU1_BUCK3_1V8) == STPMU1_BUCK3_1V8) {
+			buck3_at_1v8 = true;
+		}
+
+		status = stpmu1_register_read(LDO3_CONTROL_REG, &read_val);
+		if (status != 0) {
+			return status;
+		}
+
+		read_val &= ~STPMU1_LDO3_MODE;
+		read_val &= ~STPMU1_LDO12356_OUTPUT_MASK;
+		read_val |= STPMU1_LDO3_1800000;
+		if (buck3_at_1v8) {
+			read_val |= STPMU1_LDO3_MODE;
+		}
+
+		status = stpmu1_register_write(LDO3_CONTROL_REG, read_val);
+		if (status != 0) {
+			return status;
+		}
+
+		status = stpmu1_regulator_voltage_set("buck2", 1200);
+		if (status != 0) {
+			return status;
+		}
+
+		status = stpmu1_regulator_enable("ldo3");
+		if (status != 0) {
+			return status;
+		}
+
+		mdelay(STPMU1_DEFAULT_START_UP_DELAY_MS);
+
+		status = stpmu1_regulator_enable("buck2");
+		if (status != 0) {
+			return status;
+		}
+
+		mdelay(STPMU1_DEFAULT_START_UP_DELAY_MS);
+
+		status = stpmu1_regulator_enable("vref_ddr");
+		if (status != 0) {
+			return status;
+		}
+
+		mdelay(STPMU1_DEFAULT_START_UP_DELAY_MS);
+		break;
+
+	default:
+		break;
+	};
+
+	return 0;
+}

drivers/st/pmic/stpmu1.c

+/*
+ * Copyright (c) 2016-2018, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <debug.h>
+#include <platform.h>
+#include <stpmu1.h>
+#include <string.h>
+
+struct regul_struct {
+	const char *dt_node_name;
+	const uint16_t *voltage_table;
+	uint8_t voltage_table_size;
+	uint8_t control_reg;
+	uint8_t low_power_reg;
+};
+
+static struct i2c_handle_s *stpmu_i2c_handle;
+static uint16_t stpmu_i2c_addr;
+
+/* Voltage tables in mV */
+static const uint16_t buck1_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,
+	1325,
+	1350,
+	1350,
+};
+
+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,
+	0xFFFF, /* VREFDDR */
+};
+
+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,
+};
+
+/* 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,
+		.low_power_reg	= BUCK1_PWRCTRL_REG,
+	},
+	{
+		.dt_node_name	= "buck2",
+		.voltage_table	= buck2_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(buck2_voltage_table),
+		.control_reg	= BUCK2_CONTROL_REG,
+		.low_power_reg	= BUCK2_PWRCTRL_REG,
+	},
+	{
+		.dt_node_name	= "buck3",
+		.voltage_table	= buck3_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(buck3_voltage_table),
+		.control_reg	= BUCK3_CONTROL_REG,
+		.low_power_reg	= BUCK3_PWRCTRL_REG,
+	},
+	{
+		.dt_node_name	= "buck4",
+		.voltage_table	= buck4_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(buck4_voltage_table),
+		.control_reg	= BUCK4_CONTROL_REG,
+		.low_power_reg	= BUCK4_PWRCTRL_REG,
+	},
+	{
+		.dt_node_name	= "ldo1",
+		.voltage_table	= ldo1_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(ldo1_voltage_table),
+		.control_reg	= LDO1_CONTROL_REG,
+		.low_power_reg	= LDO1_PWRCTRL_REG,
+	},
+	{
+		.dt_node_name	= "ldo2",
+		.voltage_table	= ldo2_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(ldo2_voltage_table),
+		.control_reg	= LDO2_CONTROL_REG,
+		.low_power_reg	= LDO2_PWRCTRL_REG,
+	},
+	{
+		.dt_node_name	= "ldo3",
+		.voltage_table	= ldo3_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(ldo3_voltage_table),
+		.control_reg	= LDO3_CONTROL_REG,
+		.low_power_reg	= LDO3_PWRCTRL_REG,
+	},
+	{
+		.dt_node_name	= "ldo4",
+		.voltage_table	= ldo4_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(ldo4_voltage_table),
+		.control_reg	= LDO4_CONTROL_REG,
+		.low_power_reg	= LDO4_PWRCTRL_REG,
+	},
+	{
+		.dt_node_name	= "ldo5",
+		.voltage_table	= ldo5_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(ldo5_voltage_table),
+		.control_reg	= LDO5_CONTROL_REG,
+		.low_power_reg	= LDO5_PWRCTRL_REG,
+	},
+	{
+		.dt_node_name	= "ldo6",
+		.voltage_table	= ldo6_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(ldo6_voltage_table),
+		.control_reg	= LDO6_CONTROL_REG,
+		.low_power_reg	= LDO6_PWRCTRL_REG,
+	},
+	{
+		.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,
+		.low_power_reg	= VREF_DDR_PWRCTRL_REG,
+	},
+};
+
+#define MAX_REGUL  ARRAY_SIZE(regulators_table)
+
+static const struct regul_struct *stpmu1_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 stpmu1_voltage_find_index(const char *name,
+					 uint16_t millivolts)
+{
+	const struct regul_struct *regul = stpmu1_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 stpmu1_switch_off(void)
+{
+	return stpmu1_register_update(MAIN_CONTROL_REG, 1,
+				      SOFTWARE_SWITCH_OFF_ENABLED);
+}
+
+int stpmu1_regulator_enable(const char *name)
+{
+	const struct regul_struct *regul = stpmu1_get_regulator_data(name);
+
+	return stpmu1_register_update(regul->control_reg, BIT(0), BIT(0));
+}
+
+int stpmu1_regulator_disable(const char *name)
+{
+	const struct regul_struct *regul = stpmu1_get_regulator_data(name);
+
+	return stpmu1_register_update(regul->control_reg, 0, BIT(0));
+}
+
+uint8_t stpmu1_is_regulator_enabled(const char *name)
+{
+	uint8_t val;
+	const struct regul_struct *regul = stpmu1_get_regulator_data(name);
+
+	if (stpmu1_register_read(regul->control_reg, &val) != 0) {
+		panic();
+	}
+
+	return (val & 0x1U);
+}
+
+int stpmu1_regulator_voltage_set(const char *name, uint16_t millivolts)
+{
+	uint8_t voltage_index = stpmu1_voltage_find_index(name, millivolts);
+	const struct regul_struct *regul = stpmu1_get_regulator_data(name);
+
+	return stpmu1_register_update(regul->control_reg, voltage_index << 2,
+				      0xFC);
+}
+
+int stpmu1_register_read(uint8_t register_id,  uint8_t *value)
+{
+	return stm32_i2c_mem_read(stpmu_i2c_handle, stpmu_i2c_addr,
+				    (uint16_t)register_id, I2C_MEMADD_SIZE_8BIT,
+				    value, 1, 100000);
+}
+
+int stpmu1_register_write(uint8_t register_id, uint8_t value)
+{
+	int status;
+
+	status = stm32_i2c_mem_write(stpmu_i2c_handle, stpmu_i2c_addr,
+				     (uint16_t)register_id,
+				     I2C_MEMADD_SIZE_8BIT, &value, 1, 100000);
+
+	if (status != 0) {
+		return status;
+	}
+
+	if ((register_id != WATCHDOG_CONTROL_REG) && (register_id <= 0x40U)) {
+		uint8_t readval;
+
+		status = stpmu1_register_read(register_id, &readval);
+		if (status != 0) {
+			return status;
+		}
+
+		if (readval != value) {
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+int stpmu1_register_update(uint8_t register_id, uint8_t value, uint8_t mask)
+{
+	int status;
+	uint8_t val;
+
+	status = stpmu1_register_read(register_id, &val);
+	if (status != 0) {
+		return status;
+	}
+
+	/* Clear bits to update */
+	val &= ~mask;
+
+	/* Update appropriate bits*/
+	val |= (value & mask);
+
+	/* Send new value on I2C Bus */
+	return stpmu1_register_write(register_id, val);
+}
+
+void stpmu1_bind_i2c(struct i2c_handle_s *i2c_handle, uint16_t i2c_addr)
+{
+	stpmu_i2c_handle = i2c_handle;
+	stpmu_i2c_addr = i2c_addr;
+}

drivers/st/reset/stm32mp1_reset.c

+/*
+ * Copyright (c) 2018, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <bl_common.h>
+#include <debug.h>
+#include <limits.h>
+#include <mmio.h>
+#include <platform_def.h>
+#include <stm32mp1_rcc.h>
+#include <stm32mp1_reset.h>
+#include <utils_def.h>
+
+#define RST_CLR_OFFSET	4U
+
+void stm32mp1_reset_assert(uint32_t id)
+{
+	uint32_t offset = (id / (uint32_t)__LONG_BIT) * sizeof(uintptr_t);
+	uint32_t bit = id % (uint32_t)__LONG_BIT;
+
+	mmio_write_32(RCC_BASE + offset, BIT(bit));
+	while ((mmio_read_32(RCC_BASE + offset) & BIT(bit)) == 0U) {
+		;
+	}
+}
+
+void stm32mp1_reset_deassert(uint32_t id)
+{
+	uint32_t offset = ((id / (uint32_t)__LONG_BIT) * sizeof(uintptr_t)) +
+			  RST_CLR_OFFSET;
+	uint32_t bit = id % (uint32_t)__LONG_BIT;
+
+	mmio_write_32(RCC_BASE + offset, BIT(bit));
+	while ((mmio_read_32(RCC_BASE + offset) & BIT(bit)) != 0U) {
+		;
+	}
+}

drivers/st/uart/aarch32/stm32_console.S

+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <asm_macros.S>
+
+#define USART_TIMEOUT		0x1000
+
+#define USART_CR1		0x00
+#define USART_CR1_UE		0x00000001
+#define USART_CR1_TE		0x00000008
+#define USART_CR1_FIFOEN	0x20000000
+
+#define USART_CR2		0x04
+#define USART_CR2_STOP		0x00003000
+
+#define USART_BRR		0x0C
+
+#define USART_ISR		0x1C
+#define USART_ISR_TC		0x00000040
+#define USART_ISR_TXE		0x00000080
+#define USART_ISR_TEACK		0x00200000
+
+#define USART_TDR		0x28
+
+	.globl	console_core_init
+	.globl	console_core_putc
+	.globl	console_core_getc
+	.globl	console_core_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_core_init
+	/* Check the input base address */
+	cmp	r0, #0
+	beq	core_init_fail
+#if defined(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 */
+	lsl	r3, r2, #1
+	add	r3, r1, r3
+	udiv	r3, r3, r2
+	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
+#endif /* IMAGE_BL2 */
+	mov	r0, #1
+	bx	lr
+core_init_fail:
+	mov	r0, #0
+	bx	lr
+endfunc console_core_init
+
+	/* ---------------------------------------------------------------
+	 * 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_core_putc
+	/* Check the input parameter */
+	cmp	r1, #0
+	beq	putc_error
+	/* Prepend '\r' to '\n' */
+	cmp	r0, #0xA
+	bne	2f
+1:
+	/* Check Transmit Data Register Empty */
+txe_loop_1:
+	ldr	r2, [r1, #USART_ISR]
+	tst	r2, #USART_ISR_TXE
+	beq	txe_loop_1
+	mov	r2, #0xD
+	str	r2, [r1, #USART_TDR]
+	/* Check transmit complete flag */
+tc_loop_1:
+	ldr	r2, [r1, #USART_ISR]
+	tst	r2, #USART_ISR_TC
+	beq	tc_loop_1
+2:
+	/* Check Transmit Data Register Empty */
+txe_loop_2:
+	ldr	r2, [r1, #USART_ISR]
+	tst	r2, #USART_ISR_TXE
+	beq	txe_loop_2
+	str	r0, [r1, #USART_TDR]
+	/* Check transmit complete flag */
+tc_loop_2:
+	ldr	r2, [r1, #USART_ISR]
+	tst	r2, #USART_ISR_TC
+	beq	tc_loop_2
+	bx	lr
+putc_error:
+	mov	r0, #-1
+	bx	lr
+endfunc console_core_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_core_getc
+	/* Not supported */
+	mov	r0, #-1
+	bx	lr
+endfunc console_core_getc
+
+	/* ---------------------------------------------------------------
+	 * int 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 : return -1 on error else return 0.
+	 * Clobber list : r0, r1
+	 * ---------------------------------------------------------------
+	 */
+func console_core_flush
+	cmp	r0, #0
+	beq	flush_error
+	/* Check Transmit Data Register Empty */
+txe_loop_3:
+	ldr	r1, [r0, #USART_ISR]
+	tst	r1, #USART_ISR_TXE
+	beq	txe_loop_3
+	mov	r0, #0
+	bx	lr
+flush_error:
+	mov	r0, #-1
+	bx	lr
+endfunc console_core_flush

fdts/stm32mp15-ddr.dtsi

+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+/ {
+	soc {
+		ddr: ddr@0x5A003000{
+
+			compatible = "st,stm32mp1-ddr";
+
+			reg = <0x5A003000 0x550
+			       0x5A004000 0x234>;
+
+			clocks = <&rcc AXIDCG>,
+				 <&rcc DDRC1>,
+				 <&rcc DDRC2>,
+				 <&rcc DDRPHYC>,
+				 <&rcc DDRCAPB>,
+				 <&rcc DDRPHYCAPB>;
+
+			clock-names = "axidcg",
+				      "ddrc1",
+				      "ddrc2",
+				      "ddrphyc",
+				      "ddrcapb",
+				      "ddrphycapb";
+
+			st,mem-name = DDR_MEM_NAME;
+			st,mem-speed = <DDR_MEM_SPEED>;
+			st,mem-size = <DDR_MEM_SIZE>;
+
+			st,ctl-reg = <
+				DDR_MSTR
+				DDR_MRCTRL0
+				DDR_MRCTRL1
+				DDR_DERATEEN
+				DDR_DERATEINT
+				DDR_PWRCTL
+				DDR_PWRTMG
+				DDR_HWLPCTL
+				DDR_RFSHCTL0
+				DDR_RFSHCTL3
+				DDR_CRCPARCTL0
+				DDR_ZQCTL0
+				DDR_DFITMG0
+				DDR_DFITMG1
+				DDR_DFILPCFG0
+				DDR_DFIUPD0
+				DDR_DFIUPD1
+				DDR_DFIUPD2
+				DDR_DFIPHYMSTR
+				DDR_ODTMAP
+				DDR_DBG0
+				DDR_DBG1
+				DDR_DBGCMD
+				DDR_POISONCFG
+				DDR_PCCFG
+			>;
+
+			st,ctl-timing = <
+				DDR_RFSHTMG
+				DDR_DRAMTMG0
+				DDR_DRAMTMG1
+				DDR_DRAMTMG2
+				DDR_DRAMTMG3
+				DDR_DRAMTMG4
+				DDR_DRAMTMG5
+				DDR_DRAMTMG6
+				DDR_DRAMTMG7
+				DDR_DRAMTMG8
+				DDR_DRAMTMG14
+				DDR_ODTCFG
+			>;
+
+			st,ctl-map = <
+				DDR_ADDRMAP1
+				DDR_ADDRMAP2
+				DDR_ADDRMAP3
+				DDR_ADDRMAP4
+				DDR_ADDRMAP5
+				DDR_ADDRMAP6
+				DDR_ADDRMAP9
+				DDR_ADDRMAP10
+				DDR_ADDRMAP11
+			>;
+
+			st,ctl-perf = <
+				DDR_SCHED
+				DDR_SCHED1
+				DDR_PERFHPR1
+				DDR_PERFLPR1
+				DDR_PERFWR1
+				DDR_PCFGR_0
+				DDR_PCFGW_0
+				DDR_PCFGQOS0_0
+				DDR_PCFGQOS1_0
+				DDR_PCFGWQOS0_0
+				DDR_PCFGWQOS1_0
+				DDR_PCFGR_1
+				DDR_PCFGW_1
+				DDR_PCFGQOS0_1
+				DDR_PCFGQOS1_1
+				DDR_PCFGWQOS0_1
+				DDR_PCFGWQOS1_1
+			>;
+
+			st,phy-reg = <
+				DDR_PGCR
+				DDR_ACIOCR
+				DDR_DXCCR
+				DDR_DSGCR
+				DDR_DCR
+				DDR_ODTCR
+				DDR_ZQ0CR1
+				DDR_DX0GCR
+				DDR_DX1GCR
+				DDR_DX2GCR
+				DDR_DX3GCR
+			>;
+
+			st,phy-timing = <
+				DDR_PTR0
+				DDR_PTR1
+				DDR_PTR2
+				DDR_DTPR0
+				DDR_DTPR1
+				DDR_DTPR2
+				DDR_MR0
+				DDR_MR1
+				DDR_MR2
+				DDR_MR3
+			>;
+
+			st,phy-cal = <
+				DDR_DX0DLLCR
+				DDR_DX0DQTR
+				DDR_DX0DQSTR
+				DDR_DX1DLLCR
+				DDR_DX1DQTR
+				DDR_DX1DQSTR
+				DDR_DX2DLLCR
+				DDR_DX2DQTR
+				DDR_DX2DQSTR
+				DDR_DX3DLLCR
+				DDR_DX3DQTR
+				DDR_DX3DQSTR
+			>;
+
+			status = "okay";
+		};
+	};
+};

fdts/stm32mp15-ddr3-2x4Gb-1066-binG.dtsi

+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+/* STM32MP157C ED1 and ED2 BOARD configuration
+ * 2x DDR3L 4Gb each, 16-bit, 533MHz, Single Die Package in flyby topology.
+ * Reference used NT5CC256M16DP-DI from NANYA
+ *
+ * DDR type / Platform	DDR3/3L
+ * freq		533MHz
+ * width	32
+ * datasheet	0  = MT41J256M16-187 / DDR3-1066 bin G
+ * DDR density	8
+ * timing mode	optimized
+ * Scheduling/QoS options : type = 2
+ * address mapping : RBC
+ */
+
+#define DDR_MEM_NAME "DDR3-1066 bin G 2x4Gb 533MHz v1.39"
+#define DDR_MEM_SPEED 533
+#define DDR_MEM_SIZE 0x40000000
+
+#define DDR_MSTR 0x00040401
+#define DDR_MRCTRL0 0x00000010
+#define DDR_MRCTRL1 0x00000000
+#define DDR_DERATEEN 0x00000000
+#define DDR_DERATEINT 0x00800000
+#define DDR_PWRCTL 0x00000000
+#define DDR_PWRTMG 0x00400010
+#define DDR_HWLPCTL 0x00000000
+#define DDR_RFSHCTL0 0x00210000
+#define DDR_RFSHCTL3 0x00000000
+#define DDR_RFSHTMG 0x0081008B
+#define DDR_CRCPARCTL0 0x00000000
+#define DDR_DRAMTMG0 0x121B2414
+#define DDR_DRAMTMG1 0x000A041C
+#define DDR_DRAMTMG2 0x0608090F
+#define DDR_DRAMTMG3 0x0050400C
+#define DDR_DRAMTMG4 0x08040608
+#define DDR_DRAMTMG5 0x06060403
+#define DDR_DRAMTMG6 0x02020002
+#define DDR_DRAMTMG7 0x00000202
+#define DDR_DRAMTMG8 0x00001005
+#define DDR_DRAMTMG14 0x000000A0
+#define DDR_ZQCTL0 0xC2000040
+#define DDR_DFITMG0 0x02060105
+#define DDR_DFITMG1 0x00000202
+#define DDR_DFILPCFG0 0x07000000
+#define DDR_DFIUPD0 0xC0400003
+#define DDR_DFIUPD1 0x00000000
+#define DDR_DFIUPD2 0x00000000
+#define DDR_DFIPHYMSTR 0x00000000
+#define DDR_ADDRMAP1 0x00080808
+#define DDR_ADDRMAP2 0x00000000
+#define DDR_ADDRMAP3 0x00000000
+#define DDR_ADDRMAP4 0x00001F1F
+#define DDR_ADDRMAP5 0x07070707
+#define DDR_ADDRMAP6 0x0F070707
+#define DDR_ADDRMAP9 0x00000000
+#define DDR_ADDRMAP10 0x00000000
+#define DDR_ADDRMAP11 0x00000000
+#define DDR_ODTCFG 0x06000600
+#define DDR_ODTMAP 0x00000001
+#define DDR_SCHED 0x00001201
+#define DDR_SCHED1 0x00000000
+#define DDR_PERFHPR1 0x01000001
+#define DDR_PERFLPR1 0x08000200
+#define DDR_PERFWR1 0x08000400
+#define DDR_DBG0 0x00000000
+#define DDR_DBG1 0x00000000
+#define DDR_DBGCMD 0x00000000
+#define DDR_POISONCFG 0x00000000
+#define DDR_PCCFG 0x00000010
+#define DDR_PCFGR_0 0x00010000
+#define DDR_PCFGW_0 0x00000000
+#define DDR_PCFGQOS0_0 0x02100B03
+#define DDR_PCFGQOS1_0 0x00800100
+#define DDR_PCFGWQOS0_0 0x01100B03
+#define DDR_PCFGWQOS1_0 0x01000200
+#define DDR_PCFGR_1 0x00010000
+#define DDR_PCFGW_1 0x00000000
+#define DDR_PCFGQOS0_1 0x02100B03
+#define DDR_PCFGQOS1_1 0x00800000
+#define DDR_PCFGWQOS0_1 0x01100B03
+#define DDR_PCFGWQOS1_1 0x01000200
+#define DDR_PGCR 0x01442E02
+#define DDR_PTR0 0x0022AA5B
+#define DDR_PTR1 0x04841104
+#define DDR_PTR2 0x042DA068
+#define DDR_ACIOCR 0x10400812
+#define DDR_DXCCR 0x00000C40
+#define DDR_DSGCR 0xF200001F
+#define DDR_DCR 0x0000000B
+#define DDR_DTPR0 0x38D488D0
+#define DDR_DTPR1 0x098B00D8
+#define DDR_DTPR2 0x10023600
+#define DDR_MR0 0x00000840
+#define DDR_MR1 0x00000000
+#define DDR_MR2 0x00000208
+#define DDR_MR3 0x00000000
+#define DDR_ODTCR 0x00010000
+#define DDR_ZQ0CR1 0x00000038
+#define DDR_DX0GCR 0x0000CE81
+#define DDR_DX0DLLCR 0x40000000
+#define DDR_DX0DQTR 0xFFFFFFFF
+#define DDR_DX0DQSTR 0x3DB02000
+#define DDR_DX1GCR 0x0000CE81
+#define DDR_DX1DLLCR 0x40000000
+#define DDR_DX1DQTR 0xFFFFFFFF
+#define DDR_DX1DQSTR 0x3DB02000
+#define DDR_DX2GCR 0x0000CE81
+#define DDR_DX2DLLCR 0x40000000
+#define DDR_DX2DQTR 0xFFFFFFFF
+#define DDR_DX2DQSTR 0x3DB02000
+#define DDR_DX3GCR 0x0000CE81
+#define DDR_DX3DLLCR 0x40000000
+#define DDR_DX3DQTR 0xFFFFFFFF
+#define DDR_DX3DQSTR 0x3DB02000
+
+#include "stm32mp15-ddr.dtsi"

fdts/stm32mp157-pinctrl.dtsi

+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2017 - All Rights Reserved
+ * Author: Ludovic Barre <ludovic.barre@st.com> for STMicroelectronics.
+ */
+
+#include <dt-bindings/pinctrl/stm32-pinfunc.h>
+/ {
+	soc {
+		pinctrl: pin-controller {
+			#address-cells = <1>;
+			#size-cells = <1>;
+			ranges = <0 0x50002000 0xa400>;
+			pins-are-numbered;
+
+			gpioa: gpio@50002000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x0 0x400>;
+				clocks = <&rcc GPIOA>;
+				st,bank-name = "GPIOA";
+				status = "disabled";
+			};
+
+			gpiob: gpio@50003000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x1000 0x400>;
+				clocks = <&rcc GPIOB>;
+				st,bank-name = "GPIOB";
+				status = "disabled";
+			};
+
+			gpioc: gpio@50004000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x2000 0x400>;
+				clocks = <&rcc GPIOC>;
+				st,bank-name = "GPIOC";
+				status = "disabled";
+			};
+
+			gpiod: gpio@50005000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x3000 0x400>;
+				clocks = <&rcc GPIOD>;
+				st,bank-name = "GPIOD";
+				status = "disabled";
+			};
+
+			gpioe: gpio@50006000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x4000 0x400>;
+				clocks = <&rcc GPIOE>;
+				st,bank-name = "GPIOE";
+				status = "disabled";
+			};
+
+			gpiof: gpio@50007000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x5000 0x400>;
+				clocks = <&rcc GPIOF>;
+				st,bank-name = "GPIOF";
+				status = "disabled";
+			};
+
+			gpiog: gpio@50008000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x6000 0x400>;
+				clocks = <&rcc GPIOG>;
+				st,bank-name = "GPIOG";
+				status = "disabled";
+			};
+
+			gpioh: gpio@50009000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x7000 0x400>;
+				clocks = <&rcc GPIOH>;
+				st,bank-name = "GPIOH";
+				status = "disabled";
+			};
+
+			gpioi: gpio@5000a000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x8000 0x400>;
+				clocks = <&rcc GPIOI>;
+				st,bank-name = "GPIOI";
+				status = "disabled";
+			};
+
+			gpioj: gpio@5000b000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x9000 0x400>;
+				clocks = <&rcc GPIOJ>;
+				st,bank-name = "GPIOJ";
+				status = "disabled";
+			};
+
+			gpiok: gpio@5000c000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0xa000 0x400>;
+				clocks = <&rcc GPIOK>;
+				st,bank-name = "GPIOK";
+				status = "disabled";
+			};
+
+			uart4_pins_a: uart4@0 {
+				pins1 {
+					pinmux = <STM32_PINMUX('G', 11, AF6)>; /* UART4_TX */
+					bias-disable;
+					drive-push-pull;
+					slew-rate = <0>;
+				};
+				pins2 {
+					pinmux = <STM32_PINMUX('B', 2, AF8)>; /* UART4_RX */
+					bias-disable;
+				};
+			};
+
+			usart3_pins_a: usart3@0 {
+				pins1 {
+					pinmux = <STM32_PINMUX('B', 10, AF7)>, /* USART3_TX */
+						 <STM32_PINMUX('G', 8, AF8)>; /* USART3_RTS */
+					bias-disable;
+					drive-push-pull;
+					slew-rate = <0>;
+				};
+				pins2 {
+					pinmux = <STM32_PINMUX('B', 12, AF8)>, /* USART3_RX */
+						 <STM32_PINMUX('I', 10, AF8)>; /* USART3_CTS_NSS */
+					bias-disable;
+				};
+			};
+
+			sdmmc1_b4_pins_a: sdmmc1-b4@0 {
+				pins {
+					pinmux = <STM32_PINMUX('C', 8, AF12)>, /* SDMMC1_D0 */
+						 <STM32_PINMUX('C', 9, AF12)>, /* SDMMC1_D1 */
+						 <STM32_PINMUX('C', 10, AF12)>, /* SDMMC1_D2 */
+						 <STM32_PINMUX('C', 11, AF12)>, /* SDMMC1_D3 */
+						 <STM32_PINMUX('C', 12, AF12)>, /* SDMMC1_CK */
+						 <STM32_PINMUX('D', 2, AF12)>; /* SDMMC1_CMD */
+					slew-rate = <3>;
+					drive-push-pull;
+					bias-disable;
+				};
+			};
+
+			sdmmc1_dir_pins_a: sdmmc1-dir@0 {
+				pins1 {
+					pinmux = <STM32_PINMUX('F', 2, AF11)>, /* SDMMC1_D0DIR */
+						 <STM32_PINMUX('C', 7, AF8)>, /* SDMMC1_D123DIR */
+						 <STM32_PINMUX('B', 9, AF11)>; /* SDMMC1_CDIR */
+					slew-rate = <3>;
+					drive-push-pull;
+					bias-pull-up;
+				};
+				pins2{
+					pinmux = <STM32_PINMUX('E', 4, AF8)>; /* SDMMC1_CKIN */
+					bias-pull-up;
+				};
+			};
+
+			sdmmc2_b4_pins_a: sdmmc2-b4@0 {
+				pins {
+					pinmux = <STM32_PINMUX('B', 14, AF9)>, /* SDMMC2_D0 */
+						 <STM32_PINMUX('B', 15, AF9)>, /* SDMMC2_D1 */
+						 <STM32_PINMUX('B', 3, AF9)>, /* SDMMC2_D2 */
+						 <STM32_PINMUX('B', 4, AF9)>, /* SDMMC2_D3 */
+						 <STM32_PINMUX('E', 3, AF9)>, /* SDMMC2_CK */
+						 <STM32_PINMUX('G', 6, AF10)>; /* SDMMC2_CMD */
+					slew-rate = <3>;
+					drive-push-pull;
+					bias-pull-up;
+				};
+			};
+
+			sdmmc2_d47_pins_a: sdmmc2-d47@0 {
+				pins {
+					pinmux = <STM32_PINMUX('A', 8, AF9)>, /* SDMMC2_D4 */
+						 <STM32_PINMUX('A', 9, AF10)>, /* SDMMC2_D5 */
+						 <STM32_PINMUX('E', 5, AF9)>, /* SDMMC2_D6 */
+						 <STM32_PINMUX('D', 3, AF9)>; /* SDMMC2_D7 */
+					slew-rate = <3>;
+					drive-push-pull;
+					bias-pull-up;
+				};
+			};
+		};
+
+		pinctrl_z: pin-controller-z {
+			#address-cells = <1>;
+			#size-cells = <1>;
+			ranges = <0 0x54004000 0x400>;
+			pins-are-numbered;
+
+			gpioz: gpio@54004000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0 0x400>;
+				clocks = <&rcc GPIOZ>;
+				st,bank-name = "GPIOZ";
+				st,bank-ioport = <11>;
+				status = "disabled";
+			};
+
+			i2c4_pins_a: i2c4@0 {
+				pins {
+					pinmux = <STM32_PINMUX('Z', 4, AF6)>, /* I2C4_SCL */
+						 <STM32_PINMUX('Z', 5, AF6)>; /* I2C4_SDA */
+					bias-disable;
+					drive-open-drain;
+					slew-rate = <0>;
+				};
+			};
+		};
+	};
+};

fdts/stm32mp157c-ed1.dts

+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2017 - All Rights Reserved
+ * Author: Ludovic Barre <ludovic.barre@st.com> for STMicroelectronics.
+ */
+
+/dts-v1/;
+
+#include "stm32mp157c.dtsi"
+#include "stm32mp157caa-pinctrl.dtsi"
+
+/ {
+	model = "STMicroelectronics STM32MP157C-ED1 pmic eval daughter";
+	compatible = "st,stm32mp157c-ed1", "st,stm32mp157";
+
+	chosen {
+		bootargs = "earlyprintk console=ttyS3,115200 root=/dev/ram";
+		stdout-path = "serial3:115200n8";
+	};
+};
+
+&i2c4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&i2c4_pins_a>;
+	i2c-scl-rising-time-ns = <185>;
+	i2c-scl-falling-time-ns = <20>;
+	status = "okay";
+
+	pmic: stpmu1@33 {
+		compatible = "st,stpmu1";
+		reg = <0x33>;
+		status = "okay";
+
+		st,main_control_register = <0x04>;
+		st,vin_control_register = <0xc0>;
+		st,usb_control_register = <0x30>;
+
+		regulators {
+			compatible = "st,stpmu1-regulators";
+
+			v3v3: buck4 {
+				regulator-name = "v3v3";
+				regulator-min-microvolt = <3300000>;
+				regulator-max-microvolt = <3300000>;
+				regulator-boot-on;
+				regulator-over-current-protection;
+				regulator-initial-mode = <8>;
+
+				regulator-state-standby {
+					regulator-suspend-microvolt = <3300000>;
+					regulator-unchanged-in-suspend;
+					regulator-mode = <8>;
+				};
+				regulator-state-mem {
+					regulator-off-in-suspend;
+				};
+				regulator-state-disk {
+					regulator-off-in-suspend;
+				};
+			};
+
+			vdd_sd: ldo5 {
+				regulator-name = "vdd_sd";
+				regulator-min-microvolt = <2900000>;
+				regulator-max-microvolt = <2900000>;
+				regulator-boot-on;
+
+				regulator-state-standby {
+					regulator-suspend-microvolt = <2900000>;
+					regulator-unchanged-in-suspend;
+				};
+				regulator-state-mem {
+					regulator-off-in-suspend;
+				};
+				regulator-state-disk {
+					regulator-off-in-suspend;
+				};
+			};
+		};
+	};
+};
+
+&iwdg2 {
+	instance = <2>;
+	timeout-sec = <32>;
+	status = "okay";
+};
+
+&rng1 {
+	status = "okay";
+};
+
+&sdmmc1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&sdmmc1_b4_pins_a &sdmmc1_dir_pins_a>;
+	broken-cd;
+	st,dirpol;
+	st,negedge;
+	st,pin-ckin;
+	bus-width = <4>;
+	sd-uhs-sdr12;
+	sd-uhs-sdr25;
+	sd-uhs-sdr50;
+	sd-uhs-ddr50;
+	sd-uhs-sdr104;
+	status = "okay";
+};
+
+&sdmmc2 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&sdmmc2_b4_pins_a &sdmmc2_d47_pins_a>;
+	non-removable;
+	no-sd;
+	no-sdio;
+	st,dirpol;
+	st,negedge;
+	bus-width = <8>;
+	status = "okay";
+};
+
+&uart4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&uart4_pins_a>;
+	resets = <&rcc UART4_R>;
+	status = "okay";
+};
+
+/* ATF Specific */
+#include <dt-bindings/clock/stm32mp1-clksrc.h>
+#include "stm32mp15-ddr3-2x4Gb-1066-binG.dtsi"
+
+/ {
+	aliases {
+		gpio0 = &gpioa;
+		gpio1 = &gpiob;
+		gpio2 = &gpioc;
+		gpio3 = &gpiod;
+		gpio4 = &gpioe;
+		gpio5 = &gpiof;
+		gpio6 = &gpiog;
+		gpio7 = &gpioh;
+		gpio8 = &gpioi;
+		gpio9 = &gpioj;
+		gpio10 = &gpiok;
+		gpio25 = &gpioz;
+		i2c3 = &i2c4;
+	};
+
+	soc {
+		stgen: stgen@5C008000 {
+			compatible = "st,stm32-stgen";
+			reg = <0x5C008000 0x1000>;
+			status = "okay";
+		};
+	};
+};
+
+/* CLOCK init */
+&rcc {
+	st,clksrc = <
+		CLK_MPU_PLL1P
+		CLK_AXI_PLL2P
+		CLK_PLL12_HSE
+		CLK_PLL3_HSE
+		CLK_PLL4_HSE
+		CLK_RTC_LSE
+		CLK_MCO1_DISABLED
+		CLK_MCO2_DISABLED
+	>;
+
+	st,clkdiv = <
+		1 /*MPU*/
+		0 /*AXI*/
+		1 /*APB1*/
+		1 /*APB2*/
+		1 /*APB3*/
+		1 /*APB4*/
+		2 /*APB5*/
+		23 /*RTC*/
+		0 /*MCO1*/
+		0 /*MCO2*/
+	>;
+
+	st,pkcs = <
+		CLK_CKPER_HSE
+		CLK_FMC_ACLK
+		CLK_QSPI_ACLK
+		CLK_ETH_DISABLED
+		CLK_SDMMC12_PLL3R
+		CLK_DSI_DSIPLL
+		CLK_STGEN_HSE
+		CLK_USBPHY_HSE
+		CLK_SPI2S1_PLL3Q
+		CLK_SPI2S23_PLL3Q
+		CLK_SPI45_HSI
+		CLK_SPI6_HSI
+		CLK_I2C46_HSI
+		CLK_SDMMC3_PLL3R
+		CLK_USBO_USBPHY
+		CLK_ADC_CKPER
+		CLK_CEC_LSE
+		CLK_I2C12_HSI
+		CLK_I2C35_HSI
+		CLK_UART1_HSI
+		CLK_UART24_HSI
+		CLK_UART35_HSI
+		CLK_UART6_HSI
+		CLK_UART78_HSI
+		CLK_SPDIF_PLL3Q
+		CLK_FDCAN_PLL4Q
+		CLK_SAI1_PLL3Q
+		CLK_SAI2_PLL3Q
+		CLK_SAI3_PLL3Q
+		CLK_SAI4_PLL3Q
+		CLK_RNG1_CSI
+		CLK_RNG2_CSI
+		CLK_LPTIM1_PCLK1
+		CLK_LPTIM23_PCLK3
+		CLK_LPTIM45_PCLK3
+	>;
+
+	/* VCO = 1300.0 MHz => P = 650 (CPU) */
+	pll1: st,pll@0 {
+		cfg = < 2 80 0 0 0 PQR(1,0,0) >;
+		frac = < 0x800 >;
+	};
+
+	/* VCO = 1066.0 MHz => P = 266 (AXI), Q = 533 (GPU), R = 533 (DDR) */
+	pll2: st,pll@1 {
+		cfg = < 2 65 1 0 0 PQR(1,1,1) >;
+		frac = < 0x1400 >;
+	};
+
+	/* VCO = 786.4 MHz => P = 197, Q = 49, R = 98 */
+	pll3: st,pll@2 {
+		cfg = < 2 97 3 15 7 PQR(1,1,1) >;
+		frac = < 0x9ba >;
+	};
+
+	/* VCO = 508.0 MHz => P = 56, Q = 56, R = 56 */
+	pll4: st,pll@3 {
+		cfg = < 5 126 8 8 8 PQR(1,1,1) >;
+	};
+};
+
+/delete-node/ &clk_csi;

fdts/stm32mp157c-ev1.dts

+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2017 - All Rights Reserved
+ * Author: Ludovic Barre <ludovic.barre@st.com> for STMicroelectronics.
+ */
+
+/dts-v1/;
+#include "stm32mp157c-ed1.dts"
+
+/ {
+	model = "STMicroelectronics STM32MP157C-EV1 pmic eval daughter on eval mother";
+	compatible = "st,stm32mp157c-ev1", "st,stm32mp157c-ed1", "st,stm32mp157";
+
+	chosen {
+		bootargs = "earlyprintk console=ttyS3,115200 root=/dev/ram";
+		stdout-path = "serial3:115200n8";
+	};
+};
+
+&usart3 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&usart3_pins_a>;
+	resets = <&rcc USART3_R>;
+	status = "disabled";
+};

fdts/stm32mp157c.dtsi

+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2017 - All Rights Reserved
+ * Author: Ludovic Barre <ludovic.barre@st.com> for STMicroelectronics.
+ */
+
+#include <dt-bindings/clock/stm32mp1-clks.h>
+#include <dt-bindings/reset/stm32mp1-resets.h>
+
+/ {
+	#address-cells = <1>;
+	#size-cells = <1>;
+
+	aliases {
+		serial0 = &usart1;
+		serial1 = &usart2;
+		serial2 = &usart3;
+		serial3 = &uart4;
+		serial4 = &uart5;
+		serial5 = &usart6;
+		serial6 = &uart7;
+		serial7 = &uart8;
+	};
+
+	clocks {
+		clk_hse: clk-hse {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			clock-frequency = <24000000>;
+		};
+
+		clk_hsi: clk-hsi {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			clock-frequency = <64000000>;
+		};
+
+		clk_lse: clk-lse {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			clock-frequency = <32768>;
+		};
+
+		clk_lsi: clk-lsi {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			clock-frequency = <32000>;
+		};
+
+		clk_csi: clk-csi {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			clock-frequency = <4000000>;
+		};
+
+		clk_i2s_ckin: i2s_ckin {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			clock-frequency = <64000000>;
+		};
+
+		clk_dsi_phy: ck_dsi_phy {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			clock-frequency = <0>;
+		};
+
+		clk_usbo_48m: ck_usbo_48m {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			clock-frequency = <48000000>;
+		};
+	};
+
+	soc {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges;
+
+		usart2: serial@4000e000 {
+			compatible = "st,stm32h7-usart";
+			reg = <0x4000e000 0x400>;
+			clocks = <&rcc USART2_K>;
+			status = "disabled";
+		};
+
+		usart3: serial@4000f000 {
+			compatible = "st,stm32h7-usart";
+			reg = <0x4000f000 0x400>;
+			clocks = <&rcc USART3_K>;
+			status = "disabled";
+		};
+
+		uart4: serial@40010000 {
+			compatible = "st,stm32h7-uart";
+			reg = <0x40010000 0x400>;
+			clocks = <&rcc UART4_K>;
+			status = "disabled";
+		};
+
+		uart5: serial@40011000 {
+			compatible = "st,stm32h7-uart";
+			reg = <0x40011000 0x400>;
+			clocks = <&rcc UART5_K>;
+			status = "disabled";
+		};
+
+
+		uart7: serial@40018000 {
+			compatible = "st,stm32h7-uart";
+			reg = <0x40018000 0x400>;
+			clocks = <&rcc UART7_K>;
+			status = "disabled";
+		};
+
+		uart8: serial@40019000 {
+			compatible = "st,stm32h7-uart";
+			reg = <0x40019000 0x400>;
+			clocks = <&rcc UART8_K>;
+			status = "disabled";
+		};
+
+		usart6: serial@44003000 {
+			compatible = "st,stm32h7-usart";
+			reg = <0x44003000 0x400>;
+			clocks = <&rcc USART6_K>;
+			status = "disabled";
+		};
+
+		sdmmc3: sdmmc@48004000 {
+			compatible = "st,stm32-sdmmc2";
+			reg = <0x48004000 0x400>, <0x48005000 0x400>;
+			reg-names = "sdmmc", "delay";
+			clocks = <&rcc SDMMC3_K>;
+			resets = <&rcc SDMMC3_R>;
+			cap-sd-highspeed;
+			cap-mmc-highspeed;
+			max-frequency = <120000000>;
+			status = "disabled";
+		};
+
+		rcc: rcc@50000000 {
+			compatible = "syscon", "st,stm32mp1-rcc";
+			#clock-cells = <1>;
+			#reset-cells = <1>;
+			reg = <0x50000000 0x1000>;
+		};
+
+		rcc_reboot: rcc-reboot@50000000 {
+				compatible = "syscon-reboot";
+				regmap = <&rcc>;
+				offset = <0x404>;
+				mask = <0x1>;
+		};
+
+		rng1: rng@54003000 {
+			compatible = "st,stm32-rng";
+			reg = <0x54003000 0x400>;
+			clocks = <&rcc RNG1_K>;
+			resets = <&rcc RNG1_R>;
+			status = "disabled";
+		};
+
+		fmc_nand: fmc_nand@58002000 {
+			compatible = "st,stm32mp1-fmc";
+			reg = <0x58002000 0x1000>,
+			      <0x80000000 0x40000>,
+			      <0x81000000 0x40000>,
+			      <0x88000000 0x40000>,
+			      <0x89000000 0x40000>;
+			clocks = <&rcc FMC_K>;
+			resets = <&rcc FMC_R>;
+			status = "disabled";
+		};
+
+		qspi: qspi@58003000 {
+			compatible = "st,stm32f469-qspi";
+			reg = <0x58003000 0x1000>, <0x70000000 0x10000000>;
+			clocks = <&rcc QSPI_K>;
+			status = "disabled";
+		};
+
+		sdmmc1: sdmmc@58005000 {
+			compatible = "st,stm32-sdmmc2";
+			reg = <0x58005000 0x1000>, <0x58006000 0x1000>;
+			reg-names = "sdmmc", "delay";
+			clocks = <&rcc SDMMC1_K>;
+			resets = <&rcc SDMMC1_R>;
+			cap-sd-highspeed;
+			cap-mmc-highspeed;
+			max-frequency = <120000000>;
+			status = "disabled";
+		};
+
+		sdmmc2: sdmmc@58007000 {
+			compatible = "st,stm32-sdmmc2";
+			reg = <0x58007000 0x1000>, <0x58008000 0x1000>;
+			reg-names = "sdmmc", "delay";
+			clocks = <&rcc SDMMC2_K>;
+			resets = <&rcc SDMMC2_R>;
+			cap-sd-highspeed;
+			cap-mmc-highspeed;
+			max-frequency = <120000000>;
+			status = "disabled";
+		};
+
+		iwdg2: iwdg@5a002000 {
+			compatible = "st,stm32mp1-iwdg";
+			reg = <0x5a002000 0x400>;
+			clocks = <&rcc IWDG2>, <&rcc CK_LSI>;
+			clock-names = "pclk", "lsi";
+			status = "disabled";
+		};
+
+		usart1: serial@5c000000 {
+			compatible = "st,stm32h7-usart";
+			reg = <0x5c000000 0x400>;
+			clocks = <&rcc USART1_K>;
+			status = "disabled";
+		};
+
+		i2c4: i2c@5c002000 {
+			compatible = "st,stm32f7-i2c";
+			reg = <0x5c002000 0x400>;
+			clocks = <&rcc I2C4_K>;
+			resets = <&rcc I2C4_R>;
+			#address-cells = <1>;
+			#size-cells = <0>;
+			status = "disabled";
+		};
+
+		rtc: rtc@5c004000 {
+			compatible = "st,stm32mp1-rtc";
+			reg = <0x5c004000 0x400>;
+			clocks = <&rcc RTCAPB>, <&rcc RTC>;
+			clock-names = "pclk", "rtc_ck";
+		};
+	};
+};