diff --git a/recipes-devtools/python/python-terminaltables.inc b/recipes-devtools/python/python-terminaltables.inc new file mode 100644 index 0000000..7ae2a1a --- /dev/null +++ b/recipes-devtools/python/python-terminaltables.inc @@ -0,0 +1,12 @@ +SUMMARY = "Python 2 and 3 compatibility library" +HOMEPAGE = "https://pypi.org/project/terminaltables/" +SECTION = "devel/python" +LICENSE = "MIT" +LIC_FILES_CHKSUM = "file://PKG-INFO;beginline=8;endline=8;md5=8227180126797a0148f94f483f3e1489" + +SRC_URI[md5sum] = "863797674d8f75d22e16e6c1fdcbeb41" +SRC_URI[sha256sum] = "f3eb0eb92e3833972ac36796293ca0906e998dc3be91fbe1f8615b331b853b81" + +inherit pypi + +BBCLASSEXTEND += "native nativesdk" diff --git a/recipes-devtools/python/python3-terminaltables_3.1.0.bb b/recipes-devtools/python/python3-terminaltables_3.1.0.bb new file mode 100644 index 0000000..54789f6 --- /dev/null +++ b/recipes-devtools/python/python3-terminaltables_3.1.0.bb @@ -0,0 +1,2 @@ +inherit setuptools3 +require python-terminaltables.inc diff --git a/recipes-security/optee/optee-os-stm32mp_3.3.0.bb b/recipes-security/optee/optee-os-stm32mp_3.3.0.bb index 66aa6e0..89c8de1 100644 --- a/recipes-security/optee/optee-os-stm32mp_3.3.0.bb +++ b/recipes-security/optee/optee-os-stm32mp_3.3.0.bb @@ -9,7 +9,8 @@ SRC_URI[sha256sum] = "7b62e9fe650e197473eb2f4dc35c09d1e6395eb48dc1c16cc139d401b3 SRC_URI += " \ file://0001-st-updates-r1.patch \ file://0002-st-updates-r2.patch \ - " + file://0003-st-updates-r3.patch \ + " OPTEE_VERSION = "3.3.0" PV = "${OPTEE_VERSION}" @@ -20,6 +21,9 @@ COMPATIBLE_MACHINE = "(stm32mpcommon)" PROVIDES += "optee-os" +# The package is empty but must be generated to avoid apt-get installation issue +ALLOW_EMPTY_${PN} = "1" + require optee-os-stm32mp-common.inc # --------------------------------- @@ -33,7 +37,7 @@ include ${@oe.utils.ifelse(d.getVar('ST_ARCHIVER_ENABLE') == '1', 'optee-os-stm3 BBCLASSEXTEND = "devupstream:target" SRC_URI_class-devupstream = "git://github.com/STMicroelectronics/optee_os.git;protocol=https;name=opteeos;branch=3.3.0-stm32mp" -SRCREV_class-devupstream = "2d2ec9752d49db633b4295ee509629642dfd8748" +SRCREV_class-devupstream = "273094317924bd18d2b7e7ed1d98e5118ed9c1fa" SRCREV_FORMAT_class-devupstream = "opteeos" PV_class-devupstream = "${OPTEE_VERSION}+github+${SRCPV}" diff --git a/recipes-security/optee/optee-os/0003-st-updates-r3.patch b/recipes-security/optee/optee-os/0003-st-updates-r3.patch new file mode 100644 index 0000000..6f76302 --- /dev/null +++ b/recipes-security/optee/optee-os/0003-st-updates-r3.patch @@ -0,0 +1,4025 @@ +From 120829073d96f24a5cc92626675a5d591b2fa726 Mon Sep 17 00:00:00 2001 +From: Romuald JEANNE +Date: Mon, 20 Jan 2020 15:46:53 +0100 +Subject: [PATCH] st updates r3 + +Signed-off-by: Romuald JEANNE +--- + core/arch/arm/fdts/stm32mp15-ddr.dtsi | 4 +- + core/arch/arm/fdts/stm32mp157a-dk1.dts | 34 +- + core/arch/arm/fdts/stm32mp157c-ed1.dts | 51 +- + core/arch/arm/fdts/stm32mp157c-security.dtsi | 52 +- + core/arch/arm/fdts/stm32mp157c.dtsi | 38 ++ + core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.c | 669 ++++++++++++++++++++- + core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.h | 9 +- + .../arm/plat-stm32mp1/drivers/stm32mp1_clkfunc.c | 58 +- + .../arch/arm/plat-stm32mp1/drivers/stm32mp1_ddrc.c | 20 + + .../arch/arm/plat-stm32mp1/drivers/stm32mp1_ddrc.h | 1 + + .../arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.c | 76 +-- + .../arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.h | 7 +- + .../arm/plat-stm32mp1/drivers/stm32mp1_syscfg.c | 97 +++ + core/arch/arm/plat-stm32mp1/drivers/sub.mk | 1 + + core/arch/arm/plat-stm32mp1/main.c | 162 ++++- + core/arch/arm/plat-stm32mp1/platform_config.h | 49 +- + core/arch/arm/plat-stm32mp1/pm/context.c | 45 +- + core/arch/arm/plat-stm32mp1/pm/low_power.c | 10 +- + core/arch/arm/plat-stm32mp1/pm/pm_helpers.S | 143 ++++- + core/arch/arm/plat-stm32mp1/pm/power_config.c | 4 +- + core/arch/arm/plat-stm32mp1/service/bsec_svc.c | 4 + + core/arch/arm/plat-stm32mp1/service/rcc_svc.c | 30 +- + core/arch/arm/plat-stm32mp1/service/rcc_svc.h | 3 +- + core/arch/arm/plat-stm32mp1/service/stm32mp1_smc.h | 16 + + .../arm/plat-stm32mp1/service/stm32mp1_svc_setup.c | 5 +- + core/arch/arm/plat-stm32mp1/stm32_util.h | 20 +- + core/arch/arm/plat-stm32mp1/stm32mp1_dt.c | 194 +++++- + core/arch/arm/plat-stm32mp1/stm32mp_dt.h | 10 +- + core/drivers/stm32_bsec.c | 188 +++++- + core/drivers/stm32_i2c.c | 232 ++++--- + core/drivers/stm32_iwdg.c | 25 +- + core/drivers/stm32_rtc.c | 39 +- + core/drivers/stpmic1.c | 25 +- + core/include/drivers/stm32_bsec.h | 13 + + core/include/drivers/stm32_i2c.h | 24 +- + core/include/drivers/stm32_iwdg.h | 6 +- + core/include/drivers/stpmic1.h | 27 +- + 37 files changed, 2030 insertions(+), 361 deletions(-) + create mode 100644 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_syscfg.c + +diff --git a/core/arch/arm/fdts/stm32mp15-ddr.dtsi b/core/arch/arm/fdts/stm32mp15-ddr.dtsi +index 1a5c51c..d3481e5 100644 +--- a/core/arch/arm/fdts/stm32mp15-ddr.dtsi ++++ b/core/arch/arm/fdts/stm32mp15-ddr.dtsi +@@ -1,11 +1,11 @@ + // SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause + /* +- * Copyright (C) 2018, STMicroelectronics - All Rights Reserved ++ * Copyright (C) 2018-2019, STMicroelectronics - All Rights Reserved + */ + + / { + soc { +- ddr: ddr@5A003000{ ++ ddr: ddr@5a003000{ + + compatible = "st,stm32mp1-ddr"; + +diff --git a/core/arch/arm/fdts/stm32mp157a-dk1.dts b/core/arch/arm/fdts/stm32mp157a-dk1.dts +index 78cb849..b54b284 100644 +--- a/core/arch/arm/fdts/stm32mp157a-dk1.dts ++++ b/core/arch/arm/fdts/stm32mp157a-dk1.dts +@@ -21,8 +21,7 @@ + + chosen { + stdout-path = "serial0:115200n8"; +-}; +- ++ }; + }; + + &clk_hse { +@@ -34,6 +33,7 @@ + pinctrl-0 = <&i2c4_pins_a>; + i2c-scl-rising-time-ns = <185>; + i2c-scl-falling-time-ns = <20>; ++ clock-frequency = <400000>; + status = "okay"; + + pmic: stpmic@33 { +@@ -43,10 +43,7 @@ + interrupt-controller; + #interrupt-cells = <2>; + status = "okay"; +- +- st,main-control-register = <0x04>; +- st,vin-control-register = <0xc0>; +- st,usb-control-register = <0x20>; ++ wakeup-source; + + regulators { + compatible = "st,stpmic1-regulators"; +@@ -224,6 +221,13 @@ + }; + + /* Security specific */ ++&bsec { ++ board_id: board_id@ec { ++ reg = <0xec 0x4>; ++ st,non-secure-otp; ++ }; ++}; ++ + &etzpc { + st,decprot = < + DECPROT(STM32MP1_ETZPC_USART1_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +@@ -240,6 +244,24 @@ + secure-status = "okay"; + }; + ++&nvmem_layout { ++ nvmem-cells = <&part_number_otp>, ++ <&monotonic_otp>, ++ <&nand_otp>, ++ <&uid_otp>, ++ <&package_otp>, ++ <&hw2_otp>, ++ <&board_id>; ++ ++ nvmem-cell-names = "part_number_otp", ++ "monotonic_otp", ++ "nand_otp", ++ "uid_otp", ++ "package_otp", ++ "hw2_otp", ++ "board_id"; ++}; ++ + &pwr { + system_suspend_supported_soc_modes = < + STM32_PM_CSLEEP_RUN +diff --git a/core/arch/arm/fdts/stm32mp157c-ed1.dts b/core/arch/arm/fdts/stm32mp157c-ed1.dts +index 81ae94c..ff9441e 100644 +--- a/core/arch/arm/fdts/stm32mp157c-ed1.dts ++++ b/core/arch/arm/fdts/stm32mp157c-ed1.dts +@@ -30,6 +30,7 @@ + pinctrl-0 = <&i2c4_pins_a>; + i2c-scl-rising-time-ns = <185>; + i2c-scl-falling-time-ns = <20>; ++ clock-frequency = <400000>; + status = "okay"; + + pmic: stpmic@33 { +@@ -39,10 +40,7 @@ + interrupt-controller; + #interrupt-cells = <2>; + status = "okay"; +- +- st,main-control-register = <0x04>; +- st,vin-control-register = <0xc0>; +- st,usb-control-register = <0x20>; ++ wakeup-source; + + regulators { + compatible = "st,stpmic1-regulators"; +@@ -229,6 +227,13 @@ + }; + + /* Security specific */ ++&bsec { ++ board_id: board_id@ec { ++ reg = <0xec 0x4>; ++ st,non-secure-otp; ++ }; ++}; ++ + &etzpc { + st,decprot = < + DECPROT(STM32MP1_ETZPC_USART1_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +@@ -245,13 +250,31 @@ + secure-status = "okay"; + }; + ++&nvmem_layout { ++ nvmem-cells = <&part_number_otp>, ++ <&monotonic_otp>, ++ <&nand_otp>, ++ <&uid_otp>, ++ <&package_otp>, ++ <&hw2_otp>, ++ <&board_id>; ++ ++ nvmem-cell-names = "part_number_otp", ++ "monotonic_otp", ++ "nand_otp", ++ "uid_otp", ++ "package_otp", ++ "hw2_otp", ++ "board_id"; ++}; ++ + &pwr { + system_suspend_supported_soc_modes = < + STM32_PM_CSLEEP_RUN + STM32_PM_CSTOP_ALLOW_LP_STOP ++ STM32_PM_CSTOP_ALLOW_LPLV_STOP + STM32_PM_CSTOP_ALLOW_STANDBY_DDR_SR + >; +- + system_off_soc_mode = ; + }; + +@@ -267,6 +290,10 @@ + regulator-on-in-suspend; + regulator-suspend-microvolt = <1200000>; + }; ++ lplv-stop { ++ regulator-on-in-suspend; ++ regulator-suspend-microvolt = <900000>; ++ }; + standby-ddr-sr { + regulator-off-in-suspend; + }; +@@ -280,6 +307,10 @@ + regulator-suspend-microvolt = <1350000>; + regulator-on-in-suspend; + }; ++ lplv-stop { ++ regulator-suspend-microvolt = <1350000>; ++ regulator-on-in-suspend; ++ }; + standby-ddr-sr { + regulator-suspend-microvolt = <1350000>; + regulator-on-in-suspend; +@@ -294,6 +325,10 @@ + regulator-suspend-microvolt = <3300000>; + regulator-on-in-suspend; + }; ++ lplv-stop { ++ regulator-suspend-microvolt = <3300000>; ++ regulator-on-in-suspend; ++ }; + standby-ddr-sr { + regulator-suspend-microvolt = <3300000>; + regulator-on-in-suspend; +@@ -335,6 +370,9 @@ + lp-stop { + regulator-off-in-suspend; + }; ++ lplv-stop { ++ regulator-off-in-suspend; ++ }; + standby-ddr-sr { + regulator-off-in-suspend; + }; +@@ -374,6 +412,9 @@ + lp-stop { + regulator-on-in-suspend; + }; ++ lplv-stop { ++ regulator-on-in-suspend; ++ }; + standby-ddr-sr { + regulator-on-in-suspend; + }; +diff --git a/core/arch/arm/fdts/stm32mp157c-security.dtsi b/core/arch/arm/fdts/stm32mp157c-security.dtsi +index bff1043..5f5404f 100644 +--- a/core/arch/arm/fdts/stm32mp157c-security.dtsi ++++ b/core/arch/arm/fdts/stm32mp157c-security.dtsi +@@ -1,5 +1,5 @@ + /* +- * Copyright : STMicroelectronics 2017 ++ * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ BSD-3-Clause + */ +@@ -8,7 +8,7 @@ + + / { + soc { +- iwdg1: iwdg@5C003000 { ++ iwdg1: watchdog@5c003000 { + compatible = "st,stm32mp1-iwdg"; + reg = <0x5C003000 0x400>; + clocks = <&rcc IWDG1>, <&rcc CK_LSI>; +@@ -18,7 +18,7 @@ + secure-status = "disabled"; + }; + +- etzpc: etzpc@5C007000 { ++ etzpc: etzpc@5c007000 { + compatible = "st,stm32-etzpc"; + reg = <0x5C007000 0x400>; + clocks = <&rcc TZPC>; +@@ -26,23 +26,53 @@ + secure-status = "okay"; + }; + +- stgen: stgen@5C008000 { ++ stgen: stgen@5c008000 { + compatible = "st,stm32-stgen"; + reg = <0x5C008000 0x1000>; + }; ++ ++ nvmem_layout: nvmem_layout@0 { ++ compatible = "st,stm32-nvmem-layout"; ++ ++ nvmem-cells = <&part_number_otp>, ++ <&monotonic_otp>, ++ <&nand_otp>, ++ <&uid_otp>, ++ <&package_otp>, ++ <&hw2_otp>; ++ ++ nvmem-cell-names = "part_number_otp", ++ "monotonic_otp", ++ "nand_otp", ++ "uid_otp", ++ "package_otp", ++ "hw2_otp"; ++ }; + }; + }; + + &bsec { +- mac_addr: mac_addr@e4 { +- reg = <0xe4 0x6>; ++ part_number_otp: part_number_otp@4 { ++ reg = <0x4 0x1>; + }; +- /* Spare field to align on 32-bit OTP granularity */ +- spare_ns_ea: spare_ns_ea@ea { +- reg = <0xea 0x2>; ++ monotonic_otp: monotonic_otp@10 { ++ reg = <0x10 0x4>; + }; +- board_id: board_id@ec { +- reg = <0xec 0x4>; ++ nand_otp: nand_otp@24 { ++ reg = <0x24 0x4>; ++ }; ++ uid_otp: uid_otp@34 { ++ reg = <0x34 0xc>; ++ }; ++ package_otp: package_otp@40 { ++ reg = <0x40 0x4>; ++ }; ++ hw2_otp: hw2_otp@48 { ++ reg = <0x48 0x4>; ++ }; ++ mac_addr: mac_addr@e4 { ++ reg = <0xe4 0x8>; ++ st,non-secure-otp; + }; + }; + +diff --git a/core/arch/arm/fdts/stm32mp157c.dtsi b/core/arch/arm/fdts/stm32mp157c.dtsi +index 06c2cf1..13125b1 100644 +--- a/core/arch/arm/fdts/stm32mp157c.dtsi ++++ b/core/arch/arm/fdts/stm32mp157c.dtsi +@@ -290,6 +290,27 @@ + status = "disabled"; + }; + ++ usbphyc: usbphyc@5a006000 { ++ #address-cells = <1>; ++ #size-cells = <0>; ++ #clock-cells = <0>; ++ compatible = "st,stm32mp1-usbphyc"; ++ reg = <0x5a006000 0x1000>; ++ clocks = <&rcc USBPHY_K>; ++ resets = <&rcc USBPHY_R>; ++ status = "disabled"; ++ ++ usbphyc_port0: usb-phy@0 { ++ #phy-cells = <0>; ++ reg = <0>; ++ }; ++ ++ usbphyc_port1: usb-phy@1 { ++ #phy-cells = <1>; ++ reg = <1>; ++ }; ++ }; ++ + usart1: serial@5c000000 { + compatible = "st,stm32h7-uart"; + reg = <0x5c000000 0x400>; +@@ -367,5 +388,22 @@ + compatible = "simple-bus", "syscon", "simple-mfd"; + reg = <0x5c00a000 0x400>; + }; ++ ++ cpu_opp_table: cpu0-opp-table { ++ compatible = "operating-points-v2"; ++ opp-shared; ++ ++ opp-650000000 { ++ opp-hz = /bits/ 64 <650000000>; ++ opp-microvolt = <1200000>; ++ opp-supported-hw = <0x1>; ++ }; ++ ++ opp-800000000 { ++ opp-hz = /bits/ 64 <800000000>; ++ opp-microvolt = <1350000>; ++ opp-supported-hw = <0x2>; ++ }; ++ }; + }; + }; +diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.c b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.c +index 69c3c74..5a02de1 100644 +--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.c ++++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.c +@@ -1,6 +1,6 @@ + // SPDX-License-Identifier: GPL-2.0+ BSD-3-Clause + /* +- * Copyright (C) 2017-2018, STMicroelectronics - All Rights Reserved ++ * Copyright (C) 2017-2019, STMicroelectronics - All Rights Reserved + */ + + #include +@@ -33,6 +33,22 @@ + #endif + + #define CLKSRC_TIMEOUT_US 200000U ++#define PLLRDY_TIMEOUT_US 200000U ++ ++/* PLL settings computation related definitions */ ++#define POST_DIVM_MIN 8000000 ++#define POST_DIVM_MAX 16000000 ++#define DIVM_MIN 0 ++#define DIVM_MAX 63 ++#define DIVN_MIN 24 ++#define DIVN_MAX 99 ++#define DIVP_MIN 0 ++#define DIVP_MAX 127 ++#define FRAC_MAX 8192 ++#define VCO_MIN 800000000 ++#define VCO_MAX 1600000000 ++ ++#define PLL1_SETTINGS_VALID_ID 0x504C4C31 /* "PLL1" */ + + enum stm32mp1_parent_id { + /* Oscillators are defined in enum stm32mp_osc_id */ +@@ -191,6 +207,15 @@ struct stm32mp1_clk_pll { + enum stm32mp_osc_id refclk[REFCLK_SIZE]; + }; + ++/* Compact structure of 32bit cells, copied raw when suspending */ ++struct __attribute__((__packed__)) stm32mp1_pll_settings { ++ uint32_t valid_id; ++ uint32_t freq[PLAT_MAX_OPP_NB]; ++ uint32_t volt[PLAT_MAX_OPP_NB]; ++ uint32_t cfg[PLAT_MAX_OPP_NB][PLAT_MAX_PLLCFG_NB]; ++ uint32_t frac[PLAT_MAX_OPP_NB]; ++}; ++ + /* Clocks with selectable source and not set/clr register access */ + #define _CLK_SELEC(off, b, idx, s) \ + { \ +@@ -364,6 +389,7 @@ static const struct stm32mp1_clk_gate stm32mp1_clk_gate[] = { + _CLK_SELEC(RCC_DBGCFGR, 8, CK_DBG, _UNKNOWN_SEL), + _CLK_SC_FIXED(RCC_MP_APB1ENSETR, 6, TIM12_K, _PCLK1), + _CLK_SC_FIXED(RCC_MP_APB2ENSETR, 2, TIM15_K, _PCLK2), ++ _CLK_SC_FIXED(RCC_MP_APB3ENSETR, 11, SYSCFG, _UNKNOWN_ID), + _CLK_SC_SELEC(RCC_MP_AHB6ENSETR, 0, MDMA, _UNKNOWN_SEL), + _CLK_SC_SELEC(RCC_MP_AHB6ENSETR, 5, GPU, _UNKNOWN_SEL), + _CLK_SC_FIXED(RCC_MP_AHB6ENSETR, 10, ETHMAC, _ACLK), +@@ -447,6 +473,20 @@ static const struct stm32mp1_clk_sel stm32mp1_clk_sel[_PARENT_SEL_NB] = { + _CLK_PARENT(_USBO_SEL, RCC_USBCKSELR, 4, 0x1, usbo_parents), + }; + ++/* Define characteristics of PLL according type */ ++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, +@@ -491,7 +531,7 @@ static const uint8_t stm32mp1_axi_div[8] = { + 1, 2, 3, 4, 4, 4, 4, 4 + }; + +-#if TRACE_LEVEL >= TRACE_DEBUG ++#if TRACE_LEVEL >= TRACE_FLOW + static const char *const __maybe_unused stm32mp1_clk_parent_name[_PARENT_NB] = { + [_HSI] = "HSI", + [_HSE] = "HSE", +@@ -534,6 +574,8 @@ static const char *const __maybe_unused stm32mp1_clk_parent_name[_PARENT_NB] = { + static unsigned long stm32mp1_osc[NB_OSC]; + static unsigned int gate_refcounts[NB_GATES]; + static unsigned int refcount_lock; ++static struct stm32mp1_pll_settings pll1_settings; ++static uint32_t current_opp_khz; + + static const struct stm32mp1_clk_gate *gate_ref(unsigned int idx) + { +@@ -606,6 +648,155 @@ static enum stm32mp1_parent_id stm32mp1_clk_get_fixed_parent(int i) + return (enum stm32mp1_parent_id)gate_ref(i)->fixed; + } + ++static void pll_start(enum stm32mp1_pll_id pll_id) ++{ ++ const struct stm32mp1_clk_pll *pll = pll_ref(pll_id); ++ uint32_t pllxcr = stm32_rcc_base() + pll->pllxcr; ++ ++ mmio_clrsetbits_32(pllxcr, ++ RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN | ++ RCC_PLLNCR_DIVREN, ++ RCC_PLLNCR_PLLON); ++} ++ ++static int pll_output(enum stm32mp1_pll_id pll_id, uint32_t output) ++{ ++ const struct stm32mp1_clk_pll *pll = pll_ref(pll_id); ++ uint32_t pllxcr = stm32_rcc_base() + pll->pllxcr; ++ uint64_t start; ++ ++ start = utimeout_init(PLLRDY_TIMEOUT_US); ++ /* Wait PLL lock */ ++ while ((mmio_read_32(pllxcr) & RCC_PLLNCR_PLLRDY) == 0U) { ++ if (utimeout_elapsed(PLLRDY_TIMEOUT_US, start)) { ++ EMSG("PLL%d start failed @ 0x%"PRIx32": 0x%"PRIx32, ++ pll_id, pllxcr, mmio_read_32(pllxcr)); ++ return -1; ++ } ++ } ++ ++ /* Start the requested output */ ++ mmio_setbits_32(pllxcr, output << RCC_PLLNCR_DIVEN_SHIFT); ++ ++ return 0; ++} ++ ++static int pll_stop(enum stm32mp1_pll_id pll_id) ++{ ++ const struct stm32mp1_clk_pll *pll = pll_ref(pll_id); ++ uint32_t pllxcr = stm32_rcc_base() + pll->pllxcr; ++ uint64_t 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 = utimeout_init(PLLRDY_TIMEOUT_US); ++ /* Wait PLL stopped */ ++ while ((mmio_read_32(pllxcr) & RCC_PLLNCR_PLLRDY) != 0U) { ++ if (utimeout_elapsed(PLLRDY_TIMEOUT_US, start)) { ++ EMSG("PLL%d stop failed @ 0x%"PRIx32": 0x%"PRIx32, ++ pll_id, pllxcr, mmio_read_32(pllxcr)); ++ ++ return -1; ++ } ++ } ++ ++ return 0; ++} ++ ++static uint32_t pll_compute_pllxcfgr2(uint32_t *pllcfg) ++{ ++ 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; ++ ++ return value; ++} ++ ++static void pll_config_output(enum stm32mp1_pll_id pll_id, uint32_t *pllcfg) ++{ ++ const struct stm32mp1_clk_pll *pll = pll_ref(pll_id); ++ uintptr_t rcc_base = stm32_rcc_base(); ++ uint32_t value; ++ ++ value = pll_compute_pllxcfgr2(pllcfg); ++ ++ mmio_write_32(rcc_base + pll->pllxcfgr2, value); ++} ++ ++static int pll_compute_pllxcfgr1(const struct stm32mp1_clk_pll *pll, ++ uint32_t *pllcfg, uint32_t *cfgr1) ++{ ++ uint32_t rcc_base = stm32_rcc_base(); ++ enum stm32mp1_plltype type = pll->plltype; ++ unsigned long refclk; ++ uint32_t ifrge = 0; ++ uint32_t src; ++ ++ src = mmio_read_32(rcc_base + pll->rckxselr) & ++ RCC_SELR_REFCLK_SRC_MASK; ++ ++ refclk = stm32mp1_clk_get_fixed(pll->refclk[src]) / ++ (pllcfg[PLLCFG_M] + 1U); ++ ++ if ((refclk < (stm32mp1_pll[type].refclk_min * 1000000U)) || ++ (refclk > (stm32mp1_pll[type].refclk_max * 1000000U))) { ++ return -1; ++ } ++ ++ if ((type == PLL_800) && (refclk >= 8000000U)) { ++ ifrge = 1U; ++ } ++ ++ *cfgr1 = (pllcfg[PLLCFG_N] << RCC_PLLNCFGR1_DIVN_SHIFT) & ++ RCC_PLLNCFGR1_DIVN_MASK; ++ *cfgr1 |= (pllcfg[PLLCFG_M] << RCC_PLLNCFGR1_DIVM_SHIFT) & ++ RCC_PLLNCFGR1_DIVM_MASK; ++ *cfgr1 |= (ifrge << RCC_PLLNCFGR1_IFRGE_SHIFT) & ++ RCC_PLLNCFGR1_IFRGE_MASK; ++ ++ return 0; ++} ++ ++static int pll_config(enum stm32mp1_pll_id pll_id, uint32_t *pllcfg, ++ uint32_t fracv) ++{ ++ const struct stm32mp1_clk_pll *pll = pll_ref(pll_id); ++ uint32_t rcc_base = stm32_rcc_base(); ++ uint32_t value; ++ int ret; ++ ++ ret = pll_compute_pllxcfgr1(pll, pllcfg, &value); ++ if (ret != 0) { ++ return ret; ++ } ++ ++ mmio_write_32(rcc_base + pll->pllxcfgr1, value); ++ ++ /* Fractional configuration */ ++ value = 0; ++ mmio_write_32(rcc_base + pll->pllxfracr, value); ++ ++ /* Frac must be enabled only once its configuration is loaded */ ++ value = fracv << RCC_PLLNFRACR_FRACV_SHIFT; ++ mmio_write_32(rcc_base + pll->pllxfracr, value); ++ value = mmio_read_32(rcc_base + pll->pllxfracr); ++ mmio_write_32(rcc_base + pll->pllxfracr, value | RCC_PLLNFRACR_FRACLE); ++ ++ pll_config_output(pll_id, pllcfg); ++ ++ return 0; ++} ++ + static int stm32mp1_set_clksrc(unsigned int clksrc) + { + uintptr_t address = stm32_rcc_base() + (clksrc >> 4); +@@ -617,7 +808,7 @@ static int stm32mp1_set_clksrc(unsigned int clksrc) + timeout_ref = utimeout_init(CLKSRC_TIMEOUT_US); + while ((mmio_read_32(address) & RCC_SELR_SRCRDY) == 0U) { + if (utimeout_elapsed(CLKSRC_TIMEOUT_US, timeout_ref)) { +- EMSG("CLKSRC %x start failed @ 0x%x: 0x%x\n", ++ EMSG("CLKSRC %x start failed @%"PRIxPTR": 0x%"PRIx32"\n", + clksrc, address, mmio_read_32(address)); + return -1; + } +@@ -1085,6 +1276,274 @@ unsigned long stm32mp1_clk_get_rate(unsigned long id) + } + + #ifdef CFG_DT ++static int clk_compute_pll1_settings(unsigned long input_freq, int idx) ++{ ++ unsigned long post_divm; ++ unsigned long long output_freq = pll1_settings.freq[idx] * 1000U; ++ unsigned long long freq; ++ unsigned long long vco; ++ int divm; ++ int divn; ++ int divp; ++ int frac; ++ int i; ++ unsigned int diff; ++ unsigned int best_diff = UINT_MAX; ++ ++ /* Following parameters have always the same value */ ++ pll1_settings.cfg[idx][PLLCFG_Q] = 0; ++ pll1_settings.cfg[idx][PLLCFG_R] = 0; ++ pll1_settings.cfg[idx][PLLCFG_O] = PQR(1, 0, 0); ++ ++ for (divm = DIVM_MAX; divm >= DIVM_MIN; divm--) { ++ post_divm = input_freq / (unsigned long)(divm + 1); ++ ++ if ((post_divm < POST_DIVM_MIN) || ++ (post_divm > POST_DIVM_MAX)) { ++ continue; ++ } ++ ++ for (divp = DIVP_MIN; divp <= DIVP_MAX; divp++) { ++ ++ freq = output_freq * (divm + 1) * (divp + 1); ++ ++ divn = (int)((freq / input_freq) - 1); ++ if ((divn < DIVN_MIN) || (divn > DIVN_MAX)) { ++ continue; ++ } ++ ++ frac = (int)(((freq * FRAC_MAX) / input_freq) - ++ ((divn + 1) * FRAC_MAX)); ++ ++ /* 2 loops to refine the fractional part */ ++ for (i = 2; i != 0; i--) { ++ if (frac > FRAC_MAX) { ++ break; ++ } ++ ++ vco = (post_divm * (divn + 1)) + ++ ((post_divm * (unsigned long long)frac) / ++ FRAC_MAX); ++ ++ if ((vco < (VCO_MIN / 2)) || ++ (vco > (VCO_MAX / 2))) { ++ frac++; ++ continue; ++ } ++ ++ freq = vco / (divp + 1); ++ if (output_freq < freq) { ++ diff = (unsigned int)(freq - ++ output_freq); ++ } else { ++ diff = (unsigned int)(output_freq - ++ freq); ++ } ++ ++ if (diff < best_diff) { ++ pll1_settings.cfg[idx][PLLCFG_M] = divm; ++ pll1_settings.cfg[idx][PLLCFG_N] = divn; ++ pll1_settings.cfg[idx][PLLCFG_P] = divp; ++ pll1_settings.frac[idx] = frac; ++ ++ if (diff == 0) { ++ return 0; ++ } ++ ++ best_diff = diff; ++ } ++ ++ frac++; ++ } ++ } ++ } ++ ++ if (best_diff == UINT_MAX) { ++ return -1; ++ } ++ ++ return 0; ++} ++ ++static int clk_get_pll1_settings(uint32_t clksrc, int index) ++{ ++ unsigned long input_freq; ++ unsigned int i; ++ ++ for (i = 0; i < PLAT_MAX_OPP_NB; i++) { ++ if (pll1_settings.freq[i] == pll1_settings.freq[index]) { ++ break; ++ } ++ } ++ ++ if (((i == PLAT_MAX_OPP_NB) && ++ !stm32mp1_clk_pll1_settings_are_valid()) || ++ ((i < PLAT_MAX_OPP_NB) && ++ (pll1_settings.cfg[i][PLLCFG_O] == 0U))) { ++ /* ++ * Either PLL1 settings structure is completely empty, ++ * or these settings are not yet computed: do it. ++ */ ++ switch (clksrc) { ++ case CLK_PLL12_HSI: ++ input_freq = stm32mp1_clk_get_rate(CK_HSI); ++ break; ++ case CLK_PLL12_HSE: ++ input_freq = stm32mp1_clk_get_rate(CK_HSE); ++ break; ++ default: ++ panic(); ++ } ++ ++ return clk_compute_pll1_settings(input_freq, index); ++ } ++ ++ if ((i < PLAT_MAX_OPP_NB) && ++ (pll1_settings.cfg[i][PLLCFG_O] != 0U)) { ++ /* ++ * Index is in range and PLL1 settings are computed: ++ * use content to answer to the request. ++ */ ++ memcpy(&pll1_settings.cfg[index][0], &pll1_settings.cfg[i][0], ++ sizeof(uint32_t) * PLAT_MAX_PLLCFG_NB); ++ pll1_settings.frac[index] = pll1_settings.frac[i]; ++ ++ return 0; ++ } ++ ++ return -1; ++} ++ ++static int clk_save_current_pll1_settings(uint32_t buck1_voltage) ++{ ++ const struct stm32mp1_clk_pll *pll = pll_ref(_PLL1); ++ uint32_t rcc_base = stm32_rcc_base(); ++ uint32_t freq; ++ unsigned int i; ++ ++ freq = UDIV_ROUND_NEAREST(stm32mp1_clk_get_rate(CK_MPU), 1000L); ++ ++ for (i = 0; i < PLAT_MAX_OPP_NB; i++) { ++ if (pll1_settings.freq[i] == freq) { ++ break; ++ } ++ } ++ ++ if ((i == PLAT_MAX_OPP_NB) || ++ ((pll1_settings.volt[i] != buck1_voltage) && ++ (buck1_voltage != 0U))) { ++ return -1; ++ } ++ ++ pll1_settings.cfg[i][PLLCFG_M] = ++ (mmio_read_32(rcc_base + pll->pllxcfgr1) & ++ RCC_PLLNCFGR1_DIVM_MASK) >> RCC_PLLNCFGR1_DIVM_SHIFT; ++ ++ pll1_settings.cfg[i][PLLCFG_N] = ++ (mmio_read_32(rcc_base + pll->pllxcfgr1) & ++ RCC_PLLNCFGR1_DIVN_MASK) >> RCC_PLLNCFGR1_DIVN_SHIFT; ++ ++ pll1_settings.cfg[i][PLLCFG_P] = ++ (mmio_read_32(rcc_base + pll->pllxcfgr2) & ++ RCC_PLLNCFGR2_DIVP_MASK) >> RCC_PLLNCFGR2_DIVP_SHIFT; ++ ++ pll1_settings.cfg[i][PLLCFG_Q] = ++ (mmio_read_32(rcc_base + pll->pllxcfgr2) & ++ RCC_PLLNCFGR2_DIVQ_MASK) >> RCC_PLLNCFGR2_DIVQ_SHIFT; ++ ++ pll1_settings.cfg[i][PLLCFG_R] = ++ (mmio_read_32(rcc_base + pll->pllxcfgr2) & ++ RCC_PLLNCFGR2_DIVR_MASK) >> RCC_PLLNCFGR2_DIVR_SHIFT; ++ ++ pll1_settings.cfg[i][PLLCFG_O] = ++ mmio_read_32(rcc_base + pll->pllxcr) >> ++ RCC_PLLNCR_DIVEN_SHIFT; ++ ++ pll1_settings.frac[i] = ++ (mmio_read_32(rcc_base + pll->pllxfracr) & ++ RCC_PLLNFRACR_FRACV_MASK) >> RCC_PLLNFRACR_FRACV_SHIFT; ++ ++ return i; ++} ++ ++static uint32_t stm32mp1_clk_get_pll1_current_clksrc(void) ++{ ++ uint32_t value; ++ const struct stm32mp1_clk_pll *pll = pll_ref(_PLL1); ++ uint32_t rcc_base = stm32_rcc_base(); ++ ++ value = mmio_read_32(rcc_base + pll->rckxselr); ++ ++ switch (value & RCC_SELR_REFCLK_SRC_MASK) { ++ case 0: ++ return CLK_PLL12_HSI; ++ case 1: ++ return CLK_PLL12_HSE; ++ default: ++ panic(); ++ } ++} ++ ++int stm32mp1_clk_compute_all_pll1_settings(uint32_t buck1_voltage) ++{ ++ unsigned int i; ++ int ret; ++ int index; ++ uint32_t count = PLAT_MAX_OPP_NB; ++ uint32_t clksrc; ++ void *fdt; ++ ++ fdt = get_dt_blob(); ++ if (fdt == NULL) { ++ panic(); ++ } ++ ++ ret = fdt_get_all_opp_freqvolt(fdt, &count, pll1_settings.freq, ++ pll1_settings.volt); ++ switch (ret) { ++ case 0: ++ break; ++ case -FDT_ERR_NOTFOUND: ++ DMSG("Cannot find all OPP info in DT: use default settings"); ++ return 0; ++ default: ++ EMSG("Inconsistent OPP settings found in DT, ignored."); ++ return 0; ++ } ++ ++ index = clk_save_current_pll1_settings(buck1_voltage); ++ ++ clksrc = stm32mp1_clk_get_pll1_current_clksrc(); ++ ++ for (i = 0; i < count; i++) { ++ if ((index >= 0) && (i == (unsigned int)index)) ++ continue; ++ ++ ret = clk_get_pll1_settings(clksrc, i); ++ if (ret) ++ return ret; ++ } ++ ++ pll1_settings.valid_id = PLL1_SETTINGS_VALID_ID; ++ ++ return 0; ++} ++ ++void stm32mp1_clk_lp_save_opp_pll1_settings(uint8_t *data, size_t size) ++{ ++ if ((size != sizeof(pll1_settings)) || ++ !stm32mp1_clk_pll1_settings_are_valid()) { ++ panic(); ++ } ++ ++ memcpy(data, &pll1_settings, size); ++} ++ ++bool stm32mp1_clk_pll1_settings_are_valid(void) ++{ ++ return pll1_settings.valid_id == PLL1_SETTINGS_VALID_ID; ++} ++ + static void stm32mp1_osc_clk_init(const char *name, + enum stm32mp_osc_id index) + { +@@ -1113,6 +1572,21 @@ static void stm32mp1_osc_init(void) + DMSG("Osc %s frequency: %lu", name[i], stm32mp1_osc[i]); + } + } ++#else /* CFG_DT */ ++int stm32mp1_clk_compute_all_pll1_settings(uint32_t buck1_voltage __unused) ++{ ++ return 0; ++} ++ ++void stm32mp1_clk_lp_save_opp_pll1_settings(uint8_t *data __unused, ++ size_t size __unused) ++{ ++} ++ ++bool stm32mp1_clk_pll1_settings_are_valid(void) ++{ ++ return false; ++} + #endif + + /* +@@ -1508,16 +1982,187 @@ static void sync_earlyboot_clocks_state(void) + stm32mp_register_clock_parents_secure(BKPSRAM); + + stm32mp_register_clock_parents_secure(RTCAPB); +- +-#if CFG_TEE_CORE_NB_CORE > 1 + stm32mp1_clk_enable_secure(RTCAPB); +-#endif + + /* The low power sequences mandates RNG1 and CRYP1 support */ + stm32mp_register_clock_parents_secure(RNG1_K); + stm32mp_register_clock_parents_secure(CRYP1); + } + ++/* ++ * Check if PLL1 can be configured on the fly. ++ * @result (-1) => config on the fly is not possible. ++ * (0) => config on the fly is possible. ++ * (+1) => same parameters as those in place, no need to reconfig. ++ * Return value is 0 if no error. ++ */ ++static int is_pll_config_on_the_fly(enum stm32mp1_pll_id pll_id, ++ uint32_t *pllcfg, uint32_t fracv, ++ int *result) ++{ ++ const struct stm32mp1_clk_pll *pll = pll_ref(pll_id); ++ uintptr_t rcc_base = stm32_rcc_base(); ++ uint32_t fracr; ++ uint32_t value; ++ int ret; ++ ++ ret = pll_compute_pllxcfgr1(pll, pllcfg, &value); ++ if (ret != 0) { ++ return ret; ++ } ++ ++ if (mmio_read_32(rcc_base + pll->pllxcfgr1) != value) { ++ /* Different DIVN/DIVM, can't config on the fly */ ++ *result = -1; ++ return 0; ++ } ++ ++ fracr = fracv << RCC_PLLNFRACR_FRACV_SHIFT; ++ fracr |= RCC_PLLNFRACR_FRACLE; ++ value = pll_compute_pllxcfgr2(pllcfg); ++ ++ if ((mmio_read_32(rcc_base + pll->pllxfracr) == fracr) && ++ (mmio_read_32(rcc_base + pll->pllxcfgr2) == value)) { ++ /* Same parameters, no need to config */ ++ *result = 1; ++ } else { ++ *result = 0; ++ } ++ ++ return 0; ++} ++ ++/* Configure PLL1 from input frequency OPP parameters */ ++static int pll1_config_from_opp_khz(uint32_t freq_khz) ++{ ++ unsigned int i; ++ int ret; ++ int config_on_the_fly = -1; ++ ++ for (i = 0; i < PLAT_MAX_OPP_NB; i++) { ++ if (pll1_settings.freq[i] == freq_khz) { ++ break; ++ } ++ } ++ ++ if (i == PLAT_MAX_OPP_NB) { ++ return -1; ++ } ++ ++ ret = is_pll_config_on_the_fly(_PLL1, &pll1_settings.cfg[i][0], ++ pll1_settings.frac[i], ++ &config_on_the_fly); ++ if (ret != 0) { ++ return ret; ++ } ++ ++ if (config_on_the_fly == 1) { ++ /* No need to reconfig, setup already OK */ ++ return 0; ++ } ++ ++ if (config_on_the_fly == -1) { ++ /* Switch to HSI and stop PLL1 before reconfiguration */ ++ ret = stm32mp1_set_clksrc(CLK_MPU_HSI); ++ if (ret != 0) { ++ return ret; ++ } ++ ++ ret = pll_stop(_PLL1); ++ if (ret != 0) { ++ return ret; ++ } ++ } ++ ++ ret = pll_config(_PLL1, &pll1_settings.cfg[i][0], ++ pll1_settings.frac[i]); ++ if (ret != 0) { ++ return ret; ++ } ++ ++ if (config_on_the_fly == -1) { ++ /* Start PLL1 and switch back to after reconfiguration */ ++ pll_start(_PLL1); ++ ++ ret = pll_output(_PLL1, pll1_settings.cfg[i][PLLCFG_O]); ++ if (ret != 0) { ++ return ret; ++ } ++ ++ ret = stm32mp1_set_clksrc(CLK_MPU_PLL1P); ++ if (ret != 0) { ++ return ret; ++ } ++ } ++ ++ return 0; ++} ++ ++int stm32mp1_set_opp_khz(uint32_t freq_khz) ++{ ++ if (freq_khz == current_opp_khz) { ++ /* OPP already set, nothing to do */ ++ return 0; ++ } ++ ++ if (!stm32mp1_clk_pll1_settings_are_valid()) { ++ /* ++ * No OPP table in DT or an error occurred during PLL1 ++ * settings computation, system can only work on current ++ * operating point so return error. ++ */ ++ return -1; ++ } ++ ++ /* Check that PLL1 (without MPUDIV) is MPU clock source */ ++ if (((mmio_read_32(stm32_rcc_base() + RCC_MPCKSELR) & ++ RCC_SELR_SRC_MASK)) != RCC_MPCKSELR_PLL) { ++ return -1; ++ } ++ ++ if (pll1_config_from_opp_khz(freq_khz) != 0) { ++ /* Restore original value */ ++ if (pll1_config_from_opp_khz(current_opp_khz) != 0) { ++ EMSG("No CPU operating point can be set"); ++ panic(); ++ } ++ ++ return -1; ++ } ++ ++ current_opp_khz = freq_khz; ++ ++ return 0; ++} ++ ++int stm32mp1_round_opp_khz(uint32_t *freq_khz) ++{ ++ unsigned int i; ++ uint32_t round_opp = 0U; ++ ++ if (!stm32mp1_clk_pll1_settings_are_valid()) { ++ /* ++ * No OPP table in DT, or an error occurred during PLL1 ++ * settings computation, system can only work on current ++ * operating point, so return current CPU frequency. ++ */ ++ *freq_khz = current_opp_khz; ++ ++ return 0; ++ } ++ ++ for (i = 0; i < PLAT_MAX_OPP_NB; i++) { ++ if ((pll1_settings.freq[i] <= *freq_khz) && ++ (pll1_settings.freq[i] > round_opp)) { ++ round_opp = pll1_settings.freq[i]; ++ } ++ } ++ ++ *freq_khz = round_opp; ++ ++ return 0; ++} ++ + static void _clock_mpu_suspend(void) + { + uintptr_t mpckselr = stm32_rcc_base() + RCC_MPCKSELR; +@@ -1582,10 +2227,22 @@ void stm32mp_clock_suspend_resume(enum pm_op op) + + static TEE_Result stm32mp1_clk_probe(void) + { ++ unsigned long freq_khz; ++ ++ assert(PLLCFG_NB == PLAT_MAX_PLLCFG_NB); ++ + init_clock_tree_from_dt(); + + sync_earlyboot_clocks_state(); + ++ /* Save current CPU operating point value */ ++ freq_khz = UDIV_ROUND_NEAREST(stm32mp1_clk_get_rate(CK_MPU), 1000UL); ++ if (freq_khz > (unsigned long)UINT32_MAX) { ++ panic(); ++ } ++ ++ current_opp_khz = (uint32_t)freq_khz; ++ + return TEE_SUCCESS; + } + /* Setup clock support before driver initialization */ +diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.h b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.h +index 783d81d..26fda3d 100644 +--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.h ++++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.h +@@ -1,6 +1,6 @@ + /* SPDX-License-Identifier: BSD-3-Clause */ + /* +- * Copyright (C) 2018, STMicroelectronics - All Rights Reserved ++ * Copyright (C) 2018-2019, STMicroelectronics - All Rights Reserved + */ + + #ifndef __STM32MP1_CLK_H__ +@@ -22,6 +22,10 @@ enum stm32mp_osc_id { + _UNKNOWN_OSC_ID = 0xFF + }; + ++int stm32mp1_clk_compute_all_pll1_settings(uint32_t buck1_voltage); ++void stm32mp1_clk_lp_save_opp_pll1_settings(uint8_t *data, size_t size); ++bool stm32mp1_clk_pll1_settings_are_valid(void); ++ + void __stm32mp1_clk_enable(unsigned long id, bool caller_is_secure); + void __stm32mp1_clk_disable(unsigned long id, bool caller_is_secure); + bool stm32mp1_clk_is_enabled(unsigned long id); +@@ -56,6 +60,9 @@ void stm32mp_register_clock_parents_secure(unsigned long id); + + void stm32mp_update_earlyboot_clocks_state(void); + ++int stm32mp1_set_opp_khz(uint32_t freq_khz); ++int stm32mp1_round_opp_khz(uint32_t *freq_khz); ++ + void stm32mp1_clock_suspend(void); + void stm32mp1_clock_resume(void); + +diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clkfunc.c b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clkfunc.c +index c83d561..ea57c9a 100644 +--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clkfunc.c ++++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clkfunc.c +@@ -1,6 +1,6 @@ + // SPDX-License-Identifier: BSD-3-Clause + /* +- * Copyright (c) 2017-2018, STMicroelectronics - All Rights Reserved ++ * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved + */ + + #include +@@ -55,6 +55,11 @@ int fdt_osc_read_freq(void *fdt, const char *name, uint32_t *freq) + if (strncmp(cchar, name, (size_t)ret) == 0) { + const fdt32_t *cuint; + ++ if (_fdt_get_status(fdt, subnode) == ++ DT_STATUS_DISABLED) { ++ goto exit; ++ } ++ + cuint = fdt_getprop(fdt, subnode, "clock-frequency", + &ret); + if (cuint == NULL) { +@@ -67,7 +72,8 @@ int fdt_osc_read_freq(void *fdt, const char *name, uint32_t *freq) + } + } + +- /* Oscillator not found, freq=0 */ ++exit: ++ /* Oscillator not found or disabled, freq=0 */ + *freq = 0; + return 0; + } +@@ -196,7 +202,7 @@ uint32_t fdt_rcc_read_addr(void *fdt) + ******************************************************************************/ + int fdt_get_rcc_node(void *fdt) + { +- return fdt_node_offset_by_compatible(fdt, -1, DT_RCC_CLK_COMPAT); ++ return fdt_get_node_by_compatible(fdt, DT_RCC_CLK_COMPAT); + } + + /******************************************************************************* +@@ -272,8 +278,6 @@ const fdt32_t *fdt_rcc_read_prop(void *fdt, const char *prop_name, int *lenp) + ******************************************************************************/ + uintptr_t get_stgen_base(void) + { +- int node; +- const fdt32_t *cuint; + void *fdt; + + fdt = get_dt_blob(); +@@ -281,17 +285,7 @@ uintptr_t get_stgen_base(void) + 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); ++ return fdt_get_peripheral_base(fdt, DT_STGEN_COMPAT); + } + + /******************************************************************************* +@@ -305,36 +299,22 @@ unsigned long get_uart_clock_freq(uint32_t instance) + { + int node; + void *fdt; ++ int clk_id; + + fdt = get_dt_blob(); + if (fdt == NULL) { + return 0; + } + +- /* Check for UART nodes */ +- node = fdt_node_offset_by_compatible(fdt, -1, DT_UART_COMPAT); +- while (node != -FDT_ERR_NOTFOUND) { +- const fdt32_t *cuint; +- +- cuint = fdt_getprop(fdt, node, "reg", NULL); +- if (cuint == NULL) +- goto next; +- +- if ((uint32_t)fdt32_to_cpu(*cuint) == instance) { +- unsigned long clk_id; +- +- cuint = fdt_getprop(fdt, node, "clocks", NULL); +- if (cuint == NULL) +- goto next; +- +- cuint++; +- clk_id = (unsigned long)(fdt32_to_cpu(*cuint)); ++ node = fdt_match_instance_by_compatible(fdt, DT_UART_COMPAT, instance); ++ if (node < 0) { ++ return 0; ++ } + +- return stm32mp1_clk_get_rate(clk_id); +- } +-next: +- node = fdt_node_offset_by_compatible(fdt, node, DT_UART_COMPAT); ++ clk_id = fdt_get_clock_id(fdt, node); ++ if (clk_id < 0) { ++ return 0; + } + +- return 0; ++ return stm32mp1_clk_get_rate((unsigned long)clk_id); + } +diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_ddrc.c b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_ddrc.c +index 03ded28..8bb2635 100644 +--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_ddrc.c ++++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_ddrc.c +@@ -102,6 +102,14 @@ static void do_sw_ack(void) + } + } + ++static bool ddr_supports_ssr_asr(void) ++{ ++ uintptr_t ddrctrl_base = get_ddrctrl_base(); ++ uint32_t mstr = mmio_read_32(ddrctrl_base + DDRCTRL_MSTR); ++ ++ return (mstr & DDRCTRL_MSTR_LPDDR2) != 0U; ++} ++ + static int ddr_sw_self_refresh_in(void) + { + uint64_t to_ref; +@@ -177,6 +185,9 @@ static int ddr_sw_self_refresh_in(void) + DDRPHYC_ACIOCR_CSPDD_MASK, + DDRPHYC_ACIOCR_CSPDD_0); + ++ /* Disable command/address output driver */ ++ mmio_clrbits_32(ddrphy_base + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACOE); ++ + mmio_setbits_32(ddrphy_base + DDRPHYC_DXCCR, DDRPHYC_DXCCR_DXPDD); + + mmio_setbits_32(ddrphy_base + DDRPHYC_DXCCR, DDRPHYC_DXCCR_DXPDR); +@@ -321,6 +332,9 @@ int ddr_sw_self_refresh_exit(void) + /* Enable pad drivers */ + mmio_clrbits_32(ddrphy_base + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACPDD); + ++ /* Enable command/address output driver */ ++ mmio_setbits_32(ddrphy_base + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACOE); ++ + mmio_clrbits_32(ddrphy_base + DDRPHYC_ACIOCR, + DDRPHYC_ACIOCR_CKPDD_MASK); + +@@ -398,6 +412,9 @@ void ddr_sr_mode_ssr(void) + uintptr_t rcc_ddritfcr = stm32_rcc_base() + RCC_DDRITFCR; + uintptr_t ddrctrl_base = get_ddrctrl_base(); + ++ if (!ddr_supports_ssr_asr()) ++ return; ++ + mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRC1LPEN); + + mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRC2LPEN); +@@ -446,6 +463,9 @@ void ddr_sr_mode_asr(void) + uintptr_t rcc_ddritfcr = stm32_rcc_base() + RCC_DDRITFCR; + uintptr_t ddrctrl_base = get_ddrctrl_base(); + ++ if (!ddr_supports_ssr_asr()) ++ return; ++ + mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_AXIDCGEN); + + mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRC1LPEN); +diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_ddrc.h b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_ddrc.h +index 59014b4..4afa313 100644 +--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_ddrc.h ++++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_ddrc.h +@@ -162,6 +162,7 @@ + #define DDRPHYC_PTR0_TITMSRST_OFFSET 18 + #define DDRPHYC_PTR0_TITMSRST_MASK GENMASK_32(21, 18) + ++#define DDRPHYC_ACIOCR_ACOE BIT(1) + #define DDRPHYC_ACIOCR_ACPDD BIT(3) + #define DDRPHYC_ACIOCR_ACPDR BIT(4) + #define DDRPHYC_ACIOCR_CKPDD_MASK GENMASK_32(10, 8) +diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.c b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.c +index f2b7fe1..82225bd 100644 +--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.c ++++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.c +@@ -1,6 +1,6 @@ + // SPDX-License-Identifier: BSD-3-Clause + /* +- * Copyright (c) 2017-2018, STMicroelectronics - All Rights Reserved ++ * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved + */ + + #include +@@ -23,15 +23,6 @@ + #include + #include + +-#define STPMIC1_LDO12356_OUTPUT_MASK (uint8_t)(GENMASK_32(6, 2)) +-#define STPMIC1_LDO12356_OUTPUT_SHIFT 2 +-#define STPMIC1_LDO3_MODE (uint8_t)(BIT(7)) +-#define STPMIC1_LDO3_DDR_SEL 31U +-#define STPMIC1_LDO3_1800000 (9U << STPMIC1_LDO12356_OUTPUT_SHIFT) +- +-#define STPMIC1_BUCK_OUTPUT_SHIFT 2 +-#define STPMIC1_BUCK3_1V8 (39U << STPMIC1_BUCK_OUTPUT_SHIFT) +- + #define MODE_STANDBY 8U + + #define STPMIC1_DEFAULT_START_UP_DELAY_MS 1 +@@ -46,10 +37,28 @@ bool stm32mp_with_pmic(void) + + static int dt_get_pmic_node(void *fdt) + { +- return fdt_node_offset_by_compatible(fdt, -1, "st,stpmic1"); ++ static int node = -FDT_ERR_BADOFFSET; ++ ++ if (node == -FDT_ERR_BADOFFSET) { ++ node = fdt_get_node_by_compatible(fdt, "st,stpmic1"); ++ } ++ ++ return node; ++} ++ ++static int dt_get_regulators_node(void *fdt) ++{ ++ int node; ++ ++ node = dt_get_pmic_node(fdt); ++ if (node < 0) { ++ panic(); ++ } ++ ++ return fdt_subnode_offset(fdt, node, "regulators"); + } + +-static int dt_pmic_status(void) ++int stm32mp_dt_pmic_status(void) + { + void *fdt = get_dt_blob(); + +@@ -66,7 +75,7 @@ static int dt_pmic_status(void) + + static bool dt_pmic_is_secure(void) + { +- int status = dt_pmic_status(); ++ int status = stm32mp_dt_pmic_status(); + + return ((unsigned)status == DT_STATUS_OK_SEC) && + (i2c_handle.dt_status == DT_STATUS_OK_SEC); +@@ -92,7 +101,7 @@ static size_t regu_bo_count; + + static int save_boot_on_config(void) + { +- int pmic_node, regulators_node, regulator_node; ++ int regulators_node, subnode; + void *fdt; + + assert(!regu_bo_config && !regu_bo_count); +@@ -102,42 +111,40 @@ static int save_boot_on_config(void) + panic(); + } + +- pmic_node = dt_get_pmic_node(fdt); +- if (pmic_node < 0) { +- panic(); +- } +- +- regulators_node = fdt_subnode_offset(fdt, pmic_node, "regulators"); ++ regulators_node = dt_get_regulators_node(fdt); + +- fdt_for_each_subnode(regulator_node, fdt, regulators_node) { ++ fdt_for_each_subnode(subnode, fdt, regulators_node) { + const fdt32_t *cuint; + const char *name; + struct regu_bo_config regu_cfg; + uint16_t mv; + +- if (fdt_getprop(fdt, regulator_node, "regulator-boot-on", +- NULL) == NULL) { ++ if ((fdt_getprop(fdt, subnode, "regulator-boot-on", NULL) == ++ NULL) && ++ (fdt_getprop(fdt, subnode, "regulator-always-on", NULL) == ++ NULL)) { + continue; + } + + memset(®u_cfg, 0, sizeof(regu_cfg)); +- name = fdt_get_name(fdt, regulator_node, NULL); ++ name = fdt_get_name(fdt, subnode, NULL); + + regu_cfg.flags |= REGU_BO_FLAG_ENABLE_REGU; ++ stpmic1_bo_enable_cfg(name, ®u_cfg.cfg); + +- if (fdt_getprop(fdt, regulator_node, "regulator-pull-down", ++ if (fdt_getprop(fdt, subnode, "regulator-pull-down", + NULL) != NULL) { + stpmic1_bo_pull_down_cfg(name, ®u_cfg.cfg); + regu_cfg.flags |= REGU_BO_FLAG_PULL_DOWN; + } + +- if (fdt_getprop(fdt, regulator_node, "st,mask-reset", ++ if (fdt_getprop(fdt, subnode, "st,mask-reset", + NULL) != NULL) { + stpmic1_bo_mask_reset_cfg(name, ®u_cfg.cfg); + regu_cfg.flags |= REGU_BO_FLAG_MASK_RESET; + } + +- cuint = fdt_getprop(fdt, regulator_node, ++ cuint = fdt_getprop(fdt, subnode, + "regulator-min-microvolt", NULL); + if (cuint != NULL) { + /* DT uses microvolts, whereas driver awaits millivolts */ +@@ -246,7 +253,7 @@ static unsigned int regu_lp_state2idx(const char *name) + + static int save_low_power_config(const char *lp_state) + { +- int pmic_node, regulators_node, regulator_node; ++ int regulators_node, subnode; + void *fdt; + unsigned int state_idx = regu_lp_state2idx(lp_state); + struct regu_lp_state *state = ®u_lp_state[state_idx]; +@@ -258,14 +265,9 @@ static int save_low_power_config(const char *lp_state) + panic(); + } + +- pmic_node = dt_get_pmic_node(fdt); +- if (pmic_node < 0) { +- return -FDT_ERR_NOTFOUND; +- } +- +- regulators_node = fdt_subnode_offset(fdt, pmic_node, "regulators"); ++ regulators_node = dt_get_regulators_node(fdt); + +- fdt_for_each_subnode(regulator_node, fdt, regulators_node) { ++ fdt_for_each_subnode(subnode, fdt, regulators_node) { + const fdt32_t *cuint; + const char *reg_name; + int regulator_state_node; +@@ -282,7 +284,7 @@ static int save_low_power_config(const char *lp_state) + + memset(regu_cfg, 0, sizeof(*regu_cfg)); + +- reg_name = fdt_get_name(fdt, regulator_node, NULL); ++ reg_name = fdt_get_name(fdt, subnode, NULL); + + if (stpmic1_lp_cfg(reg_name, ®u_cfg->cfg) != 0) { + EMSG("Invalid regu name %s", reg_name); +@@ -298,7 +300,7 @@ static int save_low_power_config(const char *lp_state) + + /* Then apply configs from regulator_state_node */ + regulator_state_node = fdt_subnode_offset(fdt, +- regulator_node, ++ subnode, + lp_state); + if (regulator_state_node <= 0) { + continue; +diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.h b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.h +index 01f15fd..4d26e70 100644 +--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.h ++++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.h +@@ -1,6 +1,6 @@ + /* SPDX-License-Identifier: BSD-3-Clause */ + /* +- * Copyright (c) 2017-2018, STMicroelectronics - All Rights Reserved ++ * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved + */ + + #ifndef __STM32MP1_PMIC_H__ +@@ -13,6 +13,7 @@ void stm32mp_pmic_apply_boot_on_config(void); + void stm32mp_pmic_apply_lp_config(const char *lp_state); + void stm32mp_get_pmic(void); + void stm32mp_put_pmic(void); ++int stm32mp_dt_pmic_status(void); + #else + static inline void stm32mp_pmic_apply_boot_on_config(void) + { +@@ -28,6 +29,10 @@ static inline void stm32mp_put_pmic(void) + { + panic(); + } ++static inline int stm32mp_dt_pmic_status(void) ++{ ++ return -1; ++} + #endif + + #endif /*__STM32MP1_PMIC_H__*/ +diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_syscfg.c b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_syscfg.c +new file mode 100644 +index 0000000..ccc5352 +--- /dev/null ++++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_syscfg.c +@@ -0,0 +1,97 @@ ++// SPDX-License-Identifier: BSD-3-Clause ++/* ++ * Copyright (c) 2019, STMicroelectronics - All Rights Reserved ++ */ ++ ++#include ++#include ++#include ++#include ++#include ++ ++/* ++ * SYSCFG register offsets (base relative) ++ */ ++#define SYSCFG_CMPCR 0x20U ++#define SYSCFG_CMPENSETR 0x24U ++ ++/* ++ * SYSCFG_CMPCR Register ++ */ ++#define SYSCFG_CMPCR_SW_CTRL BIT(1) ++#define SYSCFG_CMPCR_READY BIT(8) ++#define SYSCFG_CMPCR_RANSRC GENMASK_32(19, 16) ++#define SYSCFG_CMPCR_RANSRC_SHIFT 16 ++#define SYSCFG_CMPCR_RAPSRC GENMASK_32(23, 20) ++#define SYSCFG_CMPCR_ANSRC_SHIFT 24 ++ ++#define SYSCFG_CMPCR_READY_TIMEOUT_US 1000U ++ ++/* ++ * SYSCFG_CMPENSETR Register ++ */ ++#define SYSCFG_CMPENSETR_MPU_EN BIT(0) ++ ++void stm32mp1_syscfg_enable_io_compensation(void) ++{ ++ uintptr_t syscfg_base = stm32_get_syscfg_base(); ++ uint64_t start; ++ ++ /* ++ * Activate automatic I/O compensation. ++ * Warning: need to ensure CSI enabled and ready in clock driver. ++ * Enable non-secure clock, we assume non-secure is suspended. ++ */ ++ stm32mp1_clk_enable_non_secure(SYSCFG); ++ ++ mmio_setbits_32(syscfg_base + SYSCFG_CMPENSETR, ++ SYSCFG_CMPENSETR_MPU_EN); ++ ++ start = utimeout_init(SYSCFG_CMPCR_READY_TIMEOUT_US); ++ ++ while ((mmio_read_32(syscfg_base + SYSCFG_CMPCR) & ++ SYSCFG_CMPCR_READY) == 0U) { ++ if (utimeout_elapsed(SYSCFG_CMPCR_READY_TIMEOUT_US, start)) { ++ EMSG("IO compensation cell not ready"); ++ break; ++ } ++ } ++ ++ mmio_clrbits_32(syscfg_base + SYSCFG_CMPCR, SYSCFG_CMPCR_SW_CTRL); ++ ++ DMSG("[0x%"PRIxPTR"] SYSCFG.cmpcr = 0x%"PRIx32, ++ syscfg_base + SYSCFG_CMPCR, ++ mmio_read_32(syscfg_base + SYSCFG_CMPCR)); ++} ++ ++void stm32mp1_syscfg_disable_io_compensation(void) ++{ ++ uintptr_t syscfg_base = stm32_get_syscfg_base(); ++ uint32_t value; ++ ++ /* ++ * Deactivate automatic I/O compensation. ++ * Warning: CSI is disabled automatically in STOP if not ++ * requested for other usages and always OFF in STANDBY. ++ * Disable non-secure SYSCFG clock, we assume non-secure is suspended. ++ */ ++ value = mmio_read_32(syscfg_base + SYSCFG_CMPCR) >> ++ SYSCFG_CMPCR_ANSRC_SHIFT; ++ ++ mmio_clrbits_32(syscfg_base + SYSCFG_CMPCR, ++ SYSCFG_CMPCR_RANSRC | SYSCFG_CMPCR_RAPSRC); ++ ++ value = mmio_read_32(syscfg_base + SYSCFG_CMPCR) | ++ (value << SYSCFG_CMPCR_RANSRC_SHIFT); ++ ++ mmio_write_32(syscfg_base + SYSCFG_CMPCR, value | SYSCFG_CMPCR_SW_CTRL); ++ ++ DMSG("[0x%"PRIxPTR"] SYSCFG.cmpcr = 0x%"PRIx32, ++ syscfg_base + SYSCFG_CMPCR, ++ mmio_read_32(syscfg_base + SYSCFG_CMPCR)); ++ ++ mmio_clrbits_32(syscfg_base + SYSCFG_CMPENSETR, ++ SYSCFG_CMPENSETR_MPU_EN); ++ ++ stm32mp1_clk_disable_non_secure(SYSCFG); ++} +diff --git a/core/arch/arm/plat-stm32mp1/drivers/sub.mk b/core/arch/arm/plat-stm32mp1/drivers/sub.mk +index c668efa..0c5f3bc 100644 +--- a/core/arch/arm/plat-stm32mp1/drivers/sub.mk ++++ b/core/arch/arm/plat-stm32mp1/drivers/sub.mk +@@ -6,3 +6,4 @@ srcs-$(CFG_STPMIC1) += stm32mp1_pmic.c + srcs-y += stm32mp1_clk.c + srcs-$(CFG_DT) += stm32mp1_clkfunc.c + srcs-y += stm32mp1_ddrc.c ++srcs-y += stm32mp1_syscfg.c +diff --git a/core/arch/arm/plat-stm32mp1/main.c b/core/arch/arm/plat-stm32mp1/main.c +index b501541..08deeea 100644 +--- a/core/arch/arm/plat-stm32mp1/main.c ++++ b/core/arch/arm/plat-stm32mp1/main.c +@@ -1,6 +1,6 @@ + // SPDX-License-Identifier: BSD-2-Clause + /* +- * Copyright (c) 2017-2018, STMicroelectronics ++ * Copyright (c) 2017-2019, STMicroelectronics + * Copyright (c) 2016-2018, Linaro Limited + */ + +@@ -13,7 +13,9 @@ + #include + #include + #include ++#include + #include ++#include + #include + #include + #include +@@ -47,6 +49,7 @@ register_phys_mem(MEM_AREA_IO_NSEC, RNG1_BASE, SMALL_PAGE_SIZE); + register_phys_mem(MEM_AREA_IO_NSEC, IWDG1_BASE, SMALL_PAGE_SIZE); + register_phys_mem(MEM_AREA_IO_NSEC, IWDG2_BASE, SMALL_PAGE_SIZE); + register_phys_mem(MEM_AREA_IO_NSEC, RTC_BASE, SMALL_PAGE_SIZE); ++register_phys_mem(MEM_AREA_IO_NSEC, SYSCFG_BASE, SMALL_PAGE_SIZE); + #ifdef CFG_WITH_NSEC_UARTS + register_phys_mem(MEM_AREA_IO_NSEC, USART1_BASE, SMALL_PAGE_SIZE); + register_phys_mem(MEM_AREA_IO_NSEC, USART2_BASE, SMALL_PAGE_SIZE); +@@ -237,6 +240,38 @@ static TEE_Result stm32_uart_console_probe(void) + return TEE_SUCCESS; + } + service_init_late(stm32_uart_console_probe); ++ ++/* Compute PLL1 settings once PMIC init is completed */ ++static TEE_Result initialize_pll1_settings(void) ++{ ++ uint32_t vddcore_voltage = 0U; ++ int ret; ++ ++ if (stm32mp1_clk_pll1_settings_are_valid()) { ++ return TEE_SUCCESS; ++ } ++ ++ if (stm32mp_dt_pmic_status() > 0) { ++ stm32mp_get_pmic(); ++ ++ ret = stpmic1_regulator_voltage_get("buck1"); ++ if (ret < 0) { ++ panic(); ++ } ++ ++ vddcore_voltage = (uint32_t)ret; ++ ++ stm32mp_put_pmic(); ++ } ++ ++ if (stm32mp1_clk_compute_all_pll1_settings(vddcore_voltage) != 0) { ++ panic(); ++ } ++ ++ return TEE_SUCCESS; ++} ++driver_init_late(initialize_pll1_settings); ++ + #endif + + /* +@@ -360,15 +395,39 @@ bool sm_platform_handler(struct sm_ctx *ctx) + } + } + ++static uintptr_t stm32_dbgmcu_base(void) ++{ ++ static void *va; ++ ++ if (!cpu_mmu_enabled()) ++ return DBGMCU_BASE; ++ ++ if (!va) ++ va = phys_to_virt(DBGMCU_BASE, MEM_AREA_IO_SEC); ++ ++ return (uintptr_t)va; ++} ++ + /* SoC versioning util */ +-uint32_t stm32mp1_dbgmcu_get_chip_version(void) ++int stm32mp1_dbgmcu_get_chip_version(uint32_t *chip_version) + { +- uintptr_t base = DBGMCU_BASE; ++ assert(chip_version != NULL); + +- if (cpu_mmu_enabled()) +- base = (uintptr_t)phys_to_virt(DBGMCU_BASE, MEM_AREA_IO_SEC); ++ *chip_version = (read32(stm32_dbgmcu_base() + DBGMCU_IDC) & ++ DBGMCU_IDC_REV_ID_MASK) >> DBGMCU_IDC_REV_ID_SHIFT; + +- return read32(base + DBGMCU_IDC) >> 16; ++ return 0; ++} ++ ++/* SoC device ID util */ ++int stm32mp1_dbgmcu_get_chip_dev_id(uint32_t *chip_dev_id) ++{ ++ assert(chip_dev_id != NULL); ++ ++ *chip_dev_id = read32(stm32_dbgmcu_base() + DBGMCU_IDC) & ++ DBGMCU_IDC_DEV_ID_MASK; ++ ++ return 0; + } + + static uintptr_t stm32_tamp_base(void) +@@ -420,6 +479,19 @@ uintptr_t stm32_get_stgen_base(void) + return va; + } + ++uintptr_t stm32_get_syscfg_base(void) ++{ ++ static uintptr_t va; ++ ++ if (!cpu_mmu_enabled()) ++ return SYSCFG_BASE; ++ ++ if (!va) ++ va = (uintptr_t)phys_to_virt(SYSCFG_BASE, MEM_AREA_IO_NSEC); ++ ++ return va; ++} ++ + uintptr_t stm32_get_gpio_bank_base(unsigned int bank) + { + /* Non secure banks and mapped together, same for secure banks */ +@@ -522,25 +594,93 @@ unsigned long stm32_get_iwdg_otp_config(uintptr_t pbase) + { + unsigned int idx; + unsigned long iwdg_cfg = 0; ++ uint32_t otp; + uint32_t otp_value; + + idx = stm32mp_iwdg_iomem2instance(pbase); + +- if (bsec_read_otp(&otp_value, HW2_OTP)) ++ if (bsec_find_otp_name_in_nvmem_layout(HW2_OTP, &otp, NULL)) ++ panic(); ++ ++ if (bsec_read_otp(&otp_value, otp)) + panic(); + + if (otp_value & BIT(idx + HW2_OTP_IWDG_HW_ENABLE_SHIFT)) + iwdg_cfg |= IWDG_HW_ENABLED; + +- if (!(otp_value & BIT(idx + HW2_OTP_IWDG_FZ_STOP_SHIFT))) +- iwdg_cfg |= IWDG_ENABLE_ON_STOP; ++ if (otp_value & BIT(idx + HW2_OTP_IWDG_FZ_STOP_SHIFT)) ++ iwdg_cfg |= IWDG_DISABLE_ON_STOP; + +- if (!(otp_value & BIT(idx + HW2_OTP_IWDG_FZ_STANDBY_SHIFT))) +- iwdg_cfg |= IWDG_ENABLE_ON_STANDBY; ++ if (otp_value & BIT(idx + HW2_OTP_IWDG_FZ_STANDBY_SHIFT)) ++ iwdg_cfg |= IWDG_DISABLE_ON_STANDBY; + + return iwdg_cfg; + } + ++static int get_part_number(uint32_t *part_nb) ++{ ++ uint32_t part_number; ++ uint32_t dev_id; ++ uint32_t otp; ++ size_t bit_len; ++ ++ assert(part_nb != NULL); ++ ++ if (stm32mp1_dbgmcu_get_chip_dev_id(&dev_id) != 0) { ++ return -1; ++ } ++ ++ if (bsec_find_otp_name_in_nvmem_layout(PART_NUMBER_OTP, &otp, ++ &bit_len) != BSEC_OK) { ++ return -1; ++ } ++ ++ assert(bit_len == 8); ++ ++ if (bsec_read_otp(&part_number, otp) != BSEC_OK) { ++ return -1; ++ } ++ ++ part_number = (part_number & PART_NUMBER_OTP_PART_MASK) >> ++ PART_NUMBER_OTP_PART_SHIFT; ++ ++ *part_nb = part_number | (dev_id << 16); ++ ++ return 0; ++} ++ ++bool stm32mp_supports_cpu_opp(uint32_t opp_id) ++{ ++ uint32_t part_number; ++ uint32_t id; ++ ++ if (get_part_number(&part_number) != 0) { ++ DMSG("Cannot get part number"); ++ panic(); ++ } ++ ++ switch (opp_id) { ++ case PLAT_OPP_ID1: ++ case PLAT_OPP_ID2: ++ id = opp_id; ++ break; ++ default: ++ return false; ++ } ++ ++ switch (part_number) { ++ case STM32MP157F_PART_NB: ++ case STM32MP157D_PART_NB: ++ case STM32MP153F_PART_NB: ++ case STM32MP153D_PART_NB: ++ case STM32MP151F_PART_NB: ++ case STM32MP151D_PART_NB: ++ return true; ++ default: ++ return id == PLAT_OPP_ID1; ++ } ++} ++ + uintptr_t stm32mp_get_etzpc_base(void) + { + return ETZPC_BASE; +diff --git a/core/arch/arm/plat-stm32mp1/platform_config.h b/core/arch/arm/plat-stm32mp1/platform_config.h +index e8d1660..d635efd 100644 +--- a/core/arch/arm/plat-stm32mp1/platform_config.h ++++ b/core/arch/arm/plat-stm32mp1/platform_config.h +@@ -1,6 +1,6 @@ + /* SPDX-License-Identifier: BSD-2-Clause */ + /* +- * Copyright (c) 2017-2018, STMicroelectronics ++ * Copyright (c) 2017-2019, STMicroelectronics + */ + + #ifndef PLATFORM_CONFIG_H +@@ -78,6 +78,7 @@ + #define RTC_BASE 0x5c004000 + #define SPI6_BASE 0x5c001000 + #define STGEN_BASE 0x5C008000 ++#define SYSCFG_BASE 0x50020000 + #define TAMP_BASE 0x5c00a000 + #define USART1_BASE 0x5c000000 + #define USART2_BASE 0x4000e000 +@@ -95,20 +96,15 @@ + + #define OTP_MAX_SIZE (STM32MP1_OTP_MAX_ID + 1U) + +-#define DATA0_OTP 0 +-#define PART_NUMBER_OTP 1 +-#define MONOTONIC_OTP 4 +-#define NAND_OTP 9 +-#define UID0_OTP 13 +-#define UID1_OTP 14 +-#define UID2_OTP 15 +-#define HW2_OTP 18 ++#define HW2_OTP "hw2_otp" ++#define PART_NUMBER_OTP "part_number_otp" + +-#define DATA0_OTP_SECURED BIT(6) ++#define HW2_OTP_IWDG_HW_ENABLE_SHIFT 3 ++#define HW2_OTP_IWDG_FZ_STOP_SHIFT 5 ++#define HW2_OTP_IWDG_FZ_STANDBY_SHIFT 7 + +-#define HW2_OTP_IWDG_HW_ENABLE_SHIFT 3 +-#define HW2_OTP_IWDG_FZ_STOP_SHIFT 5 +-#define HW2_OTP_IWDG_FZ_STANDBY_SHIFT 7 ++#define PART_NUMBER_OTP_PART_MASK GENMASK_32(7, 0) ++#define PART_NUMBER_OTP_PART_SHIFT 0 + + /* + * GPIO banks: 11 non secure banks (A to K) and 1 secure bank (Z) +@@ -150,12 +146,35 @@ + /* RCC platform resources */ + #define RCC_WAKEUP_IT 177 + +-/* SoC revision */ ++/* SoC part numbers and revisions */ ++#define STM32MP157C_PART_NB 0x05000000 ++#define STM32MP157A_PART_NB 0x05000001 ++#define STM32MP153C_PART_NB 0x05000024 ++#define STM32MP153A_PART_NB 0x05000025 ++#define STM32MP151C_PART_NB 0x0500002E ++#define STM32MP151A_PART_NB 0x0500002F ++#define STM32MP157F_PART_NB 0x05000080 ++#define STM32MP157D_PART_NB 0x05000081 ++#define STM32MP153F_PART_NB 0x050000A4 ++#define STM32MP153D_PART_NB 0x050000A5 ++#define STM32MP151F_PART_NB 0x050000AE ++#define STM32MP151D_PART_NB 0x050000AF ++ + #define STM32MP1_REV_A 0x00001000 + #define STM32MP1_REV_B 0x00002000 ++#define STM32MP1_REV_Z 0x00002001 ++ ++/* OPP */ ++#define PLAT_OPP_ID1 1U ++#define PLAT_OPP_ID2 2U ++#define PLAT_MAX_OPP_NB 2U ++#define PLAT_MAX_PLLCFG_NB 6U + + /* DBGMCU resources */ + #define DBGMCU_IDC 0x0 ++#define DBGMCU_IDC_DEV_ID_MASK GENMASK_32(11, 0) ++#define DBGMCU_IDC_REV_ID_MASK GENMASK_32(31, 16) ++#define DBGMCU_IDC_REV_ID_SHIFT 16 + + /* BKPSRAM layout */ + #define BKPSRAM_SIZE SMALL_PAGE_SIZE +@@ -167,7 +186,7 @@ + #define BKPSRAM_PM_MAILBOX_SIZE 0x100 + #define BKPSRAM_PM_CONTEXT_OFFSET (BKPSRAM_PM_MAILBOX_OFFSET + \ + BKPSRAM_PM_MAILBOX_SIZE) +-#define BKPSRAM_PM_CONTEXT_SIZE 0x700 ++#define BKPSRAM_PM_CONTEXT_SIZE 0xF00 + + /* SYSRAM */ + #define SYSRAM_SIZE 0x40000 +diff --git a/core/arch/arm/plat-stm32mp1/pm/context.c b/core/arch/arm/plat-stm32mp1/pm/context.c +index 20e9e84..fcdad91 100644 +--- a/core/arch/arm/plat-stm32mp1/pm/context.c ++++ b/core/arch/arm/plat-stm32mp1/pm/context.c +@@ -1,6 +1,6 @@ + // SPDX-License-Identifier: BSD-3-Clause + /* +- * Copyright (c) 2018, STMicroelectronics - All Rights Reserved ++ * Copyright (c) 2018-2019, STMicroelectronics - All Rights Reserved + * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved. + */ + +@@ -32,7 +32,28 @@ + + #define TRAINING_AREA_SIZE 64 + +-#define STANDBY_CONTEXT_MAGIC (0x00010000 + TRAINING_AREA_SIZE) ++/* ++ * STANDBY_CONTEXT_MAGIC0: ++ * Context provides magic, resume entry, zq0cr0 zdata and DDR training buffer. ++ * ++ * STANDBY_CONTEXT_MAGIC1: ++ * Context provides magic, resume entry, zq0cr0 zdata, DDR training buffer ++ * and PLL1 dual OPP settings structure (86 bytes). ++ */ ++#define STANDBY_CONTEXT_MAGIC0 (0x0001 << 16) ++#define STANDBY_CONTEXT_MAGIC1 (0x0002 << 16) ++ ++#define STANDBY_CONTEXT_MAGIC (STANDBY_CONTEXT_MAGIC1 | \ ++ TRAINING_AREA_SIZE) ++ ++#if (PLAT_MAX_OPP_NB != 2) || (PLAT_MAX_PLLCFG_NB != 6) ++#error STANDBY_CONTEXT_MAGIC1 does not support expected PLL1 settings ++#endif ++ ++/* pll_settings structure size definitions (reference to clock driver) */ ++#define PLL1_SETTINGS_SIZE (((PLAT_MAX_OPP_NB * \ ++ (PLAT_MAX_PLLCFG_NB + 3)) + 1) * \ ++ sizeof(uint32_t)) + + /* + * Context saved in TEE RAM during lower power sequence. +@@ -58,11 +79,12 @@ static struct pm_context plat_ctx; + * @zq0cr0_zdata: DDRPHY configuration to be restored. + * @ddr_training_backup: DDR area saved at suspend and backed up at resume + */ +-struct pm_mailbox { ++struct __attribute__((__packed__)) pm_mailbox { + uint32_t magic; + uint32_t core0_resume_ep; + uint32_t zq0cr0_zdata; + uint8_t ddr_training_backup[TRAINING_AREA_SIZE]; ++ uint8_t pll1_settings[PLL1_SETTINGS_SIZE]; + }; + + /* +@@ -323,6 +345,21 @@ static void save_ddr_training_area(void) + mobj_free(mobj); + } + ++/* ++ * When returning from STANDBY, warm boot boot stage needs to access to PLL1 ++ * settings. This avoids to re-compute them and optimizes performances. This ++ * structure must then be saved before going to STANDBY in the PM mailbox ++ * shared with the warm boot boot stage. ++ */ ++static void save_pll1_settings(void) ++{ ++ struct pm_mailbox *mailbox = get_pm_mailbox(); ++ size_t size = sizeof(mailbox->pll1_settings); ++ uint8_t *data = &mailbox->pll1_settings[0]; ++ ++ stm32mp1_clk_lp_save_opp_pll1_settings(data, size); ++} ++ + static void load_earlyboot_pm_mailbox(void) + { + struct pm_mailbox *mailbox = get_pm_mailbox(); +@@ -337,6 +374,8 @@ static void load_earlyboot_pm_mailbox(void) + mailbox->zq0cr0_zdata = get_ddrphy_calibration(); + + save_ddr_training_area(); ++ ++ save_pll1_settings(); + } + + #ifdef CFG_STM32_RNG +diff --git a/core/arch/arm/plat-stm32mp1/pm/low_power.c b/core/arch/arm/plat-stm32mp1/pm/low_power.c +index 48bddb7..450b68a 100644 +--- a/core/arch/arm/plat-stm32mp1/pm/low_power.c ++++ b/core/arch/arm/plat-stm32mp1/pm/low_power.c +@@ -170,6 +170,10 @@ int stm32_enter_cstop(uint32_t mode) + uintptr_t rcc_base = stm32_rcc_base(); + int rc; + ++ stm32mp1_syscfg_disable_io_compensation(); ++ ++ ddr_sr_mode_ssr(); ++ + stm32_apply_pmic_suspend_config(mode); + + if (stm32mp_with_pmic() && (mode == STM32_PM_CSTOP_ALLOW_LP_STOP)) { +@@ -233,6 +237,8 @@ void stm32_exit_cstop(void) + ddr_in_selfrefresh = false; + } + ++ ddr_sr_mode_asr(); ++ + restore_rcc_it_priority(gicd_rcc_wakeup, gicc_pmr); + + /* Disable STOP request */ +@@ -247,6 +253,8 @@ void stm32_exit_cstop(void) + + /* Disable retention and backup RAM content after stop */ + mmio_clrbits_32(pwr_base + PWR_CR2_OFF, PWR_CR2_BREN | PWR_CR2_RREN); ++ ++ stm32mp1_syscfg_enable_io_compensation(); + } + + /* +@@ -412,7 +420,7 @@ void __noreturn stm32_cores_reset(void) + } + KEEP_PAGER(stm32_cores_reset); + +-static void reset_other_core(void) ++static void __maybe_unused reset_other_core(void) + { + uintptr_t rcc_base = stm32_rcc_base(); + uint32_t reset_mask; +diff --git a/core/arch/arm/plat-stm32mp1/pm/pm_helpers.S b/core/arch/arm/plat-stm32mp1/pm/pm_helpers.S +index d9ed746..cd8e48c 100644 +--- a/core/arch/arm/plat-stm32mp1/pm/pm_helpers.S ++++ b/core/arch/arm/plat-stm32mp1/pm/pm_helpers.S +@@ -64,6 +64,36 @@ + #define CRYP_SR_IFNF BIT(1) + #define CRYP_SR_IFEM BIT(0) + ++/* ++ * Enable TRACE_SYSRAM_RESTORE to get some UART console traces ++ * at resume time. ++ */ ++#if defined(TRACE_SYSRAM_RESTORE) ++ ++#define UART_BASE UART4_BASE ++#define UART_ISR_OFF 0x1c ++#define UART_TDR_OFF 0x28 ++#define USART_ISR_TXE_TXFNF (1<< 7) ++ ++ .macro PRINT_CHAR _reg0, _reg1, _char ++ /* Trace only at resume when MMU is OFF */ ++ read_sctlr \_reg0 ++ ands \_reg0, #SCTLR_M ++ 101: ++ bne 102f ++ mov_imm \_reg0, UART4_BASE ++ ldr \_reg1, [\_reg0, #UART_ISR_OFF] ++ ands \_reg1, #USART_ISR_TXE_TXFNF ++ beq 101b ++ mov_imm \_reg1, (\_char) ++ str \_reg1, [\_reg0, #UART_TDR_OFF] ++ 102: ++ .endm ++#else ++ .macro PRINT_CHAR _reg0, _reg1, _char ++ .endm ++#endif ++ + /* Bound of the binary image loaded in retained memory */ + .global stm32mp_bkpsram_image_end + +@@ -80,6 +110,9 @@ + FUNC stm32mp_bkpsram_resume , : + UNWIND( .fnstart) + UNWIND( .cantunwind) ++ ++ PRINT_CHAR r0, r1, '0' ++ + /* + * Almost all sequences here expect PM context structure base address + * from CPU register r11. +@@ -105,6 +138,11 @@ UNWIND( .cantunwind) + cmp r0, #0 + bne _failed + ++ PRINT_CHAR r0, r1, 'T' ++ PRINT_CHAR r0, r1, 'a' ++ PRINT_CHAR r0, r1, 'g' ++ PRINT_CHAR r0, r1, '\n' ++ + /* Compare the generated and reference tags */ + add r8, r11, #PM_CTX_CCM_TAG + add r9, r11, #PM_CTX_CCM_REF_TAG +@@ -123,11 +161,18 @@ UNWIND( .cantunwind) + bne _failed + bl _save_resume_time + ++ PRINT_CHAR r1, r2, 'O' ++ PRINT_CHAR r1, r2, 'k' ++ PRINT_CHAR r1, r2, '\n' ++ + /* Resume into the restored TEE RAM */ + ldr r1, [r11, #PM_CTX_RESUME_PA] + bx r1 + + _failed: ++ PRINT_CHAR r0, r12, 'F' ++ PRINT_CHAR r0, r12, '\n' ++ + /* Clear context including key and reference tag */ + mov r0, #0xa5 + mov_imm r12, BKPSRAM_PM_CONTEXT_SIZE +@@ -279,16 +324,23 @@ _ccm_arm_8ms_timeout: + moveq r0, #1 + ldr r1, [r1, #CNTCVL_OFFSET] + adds r0, r0, r1 +- bcs _ccm_failed ++ bcs _ccm_failed_on_timeout + bx lr + + _ccm_fail_on_timeout: ++ + ldr r1, [r11, #PM_CTX_STGEN_BASE] + ldr r1, [r1, #CNTCVL_OFFSET] + cmp r1, r0 +- bge _ccm_failed ++ bge _ccm_failed_on_timeout + bx lr + ++_ccm_failed_on_timeout: ++ PRINT_CHAR r0, r1, 'T' ++ PRINT_CHAR r0, r1, 'o' ++ PRINT_CHAR r0, r1, '\n' ++ b _ccm_failed ++ + /* + * Macro WAIT_FLAG_TIMEOUT compares timeout threshold (r0) with + * current time and branches the CCM failure entry on timeout. +@@ -346,8 +398,12 @@ stm32mp_ccm_teeram: + mov r7, r2 + mov r6, r3 + ++ PRINT_CHAR r0, r1, '1' ++ + bl _setup_cryp1 + ++ PRINT_CHAR r0, r1, '2' ++ + mov_imm r0, (CRYP_CR_ALGOMODE(ALGOMODE_AES_CCM) | \ + CRYP_CR_DATATYPE_8BIT | CRYP_CR_FFLUSH | \ + CRYP_CR_KEYSIZE_256BIT) +@@ -355,6 +411,8 @@ stm32mp_ccm_teeram: + orrne r0, r0, #CRYP_CR_ALGODIR_DECRYPT + str r0, [r10, #CRYP_CR] + ++ PRINT_CHAR r0, r1, '3' ++ + /* Check data alignment (addresses and size) */ + ands r0, r7, #0x0F + bne _ccm_failed +@@ -363,6 +421,8 @@ stm32mp_ccm_teeram: + ands r0, r9, #0x03 + bne _ccm_failed + ++ PRINT_CHAR r0, r1, '4' ++ + ldr r0, [r11, #PM_CTX_CCM_KEY] + str r0, [r10, #CRYP_KEYR_BASE] + ldr r0, [r11, #(PM_CTX_CCM_KEY + 4)] +@@ -404,6 +464,8 @@ stm32mp_ccm_teeram: + ldr r0, [r11, #(PM_CTX_CCM_B0 + 12)] + str r0, [r10, #CRYP_DIN] + ++ PRINT_CHAR r0, r1, '5' ++ + WAIT_FLAG_TIMEOUT CRYP_CR, CRYP_CR_CRYPEN, 0 + + /* Setup CRYP for the CCM Payload phase */ +@@ -414,7 +476,11 @@ stm32mp_ccm_teeram: + str r0, [r10, #CRYP_CR] + ldr r0, [r10, #CRYP_CR] + ++ PRINT_CHAR r0, r1, '\n' ++ + _next_block: ++ PRINT_CHAR r0, r1, 'b' ++ + WAIT_FLAG_TIMEOUT CRYP_SR, CRYP_SR_IFEM, CRYP_SR_IFEM + + /* Feed input data, r8 stores the current source buffer */ +@@ -451,6 +517,9 @@ _next_block: + subs r7, r7, #16 + bne _next_block; + ++ PRINT_CHAR r0, r1, '\n' ++ PRINT_CHAR r0, r1, '6' ++ + WAIT_FLAG_TIMEOUT CRYP_SR, CRYP_SR_BUSY, 0 + + /* +@@ -461,6 +530,8 @@ _next_block: + cmp r0, #(CRYP_SR_IFEM | CRYP_SR_IFNF) + bne _ccm_failed + ++ PRINT_CHAR r0, r1, '7' ++ + /* Setup CRYP1 for the CCM Final Phase */ + ldr r0, [r10, #CRYP_CR] + bic r0, r0, #CRYP_CR_CRYPEN +@@ -483,6 +554,8 @@ _next_block: + ldr r0, [r11, #(PM_CTX_CCM_CTR0 + 12)] + str r0, [r10, #CRYP_DIN] + ++ PRINT_CHAR r0, r1, '8' ++ + WAIT_FLAG_TIMEOUT CRYP_SR, CRYP_SR_OFNE, CRYP_SR_OFNE + + /* Store generated tag in the PM_CTX structure */ +@@ -503,12 +576,18 @@ _next_block: + ands r0, r0, #CRYP_SR_OFNE + bne _ccm_failed + ++ PRINT_CHAR r0, r1, '9' ++ + /* Successful return */ + bl _reset_cryp1 + mov r0, #0 + bx r12 + + _ccm_failed: ++ ++ PRINT_CHAR r0, r1, 'K' ++ PRINT_CHAR r0, r1, 'O' ++ + bl _reset_cryp1 + mov r0, #1 + bx r12 +@@ -577,24 +656,50 @@ FUNC stm32mp_sysram_resume, : + UNWIND( .fnstart) + UNWIND( .cantunwind) + /* Invalidate the data cache */ +- mov r0, #0 @ ; write the cache size selection register to be +- write_csselr r0 @ ; sure we address the data cache +- isb @ ; isb to sync the change to the cachesizeid reg +- +- mov r0, #0 @ ; set way number to 0 +-_inv_nextway: +- mov r1, #0 @ ; set line number (=index) to 0 +-_inv_nextline: +- orr r2, r0, r1 @ ; construct way/index value +- write_dcisw r2 @ ; inval data or unified cache line by set/way +- add r1, r1, #1 << LINE_FIELD_OFFSET @ ; increment the index +- cmp r1, #1 << LINE_FIELD_OVERFLOW @ ; overflow out of set field? +- bne _inv_nextline +- add r0, r0, #1 << WAY_FIELD_OFFSET @ ; increment the way number +- cmp r0, #0 @ ; overflow out of way field? +- bne _inv_nextway + +- dsb ++ read_clidr r2 ++ ubfx r3, r2, #CLIDR_LOC_SHIFT, #CLIDR_FIELD_WIDTH ++ lsl r3, r3, #CSSELR_LEVEL_SHIFT ++ mov r1, #0 ++ ++loop1: ++ add r10, r1, r1, LSR #1 // Work out 3x current cache level ++ mov r12, r2, LSR r10 // extract cache type bits from clidr ++ and r12, r12, #7 // mask the bits for current cache only ++ cmp r12, #2 // see what cache we have at this level ++ blo level_done // no cache or only instruction cache at this level ++ ++ write_csselr r1 // select current cache level in csselr ++ isb // isb to sych the new cssr&csidr ++ read_ccsidr r12 // read the new ccsidr ++ and r10, r12, #7 // extract the length of the cache lines ++ add r10, r10, #4 // add 4 (r10 = line length offset) ++ ubfx r4, r12, #3, #10 // r4 = maximum way number (right aligned) ++ clz r5, r4 // r5 = the bit position of the way size increment ++ mov r9, r4 // r9 working copy of the aligned max way number ++ ++loop2: ++ ubfx r7, r12, #13, #15 // r7 = max set number (right aligned) ++ ++loop3: ++ orr r0, r1, r9, LSL r5 // factor in the way number and cache level into r0 ++ orr r0, r0, r7, LSL r10 // factor in the set number ++ ++ write_dcisw r0 ++ ++ subs r7, r7, #1 // decrement the set number ++ bhs loop3 ++ subs r9, r9, #1 // decrement the way number ++ bhs loop2 ++level_done: ++ add r1, r1, #2 // increment the cache number ++ cmp r3, r1 ++ dsb sy // ensure completion of previous cache maintenance instruction ++ bhi loop1 ++ ++ mov r6, #0 ++ write_csselr r6 //select cache level 0 in csselr ++ dsb sy + isb + + /* Resume sequence executes in Monitor mode */ +diff --git a/core/arch/arm/plat-stm32mp1/pm/power_config.c b/core/arch/arm/plat-stm32mp1/pm/power_config.c +index 7845ede..ef26608 100644 +--- a/core/arch/arm/plat-stm32mp1/pm/power_config.c ++++ b/core/arch/arm/plat-stm32mp1/pm/power_config.c +@@ -1,6 +1,6 @@ + // SPDX-License-Identifier: BSD-3-Clause + /* +- * Copyright (c) 2017-2018, STMicroelectronics - All Rights Reserved ++ * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved + */ + + #include +@@ -57,7 +57,7 @@ bool need_to_backup_cpu_context(unsigned int soc_mode) + #ifdef CFG_DT + static int dt_get_pwr_node(void *fdt) + { +- return fdt_node_offset_by_compatible(fdt, -1, DT_PWR_COMPAT); ++ return fdt_get_node_by_compatible(fdt, DT_PWR_COMPAT); + } + #endif + +diff --git a/core/arch/arm/plat-stm32mp1/service/bsec_svc.c b/core/arch/arm/plat-stm32mp1/service/bsec_svc.c +index 1ce7195..f345978 100644 +--- a/core/arch/arm/plat-stm32mp1/service/bsec_svc.c ++++ b/core/arch/arm/plat-stm32mp1/service/bsec_svc.c +@@ -48,6 +48,10 @@ uint32_t bsec_main(uint32_t x1, uint32_t x2, uint32_t x3, uint32_t *out) + result = bsec_write_otp(tmp, x2); + FMSG("read @%" PRIx32 " = %" PRIx32, x2, *out); + break; ++ case STM32_SIP_BSEC_WRLOCK_OTP: ++ FMSG("permanent write lock @%" PRIx32, x2); ++ result = bsec_permanent_lock_otp(x2); ++ break; + default: + EMSG("Invalid %" PRIx32, x1); + result = BSEC_ERROR; +diff --git a/core/arch/arm/plat-stm32mp1/service/rcc_svc.c b/core/arch/arm/plat-stm32mp1/service/rcc_svc.c +index 7bdaea8..3e7b4d3 100644 +--- a/core/arch/arm/plat-stm32mp1/service/rcc_svc.c ++++ b/core/arch/arm/plat-stm32mp1/service/rcc_svc.c +@@ -1,6 +1,6 @@ + // SPDX-License-Identifier: BSD-3-Clause + /* +- * Copyright (c) 2017-2018, STMicroelectronics ++ * Copyright (c) 2017-2019, STMicroelectronics + */ + + #include +@@ -438,3 +438,31 @@ uint32_t rcc_scv_handler(uint32_t x1, uint32_t x2, uint32_t x3) + + return STM32_SIP_OK; + } ++ ++uint32_t rcc_opp_scv_handler(uint32_t x1, uint32_t x2, uint32_t *res) ++{ ++ uint32_t cmd = x1; ++ uint32_t opp = x2 / 1000U; /* KHz */ ++ ++ switch (cmd) { ++ case STM32_SIP_RCC_OPP_SET: ++ if (stm32mp1_set_opp_khz(opp) != 0) { ++ return STM32_SIP_FAILED; ++ } ++ break; ++ ++ case STM32_SIP_RCC_OPP_ROUND: ++ if(stm32mp1_round_opp_khz(&opp) != 0) { ++ return STM32_SIP_FAILED; ++ } ++ ++ if (MUL_OVERFLOW(opp, 1000, res)) ++ return STM32_SIP_FAILED; ++ break; ++ ++ default: ++ return STM32_SIP_INVALID_PARAMS; ++ } ++ ++ return STM32_SIP_OK; ++} +diff --git a/core/arch/arm/plat-stm32mp1/service/rcc_svc.h b/core/arch/arm/plat-stm32mp1/service/rcc_svc.h +index f198ebd..aaa1f75 100644 +--- a/core/arch/arm/plat-stm32mp1/service/rcc_svc.h ++++ b/core/arch/arm/plat-stm32mp1/service/rcc_svc.h +@@ -1,11 +1,12 @@ + /* SPDX-License-Identifier: BSD-2-Clause */ + /* +- * Copyright (c) 2017-2018, STMicroelectronics ++ * Copyright (c) 2017-2019, STMicroelectronics + */ + + #ifndef __RCC_SVC_H__ + #define __RCC_SVC_H__ + + uint32_t rcc_scv_handler(uint32_t x1, uint32_t x2, uint32_t x3); ++uint32_t rcc_opp_scv_handler(uint32_t x1, uint32_t x2, uint32_t *res); + + #endif /*__RCC_SVC_H__*/ +diff --git a/core/arch/arm/plat-stm32mp1/service/stm32mp1_smc.h b/core/arch/arm/plat-stm32mp1/service/stm32mp1_smc.h +index 2db7b10..3482c34 100644 +--- a/core/arch/arm/plat-stm32mp1/service/stm32mp1_smc.h ++++ b/core/arch/arm/plat-stm32mp1/service/stm32mp1_smc.h +@@ -110,6 +110,7 @@ + #define STM32_SIP_BSEC_PROG_OTP 0x2 + #define STM32_SIP_BSEC_WRITE_SHADOW 0x3 + #define STM32_SIP_BSEC_READ_OTP 0x4 ++#define STM32_SIP_BSEC_WRLOCK_OTP 0x5 + + /* + * SIP functions STM32_SIP_FUNC_SR_MODE +@@ -182,6 +183,21 @@ + #define STM32_SIP_PD_MAX_PM_DOMAIN 0x3 + + /* ++ * SIP function STM32_SIP_FUNC_RCC_OPP. ++ * ++ * Argument a0: (input) SMCC ID. ++ * (output) Status return code. ++ * Argument a1: (input) Service ID (STM32_SIP_RCC_OPP_xxx). ++ * (output) Rounded frequency, if applicable. ++ * Argument a2: (input) Requested frequency. ++ */ ++#define STM32_SIP_FUNC_RCC_OPP 0x1009 ++ ++/* Service ID for STM32_SIP_FUNC_RCC_OPP */ ++#define STM32_SIP_RCC_OPP_SET 0x0 ++#define STM32_SIP_RCC_OPP_ROUND 0x1 ++ ++/* + * OEM Functions + */ + #define STM32_OEM_SVC_VERSION_MAJOR 0x0 +diff --git a/core/arch/arm/plat-stm32mp1/service/stm32mp1_svc_setup.c b/core/arch/arm/plat-stm32mp1/service/stm32mp1_svc_setup.c +index 3c7958c..a88577a 100644 +--- a/core/arch/arm/plat-stm32mp1/service/stm32mp1_svc_setup.c ++++ b/core/arch/arm/plat-stm32mp1/service/stm32mp1_svc_setup.c +@@ -1,6 +1,6 @@ + // SPDX-License-Identifier: BSD-3-Clause + /* +- * Copyright (c) 2017-2018, STMicroelectronics ++ * Copyright (c) 2017-2019, STMicroelectronics + */ + + #include +@@ -70,6 +70,9 @@ bool stm32_sip_service(struct sm_ctx __unused *ctx, + case STM32_SIP_FUNC_RCC: + *a0 = rcc_scv_handler(*a1, *a2, *a3); + break; ++ case STM32_SIP_FUNC_RCC_OPP: ++ *a0 = rcc_opp_scv_handler(*a1, *a2, a1); ++ break; + #endif + #ifdef CFG_STM32_CLOCKSRC_CALIB + case STM32_SIP_RCC_CAL: +diff --git a/core/arch/arm/plat-stm32mp1/stm32_util.h b/core/arch/arm/plat-stm32mp1/stm32_util.h +index 347499b..a2818c0 100644 +--- a/core/arch/arm/plat-stm32mp1/stm32_util.h ++++ b/core/arch/arm/plat-stm32mp1/stm32_util.h +@@ -1,6 +1,6 @@ + /* SPDX-License-Identifier: BSD-3-Clause */ + /* +- * Copyright (c) 2017-2018, STMicroelectronics ++ * Copyright (c) 2017-2019, STMicroelectronics + */ + + #ifndef __STM32_UTIL_H__ +@@ -15,8 +15,12 @@ + #include + #include + +-/* SoC versioning */ +-uint32_t stm32mp1_dbgmcu_get_chip_version(void); ++/* SoC versioning and device ID */ ++int stm32mp1_dbgmcu_get_chip_version(uint32_t *chip_version); ++int stm32mp1_dbgmcu_get_chip_dev_id(uint32_t *chip_dev_id); ++ ++/* OPP service */ ++bool stm32mp_supports_cpu_opp(uint32_t opp_id); + + /* SiP & OEM platform services */ + bool stm32_sip_service(struct sm_ctx *ctx, +@@ -28,6 +32,9 @@ bool stm32_oem_service(struct sm_ctx *ctx, + /* Platform util for the STGEN driver */ + uintptr_t stm32_get_stgen_base(void); + ++/* Platform util for the SYSCFG driver */ ++uintptr_t stm32_get_syscfg_base(void); ++ + /* Platform util for the GIC */ + uintptr_t get_gicc_base(void); + uintptr_t get_gicd_base(void); +@@ -120,6 +127,13 @@ void stm32mp_platform_reset(int cpu); + int stm32mp_start_clock_calib(unsigned int clock_id); + + /* ++ * SYSCFG IO compensation. ++ * These functions assume non-secure world is suspended. ++ */ ++void stm32mp1_syscfg_enable_io_compensation(void); ++void stm32mp1_syscfg_disable_io_compensation(void); ++ ++/* + * Shared reference counter: increments by 2 on secure increment + * request, decrements by 2 on secure decrement request. Bit #0 + * is set to 1 on non-secure increment request and reset to 0 on +diff --git a/core/arch/arm/plat-stm32mp1/stm32mp1_dt.c b/core/arch/arm/plat-stm32mp1/stm32mp1_dt.c +index 9ce6ba8..7413ebb 100644 +--- a/core/arch/arm/plat-stm32mp1/stm32mp1_dt.c ++++ b/core/arch/arm/plat-stm32mp1/stm32mp1_dt.c +@@ -1,6 +1,6 @@ + // SPDX-License-Identifier: BSD-3-Clause + /* +- * Copyright (c) 2018, STMicroelectronics - All Rights Reserved ++ * Copyright (c) 2018-2019, STMicroelectronics - All Rights Reserved + * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved. + */ + +@@ -230,6 +230,74 @@ int fdt_get_stdout_node_offset(void *fdt) + } + + /******************************************************************************* ++ * This function returns the offset of the first matching compatible node ++ * in the DT. It should be used only for single instanced peripherals. ++ * Returns node on success and a negative FDT error code on failure. ++ ******************************************************************************/ ++int fdt_get_node_by_compatible(void *fdt, const char *compatible) ++{ ++ int node = fdt_node_offset_by_compatible(fdt, -1, compatible); ++ ++ if (node < 0) { ++ DMSG("Cannot find %s node in DT", compatible); ++ } ++ ++ return node; ++} ++ ++/******************************************************************************* ++ * This function returns the node offset matching compatible string in the DT, ++ * and also matching the reg property with the given address. ++ * Returns node on success and a negative FDT error code on failure. ++ ******************************************************************************/ ++int fdt_match_instance_by_compatible(void *fdt, const char *compatible, ++ uintptr_t address) ++{ ++ int node; ++ ++ for (node = fdt_node_offset_by_compatible(fdt, -1, compatible); ++ node != -FDT_ERR_NOTFOUND; ++ node = fdt_node_offset_by_compatible(fdt, node, compatible)) { ++ const fdt32_t *cuint; ++ ++ cuint = fdt_getprop(fdt, node, "reg", NULL); ++ if (cuint == NULL) { ++ continue; ++ } ++ ++ if ((uintptr_t)fdt32_to_cpu(*cuint) == address) { ++ return node; ++ } ++ } ++ ++ return -FDT_ERR_NOTFOUND; ++} ++ ++/******************************************************************************* ++ * This function returns the peripheral base address information from the ++ * first matching compatible string in the DT. It should be used only for ++ * single instanced peripherals. ++ * Returns non null base address on success, and 0 on failure. ++ ******************************************************************************/ ++uintptr_t fdt_get_peripheral_base(void *fdt, const char *compatible) ++{ ++ int node; ++ const fdt32_t *cuint; ++ ++ node = fdt_get_node_by_compatible(fdt, compatible); ++ 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 DDR size information from the DT. + * Returns value in bytes if success, and STM32MP1_DDR_SIZE_DFLT else. + ******************************************************************************/ +@@ -237,9 +305,8 @@ uint32_t fdt_get_ddr_size(void *fdt) + { + int node; + +- node = fdt_node_offset_by_compatible(fdt, -1, DT_DDR_COMPAT); ++ node = fdt_get_node_by_compatible(fdt, DT_DDR_COMPAT); + if (node < 0) { +- IMSG("%s: Cannot read DDR node in DT\n", __func__); + return STM32MP1_DDR_SIZE_DFLT; + } + +@@ -248,6 +315,127 @@ uint32_t fdt_get_ddr_size(void *fdt) + } + + /******************************************************************************* ++ * This function gets OPP table node from the DT. ++ * Returns node offset on success and a negative FDT error code on failure. ++ ******************************************************************************/ ++static int fdt_get_opp_table_node(void *fdt) ++{ ++ return fdt_get_node_by_compatible(fdt, DT_OPP_COMPAT); ++} ++ ++/******************************************************************************* ++ * This function gets OPP parameters (frequency in KHz and voltage in mV) from ++ * an OPP table subnode. Platform HW support capabilities are also checked. ++ * Returns 0 on success and a negative FDT error code on failure. ++ ******************************************************************************/ ++static int fdt_get_opp_freqvolt_from_subnode(void *fdt, int subnode, ++ uint32_t *freq_khz, ++ uint32_t *voltage_mv) ++{ ++ const fdt64_t *cuint64; ++ const fdt32_t *cuint32; ++ uint64_t read_freq_64; ++ uint32_t read_voltage_32; ++ ++ assert(freq_khz != NULL); ++ assert(voltage_mv != NULL); ++ ++ cuint32 = fdt_getprop(fdt, subnode, "opp-supported-hw", NULL); ++ if (cuint32 != NULL) { ++ if (!stm32mp_supports_cpu_opp(fdt32_to_cpu(*cuint32))) { ++ DMSG("Invalid opp-supported-hw 0x%"PRIx32, ++ fdt32_to_cpu(*cuint32)); ++ return -FDT_ERR_BADVALUE; ++ } ++ } ++ ++ cuint64 = fdt_getprop(fdt, subnode, "opp-hz", NULL); ++ if (cuint64 == NULL) { ++ DMSG("Missing opp-hz"); ++ return -FDT_ERR_NOTFOUND; ++ } ++ ++ /* Frequency value expressed in KHz must fit on 32 bits */ ++ read_freq_64 = fdt64_to_cpu(*cuint64) / 1000ULL; ++ if (read_freq_64 > (uint64_t)UINT32_MAX) { ++ DMSG("Invalid opp-hz %"PRIu64, read_freq_64); ++ return -FDT_ERR_BADVALUE; ++ } ++ ++ cuint32 = fdt_getprop(fdt, subnode, "opp-microvolt", NULL); ++ if (cuint32 == NULL) { ++ DMSG("Missing opp-microvolt"); ++ return -FDT_ERR_NOTFOUND; ++ } ++ ++ /* Millivolt value must fit on 16 bits */ ++ read_voltage_32 = fdt32_to_cpu(*cuint32) / 1000U; ++ if (read_voltage_32 > UINT16_MAX) { ++ DMSG("Invalid opp-microvolt %"PRIu32, read_voltage_32); ++ return -FDT_ERR_BADVALUE; ++ } ++ ++ *freq_khz = (uint32_t)read_freq_64; ++ ++ *voltage_mv = read_voltage_32; ++ ++ return 0; ++} ++ ++/******************************************************************************* ++ * This function parses OPP table in DT and finds all parameters supported by ++ * the HW platform. ++ * If found, the corresponding frequency and voltage values are respectively ++ * stored in @*freq_khz_array and @*voltage_mv_array. ++ * Note that @*count has to be set by caller to the effective size allocated ++ * for both tables. Its value is then replaced by the number of filled elements. ++ * Returns 0 on success and a negative FDT error code on failure. ++ ******************************************************************************/ ++int fdt_get_all_opp_freqvolt(void *fdt, uint32_t *count, ++ uint32_t *freq_khz_array, ++ uint32_t *voltage_mv_array) ++{ ++ int node; ++ int subnode; ++ uint32_t idx = 0U; ++ ++ assert(count != NULL); ++ assert(freq_khz_array != NULL); ++ assert(voltage_mv_array != NULL); ++ ++ node = fdt_get_opp_table_node(fdt); ++ if (node < 0) { ++ return node; ++ } ++ ++ fdt_for_each_subnode(subnode, fdt, node) { ++ uint32_t read_freq; ++ uint32_t read_voltage; ++ ++ if (fdt_get_opp_freqvolt_from_subnode(fdt, subnode, &read_freq, ++ &read_voltage) != 0) { ++ continue; ++ } ++ ++ if (idx >= *count) { ++ return -FDT_ERR_NOSPACE; ++ } ++ ++ freq_khz_array[idx] = read_freq; ++ voltage_mv_array[idx] = read_voltage; ++ idx++; ++ } ++ ++ if (idx == 0U) { ++ return -FDT_ERR_NOTFOUND; ++ } ++ ++ *count = idx; ++ ++ return 0; ++} ++ ++/******************************************************************************* + * This function retrieves board model from DT. + * Returns string taken from model node, NULL otherwise + ******************************************************************************/ +diff --git a/core/arch/arm/plat-stm32mp1/stm32mp_dt.h b/core/arch/arm/plat-stm32mp1/stm32mp_dt.h +index ec8aa4d..3229f2e 100644 +--- a/core/arch/arm/plat-stm32mp1/stm32mp_dt.h ++++ b/core/arch/arm/plat-stm32mp1/stm32mp_dt.h +@@ -1,6 +1,6 @@ + /* SPDX-License-Identifier: BSD-3-Clause */ + /* +- * Copyright (c) 2018, STMicroelectronics - All Rights Reserved ++ * Copyright (c) 2018-2019, STMicroelectronics - All Rights Reserved + * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved. + */ + +@@ -11,6 +11,7 @@ + #include + + #define DT_DDR_COMPAT "st,stm32mp1-ddr" ++#define DT_OPP_COMPAT "operating-points-v2" + + struct dt_node_info { + uint32_t base; +@@ -28,7 +29,14 @@ void fdt_fill_device_info(void *fdt, struct dt_node_info *info, int node); + int fdt_get_node(void *fdt, struct dt_node_info *info, int offset, + const char *compat); + int fdt_get_stdout_node_offset(void *fdt); ++int fdt_get_node_by_compatible(void *fdt, const char *compatible); ++int fdt_match_instance_by_compatible(void *fdt, const char *compatible, ++ uintptr_t address); ++uintptr_t fdt_get_peripheral_base(void *fdt, const char *compatible); + uint32_t fdt_get_ddr_size(void *fdt); ++int fdt_get_all_opp_freqvolt(void *fdt, uint32_t *count, ++ uint32_t *freq_khz_array, ++ uint32_t *voltage_mv_array); + const char *fdt_get_board_model(void *fdt); + + int fdt_get_clock_id(void *fdt, int node); +diff --git a/core/drivers/stm32_bsec.c b/core/drivers/stm32_bsec.c +index 7fcefdb..0e4c76b 100644 +--- a/core/drivers/stm32_bsec.c ++++ b/core/drivers/stm32_bsec.c +@@ -19,9 +19,12 @@ + + #ifdef CFG_DT + #include ++#include + #endif + +-#define BSEC_COMPAT "st,stm32mp15-bsec" ++#define BSEC_COMPAT "st,stm32mp15-bsec" ++#define DT_NVMEM_LAYOUT_COMPAT "st,stm32-nvmem-layout" ++ + #define BITS_PER_WORD (CHAR_BIT * sizeof(uint32_t)) + #define OTP_ACCESS_SIZE (ROUNDUP(OTP_MAX_SIZE, BITS_PER_WORD) / BITS_PER_WORD) + +@@ -76,35 +79,151 @@ static int bsec_dt_otp_nsec_access(void *fdt, int bsec_node) + + fdt_for_each_subnode(bsec_subnode, fdt, bsec_node) { + const fdt32_t *cuint; +- uint32_t reg; ++ uint32_t otp; + uint32_t i; + uint32_t size; +- uint8_t status; ++ uint32_t offset; ++ uint32_t length; + + cuint = fdt_getprop(fdt, bsec_subnode, "reg", NULL); + if (cuint == NULL) { + panic(); + } + +- reg = fdt32_to_cpu(*cuint) / sizeof(uint32_t); +- if (reg < STM32MP1_UPPER_OTP_START) { ++ offset = fdt32_to_cpu(*cuint); ++ cuint++; ++ length = fdt32_to_cpu(*cuint); ++ ++ otp = offset / sizeof(uint32_t); ++ ++ if (otp < STM32MP1_UPPER_OTP_START) { ++ unsigned int otp_end = ROUNDUP(offset + length, ++ sizeof(uint32_t)) / ++ sizeof(uint32_t); ++ ++ if (otp_end > STM32MP1_UPPER_OTP_START) { ++ /* ++ * OTP crosses Lower/Upper boundary, consider ++ * only the upper part. ++ */ ++ otp = STM32MP1_UPPER_OTP_START; ++ length -= (STM32MP1_UPPER_OTP_START * ++ sizeof(uint32_t)) - offset; ++ offset = STM32MP1_UPPER_OTP_START * ++ sizeof(uint32_t); ++ ++ IMSG("OTP crosses Lower/Upper boundary"); ++ } else { ++ continue; ++ } ++ } ++ ++ if ((fdt_getprop(fdt, bsec_subnode, ++ "st,non-secure-otp", NULL)) == NULL) { ++ continue; ++ } ++ ++ if (((offset % sizeof(uint32_t)) != 0) || ++ ((length % sizeof(uint32_t)) != 0)) { ++ EMSG("Unaligned non-secure OTP\n"); ++ panic(); ++ } ++ ++ size = length / sizeof(uint32_t); ++ ++ for (i = otp; i < (otp + size); i++) { ++ enable_non_secure_access(i); ++ } ++ } ++ ++ return 0; ++} ++ ++struct nvmem_layout { ++ char *name; ++ uint32_t number; ++ size_t bit_len; ++}; ++ ++static struct nvmem_layout *nvmem_layout; ++static size_t nvmem_layout_count; ++ ++static int save_dt_nvmem_layout(void *fdt) ++{ ++ const fdt32_t *cells; ++ int i; ++ int cell_nb; ++ int nvmem_node; ++ ++ nvmem_node = fdt_get_node_by_compatible(fdt, DT_NVMEM_LAYOUT_COMPAT); ++ if (nvmem_node < 0) { ++ return 0; ++ } ++ ++ cells = fdt_getprop(fdt, nvmem_node, "nvmem-cells", &cell_nb); ++ if (cells == NULL) { ++ cell_nb = 0; ++ } ++ ++ cell_nb /= sizeof(uint32_t); ++ ++ i = fdt_stringlist_count(fdt, nvmem_node, "nvmem-cell-names"); ++ if (i < 0) { ++ i = 0; ++ } ++ ++ if (cell_nb != i) { ++ EMSG("Inconsistent NVMEM layout"); ++ panic(); ++ } ++ ++ nvmem_layout = calloc(cell_nb, sizeof(*nvmem_layout)); ++ if (nvmem_layout == NULL) { ++ panic(); ++ } ++ ++ nvmem_layout_count = (size_t)cell_nb; ++ ++ for (i = 0; i < cell_nb; i++) { ++ const fdt32_t *cuint; ++ const char *string; ++ int len; ++ int node; ++ ++ node = fdt_node_offset_by_phandle(fdt, ++ fdt32_to_cpu(*(cells + i))); ++ if (node < 0) { ++ EMSG("Malformed nvmem_layout node: ignored"); ++ continue; ++ } ++ ++ cuint = fdt_getprop(fdt, node, "reg", &len); ++ if ((cuint == NULL) || (len != (2 * (int)sizeof(uint32_t)))) { ++ EMSG("Malformed nvmem_layout node: ignored"); + continue; + } + +- status = _fdt_get_status(fdt, bsec_subnode); +- if ((status & DT_STATUS_OK_NSEC) == 0U) { ++ if (fdt32_to_cpu(*cuint) % sizeof(uint32_t)) { ++ EMSG("Misaligned nvmem_layout element: ignored"); + continue; + } + +- size = fdt32_to_cpu(*(cuint + 1)) / sizeof(uint32_t); ++ nvmem_layout[i].number = fdt32_to_cpu(*cuint) / ++ sizeof(uint32_t); ++ nvmem_layout[i].bit_len = fdt32_to_cpu(*(cuint + 1)) * CHAR_BIT; + +- if ((fdt32_to_cpu(*(cuint + 1)) % sizeof(uint32_t)) != 0) { +- size++; ++ string = fdt_stringlist_get(fdt, nvmem_node, "nvmem-cell-names", ++ i, &len); ++ if ((string == NULL) || (len == 0)) { ++ continue; + } + +- for (i = reg; i < (reg + size); i++) { +- enable_non_secure_access(i); ++ nvmem_layout[i].name = calloc(1, len + 1); ++ if (nvmem_layout[i].name == NULL) { ++ panic(); + } ++ ++ memcpy(nvmem_layout[i].name, string, len); + } + + return 0; +@@ -158,6 +277,44 @@ static uint32_t bsec_check_error(uint32_t otp, bool check_disturbed) + return BSEC_OK; + } + ++#ifdef CFG_DT ++/* ++ * bsec_find_otp_name_in_nvmem_layout: find and get OTP location from its name. ++ * name: sub-node name to look up. ++ * otp: pointer to read OTP number or NULL. ++ * otp_bit_len: pointer to read OTP length in bits or NULL. ++ * return value: BSEC_OK if no error. ++ */ ++uint32_t bsec_find_otp_name_in_nvmem_layout(const char *name, uint32_t *otp, ++ uint32_t *otp_bit_len) ++{ ++ size_t i; ++ ++ if (name == NULL) { ++ return BSEC_INVALID_PARAM; ++ } ++ ++ for (i = 0; i < nvmem_layout_count; i++) { ++ if (!nvmem_layout[i].name || ++ strcmp(name, nvmem_layout[i].name)) { ++ continue; ++ } ++ ++ if (otp != NULL) { ++ *otp = nvmem_layout[i].number; ++ } ++ ++ if (otp_bit_len != NULL) { ++ *otp_bit_len = nvmem_layout[i].bit_len; ++ } ++ ++ return BSEC_OK; ++ } ++ ++ return BSEC_ERROR; ++} ++#endif ++ + /* + * bsec_shadow_register: copy SAFMEM OTP to BSEC data. + * otp: OTP number. +@@ -209,11 +366,8 @@ uint32_t bsec_shadow_register(uint32_t otp) + */ + uint32_t bsec_read_otp(uint32_t *val, uint32_t otp) + { +- uint32_t exc; +- +- if (otp > stm32mp_get_otp_max()) { ++ if (otp > stm32mp_get_otp_max()) + return BSEC_INVALID_PARAM; +- } + + *val = read32(bsec_get_base() + BSEC_OTP_DATA_OFF + + (otp * sizeof(uint32_t))); +@@ -747,6 +901,8 @@ static TEE_Result initialize_bsec(void) + + bsec_dt_otp_nsec_access(fdt, node); + ++ save_dt_nvmem_layout(fdt); ++ + return TEE_SUCCESS; + } + driver_init(initialize_bsec); +diff --git a/core/drivers/stm32_i2c.c b/core/drivers/stm32_i2c.c +index 8ba7178..a68ef96 100644 +--- a/core/drivers/stm32_i2c.c ++++ b/core/drivers/stm32_i2c.c +@@ -1,6 +1,6 @@ + // SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause + /* +- * Copyright (c) 2016-2018, STMicroelectronics - All Rights Reserved ++ * Copyright (c) 2016-2019, STMicroelectronics - All Rights Reserved + */ + + #include +@@ -14,6 +14,7 @@ + #include + #include + #include ++#include + + /* STM32 I2C registers offsets */ + #define I2C_CR1 0x00U +@@ -32,13 +33,14 @@ + + #define MAX_NBYTE_SIZE 255U + +-#define I2C_NSEC_PER_SEC 1000000000L ++#define I2C_NSEC_PER_SEC 1000000000UL ++ ++/* Effective rate cannot be lower than 80% target rate */ ++#define RATE_MIN(rate) (((rate) * 80U) / 100U) + + /* + * struct i2c_spec_s - Private I2C timing specifications. + * @rate: I2C bus speed (Hz) +- * @rate_min: 80% of I2C bus speed (Hz) +- * @rate_max: 120% of I2C bus speed (Hz) + * @fall_max: Max fall time of both SDA and SCL signals (ns) + * @rise_max: Max rise time of both SDA and SCL signals (ns) + * @hddat_min: Min data hold time (ns) +@@ -49,8 +51,6 @@ + */ + struct i2c_spec_s { + uint32_t rate; +- uint32_t rate_min; +- uint32_t rate_max; + uint32_t fall_max; + uint32_t rise_max; + uint32_t hddat_min; +@@ -83,11 +83,11 @@ struct i2c_timing_s { + * + * [1] https://www.nxp.com/docs/en/user-guide/UM10204.pdf + */ ++/* This table must be sorted in increasing value for field @rate */ + static const struct i2c_spec_s i2c_specs[] = { +- [I2C_SPEED_STANDARD] = { ++ /* Standard - 100KHz */ ++ { + .rate = STANDARD_RATE, +- .rate_min = (STANDARD_RATE * 80) / 100, +- .rate_max = (STANDARD_RATE * 120) / 100, + .fall_max = 300, + .rise_max = 1000, + .hddat_min = 0, +@@ -96,10 +96,9 @@ static const struct i2c_spec_s i2c_specs[] = { + .l_min = 4700, + .h_min = 4000, + }, +- [I2C_SPEED_FAST] = { ++ /* Fast - 400KHz */ ++ { + .rate = FAST_RATE, +- .rate_min = (FAST_RATE * 80) / 100, +- .rate_max = (FAST_RATE * 120) / 100, + .fall_max = 300, + .rise_max = 300, + .hddat_min = 0, +@@ -108,10 +107,9 @@ static const struct i2c_spec_s i2c_specs[] = { + .l_min = 1300, + .h_min = 600, + }, +- [I2C_SPEED_FAST_PLUS] = { ++ /* FastPlus - 1MHz */ ++ { + .rate = FAST_PLUS_RATE, +- .rate_min = (FAST_PLUS_RATE * 80) / 100, +- .rate_max = (FAST_PLUS_RATE * 120) / 100, + .fall_max = 100, + .rise_max = 120, + .hddat_min = 0, +@@ -184,6 +182,19 @@ static void notif_i2c_timeout(struct i2c_handle_s *hi2c) + hi2c->i2c_state = I2C_STATE_READY; + } + ++static const struct i2c_spec_s *get_specs(uint32_t rate) ++{ ++ size_t i; ++ ++ for (i = 0; i < ARRAY_SIZE(i2c_specs); i++) { ++ if (rate <= i2c_specs[i].rate) { ++ return &i2c_specs[i]; ++ } ++ } ++ ++ return NULL; ++} ++ + static void save_cfg(struct i2c_handle_s *hi2c, struct i2c_cfg *cfg) + { + uintptr_t base = get_base(hi2c); +@@ -222,11 +233,11 @@ static void restore_cfg(struct i2c_handle_s *hi2c, struct i2c_cfg *cfg) + + static void __maybe_unused dump_cfg(struct i2c_cfg *cfg __maybe_unused) + { +- DMSG("CR1: %x", (unsigned)cfg->cr1); +- DMSG("CR2: %x", (unsigned)cfg->cr2); +- DMSG("OAR1: %x", (unsigned)cfg->oar1); +- DMSG("OAR2: %x", (unsigned)cfg->oar2); +- DMSG("TIM: %x", (unsigned)cfg->timingr); ++ DMSG("CR1: %"PRIx32, cfg->cr1); ++ DMSG("CR2: %"PRIx32, cfg->cr2); ++ DMSG("OAR1: %"PRIx32, cfg->oar1); ++ DMSG("OAR2: %"PRIx32, cfg->oar2); ++ DMSG("TIM: %"PRIx32, cfg->timingr); + } + + static void __maybe_unused dump_i2c(struct i2c_handle_s *hi2c) +@@ -235,11 +246,11 @@ static void __maybe_unused dump_i2c(struct i2c_handle_s *hi2c) + + stm32_clock_enable(hi2c->clock); + +- DMSG("CR1: %x", (unsigned)mmio_read_32(base + I2C_CR1)); +- DMSG("CR2: %x", (unsigned)mmio_read_32(base + I2C_CR2)); +- DMSG("OAR1: %x", (unsigned)mmio_read_32(base + I2C_OAR1)); +- DMSG("OAR2: %x", (unsigned)mmio_read_32(base + I2C_OAR2)); +- DMSG("TIM: %x", (unsigned)mmio_read_32(base + I2C_TIMINGR)); ++ DMSG("CR1: %"PRIx32, mmio_read_32(base + I2C_CR1)); ++ DMSG("CR2: %"PRIx32, mmio_read_32(base + I2C_CR2)); ++ DMSG("OAR1: %"PRIx32, mmio_read_32(base + I2C_OAR1)); ++ DMSG("OAR2: %"PRIx32, mmio_read_32(base + I2C_OAR2)); ++ DMSG("TIM: %"PRIx32, mmio_read_32(base + I2C_TIMINGR)); + + stm32_clock_disable(hi2c->clock); + } +@@ -254,7 +265,7 @@ static void __maybe_unused dump_i2c(struct i2c_handle_s *hi2c) + static int i2c_compute_timing(struct stm32_i2c_init_s *init, + uint32_t clock_src, uint32_t *timing) + { +- enum i2c_speed_e mode = init->speed_mode; ++ const struct i2c_spec_s *specs; + uint32_t speed_freq; + uint32_t i2cclk = UDIV_ROUND_NEAREST(I2C_NSEC_PER_SEC, clock_src); + uint32_t i2cbus; +@@ -277,31 +288,26 @@ static int i2c_compute_timing(struct stm32_i2c_init_s *init, + int s = -1; + struct i2c_timing_s solutions[I2C_TIMINGR_PRESC_MAX]; + +- switch (mode) { +- case I2C_SPEED_STANDARD: +- case I2C_SPEED_FAST: +- case I2C_SPEED_FAST_PLUS: +- break; +- default: +- EMSG("I2C speed out of bound {%d/%d}\n", +- mode, I2C_SPEED_FAST_PLUS); ++ specs = get_specs(init->bus_rate); ++ if (specs == NULL) { ++ EMSG("I2C speed out of bound: %"PRId32"Hz", init->bus_rate); + return -1; + } + +- speed_freq = i2c_specs[mode].rate; ++ speed_freq = specs->rate; + i2cbus = UDIV_ROUND_NEAREST(I2C_NSEC_PER_SEC, speed_freq); + clk_error_prev = INT_MAX; + +- if ((init->rise_time > i2c_specs[mode].rise_max) || +- (init->fall_time > i2c_specs[mode].fall_max)) { +- EMSG(" I2C timings out of bound Rise{%d>%d}/Fall{%d>%d}\n", +- init->rise_time, i2c_specs[mode].rise_max, +- init->fall_time, i2c_specs[mode].fall_max); ++ if ((init->rise_time > specs->rise_max) || ++ (init->fall_time > specs->fall_max)) { ++ EMSG(" I2C timings out of bound Rise{%"PRId32">%"PRId32"}/Fall{%"PRId32">%"PRId32"}", ++ init->rise_time, specs->rise_max, ++ init->fall_time, specs->fall_max); + return -1; + } + + if (init->digital_filter_coef > STM32_I2C_DIGITAL_FILTER_MAX) { +- EMSG("DNF out of bound %d/%d\n", ++ EMSG("DNF out of bound %"PRId8"/%d", + init->digital_filter_coef, STM32_I2C_DIGITAL_FILTER_MAX); + return -1; + } +@@ -313,19 +319,19 @@ static int i2c_compute_timing(struct stm32_i2c_init_s *init, + STM32_I2C_ANALOG_FILTER_DELAY_MAX : 0); + dnf_delay = init->digital_filter_coef * i2cclk; + +- sdadel_min = i2c_specs[mode].hddat_min + init->fall_time; ++ sdadel_min = specs->hddat_min + init->fall_time; + delay = af_delay_min - ((init->digital_filter_coef + 3) * i2cclk); + if (SUB_OVERFLOW(sdadel_min, delay ,&sdadel_min)) + sdadel_min = 0; + +- sdadel_max = i2c_specs[mode].vddat_max - init->rise_time; ++ sdadel_max = specs->vddat_max - init->rise_time; + delay = af_delay_max - ((init->digital_filter_coef + 4) * i2cclk); + if (SUB_OVERFLOW(sdadel_max, delay ,&sdadel_max)) + sdadel_max = 0; + +- scldel_min = init->rise_time + i2c_specs[mode].sudat_min; ++ scldel_min = init->rise_time + specs->sudat_min; + +- DMSG("I2C SDADEL(min/max): %u/%u, SCLDEL(Min): %u\n", ++ DMSG("I2C SDADEL(min/max): %u/%u, SCLDEL(Min): %u", + sdadel_min, sdadel_max, scldel_min); + + memset(&solutions, 0, sizeof(solutions)); +@@ -360,13 +366,13 @@ static int i2c_compute_timing(struct stm32_i2c_init_s *init, + } + + if (p_prev == I2C_TIMINGR_PRESC_MAX) { +- EMSG(" I2C no Prescaler solution\n"); ++ EMSG(" I2C no Prescaler solution"); + return -1; + } + + tsync = af_delay_min + dnf_delay + (2 * i2cclk); +- clk_max = I2C_NSEC_PER_SEC / i2c_specs[mode].rate_min; +- clk_min = I2C_NSEC_PER_SEC / i2c_specs[mode].rate_max; ++ clk_max = I2C_NSEC_PER_SEC / RATE_MIN(specs->rate); ++ clk_min = I2C_NSEC_PER_SEC / specs->rate; + + /* + * Among prescaler possibilities discovered above figures out SCL Low +@@ -388,7 +394,7 @@ static int i2c_compute_timing(struct stm32_i2c_init_s *init, + for (l = 0; l < I2C_TIMINGR_SCLL_MAX; l++) { + uint32_t tscl_l = ((l + 1) * prescaler) + tsync; + +- if ((tscl_l < i2c_specs[mode].l_min) || ++ if ((tscl_l < specs->l_min) || + (i2cclk >= + ((tscl_l - af_delay_min - dnf_delay) / 4))) { + continue; +@@ -401,7 +407,7 @@ static int i2c_compute_timing(struct stm32_i2c_init_s *init, + init->fall_time; + + if ((tscl >= clk_min) && (tscl <= clk_max) && +- (tscl_h >= i2c_specs[mode].h_min) && ++ (tscl_h >= specs->h_min) && + (i2cclk < tscl_h)) { + int clk_error = tscl - i2cbus; + +@@ -421,7 +427,7 @@ static int i2c_compute_timing(struct stm32_i2c_init_s *init, + } + + if (s < 0) { +- EMSG(" I2C no solution at all\n"); ++ EMSG(" I2C no solution at all"); + return -1; + } + +@@ -432,15 +438,47 @@ static int i2c_compute_timing(struct stm32_i2c_init_s *init, + I2C_SET_TIMINGR_SCLH(solutions[s].sclh) | + I2C_SET_TIMINGR_SCLL(solutions[s].scll); + +- DMSG("I2C TIMINGR (PRESC/SCLDEL/SDADEL): %i/%i/%i\n", ++ DMSG("I2C TIMINGR (PRESC/SCLDEL/SDADEL): %i/%"PRIu8"/%"PRIu8"", + s, solutions[s].scldel, solutions[s].sdadel); +- DMSG("I2C TIMINGR (SCLH/SCLL): %i/%i\n", ++ DMSG("I2C TIMINGR (SCLH/SCLL): %"PRIu8"/%"PRIu8"", + solutions[s].sclh, solutions[s].scll); +- DMSG("I2C TIMINGR: 0x%x\n", *timing); ++ DMSG("I2C TIMINGR: 0x%"PRIx32"", *timing); + + return 0; + } + ++ ++/* i2c_specs[] must be sorted by increasing rate */ ++static bool __maybe_unused i2c_specs_is_consistent(void) ++{ ++ size_t i; ++ ++ COMPILE_TIME_ASSERT(ARRAY_SIZE(i2c_specs)); ++ ++ for (i = 1; i < ARRAY_SIZE(i2c_specs); i++) ++ if (i2c_specs[i - 1].rate >= i2c_specs[i].rate) ++ return false; ++ ++ return true; ++} ++ ++/* ++ * @brief From requested rate, get the closest I2C rate without exceeding it, ++ * within I2C specification values defined in @i2c_specs. ++ * @param rate: The requested rate. ++ * @retval Found rate, else the lowest value supported by platform. ++ */ ++static uint32_t get_lower_rate(uint32_t rate) ++{ ++ int i; ++ ++ for (i = (int)ARRAY_SIZE(i2c_specs) - 1; i >= 0; i--) ++ if (rate > i2c_specs[i].rate) ++ return i2c_specs[i].rate; ++ ++ return i2c_specs[0].rate; ++} ++ + /* + * @brief Setup the I2C device timings. + * @param hi2c: Pointer to a struct i2c_handle_s structure that contains +@@ -456,20 +494,31 @@ static int i2c_setup_timing(struct i2c_handle_s *hi2c, + int rc = 0; + uint32_t clock_src; + +- clock_src = stm32_clock_get_rate(hi2c->clock); ++ assert(i2c_specs_is_consistent()); ++ ++ clock_src = (uint32_t)stm32_clock_get_rate(hi2c->clock); + if (clock_src == 0U) { +- EMSG("I2C clock rate is 0\n"); ++ EMSG("I2C clock rate is 0"); + return -1; + } + ++ /* ++ * If the timing has already been computed, and the frequency is the ++ * same as when it was computed, then use the saved timing. ++ */ ++ if (clock_src == hi2c->saved_frequency) { ++ *timing = hi2c->saved_timing; ++ return 0; ++ } ++ + do { + rc = i2c_compute_timing(init, clock_src, timing); + if (rc != 0) { +- EMSG("Failed to compute I2C timings\n"); +- if (init->speed_mode > I2C_SPEED_STANDARD) { +- init->speed_mode--; +- IMSG("Downgrade I2C speed to %uHz)\n", +- i2c_specs[init->speed_mode].rate); ++ EMSG("Failed to compute I2C timings"); ++ if (init->bus_rate > STANDARD_RATE) { ++ init->bus_rate = get_lower_rate(init->bus_rate); ++ IMSG("Downgrade I2C speed to %"PRIu32"Hz)", ++ init->bus_rate); + } else { + break; + } +@@ -477,17 +526,20 @@ static int i2c_setup_timing(struct i2c_handle_s *hi2c, + } while (rc != 0); + + if (rc != 0) { +- EMSG("Impossible to compute I2C timings\n"); ++ EMSG("Impossible to compute I2C timings"); + return rc; + } + +- DMSG("I2C Speed Mode(%i), Freq(%i), Clk Source(%i)\n", +- init->speed_mode, i2c_specs[init->speed_mode].rate, clock_src); +- DMSG("I2C Rise(%i) and Fall(%i) Time\n", ++ DMSG("I2C Freq(%"PRIu32"Hz), Clk Source(%"PRId32")", ++ init->bus_rate, clock_src); ++ DMSG("I2C Rise(%"PRId32") and Fall(%"PRId32") Time", + init->rise_time, init->fall_time); +- DMSG("I2C Analog Filter(%s), DNF(%i)\n", ++ DMSG("I2C Analog Filter(%s), DNF(%"PRId8")", + (init->analog_filter ? "On" : "Off"), init->digital_filter_coef); + ++ hi2c->saved_timing = *timing; ++ hi2c->saved_frequency = clock_src; ++ + return 0; + } + +@@ -543,43 +595,27 @@ int stm32_i2c_get_setup_from_fdt(void *fdt, int node, + struct stm32_pinctrl **pinctrl, + size_t *pinctrl_count) + { +- const fdt32_t *cuint; ++ uint32_t read_val; + int count; + +- cuint = fdt_getprop(fdt, node, "i2c-scl-rising-time-ns", NULL); +- if (cuint == NULL) { +- init->rise_time = STM32_I2C_RISE_TIME_DEFAULT; +- } else { +- init->rise_time = fdt32_to_cpu(*cuint); +- } ++ init->rise_time = fdt_read_uint32_default(fdt, node, ++ "i2c-scl-rising-time-ns", ++ STM32_I2C_RISE_TIME_DEFAULT); + +- cuint = fdt_getprop(fdt, node, "i2c-scl-falling-time-ns", NULL); +- if (cuint == NULL) { +- init->fall_time = STM32_I2C_FALL_TIME_DEFAULT; +- } else { +- init->fall_time = fdt32_to_cpu(*cuint); +- } ++ init->fall_time = fdt_read_uint32_default(fdt, node, ++ "i2c-scl-falling-time-ns", ++ STM32_I2C_FALL_TIME_DEFAULT); + +- cuint = fdt_getprop(fdt, node, "clock-frequency", NULL); +- if (cuint == NULL) { +- init->speed_mode = STM32_I2C_SPEED_DEFAULT; +- } else { +- switch (fdt32_to_cpu(*cuint)) { +- case STANDARD_RATE: +- init->speed_mode = I2C_SPEED_STANDARD; +- break; +- case FAST_RATE: +- init->speed_mode = I2C_SPEED_FAST; +- break; +- case FAST_PLUS_RATE: +- init->speed_mode = I2C_SPEED_FAST_PLUS; +- break; +- default: +- init->speed_mode = STM32_I2C_SPEED_DEFAULT; +- break; +- } ++ read_val = fdt_read_uint32_default(fdt, node, "clock-frequency", ++ STANDARD_RATE); ++ if (read_val > FAST_PLUS_RATE) { ++ EMSG("Invalid bus speed (%"PRIu32" > %i)", ++ read_val, FAST_PLUS_RATE); ++ return -FDT_ERR_BADVALUE; + } + ++ init->bus_rate = read_val; ++ + count = stm32_pinctrl_fdt_get_pinctrl(fdt, node, NULL, 0); + if (count <= 0) { + *pinctrl = NULL; +@@ -691,7 +727,7 @@ int stm32_i2c_init(struct i2c_handle_s *hi2c, + I2C_ANALOGFILTER_ENABLE : + I2C_ANALOGFILTER_DISABLE); + if (rc != 0) { +- EMSG("Cannot initialize I2C analog filter (%d)\n", rc); ++ EMSG("Cannot initialize I2C analog filter (%d)", rc); + stm32_clock_disable(hi2c->clock); + return rc; + } +diff --git a/core/drivers/stm32_iwdg.c b/core/drivers/stm32_iwdg.c +index f1cdc1a..f042808 100644 +--- a/core/drivers/stm32_iwdg.c ++++ b/core/drivers/stm32_iwdg.c +@@ -1,6 +1,6 @@ + // SPDX-License-Identifier: BSD-3-Clause + /* +- * Copyright (c) 2017-2018, STMicroelectronics - All Rights Reserved ++ * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved + * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved. + */ + +@@ -202,13 +202,14 @@ static int stm32_iwdg_conf_etimeout(void *fdt, int node, + void stm32_iwdg_refresh(uint32_t instance) + { + struct stm32_iwdg_instance *iwdg = get_iwdg(instance); +- uintptr_t iwdg_base = get_base(iwdg); + +- assert(iwdg); ++ if (iwdg == NULL) { ++ return; ++ } + + stm32_clock_enable(iwdg->clock); + +- write32(IWDG_KR_RELOAD_KEY, iwdg_base + IWDG_KR_OFFSET); ++ write32(IWDG_KR_RELOAD_KEY, get_base(iwdg) + IWDG_KR_OFFSET); + + stm32_clock_disable(iwdg->clock); + } +@@ -246,14 +247,14 @@ static TEE_Result iwdg_init(void) + iwdg.vbase = (uintptr_t)phys_to_virt(iwdg.pbase, memtype); + + /* DT can specify low power cases */ +- if (fdt_getprop(fdt, node, "stm32,enable-on-stop", NULL) != ++ if (fdt_getprop(fdt, node, "stm32,enable-on-stop", NULL) == + NULL) { +- iwdg.flags |= IWDG_ENABLE_ON_STOP; ++ iwdg.flags |= IWDG_DISABLE_ON_STOP; + } + +- if (fdt_getprop(fdt, node, "stm32,enable-on-standby", NULL) != ++ if (fdt_getprop(fdt, node, "stm32,enable-on-standby", NULL) == + NULL) { +- iwdg.flags |= IWDG_ENABLE_ON_STANDBY; ++ iwdg.flags |= IWDG_DISABLE_ON_STANDBY; + } + + hw_init = stm32_get_iwdg_otp_config(iwdg.pbase); +@@ -265,12 +266,12 @@ static TEE_Result iwdg_init(void) + iwdg.flags |= IWDG_HW_ENABLED; + } + +- if ((hw_init & IWDG_ENABLE_ON_STOP) != 0) { +- iwdg.flags |= IWDG_ENABLE_ON_STOP; ++ if ((hw_init & IWDG_DISABLE_ON_STOP) != 0) { ++ iwdg.flags |= IWDG_DISABLE_ON_STOP; + } + +- if ((hw_init & IWDG_ENABLE_ON_STANDBY) != 0) { +- iwdg.flags |= IWDG_ENABLE_ON_STANDBY; ++ if ((hw_init & IWDG_DISABLE_ON_STANDBY) != 0) { ++ iwdg.flags |= IWDG_DISABLE_ON_STANDBY; + } + + if (dt_info.status == DT_STATUS_DISABLED) { +diff --git a/core/drivers/stm32_rtc.c b/core/drivers/stm32_rtc.c +index 709fafa..9834120 100644 +--- a/core/drivers/stm32_rtc.c ++++ b/core/drivers/stm32_rtc.c +@@ -233,20 +233,11 @@ static uint32_t stm32_rtc_get_second_fraction(struct stm32_rtc_calendar *cal) + * This function computes the fraction difference between two timestamps. + * Here again the returned value is in milliseconds. + ******************************************************************************/ +-static unsigned long long stm32_rtc_diff_frac(struct stm32_rtc_calendar *cur, +- struct stm32_rtc_calendar *ref) ++static signed long long stm32_rtc_diff_frac(struct stm32_rtc_calendar *cur, ++ struct stm32_rtc_calendar *ref) + { +- unsigned long long val_r; +- unsigned long long val_c; +- +- val_r = stm32_rtc_get_second_fraction(ref); +- val_c = stm32_rtc_get_second_fraction(cur); +- +- if (val_c >= val_r) { +- return val_c - val_r; +- } else { +- return 1000U - val_r + val_c; +- } ++ return (signed long long)stm32_rtc_get_second_fraction(cur) - ++ (signed long long)stm32_rtc_get_second_fraction(ref); + } + + /******************************************************************************* +@@ -254,10 +245,9 @@ static unsigned long long stm32_rtc_diff_frac(struct stm32_rtc_calendar *cur, + * It includes seconds, minutes and hours. + * Here again the returned value is in milliseconds. + ******************************************************************************/ +-static unsigned long long stm32_rtc_diff_time(struct stm32_rtc_time *current, +- struct stm32_rtc_time *ref) ++static signed long long stm32_rtc_diff_time(struct stm32_rtc_time *current, ++ struct stm32_rtc_time *ref) + { +- signed long long diff_in_s; + signed long long curr_s; + signed long long ref_s; + +@@ -269,12 +259,7 @@ static unsigned long long stm32_rtc_diff_time(struct stm32_rtc_time *current, + (((signed long long)ref->min + + (((signed long long)ref->hour * 60))) * 60); + +- diff_in_s = curr_s - ref_s; +- if (diff_in_s < 0) { +- diff_in_s += 24 * 60 * 60; +- } +- +- return (unsigned long long)diff_in_s * 1000U; ++ return (curr_s - ref_s) * 1000U; + } + + /******************************************************************************* +@@ -292,8 +277,8 @@ static bool stm32_is_a_leap_year(uint32_t year) + * It includes days, months, years, with exceptions. + * Here again the returned value is in milliseconds. + ******************************************************************************/ +-static unsigned long long stm32_rtc_diff_date(struct stm32_rtc_time *current, +- struct stm32_rtc_time *ref) ++static signed long long stm32_rtc_diff_date(struct stm32_rtc_time *current, ++ struct stm32_rtc_time *ref) + { + uint32_t diff_in_days = 0; + uint32_t m; +@@ -367,7 +352,7 @@ static unsigned long long stm32_rtc_diff_date(struct stm32_rtc_time *current, + } + } + +- return (24ULL * 60U * 60U * 1000U) * (unsigned long long)diff_in_days; ++ return (24 * 60 * 60 * 1000) * (signed long long)diff_in_days; + } + + /******************************************************************************* +@@ -377,7 +362,7 @@ static unsigned long long stm32_rtc_diff_date(struct stm32_rtc_time *current, + unsigned long long stm32_rtc_diff_calendar(struct stm32_rtc_calendar *cur, + struct stm32_rtc_calendar *ref) + { +- unsigned long long diff_in_ms = 0; ++ signed long long diff_in_ms = 0; + struct stm32_rtc_time curr_t; + struct stm32_rtc_time ref_t; + +@@ -394,7 +379,7 @@ unsigned long long stm32_rtc_diff_calendar(struct stm32_rtc_calendar *cur, + + stm32_clock_disable(rtc_dev.clock); + +- return diff_in_ms; ++ return (unsigned long long)diff_in_ms; + } + + /******************************************************************************* +diff --git a/core/drivers/stpmic1.c b/core/drivers/stpmic1.c +index 3831d40..d80edbc 100644 +--- a/core/drivers/stpmic1.c ++++ b/core/drivers/stpmic1.c +@@ -1,6 +1,6 @@ + // SPDX-License-Identifier: BSD-3-Clause + /* +- * Copyright (c) 2016-2018, STMicroelectronics - All Rights Reserved ++ * Copyright (c) 2016-2019, STMicroelectronics - All Rights Reserved + */ + + #include +@@ -347,6 +347,7 @@ static const uint16_t ldo3_voltage_table[] = { + 3300, + 3300, + 3300, ++ 500, /* VOUT2/2 (Sink/source mode) */ + 0xFFFF, /* VREFDDR */ + }; + +@@ -582,17 +583,19 @@ int stpmic1_regulator_enable(const char *name) + { + const struct regul_struct *regul = get_regulator_data(name); + +- return stpmic1_register_update(regul->control_reg, BIT(0), BIT(0)); ++ return stpmic1_register_update(regul->control_reg, LDO_BUCK_ENABLE_MASK, ++ LDO_BUCK_ENABLE_MASK); + } + + int stpmic1_regulator_disable(const char *name) + { + const struct regul_struct *regul = get_regulator_data(name); + +- return stpmic1_register_update(regul->control_reg, 0, BIT(0)); ++ return stpmic1_register_update(regul->control_reg, 0, ++ LDO_BUCK_ENABLE_MASK); + } + +-uint8_t stpmic1_is_regulator_enabled(const char *name) ++bool stpmic1_is_regulator_enabled(const char *name) + { + uint8_t val; + const struct regul_struct *regul = get_regulator_data(name); +@@ -601,7 +604,7 @@ uint8_t stpmic1_is_regulator_enabled(const char *name) + panic(); + } + +- return (val & 0x1U); ++ return (val & LDO_BUCK_ENABLE_MASK) == LDO_BUCK_ENABLE_MASK; + } + + int stpmic1_regulator_voltage_set(const char *name, uint16_t millivolts) +@@ -635,9 +638,19 @@ int stpmic1_regulator_mask_reset_set(const char *name) + regul->mask_reset); + } + ++int stpmic1_bo_enable_cfg(const char *name, struct stpmic1_bo_cfg *cfg) ++{ ++ const struct regul_struct *regul = get_regulator_data(name); ++ ++ cfg->ctrl_reg = regul->control_reg; ++ ++ return 0; ++} ++ + int stpmic1_bo_enable_unpg(struct stpmic1_bo_cfg *cfg) + { +- return stpmic1_register_update(cfg->ctrl_reg, BIT(0), BIT(0)); ++ return stpmic1_register_update(cfg->ctrl_reg, LDO_BUCK_ENABLE_MASK, ++ LDO_BUCK_ENABLE_MASK); + } + + /* Returns 1 if no configuration are expected applied at runtime, 0 otherwise */ +diff --git a/core/include/drivers/stm32_bsec.h b/core/include/drivers/stm32_bsec.h +index bd33a7b..ee3c918 100644 +--- a/core/include/drivers/stm32_bsec.h ++++ b/core/include/drivers/stm32_bsec.h +@@ -112,6 +112,19 @@ + #define FREQ_30_45_MHZ 0x2 + #define FREQ_45_67_MHZ 0x3 + ++#ifdef CFG_DT ++uint32_t bsec_find_otp_name_in_nvmem_layout(const char *name, uint32_t *otp, ++ uint32_t *otp_bit_len); ++#else ++static inline uint32_t bsec_find_otp_name_in_nvmem_layout( ++ const char *name __unused, ++ uint32_t *otp __unused, ++ uint32_t *otp_bit_len __unused) ++{ ++ return BSEC_ERROR; ++} ++#endif ++ + uint32_t bsec_shadow_register(uint32_t otp); + uint32_t bsec_read_otp(uint32_t *val, uint32_t otp); + uint32_t bsec_write_otp(uint32_t val, uint32_t otp); +diff --git a/core/include/drivers/stm32_i2c.h b/core/include/drivers/stm32_i2c.h +index 3c6e326..83a5b52 100644 +--- a/core/include/drivers/stm32_i2c.h ++++ b/core/include/drivers/stm32_i2c.h +@@ -1,6 +1,6 @@ + /* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */ + /* +- * Copyright (c) 2016-2018, STMicroelectronics - All Rights Reserved ++ * Copyright (c) 2016-2019, STMicroelectronics - All Rights Reserved + */ + + #ifndef __STM32_I2C_H +@@ -134,15 +134,9 @@ + #define I2C_ICR_TIMOUTCF BIT(12) + #define I2C_ICR_ALERTCF BIT(13) + +-enum i2c_speed_e { +- I2C_SPEED_STANDARD, /* 100 kHz */ +- I2C_SPEED_FAST, /* 400 kHz */ +- I2C_SPEED_FAST_PLUS, /* 1 MHz */ +-}; +- +-#define STANDARD_RATE 100000 +-#define FAST_RATE 400000 +-#define FAST_PLUS_RATE 1000000 ++#define STANDARD_RATE 100000 ++#define FAST_RATE 400000 ++#define FAST_PLUS_RATE 1000000 + + struct stm32_i2c_init_s { + uint32_t own_address1; /* +@@ -204,12 +198,7 @@ struct stm32_i2c_init_s { + * time in nanoseconds. + */ + +- enum i2c_speed_e speed_mode; /* +- * Specifies the I2C clock source +- * frequency mode. +- * This parameter can be a value of @ref +- * i2c_speed_mode_e. +- */ ++ uint32_t bus_rate; /* Specifies the I2C clock frequency */ + + int analog_filter; /* + * Specifies if the I2C analog noise +@@ -273,6 +262,8 @@ struct i2c_handle_s { + enum i2c_state_e i2c_state; /* Communication state */ + enum i2c_mode_e i2c_mode; /* Communication mode */ + uint32_t i2c_err; /* Error code */ ++ uint32_t saved_timing; /* Saved timing value */ ++ uint32_t saved_frequency; /* Saved frequency value */ + struct stm32_pinctrl *pinctrl; /* PINCTRLs configuration for the I2C PINs */ + size_t pinctrl_count; /* Number of PINCTRLs elements */ + struct i2c_cfg sec_cfg; /* Context for secure usage */ +@@ -335,7 +326,6 @@ struct i2c_handle_s { + /* STM32 specific defines */ + #define STM32_I2C_RISE_TIME_DEFAULT 25 /* ns */ + #define STM32_I2C_FALL_TIME_DEFAULT 10 /* ns */ +-#define STM32_I2C_SPEED_DEFAULT I2C_SPEED_STANDARD + #define STM32_I2C_ANALOG_FILTER_DELAY_MIN 50 /* ns */ + #define STM32_I2C_ANALOG_FILTER_DELAY_MAX 260 /* ns */ + #define STM32_I2C_DIGITAL_FILTER_MAX 16 +diff --git a/core/include/drivers/stm32_iwdg.h b/core/include/drivers/stm32_iwdg.h +index b649dee..7dbfbbf 100644 +--- a/core/include/drivers/stm32_iwdg.h ++++ b/core/include/drivers/stm32_iwdg.h +@@ -1,6 +1,6 @@ + /* SPDX-License-Identifier: BSD-3-Clause */ + /* +- * Copyright (c) 2018, STMicroelectronics - All Rights Reserved ++ * Copyright (c) 2018-2019, STMicroelectronics - All Rights Reserved + */ + + #ifndef __STM32_IWDG_H__ +@@ -9,8 +9,8 @@ + #include + + #define IWDG_HW_ENABLED BIT(0) +-#define IWDG_ENABLE_ON_STOP BIT(1) +-#define IWDG_ENABLE_ON_STANDBY BIT(2) ++#define IWDG_DISABLE_ON_STOP BIT(1) ++#define IWDG_DISABLE_ON_STANDBY BIT(2) + + void stm32_iwdg_refresh(uint32_t instance); + +diff --git a/core/include/drivers/stpmic1.h b/core/include/drivers/stpmic1.h +index 3073662..7be6931 100644 +--- a/core/include/drivers/stpmic1.h ++++ b/core/include/drivers/stpmic1.h +@@ -1,6 +1,6 @@ + /* SPDX-License-Identifier: BSD-3-Clause */ + /* +- * Copyright (c) 2016-2018, STMicroelectronics - All Rights Reserved ++ * Copyright (c) 2016-2019, STMicroelectronics - All Rights Reserved + */ + + #ifndef __STPMIC1_H__ +@@ -85,15 +85,15 @@ + #define ITSOURCE4_REG 0xB3U + + /* Registers masks */ +-#define LDO_VOLTAGE_MASK 0x7CU +-#define BUCK_VOLTAGE_MASK 0xFCU ++#define LDO_VOLTAGE_MASK GENMASK_32(6, 2) ++#define BUCK_VOLTAGE_MASK GENMASK_32(7, 2) + #define LDO_BUCK_VOLTAGE_SHIFT 2 +-#define LDO_BUCK_ENABLE_MASK 0x01U +-#define LDO_BUCK_HPLP_ENABLE_MASK 0x02U ++#define LDO_BUCK_ENABLE_MASK BIT(0) ++#define LDO_BUCK_HPLP_ENABLE_MASK BIT(1) + #define LDO_BUCK_HPLP_SHIFT 1 +-#define LDO_BUCK_RANK_MASK 0x01U +-#define LDO_BUCK_RESET_MASK 0x01U +-#define LDO_BUCK_PULL_DOWN_MASK 0x03U ++#define LDO_BUCK_RANK_MASK BIT(0) ++#define LDO_BUCK_RESET_MASK BIT(0) ++#define LDO_BUCK_PULL_DOWN_MASK GENMASK_32(1, 0) + + /* Pull down register */ + #define BUCK1_PULL_DOWN_SHIFT 0 +@@ -134,12 +134,12 @@ + /* Main PMIC VINLOW Control Register (VIN_CONTROL_REGC DMSC) */ + #define SWIN_DETECTOR_ENABLED BIT(7) + #define SWOUT_DETECTOR_ENABLED BIT(6) +-#define VINLOW_HYST_MASK 0x3 ++#define VINLOW_HYST_MASK GENMASK_32(5, 4) + #define VINLOW_HYST_SHIFT 4 +-#define VINLOW_THRESHOLD_MASK 0x7 ++#define VINLOW_THRESHOLD_MASK GENMASK_32(3, 1) + #define VINLOW_THRESHOLD_SHIFT 1 +-#define VINLOW_ENABLED 0x01 +-#define VINLOW_CTRL_REG_MASK 0xFF ++#define VINLOW_ENABLED BIT(0) ++#define VINLOW_CTRL_REG_MASK GENMASK_32(7, 0) + + /* USB Control Register */ + #define BOOST_OVP_DISABLED BIT(7) +@@ -157,7 +157,7 @@ int stpmic1_register_update(uint8_t register_id, uint8_t value, uint8_t mask); + + int stpmic1_regulator_enable(const char *name); + int stpmic1_regulator_disable(const char *name); +-uint8_t stpmic1_is_regulator_enabled(const char *name); ++bool stpmic1_is_regulator_enabled(const char *name); + + int stpmic1_regulator_voltage_set(const char *name, uint16_t millivolts); + int stpmic1_regulator_voltage_get(const char *name); +@@ -199,6 +199,7 @@ struct stpmic1_lp_cfg { + uint8_t mask; + }; + ++int stpmic1_bo_enable_cfg(const char *name, struct stpmic1_bo_cfg *cfg); + int stpmic1_bo_enable_unpg(struct stpmic1_bo_cfg *cfg); + int stpmic1_bo_voltage_cfg(const char *name, uint16_t millivolts, + struct stpmic1_bo_cfg *cfg); +-- +2.7.4 +