From 83a6bfebc60c9e6a8a3968fba0172cd68b64be2d Mon Sep 17 00:00:00 2001 From: Romuald JEANNE Date: Fri, 17 Jun 2022 11:44:13 +0200 Subject: [PATCH] OPTEE-OS-STM32MP: 3.16.0-stm32mp-r2 Signed-off-by: Romuald JEANNE Change-Id: I4ef63ca08c7b11c8702d5498b1a9cb9f0950d848 --- .../optee/optee-os-stm32mp-archiver.inc | 52 +- .../optee/optee-os-stm32mp-common.inc | 3 + .../optee/optee-os-stm32mp_3.16.0.bb | 5 +- .../optee-os/0002-3.16.0-stm32mp-r2.patch | 9436 +++++++++++++++++ 4 files changed, 9443 insertions(+), 53 deletions(-) create mode 100644 recipes-security/optee/optee-os/0002-3.16.0-stm32mp-r2.patch diff --git a/recipes-security/optee/optee-os-stm32mp-archiver.inc b/recipes-security/optee/optee-os-stm32mp-archiver.inc index 3f9cb73..7e42b7f 100644 --- a/recipes-security/optee/optee-os-stm32mp-archiver.inc +++ b/recipes-security/optee/optee-os-stm32mp-archiver.inc @@ -20,9 +20,6 @@ DEPLOYDIR ?= \$(SRC_PATH)/../deploy # Set default optee-os config CFG_EMBED_DTB_SOURCE_FILE ?= ${OPTEE_CONF} -OPTEE_DRAMSIZE ?= -OPTEE_DRAMSIZE_EV ?= 0x40000000 -OPTEE_DRAMSIZE_DK ?= 0x20000000 # Remove default variables LDFLAGS = @@ -31,26 +28,6 @@ CPPFLAGS = # Define default make options EXTRA_OEMAKE = $(echo "${EXTRA_OEMAKE}" | sed "s|LIBGCC_LOCATE_CFLAGS=[^ ]* |LIBGCC_LOCATE_CFLAGS=\$(KCFLAGS) |") -# Check that provided devicetree file follow the default naming rules: -# devicetree name should contains the original stm32mp devicetree name to allow proper auto-configuration -check_dt:= \$(shell \\ - for dt in \$(CFG_EMBED_DTB_SOURCE_FILE); do \\ - match_naming="\$\$dt" ; \\ - for stdt in ${STM32MP_DEVICETREE} ; do \\ - if [ "\$\$(echo \$\$dt | grep -cE "\$\$stdt\$\$|\$\$stdt-")" -eq 1 ]; then \\ - match_naming="" ; \\ - break ; \\ - fi ; \\ - done ; \\ - echo \$\$match_naming ; \\ - done) - -ifneq (\$(check_dt),) -ifeq (\$(OPTEE_DRAMSIZE),) -\$(error Devicetree name "\$(check_dt)" is not compatible with optee-os-stm32mp auto configuration switch: you should configure OPTEE_DRAMSIZE by yourself) -endif -endif - # Set dependencies list for building all DEPS = optee DEPS += fip @@ -59,21 +36,6 @@ help: @echo @echo "OPTEE-OS configuration:" @echo " CFG_EMBED_DTB_SOURCE_FILE = \$(CFG_EMBED_DTB_SOURCE_FILE)" - @echo " DRAM size setting:" - @for dt in \$(CFG_EMBED_DTB_SOURCE_FILE); do \\ - if [ -n "\$(OPTEE_DRAMSIZE)" ]; then \ - dramsize_config="OPTEE_DRAMSIZE = \$(OPTEE_DRAMSIZE)" ; \ - else \ - dramsize_config="OPTEE_DRAMSIZE_EV = \$(OPTEE_DRAMSIZE_EV)" ; \\ - for dk in ${STM32MP_DT_FILES_DK} ; do \\ - if [ "\$\$(echo \$\$dt | grep -cE "\$\$dk\$\$|\$\$dk-")" -eq 1 ]; then \\ - dramsize_config="OPTEE_DRAMSIZE_DK = \$(OPTEE_DRAMSIZE_DK)" ; \\ - break ; \\ - fi ; \\ - done ; \\ - fi ; \\ - echo " \$\$dt : \$\$dramsize_config" ; \\ - done @echo @echo "OPTEE-OS folder configuration:" @echo " SRC_PATH = \$(SRC_PATH)" @@ -105,19 +67,7 @@ optee: break ; \\ fi ; \\ done ; \\ - # Configure DRAM_SIZE switch \\ - if [ -n "\$(OPTEE_DRAMSIZE)" ]; then \\ - dramsize="\$(OPTEE_DRAMSIZE)" ; \\ - else \\ - dramsize="\$(OPTEE_DRAMSIZE_EV)" ; \\ - for dk in ${STM32MP_DT_FILES_DK} ; do \\ - if [ "\$\$(echo \$\$dt | grep -cE "\$\$dk\$\$|\$\$dk-")" -eq 1 ]; then \\ - dramsize="\$(OPTEE_DRAMSIZE_DK)" ; \\ - break ; \\ - fi ; \\ - done ; \\ - fi ; \\ - \$(MAKE) \$(EXTRA_OEMAKE) -C \$(SRC_PATH) PREFIX=\$(SDKTARGETSYSROOT) O=\$(BLD_PATH)/\$\$dt CFG_EMBED_DTB_SOURCE_FILE=\$\$dt.dts CFG_DRAM_SIZE=\$\$dramsize \$\$soc_extra ; \\ + \$(MAKE) \$(EXTRA_OEMAKE) -C \$(SRC_PATH) PREFIX=\$(SDKTARGETSYSROOT) O=\$(BLD_PATH)/\$\$dt CFG_EMBED_DTB_SOURCE_FILE=\$\$dt.dts \$\$soc_extra ; \\ # Copy binary files with explicit name \\ cp \$(BLD_PATH)/\$\$dt/core/${OPTEE_HEADER}.${OPTEE_SUFFIX} \$(DEPLOYDIR)/${OPTEE_HEADER}-\$\$dt.${OPTEE_SUFFIX} ; \\ cp \$(BLD_PATH)/\$\$dt/core/${OPTEE_PAGER}.${OPTEE_SUFFIX} \$(DEPLOYDIR)/${OPTEE_PAGER}-\$\$dt.${OPTEE_SUFFIX} ; \\ diff --git a/recipes-security/optee/optee-os-stm32mp-common.inc b/recipes-security/optee/optee-os-stm32mp-common.inc index 4bbd632..8697bed 100644 --- a/recipes-security/optee/optee-os-stm32mp-common.inc +++ b/recipes-security/optee/optee-os-stm32mp-common.inc @@ -49,9 +49,12 @@ do_compile() { export OPENSSL_MODULES=${STAGING_LIBDIR_NATIVE}/ossl-modules/ if [ -n "${OPTEE_CONF}" ]; then for conf in ${OPTEE_CONF}; do + # Configure SOC switch + soc_extra="" for soc in ${STM32MP_SOC_NAME}; do if [ "$(echo ${conf} | grep -c ${soc})" -eq 1 ]; then soc_extra="$(echo CFG_${soc} | awk '{print toupper($0)}')=y" + break fi done diff --git a/recipes-security/optee/optee-os-stm32mp_3.16.0.bb b/recipes-security/optee/optee-os-stm32mp_3.16.0.bb index 4d36934..f923e5d 100644 --- a/recipes-security/optee/optee-os-stm32mp_3.16.0.bb +++ b/recipes-security/optee/optee-os-stm32mp_3.16.0.bb @@ -8,13 +8,14 @@ SRCREV = "d0b742d1564834dac903f906168d7357063d5459" SRC_URI += " \ file://fonts.tar.gz;subdir=git;name=fonts \ file://0001-3.16.0-stm32mp-r1.patch \ + file://0002-3.16.0-stm32mp-r2.patch \ " SRC_URI[fonts.sha256sum] = "4941e8bb6d8ac377838e27b214bf43008c496a24a8f897e0b06433988cbd53b2" OPTEE_VERSION = "3.16.0" OPTEE_SUBVERSION = "stm32mp" -OPTEE_RELEASE = "r1" +OPTEE_RELEASE = "r2" PV = "${OPTEE_VERSION}-${OPTEE_SUBVERSION}-${OPTEE_RELEASE}" @@ -49,7 +50,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;branch=${ARCHIVER_ST_BRANCH}" -SRCREV:class-devupstream = "4bbca1689f6854f660a80327a0d2959537bea222" +SRCREV:class-devupstream = "0f631da995da50fe7e19e0b7291032d77f7048bc" # --------------------------------- # Configure default preference to manage dynamic selection between tarball and github diff --git a/recipes-security/optee/optee-os/0002-3.16.0-stm32mp-r2.patch b/recipes-security/optee/optee-os/0002-3.16.0-stm32mp-r2.patch new file mode 100644 index 0000000..1b5f0f6 --- /dev/null +++ b/recipes-security/optee/optee-os/0002-3.16.0-stm32mp-r2.patch @@ -0,0 +1,9436 @@ +From 4291a8c19f84ca93a9c02da66c3fa08a32cae297 Mon Sep 17 00:00:00 2001 +From: Romuald JEANNE +Date: Tue, 20 Sep 2022 14:56:51 +0200 +Subject: [PATCH] 3.16.0-stm32mp-r2 + +Signed-off-by: Romuald JEANNE +--- + core/arch/arm/dts/stm32mp131.dtsi | 44 + + core/arch/arm/dts/stm32mp135d-dk.dts | 630 ---------- + core/arch/arm/dts/stm32mp135f-dk.dts | 42 +- + core/arch/arm/dts/stm32mp157a-dk1.dts | 6 + + core/arch/arm/dts/stm32mp157a-ed1.dts | 6 + + core/arch/arm/dts/stm32mp157c-dk2.dts | 6 + + core/arch/arm/dts/stm32mp157c-ed1.dts | 6 + + core/arch/arm/dts/stm32mp157d-dk1.dts | 6 + + core/arch/arm/dts/stm32mp157d-ed1.dts | 6 + + core/arch/arm/dts/stm32mp157f-dk2.dts | 6 + + core/arch/arm/dts/stm32mp157f-ed1.dts | 6 + + core/arch/arm/include/sm/optee_smc.h | 71 ++ + core/arch/arm/plat-stm32mp1/conf.mk | 19 +- + .../arm/plat-stm32mp1/drivers/stm32mp1_pmic.c | 24 +- + .../arm/plat-stm32mp1/drivers/stm32mp1_pwr.c | 51 +- + .../arm/plat-stm32mp1/drivers/stm32mp1_pwr.h | 13 +- + .../plat-stm32mp1/drivers/stm32mp1_pwr_irq.c | 109 +- + .../plat-stm32mp1/drivers/stm32mp1_syscfg.c | 38 +- + .../plat-stm32mp1/drivers/stm32mp1_syscfg.h | 28 + + core/arch/arm/plat-stm32mp1/link.mk | 20 + + core/arch/arm/plat-stm32mp1/main.c | 10 +- + core/arch/arm/plat-stm32mp1/plat_tzc400.c | 7 +- + core/arch/arm/plat-stm32mp1/platform_config.h | 2 + + core/arch/arm/plat-stm32mp1/stm32_util.h | 76 +- + core/arch/arm/tee/entry_fast.c | 37 + + core/drivers/adc/adc_fw.c | 410 +++++++ + core/drivers/adc/stm32_adc.c | 1064 +++++++++++++++++ + core/drivers/adc/stm32_adc_core.c | 373 ++++++ + core/drivers/adc/sub.mk | 2 + + core/drivers/clk/clk-stm32-core.c | 272 ++--- + core/drivers/clk/clk-stm32-core.h | 197 ++- + core/drivers/clk/clk-stm32mp13.c | 948 ++++++++------- + core/drivers/crypto/stm32/stm32_hash.c | 20 +- + core/drivers/crypto/stm32/stm32_saes.c | 270 ++++- + core/drivers/crypto/stm32/stm32_saes.h | 6 + + core/drivers/regulator/stm32_regulator_gpio.c | 242 ++-- + .../regulator/stm32mp1_regulator_iod.c | 323 +++++ + core/drivers/regulator/sub.mk | 1 + + core/drivers/stm32_bsec.c | 33 +- + core/drivers/stm32_huk.c | 80 ++ + core/drivers/stm32_rng.c | 24 +- + core/drivers/sub.mk | 2 + + core/include/drivers/adc.h | 240 ++++ + core/include/drivers/stm32_adc_core.h | 37 + + core/include/drivers/stm32mp13_rcc.h | 402 +++---- + .../dt-bindings/clock/stm32mp13-clks.h | 12 +- + .../dt-bindings/clock/stm32mp13-clksrc.h | 30 +- + .../dt-bindings/reset/stm32mp13-resets.h | 4 +- + core/include/kernel/dt.h | 1 + + core/include/kernel/notif.h | 25 + + core/kernel/dt.c | 3 - + core/kernel/dt_driver.c | 16 +- + core/kernel/huk_subkey.c | 1 + + core/kernel/notif.c | 82 ++ + core/tee/entry_std.c | 25 +- + .../regulator/st,stm32-regulator-gpio.yaml | 76 ++ + scripts/sign_rproc_fw.py | 8 +- + ta/remoteproc/remoteproc_core.c | 14 +- + ta/stm32mp_nvmem/ta_stm32mp_nvmem.c | 23 +- + 59 files changed, 4721 insertions(+), 1814 deletions(-) + delete mode 100644 core/arch/arm/dts/stm32mp135d-dk.dts + create mode 100644 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_syscfg.h + create mode 100644 core/drivers/adc/adc_fw.c + create mode 100644 core/drivers/adc/stm32_adc.c + create mode 100644 core/drivers/adc/stm32_adc_core.c + create mode 100644 core/drivers/adc/sub.mk + create mode 100644 core/drivers/regulator/stm32mp1_regulator_iod.c + create mode 100644 core/drivers/stm32_huk.c + create mode 100644 core/include/drivers/adc.h + create mode 100644 core/include/drivers/stm32_adc_core.h + create mode 100644 documentation/devicetree/bindings/regulator/st,stm32-regulator-gpio.yaml + +diff --git a/core/arch/arm/dts/stm32mp131.dtsi b/core/arch/arm/dts/stm32mp131.dtsi +index 80e854fdbc..00ff7723cf 100644 +--- a/core/arch/arm/dts/stm32mp131.dtsi ++++ b/core/arch/arm/dts/stm32mp131.dtsi +@@ -447,6 +447,50 @@ + #address-cells = <1>; + #size-cells = <1>; + ++ adc_2: adc@48004000 { ++ reg = <0x48004000 0x400>; ++ compatible = "st,stm32mp13-adc-core"; ++ interrupts = ; ++ clocks = <&rcc ADC2>, <&rcc ADC2_K>; ++ clock-names = "bus", "adc"; ++ interrupt-controller; ++ #interrupt-cells = <1>; ++ #address-cells = <1>; ++ #size-cells = <0>; ++ status = "disabled"; ++ ++ adc2: adc@0 { ++ compatible = "st,stm32mp13-adc"; ++ reg = <0x0>; ++ #io-channel-cells = <1>; ++ #address-cells = <1>; ++ #size-cells = <0>; ++ interrupt-parent = <&adc_2>; ++ interrupts = <0>; ++ status = "disabled"; ++ ++ channel@13 { ++ reg = <13>; ++ label = "vrefint"; ++ }; ++ ++ channel@14 { ++ reg = <14>; ++ label = "vddcore"; ++ }; ++ ++ channel@16 { ++ reg = <16>; ++ label = "vddcpu"; ++ }; ++ ++ channel@17 { ++ reg = <17>; ++ label = "vddq_ddr"; ++ }; ++ }; ++ }; ++ + usart1: serial@4c000000 { + compatible = "st,stm32h7-uart"; + reg = <0x4c000000 0x400>; +diff --git a/core/arch/arm/dts/stm32mp135d-dk.dts b/core/arch/arm/dts/stm32mp135d-dk.dts +deleted file mode 100644 +index a4ea42488b..0000000000 +--- a/core/arch/arm/dts/stm32mp135d-dk.dts ++++ /dev/null +@@ -1,630 +0,0 @@ +-// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause) +-/* +- * Copyright (C) STMicroelectronics 2022 - All Rights Reserved +- * Author: Alexandre Torgue for STMicroelectronics. +- */ +- +-/dts-v1/; +- +-#include +-#include +-#include +-#include +-#include +-#include +-#include +-#include +-#include "stm32mp135.dtsi" +-#include "stm32mp13xd.dtsi" +-#include "stm32mp13-pinctrl.dtsi" +- +-/ { +- model = "STMicroelectronics STM32MP135D-DK Discovery Board"; +- compatible = "st,stm32mp135d-dk", "st,stm32mp135"; +- +- aliases { +- serial0 = &uart4; +- serial1 = &usart1; +- }; +- +- chosen { +- stdout-path = "serial0:115200n8"; +- }; +- +- memory@c0000000 { +- device_type = "memory"; +- reg = <0xc0000000 0x20000000>; +- }; +- +- reserved-memory { +- #address-cells = <1>; +- #size-cells = <1>; +- ranges; +- +- optee_framebuffer: optee-framebuffer@dd000000 { +- /* Secure framebuffer memory */ +- reg = <0xdd000000 0x1000000>; +- st,protreg = ; +- }; +- }; +- +- vin: vin { +- compatible = "regulator-fixed"; +- regulator-name = "vin"; +- regulator-min-microvolt = <5000000>; +- regulator-max-microvolt = <5000000>; +- regulator-always-on; +- }; +- +- v3v3_ao: v3v3_ao { +- compatible = "regulator-fixed"; +- regulator-name = "v3v3_ao"; +- regulator-min-microvolt = <3300000>; +- regulator-max-microvolt = <3300000>; +- regulator-always-on; +- }; +-}; +- +-&bsec { +- board_id: board_id@f0 { +- reg = <0xf0 0x4>; +- st,non-secure-otp; +- }; +-}; +- +-&cpu0 { +- cpu-supply = <&vddcpu>; +-}; +- +-&etzpc { +- st,decprot = < +- DECPROT(STM32MP1_ETZPC_ADC1_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_ADC2_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_BKPSRAM_ID, DECPROT_S_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_DCMIPP_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_DDRCTRLPHY_ID, DECPROT_NS_R_S_W, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_ETH1_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_ETH2_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_FMC_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_HASH_ID, DECPROT_S_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_I2C3_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_I2C4_ID, DECPROT_S_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_I2C5_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_IWDG1_ID, DECPROT_S_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_LPTIM2_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_LPTIM3_ID, DECPROT_S_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_LTDC_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_MCE_ID, DECPROT_S_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_OTG_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_QSPI_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_RNG_ID, DECPROT_S_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_SDMMC1_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_SDMMC2_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_SPI4_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_SPI5_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_SRAM1_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_SRAM2_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_SRAM3_ID, DECPROT_S_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_STGENC_ID, DECPROT_S_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_TIM12_ID, DECPROT_S_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_TIM13_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_TIM14_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_TIM15_ID, DECPROT_S_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_TIM16_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_TIM17_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_USART1_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_USART2_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_USBPHYCTRL_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_VREFBUF_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- >; +-}; +- +-&gpiob { +- st,protreg = < (TZPROT(9)) >; +-}; +- +-&gpiod { +- st,protreg = < (TZPROT(7)) >; +-}; +- +-&gpioe { +- st,protreg = < (TZPROT(15)) >; +-}; +- +-&gpiof { +- st,protreg = < (TZPROT(8)) >; +-}; +- +-&hash { +- status = "okay"; +-}; +- +-&hse_monitor { +- status = "okay"; +-}; +- +-&i2c4 { +- pinctrl-names = "default"; +- 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 { +- compatible = "st,stpmic1"; +- reg = <0x33>; +- status = "okay"; +- st,wakeup-pin-number = <1>; +- +- regulators { +- compatible = "st,stpmic1-regulators"; +- buck1-supply = <&vin>; +- buck2-supply = <&vin>; +- buck3-supply = <&vin>; +- buck4-supply = <&vin>; +- ldo1-supply = <&vin>; +- ldo4-supply = <&vin>; +- ldo5-supply = <&vin>; +- ldo6-supply = <&vin>; +- vref_ddr-supply = <&vin>; +- pwr_sw1-supply = <&bst_out>; +- pwr_sw2-supply = <&v3v3_ao>; +- +- vddcpu: buck1 { +- regulator-name = "vddcpu"; +- regulator-min-microvolt = <1250000>; +- regulator-max-microvolt = <1350000>; +- regulator-always-on; +- regulator-over-current-protection; +- +- lp-stop { +- regulator-suspend-microvolt = <1250000>; +- }; +- lplv-stop { +- regulator-suspend-microvolt = <900000>; +- }; +- lplv-stop2 { +- regulator-off-in-suspend; +- }; +- standby-ddr-sr { +- regulator-off-in-suspend; +- }; +- standby-ddr-off { +- regulator-off-in-suspend; +- }; +- }; +- +- vdd_ddr: buck2 { +- regulator-name = "vdd_ddr"; +- regulator-min-microvolt = <1350000>; +- regulator-max-microvolt = <1350000>; +- regulator-always-on; +- regulator-over-current-protection; +- +- standby-ddr-off { +- regulator-off-in-suspend; +- }; +- }; +- +- vdd: buck3 { +- regulator-name = "vdd"; +- regulator-min-microvolt = <3300000>; +- regulator-max-microvolt = <3300000>; +- regulator-always-on; +- st,mask-reset; +- regulator-over-current-protection; +- }; +- +- vddcore: buck4 { +- regulator-name = "vddcore"; +- regulator-min-microvolt = <1250000>; +- regulator-max-microvolt = <1250000>; +- regulator-always-on; +- regulator-over-current-protection; +- +- lplv-stop { +- regulator-suspend-microvolt = <900000>; +- }; +- lplv-stop2 { +- regulator-suspend-microvolt = <900000>; +- }; +- standby-ddr-sr { +- regulator-off-in-suspend; +- }; +- standby-ddr-off { +- regulator-off-in-suspend; +- }; +- }; +- +- vdd_adc: ldo1 { +- regulator-name = "vdd_adc"; +- regulator-min-microvolt = <3300000>; +- regulator-max-microvolt = <3300000>; +- +- standby-ddr-sr { +- regulator-off-in-suspend; +- }; +- standby-ddr-off { +- regulator-off-in-suspend; +- }; +- }; +- +- unused1: ldo2 { +- regulator-name = "ldo2"; +- }; +- +- unused2: ldo3 { +- regulator-name = "ldo3"; +- }; +- +- vdd_usb: ldo4 { +- regulator-name = "vdd_usb"; +- regulator-min-microvolt = <3300000>; +- regulator-max-microvolt = <3300000>; +- +- standby-ddr-sr { +- regulator-off-in-suspend; +- }; +- standby-ddr-off { +- regulator-off-in-suspend; +- }; +- }; +- +- vdd_sd: ldo5 { +- regulator-name = "vdd_sd"; +- regulator-min-microvolt = <3300000>; +- regulator-max-microvolt = <3300000>; +- regulator-boot-on; +- +- standby-ddr-sr { +- regulator-off-in-suspend; +- }; +- standby-ddr-off { +- regulator-off-in-suspend; +- }; +- }; +- +- v1v8_periph: ldo6 { +- regulator-name = "v1v8_periph"; +- regulator-min-microvolt = <1800000>; +- regulator-max-microvolt = <1800000>; +- +- standby-ddr-sr { +- regulator-off-in-suspend; +- }; +- standby-ddr-off { +- regulator-off-in-suspend; +- }; +- }; +- +- vref_ddr: vref_ddr { +- regulator-name = "vref_ddr"; +- regulator-always-on; +- +- standby-ddr-sr { +- regulator-off-in-suspend; +- }; +- standby-ddr-off { +- regulator-off-in-suspend; +- }; +- }; +- +- bst_out: boost { +- regulator-name = "bst_out"; +- }; +- +- v3v3_sw: pwr_sw2 { +- regulator-name = "v3v3_sw"; +- regulator-active-discharge = <1>; +- regulator-min-microvolt = <3300000>; +- regulator-max-microvolt = <3300000>; +- }; +- }; +- }; +-}; +- +-&iwdg2 { +- timeout-sec = <32>; +- secure-timeout-sec = <5>; +- status = "okay"; +-}; +- +-&lptimer3 { +- status = "okay"; +- +- counter { +- status = "okay"; +- }; +-}; +- +-<dc { +- pinctrl-names = "default"; +- pinctrl-0 = <<dc_pins_a>; +- status = "okay"; +-}; +- +-&oem_enc_key { +- st,non-secure-otp-provisioning; +-}; +- +-&osc_calibration { +- csi-calibration { +- status = "okay"; +- }; +- +- hsi-calibration { +- status = "okay"; +- }; +-}; +- +-&pwr_irq { +- pinctrl-names = "default"; +- pinctrl-0 = <&wakeup_pins>; +- status = "okay"; +-}; +- +-&pwr_regulators { +- 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_LPLV_STOP2 +- STM32_PM_CSTOP_ALLOW_STANDBY_DDR_SR +- >; +- +- system_off_soc_mode = ; +- vdd-supply = <&vdd>; +- vdd_3v3_usbfs-supply = <&vdd_usb>; +-}; +- +-&rcc { +- compatible = "st,stm32mp13-rcc", "syscon", "st,stm32mp13-rcc-mco"; +- pinctrl-0 = <&rcc_mco_pins_a>; +- pinctrl-names = "default"; +- +- st,clksrc = < +- CLK_MPU_PLL1P +- CLK_AXI_PLL2P +- CLK_MLAHBS_PLL3 +- CLK_RTC_LSE +- CLK_MCO1_HSE +- CLK_MCO2_DISABLED +- CLK_CKPER_HSE +- CLK_ETH1_PLL4P +- CLK_ETH2_PLL4P +- CLK_SDMMC1_PLL4P +- CLK_SDMMC2_PLL4P +- CLK_STGEN_HSE +- CLK_USBPHY_HSE +- CLK_I2C4_HSI +- CLK_USBO_USBPHY +- CLK_ADC2_CKPER +- CLK_I2C12_HSI +- CLK_UART1_HSI +- CLK_UART2_HSI +- CLK_UART35_HSI +- CLK_UART4_HSI +- CLK_UART6_HSI +- CLK_UART78_HSI +- CLK_DCMIPP_PLL2Q +- CLK_LPTIM3_PCLK3 +- CLK_RNG1_PLL4R +- >; +- +- st,clkdiv = < +- DIV(DIV_MPU, 1) +- DIV(DIV_AXI, 0) +- DIV(DIV_MLAHB, 0) +- DIV(DIV_APB1, 1) +- DIV(DIV_APB2, 1) +- DIV(DIV_APB3, 1) +- DIV(DIV_APB4, 1) +- DIV(DIV_APB5, 2) +- DIV(DIV_APB6, 1) +- DIV(DIV_RTC, 0) +- DIV(DIV_MCO1, 0) +- DIV(DIV_MCO2, 0) +- >; +- +- st,pll_vco { +- pll1_vco_2000Mhz: pll1-vco-2000Mhz { +- src = < CLK_PLL12_HSE >; +- divmn = < 1 82 >; +- frac = < 0xAAA >; +- }; +- +- pll1_vco_1300Mhz: pll1-vco-1300Mhz { +- src = < CLK_PLL12_HSE >; +- divmn = < 2 80 >; +- frac = < 0x800 >; +- }; +- +- pll2_vco_1066Mhz: pll2-vco-1066Mhz { +- src = < CLK_PLL12_HSE >; +- divmn = < 2 65 >; +- frac = < 0x1400 >; +- }; +- +- pll3_vco_417_8Mhz: pll3-vco-417_8Mhz { +- src = < CLK_PLL3_HSE >; +- divmn = < 1 33 >; +- frac = < 0x1a04 >; +- }; +- +- pll4_vco_600Mhz: pll4-vco-600Mhz { +- src = < CLK_PLL4_HSE >; +- divmn = < 1 49 >; +- }; +- }; +- +- /* VCO = 1300.0 MHz => P = 650 (CPU) */ +- pll1:st,pll@0 { +- compatible = "st,stm32mp1-pll"; +- reg = <0>; +- +- st,pll = < &pll1_cfg1 >; +- +- pll1_cfg1: pll1_cfg1 { +- st,pll_vco = < &pll1_vco_1300Mhz >; +- st,pll_div_pqr = < 0 1 1 >; +- }; +- +- pll1_cfg2: pll1_cfg2 { +- st,pll_vco = < &pll1_vco_2000Mhz >; +- st,pll_div_pqr = < 0 1 1 >; +- }; +- }; +- +- /* VCO = 1066.0 MHz => P = 266 (AXI), Q = 266, R = 533 (DDR) */ +- pll2:st,pll@1 { +- compatible = "st,stm32mp1-pll"; +- reg = <1>; +- +- st,pll = < &pll2_cfg1 >; +- +- pll2_cfg1: pll2_cfg1 { +- st,pll_vco = < &pll2_vco_1066Mhz >; +- st,pll_div_pqr = < 1 1 0 >; +- }; +- }; +- +- /* VCO = 417.8 MHz => P = 209, Q = 24, R = 11 */ +- pll3:st,pll@2 { +- compatible = "st,stm32mp1-pll"; +- reg = <2>; +- +- st,pll = < &pll3_cfg1 >; +- +- pll3_cfg1: pll3_cfg1 { +- st,pll_vco = < &pll3_vco_417_8Mhz >; +- st,pll_div_pqr = < 1 16 36 >; +- }; +- }; +- +- /* VCO = 600.0 MHz => P = 50, Q = 10, R = 50 */ +- pll4:st,pll@3 { +- compatible = "st,stm32mp1-pll"; +- reg = <3>; +- st,pll = < &pll4_cfg1 >; +- +- pll4_cfg1: pll4_cfg1 { +- st,pll_vco = < &pll4_vco_600Mhz >; +- st,pll_div_pqr = < 11 59 11 >; +- }; +- }; +- +- st,clk_opp { +- /* CK_MPU clock config for MP13 */ +- st,ck_mpu { +- +- cfg_1 { +- hz = < 1000000000 >; +- st,clksrc = < CLK_MPU_PLL1P >; +- st,pll = < &pll1_cfg2 >; +- }; +- +- cfg_2 { +- hz = < 650000000 >; +- st,clksrc = < CLK_MPU_PLL1P >; +- st,pll = < &pll1_cfg1 >; +- }; +- }; +- }; +-}; +- +-&rng { +- status = "okay"; +- clock-error-detect; +-}; +- +-&rtc { +- status = "okay"; +-}; +- +-&scmi_regu { +- scmi_vddcpu: voltd-vddcpu { +- voltd-name = "vddcpu"; +- regulator-name = "vddcpu"; +- }; +- scmi_vdd: voltd-vdd { +- voltd-name = "vdd"; +- regulator-name = "vdd"; +- }; +- scmi_vddcore: voltd-vddcore { +- voltd-name = "vddcore"; +- regulator-name = "vddcore"; +- }; +- scmi_vdd_adc: voltd-vdd_adc { +- voltd-name = "vdd_adc"; +- regulator-name = "vdd_adc"; +- }; +- scmi_vdd_usb: voltd-vdd_usb { +- voltd-name = "vdd_usb"; +- regulator-name = "vdd_usb"; +- }; +- scmi_vdd_sd: voltd-vdd_sd { +- voltd-name = "vdd_sd"; +- regulator-name = "vdd_sd"; +- }; +- scmi_v1v8_periph: voltd-v1v8_periph { +- voltd-name = "v1v8_periph"; +- regulator-name = "v1v8_periph"; +- }; +- scmi_v3v3_sw: voltd-v3v3_sw { +- voltd-name = "v3v3_sw"; +- regulator-name = "v3v3_sw"; +- }; +-}; +- +-&tamp { +- status = "okay"; +- st,tamp_passive_nb_sample = <4>; +- st,tamp_passive_sample_clk_div = <16384>; +- +- tamp_passive@2 { +- pinctrl-0 = <&tamp0_in2_pin_a>; +- status = "okay"; +- }; +- +- /* Connect pin8 and pin22 from CN3 */ +- tamp_active@1 { +- pinctrl-0 = <&tamp0_in3_pin_a &tamp0_out1_pin_a>; +- status = "disabled"; +- }; +-}; +- +-&timers12 { +- status = "okay"; +- +- counter { +- status = "okay"; +- }; +-}; +- +-&tzc400 { +- memory-region = <&optee_framebuffer>; +-}; +- +-&uart4 { +- pinctrl-names = "default"; +- pinctrl-0 = <&uart4_pins_a>; +- status = "okay"; +-}; +- +-&usart1 { +- pinctrl-names = "default"; +- pinctrl-0 = <&usart1_pins_a>; +- uart-has-rtscts; +- status = "disabled"; +-}; +- +-&uart8 { +- pinctrl-names = "default"; +- pinctrl-0 = <&uart8_pins_a>; +- status = "disabled"; +-}; +- +-&wakeup_pin_1 { +- bias-pull-up; +-}; +diff --git a/core/arch/arm/dts/stm32mp135f-dk.dts b/core/arch/arm/dts/stm32mp135f-dk.dts +index d40194d850..4400b2b13c 100644 +--- a/core/arch/arm/dts/stm32mp135f-dk.dts ++++ b/core/arch/arm/dts/stm32mp135f-dk.dts +@@ -63,6 +63,35 @@ + regulator-max-microvolt = <3300000>; + regulator-always-on; + }; ++ ++ magic_wol: magic_wol { ++ compatible = "st,stm32mp1,pwr-irq-user"; ++ st,wakeup-pin-number = <2>; ++ status = "okay"; ++ }; ++ ++ typec_wakeup: typec_wakeup { ++ compatible = "st,stm32mp1,pwr-irq-user"; ++ /* Alert pin on PI2, wakeup_pin_5 */ ++ st,wakeup-pin-number = <5>; ++ st,notif-it-id = <1>; ++ status = "okay"; ++ }; ++}; ++ ++&adc_2 { ++ vdda-supply = <&vdd_adc>; ++ vref-supply = <&vdd_adc>; ++ status = "okay"; ++ ++ adc2: adc@0 { ++ status = "okay"; ++ ++ channel@15 { ++ reg = <15>; ++ label = "vbat"; ++ }; ++ }; + }; + + &bsec { +@@ -79,7 +108,7 @@ + &etzpc { + st,decprot = < + DECPROT(STM32MP1_ETZPC_ADC1_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +- DECPROT(STM32MP1_ETZPC_ADC2_ID, DECPROT_NS_RW, DECPROT_UNLOCK) ++ DECPROT(STM32MP1_ETZPC_ADC2_ID, DECPROT_S_RW, DECPROT_UNLOCK) + DECPROT(STM32MP1_ETZPC_BKPSRAM_ID, DECPROT_S_RW, DECPROT_UNLOCK) + DECPROT(STM32MP1_ETZPC_CRYP_ID, DECPROT_NS_RW, DECPROT_UNLOCK) + DECPROT(STM32MP1_ETZPC_DCMIPP_ID, DECPROT_NS_RW, DECPROT_UNLOCK) +@@ -138,6 +167,10 @@ + st,protreg = < (TZPROT(8)) >; + }; + ++&gpioi { ++ st,protreg = < (TZPROT(2)|TZPROT(3)) >; ++}; ++ + &hash { + status = "okay"; + }; +@@ -159,6 +192,7 @@ + reg = <0x33>; + status = "okay"; + st,wakeup-pin-number = <1>; ++ st,notif-it-id = <0>; + + regulators { + compatible = "st,stpmic1-regulators"; +@@ -405,6 +439,7 @@ + CLK_STGEN_HSE + CLK_USBPHY_HSE + CLK_I2C4_HSI ++ CLK_I2C5_HSI + CLK_USBO_USBPHY + CLK_ADC2_CKPER + CLK_I2C12_HSI +@@ -590,7 +625,6 @@ + }; + + &tamp { +- status = "okay"; + st,tamp_passive_nb_sample = <4>; + st,tamp_passive_sample_clk_div = <16384>; + +@@ -640,3 +674,7 @@ + &wakeup_pin_1 { + bias-pull-up; + }; ++ ++&wakeup_pin_5 { ++ bias-pull-up; ++}; +diff --git a/core/arch/arm/dts/stm32mp157a-dk1.dts b/core/arch/arm/dts/stm32mp157a-dk1.dts +index acc1d7041d..319ae09da4 100644 +--- a/core/arch/arm/dts/stm32mp157a-dk1.dts ++++ b/core/arch/arm/dts/stm32mp157a-dk1.dts +@@ -28,6 +28,12 @@ + }; + }; + ++&bsec { ++ huk_otp: huk_otp@f0 { ++ reg = <0xf0 0x10>; ++ }; ++}; ++ + &cpu1{ + cpu-supply = <&vddcore>; + }; +diff --git a/core/arch/arm/dts/stm32mp157a-ed1.dts b/core/arch/arm/dts/stm32mp157a-ed1.dts +index de8e2c57eb..3ec1a858be 100644 +--- a/core/arch/arm/dts/stm32mp157a-ed1.dts ++++ b/core/arch/arm/dts/stm32mp157a-ed1.dts +@@ -22,6 +22,12 @@ + }; + }; + ++&bsec { ++ huk_otp: huk_otp@f0 { ++ reg = <0xf0 0x10>; ++ }; ++}; ++ + &cpu1{ + cpu-supply = <&vddcore>; + }; +diff --git a/core/arch/arm/dts/stm32mp157c-dk2.dts b/core/arch/arm/dts/stm32mp157c-dk2.dts +index e72ab72974..f47224f561 100644 +--- a/core/arch/arm/dts/stm32mp157c-dk2.dts ++++ b/core/arch/arm/dts/stm32mp157c-dk2.dts +@@ -29,6 +29,12 @@ + }; + }; + ++&bsec { ++ huk_otp: huk_otp@f0 { ++ reg = <0xf0 0x10>; ++ }; ++}; ++ + &cpu1{ + cpu-supply = <&vddcore>; + }; +diff --git a/core/arch/arm/dts/stm32mp157c-ed1.dts b/core/arch/arm/dts/stm32mp157c-ed1.dts +index 8a6869aff8..284968fcfe 100644 +--- a/core/arch/arm/dts/stm32mp157c-ed1.dts ++++ b/core/arch/arm/dts/stm32mp157c-ed1.dts +@@ -21,6 +21,12 @@ + }; + }; + ++&bsec { ++ huk_otp: huk_otp@f0 { ++ reg = <0xf0 0x10>; ++ }; ++}; ++ + &cpu1{ + cpu-supply = <&vddcore>; + }; +diff --git a/core/arch/arm/dts/stm32mp157d-dk1.dts b/core/arch/arm/dts/stm32mp157d-dk1.dts +index 59bda553f0..2309ff8a99 100644 +--- a/core/arch/arm/dts/stm32mp157d-dk1.dts ++++ b/core/arch/arm/dts/stm32mp157d-dk1.dts +@@ -28,6 +28,12 @@ + }; + }; + ++&bsec { ++ huk_otp: huk_otp@f0 { ++ reg = <0xf0 0x10>; ++ }; ++}; ++ + &cpu1{ + cpu-supply = <&vddcore>; + }; +diff --git a/core/arch/arm/dts/stm32mp157d-ed1.dts b/core/arch/arm/dts/stm32mp157d-ed1.dts +index b5668569af..81e49ad226 100644 +--- a/core/arch/arm/dts/stm32mp157d-ed1.dts ++++ b/core/arch/arm/dts/stm32mp157d-ed1.dts +@@ -22,6 +22,12 @@ + }; + }; + ++&bsec { ++ huk_otp: huk_otp@f0 { ++ reg = <0xf0 0x10>; ++ }; ++}; ++ + &cpu1{ + cpu-supply = <&vddcore>; + }; +diff --git a/core/arch/arm/dts/stm32mp157f-dk2.dts b/core/arch/arm/dts/stm32mp157f-dk2.dts +index 3a14cd98bc..026306a75a 100644 +--- a/core/arch/arm/dts/stm32mp157f-dk2.dts ++++ b/core/arch/arm/dts/stm32mp157f-dk2.dts +@@ -29,6 +29,12 @@ + }; + }; + ++&bsec { ++ huk_otp: huk_otp@f0 { ++ reg = <0xf0 0x10>; ++ }; ++}; ++ + &cpu1{ + cpu-supply = <&vddcore>; + }; +diff --git a/core/arch/arm/dts/stm32mp157f-ed1.dts b/core/arch/arm/dts/stm32mp157f-ed1.dts +index 539e373bc0..97d5711653 100644 +--- a/core/arch/arm/dts/stm32mp157f-ed1.dts ++++ b/core/arch/arm/dts/stm32mp157f-ed1.dts +@@ -22,6 +22,12 @@ + }; + }; + ++&bsec { ++ huk_otp: huk_otp@f0 { ++ reg = <0xf0 0x10>; ++ }; ++}; ++ + &cpu1{ + cpu-supply = <&vddcore>; + }; +diff --git a/core/arch/arm/include/sm/optee_smc.h b/core/arch/arm/include/sm/optee_smc.h +index c2f4d05569..c9c2771fb3 100644 +--- a/core/arch/arm/include/sm/optee_smc.h ++++ b/core/arch/arm/include/sm/optee_smc.h +@@ -530,6 +530,77 @@ + #define OPTEE_SMC_GET_ASYNC_NOTIF_VALUE \ + OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_GET_ASYNC_NOTIF_VALUE) + ++/* ++ * Retrieve a value of interrupt notifications pending since the last call ++ * of this function. Interrupt notification (IT_NOTIF) differs from ASYNC_NOTIF ++ * in that ASYNC_NOTIF allows non-secure world to wake or request a secure threaded ++ * execution while IT_NOTIF triggers an interrupt context event in the non-secure ++ * world, that is a event handler from a top half interrupt context. ++ * ++ * OP-TEE keeps a record of all posted values. When an interrupt is ++ * received by the REE, which indicates that there are posted values, ++ * this function should be called until all pended values have been retrieved. ++ * When a value is retrieved it's cleared from the record in secure world. ++ * ++ * It is expected that this function is called from an interrupt handler ++ * in normal world. ++ * ++ * Call requests usage: ++ * a0 SMC Function ID, OPTEE_SMC_GET_IT_NOTIF_VALUE ++ * a1-6 Not used ++ * a7 Hypervisor Client ID register ++ * ++ * Normal return register usage: ++ * a0 OPTEE_SMC_RETURN_OK ++ * a1 IT_NOTIF interrupt identifier value ++ * a2 Bit[0]: OPTEE_SMC_IT_NOTIF_VALUE_VALID if the value in a1 is ++ * valid, else 0 if no values where pending ++ * a2 Bit[1]: OPTEE_SMC_IT_NOTIF_VALUE_PENDING if another value is ++ * pending, else 0. ++ * Bit[31:2]: MBZ ++ * a3-7 Preserved ++ * ++ * Not supported return register usage: ++ * a0 OPTEE_SMC_RETURN_ENOTAVAIL ++ * a1-7 Preserved ++ */ ++#define OPTEE_SMC_IT_NOTIF_VALID BIT(0) ++#define OPTEE_SMC_IT_NOTIF_PENDING BIT(1) ++ ++#define OPTEE_SMC_ASYNC_NOTIF_VALUE_DO_IT 1 ++ ++/* ++ * Interrupts from OP-TEE. ++ */ ++#define OPTEE_SMC_FUNCID_GET_IT_NOTIF_VALUE 53 ++#define OPTEE_SMC_GET_IT_NOTIF_VALUE \ ++ OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_GET_IT_NOTIF_VALUE) ++ ++/* ++ * Mask or unmask an interrupt notification event. ++ * ++ * It is expected that this function is called from an interrupt handler ++ * in normal world. ++ * ++ * Call requests usage: ++ * a0 SMC Function ID, OPTEE_SMC_SET_IT_NOTIF_MASK ++ * a1 Interrupt identifer value ++ * a2 1 if interrupt is to be masked, 0 if interrupt is to be masked ++ * a3-6 Not used ++ * a7 Hypervisor Client ID register ++ * ++ * Normal return register usage: ++ * a0 OPTEE_SMC_RETURN_OK ++ * a1-7 Preserved ++ * ++ * Not supported return register usage: ++ * a0 OPTEE_SMC_RETURN_ENOTAVAIL ++ * a1-7 Preserved ++ */ ++#define OPTEE_SMC_FUNCID_SET_IT_NOTIF_MASK 54 ++#define OPTEE_SMC_SET_IT_NOTIF_MASK \ ++ OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_SET_IT_NOTIF_MASK) ++ + /* + * Resume from RPC (for example after processing a foreign interrupt) + * +diff --git a/core/arch/arm/plat-stm32mp1/conf.mk b/core/arch/arm/plat-stm32mp1/conf.mk +index 23e82adb4e..6c959d4bcb 100644 +--- a/core/arch/arm/plat-stm32mp1/conf.mk ++++ b/core/arch/arm/plat-stm32mp1/conf.mk +@@ -11,14 +11,12 @@ flavor_dts_file-157D_EV1 = stm32mp157d-ev1.dts + flavor_dts_file-157F_DK2 = stm32mp157f-dk2.dts + flavor_dts_file-157F_ED1 = stm32mp157f-ed1.dts + flavor_dts_file-157F_EV1 = stm32mp157f-ev1.dts +-flavor_dts_file-135D_DK = stm32mp135d-dk.dts + flavor_dts_file-135F_DK = stm32mp135f-dk.dts + + flavorlist-512M = $(flavor_dts_file-157A_DK1) \ + $(flavor_dts_file-157C_DK2) \ + $(flavor_dts_file-157D_DK1) \ + $(flavor_dts_file-157F_DK2) \ +- $(flavor_dts_file-135D_DK) \ + $(flavor_dts_file-135F_DK) + + flavorlist-1G = $(flavor_dts_file-157A_ED1) \ +@@ -43,8 +41,7 @@ flavorlist-MP15 = $(flavor_dts_file-157A_DK1) \ + $(flavor_dts_file-157F_ED1) \ + $(flavor_dts_file-157F_EV1) + +-flavorlist-MP13 = $(flavor_dts_file-135D_DK) \ +- $(flavor_dts_file-135F_DK) ++flavorlist-MP13 = $(flavor_dts_file-135F_DK) + + ifneq ($(PLATFORM_FLAVOR),) + ifeq ($(flavor_dts_file-$(PLATFORM_FLAVOR)),) +@@ -81,11 +78,13 @@ ifeq ($(CFG_STM32MP13),y) + $(call force,CFG_CORE_ASYNC_NOTIF,y) + $(call force,CFG_CORE_ASYNC_NOTIF_GIC_INTID,31) + $(call force,CFG_BOOT_SECONDARY_REQUEST,n) ++$(call force,CFG_DRIVERS_ADC,y) + $(call force,CFG_DRIVERS_CLK,y) + $(call force,CFG_DRIVERS_CLK_FIXED,y) + $(call force,CFG_RPROC_PTA,n) + $(call force,CFG_REGULATOR_DRIVERS,y) + $(call force,CFG_SECONDARY_INIT_CNTFRQ,n) ++$(call force,CFG_STM32_ADC,y) + $(call force,CFG_STM32_CRYP,n) + $(call force,CFG_STM32_EXTI,y) + $(call force,CFG_STM32_GPIO,y) +@@ -99,6 +98,7 @@ $(call force,CFG_TZSRAM_START,0x2ffe0000) + $(call force,CFG_TZSRAM_SIZE,0x0001f000) + $(call force,CFG_WITH_NSEC_GPIOS,n) + CFG_NUM_THREADS ?= 5 ++CFG_STM32MP_OPP_COUNT ?= 2 + CFG_WITH_PAGER ?= n + CFG_WITH_TUI ?= y + else # Assume CFG_STM32MP15 +@@ -111,6 +111,7 @@ $(call force,CFG_SCMI_MSG_PERF_DOMAIN,n) + $(call force,CFG_SECONDARY_INIT_CNTFRQ,y) + $(call force,CFG_STM32_PKA,n) + $(call force,CFG_STM32_SAES,n) ++$(call force,CFG_STM32_HUK,y) + $(call force,CFG_STM32_VREFBUF,n) + $(call force,CFG_STM32MP13,n) + $(call force,CFG_STM32MP15,y) +@@ -145,7 +146,7 @@ CFG_STM32MP1_SCMI_SHM_SIZE ?= 0x00001000 + CFG_TZDRAM_SIZE ?= 0x01e00000 + CFG_SHMEM_SIZE ?= 0x00200000 + CFG_DRAM_SIZE ?= 0x40000000 +-CFG_TZDRAM_START ?= ($(CFG_DRAM_BASE) + $(CFG_DRAM_SIZE) - $(CFG_TZDRAM_SIZE)) ++CFG_TZDRAM_START ?= ($(CFG_DRAM_BASE) + CFG_DRAM_SIZE - $(CFG_TZDRAM_SIZE)) + CFG_SHMEM_START ?= ($(CFG_TZDRAM_START) - $(CFG_SHMEM_SIZE)) + + CFG_WITH_LPAE ?= y +@@ -180,6 +181,7 @@ $(call force,CFG_STM32_TAMP,n) + $(call force,CFG_STM32_TIM,n) + $(call force,CFG_STM32_VREFBUF,y) + $(call force,CFG_STPMIC1,n) ++$(call force,CFG_STM32MP1_REGULATOR_IOD,n) + $(call force,CFG_STM32MP1_SCMI_SIP,n) + $(call force,CFG_SCMI_PTA,n) + else +@@ -225,6 +227,7 @@ CFG_STM32_TIM ?= y + CFG_STM32_UART ?= y + CFG_STM32_VREFBUF ?= y + CFG_STM32MP1_CPU_OPP ?= y ++CFG_STM32MP1_REGULATOR_IOD ?= y + CFG_STPMIC1 ?= y + CFG_SYSCFG ?= y + CFG_TZC400 ?= y +@@ -331,3 +334,9 @@ CFG_STM32_EARLY_CONSOLE_UART ?= 4 + + # Generate the STM32 files + CFG_STM32MP15x_STM32IMAGE ?= n ++ ++# Default use a software HUK and not the unique key read from OTP ++# Not suitable for production ++ifeq ($(CFG_STM32_HUK),y) ++CFG_OTP_HW_TESTKEY ?= y ++endif +diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.c b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.c +index 1f7d61f228..4eda619cfe 100644 +--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.c ++++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.c +@@ -17,6 +17,7 @@ + #include + #include + #include ++#include + #include + #include + #include +@@ -507,10 +508,11 @@ static void register_non_secure_pmic(void) + } + } + +-static enum itr_return stpmic1_irq_handler(struct itr_handler *handler __unused) ++static enum itr_return stpmic1_irq_handler(struct itr_handler *handler) + { + uint8_t read_val = 0U; + unsigned int i = 0U; ++ uint32_t *it_id = handler->data; + + FMSG("Stpmic1 irq handler"); + +@@ -526,6 +528,11 @@ static enum itr_return stpmic1_irq_handler(struct itr_handler *handler __unused) + if (stpmic1_register_write(ITCLEARLATCH1_REG + i, + read_val)) + panic(); ++ ++ /* forward falling interrupt to non-secure */ ++ if (i == 0 && (read_val & BIT(IT_PONKEY_F))) ++ if (it_id) ++ notif_send_it(*it_id); + } + } + +@@ -587,6 +594,7 @@ static TEE_Result stm32_pmic_probe(const void *fdt, int node, + const fdt32_t *cuint = NULL; + struct itr_handler *hdl = NULL; + size_t it = 0; ++ uint32_t *it_id = NULL; + + res = i2c_dt_get_by_subnode(fdt, node, &i2c_pmic_handle); + if (res) +@@ -603,9 +611,19 @@ static TEE_Result stm32_pmic_probe(const void *fdt, int node, + + it = fdt32_to_cpu(*cuint) - 1U; + ++ cuint = fdt_getprop(fdt, node, "st,notif-it-id", NULL); ++ if (cuint) { ++ it_id = calloc(1, sizeof(it_id)); ++ if (!it_id) ++ return TEE_ERROR_OUT_OF_MEMORY; ++ ++ *it_id = fdt32_to_cpu(*cuint); ++ } ++ + res = stm32mp1_pwr_itr_alloc_add(it, stpmic1_irq_handler, +- PWR_WKUP_FLAG_FALLING, NULL, +- &hdl); ++ PWR_WKUP_FLAG_FALLING | ++ PWR_WKUP_FLAG_THREADED, ++ it_id, &hdl); + if (res) + panic("pmic: Couldn't allocate itr"); + +diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.c b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.c +index dcaf93d19c..57bd22b0fd 100644 +--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.c ++++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.c +@@ -12,6 +12,7 @@ + #include + #include + #include ++#include + #include + #include + #include +@@ -23,14 +24,6 @@ + #define PWR_CR3_VBE BIT(8) + #define PWR_CR3_VBRS BIT(9) + +-/* STM32MP13x VDDSD1/2 controls */ +-#define PWR_CR3_VDDSD1_EN BIT(13) +-#define PWR_CR3_VDDSD1_RDY BIT(14) +-#define PWR_CR3_VDDSD2_EN BIT(15) +-#define PWR_CR3_VDDSD2_RDY BIT(16) +-#define PWR_CR3_VDDSD1_VALID BIT(22) +-#define PWR_CR3_VDDSD2_VALID BIT(23) +- + #define PWR_CR3_USB33_EN BIT(24) + #define PWR_CR3_USB33_RDY BIT(26) + #define PWR_CR3_REG18_EN BIT(28) +@@ -142,13 +135,13 @@ static TEE_Result pwr_list_voltages(const struct regul_desc *desc, + return TEE_SUCCESS; + } + +-static void pwr_regul_lock(const struct regul_desc *desc __unused) ++void stm32mp1_pwr_regul_lock(const struct regul_desc *desc __unused) + { + if (thread_get_id_may_fail() != THREAD_ID_INVALID) + mutex_lock(&pwr_regul_mu); + } + +-static void pwr_regul_unlock(const struct regul_desc *desc __unused) ++void stm32mp1_pwr_regul_unlock(const struct regul_desc *desc __unused) + { + if (thread_get_id_may_fail() != THREAD_ID_INVALID) + mutex_unlock(&pwr_regul_mu); +@@ -159,8 +152,8 @@ struct regul_ops pwr_ops = { + .get_state = pwr_get_state, + .get_voltage = pwr_get_voltage, + .list_voltages = pwr_list_voltages, +- .lock = pwr_regul_lock, +- .unlock = pwr_regul_unlock, ++ .lock = stm32mp1_pwr_regul_lock, ++ .unlock = stm32mp1_pwr_regul_unlock, + }; + + #define DEFINE_REG(id, name, supply) { \ +@@ -211,23 +204,22 @@ static TEE_Result stm32mp1_pwr_regu_probe(const void *fdt, int node, + + for (i = 0; i < ARRAY_SIZE(stm32mp1_pwr_regs); i++) { + desc = &stm32mp1_pwr_regs[i]; +- if (!strcmp(stm32mp1_pwr_regs[i].node_name, reg_name)) +- break; +- } +- assert(i != ARRAY_SIZE(stm32mp1_pwr_regs)); +- +- res = regulator_register(desc, subnode); +- if (res) { +- EMSG("Can't register %s: %#"PRIx32, reg_name, res); +- panic(); ++ if (!strcmp(stm32mp1_pwr_regs[i].node_name, reg_name)) { ++ res = regulator_register(desc, subnode); ++ if (res) { ++ EMSG("Can't register %s: %#"PRIx32, ++ reg_name, res); ++ panic(); ++ } ++ } + } + } + + if (IS_ENABLED(CFG_STM32MP13)) { +- enable_sd_io(PWR_CR3_VDDSD1_EN, PWR_CR3_VDDSD1_RDY, +- PWR_CR3_VDDSD1_VALID); +- enable_sd_io(PWR_CR3_VDDSD2_EN, PWR_CR3_VDDSD2_RDY, +- PWR_CR3_VDDSD2_VALID); ++ enable_sd_io(PWR_CR3_VDDSD1EN, PWR_CR3_VDDSD1RDY, ++ PWR_CR3_VDDSD1VALID); ++ enable_sd_io(PWR_CR3_VDDSD2EN, PWR_CR3_VDDSD2RDY, ++ PWR_CR3_VDDSD2VALID); + } + + if (fdt_getprop(fdt, node, "st,enable-vbat-charge", NULL)) { +@@ -239,6 +231,15 @@ static TEE_Result stm32mp1_pwr_regu_probe(const void *fdt, int node, + io_setbits32(cr3, PWR_CR3_VBRS); + } + ++ fdt_for_each_subnode(subnode, fdt, node) { ++ res = dt_driver_maybe_add_probe_node(fdt, subnode); ++ if (res) { ++ EMSG("Failed on node %s with %#"PRIx32, ++ fdt_get_name(fdt, subnode, NULL), res); ++ panic(); ++ } ++ } ++ + return TEE_SUCCESS; + } + +diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.h b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.h +index cba6f1cc9c..d0d22dfb84 100644 +--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.h ++++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.h +@@ -6,6 +6,7 @@ + #ifndef __STM32MP1_PWR_H + #define __STM32MP1_PWR_H + ++#include + #include + #include + #include +@@ -38,6 +39,12 @@ + #define PWR_CR3_DDRSREN BIT(10) + #define PWR_CR3_DDRSRDIS BIT(11) + #define PWR_CR3_DDRRETEN BIT(12) ++#define PWR_CR3_VDDSD1EN BIT(13) ++#define PWR_CR3_VDDSD1RDY BIT(14) ++#define PWR_CR3_VDDSD2EN BIT(15) ++#define PWR_CR3_VDDSD2RDY BIT(16) ++#define PWR_CR3_VDDSD1VALID BIT(22) ++#define PWR_CR3_VDDSD2VALID BIT(23) + #define PWR_CR3_USB33DEN BIT(24) + #define PWR_CR3_REG18EN BIT(28) + #define PWR_CR3_REG11EN BIT(30) +@@ -69,6 +76,9 @@ unsigned int stm32mp1_pwr_regulator_mv(enum pwr_regulator id); + void stm32mp1_pwr_regulator_set_state(enum pwr_regulator id, bool enable); + bool stm32mp1_pwr_regulator_is_enabled(enum pwr_regulator id); + ++void stm32mp1_pwr_regul_lock(const struct regul_desc *desc __unused); ++void stm32mp1_pwr_regul_unlock(const struct regul_desc *desc __unused); ++ + /* wakeup-pins irq chip */ + enum pwr_wkup_pins { + PWR_WKUP_PIN1 = 0, +@@ -82,7 +92,8 @@ enum pwr_wkup_pins { + + enum pwr_wkup_flags { + PWR_WKUP_FLAG_RISING = 0, +- PWR_WKUP_FLAG_FALLING, ++ PWR_WKUP_FLAG_FALLING = BIT(0), ++ PWR_WKUP_FLAG_THREADED = BIT(1), + }; + + TEE_Result +diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr_irq.c b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr_irq.c +index 92e1f01225..0913e91514 100644 +--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr_irq.c ++++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr_irq.c +@@ -45,43 +45,48 @@ struct stm32_pwr_data { + struct stm32_pinctrl_list *pinctrl_list; + struct itr_handler *hdl[PWR_NB_WAKEUPPINS]; + struct itr_handler *gic_hdl; ++ bool threaded[PWR_NB_WAKEUPPINS]; ++ bool pending[PWR_NB_WAKEUPPINS]; + }; + + static struct stm32_pwr_data *pwr_data; + ++static enum itr_return pwr_it_call_handler(struct stm32_pwr_data *priv, ++ uint32_t pin) ++{ ++ uint32_t wkupenr = io_read32(priv->base + MPUWKUPENR); ++ ++ if (wkupenr & BIT(pin)) { ++ VERBOSE_PWR("call wkup handler irq:%d\n", pin); ++ ++ if (priv->hdl[pin]) { ++ struct itr_handler *h = priv->hdl[pin]; ++ ++ if (h->handler(h) != ITRR_HANDLED) { ++ EMSG("Disabling unhandled interrupt %u", pin); ++ stm32mp1_pwr_itr_disable(pin); ++ } ++ } ++ } ++ ++ return ITRR_HANDLED; ++} ++ + static enum itr_return pwr_it_threaded_handler(void) + { + struct stm32_pwr_data *priv = pwr_data; +- uint32_t wkupfr = 0; +- uint32_t wkupenr = 0; + uint32_t i = 0; + + VERBOSE_PWR(""); + +- wkupfr = io_read32(priv->base + WKUPFR); +- wkupenr = io_read32(priv->base + MPUWKUPENR); +- + for (i = 0; i < PWR_NB_WAKEUPPINS; i++) { +- if ((wkupfr & BIT(i)) && (wkupenr & BIT(i))) { +- VERBOSE_PWR("handle wkup irq:%d\n", i); +- +- if (priv->hdl[i]) { +- struct itr_handler *h = priv->hdl[i]; +- +- if (h->handler(h) != ITRR_HANDLED) { +- EMSG("Disabling unhandled interrupt %u", +- i); +- stm32mp1_pwr_itr_disable(i); +- } +- } +- +- /* Ack IRQ */ +- io_setbits32(priv->base + WKUPCR, BIT(i)); ++ if (priv->pending[i]) { ++ VERBOSE_PWR("handle pending wkup irq:%d\n", i); ++ priv->pending[i] = false; ++ pwr_it_call_handler(priv, i); + } + } + +- itr_enable(priv->gic_hdl->it); +- + return ITRR_HANDLED; + } + +@@ -109,15 +114,32 @@ struct notif_driver stm32_pwr_notif = { + static enum itr_return pwr_it_handler(struct itr_handler *handler) + { + struct stm32_pwr_data *priv = (struct stm32_pwr_data *)handler->data; ++ uint32_t wkupfr = 0; ++ uint32_t i = 0; + + VERBOSE_PWR(""); + + itr_disable(priv->gic_hdl->it); + +- if (notif_async_is_started()) +- notif_send_async(NOTIF_VALUE_DO_BOTTOM_HALF); +- else +- return pwr_it_threaded_handler(); ++ wkupfr = io_read32(priv->base + WKUPFR); ++ ++ for (i = 0; i < PWR_NB_WAKEUPPINS; i++) { ++ if (wkupfr & BIT(i)) { ++ VERBOSE_PWR("handle wkup irq:%d\n", i); ++ ++ /* Ack IRQ */ ++ io_setbits32(priv->base + WKUPCR, BIT(i)); ++ ++ if (priv->threaded[i] && notif_async_is_started()) { ++ priv->pending[i] = true; ++ notif_send_async(NOTIF_VALUE_DO_BOTTOM_HALF); ++ } else { ++ pwr_it_call_handler(priv, i); ++ } ++ } ++ } ++ ++ itr_enable(priv->gic_hdl->it); + + return ITRR_HANDLED; + } +@@ -207,6 +229,7 @@ static TEE_Result stm32mp1_pwr_irt_add(struct itr_handler *hdl) + struct stm32_pwr_data *priv = pwr_data; + int it = hdl->it; + struct stm32_pinctrl_list pinctrl_list = { }; ++ struct stm32_pinctrl pin = { }; + struct stm32_pinctrl *pinctrl = NULL; + unsigned int i = 0; + +@@ -223,6 +246,9 @@ static TEE_Result stm32mp1_pwr_irt_add(struct itr_handler *hdl) + + priv->hdl[it] = hdl; + ++ if (hdl->flags & PWR_WKUP_FLAG_THREADED) ++ priv->threaded[it] = true; ++ + STAILQ_FOREACH(pinctrl, priv->pinctrl_list, link) { + if ((unsigned int)it == i) + break; +@@ -231,8 +257,10 @@ static TEE_Result stm32mp1_pwr_irt_add(struct itr_handler *hdl) + } + assert(pinctrl); + ++ memcpy(&pin, pinctrl, sizeof(*pinctrl)); ++ + STAILQ_INIT(&pinctrl_list); +- STAILQ_INSERT_HEAD(&pinctrl_list, pinctrl, link); ++ STAILQ_INSERT_HEAD(&pinctrl_list, &pin, link); + + stm32mp1_pwr_itr_disable(it); + +@@ -255,7 +283,11 @@ static TEE_Result stm32mp1_pwr_irt_add(struct itr_handler *hdl) + panic(); + } + +- stm32_pwr_irq_set_trig(it, hdl->flags); ++ stm32_pwr_irq_set_trig(it, hdl->flags & ++ (PWR_WKUP_FLAG_FALLING | PWR_WKUP_FLAG_RISING)); ++ ++ if (IS_ENABLED(CFG_STM32_EXTI)) ++ stm32_exti_set_tz(PWR_EXTI_WKUP1 + it); + + return TEE_SUCCESS; + } +@@ -267,8 +299,6 @@ stm32mp1_pwr_itr_alloc_add(size_t it, itr_handler_t handler, uint32_t flags, + TEE_Result res = TEE_SUCCESS; + struct itr_handler *hdl = NULL; + +- assert(!(flags & ITRF_SHARED)); +- + hdl = calloc(1, sizeof(*hdl)); + if (!hdl) + return TEE_ERROR_OUT_OF_MEMORY; +@@ -350,10 +380,15 @@ DEFINE_DT_DRIVER(stm32mp1_pwr_irq_dt_driver) = { + .probe = &stm32mp1_pwr_irq_probe, + }; + +-static enum itr_return pwr_it_user_handler(struct itr_handler *handler __unused) ++static enum itr_return pwr_it_user_handler(struct itr_handler *handler) + { ++ uint32_t *it_id = handler->data; ++ + VERBOSE_PWR("pwr irq tester handler"); + ++ if (it_id) ++ notif_send_it(*it_id); ++ + return ITRR_HANDLED; + } + +@@ -365,6 +400,7 @@ stm32mp1_pwr_irq_user_dt_probe(const void *fdt, int node, + struct itr_handler *hdl = NULL; + const fdt32_t *cuint = NULL; + size_t it = 0; ++ uint32_t *it_id = NULL; + + VERBOSE_PWR("Init pwr irq user"); + +@@ -374,8 +410,17 @@ stm32mp1_pwr_irq_user_dt_probe(const void *fdt, int node, + + it = fdt32_to_cpu(*cuint) - 1U; + ++ cuint = fdt_getprop(fdt, node, "st,notif-it-id", NULL); ++ if (cuint) { ++ it_id = calloc(1, sizeof(it_id)); ++ if (!it_id) ++ return TEE_ERROR_OUT_OF_MEMORY; ++ ++ *it_id = fdt32_to_cpu(*cuint); ++ } ++ + res = stm32mp1_pwr_itr_alloc_add(it, pwr_it_user_handler, +- PWR_WKUP_FLAG_FALLING, NULL, &hdl); ++ PWR_WKUP_FLAG_FALLING, it_id, &hdl); + if (res != TEE_SUCCESS) + return res; + +diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_syscfg.c b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_syscfg.c +index b795fbade6..c3b7567f99 100644 +--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_syscfg.c ++++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_syscfg.c +@@ -7,8 +7,7 @@ + #include + #include + #include +-#include +-#include ++#include + #include + #include + #include +@@ -87,20 +86,6 @@ + /* + * HSLV definitions + */ +-#define SYSCFG_HSLV_IDX_TPIU U(0) +-#define SYSCFG_HSLV_IDX_QSPI U(1) +-#define SYSCFG_HSLV_IDX_ETH1 U(2) +-#define SYSCFG_HSLV_IDX_ETH2 U(3) +-#define SYSCFG_HSLV_IDX_SDMMC1 U(4) +-#define SYSCFG_HSLV_IDX_SDMMC2 U(5) +-#define SYSCFG_HSLV_IDX_SPI1 U(6) +-#define SYSCFG_HSLV_IDX_SPI2 U(7) +-#define SYSCFG_HSLV_IDX_SPI3 U(8) +-#define SYSCFG_HSLV_IDX_SPI4 U(9) +-#define SYSCFG_HSLV_IDX_SPI5 U(10) +-#define SYSCFG_HSLV_IDX_LTDC U(11) +-#define SYSCFG_HSLV_NB_IDX U(12) +- + #define SYSCFG_HSLV_KEY U(0x1018) + + static bool vdd_low_voltage; +@@ -247,20 +232,12 @@ static TEE_Result stm32mp1_iocomp(void) + } + driver_init(stm32mp1_iocomp); + +-static void enable_hslv_by_index(uint32_t index) ++void stm32mp_set_hslv_by_index(uint32_t index, bool state) + { + assert(index < SYSCFG_HSLV_NB_IDX); + +- switch (index) { +- case SYSCFG_HSLV_IDX_SDMMC1: +- case SYSCFG_HSLV_IDX_SDMMC2: +- DMSG("Not managing SDMMC HSLV"); +- break; +- default: +- io_write32(get_syscfg_base() + SYSCFG_HSLVEN0R + +- index * sizeof(uint32_t), SYSCFG_HSLV_KEY); +- break; +- } ++ io_write32(get_syscfg_base() + SYSCFG_HSLVEN0R + ++ index * sizeof(uint32_t), state ? SYSCFG_HSLV_KEY : 0); + } + + static void enable_high_speed_mode_low_voltage(void) +@@ -268,8 +245,11 @@ static void enable_high_speed_mode_low_voltage(void) + if (IS_ENABLED(CFG_STM32MP13_CFG)) { + unsigned int idx = 0; + +- for (idx = 0; idx < SYSCFG_HSLV_NB_IDX; idx++) +- enable_hslv_by_index(idx); ++ for (idx = 0; idx < SYSCFG_HSLV_NB_IDX; idx++) { ++ if (idx != SYSCFG_HSLV_IDX_SDMMC1 && ++ idx != SYSCFG_HSLV_IDX_SDMMC2) ++ stm32mp_set_hslv_by_index(idx, true); ++ } + } else { + io_write32(get_syscfg_base() + SYSCFG_IOCTRLSETR, + SYSCFG_IOCTRLSETR_HSLVEN_TRACE | +diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_syscfg.h b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_syscfg.h +new file mode 100644 +index 0000000000..c97784626e +--- /dev/null ++++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_syscfg.h +@@ -0,0 +1,28 @@ ++/* SPDX-License-Identifier: BSD-3-Clause */ ++/* ++ * Copyright (c) 2018-2021, STMicroelectronics ++ */ ++ ++#ifndef __STM32MP1_SYSCFG_H ++#define __STM32MP1_SYSCFG_H ++ ++/* ++ * HSLV definitions ++ */ ++#define SYSCFG_HSLV_IDX_TPIU U(0) ++#define SYSCFG_HSLV_IDX_QSPI U(1) ++#define SYSCFG_HSLV_IDX_ETH1 U(2) ++#define SYSCFG_HSLV_IDX_ETH2 U(3) ++#define SYSCFG_HSLV_IDX_SDMMC1 U(4) ++#define SYSCFG_HSLV_IDX_SDMMC2 U(5) ++#define SYSCFG_HSLV_IDX_SPI1 U(6) ++#define SYSCFG_HSLV_IDX_SPI2 U(7) ++#define SYSCFG_HSLV_IDX_SPI3 U(8) ++#define SYSCFG_HSLV_IDX_SPI4 U(9) ++#define SYSCFG_HSLV_IDX_SPI5 U(10) ++#define SYSCFG_HSLV_IDX_LTDC U(11) ++#define SYSCFG_HSLV_NB_IDX U(12) ++ ++void stm32mp_set_hslv_by_index(uint32_t index, bool state); ++ ++#endif /*__STM32MP1_SYSCFG_H*/ +diff --git a/core/arch/arm/plat-stm32mp1/link.mk b/core/arch/arm/plat-stm32mp1/link.mk +index fa1465a45f..604e286068 100644 +--- a/core/arch/arm/plat-stm32mp1/link.mk ++++ b/core/arch/arm/plat-stm32mp1/link.mk +@@ -1,3 +1,23 @@ ++ifeq ($(CFG_EMBED_DTB),y) ++# Specific hack for stm32mp1: get DDR size from the generated DTB to be ++# embedded in core. This force CFG_DRAM_SIZE value when build config ++# files are generated. ++ ++define get_memory_node ++$(shell fdtget -l $(core-embed-fdt-dtb) / | grep memory@) ++endef ++define get_memory_size ++$(shell fdtget -t u $(core-embed-fdt-dtb) /$(get_memory_node) reg | cut -d ' ' -f 2) ++endef ++ ++$(conf-file): $(core-embed-fdt-dtb) ++$(conf-file): CFG_DRAM_SIZE = $(get_memory_size) ++$(conf-mk-file): $(core-embed-fdt-dtb) ++$(conf-mk-file): CFG_DRAM_SIZE = $(get_memory_size) ++$(conf-cmake-file): $(core-embed-fdt-dtb) ++$(conf-cmake-file): CFG_DRAM_SIZE = $(get_memory_size) ++endif #CFG_EMBED_DTB ++ + include core/arch/arm/kernel/link.mk + + ifeq ($(CFG_STM32MP15x_STM32IMAGE),y) +diff --git a/core/arch/arm/plat-stm32mp1/main.c b/core/arch/arm/plat-stm32mp1/main.c +index eecd4fe0b8..2e9f7a699e 100644 +--- a/core/arch/arm/plat-stm32mp1/main.c ++++ b/core/arch/arm/plat-stm32mp1/main.c +@@ -48,6 +48,7 @@ register_phys_mem_pgdir(MEM_AREA_IO_SEC, APB4_BASE, APB4_SIZE); + register_phys_mem_pgdir(MEM_AREA_IO_SEC, APB5_BASE, APB5_SIZE); + #ifdef CFG_STM32MP13 + register_phys_mem_pgdir(MEM_AREA_IO_SEC, APB6_BASE, APB6_SIZE); ++register_phys_mem_pgdir(MEM_AREA_IO_SEC, AHB2_BASE, AHB2_SIZE); + #endif + register_phys_mem_pgdir(MEM_AREA_IO_SEC, AHB4_BASE, AHB4_SIZE); + register_phys_mem_pgdir(MEM_AREA_IO_SEC, AHB5_BASE, AHB5_SIZE); +@@ -545,7 +546,7 @@ unsigned long stm32_get_iwdg_otp_config(vaddr_t pbase) + return iwdg_cfg; + } + +-#ifdef CFG_TEE_CORE_DEBUG ++#if TRACE_LEVEL >= TRACE_DEBUG + static const char *const dump_table[] = { + "usr_sp", + "usr_lr", +@@ -569,14 +570,14 @@ static const char *const dump_table[] = { + #endif + }; + +-void stm32mp_dump_core_registers(bool force_display) ++void stm32mp_dump_core_registers(bool panicking) + { + static bool display; + size_t i = 0U; + uint32_t __maybe_unused *reg = NULL; + struct sm_nsec_ctx *sm_nsec_ctx = sm_get_nsec_ctx(); + +- if (force_display) ++ if (panicking) + display = true; + + if (!display) +@@ -587,6 +588,9 @@ void stm32mp_dump_core_registers(bool force_display) + reg = (uint32_t *)&sm_nsec_ctx->ub_regs.usr_sp; + for (i = 0U; i < ARRAY_SIZE(dump_table); i++) + MSG("%10s : 0x%08x\n", dump_table[i], reg[i]); ++ ++ MSG("%10s : %#08x", "mon_lr", sm_nsec_ctx->mon_lr); ++ MSG("%10s : %#08x", "mon_spsr", sm_nsec_ctx->mon_spsr); + } + DECLARE_KEEP_PAGER(stm32mp_dump_core_registers); + #endif +diff --git a/core/arch/arm/plat-stm32mp1/plat_tzc400.c b/core/arch/arm/plat-stm32mp1/plat_tzc400.c +index 964c269156..c35f30a9ef 100644 +--- a/core/arch/arm/plat-stm32mp1/plat_tzc400.c ++++ b/core/arch/arm/plat-stm32mp1/plat_tzc400.c +@@ -71,6 +71,7 @@ static enum itr_return tzc_it_handler(struct itr_handler *handler __unused) + { + EMSG("TZC permission failure"); + tzc_fail_dump(); ++ stm32mp_dump_core_registers(true); + + if (IS_ENABLED(CFG_STM32MP_PANIC_ON_TZC_PERM_VIOLATION)) + panic(); +@@ -257,7 +258,7 @@ static void stm32mp_tzc_cfg_boot_region(struct tzc_device *tzc_dev) + .sec_attr = TZC_REGION_S_RDWR, + .ns_device_access = 0, + }, +-#if CFG_CORE_RESERVED_SHM ++#ifdef CFG_CORE_RESERVED_SHM + { + .base = CFG_SHMEM_START, + .top = CFG_SHMEM_START + CFG_SHMEM_SIZE - 1, +@@ -270,8 +271,10 @@ static void stm32mp_tzc_cfg_boot_region(struct tzc_device *tzc_dev) + + COMPILE_TIME_ASSERT(IS_PAGE_ALIGNED(CFG_TZDRAM_START)); + COMPILE_TIME_ASSERT(IS_PAGE_ALIGNED(CFG_TZDRAM_SIZE)); ++#ifdef CFG_CORE_RESERVED_SHM + COMPILE_TIME_ASSERT(IS_PAGE_ALIGNED(CFG_SHMEM_START)); + COMPILE_TIME_ASSERT(IS_PAGE_ALIGNED(CFG_SHMEM_SIZE)); ++#endif + + stm32mp_tzc_region0(true); + +@@ -501,7 +504,7 @@ static TEE_Result stm32mp1_tzc_probe(const void *fdt, int node, + panic(); + + itr_enable(tzc_dev->pdata.irq); +- tzc_set_action(TZC_ACTION_ERR); ++ tzc_set_action(TZC_ACTION_INT); + + register_pm_core_service_cb(stm32mp1_tzc_pm, tzc_dev, + "stm32mp1-tzc400"); +diff --git a/core/arch/arm/plat-stm32mp1/platform_config.h b/core/arch/arm/plat-stm32mp1/platform_config.h +index cffd07122f..09a6b777e1 100644 +--- a/core/arch/arm/plat-stm32mp1/platform_config.h ++++ b/core/arch/arm/plat-stm32mp1/platform_config.h +@@ -51,6 +51,8 @@ + #define APB6_SIZE 0x0000d000 + #endif + ++#define AHB2_BASE 0x48000000 ++#define AHB2_SIZE 0x01040000 + #define AHB4_BASE 0x50000000 + #define AHB4_SIZE 0x00020000 + #ifdef CFG_STM32MP13 +diff --git a/core/arch/arm/plat-stm32mp1/stm32_util.h b/core/arch/arm/plat-stm32mp1/stm32_util.h +index 1c33fd37c8..04786e7608 100644 +--- a/core/arch/arm/plat-stm32mp1/stm32_util.h ++++ b/core/arch/arm/plat-stm32mp1/stm32_util.h +@@ -73,12 +73,18 @@ vaddr_t get_gicd_base(void); + /* Get IWDG_* enable flags mask from watchdog configuration read from fuses */ + unsigned long stm32_get_iwdg_otp_config(vaddr_t pbase); + +-#ifdef CFG_TEE_CORE_DEBUG +-void stm32mp_dump_core_registers(bool force_display); ++#if TRACE_LEVEL >= TRACE_DEBUG ++/* ++ * stm32mp_dump_core_registers - Print CPU registers to console ++ * ++ * @panicking: False if we are not called from a panic sequence. True if we ++ * are panicking. Trace messages are emitted only once this ++ * function is called with @panicking being true. Until then, ++ * calling with @panicking being false emits no trace. ++ */ ++void stm32mp_dump_core_registers(bool panicking); + #else +-static inline void stm32mp_dump_core_registers(bool force_display __unused) +-{ +-} ++static inline void stm32mp_dump_core_registers(bool panicking __unused) { } + #endif + + /* Platform util for PMIC support */ +@@ -202,66 +208,6 @@ static inline void io_clrbits32_stm32shregs(vaddr_t va, uint32_t value) + + void io_clrsetbits32_stm32shregs(vaddr_t va, uint32_t clr, uint32_t set); + +-/* +- * 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 +- * non-secure decrement request. These counters initialize to +- * either 0, 1 or 2 upon their expect default state. +- * Counters saturate to UINT_MAX / 2. +- */ +-#define SHREFCNT_NONSECURE_FLAG 0x1ul +-#define SHREFCNT_SECURE_STEP 0x2ul +-#define SHREFCNT_MAX (UINT_MAX / 2) +- +-/* Return 1 if refcnt increments from 0, else return 0 */ +-static inline int incr_shrefcnt(unsigned int *refcnt, bool secure) +-{ +- int rc = !*refcnt; +- +- if (secure) { +- if (*refcnt < SHREFCNT_MAX) { +- *refcnt += SHREFCNT_SECURE_STEP; +- assert(*refcnt < SHREFCNT_MAX); +- } +- } else { +- *refcnt |= SHREFCNT_NONSECURE_FLAG; +- } +- +- return rc; +-} +- +-/* Return 1 if refcnt decrements to 0, else return 0 */ +-static inline int decr_shrefcnt(unsigned int *refcnt, bool secure) +-{ +- int rc = 0; +- +- if (secure) { +- if (*refcnt < SHREFCNT_MAX) { +- if (*refcnt < SHREFCNT_SECURE_STEP) +- panic(); +- +- *refcnt -= SHREFCNT_SECURE_STEP; +- rc = !*refcnt; +- } +- } else { +- rc = (*refcnt == SHREFCNT_NONSECURE_FLAG); +- *refcnt &= ~SHREFCNT_NONSECURE_FLAG; +- } +- +- return rc; +-} +- +-static inline int incr_refcnt(unsigned int *refcnt) +-{ +- return incr_shrefcnt(refcnt, true); +-} +- +-static inline int decr_refcnt(unsigned int *refcnt) +-{ +- return decr_shrefcnt(refcnt, true); +-} +- + /* + * Shared peripherals and resources registration + * +diff --git a/core/arch/arm/tee/entry_fast.c b/core/arch/arm/tee/entry_fast.c +index 9457ac154f..1193e1f966 100644 +--- a/core/arch/arm/tee/entry_fast.c ++++ b/core/arch/arm/tee/entry_fast.c +@@ -211,6 +211,29 @@ static void get_async_notif_value(struct thread_smc_args *args) + args->a2 |= OPTEE_SMC_ASYNC_NOTIF_PENDING; + } + ++static void get_it_value(struct thread_smc_args *args) ++{ ++ bool value_valid = false; ++ bool value_pending = false; ++ ++ args->a0 = OPTEE_SMC_RETURN_OK; ++ args->a1 = it_get_value(&value_valid, &value_pending); ++ args->a2 = 0; ++ if (value_valid) ++ args->a2 |= OPTEE_SMC_IT_NOTIF_VALID; ++ if (value_pending) ++ args->a2 |= OPTEE_SMC_IT_NOTIF_PENDING; ++} ++ ++static void set_it_mask(struct thread_smc_args *args) ++{ ++ uint32_t it_value = args->a1; ++ bool masked = args->a2; ++ ++ args->a0 = OPTEE_SMC_RETURN_OK; ++ it_set_mask(it_value, masked); ++} ++ + /* + * If tee_entry_fast() is overridden, it's still supposed to call this + * function. +@@ -285,6 +308,20 @@ void __tee_entry_fast(struct thread_smc_args *args) + args->a0 = OPTEE_SMC_RETURN_UNKNOWN_FUNCTION; + break; + ++ case OPTEE_SMC_GET_IT_NOTIF_VALUE: ++ if (IS_ENABLED(CFG_CORE_ASYNC_NOTIF)) ++ get_it_value(args); ++ else ++ args->a0 = OPTEE_SMC_RETURN_UNKNOWN_FUNCTION; ++ break; ++ ++ case OPTEE_SMC_SET_IT_NOTIF_MASK: ++ if (IS_ENABLED(CFG_CORE_ASYNC_NOTIF)) ++ set_it_mask(args); ++ else ++ args->a0 = OPTEE_SMC_RETURN_UNKNOWN_FUNCTION; ++ break; ++ + default: + args->a0 = OPTEE_SMC_RETURN_UNKNOWN_FUNCTION; + break; +diff --git a/core/drivers/adc/adc_fw.c b/core/drivers/adc/adc_fw.c +new file mode 100644 +index 0000000000..fdae078055 +--- /dev/null ++++ b/core/drivers/adc/adc_fw.c +@@ -0,0 +1,410 @@ ++// SPDX-License-Identifier: BSD-2-Clause ++/* ++ * Copyright (c) 2022, STMicroelectronics ++ */ ++ ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++ ++static SLIST_HEAD(adc_consumer_list_head, adc_consumer) adc_cons_list = ++ SLIST_HEAD_INITIALIZER(adc_consumer_list_head); ++ ++TEE_Result adc_trylock(struct adc_device *dev) ++{ ++ TEE_Result res = TEE_ERROR_GENERIC; ++ ++ res = mutex_trylock(&dev->lock) ? TEE_SUCCESS : TEE_ERROR_BUSY; ++ ++ if (dev->state) { ++ DMSG("ADC is %s already running", dev->name); ++ mutex_unlock(&dev->lock); ++ res = TEE_ERROR_BUSY; ++ } ++ ++ return res; ++} ++ ++void adc_unlock(struct adc_device *dev) ++{ ++ mutex_unlock(&dev->lock); ++} ++ ++TEE_Result adc_get_by_name(const char *adc_name, struct adc_device **adc_dev) ++{ ++ struct adc_consumer *c = NULL; ++ ++ SLIST_FOREACH(c, &adc_cons_list, link) { ++ if (!strncmp(adc_name, c->dev->name, strlen(c->dev->name))) { ++ *adc_dev = c->dev; ++ return TEE_SUCCESS; ++ } ++ } ++ ++ EMSG("ADC Device '%s' not found", adc_name); ++ ++ return TEE_ERROR_ITEM_NOT_FOUND; ++} ++ ++TEE_Result adc_print_info(const char *adc_name) ++{ ++ struct adc_device *adc_dev = NULL; ++ TEE_Result res = TEE_ERROR_GENERIC; ++ size_t i; ++ ++ res = adc_get_by_name(adc_name, &adc_dev); ++ if (res) { ++ EMSG("ADC Device '%s' not found", adc_name); ++ return res; ++ } ++ ++ IMSG("ADC Device '%s':", adc_dev->name); ++ IMSG("Channel mask: %#"PRIx32, adc_dev->channel_mask); ++ IMSG("Channels:"); ++ for (i = 0; i < adc_dev->channels_nb; i++) ++ IMSG("\tChannel id=%u label='%s'", ++ adc_dev->channels[i].id, ++ adc_dev->channels[i].name); ++ ++ IMSG("Data mask: %#"PRIx32, adc_dev->data_mask); ++ IMSG("VDD: %"PRIu16"mV", adc_dev->vref_mv); ++ ++ return TEE_SUCCESS; ++} ++ ++static TEE_Result adc_channel_read_raw(struct adc_device *dev, ++ uint32_t channel, uint32_t *data) ++{ ++ struct adc_ops *ops = dev->ops; ++ ++ if (!ops || !ops->read_channel) ++ return TEE_ERROR_NOT_IMPLEMENTED; ++ ++ return ops->read_channel(dev, channel, data); ++} ++ ++static TEE_Result adc_conv_raw_data(struct adc_device *dev, ++ uint32_t data_raw, int *data_uv) ++{ ++ uint64_t data = data_raw; ++ ++ data *= 1000 * dev->vref_mv; ++ *data_uv = UDIV_ROUND_NEAREST(data, dev->data_mask); ++ ++ return TEE_SUCCESS; ++} ++ ++TEE_Result adc_clr_event_all(const char *adc_name, unsigned int id) ++{ ++ struct adc_device *adc_dev = NULL; ++ struct adc_consumer *c = NULL; ++ TEE_Result res = TEE_ERROR_GENERIC; ++ ++ res = adc_get_by_name(adc_name, &adc_dev); ++ if (res) ++ return res; ++ ++ SLIST_FOREACH(c, &adc_cons_list, link) { ++ if (c->evt && c->dev == adc_dev && c->evt->id == id) { ++ res = adc_consumer_clr_event(c, c->evt); ++ if (res) ++ return res; ++ } ++ } ++ ++ return res; ++} ++ ++static TEE_Result adc_consumer_lookup(const char *adc_name, uint32_t channel, ++ struct adc_consumer **adc_cons) ++{ ++ struct adc_consumer *c = NULL; ++ ++ SLIST_FOREACH(c, &adc_cons_list, link) ++ if (c->channel == channel && !strcmp(adc_name, c->dev->name)) ++ break; ++ ++ if (!c) { ++ EMSG("No consumer found for channel %"PRIu32" on ADC %s", ++ channel, adc_name); ++ return TEE_ERROR_ITEM_NOT_FOUND; ++ } ++ ++ *adc_cons = c; ++ ++ return TEE_SUCCESS; ++} ++ ++TEE_Result adc_consumer_get_by_name(const char *adc_name, uint32_t channel, ++ struct adc_consumer **cons) ++{ ++ return adc_consumer_lookup(adc_name, channel, cons); ++} ++ ++TEE_Result adc_consumer_register(struct adc_device *dev, ++ uint32_t channel, struct adc_consumer **cons) ++{ ++ struct adc_consumer *adc_cons = NULL; ++ ++ adc_cons = calloc(1, sizeof(*adc_cons)); ++ if (!adc_cons) ++ return TEE_ERROR_OUT_OF_MEMORY; ++ ++ adc_cons->dev = dev; ++ adc_cons->channel = channel; ++ *cons = adc_cons; ++ ++ SLIST_INSERT_HEAD(&adc_cons_list, adc_cons, link); ++ ++ return TEE_SUCCESS; ++} ++ ++void adc_consumer_print_list(void) ++{ ++ struct adc_consumer *cons = NULL; ++ ++ IMSG("ADC consumers:"); ++ ++ SLIST_FOREACH(cons, &adc_cons_list, link) { ++ IMSG("ADC %s : channel %"PRIu32, ++ cons->dev->name, cons->channel); ++ if (cons->evt) ++ IMSG("Event id=%u lt=%#"PRIx32" ht=%#"PRIx32, ++ cons->evt->id, cons->evt->lt, cons->evt->ht); ++ } ++} ++ ++static TEE_Result adc_consumer_get_count(const void *fdt, int node, ++ size_t *nb_cons) ++{ ++ int len = 0; ++ int channel_cells = 0; ++ int parent_node = 0; ++ int idx = 0; ++ int cnt = 0; ++ uint32_t phandle = 0; ++ const uint32_t *prop = NULL; ++ ++ prop = fdt_getprop(fdt, node, "io-channels", &len); ++ if (!prop) { ++ *nb_cons = 0; ++ return TEE_SUCCESS; ++ } ++ ++ len /= sizeof(uint32_t); ++ while (idx < len) { ++ phandle = fdt32_to_cpu(prop[idx]); ++ parent_node = fdt_node_offset_by_phandle(fdt, phandle); ++ if (parent_node < 0) ++ return TEE_ERROR_GENERIC; ++ ++ channel_cells = fdt_get_dt_driver_cells(fdt, parent_node, ++ DT_DRIVER_ADC); ++ if (channel_cells < 0) ++ return TEE_ERROR_GENERIC; ++ ++ idx += 1 + channel_cells; ++ cnt++; ++ } ++ ++ *nb_cons = cnt; ++ ++ return TEE_SUCCESS; ++} ++ ++TEE_Result adc_consumer_get_all(const void *fdt, int node, size_t *nb_cons, ++ struct adc_consumer ***cons) ++{ ++ TEE_Result res = TEE_ERROR_GENERIC; ++ size_t i = 0; ++ size_t cnt = 0; ++ struct adc_consumer **c = NULL; ++ struct adc_consumer **p = NULL; ++ ++ res = adc_consumer_get_count(fdt, node, &cnt); ++ if (res) ++ return res; ++ ++ if (!cnt) { ++ DMSG("No ADC consumers found"); ++ return res; ++ } ++ *nb_cons = cnt; ++ ++ c = calloc(1, cnt * sizeof(*cons)); ++ if (!c) ++ return TEE_ERROR_OUT_OF_MEMORY; ++ p = c; ++ ++ while (i < cnt) { ++ *c = dt_driver_device_from_node_idx_prop("io-channels", ++ fdt, node, i, ++ DT_DRIVER_ADC, ++ &res); ++ if (res) { ++ if (res != TEE_ERROR_DEFER_DRIVER_INIT) ++ EMSG("Failed to get ADC channel"); ++ return res; ++ } ++ ++ DMSG("Found channel %d on ADC %s", ++ (*c)->channel, (*c)->dev->name); ++ ++ i++; ++ c++; ++ } ++ ++ *cons = p; ++ ++ return TEE_SUCCESS; ++} ++ ++TEE_Result adc_consumer_read_processed(struct adc_consumer *cons, ++ int32_t *uv) ++{ ++ TEE_Result res = TEE_ERROR_GENERIC; ++ uint32_t data = 0; ++ ++ res = adc_trylock(cons->dev); ++ if (res) ++ return res; ++ ++ res = adc_channel_read_raw(cons->dev, cons->channel, &data); ++ if (!res) ++ adc_conv_raw_data(cons->dev, data, uv); ++ ++ adc_unlock(cons->dev); ++ ++ return res; ++} ++ ++TEE_Result adc_consumer_set_event(struct adc_consumer *cons, ++ struct adc_evt *evt) ++{ ++ struct adc_ops *ops = cons->dev->ops; ++ TEE_Result res = TEE_ERROR_GENERIC; ++ ++ if (!ops || !ops->set_event) ++ return TEE_ERROR_NOT_IMPLEMENTED; ++ ++ res = adc_trylock(cons->dev); ++ if (res) ++ return res; ++ ++ res = ops->set_event(cons->dev, evt, cons->channel); ++ if (res) ++ goto err; ++ ++ cons->evt = calloc(1, sizeof(*cons->evt)); ++ if (!cons->evt) { ++ res = TEE_ERROR_OUT_OF_MEMORY; ++ goto err; ++ } ++ ++ memcpy(cons->evt, evt, sizeof(*cons->evt)); ++ res = TEE_SUCCESS; ++ ++err: ++ adc_unlock(cons->dev); ++ ++ return res; ++} ++ ++TEE_Result adc_consumer_clr_event(struct adc_consumer *cons, ++ struct adc_evt *evt) ++{ ++ struct adc_ops *ops = cons->dev->ops; ++ TEE_Result res = TEE_ERROR_GENERIC; ++ ++ if (!ops || !ops->clr_event) ++ return TEE_ERROR_NOT_IMPLEMENTED; ++ ++ res = adc_trylock(cons->dev); ++ if (res) ++ return res; ++ ++ if (!cons->evt) { ++ EMSG("No event found"); ++ res = TEE_ERROR_NO_DATA; ++ goto err; ++ } ++ ++ free(cons->evt); ++ cons->evt = NULL; ++ ++ res = ops->clr_event(cons->dev, evt, cons->channel); ++ ++err: ++ adc_unlock(cons->dev); ++ ++ return res; ++} ++ ++TEE_Result adc_consumer_start_conv(struct adc_device *adc_dev, ++ uint32_t channel_mask) ++{ ++ TEE_Result res = TEE_ERROR_GENERIC; ++ struct adc_ops *ops = NULL; ++ ++ if ((channel_mask & adc_dev->channel_mask) != channel_mask || ++ !channel_mask) { ++ EMSG("Wrong channel mask %#"PRIx32" for ADC %s", ++ channel_mask, adc_dev->name); ++ ++ return TEE_ERROR_BAD_PARAMETERS; ++ } ++ ++ res = adc_trylock(adc_dev); ++ if (res) ++ return res; ++ ++ ops = adc_dev->ops; ++ if (!ops || !ops->start_conv) { ++ res = TEE_ERROR_NOT_IMPLEMENTED; ++ goto err; ++ } ++ ++ res = ops->start_conv(adc_dev, channel_mask); ++ if (!res) ++ adc_dev->state = 1; ++ ++err: ++ adc_unlock(adc_dev); ++ ++ return res; ++} ++ ++TEE_Result adc_consumer_stop_conv(struct adc_device *adc_dev) ++{ ++ struct adc_ops *ops = NULL; ++ TEE_Result res = TEE_ERROR_GENERIC; ++ ++ mutex_lock(&adc_dev->lock); ++ ++ ops = adc_dev->ops; ++ if (!ops || !ops->stop_conv) ++ return TEE_ERROR_NOT_IMPLEMENTED; ++ ++ if (!adc_dev->state) { ++ res = TEE_ERROR_BAD_STATE; ++ goto err; ++ } ++ ++ res = ops->stop_conv(adc_dev); ++ if (res) { ++ EMSG("Failed to stop ADC"); ++ goto err; ++ } ++ adc_dev->state = 0; ++ ++err: ++ mutex_unlock(&adc_dev->lock); ++ ++ return res; ++} +diff --git a/core/drivers/adc/stm32_adc.c b/core/drivers/adc/stm32_adc.c +new file mode 100644 +index 0000000000..c7875e3dc8 +--- /dev/null ++++ b/core/drivers/adc/stm32_adc.c +@@ -0,0 +1,1064 @@ ++// SPDX-License-Identifier: BSD-2-Clause ++/* ++ * Copyright (c) 2022, STMicroelectronics ++ */ ++ ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++ ++/* STM32MP13 - Registers for each ADC instance */ ++#define STM32MP13_ADC_ISR U(0x0) ++#define STM32MP13_ADC_CR U(0x8) ++#define STM32MP13_ADC_CFGR U(0xC) ++#define STM32MP13_ADC_SMPR1 U(0x14) ++#define STM32MP13_ADC_SMPR2 U(0x18) ++#define STM32MP13_ADC_TR1 U(0x20) ++#define STM32MP13_ADC_TR2 U(0x24) ++#define STM32MP13_ADC_TR3 U(0x28) ++#define STM32MP13_ADC_SQR1 U(0x30) ++#define STM32MP13_ADC_SQR2 U(0x34) ++#define STM32MP13_ADC_SQR3 U(0x38) ++#define STM32MP13_ADC_SQR4 U(0x3C) ++#define STM32MP13_ADC_DR U(0x40) ++#define STM32MP13_ADC_DIFSEL U(0xB0) ++#define STM32MP13_ADC_CALFACT U(0xB4) ++#define STM32MP13_ADC2_OR U(0xC8) ++#define STM32MP13_ADC_AWD2CR U(0xA0) ++#define STM32MP13_ADC_AWD3CR U(0xA4) ++ ++/* STM32MP13_ADC_ISR - bit fields */ ++#define STM32MP13_AWD3 BIT(9) ++#define STM32MP13_AWD2 BIT(8) ++#define STM32MP13_AWD1 BIT(7) ++#define STM32MP13_OVR BIT(4) ++#define STM32MP13_EOC BIT(2) ++#define STM32MP13_ADRDY BIT(0) ++ ++/* STM32MP13_ADC_CFGR bit fields */ ++#define STM32MP13_AWD1CH GENMASK_32(30, 26) ++#define STM32MP13_AWD1CH_SHIFT U(26) ++#define STM32MP13_EXTEN GENMASK_32(11, 10) ++#define STM32MP13_AWD1EN BIT(23) ++#define STM32MP13_AWD1SGL BIT(22) ++#define STM32MP13_CONT BIT(13) ++#define STM32MP13_OVRMOD BIT(12) ++#define STM32MP13_RES GENMASK_32(4, 3) ++#define STM32MP13_DMACFG BIT(1) ++#define STM32MP13_DMAEN BIT(0) ++ ++/* STM32MP13_ADC_CR - bit fields */ ++#define STM32MP13_ADCAL BIT(31) ++#define STM32MP13_ADCALDIF BIT(30) ++#define STM32MP13_DEEPPWD BIT(29) ++#define STM32MP13_ADVREGEN BIT(28) ++#define STM32MP13_ADSTP BIT(4) ++#define STM32MP13_ADSTART BIT(2) ++#define STM32MP13_ADDIS BIT(1) ++#define STM32MP13_ADEN BIT(0) ++ ++/* STM32MP13_ADC_TR1 - bit fields */ ++#define STM32MP13_HT1 GENMASK_32(27, 16) ++#define STM32MP13_HT1_SHIFT U(16) ++#define STM32MP13_LT1 GENMASK_32(11, 0) ++ ++/* STM32MP13_ADC_TR2 - bit fields */ ++#define STM32MP13_HT2 GENMASK_32(23, 16) ++#define STM32MP13_HT2_SHIFT U(16) ++#define STM32MP13_LT2 GENMASK_32(7, 0) ++ ++/* STM32MP13_ADC_TR3 - bit fields */ ++#define STM32MP13_HT3 GENMASK_32(23, 16) ++#define STM32MP13_HT3_SHIFT U(16) ++#define STM32MP13_LT3 GENMASK_32(7, 0) ++ ++/* STM32MP13_ADC_SQR1 - bit fields */ ++#define STM32MP13_SQ1_SHIFT U(6) ++ ++/* STM32MP13_ADC_DIFSEL - bit fields */ ++#define STM32MP13_DIFSEL_SHIFT U(0) ++#define STM32MP13_DIFSEL_MASK GENMASK_32(18, 0) ++ ++/* STM32MP13_ADC2_OR - bit fields */ ++#define STM32MP13_OP0 BIT(0) ++#define STM32MP13_OP1 BIT(1) ++#define STM32MP13_OP2 BIT(2) ++ ++/* STM32MP13_ADC_AWD2CR - bit fields */ ++#define STM32MP13_AWD2CH GENMASK_32(18, 0) ++ ++/* STM32MP13_ADC_AWD3CR - bit fields */ ++#define STM32MP13_AWD3CH GENMASK_32(18, 0) ++ ++#define STM32MP13_ADC_MAX_RES U(12) ++#define STM32MP13_ADC_CH_MAX U(19) ++#define STM32MP13_ADC_AWD_NB U(3) ++#define STM32MP13_ADC_MAX_SQ U(16) /* SQ1..SQ16 */ ++#define STM32MP13_ADC_MAX_SMP U(7) /* SMPx range is [0..7] */ ++ ++#define STM32_ADC_TIMEOUT_US U(100000) ++#define STM32_ADC_NSEC_PER_SEC UL(1000000000) ++#define STM32_ADCVREG_STUP_DELAY_US U(20) ++ ++/** ++ * Fixed timeout value for ADC calibration. ++ * worst cases: ++ * - low clock frequency (0.12 MHz min) ++ * - maximum prescalers ++ * - 20 ADC clock cycle for the offset calibration ++ * ++ * Set to 40ms for now ++ */ ++#define STM32MP13_ADC_CALIB_TIMEOUT_US U(40000) ++/* max channel name size */ ++#define STM32_ADC_CH_NAME_SZ U(16) ++ ++enum stm32_adc_int_ch { ++ STM32_ADC_INT_CH_NONE = -1, ++ STM32_ADC_INT_CH_VDDCORE, ++ STM32_ADC_INT_CH_VDDCPU, ++ STM32_ADC_INT_CH_VDDQ_DDR, ++ STM32_ADC_INT_CH_VREFINT, ++ STM32_ADC_INT_CH_VBAT, ++ STM32_ADC_INT_CH_NB ++}; ++ ++struct stm32_adc_ic { ++ const char *name; ++ uint32_t idx; ++}; ++ ++struct stm32_adc_data { ++ struct adc_device *dev; ++ struct stm32_adc_common *common; ++ vaddr_t regs; ++ uint32_t smpr[2]; ++ int int_ch[STM32_ADC_INT_CH_NB]; ++}; ++ ++struct stm32_adc_regs { ++ int reg; ++ int msk; ++ int shift; ++}; ++ ++struct stm32_adc_awd_reginfo { ++ uint32_t awd_isr_msk; ++ const struct stm32_adc_regs awd_ch; ++ const struct stm32_adc_regs awd_lt; ++ const struct stm32_adc_regs awd_ht; ++ const struct stm32_adc_regs awd_en; ++ const struct stm32_adc_regs awd_sgl; ++}; ++ ++static const struct stm32_adc_ic stm32_adc_ic[STM32_ADC_INT_CH_NB] = { ++ { .name = "vddcore", .idx = STM32_ADC_INT_CH_VDDCORE }, ++ { .name = "vddcpu", .idx = STM32_ADC_INT_CH_VDDCPU }, ++ { .name = "vddq_ddr", .idx = STM32_ADC_INT_CH_VDDQ_DDR }, ++ { .name = "vrefint", .idx = STM32_ADC_INT_CH_VREFINT }, ++ { .name = "vbat", .idx = STM32_ADC_INT_CH_VBAT }, ++}; ++ ++/* STM32MP13 programmable sampling time (ADC clock cycles, rounded down) */ ++static const unsigned int ++stm32mp13_adc_smp_cycles[STM32MP13_ADC_MAX_SMP + 1] = { ++ 2, 6, 12, 24, 47, 92, 247, 640 ++}; ++ ++/* ++ * stm32mp13_adc_smp_bits - describe sampling time register index & bit fields ++ * Sorted so it can be indexed by channel number. ++ */ ++static const struct stm32_adc_regs stm32mp13_adc_smp_bits[] = { ++ /* STM32MP13_ADC_SMPR1, smpr[] index, mask, shift for SMP0 to SMP9 */ ++ { 0, GENMASK_32(2, 0), 0 }, ++ { 0, GENMASK_32(5, 3), 3 }, ++ { 0, GENMASK_32(8, 6), 6 }, ++ { 0, GENMASK_32(11, 9), 9 }, ++ { 0, GENMASK_32(14, 12), 12 }, ++ { 0, GENMASK_32(17, 15), 15 }, ++ { 0, GENMASK_32(20, 18), 18 }, ++ { 0, GENMASK_32(23, 21), 21 }, ++ { 0, GENMASK_32(26, 24), 24 }, ++ { 0, GENMASK_32(29, 27), 27 }, ++ /* STM32MP13_ADC_SMPR2, smpr[] index, mask, shift for SMP10 to SMP18 */ ++ { 1, GENMASK_32(2, 0), 0 }, ++ { 1, GENMASK_32(5, 3), 3 }, ++ { 1, GENMASK_32(8, 6), 6 }, ++ { 1, GENMASK_32(11, 9), 9 }, ++ { 1, GENMASK_32(14, 12), 12 }, ++ { 1, GENMASK_32(17, 15), 15 }, ++ { 1, GENMASK_32(20, 18), 18 }, ++ { 1, GENMASK_32(23, 21), 21 }, ++ { 1, GENMASK_32(26, 24), 24 }, ++}; ++ ++const unsigned int stm32mp13_adc_min_ts[] = { 100, 0, 0, 4300, 9800 }; ++ ++/** ++ * STM32MP13_awd_reginfo[] - Analog watchdog description. ++ * ++ * two watchdog types are found in stm32mp13 ADC: ++ * - AWD1 has en_bits, and can select either a single or all channel(s) ++ * - AWD2 & AWD3 are enabled by channel mask (in AWDxCR) ++ * Remaining is similar ++ */ ++static const struct stm32_adc_awd_reginfo awd_reginfo[] = { ++ { ++ .awd_ch = {STM32MP13_ADC_CFGR, STM32MP13_AWD1CH, ++ STM32MP13_AWD1CH_SHIFT}, ++ .awd_en = {STM32MP13_ADC_CFGR, STM32MP13_AWD1EN}, ++ .awd_sgl = {STM32MP13_ADC_CFGR, STM32MP13_AWD1SGL}, ++ .awd_lt = {STM32MP13_ADC_TR1, STM32MP13_LT1}, ++ .awd_ht = {STM32MP13_ADC_TR1, STM32MP13_HT1, ++ STM32MP13_HT1_SHIFT}, ++ }, { ++ .awd_ch = {STM32MP13_ADC_AWD2CR, STM32MP13_AWD2CH}, ++ .awd_lt = {STM32MP13_ADC_TR2, STM32MP13_LT2}, ++ .awd_ht = {STM32MP13_ADC_TR2, STM32MP13_HT2, ++ STM32MP13_HT2_SHIFT}, ++ }, { ++ .awd_ch = {STM32MP13_ADC_AWD3CR, STM32MP13_AWD3CH}, ++ .awd_lt = {STM32MP13_ADC_TR3, STM32MP13_LT3}, ++ .awd_ht = {STM32MP13_ADC_TR3, STM32MP13_HT3, ++ STM32MP13_HT3_SHIFT}, ++ }, ++}; ++ ++static const struct stm32_adc_regs stm32_sqr[STM32MP13_ADC_MAX_SQ + 1] = { ++ /* L: len bit field description to be kept as first element */ ++ { STM32MP13_ADC_SQR1, GENMASK_32(3, 0), 0 }, ++ /* SQ1..SQ16 registers & bit fields (reg, mask, shift) */ ++ { STM32MP13_ADC_SQR1, GENMASK_32(10, 6), 6 }, ++ { STM32MP13_ADC_SQR1, GENMASK_32(16, 12), 12 }, ++ { STM32MP13_ADC_SQR1, GENMASK_32(22, 18), 18 }, ++ { STM32MP13_ADC_SQR1, GENMASK_32(28, 24), 24 }, ++ { STM32MP13_ADC_SQR2, GENMASK_32(4, 0), 0 }, ++ { STM32MP13_ADC_SQR2, GENMASK_32(10, 6), 6 }, ++ { STM32MP13_ADC_SQR2, GENMASK_32(16, 12), 12 }, ++ { STM32MP13_ADC_SQR2, GENMASK_32(22, 18), 18 }, ++ { STM32MP13_ADC_SQR2, GENMASK_32(28, 24), 24 }, ++ { STM32MP13_ADC_SQR3, GENMASK_32(4, 0), 0 }, ++ { STM32MP13_ADC_SQR3, GENMASK_32(10, 6), 6 }, ++ { STM32MP13_ADC_SQR3, GENMASK_32(16, 12), 12 }, ++ { STM32MP13_ADC_SQR3, GENMASK_32(22, 18), 18 }, ++ { STM32MP13_ADC_SQR3, GENMASK_32(28, 24), 24 }, ++ { STM32MP13_ADC_SQR4, GENMASK_32(4, 0), 0 }, ++ { STM32MP13_ADC_SQR4, GENMASK_32(10, 6), 6 }, ++}; ++ ++static TEE_Result stm32_adc_awd_enable(struct adc_device *adc_dev, ++ struct adc_evt *evt, uint32_t channel) ++{ ++ struct stm32_adc_data *adc = adc_get_drv_data(adc_dev); ++ unsigned int i = 0; ++ uint32_t val = 0; ++ ++ if (!evt->id || evt->id > STM32MP13_ADC_AWD_NB) { ++ EMSG("AWD index out of valid range [1..%u]", evt->id); ++ return TEE_ERROR_BAD_PARAMETERS; ++ } ++ i = evt->id - 1; ++ ++ /* ++ * Watchdog threshold comparison depends on ADC data format ++ * Supported data format: ++ * resolution: 12 bits (other resolutions not supported) ++ * alignment: indifferent (applied after comparison) ++ * offset: indifferent (applied after comparison) ++ */ ++ val = io_read32(adc->regs + STM32MP13_ADC_CFGR); ++ if (val & STM32MP13_RES) ++ return TEE_ERROR_NOT_IMPLEMENTED; ++ ++ /* Set AWD thresholds and enable AWD */ ++ if (awd_reginfo[i].awd_en.reg) { ++ /* ++ * AWD1: ++ * Enable AWD on a single channel. (scan mode not supported) ++ * Thresholds resolution: 12 bits ++ */ ++ if (!IS_POWER_OF_TWO(channel)) { ++ EMSG("Only single channel allowed for AWD1"); ++ return TEE_ERROR_BAD_PARAMETERS; ++ } ++ ++ io_clrsetbits32(adc->regs + awd_reginfo[i].awd_lt.reg, ++ awd_reginfo[i].awd_lt.msk, evt->lt); ++ io_clrsetbits32(adc->regs + awd_reginfo[i].awd_ht.reg, ++ awd_reginfo[i].awd_ht.msk, ++ evt->ht << awd_reginfo[i].awd_ht.shift); ++ ++ io_clrsetbits32(adc->regs + awd_reginfo[i].awd_ch.reg, ++ awd_reginfo[i].awd_ch.msk, ++ channel << awd_reginfo[i].awd_ch.shift); ++ /* Enable AWD on a single channel */ ++ io_setbits32(adc->regs + awd_reginfo[i].awd_sgl.reg, ++ awd_reginfo[i].awd_sgl.msk); ++ /* Enable AWD */ ++ io_setbits32(adc->regs + awd_reginfo[i].awd_en.reg, ++ awd_reginfo[i].awd_en.msk); ++ } else { ++ /* ++ * AWD2/3: ++ * Enable AWD through channel mask. (Scan mode supported) ++ * Thresholds resolution: 8 bits (MSBs) ++ */ ++ io_clrsetbits32(adc->regs + awd_reginfo[i].awd_lt.reg, ++ awd_reginfo[i].awd_lt.msk, evt->lt >> 4); ++ io_clrsetbits32(adc->regs + awd_reginfo[i].awd_ht.reg, ++ awd_reginfo[i].awd_ht.msk, ++ evt->ht << (awd_reginfo[i].awd_ht.shift - 4)); ++ ++ /* Enable AWD for channel. Do not clear channels already set */ ++ io_setbits32(adc->regs + awd_reginfo[i].awd_ch.reg, ++ awd_reginfo[i].awd_ch.msk & BIT(channel)); ++ } ++ ++ DMSG("AWD%u config: lt=%#"PRIx32" ht=%#"PRIx32"\n", ++ evt->id, evt->lt, evt->ht); ++ ++ return TEE_SUCCESS; ++} ++ ++static TEE_Result stm32_adc_awd_disable(struct adc_device *adc_dev, ++ struct adc_evt *evt, uint32_t channel) ++{ ++ struct stm32_adc_data *adc = adc_get_drv_data(adc_dev); ++ unsigned int i = 0; ++ ++ if (!evt->id || evt->id > STM32MP13_ADC_AWD_NB) { ++ EMSG("AWD index out of valid range [1..%u]", evt->id); ++ return TEE_ERROR_BAD_PARAMETERS; ++ } ++ i = evt->id - 1; ++ ++ if (awd_reginfo[i].awd_en.reg) { ++ /* AWD1: Only one channel set. Disable AWD immediately */ ++ io_clrbits32(adc->regs + awd_reginfo[i].awd_en.reg, ++ awd_reginfo[i].awd_en.msk); ++ } else { ++ /* ++ * AWD2/3: Clear only the selected channel ++ * Disable AWD if all channels are cleared ++ */ ++ io_clrbits32(adc->regs + awd_reginfo[i].awd_ch.reg, ++ awd_reginfo[i].awd_ch.msk & BIT(channel)); ++ } ++ ++ return TEE_SUCCESS; ++} ++ ++static int stm32_adc_conf_scan_seq(struct stm32_adc_data *adc, ++ uint32_t channel_mask) ++{ ++ uint32_t val = 0; ++ uint32_t chan_idx = 0, scan_idx = 0; ++ ++ io_setbits32(adc->regs + STM32MP13_ADC_SMPR1, adc->smpr[0]); ++ io_setbits32(adc->regs + STM32MP13_ADC_SMPR2, adc->smpr[1]); ++ ++ do { ++ if (channel_mask & 0x1) { ++ scan_idx++; ++ ++ if (scan_idx > STM32MP13_ADC_MAX_SQ) ++ return TEE_ERROR_GENERIC; ++ ++ DMSG("SQ%u set to channel %u\n", scan_idx, chan_idx); ++ ++ val = io_read32(adc->regs + stm32_sqr[scan_idx].reg); ++ val &= ~stm32_sqr[scan_idx].msk; ++ val |= chan_idx << stm32_sqr[scan_idx].shift; ++ io_write32(adc->regs + stm32_sqr[scan_idx].reg, val); ++ } ++ channel_mask >>= 1; ++ } while (chan_idx++ < STM32MP13_ADC_CH_MAX); ++ ++ /* Sequence len */ ++ val = io_read32(adc->regs + stm32_sqr[0].reg); ++ val &= ~stm32_sqr[0].msk; ++ val |= (scan_idx << stm32_sqr[0].shift); ++ io_write32(adc->regs + stm32_sqr[0].reg, val); ++ ++ return TEE_SUCCESS; ++} ++ ++static TEE_Result stm32_adc_start_conv(struct adc_device *adc_dev, ++ uint32_t channel_mask) ++{ ++ struct stm32_adc_data *adc = adc_get_drv_data(adc_dev); ++ ++ stm32_adc_conf_scan_seq(adc, channel_mask); ++ ++ /* ++ * Trigger disabled. Conversion launched in sw. Continuous mode set ++ * overrun mode set. Data not read but always updated in data register ++ */ ++ io_clrsetbits32(adc->regs + STM32MP13_ADC_CFGR, ++ STM32MP13_EXTEN | STM32MP13_DMAEN | STM32MP13_DMACFG, ++ STM32MP13_OVRMOD | STM32MP13_CONT); ++ ++ /* Start conversion */ ++ io_setbits32(adc->regs + STM32MP13_ADC_CR, STM32MP13_ADSTART); ++ ++ return TEE_SUCCESS; ++} ++ ++static TEE_Result stm32_adc_stop_conv(struct adc_device *adc_dev) ++{ ++ struct stm32_adc_data *adc = adc_get_drv_data(adc_dev); ++ uint64_t timeout = 0; ++ uint32_t val = 0; ++ ++ /* Leave right now if already stopped */ ++ if (!(io_read32(adc->regs + STM32MP13_ADC_CR) & STM32MP13_ADSTART)) ++ return TEE_ERROR_BAD_STATE; ++ ++ /* Stop conversion */ ++ io_setbits32(adc->regs + STM32MP13_ADC_CR, STM32MP13_ADSTP); ++ ++ /* Stop conversion */ ++ timeout = timeout_init_us(STM32_ADC_TIMEOUT_US); ++ do { ++ val = io_read32(adc->regs + STM32MP13_ADC_CR) & ++ STM32MP13_ADSTART; ++ if (!val) ++ break; ++ } while (!timeout_elapsed(timeout)); ++ ++ /* Disable continuous and overrun modes */ ++ io_clrbits32(adc->regs + STM32MP13_ADC_CFGR, ++ STM32MP13_CONT | STM32MP13_OVRMOD); ++ ++ /* Clear EOC and OVR bits by setting these bits in ISR register */ ++ io_setbits32(adc->regs + STM32MP13_ADC_ISR, ++ STM32MP13_OVR | STM32MP13_EOC); ++ ++ return TEE_SUCCESS; ++} ++static void stm32_adc_int_ch_enable(struct adc_device *adc_dev) ++{ ++ struct stm32_adc_data *adc = adc_get_drv_data(adc_dev); ++ uint32_t i = 0; ++ ++ for (i = 0; i < STM32_ADC_INT_CH_NB; i++) { ++ if (adc->int_ch[i] == STM32_ADC_INT_CH_NONE) ++ continue; ++ ++ switch (i) { ++ case STM32_ADC_INT_CH_VDDCORE: ++ DMSG("Enable VDDCore"); ++ io_setbits32(adc->regs + STM32MP13_ADC2_OR, ++ STM32MP13_OP0); ++ break; ++ case STM32_ADC_INT_CH_VDDCPU: ++ DMSG("Enable VDDCPU"); ++ io_setbits32(adc->regs + STM32MP13_ADC2_OR, ++ STM32MP13_OP1); ++ break; ++ case STM32_ADC_INT_CH_VDDQ_DDR: ++ DMSG("Enable VDDQ_DDR"); ++ io_setbits32(adc->regs + STM32MP13_ADC2_OR, ++ STM32MP13_OP2); ++ break; ++ case STM32_ADC_INT_CH_VREFINT: ++ DMSG("Enable VREFInt"); ++ io_setbits32(adc->common->regs + STM32MP13_ADC_CCR, ++ STM32MP13_VREFEN); ++ break; ++ case STM32_ADC_INT_CH_VBAT: ++ DMSG("Enable VBAT"); ++ io_setbits32(adc->common->regs + STM32MP13_ADC_CCR, ++ STM32MP13_VBATEN); ++ break; ++ default: ++ break; ++ } ++ } ++} ++ ++static void stm32_adc_int_ch_disable(struct adc_device *adc_dev) ++{ ++ struct stm32_adc_data *adc = adc_get_drv_data(adc_dev); ++ uint32_t i = 0; ++ ++ for (i = 0; i < STM32_ADC_INT_CH_NB; i++) { ++ if (adc->int_ch[i] == STM32_ADC_INT_CH_NONE) ++ continue; ++ ++ switch (i) { ++ case STM32_ADC_INT_CH_VDDCORE: ++ io_clrbits32(adc->regs + STM32MP13_ADC2_OR, ++ STM32MP13_OP0); ++ break; ++ case STM32_ADC_INT_CH_VDDCPU: ++ io_clrbits32(adc->regs + STM32MP13_ADC2_OR, ++ STM32MP13_OP1); ++ break; ++ case STM32_ADC_INT_CH_VDDQ_DDR: ++ io_clrbits32(adc->regs + STM32MP13_ADC2_OR, ++ STM32MP13_OP2); ++ break; ++ case STM32_ADC_INT_CH_VREFINT: ++ io_clrbits32(adc->common->regs + STM32MP13_ADC_CCR, ++ STM32MP13_VREFEN); ++ break; ++ case STM32_ADC_INT_CH_VBAT: ++ io_clrbits32(adc->common->regs + STM32MP13_ADC_CCR, ++ STM32MP13_VBATEN); ++ break; ++ default: ++ break; ++ } ++ } ++} ++ ++static void stm32_adc_enter_pwr_down(struct adc_device *adc_dev) ++{ ++ struct stm32_adc_data *adc = adc_get_drv_data(adc_dev); ++ ++ /* Return immediately if ADC is already in deep power down mode */ ++ if (io_read32(adc->regs + STM32MP13_ADC_CR) & STM32MP13_DEEPPWD) ++ return; ++ ++ /* Setting DEEPPWD disables ADC vreg and clears ADVREGEN */ ++ io_setbits32(adc->regs + STM32MP13_ADC_CR, STM32MP13_DEEPPWD); ++} ++ ++static TEE_Result stm32_adc_exit_pwr_down(struct adc_device *adc_dev) ++{ ++ struct stm32_adc_data *adc = adc_get_drv_data(adc_dev); ++ ++ /* Return immediately if ADC is not in deep power down mode */ ++ if (!(io_read32(adc->regs + STM32MP13_ADC_CR) & STM32MP13_DEEPPWD)) ++ return TEE_SUCCESS; ++ ++ /* Exit deep power down, then enable ADC voltage regulator */ ++ io_clrbits32(adc->regs + STM32MP13_ADC_CR, STM32MP13_DEEPPWD); ++ io_setbits32(adc->regs + STM32MP13_ADC_CR, STM32MP13_ADVREGEN); ++ ++ /* Wait for ADC LDO startup time (tADCVREG_STUP in datasheet) */ ++ udelay(STM32_ADCVREG_STUP_DELAY_US); ++ ++ return TEE_SUCCESS; ++} ++ ++static TEE_Result stm32_adc_disable(struct adc_device *adc_dev) ++{ ++ struct stm32_adc_data *adc = adc_get_drv_data(adc_dev); ++ uint64_t timeout = 0; ++ ++ /* Leave right now if already disabled */ ++ if (!(io_read32(adc->regs + STM32MP13_ADC_CR) & STM32MP13_ADEN)) ++ return TEE_SUCCESS; ++ ++ io_setbits32(adc->regs + STM32MP13_ADC_CR, STM32MP13_ADDIS); ++ ++ timeout = timeout_init_us(STM32_ADC_TIMEOUT_US); ++ do { ++ if (!(io_read32(adc->regs + STM32MP13_ADC_CR) & STM32MP13_ADEN)) ++ return TEE_SUCCESS; ++ } while (!timeout_elapsed(timeout)); ++ ++ EMSG("Failed to disable ADC. Timeout elapsed"); ++ ++ return TEE_ERROR_BAD_STATE; ++} ++ ++static TEE_Result stm32_adc_hw_stop(struct adc_device *adc_dev) ++{ ++ TEE_Result res = TEE_ERROR_GENERIC; ++ ++ res = stm32_adc_disable(adc_dev); ++ if (res) ++ return res; ++ ++ stm32_adc_int_ch_disable(adc_dev); ++ ++ stm32_adc_enter_pwr_down(adc_dev); ++ ++ return TEE_SUCCESS; ++} ++ ++static TEE_Result stm32_adc_enable(struct adc_device *adc_dev) ++{ ++ struct stm32_adc_data *adc = adc_get_drv_data(adc_dev); ++ uint64_t timeout = 0; ++ ++ /* Clear ADRDY by writing one */ ++ io_setbits32(adc->regs + STM32MP13_ADC_ISR, STM32MP13_ADRDY); ++ io_setbits32(adc->regs + STM32MP13_ADC_CR, STM32MP13_ADEN); ++ ++ timeout = timeout_init_us(STM32_ADC_TIMEOUT_US); ++ do { ++ if ((io_read32(adc->regs + STM32MP13_ADC_ISR) & ++ STM32MP13_ADRDY)) ++ return TEE_SUCCESS; ++ } while (!timeout_elapsed(timeout)); ++ ++ EMSG("Failed to enable ADC. Timeout elapsed"); ++ ++ return TEE_ERROR_BAD_STATE; ++} ++ ++static TEE_Result stm32_adc_selfcalib(struct adc_device *adc_dev) ++{ ++ struct stm32_adc_data *adc = adc_get_drv_data(adc_dev); ++ uint64_t to = 0; ++ TEE_Result res = TEE_ERROR_GENERIC; ++ ++ /* ADC must be disabled for calibration */ ++ res = stm32_adc_disable(adc_dev); ++ if (res) ++ return res; ++ ++ /* Offset calibration for single ended inputs */ ++ io_clrbits32(adc->regs + STM32MP13_ADC_CR, STM32MP13_ADCALDIF); ++ ++ /* Start calibration, then wait for completion */ ++ io_setbits32(adc->regs + STM32MP13_ADC_CR, STM32MP13_ADCAL); ++ ++ /* ++ * Wait for offset calibration completion. This calibration takes ++ * 1280 ADC clock cycles, which corresponds to 50us at 24MHz. ++ */ ++ to = timeout_init_us(STM32MP13_ADC_CALIB_TIMEOUT_US); ++ do { ++ if (!(io_read32(adc->regs + STM32MP13_ADC_CR) & ++ STM32MP13_ADCAL)) ++ break; ++ } while (!timeout_elapsed(to)); ++ ++ if (io_read32(adc->regs + STM32MP13_ADC_CR) & STM32MP13_ADCAL) { ++ EMSG("ADC offset calibration (se) failed. Timeout elapsed"); ++ return TEE_ERROR_BAD_STATE; ++ } ++ ++ DMSG("Calibration factors single-ended %#"PRIx32, ++ io_read32(adc->regs + STM32MP13_ADC_CALFACT)); ++ ++ /* Offset calibration for differential input */ ++ io_setbits32(adc->regs + STM32MP13_ADC_CR, STM32MP13_ADCALDIF); ++ ++ /* Start calibration, then wait for completion */ ++ do { ++ if (!(io_read32(adc->regs + STM32MP13_ADC_CR) & ++ STM32MP13_ADCAL)) ++ break; ++ } while (!timeout_elapsed(STM32MP13_ADC_CALIB_TIMEOUT_US)); ++ ++ if (io_read32(adc->regs + STM32MP13_ADC_CR) & STM32MP13_ADCAL) { ++ EMSG("ADC offset calibration (diff) failed. Timeout elapsed"); ++ return TEE_ERROR_BAD_STATE; ++ } ++ ++ io_clrbits32(adc->regs + STM32MP13_ADC_CR, STM32MP13_ADCALDIF); ++ ++ DMSG("Calibration factors differential %#"PRIx32, ++ io_read32(adc->regs + STM32MP13_ADC_CALFACT)); ++ ++ return TEE_SUCCESS; ++} ++ ++static TEE_Result stm32_adc_hw_start(struct adc_device *adc_dev) ++{ ++ struct stm32_adc_data *adc = adc_get_drv_data(adc_dev); ++ TEE_Result res = TEE_ERROR_GENERIC; ++ ++ res = stm32_adc_exit_pwr_down(adc_dev); ++ if (res) ++ return res; ++ ++ res = stm32_adc_selfcalib(adc_dev); ++ if (res) ++ goto err_pwr; ++ ++ stm32_adc_int_ch_enable(adc_dev); ++ ++ /* Only use single ended channels */ ++ io_clrbits32(adc->regs + STM32MP13_ADC_DIFSEL, STM32MP13_DIFSEL_MASK); ++ ++ res = stm32_adc_enable(adc_dev); ++ if (res) ++ goto err_ena; ++ ++ return TEE_SUCCESS; ++ ++err_ena: ++ stm32_adc_int_ch_disable(adc_dev); ++ ++err_pwr: ++ stm32_adc_enter_pwr_down(adc_dev); ++ ++ return res; ++} ++ ++static TEE_Result stm32_adc_read_channel(struct adc_device *adc_dev, ++ uint32_t channel, uint32_t *data) ++{ ++ struct stm32_adc_data *adc = adc_get_drv_data(adc_dev); ++ uint64_t timeout = 0; ++ uint32_t val = 0; ++ ++ /* Set sampling time to max value by default */ ++ io_setbits32(adc->regs + STM32MP13_ADC_SMPR1, adc->smpr[0]); ++ io_setbits32(adc->regs + STM32MP13_ADC_SMPR2, adc->smpr[1]); ++ ++ /* Program regular sequence: chan in SQ1 & len = 0 for one channel */ ++ io_clrsetbits32(adc->regs + STM32MP13_ADC_SQR1, UINT32_MAX, ++ channel << STM32MP13_SQ1_SHIFT); ++ ++ /* Trigger detection disabled (conversion can be launched in SW) */ ++ io_clrbits32(adc->regs + STM32MP13_ADC_CFGR, STM32MP13_EXTEN ++ | STM32MP13_DMAEN | STM32MP13_DMACFG); ++ ++ /* Start conversion */ ++ io_setbits32(adc->regs + STM32MP13_ADC_CR, STM32MP13_ADSTART); ++ ++ timeout = timeout_init_us(STM32_ADC_TIMEOUT_US); ++ do { ++ val = io_read32(adc->regs + STM32MP13_ADC_ISR) & STM32MP13_EOC; ++ if (val) ++ break; ++ } while (!timeout_elapsed(timeout)); ++ ++ if (!val) { ++ EMSG("conversion timed out"); ++ return TEE_ERROR_BAD_STATE; ++ } ++ ++ *data = io_read32(adc->regs + STM32MP13_ADC_DR); ++ ++ /* Stop conversion */ ++ timeout = timeout_init_us(STM32_ADC_TIMEOUT_US); ++ do { ++ val = io_read32(adc->regs + STM32MP13_ADC_CR) & ++ STM32MP13_ADSTART; ++ if (!val) ++ break; ++ } while (!timeout_elapsed(timeout)); ++ ++ if (val) { ++ EMSG("conversion stop timed out"); ++ return TEE_ERROR_BAD_STATE; ++ } ++ ++ return TEE_SUCCESS; ++} ++ ++static void stm32_adc_smpr_init(struct adc_device *dev, ++ int channel, uint32_t smp_ns) ++{ ++ const struct stm32_adc_regs *smpr = &stm32mp13_adc_smp_bits[channel]; ++ struct stm32_adc_data *adc = adc_get_drv_data(dev); ++ unsigned int r = smpr->reg; ++ unsigned int period_ns = 0; ++ unsigned int i = 0; ++ unsigned int smp = 0; ++ ++ assert(ARRAY_SIZE(stm32mp13_adc_min_ts) == STM32_ADC_INT_CH_NB); ++ ++ /* ++ * For internal channels, ensure that the sampling time cannot ++ * be lower than the one specified in the datasheet ++ */ ++ for (i = 0; i < STM32_ADC_INT_CH_NB; i++) ++ if (channel == adc->int_ch[i] && ++ adc->int_ch[i] != STM32_ADC_INT_CH_NONE) ++ smp_ns = MAX(smp_ns, stm32mp13_adc_min_ts[i]); ++ ++ /* Determine sampling time (ADC clock cycles) */ ++ period_ns = STM32_ADC_NSEC_PER_SEC / adc->common->rate; ++ for (smp = 0; smp <= STM32MP13_ADC_MAX_SMP; smp++) ++ if ((period_ns * stm32mp13_adc_smp_cycles[smp]) >= smp_ns) ++ break; ++ if (smp > STM32MP13_ADC_MAX_SMP) ++ smp = STM32MP13_ADC_MAX_SMP; ++ ++ /* Pre-build sampling time registers (e.g. smpr1, smpr2) */ ++ adc->smpr[r] = (adc->smpr[r] & ~smpr->msk) | (smp << smpr->shift); ++} ++ ++static TEE_Result stm32_adc_fdt_chan_init(struct adc_device *adc_dev, ++ const void *fdt, int node) ++{ ++ struct stm32_adc_data *adc = adc_get_drv_data(adc_dev); ++ const char *name = NULL; ++ struct adc_chan chans[STM32MP13_ADC_CH_MAX] = {}; ++ size_t ch_nb = 0; ++ int subnode = 0; ++ uint32_t ch_id = 0; ++ int rc = 0; ++ uint32_t i = 0; ++ uint32_t smp_ns = 0; ++ uint32_t smp_def = UINT32_MAX; ++ int len = 0; ++ ++ adc_dev->channel_mask = 0; ++ for (i = 0; i < STM32_ADC_INT_CH_NB; i++) ++ adc->int_ch[i] = STM32_ADC_INT_CH_NONE; ++ ++ /* ++ * Parse ADC channels. In the generic IO channels dt-bindings, ++ * each channel is described through a sub-node, where reg property ++ * corresponds to the channel index, and label to the channel name. ++ */ ++ fdt_for_each_subnode(subnode, fdt, node) { ++ rc = _fdt_read_uint32(fdt, subnode, "reg", &ch_id); ++ if (rc < 0) { ++ EMSG("Invalid or missing reg property: %d", rc); ++ return TEE_ERROR_BAD_PARAMETERS; ++ } ++ ++ if (ch_id >= STM32MP13_ADC_CH_MAX) { ++ EMSG("Invalid channel index: %"PRIu32, ch_id); ++ return TEE_ERROR_BAD_PARAMETERS; ++ } ++ ++ /* 'label' property is optional */ ++ name = fdt_getprop(fdt, subnode, "label", &len); ++ if (name) { ++ for (i = 0; i < STM32_ADC_INT_CH_NB; i++) { ++ /* Populate internal channels */ ++ if (!strncmp(stm32_adc_ic[i].name, ++ name, STM32_ADC_CH_NAME_SZ)) { ++ adc->int_ch[i] = ch_id; ++ smp_def = 0; ++ } ++ } ++ } ++ ++ /* ++ * 'st,min-sample-time-ns is optional. ++ * If property is not defined, sampling time set is as follows: ++ * - Internal channels: default value from stm32mp13_adc_min_ts ++ * - Other channels: set to max by default ++ */ ++ smp_ns = _fdt_read_uint32_default(fdt, subnode, ++ "st,min-sample-time-ns", ++ smp_def); ++ ++ stm32_adc_smpr_init(adc_dev, ch_id, smp_ns); ++ ++ adc_dev->channel_mask |= BIT(ch_id); ++ ++ chans[ch_nb].id = ch_id; ++ chans[ch_nb].name = name; ++ ++ ch_nb++; ++ } ++ ++ if (!ch_nb) { ++ EMSG("No channel found"); ++ return TEE_ERROR_NO_DATA; ++ } ++ ++ if (ch_nb > STM32MP13_ADC_CH_MAX) { ++ EMSG("too many channels: %d", ch_nb); ++ return TEE_ERROR_EXCESS_DATA; ++ } ++ ++ adc_dev->channels = calloc(1, ch_nb * sizeof(*adc_dev->channels)); ++ if (!adc_dev->channels) ++ return TEE_ERROR_OUT_OF_MEMORY; ++ ++ memcpy(adc_dev->channels, &chans, ch_nb * sizeof(*adc_dev->channels)); ++ ++ adc_dev->channels_nb = ch_nb; ++ adc_dev->data_mask = GENMASK_32(STM32MP13_ADC_MAX_RES - 1, 0); ++ ++ DMSG("%u channels found: Mask %#"PRIx32, ++ adc_dev->channels_nb, adc_dev->channel_mask); ++ ++ return TEE_SUCCESS; ++} ++ ++static TEE_Result stm32_adc_pm_resume(struct adc_device *adc_dev) ++{ ++ return stm32_adc_hw_start(adc_dev); ++} ++ ++static TEE_Result stm32_adc_pm_suspend(struct adc_device *adc_dev) ++{ ++ TEE_Result res = TEE_ERROR_GENERIC; ++ ++ /* ++ * If there are on-going conversions, return an error. ++ * Else the ADC can be stopped safely. ++ * No need to protect status check against race conditions here, ++ * as we are no more in a multi-threading context. ++ */ ++ if (adc_dev->state) ++ return TEE_ERROR_BUSY; ++ ++ res = stm32_adc_hw_stop(adc_dev); ++ ++ return res; ++} ++ ++static TEE_Result ++stm32_adc_pm(enum pm_op op, unsigned int pm_hint __unused, ++ const struct pm_callback_handle *pm_handle) ++{ ++ TEE_Result res = TEE_ERROR_GENERIC; ++ struct adc_device *dev = ++ (struct adc_device *)PM_CALLBACK_GET_HANDLE(pm_handle); ++ ++ if (op == PM_OP_RESUME) ++ res = stm32_adc_pm_resume(dev); ++ else ++ res = stm32_adc_pm_suspend(dev); ++ ++ return res; ++} ++ ++static struct adc_consumer * ++stm32_adc_register_cons(struct dt_driver_phandle_args *pargs __unused, ++ void *data, TEE_Result *res) ++{ ++ struct stm32_adc_data *adc = data; ++ struct adc_device *adc_dev = adc->dev; ++ struct adc_consumer *adc_cons = NULL; ++ uint32_t channel = 0; ++ uint32_t mask = 0; ++ ++ *res = TEE_ERROR_BAD_PARAMETERS; ++ ++ if (!pargs) ++ return NULL; ++ ++ if (!pargs->args_count) ++ return NULL; ++ ++ channel = pargs->args[0]; ++ mask = BIT(channel); ++ ++ if (!(adc_dev->channel_mask & mask)) { ++ EMSG("Channel %"PRIu32" not available", channel); ++ return NULL; ++ } ++ ++ *res = adc_consumer_register(adc_dev, channel, &adc_cons); ++ ++ return adc_cons; ++} ++ ++static struct adc_ops stm32_adc_ops = { ++ .read_channel = stm32_adc_read_channel, ++ .set_event = stm32_adc_awd_enable, ++ .clr_event = stm32_adc_awd_disable, ++ .start_conv = stm32_adc_start_conv, ++ .stop_conv = stm32_adc_stop_conv, ++}; ++ ++static TEE_Result stm32_adc_probe(const void *fdt, int node, ++ const void *compat_data __unused) ++{ ++ struct dt_node_info dt_info = { }; ++ struct adc_device *adc_dev = NULL; ++ struct stm32_adc_data *adc = NULL; ++ TEE_Result res = TEE_ERROR_GENERIC; ++ const char *name = NULL; ++ const char *pname = NULL; ++ char *p = NULL; ++ int pnode = 0; ++ ++ adc_dev = calloc(1, sizeof(*adc_dev)); ++ if (!adc_dev) ++ return TEE_ERROR_OUT_OF_MEMORY; ++ ++ adc = calloc(1, sizeof(*adc)); ++ if (!adc) { ++ free(adc_dev); ++ return TEE_ERROR_OUT_OF_MEMORY; ++ } ++ adc_set_drv_data(adc_dev, adc); ++ ++ _fdt_fill_device_info(fdt, &dt_info, node); ++ ++ if (dt_info.reg == DT_INFO_INVALID_REG || ++ dt_info.interrupt == DT_INFO_INVALID_INTERRUPT) ++ goto err; ++ ++ pnode = fdt_parent_offset(fdt, node); ++ assert(pnode >= 0); ++ ++ adc->common = dt_driver_device_from_node(pnode, DT_DRIVER_NOTYPE, &res); ++ if (res) ++ goto err; ++ ++ adc->regs = adc->common->regs + dt_info.reg; ++ adc->dev = adc_dev; ++ adc_dev->vref_mv = adc->common->vref_mv; ++ ++ name = fdt_get_name(fdt, node, NULL); ++ pname = fdt_get_name(fdt, pnode, NULL); ++ adc_dev->name = calloc(1, sizeof(adc_dev->name) * ++ (strlen(name) + strlen(pname) + 2)); ++ if (!adc_dev->name) { ++ res = TEE_ERROR_OUT_OF_MEMORY; ++ goto err; ++ } ++ ++ p = adc_dev->name; ++ memcpy(p, pname, strlen(pname)); ++ memcpy(p + strlen(pname), ":", 1); ++ memcpy(p + strlen(pname) + 1, name, strlen(name) + 1); ++ ++ res = stm32_adc_fdt_chan_init(adc_dev, fdt, node); ++ if (res) ++ goto err_name; ++ ++ adc_register(adc_dev, &stm32_adc_ops); ++ ++ res = stm32_adc_hw_start(adc_dev); ++ if (res) ++ goto err_start; ++ ++ res = dt_driver_register_provider(fdt, node, ++ (get_of_device_func) ++ stm32_adc_register_cons, ++ (void *)adc, DT_DRIVER_ADC); ++ if (res) { ++ EMSG("Couldn't register ADC IO channels"); ++ if (stm32_adc_hw_stop(adc_dev)) ++ panic(); ++ goto err_start; ++ } ++ ++ register_pm_core_service_cb(stm32_adc_pm, adc_dev, "stm32-adc"); ++ DMSG("adc %s probed", fdt_get_name(fdt, node, NULL)); ++ ++ return TEE_SUCCESS; ++ ++err_start: ++ adc_unregister(adc_dev); ++ ++err_name: ++ free(adc_dev->name); ++ ++err: ++ free(adc_dev); ++ free(adc); ++ ++ return res; ++} ++ ++static const struct dt_device_match stm32_adc_match_table[] = { ++ { .compatible = "st,stm32mp13-adc" }, ++ { } ++}; ++ ++DEFINE_DT_DRIVER(stm32_adc_dt_driver) = { ++ .name = "stm32-adc", ++ .match_table = stm32_adc_match_table, ++ .probe = stm32_adc_probe, ++}; +diff --git a/core/drivers/adc/stm32_adc_core.c b/core/drivers/adc/stm32_adc_core.c +new file mode 100644 +index 0000000000..4fa32ae8d7 +--- /dev/null ++++ b/core/drivers/adc/stm32_adc_core.c +@@ -0,0 +1,373 @@ ++// SPDX-License-Identifier: BSD-2-Clause ++/* ++ * Copyright (c) 2022, STMicroelectronics ++ */ ++ ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++ ++#define STM32MP13_ADC_MAX_CLK_RATE U(75000000) ++ ++struct stm32_adc_core_data { ++ struct clk *aclk; ++ struct clk *bclk; ++ struct rdev *vref; ++ struct rdev *vdda; ++ struct stm32_adc_common common; ++}; ++ ++struct stm32_adc_core_device { ++ struct stm32_adc_core_data data; ++}; ++ ++/** ++ * struct stm32mp13_adc_ck_spec - specification for stm32mp13 ADC clock ++ * @ckmode: ADC clock mode, Async (0) or sync (!=0) with prescaler. ++ * @presc: prescaler bitfield for async clock mode ++ * @div: prescaler division ratio ++ */ ++struct stm32mp13_adc_ck_spec { ++ uint32_t ckmode; ++ uint32_t presc; ++ int div; ++}; ++ ++static const struct stm32mp13_adc_ck_spec stm32mp13_adc_ckmodes_spec[] = { ++ /* 00: CK_ADC[1..3]: Asynchronous clock modes */ ++ { .ckmode = 0, .presc = 0, .div = 1 }, ++ { .ckmode = 0, .presc = 1, .div = 2 }, ++ { .ckmode = 0, .presc = 2, .div = 4 }, ++ { .ckmode = 0, .presc = 3, .div = 6 }, ++ { .ckmode = 0, .presc = 4, .div = 8 }, ++ { .ckmode = 0, .presc = 5, .div = 10 }, ++ { .ckmode = 0, .presc = 6, .div = 12 }, ++ { .ckmode = 0, .presc = 7, .div = 16 }, ++ { .ckmode = 0, .presc = 8, .div = 32 }, ++ { .ckmode = 0, .presc = 9, .div = 64 }, ++ { .ckmode = 0, .presc = 10, .div = 128 }, ++ { .ckmode = 0, .presc = 11, .div = 256 }, ++ /* HCLK used: Synchronous clock modes (1, 2 or 4 prescaler) */ ++ { .ckmode = 1, .presc = 0, .div = 1 }, ++ { .ckmode = 2, .presc = 0, .div = 2 }, ++ { .ckmode = 3, .presc = 0, .div = 4 }, ++}; ++ ++static TEE_Result stm32_adc_core_hw_start(struct stm32_adc_core_device *adc_dev) ++{ ++ TEE_Result res = TEE_ERROR_GENERIC; ++ struct stm32_adc_core_data *priv = &adc_dev->data; ++ ++ res = regulator_enable(priv->vdda); ++ if (res) { ++ EMSG("VDDA enable failed %#"PRIx32, res); ++ return res; ++ } ++ ++ res = regulator_enable(priv->vref); ++ if (res) { ++ EMSG("VREF enable failed %#"PRIx32, res); ++ goto err_vdda; ++ } ++ ++ res = clk_enable(priv->bclk); ++ if (res) { ++ EMSG("Failed to enable bclk %#"PRIx32, res); ++ goto err_vref; ++ } ++ ++ if (priv->aclk) { ++ res = clk_enable(priv->aclk); ++ if (res) { ++ EMSG("Failed to enable aclk %#"PRIx32, res); ++ goto err_bclk; ++ } ++ } ++ ++ return TEE_SUCCESS; ++ ++err_bclk: ++ clk_disable(priv->bclk); ++ ++err_vref: ++ regulator_disable(priv->vref); ++ ++err_vdda: ++ regulator_disable(priv->vdda); ++ ++ return res; ++} ++ ++static TEE_Result stm32_adc_core_hw_stop(struct stm32_adc_core_device *adc_dev) ++{ ++ struct stm32_adc_core_data *priv = &adc_dev->data; ++ TEE_Result res1 = TEE_ERROR_GENERIC; ++ TEE_Result res2 = TEE_ERROR_GENERIC; ++ ++ if (priv->aclk) ++ clk_disable(priv->aclk); ++ clk_disable(priv->bclk); ++ ++ res1 = regulator_disable(priv->vref); ++ res2 = regulator_disable(priv->vdda); ++ ++ if (res1) { ++ EMSG("Failed to disable VREF regulator"); ++ return res1; ++ } ++ ++ if (res2) { ++ EMSG("Failed to disable VDDA regulator"); ++ return res2; ++ } ++ ++ return TEE_SUCCESS; ++} ++ ++static TEE_Result stm32_adc_core_clk_sel(struct stm32_adc_core_device *adc_dev) ++{ ++ struct stm32_adc_core_data *priv = &adc_dev->data; ++ uint32_t ckmode = 0; ++ uint32_t presc = 0; ++ unsigned long rate = 0; ++ unsigned int i = 0; ++ int div = 0; ++ TEE_Result res = TEE_ERROR_GENERIC; ++ ++ /* ++ * The ADC can use either 'bus' or 'adc' clock for analog circuitry. ++ * So, choice is to have bus clock mandatory and ADC clock optional. ++ * If optional 'adc' clock has been found, then try to use it first. ++ */ ++ if (priv->aclk) { ++ /* ++ * Asynchronous clock modes (e.g. ckmode == 0) ++ * From spec: PLL output musn't exceed max rate ++ */ ++ rate = clk_get_rate(priv->aclk); ++ if (!rate) { ++ EMSG("Invalid %s clock rate", clk_get_name(priv->aclk)); ++ return TEE_ERROR_GENERIC; ++ } ++ ++ for (i = 0; i < ARRAY_SIZE(stm32mp13_adc_ckmodes_spec); i++) { ++ ckmode = stm32mp13_adc_ckmodes_spec[i].ckmode; ++ presc = stm32mp13_adc_ckmodes_spec[i].presc; ++ div = stm32mp13_adc_ckmodes_spec[i].div; ++ ++ if (ckmode) ++ continue; ++ ++ if ((rate / div) <= STM32MP13_ADC_MAX_CLK_RATE) ++ goto out; ++ } ++ } ++ ++ /* Synchronous clock modes (e.g. ckmode is 1, 2 or 3) */ ++ rate = clk_get_rate(priv->bclk); ++ if (!rate) { ++ EMSG("Invalid bus clock rate: 0"); ++ return TEE_ERROR_GENERIC; ++ } ++ ++ for (i = 0; i < ARRAY_SIZE(stm32mp13_adc_ckmodes_spec); i++) { ++ ckmode = stm32mp13_adc_ckmodes_spec[i].ckmode; ++ presc = stm32mp13_adc_ckmodes_spec[i].presc; ++ div = stm32mp13_adc_ckmodes_spec[i].div; ++ ++ if (!ckmode) ++ continue; ++ ++ if ((rate / div) <= STM32MP13_ADC_MAX_CLK_RATE) ++ goto out; ++ } ++ ++ EMSG("Clock selection failed"); ++ ++ return TEE_ERROR_GENERIC; ++ ++out: ++ /* rate used later by each ADC instance to control BOOST mode */ ++ priv->common.rate = rate / div; ++ ++ res = clk_enable(priv->bclk); ++ if (res) { ++ EMSG("Failed to enable bclk"); ++ return res; ++ } ++ ++ /* Set common clock mode and prescaler */ ++ io_clrsetbits32(priv->common.regs + STM32MP13_ADC_CCR, ++ STM32MP13_CKMODE_MASK | STM32MP13_PRESC_MASK, ++ ckmode << STM32MP13_CKMODE_SHIFT | ++ presc << STM32MP13_PRESC_SHIFT); ++ ++ clk_disable(priv->bclk); ++ ++ DMSG("Using %s clock/%d source at %ld kHz", ++ ckmode ? "bus" : "adc", div, priv->common.rate / 1000); ++ ++ priv->common.rate = presc; ++ ++ return TEE_SUCCESS; ++} ++ ++static struct stm32_adc_common * ++stm32_adc_core_get_common_data(struct dt_driver_phandle_args *pargs __unused, ++ void *data, TEE_Result *res) ++{ ++ if (data) ++ *res = TEE_SUCCESS; ++ else ++ *res = TEE_ERROR_GENERIC; ++ ++ return (struct stm32_adc_common *)data; ++} ++ ++static TEE_Result ++stm32_adc_core_pm_resume(struct stm32_adc_core_device *adc_dev) ++{ ++ return stm32_adc_core_hw_start(adc_dev); ++} ++ ++static TEE_Result ++stm32_adc_core_pm_suspend(struct stm32_adc_core_device *adc_dev) ++{ ++ return stm32_adc_core_hw_stop(adc_dev); ++} ++ ++static TEE_Result ++stm32_adc_core_pm(enum pm_op op, unsigned int pm_hint __unused, ++ const struct pm_callback_handle *pm_handle) ++{ ++ TEE_Result res = TEE_ERROR_GENERIC; ++ struct stm32_adc_core_device *dev = ++ (struct stm32_adc_core_device *)PM_CALLBACK_GET_HANDLE(pm_handle); ++ ++ if (op == PM_OP_RESUME) ++ res = stm32_adc_core_pm_resume(dev); ++ else ++ res = stm32_adc_core_pm_suspend(dev); ++ ++ return res; ++} ++ ++static TEE_Result stm32_adc_core_probe(const void *fdt, int node, ++ const void *compat_data __unused) ++{ ++ struct dt_node_info dt_info = { }; ++ struct stm32_adc_core_device *adc_dev = NULL; ++ struct stm32_adc_core_data *priv = NULL; ++ struct io_pa_va base = { }; ++ int subnode = 0; ++ TEE_Result res = TEE_ERROR_GENERIC; ++ ++ adc_dev = calloc(1, sizeof(*adc_dev)); ++ if (!adc_dev) ++ return TEE_ERROR_OUT_OF_MEMORY; ++ ++ priv = &adc_dev->data; ++ priv->common.dev = adc_dev; ++ ++ _fdt_fill_device_info(fdt, &dt_info, node); ++ ++ if (dt_info.reg == DT_INFO_INVALID_REG || ++ dt_info.interrupt == DT_INFO_INVALID_INTERRUPT) ++ goto err; ++ ++ base.pa = dt_info.reg; ++ priv->common.regs = io_pa_or_va_secure(&base, dt_info.reg_size); ++ ++ priv->vref = regulator_get_by_supply_name(fdt, node, "vref"); ++ if (!priv->vref) { ++ res = TEE_ERROR_DEFER_DRIVER_INIT; ++ goto err; ++ } ++ ++ priv->vdda = regulator_get_by_supply_name(fdt, node, "vdda"); ++ if (!priv->vdda) { ++ res = TEE_ERROR_DEFER_DRIVER_INIT; ++ goto err; ++ } ++ ++ res = regulator_set_min_voltage(priv->vref); ++ if (res && res != TEE_ERROR_NOT_IMPLEMENTED) ++ goto err; ++ ++ res = regulator_get_voltage(priv->vref, &priv->common.vref_mv); ++ if (res) ++ goto err; ++ ++ res = clk_dt_get_by_name(fdt, node, "bus", &priv->bclk); ++ if (res) { ++ if (res != TEE_ERROR_DEFER_DRIVER_INIT) ++ EMSG("Error %#"PRIx32" getting 'bus' clock", res); ++ goto err; ++ } ++ ++ /* The 'adc' clock is optional. Only check defer status. */ ++ res = clk_dt_get_by_name(fdt, node, "adc", &priv->aclk); ++ if (res == TEE_ERROR_DEFER_DRIVER_INIT) ++ goto err; ++ ++ res = stm32_adc_core_hw_start(adc_dev); ++ if (res) ++ goto err; ++ ++ res = stm32_adc_core_clk_sel(adc_dev); ++ if (res) { ++ EMSG("Error %#"PRIx32" selecting clock", res); ++ goto err_stop; ++ } ++ ++ register_pm_core_service_cb(stm32_adc_core_pm, adc_dev, ++ "stm32-adc-core"); ++ ++ res = dt_driver_register_provider(fdt, node, ++ (get_of_device_func) ++ stm32_adc_core_get_common_data, ++ (void *)&priv->common, ++ DT_DRIVER_NOTYPE); ++ if (res) { ++ EMSG("Couldn't register ADC core provider"); ++ goto err_stop; ++ } ++ ++ fdt_for_each_subnode(subnode, fdt, node) { ++ res = dt_driver_maybe_add_probe_node(fdt, subnode); ++ if (res) { ++ EMSG("Failed on node %s with %#"PRIx32, ++ fdt_get_name(fdt, subnode, NULL), res); ++ return res; ++ } ++ } ++ ++ DMSG("ADC core %s probed", fdt_get_name(fdt, node, NULL)); ++ ++ return TEE_SUCCESS; ++ ++err_stop: ++ stm32_adc_core_hw_stop(adc_dev); ++ ++err: ++ free(adc_dev); ++ ++ return res; ++} ++ ++static const struct dt_device_match stm32_adc_core_match_table[] = { ++ { .compatible = "st,stm32mp13-adc-core" }, ++ { } ++}; ++ ++DEFINE_DT_DRIVER(stm32_adc_core_dt_driver) = { ++ .name = "stm32-adc-core", ++ .match_table = stm32_adc_core_match_table, ++ .probe = stm32_adc_core_probe, ++}; +diff --git a/core/drivers/adc/sub.mk b/core/drivers/adc/sub.mk +new file mode 100644 +index 0000000000..fc2f114312 +--- /dev/null ++++ b/core/drivers/adc/sub.mk +@@ -0,0 +1,2 @@ ++srcs-y += adc_fw.c ++srcs-$(CFG_STM32_ADC) += stm32_adc_core.c stm32_adc.c +diff --git a/core/drivers/clk/clk-stm32-core.c b/core/drivers/clk/clk-stm32-core.c +index cee09799cc..6a9d7127ae 100644 +--- a/core/drivers/clk/clk-stm32-core.c ++++ b/core/drivers/clk/clk-stm32-core.c +@@ -1,6 +1,6 @@ + // SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause) + /* +- * Copyright (C) 2018-2021, STMicroelectronics - All Rights Reserved ++ * Copyright (C) STMicroelectronics 2022 - All Rights Reserved + */ + + #include +@@ -18,9 +18,12 @@ + + #define RCC_MP_ENCLRR_OFFSET 0x4 + ++#define TIMEOUT_US_200MS U(200000) ++#define TIMEOUT_US_1S U(1000000) ++ + static struct clk_stm32_priv *stm32_clock_data; + +-inline struct clk_stm32_priv *clk_stm32_get_priv(void) ++struct clk_stm32_priv *clk_stm32_get_priv(void) + { + return stm32_clock_data; + } +@@ -32,11 +35,8 @@ uintptr_t clk_stm32_get_rcc_base(void) + return priv->base; + } + +-#define TIMEOUT_US_200MS U(200000) +-#define TIMEOUT_US_1S U(1000000) +- + /* STM32 MUX API */ +-int clk_stm32_get_parent_mux(uint32_t mux_id) ++size_t stm32_mux_get_parent(uint32_t mux_id) + { + struct clk_stm32_priv *priv = clk_stm32_get_priv(); + const struct mux_cfg *mux = &priv->muxes[mux_id]; +@@ -45,7 +45,7 @@ int clk_stm32_get_parent_mux(uint32_t mux_id) + return (io_read32(priv->base + mux->offset) & mask) >> mux->shift; + } + +-int clk_stm32_set_parent_mux(uint16_t mux_id, uint8_t sel) ++TEE_Result stm32_mux_set_parent(uint16_t mux_id, uint8_t sel) + { + struct clk_stm32_priv *priv = clk_stm32_get_priv(); + const struct mux_cfg *mux = &priv->muxes[mux_id]; +@@ -54,45 +54,56 @@ int clk_stm32_set_parent_mux(uint16_t mux_id, uint8_t sel) + + io_clrsetbits32(address, mask, (sel << mux->shift) & mask); + +- if (mux->ready == MUX_NO_RDY) +- return 0; ++ if (mux->ready != MUX_NO_RDY) ++ return stm32_gate_wait_ready((uint16_t)mux->ready, true); + +- return clk_stm32_wait_ready_gate((uint16_t) mux->ready, true); ++ return TEE_SUCCESS; + } + + /* STM32 GATE API */ +-void clk_stm32_endisable_gate(uint16_t gate_id, bool enable) ++static void stm32_gate_endisable(uint16_t gate_id, bool enable) + { + struct clk_stm32_priv *priv = clk_stm32_get_priv(); + const struct gate_cfg *gate = &priv->gates[gate_id]; + uintptr_t addr = priv->base + gate->offset; + + if (enable) { +- if (gate->set_clr != 0U) ++ if (gate->set_clr) + io_write32(addr, BIT(gate->bit_idx)); + else + io_setbits32_stm32shregs(addr, BIT(gate->bit_idx)); + } else { +- if (gate->set_clr != 0U) +- io_write32(addr + RCC_MP_ENCLRR_OFFSET, BIT(gate->bit_idx)); ++ if (gate->set_clr) ++ io_write32(addr + RCC_MP_ENCLRR_OFFSET, ++ BIT(gate->bit_idx)); + else + io_clrbits32_stm32shregs(addr, BIT(gate->bit_idx)); + } + } + +-void clk_stm32_disable_gate(uint16_t gate_id) ++void stm32_gate_disable(uint16_t gate_id) + { +- clk_stm32_endisable_gate(gate_id, false); ++ struct clk_stm32_priv *priv = clk_stm32_get_priv(); ++ uint8_t *gate_cpt = priv->gate_cpt; ++ ++ if (--gate_cpt[gate_id] > 0) ++ return; ++ ++ stm32_gate_endisable(gate_id, false); + } + +-int clk_stm32_enable_gate(uint16_t gate_id) ++void stm32_gate_enable(uint16_t gate_id) + { +- clk_stm32_endisable_gate(gate_id, true); ++ struct clk_stm32_priv *priv = clk_stm32_get_priv(); ++ uint8_t *gate_cpt = priv->gate_cpt; + +- return 0; ++ if (gate_cpt[gate_id]++ > 0) ++ return; ++ ++ stm32_gate_endisable(gate_id, true); + } + +-bool clk_stm32_is_enabled_gate(uint16_t gate_id) ++bool stm32_gate_is_enabled(uint16_t gate_id) + { + struct clk_stm32_priv *priv = clk_stm32_get_priv(); + const struct gate_cfg *gate = &priv->gates[gate_id]; +@@ -101,7 +112,7 @@ bool clk_stm32_is_enabled_gate(uint16_t gate_id) + return (io_read32(addr) & BIT(gate->bit_idx)) != 0U; + } + +-int clk_stm32_wait_ready_gate(uint16_t gate_id, bool ready_on) ++TEE_Result stm32_gate_wait_ready(uint16_t gate_id, bool ready_on) + { + struct clk_stm32_priv *priv = clk_stm32_get_priv(); + const struct gate_cfg *gate = &priv->gates[gate_id]; +@@ -118,39 +129,38 @@ int clk_stm32_wait_ready_gate(uint16_t gate_id, bool ready_on) + break; + + if ((io_read32(address) & mask_rdy) != mask) +- return -1; ++ return TEE_ERROR_GENERIC; + +- return 0; ++ return TEE_SUCCESS; + } + +-/* STM32 GATE READY CLOCK OPS */ +-int clk_stm32_gate_ready_endisable(uint16_t gate_id, bool enable, bool wait_rdy) ++/* STM32 GATE READY clock operators */ ++static TEE_Result stm32_gate_ready_endisable(uint16_t gate_id, bool enable, ++ bool wait_rdy) + { +- clk_stm32_endisable_gate(gate_id, enable); ++ stm32_gate_endisable(gate_id, enable); + +- if (wait_rdy == true) +- return clk_stm32_wait_ready_gate(gate_id + 1, enable); ++ if (wait_rdy) ++ return stm32_gate_wait_ready(gate_id + 1, enable); + +- return 0; ++ return TEE_SUCCESS; + } + +-int clk_stm32_gate_rdy_enable(uint16_t gate_id) ++TEE_Result stm32_gate_rdy_enable(uint16_t gate_id) + { +- return clk_stm32_gate_ready_endisable(gate_id, true, true); ++ return stm32_gate_ready_endisable(gate_id, true, true); + } + +-int clk_stm32_gate_rdy_disable(uint16_t gate_id) ++TEE_Result stm32_gate_rdy_disable(uint16_t gate_id) + { +- return clk_stm32_gate_ready_endisable(gate_id, false, true); ++ return stm32_gate_ready_endisable(gate_id, false, true); + } + + /* STM32 DIV API */ +-#define clk_div_mask(_width) GENMASK_32(((_width) - 1U), 0U) +- + static unsigned int _get_table_div(const struct div_table_cfg *table, + unsigned int val) + { +- const struct div_table_cfg *clkt; ++ const struct div_table_cfg *clkt = NULL; + + for (clkt = table; clkt->div; clkt++) + if (clkt->val == val) +@@ -159,15 +169,15 @@ static unsigned int _get_table_div(const struct div_table_cfg *table, + return 0; + } + +- + static unsigned int _get_table_val(const struct div_table_cfg *table, + unsigned int div) + { +- const struct div_table_cfg *clkt; ++ const struct div_table_cfg *clkt = NULL; + + for (clkt = table; clkt->div; clkt++) + if (clkt->div == div) + return clkt->val; ++ + return 0; + } + +@@ -175,16 +185,16 @@ static unsigned int _get_div(const struct div_table_cfg *table, + unsigned int val, unsigned long flags, + uint8_t width) + { +- if ((flags & CLK_DIVIDER_ONE_BASED) != 0UL) ++ if (flags & CLK_DIVIDER_ONE_BASED) + return val; + +- if ((flags & CLK_DIVIDER_POWER_OF_TWO) != 0UL) ++ if (flags & CLK_DIVIDER_POWER_OF_TWO) + return BIT(val); + +- if ((flags & CLK_DIVIDER_MAX_AT_ZERO) != 0UL) ++ if (flags & CLK_DIVIDER_MAX_AT_ZERO) + return (val != 0U) ? val : BIT(width); + +- if (table != NULL) ++ if (table) + return _get_table_div(table, val); + + return val + 1U; +@@ -194,30 +204,25 @@ static unsigned int _get_val(const struct div_table_cfg *table, + unsigned int div, unsigned long flags, + uint8_t width) + { +- if ((flags & CLK_DIVIDER_ONE_BASED) != 0UL) ++ if (flags & CLK_DIVIDER_ONE_BASED) + return div; + +- if ((flags & CLK_DIVIDER_POWER_OF_TWO) != 0UL) ++ if (flags & CLK_DIVIDER_POWER_OF_TWO) + return __builtin_ffs(div) - 1; + +- if ((flags & CLK_DIVIDER_MAX_AT_ZERO) != 0UL) ++ if (flags & CLK_DIVIDER_MAX_AT_ZERO) + return (div != 0U) ? div : BIT(width); + +- if (table != NULL) ++ if (table) + return _get_table_val(table, div); + + return div - 1U; + } + +-static bool is_power_of_2(unsigned long n) +-{ +- return (n != 0 && ((n & (n - 1)) == 0)); +-} +- + static bool _is_valid_table_div(const struct div_table_cfg *table, + unsigned int div) + { +- const struct div_table_cfg *clkt; ++ const struct div_table_cfg *clkt = NULL; + + for (clkt = table; clkt->div; clkt++) + if (clkt->div == div) +@@ -230,7 +235,7 @@ static bool _is_valid_div(const struct div_table_cfg *table, + unsigned int div, unsigned long flags) + { + if (flags & CLK_DIVIDER_POWER_OF_TWO) +- return is_power_of_2(div); ++ return IS_POWER_OF_TWO(div); + + if (table) + return _is_valid_table_div(table, div); +@@ -252,27 +257,27 @@ static int divider_get_val(unsigned long rate, unsigned long parent_rate, + + value = _get_val(table, div, flags, width); + +- return MIN(value, clk_div_mask(width)); ++ return MIN(value, MASK_WIDTH_SHIFT(width, 0)); + } + +-uint32_t clk_stm32_div_get_value(int div_id) ++uint32_t stm32_div_get_value(int div_id) + { + struct clk_stm32_priv *priv = clk_stm32_get_priv(); + const struct div_cfg *divider = &priv->div[div_id]; + uint32_t val = 0; + + val = io_read32(priv->base + divider->offset) >> divider->shift; +- val &= clk_div_mask(divider->width); ++ val &= MASK_WIDTH_SHIFT(divider->width, 0); + + return val; + } + +-int clk_stm32_set_div_value(uint32_t div_id, uint32_t value) ++TEE_Result stm32_div_set_value(uint32_t div_id, uint32_t value) + { + struct clk_stm32_priv *priv = clk_stm32_get_priv(); +- const struct div_cfg *divider; +- uintptr_t address; +- uint32_t mask; ++ const struct div_cfg *divider = NULL; ++ uintptr_t address = 0; ++ uint32_t mask = 0; + + if (div_id >= priv->nb_div) + panic(); +@@ -284,27 +289,27 @@ int clk_stm32_set_div_value(uint32_t div_id, uint32_t value) + io_clrsetbits32(address, mask, (value << divider->shift) & mask); + + if (divider->ready == DIV_NO_RDY) +- return 0; ++ return TEE_SUCCESS; + +- return clk_stm32_wait_ready_gate((uint16_t) divider->ready, true); ++ return stm32_gate_wait_ready((uint16_t)divider->ready, true); + } + +-unsigned long clk_stm32_get_rate_divider(int div_id, unsigned long prate) ++static unsigned long stm32_div_get_rate(int div_id, unsigned long prate) + { + struct clk_stm32_priv *priv = clk_stm32_get_priv(); + const struct div_cfg *divider = &priv->div[div_id]; +- uint32_t val = clk_stm32_div_get_value(div_id); ++ uint32_t val = stm32_div_get_value(div_id); + unsigned int div = 0U; + + div = _get_div(divider->table, val, divider->flags, divider->width); +- if (div == 0U) ++ if (!div) + return prate; + + return ROUNDUP_DIV((uint64_t)prate, div); + } + +-int clk_stm32_set_rate_divider(int div_id, unsigned long rate, +- unsigned long prate) ++TEE_Result stm32_div_set_rate(int div_id, unsigned long rate, ++ unsigned long prate) + { + struct clk_stm32_priv *priv = clk_stm32_get_priv(); + const struct div_cfg *divider = &priv->div[div_id]; +@@ -314,24 +319,24 @@ int clk_stm32_set_rate_divider(int div_id, unsigned long rate, + divider->width, divider->flags); + + if (value < 0) +- return value; ++ return TEE_ERROR_GENERIC; + +- return clk_stm32_set_div_value(div_id, value); ++ return stm32_div_set_value(div_id, value); + } + +-/* STM32 MUX CLOCK OPS */ +-size_t clk_stm32_mux_get_parent(struct clk *clk) ++/* STM32 MUX clock operators */ ++static size_t clk_stm32_mux_get_parent(struct clk *clk) + { + struct clk_stm32_mux_cfg *cfg = clk->priv; + +- return clk_stm32_get_parent_mux(cfg->mux_id); ++ return stm32_mux_get_parent(cfg->mux_id); + } + +-TEE_Result clk_stm32_mux_set_parent(struct clk *clk, size_t pidx) ++static TEE_Result clk_stm32_mux_set_parent(struct clk *clk, size_t pidx) + { + struct clk_stm32_mux_cfg *cfg = clk->priv; + +- return clk_stm32_set_parent_mux(cfg->mux_id, pidx); ++ return stm32_mux_set_parent(cfg->mux_id, pidx); + } + + const struct clk_ops clk_stm32_mux_ops = { +@@ -339,26 +344,28 @@ const struct clk_ops clk_stm32_mux_ops = { + .set_parent = clk_stm32_mux_set_parent, + }; + +-/* STM32 GATE CLOCK OPS */ ++/* STM32 GATE clock operators */ + static TEE_Result clk_stm32_gate_enable(struct clk *clk) + { + struct clk_stm32_gate_cfg *cfg = clk->priv; + +- return clk_stm32_enable_gate(cfg->gate_id); ++ stm32_gate_enable(cfg->gate_id); ++ ++ return TEE_SUCCESS; + } + + static void clk_stm32_gate_disable(struct clk *clk) + { + struct clk_stm32_gate_cfg *cfg = clk->priv; + +- clk_stm32_disable_gate(cfg->gate_id); ++ stm32_gate_disable(cfg->gate_id); + } + + static bool clk_stm32_gate_is_enabled(struct clk *clk) + { + struct clk_stm32_gate_cfg *cfg = clk->priv; + +- return clk_stm32_is_enabled_gate(cfg->gate_id); ++ return stm32_gate_is_enabled(cfg->gate_id); + } + + const struct clk_ops clk_stm32_gate_ops = { +@@ -371,19 +378,14 @@ static TEE_Result clk_stm32_gate_ready_enable(struct clk *clk) + { + struct clk_stm32_gate_cfg *cfg = clk->priv; + +- if (clk_stm32_gate_rdy_enable(cfg->gate_id) != 0U) { +- EMSG("%s timeout\n", clk_get_name(clk)); +- panic(); +- } +- +- return 0; ++ return stm32_gate_rdy_enable(cfg->gate_id); + } + + static void clk_stm32_gate_ready_disable(struct clk *clk) + { + struct clk_stm32_gate_cfg *cfg = clk->priv; + +- if (clk_stm32_gate_rdy_disable(cfg->gate_id) != 0U) ++ if (stm32_gate_rdy_disable(cfg->gate_id)) + panic(); + } + +@@ -393,13 +395,13 @@ const struct clk_ops clk_stm32_gate_ready_ops = { + .is_enabled = clk_stm32_gate_is_enabled, + }; + +-/* STM32 DIV CLOCK OPS */ ++/* STM32 DIV clock operators */ + unsigned long clk_stm32_divider_get_rate(struct clk *clk, +- unsigned long parent_rate) ++ unsigned long parent_rate) + { + struct clk_stm32_div_cfg *cfg = clk->priv; + +- return clk_stm32_get_rate_divider(cfg->div_id, parent_rate); ++ return stm32_div_get_rate(cfg->div_id, parent_rate); + } + + TEE_Result clk_stm32_divider_set_rate(struct clk *clk, +@@ -408,7 +410,7 @@ TEE_Result clk_stm32_divider_set_rate(struct clk *clk, + { + struct clk_stm32_div_cfg *cfg = clk->priv; + +- return clk_stm32_set_rate_divider(cfg->div_id, rate, parent_rate); ++ return stm32_div_set_rate(cfg->div_id, rate, parent_rate); + } + + const struct clk_ops clk_stm32_divider_ops = { +@@ -416,28 +418,27 @@ const struct clk_ops clk_stm32_divider_ops = { + .set_rate = clk_stm32_divider_set_rate, + }; + +-/* STM32 COMPOSITE CLOCK OPS */ ++/* STM32 COMPOSITE clock operators */ + size_t clk_stm32_composite_get_parent(struct clk *clk) + { + struct clk_stm32_composite_cfg *cfg = clk->priv; + + if (cfg->mux_id == NO_MUX) { +- /* TODO: it could be a normal case */ ++ /* It could be a normal case */ + return 0; + } + +- return clk_stm32_get_parent_mux(cfg->mux_id); ++ return stm32_mux_get_parent(cfg->mux_id); + } + + TEE_Result clk_stm32_composite_set_parent(struct clk *clk, size_t pidx) + { + struct clk_stm32_composite_cfg *cfg = clk->priv; + +- if (cfg->mux_id == NO_MUX) { ++ if (cfg->mux_id == NO_MUX) + panic(); +- } + +- return clk_stm32_set_parent_mux(cfg->mux_id, pidx); ++ return stm32_mux_set_parent(cfg->mux_id, pidx); + } + + unsigned long clk_stm32_composite_get_rate(struct clk *clk, +@@ -448,7 +449,7 @@ unsigned long clk_stm32_composite_get_rate(struct clk *clk, + if (cfg->div_id == NO_DIV) + return parent_rate; + +- return clk_stm32_get_rate_divider(cfg->div_id, parent_rate); ++ return stm32_div_get_rate(cfg->div_id, parent_rate); + } + + TEE_Result clk_stm32_composite_set_rate(struct clk *clk, unsigned long rate, +@@ -457,30 +458,32 @@ TEE_Result clk_stm32_composite_set_rate(struct clk *clk, unsigned long rate, + struct clk_stm32_composite_cfg *cfg = clk->priv; + + if (cfg->div_id == NO_DIV) +- return 0; ++ return TEE_SUCCESS; + +- return clk_stm32_set_rate_divider(cfg->div_id, rate, parent_rate); ++ return stm32_div_set_rate(cfg->div_id, rate, parent_rate); + } + + TEE_Result clk_stm32_composite_gate_enable(struct clk *clk) + { + struct clk_stm32_composite_cfg *cfg = clk->priv; + +- return clk_stm32_enable_gate(cfg->gate_id); ++ stm32_gate_enable(cfg->gate_id); ++ ++ return TEE_SUCCESS; + } + + void clk_stm32_composite_gate_disable(struct clk *clk) + { + struct clk_stm32_composite_cfg *cfg = clk->priv; + +- clk_stm32_disable_gate(cfg->gate_id); ++ stm32_gate_disable(cfg->gate_id); + } + + bool clk_stm32_composite_gate_is_enabled(struct clk *clk) + { + struct clk_stm32_composite_cfg *cfg = clk->priv; + +- return clk_stm32_is_enabled_gate(cfg->gate_id); ++ return stm32_gate_is_enabled(cfg->gate_id); + } + + const struct clk_ops clk_stm32_composite_ops = { +@@ -519,7 +522,7 @@ static bool clk_stm32_get_ignore_unused_property(void) + return false; + + prop = fdt_getprop(fdt, node, "bootargs", NULL); +- if (prop == NULL) ++ if (!prop) + return false; + + return strcmp(prop, "clk_ignore_unused") == 0; +@@ -528,42 +531,44 @@ static bool clk_stm32_get_ignore_unused_property(void) + int clk_stm32_parse_fdt_by_name(const void *fdt, int node, const char *name, + uint32_t *tab, uint32_t *nb) + { +- const fdt32_t *cell; ++ const fdt32_t *cell = NULL; + int len = 0; +- uint32_t i; ++ uint32_t i = 0; + + cell = fdt_getprop(fdt, node, name, &len); +- if (cell != NULL) { +- for (i = 0; i < ((uint32_t)len / sizeof(uint32_t)); i++) { +- uint32_t val = fdt32_to_cpu(cell[i]); +- +- tab[i] = val; +- } +- } ++ if (cell) ++ for (i = 0; i < ((uint32_t)len / sizeof(uint32_t)); i++) ++ tab[i] = fdt32_to_cpu(cell[i]); + + *nb = (uint32_t)len / sizeof(uint32_t); + + return 0; + } +-int clk_stm32_init(struct clk_stm32_priv *priv, uintptr_t base) ++ ++TEE_Result clk_stm32_init(struct clk_stm32_priv *priv, uintptr_t base) + { + stm32_clock_data = priv; + + priv->base = base; + ++ priv->gate_cpt = calloc(priv->nb_gates, sizeof(uint8_t)); ++ if (!priv->gate_cpt) ++ return TEE_ERROR_OUT_OF_MEMORY; ++ + priv->clk_ignore_unused = clk_stm32_get_ignore_unused_property(); + +- return 0; ++ return TEE_SUCCESS; + } + +-unsigned long fixed_factor_get_rate(struct clk *clk, unsigned long parent_rate) ++static unsigned long fixed_factor_get_rate(struct clk *clk, ++ unsigned long parent_rate) + { + struct fixed_factor_cfg *d = clk->priv; + + unsigned long long rate = (unsigned long long)parent_rate * d->mult; + + if (d->div == 0U) +- panic("error division by zero\n"); ++ panic("error division by zero"); + + return (unsigned long)(rate / d->div); + }; +@@ -615,12 +620,12 @@ static struct clk *stm32mp_clk_dt_get_clk(struct dt_driver_phandle_args *pargs, + + static void clk_stm32_register_clocks(struct clk_stm32_priv *priv) + { +- size_t i = 0; ++ unsigned int i = 0; + +- for(i = 0; i < priv->nb_clk_refs; i++) { ++ for (i = 0; i < priv->nb_clk_refs; i++) { + struct clk *clk = priv->clk_refs[i]; + +- if (clk == NULL) ++ if (!clk) + continue; + + refcount_set(&clk->enabled_count, 0); +@@ -630,10 +635,10 @@ static void clk_stm32_register_clocks(struct clk_stm32_priv *priv) + } + + /* Critical clocks management */ +- for(i = 0; i < priv->nb_clk_refs; i++) { ++ for (i = 0; i < priv->nb_clk_refs; i++) { + struct clk *clk = priv->clk_refs[i]; + +- if (clk == NULL) ++ if (!clk) + continue; + + if (priv->is_critical && priv->is_critical(clk)) +@@ -645,26 +650,26 @@ static void clk_stm32_register_clocks(struct clk_stm32_priv *priv) + static void clk_stm32_display_clock_ignore_unused(void) + { + struct clk_stm32_priv *priv = clk_stm32_get_priv(); +- size_t i; ++ unsigned int i = 0; + + printf("\nCLOCK CLK_IGNORE_UNUSED:\n"); + + printf("\tSTATUS = %s\n", +- priv->clk_ignore_unused ? "ENABLED" : "DISABLED"); ++ priv->clk_ignore_unused ? "ENABLED" : "DISABLED"); + + printf("CLOCK WITH CLK_IGNORE_UNUSED FLAGS:\n"); +- for(i = 0; i < priv->nb_clk_refs; i++) { ++ for (i = 0; i < priv->nb_clk_refs; i++) { + struct clk *clk = priv->clk_refs[i]; + + if (priv->is_ignore_unused && priv->is_ignore_unused(clk)) +- printf("\t%s\n", clk_get_name(clk)); ++ printf("\t%s\n", clk_get_name(clk)); + } + + printf("CLOCK DISABLED IF CLK_IGNORE_UNUSED IS DISABLED:\n"); +- for(i = 0; i < priv->nb_clk_refs; i++) { ++ for (i = 0; i < priv->nb_clk_refs; i++) { + struct clk *clk = priv->clk_refs[i]; + +- if (clk == NULL) ++ if (!clk) + continue; + + /* if counter > 0 */ +@@ -676,9 +681,10 @@ static void clk_stm32_display_clock_ignore_unused(void) + + if (clk->ops->is_enabled && clk->ops->is_enabled(clk) && + clk->ops->disable) { +- printf("\t%s, EN = %d COUNTER = %d\n", clk_get_name(clk), +- clk->ops->is_enabled(clk), +- clk->enabled_count.val); ++ printf("\t%s, EN = %d COUNTER = %d\n", ++ clk_get_name(clk), ++ clk->ops->is_enabled(clk), ++ clk->enabled_count.val); + } + } + printf("\n"); +@@ -688,7 +694,7 @@ static void clk_stm32_display_clock_ignore_unused(void) + void clk_stm32_clock_ignore_unused(void) + { + struct clk_stm32_priv *priv = clk_stm32_get_priv(); +- size_t i; ++ unsigned int i = 0; + + #ifdef CFG_STM32_CLK_DEBUG + clk_stm32_display_clock_ignore_unused(); +@@ -697,10 +703,10 @@ void clk_stm32_clock_ignore_unused(void) + if (priv->clk_ignore_unused) + return; + +- for(i = 0; i < priv->nb_clk_refs; i++) { ++ for (i = 0; i < priv->nb_clk_refs; i++) { + struct clk *clk = priv->clk_refs[i]; + +- if (clk == NULL) ++ if (!clk) + continue; + + /* if counter > 0 */ +@@ -715,7 +721,7 @@ void clk_stm32_clock_ignore_unused(void) + DMSG("%s: disabling %s ...\n", __func__, + clk_get_name(clk)); + clk->ops->disable(clk); +- } ++ } + } + } + +diff --git a/core/drivers/clk/clk-stm32-core.h b/core/drivers/clk/clk-stm32-core.h +index c05beebdc9..e56b25acf6 100644 +--- a/core/drivers/clk/clk-stm32-core.h ++++ b/core/drivers/clk/clk-stm32-core.h +@@ -1,6 +1,6 @@ + /* SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause) */ + /* +- * Copyright (C) 2018-2022, STMicroelectronics - All Rights Reserved ++ * Copyright (C) STMicroelectronics 2022 - All Rights Reserved + */ + + #ifndef CLK_STM32_CORE_H +@@ -42,6 +42,7 @@ struct clk_stm32_priv { + const struct mux_cfg *muxes; + const uint32_t nb_muxes; + const struct gate_cfg *gates; ++ uint8_t *gate_cpt; + const uint32_t nb_gates; + const struct div_cfg *div; + const uint32_t nb_div; +@@ -107,41 +108,34 @@ struct clk_stm32_gate_ready_cfg { + #define MUX_NO_RDY UINT8_MAX + + #define MASK_WIDTH_SHIFT(_width, _shift) \ +- GENMASK_32(((_width) + (_shift) - 1U), _shift) ++ GENMASK_32(((_width) + (_shift) - 1U), (_shift)) + + /* Define for composite clocks */ + #define NO_MUX INT32_MAX + #define NO_DIV INT32_MAX + #define NO_GATE INT32_MAX + +-int clk_stm32_enable_gate(uint16_t gate_id); +-void clk_stm32_disable_gate(uint16_t gate_id); +-void clk_stm32_endisable_gate(uint16_t gate_id, bool enable); +-bool clk_stm32_is_enabled_gate(uint16_t gate_id); +-int clk_stm32_gate_ready_endisable(uint16_t gate_id, bool enable, +- bool wait_rdy); +-int clk_stm32_wait_ready_gate(uint16_t gate_id, bool ready_on); +-int clk_stm32_gate_rdy_enable(uint16_t gate_id); +-int clk_stm32_gate_rdy_disable(uint16_t gate_id); +-int clk_stm32_get_parent_mux(uint32_t mux_id); +-int clk_stm32_set_parent_mux(uint16_t pid, uint8_t sel); +-int clk_stm32_set_rate_divider(int div_id, unsigned long rate, +- unsigned long prate); +- +- +-uint32_t clk_stm32_div_get_value(int div_id); +-int clk_stm32_set_div_value(uint32_t div_id, uint32_t value); +-unsigned long clk_stm32_get_rate_divider(int div_id, unsigned long prate); ++void stm32_gate_enable(uint16_t gate_id); ++void stm32_gate_disable(uint16_t gate_id); ++bool stm32_gate_is_enabled(uint16_t gate_id); ++TEE_Result stm32_gate_wait_ready(uint16_t gate_id, bool ready_on); ++TEE_Result stm32_gate_rdy_enable(uint16_t gate_id); ++TEE_Result stm32_gate_rdy_disable(uint16_t gate_id); + +-int clk_stm32_parse_fdt_by_name(const void *fdt, int node, const char *name, +- uint32_t *tab, uint32_t *nb); ++size_t stm32_mux_get_parent(uint32_t mux_id); ++TEE_Result stm32_mux_set_parent(uint16_t pid, uint8_t sel); + ++TEE_Result stm32_div_set_rate(int div_id, unsigned long rate, ++ unsigned long prate); + +-size_t clk_stm32_mux_get_parent(struct clk *clk); +-TEE_Result clk_stm32_mux_set_parent(struct clk *clk, size_t pidx); ++uint32_t stm32_div_get_value(int div_id); ++TEE_Result stm32_div_set_value(uint32_t div_id, uint32_t value); ++ ++int clk_stm32_parse_fdt_by_name(const void *fdt, int node, const char *name, ++ uint32_t *tab, uint32_t *nb); + + unsigned long clk_stm32_divider_get_rate(struct clk *clk, +- unsigned long parent_rate); ++ unsigned long parent_rate); + + TEE_Result clk_stm32_divider_set_rate(struct clk *clk, + unsigned long rate, +@@ -159,9 +153,6 @@ bool clk_stm32_composite_gate_is_enabled(struct clk *clk); + + TEE_Result clk_stm32_set_parent_by_index(struct clk *clk, size_t pidx); + +-unsigned long fixed_factor_get_rate(struct clk *clk, unsigned long parent_rate); +- +- + extern const struct clk_ops clk_fixed_factor_ops; + extern const struct clk_ops clk_fixed_clk_ops; + extern const struct clk_ops clk_stm32_gate_ops; +@@ -173,96 +164,96 @@ extern const struct clk_ops clk_stm32_composite_ops; + #define PARENT(x...) { x } + + #define STM32_FIXED_RATE(_name, _rate)\ +- struct clk _name = {\ +- .ops = &clk_fixed_clk_ops,\ +- .priv = &(struct clk_fixed_rate_cfg) {\ +- .rate = (_rate),\ +- },\ +- .name = #_name,\ +- .flags = 0,\ +- .num_parents = 0,\ +-} +- +-#define STM32_FIXED_FACTOR(_name, _parent, _flags, _mult_, _div)\ +- struct clk _name = {\ +- .ops = &clk_fixed_factor_ops,\ +- .priv = &(struct fixed_factor_cfg) {\ +- .mult = _mult_,\ +- .div = _div,\ +- },\ +- .name = #_name,\ +- .flags = (_flags),\ +- .num_parents = 1,\ +- .parents = { _parent },\ +-} ++ struct clk _name = {\ ++ .ops = &clk_fixed_clk_ops,\ ++ .priv = &(struct clk_fixed_rate_cfg) {\ ++ .rate = (_rate),\ ++ },\ ++ .name = #_name,\ ++ .flags = 0,\ ++ .num_parents = 0,\ ++ } ++ ++#define STM32_FIXED_FACTOR(_name, _parent, _flags, _mult, _div)\ ++ struct clk _name = {\ ++ .ops = &clk_fixed_factor_ops,\ ++ .priv = &(struct fixed_factor_cfg) {\ ++ .mult = _mult,\ ++ .div = _div,\ ++ },\ ++ .name = #_name,\ ++ .flags = (_flags),\ ++ .num_parents = 1,\ ++ .parents = { (_parent) },\ ++ } + + #define STM32_GATE(_name, _parent, _flags, _gate_id)\ +-struct clk _name = {\ +- .ops = &clk_stm32_gate_ops,\ +- .priv = &(struct clk_stm32_gate_cfg) {\ +- .gate_id = _gate_id,\ +- },\ +- .name = #_name,\ +- .flags = (_flags),\ +- .num_parents = 1,\ +- .parents = { _parent },\ +-} ++ struct clk _name = {\ ++ .ops = &clk_stm32_gate_ops,\ ++ .priv = &(struct clk_stm32_gate_cfg) {\ ++ .gate_id = _gate_id,\ ++ },\ ++ .name = #_name,\ ++ .flags = (_flags),\ ++ .num_parents = 1,\ ++ .parents = { (_parent) },\ ++ } + + #define STM32_DIVIDER(_name, _parent, _flags, _div_id)\ +-struct clk _name = {\ +- .ops = &clk_stm32_divider_ops,\ +- .priv = &(struct clk_stm32_div_cfg) {\ +- .div_id = (_div_id),\ +- },\ +- .name = #_name,\ +- .flags = (_flags),\ +- .num_parents = 1,\ +- .parents = { _parent },\ +-} ++ struct clk _name = {\ ++ .ops = &clk_stm32_divider_ops,\ ++ .priv = &(struct clk_stm32_div_cfg) {\ ++ .div_id = (_div_id),\ ++ },\ ++ .name = #_name,\ ++ .flags = (_flags),\ ++ .num_parents = 1,\ ++ .parents = { (_parent) },\ ++ } + + #define STM32_MUX(_name, _nb_parents, _parents, _flags, _mux_id)\ +-struct clk _name = {\ +- .ops = &clk_stm32_mux_ops,\ +- .priv = &(struct clk_stm32_mux_cfg) {\ +- .mux_id = (_mux_id),\ +- },\ +- .name = #_name,\ +- .flags = (_flags),\ +- .num_parents = (_nb_parents),\ +- .parents = _parents ,\ +-} ++ struct clk _name = {\ ++ .ops = &clk_stm32_mux_ops,\ ++ .priv = &(struct clk_stm32_mux_cfg) {\ ++ .mux_id = (_mux_id),\ ++ },\ ++ .name = #_name,\ ++ .flags = (_flags),\ ++ .num_parents = (_nb_parents),\ ++ .parents = _parents,\ ++ } + + #define STM32_GATE_READY(_name, _parent, _flags, _gate_id)\ +-struct clk _name = {\ +- .ops = &clk_stm32_gate_ready_ops,\ +- .priv = &(struct clk_stm32_gate_cfg) {\ +- .gate_id = _gate_id,\ +- },\ +- .name = #_name,\ +- .flags = (_flags),\ +- .num_parents = 1,\ +- .parents = { _parent },\ +-} ++ struct clk _name = {\ ++ .ops = &clk_stm32_gate_ready_ops,\ ++ .priv = &(struct clk_stm32_gate_cfg) {\ ++ .gate_id = _gate_id,\ ++ },\ ++ .name = #_name,\ ++ .flags = (_flags),\ ++ .num_parents = 1,\ ++ .parents = { _parent },\ ++ } + + #define STM32_COMPOSITE(_name, _nb_parents, _parents, _flags,\ + _gate_id, _div_id, _mux_id)\ +- struct clk _name = {\ +- .ops = &clk_stm32_composite_ops,\ +- .priv = &(struct clk_stm32_composite_cfg) {\ +- .gate_id = (_gate_id),\ +- .div_id = (_div_id),\ +- .mux_id = (_mux_id),\ +- },\ +- .name = #_name,\ +- .flags = (_flags),\ +- .num_parents = (_nb_parents),\ +- .parents = _parents ,\ +-} ++ struct clk _name = {\ ++ .ops = &clk_stm32_composite_ops,\ ++ .priv = &(struct clk_stm32_composite_cfg) {\ ++ .gate_id = (_gate_id),\ ++ .div_id = (_div_id),\ ++ .mux_id = (_mux_id),\ ++ },\ ++ .name = #_name,\ ++ .flags = (_flags),\ ++ .num_parents = (_nb_parents),\ ++ .parents = _parents,\ ++ } + + struct clk_stm32_priv *clk_stm32_get_priv(void); + uintptr_t clk_stm32_get_rcc_base(void); + +-int clk_stm32_init(struct clk_stm32_priv *priv, uintptr_t base); ++TEE_Result clk_stm32_init(struct clk_stm32_priv *priv, uintptr_t base); + + void stm32mp_clk_provider_probe_final(const void *fdt, int node, + struct clk_stm32_priv *priv); +diff --git a/core/drivers/clk/clk-stm32mp13.c b/core/drivers/clk/clk-stm32mp13.c +index 9fcee96fd0..e786017a81 100644 +--- a/core/drivers/clk/clk-stm32mp13.c ++++ b/core/drivers/clk/clk-stm32mp13.c +@@ -1,6 +1,6 @@ + // SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause) + /* +- * Copyright (C) 2018-2021, STMicroelectronics - All Rights Reserved ++ * Copyright (C) STMicroelectronics 2022 - All Rights Reserved + */ + + #include +@@ -27,6 +27,10 @@ + #define TIMEOUT_US_200MS U(200000) + #define HSIDIV_TIMEOUT TIMEOUT_US_200MS + ++#define MAX_OPP CFG_STM32MP_OPP_COUNT ++ ++#define RCC_PLL_NAME_SIZE 12 ++ + struct stm32_osci_dt_cfg { + unsigned long freq; + bool bypass; +@@ -80,8 +84,6 @@ struct stm32_clk_opp_cfg { + struct stm32_pll_dt_cfg pll_cfg; + }; + +-#define MAX_OPP 5 +- + struct stm32_clk_opp_dt_cfg { + struct stm32_clk_opp_cfg mpu_opp[MAX_OPP]; + struct stm32_clk_opp_cfg axi_opp[MAX_OPP]; +@@ -106,7 +108,7 @@ struct stm32_clk_platdata { + * GATE CONFIG + */ + +-/* WARNING GATE_XXX_RDY MUST FOLOW GATE_XXX */ ++/* Warning GATE_XXX_RDY must follow GATE_XXX */ + enum enum_gate_cfg { + GATE_LSE, + GATE_LSE_RDY, +@@ -451,7 +453,7 @@ static const struct gate_cfg gates_mp13[GATE_NB] = { + .offset = (_offset),\ + .shift = (_shift),\ + .width = (_witdh),\ +- .ready = _rdy,\ ++ .ready = (_rdy),\ + } + + #define MUX_CFG(_id, _offset, _shift, _witdh)\ +@@ -621,23 +623,23 @@ struct clk_oscillator_data { + struct clk_stm32_drive *drive; + }; + +-#define BYPASS(_offset, _bit_byp, _bit_digbyp) &(struct clk_stm32_bypass){\ ++#define BYPASS(_offset, _bit_byp, _bit_digbyp) (&(struct clk_stm32_bypass){\ + .offset = (_offset),\ + .bit_byp = (_bit_byp),\ + .bit_digbyp = (_bit_digbyp),\ +-} ++}) + +-#define CSS(_offset, _bit_css) &(struct clk_stm32_css){\ ++#define CSS(_offset, _bit_css) (&(struct clk_stm32_css){\ + .offset = (_offset),\ + .bit_css = (_bit_css),\ +-} ++}) + +-#define DRIVE(_offset, _shift, _width, _default) &(struct clk_stm32_drive){\ ++#define DRIVE(_offset, _shift, _width, _default) (&(struct clk_stm32_drive){\ + .offset = (_offset),\ + .drv_shift = (_shift),\ + .drv_width = (_width),\ + .drv_default = (_default),\ +-} ++}) + + #define OSCILLATOR(idx_osc, _name, _gate_id, _bypass, _css, _drive) \ + [(idx_osc)] = (struct clk_oscillator_data){\ +@@ -669,8 +671,10 @@ static struct clk_oscillator_data stm32mp13_osc_data[NB_OSCILLATOR] = { + NULL), + }; + +-static inline struct clk_oscillator_data *clk_oscillator_get_data(int osc_id) ++static struct clk_oscillator_data *clk_oscillator_get_data(int osc_id) + { ++ assert(osc_id >= 0 && osc_id < (int)ARRAY_SIZE(stm32mp13_osc_data)); ++ + return &stm32mp13_osc_data[osc_id]; + } + +@@ -688,9 +692,9 @@ static void clk_oscillator_set_bypass(struct clk_stm32_priv *priv, + bool digbyp, bool bypass) + { + struct clk_stm32_bypass *bypass_data = osc_data->bypass; +- uintptr_t address; ++ uintptr_t address = 0; + +- if (bypass_data == NULL) ++ if (!bypass_data) + return; + + address = priv->base + bypass_data->offset; +@@ -707,9 +711,9 @@ static void clk_oscillator_set_css(struct clk_stm32_priv *priv, + bool css) + { + struct clk_stm32_css *css_data = osc_data->css; +- uintptr_t address; ++ uintptr_t address = 0; + +- if (css_data == NULL) ++ if (!css_data) + return; + + address = priv->base + css_data->offset; +@@ -723,11 +727,11 @@ static void clk_oscillator_set_drive(struct clk_stm32_priv *priv, + uint8_t lsedrv) + { + struct clk_stm32_drive *drive_data = osc_data->drive; +- uintptr_t address; +- uint32_t mask; +- uint32_t value; ++ uintptr_t address = 0; ++ uint32_t mask = 0; ++ uint32_t value = 0; + +- if (drive_data == NULL) ++ if (!drive_data) + return; + + address = priv->base + drive_data->offset; +@@ -762,7 +766,10 @@ static void stm32_enable_oscillator_hse(struct clk_stm32_priv *priv, + clk_oscillator_set_bypass(priv, osc_data, osci->digbyp, osci->bypass); + + /* Enable clock and wait ready bit */ +- clk_stm32_gate_ready_endisable(osc_data->gate_id, true, true); ++ if (stm32_gate_rdy_enable(osc_data->gate_id)) { ++ EMSG("timeout to enable hse clock"); ++ panic(); ++ } + + clk_oscillator_set_css(priv, osc_data, osci->css); + } +@@ -776,7 +783,7 @@ static void stm32_enable_oscillator_lse(struct clk_stm32_priv *priv, + if (osci->freq == 0U) + return; + +- if (clk_stm32_is_enabled_gate(osc_data->gate_id)) ++ if (stm32_gate_is_enabled(osc_data->gate_id)) + return; + + clk_oscillator_set_bypass(priv, osc_data, osci->digbyp, osci->bypass); +@@ -784,11 +791,12 @@ static void stm32_enable_oscillator_lse(struct clk_stm32_priv *priv, + clk_oscillator_set_drive(priv, osc_data, osci->drive); + + /* Enable lse clock, but don't wait ready bit */ +- clk_stm32_gate_ready_endisable(osc_data->gate_id, true, false); ++ stm32_gate_enable(osc_data->gate_id); + } + +-static void stm32_enable_oscillator_lsi(__maybe_unused struct clk_stm32_priv *priv, +- struct stm32_clk_platdata *pdata) ++static void ++stm32_enable_oscillator_lsi(struct clk_stm32_priv *priv __maybe_unused, ++ struct stm32_clk_platdata *pdata) + { + struct clk_oscillator_data *osc_data = clk_oscillator_get_data(OSC_LSI); + struct stm32_osci_dt_cfg *osci = &pdata->osci[OSC_LSI]; +@@ -797,11 +805,15 @@ static void stm32_enable_oscillator_lsi(__maybe_unused struct clk_stm32_priv *pr + return; + + /* Enable clock and wait ready bit */ +- clk_stm32_gate_ready_endisable(osc_data->gate_id, true, true); ++ if (stm32_gate_rdy_enable(osc_data->gate_id)) { ++ EMSG("timeout to enable lsi clock"); ++ panic(); ++ } + } + +-static void stm32_enable_oscillator_csi(__maybe_unused struct clk_stm32_priv *priv, +- struct stm32_clk_platdata *pdata) ++static void ++stm32_enable_oscillator_csi(struct clk_stm32_priv *priv __maybe_unused, ++ struct stm32_clk_platdata *pdata) + { + struct clk_oscillator_data *osc_data = clk_oscillator_get_data(OSC_CSI); + struct stm32_osci_dt_cfg *osci = &pdata->osci[OSC_CSI]; +@@ -810,7 +822,10 @@ static void stm32_enable_oscillator_csi(__maybe_unused struct clk_stm32_priv *pr + return; + + /* Enable clock and wait ready bit */ +- clk_stm32_gate_ready_endisable(osc_data->gate_id, true, true); ++ if (stm32_gate_rdy_enable(osc_data->gate_id)) { ++ EMSG("timeout to enable csi clock"); ++ panic(); ++ } + } + + static int stm32_clk_oscillators_lse_set_css(struct clk_stm32_priv *priv, +@@ -825,17 +840,20 @@ static int stm32_clk_oscillators_lse_set_css(struct clk_stm32_priv *priv, + return 0; + } + +-static int stm32_clk_oscillators_wait_lse_ready(__maybe_unused struct clk_stm32_priv *priv, +- struct stm32_clk_platdata *pdata) ++static int ++stm32_clk_oscillators_wait_lse_ready(struct clk_stm32_priv *priv __maybe_unused, ++ struct stm32_clk_platdata *pdata) + { + struct clk_oscillator_data *osc_data = clk_oscillator_get_data(OSC_LSE); + struct stm32_osci_dt_cfg *osci = &pdata->osci[OSC_LSE]; +- int ret = 0; + +- if (osci->freq != 0U) +- ret = clk_stm32_wait_ready_gate(osc_data->gate_id, true); ++ if (osci->freq == 0U) ++ return 0; ++ ++ if (stm32_gate_wait_ready(osc_data->gate_id, true)) ++ return -1; + +- return ret; ++ return 0; + } + + static void stm32_clk_oscillators_enable(struct clk_stm32_priv *priv, +@@ -893,7 +911,7 @@ static const struct stm32mp1_pll stm32mp1_pll[PLL_TYPE_NB] = { + [PLL_2000] = { + .refclk_min = 8, + .refclk_max = 16, +- }, ++ } + }; + + #define CLK_PLL_CFG(_idx, _type, _gate_id, _mux_id, _reg)\ +@@ -923,76 +941,88 @@ static unsigned int stm32_clk_configure_clk_get_binding_id(uint32_t data) + return (data & CLK_ID_MASK) >> CLK_ID_SHIFT; + } + +-static int stm32_clk_configure_clk(__maybe_unused struct clk_stm32_priv *priv, ++static int stm32_clk_configure_clk(struct clk_stm32_priv *priv __maybe_unused, + uint32_t data) + { + int sel = (data & CLK_SEL_MASK) >> CLK_SEL_SHIFT; + int enable = (data & CLK_ON_MASK) >> CLK_ON_SHIFT; +- struct clk *clk = NULL; + int clk_id = 0; + int ret = 0; ++ int mux = -1; ++ int gate = -1; + + clk_id = stm32_clk_configure_clk_get_binding_id(data); +- if (clk_id < 0) +- return clk_id; + +- clk = stm32mp_rcc_clock_id_to_clk(clk_id); +- if (clk == NULL) +- panic(); ++ switch (clk_id) { ++ case CK_MCO1: ++ mux = MUX_MCO1; ++ gate = GATE_MCO1; ++ break; ++ ++ case CK_MCO2: ++ mux = MUX_MCO2; ++ gate = GATE_MCO2; ++ break; ++ default: ++ ret = -1; ++ break; ++ } + +- ret = clk_stm32_set_parent_by_index(clk, sel); + if (ret != 0) + return ret; + +- if (clk->ops) { +- if (enable) +- clk->ops->enable(clk); +- else +- clk->ops->disable(clk); +- } ++ if (stm32_mux_set_parent(mux, sel)) ++ return -1; ++ ++ if (enable) ++ stm32_gate_enable(gate); ++ else ++ stm32_gate_disable(gate); + + return 0; + } + +-static int stm32_clk_configure_mux(__maybe_unused struct clk_stm32_priv *priv, uint32_t data) ++static int stm32_clk_configure_mux(__unused struct clk_stm32_priv *priv, ++ uint32_t data) + { + int mux = (data & MUX_ID_MASK) >> MUX_ID_SHIFT; + int sel = (data & MUX_SEL_MASK) >> MUX_SEL_SHIFT; + + if (mux == MUX_RTC) { ++ /* Mux RTC clock only is selector is valid and RTC not yet ++ * enabled ++ */ + if (sel == 0) + return 0; + +- if (clk_stm32_is_enabled_gate(GATE_RTCCK)) ++ if (stm32_gate_is_enabled(GATE_RTCCK)) + return 0; +- +- return clk_stm32_set_parent_mux(mux, sel); + } + +- return clk_stm32_set_parent_mux(mux, sel); ++ if (stm32_mux_set_parent(mux, sel)) ++ return -1; ++ ++ return 0; + } + +-static int stm32_clk_configure_div(__maybe_unused struct clk_stm32_priv *priv, +- uint32_t data) ++static TEE_Result ++stm32_clk_configure_div(struct clk_stm32_priv *priv __maybe_unused, ++ uint32_t data) + { +- int div_id, div_n; ++ int div_id = (data & DIV_ID_MASK) >> DIV_ID_SHIFT; ++ int div_n = (data & DIV_DIVN_MASK) >> DIV_DIVN_SHIFT; + +- div_id = (data & DIV_ID_MASK) >> DIV_ID_SHIFT; +- div_n = (data & DIV_DIVN_MASK) >> DIV_DIVN_SHIFT; +- +- return clk_stm32_set_div_value(div_id, div_n); ++ return stm32_div_set_value(div_id, div_n); + } + + static int stm32_clk_dividers_configure(struct clk_stm32_priv *priv) + { + struct stm32_clk_platdata *pdata = priv->pdata; +- uint32_t i = 0; +- int ret = 0; ++ unsigned int i = 0; + + for (i = 0; i < pdata->nclkdiv; i++) { +- ret = stm32_clk_configure_div(priv, pdata->clkdiv[i]); +- if (ret != 0) +- return ret; ++ if (stm32_clk_configure_div(priv, pdata->clkdiv[i])) ++ return -1; + } + + return 0; +@@ -1031,9 +1061,8 @@ static int stm32_clk_source_configure(struct clk_stm32_priv *priv) + break; + } + +- if (ret != 0) { ++ if (ret != 0) + return ret; +- } + } + + /* +@@ -1043,10 +1072,10 @@ static int stm32_clk_source_configure(struct clk_stm32_priv *priv) + * => deactivate CKPER only after switching clock + */ + if (ckper_disabled) { +- ret = stm32_clk_configure_mux(priv, CLK_CKPER_DISABLED & CMD_MASK); +- if (ret != 0) { ++ ret = stm32_clk_configure_mux(priv, ++ CLK_CKPER_DISABLED & CMD_MASK); ++ if (ret != 0) + return ret; +- } + } + + return 0; +@@ -1054,7 +1083,9 @@ static int stm32_clk_source_configure(struct clk_stm32_priv *priv) + + static unsigned long clk_stm32_pll_get_oscillator_rate(int sel) + { +- int osc[] = { OSC_HSI, OSC_HSE, OSC_CSI }; ++ const int osc[] = { OSC_HSI, OSC_HSE, OSC_CSI }; ++ ++ assert(sel >= 0 && sel < (int)ARRAY_SIZE(osc)); + + return clk_stm32_get_rate_oscillateur(osc[sel]); + } +@@ -1076,7 +1107,7 @@ static int clk_stm32_pll_compute_cfgr1(const struct stm32_clk_pll *pll, + + *value = 0; + +- if ((pll->plltype == PLL_800) && (refclk >= 8000000U)) ++ if (pll->plltype == PLL_800 && refclk >= 8000000U) + *value = 1U << RCC_PLLNCFGR1_IFRGE_SHIFT; + + *value |= (divn << RCC_PLLNCFGR1_DIVN_SHIFT) & RCC_PLLNCFGR1_DIVN_MASK; +@@ -1089,9 +1120,12 @@ static uint32_t clk_stm32_pll_compute_cfgr2(struct stm32_pll_output *out) + { + uint32_t value = 0; + +- value |= (out->output[PLL_CFG_P] << RCC_PLLNCFGR2_DIVP_SHIFT) & RCC_PLLNCFGR2_DIVP_MASK; +- value |= (out->output[PLL_CFG_Q] << RCC_PLLNCFGR2_DIVQ_SHIFT) & RCC_PLLNCFGR2_DIVQ_MASK; +- value |= (out->output[PLL_CFG_R] << RCC_PLLNCFGR2_DIVR_SHIFT) & RCC_PLLNCFGR2_DIVR_MASK; ++ value |= (out->output[PLL_CFG_P] << RCC_PLLNCFGR2_DIVP_SHIFT) & ++ RCC_PLLNCFGR2_DIVP_MASK; ++ value |= (out->output[PLL_CFG_Q] << RCC_PLLNCFGR2_DIVQ_SHIFT) & ++ RCC_PLLNCFGR2_DIVQ_MASK; ++ value |= (out->output[PLL_CFG_R] << RCC_PLLNCFGR2_DIVR_SHIFT) & ++ RCC_PLLNCFGR2_DIVR_MASK; + + return value; + } +@@ -1103,7 +1137,6 @@ static uint32_t clk_stm32_pll_compute_cfgr2(struct stm32_pll_output *out) + * (+1) => same parameters, no need to reconfigure. + * Return value is 0 if no error. + */ +- + static int clk_stm32_is_pll_config_on_the_fly(struct clk_stm32_priv *priv, + const struct stm32_clk_pll *pll, + struct stm32_pll_dt_cfg *pll_conf, +@@ -1115,15 +1148,14 @@ static int clk_stm32_is_pll_config_on_the_fly(struct clk_stm32_priv *priv, + uint32_t fracr = 0; + uint32_t value = 0; + int ret = 0; +- int sel = 0; ++ size_t sel = 0; + + ret = clk_stm32_pll_compute_cfgr1(pll, vco, &value); +- if (ret != 0) { ++ if (ret != 0) + return ret; +- } + + sel = (vco->src & MUX_SEL_MASK) >> MUX_SEL_SHIFT; +- if (sel != clk_stm32_get_parent_mux(pll->mux_id)) { ++ if (sel != stm32_mux_get_parent(pll->mux_id)) { + /* Clock source of the PLL is different */ + *result = -1; + return 0; +@@ -1142,11 +1174,12 @@ static int clk_stm32_is_pll_config_on_the_fly(struct clk_stm32_priv *priv, + value = clk_stm32_pll_compute_cfgr2(out); + + if ((io_read32(pll_base + RCC_OFFSET_PLLXFRACR) == fracr) && +- (io_read32(pll_base + RCC_OFFSET_PLLXCFGR2) == value)) ++ (io_read32(pll_base + RCC_OFFSET_PLLXCFGR2) == value)) { + /* Same parameters, no need to config */ + *result = 1; +- else ++ } else { + *result = 0; ++ } + + return 0; + } +@@ -1156,7 +1189,7 @@ static int stm32_clk_hsidiv_configure(struct clk_stm32_priv *priv) + struct stm32_clk_platdata *pdata = priv->pdata; + struct stm32_osci_dt_cfg *osci = &pdata->osci[OSC_HSI]; + +- return clk_stm32_set_rate_divider(DIV_HSI, osci->freq, MAX_HSI_HZ); ++ return stm32_div_set_rate(DIV_HSI, osci->freq, MAX_HSI_HZ); + } + + static void clk_stm32_pll_config_vco(struct clk_stm32_priv *priv, +@@ -1167,7 +1200,7 @@ static void clk_stm32_pll_config_vco(struct clk_stm32_priv *priv, + uint32_t value = 0; + + if (clk_stm32_pll_compute_cfgr1(pll, vco, &value) != 0) { +- EMSG("Invalid Vref clock !\n"); ++ EMSG("Invalid Vref clock"); + panic(); + } + +@@ -1201,12 +1234,15 @@ static void clk_stm32_pll_config_csg(struct clk_stm32_priv *priv, + inc_step = vco->csg[PLL_CSG_INC_STEP]; + sscg_mode = vco->csg[PLL_CSG_SSCG_MODE]; + +- value |= (mod_per << RCC_PLLNCSGR_MOD_PER_SHIFT) & RCC_PLLNCSGR_MOD_PER_MASK; +- value |= (inc_step << RCC_PLLNCSGR_INC_STEP_SHIFT) & RCC_PLLNCSGR_INC_STEP_MASK; +- value |= (sscg_mode << RCC_PLLNCSGR_SSCG_MODE_SHIFT) & RCC_PLLNCSGR_SSCG_MODE_MASK; ++ value |= (mod_per << RCC_PLLNCSGR_MOD_PER_SHIFT) & ++ RCC_PLLNCSGR_MOD_PER_MASK; ++ value |= (inc_step << RCC_PLLNCSGR_INC_STEP_SHIFT) & ++ RCC_PLLNCSGR_INC_STEP_MASK; ++ value |= (sscg_mode << RCC_PLLNCSGR_SSCG_MODE_SHIFT) & ++ RCC_PLLNCSGR_SSCG_MODE_MASK; + + io_write32(pll_base + RCC_OFFSET_PLLXCSGR, value); +- io_setbits32(pll_base+ RCC_OFFSET_PLLXCR, RCC_PLLNCR_SSCG_CTRL); ++ io_setbits32(pll_base + RCC_OFFSET_PLLXCR, RCC_PLLNCR_SSCG_CTRL); + } + + static void clk_stm32_pll_config_out(struct clk_stm32_priv *priv, +@@ -1221,12 +1257,12 @@ static void clk_stm32_pll_config_out(struct clk_stm32_priv *priv, + io_write32(pll_base + RCC_OFFSET_PLLXCFGR2, value); + } + +-static inline struct stm32_pll_dt_cfg *clk_stm32_pll_get_pdata(int pll_idx) ++static struct stm32_pll_dt_cfg *clk_stm32_pll_get_pdata(int pll_idx) + { + struct clk_stm32_priv *priv = clk_stm32_get_priv(); + struct stm32_clk_platdata *pdata = priv->pdata; + +- return &pdata->pll[pll_idx]; ++ return &pdata->pll[pll_idx]; + } + + static int clk_stm32_pll_init_switch_to_hsi_clk_system(int mux_sys) +@@ -1237,31 +1273,24 @@ static int clk_stm32_pll_init_switch_to_hsi_clk_system(int mux_sys) + return -1; + + /* Make a backup to the current parent */ +- sel = clk_stm32_get_parent_mux(mux_sys); ++ sel = stm32_mux_get_parent(mux_sys); + + /* Switch to HSI */ +- clk_stm32_set_parent_mux(mux_sys, 0); ++ if (stm32_mux_set_parent(mux_sys, 0)) ++ return -1; + + return sel; + } + +-static void clk_stm32_pll_init_restore_clk_system(int mux_sys, int sel) +-{ +- if (mux_sys != -1) +- /* Restore the parent */ +- clk_stm32_set_parent_mux(mux_sys, sel); +-} +- +-static uint32_t clk_stm32_pll_backup_output_diven(const struct stm32_clk_pll *pll) ++static uint32_t ++clk_stm32_pll_backup_output_diven(const struct stm32_clk_pll *pll) + { + struct clk_stm32_priv *priv = clk_stm32_get_priv(); + uintptr_t addr = priv->base + pll->reg_pllxcr; +- uint32_t value = 0; +- +- value = io_read32(addr + RCC_OFFSET_PLLXCR); +- value &= (RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN | RCC_PLLNCR_DIVREN); + +- return value; ++ return io_read32(addr + RCC_OFFSET_PLLXCR) & ++ (RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN | ++ RCC_PLLNCR_DIVREN); + } + + static void clk_stm32_pll_restore_output_diven(const struct stm32_clk_pll *pll, +@@ -1286,7 +1315,8 @@ static int clk_stm32_pll_init(struct clk_stm32_priv *priv, int pll_idx, + int mux_system[] = { MUX_MPU, MUX_AXI, MUX_MLAHB, -1 }; + int mux_sys = mux_system[pll_idx]; + +- ret = clk_stm32_is_pll_config_on_the_fly(priv, pll, pll_conf, &config_on_the_fly); ++ ret = clk_stm32_is_pll_config_on_the_fly(priv, pll, pll_conf, ++ &config_on_the_fly); + if (ret != 0) + return ret; + +@@ -1298,12 +1328,13 @@ static int clk_stm32_pll_init(struct clk_stm32_priv *priv, int pll_idx, + sel = clk_stm32_pll_init_switch_to_hsi_clk_system(mux_sys); + + /* Stop the PLL before */ +- if (clk_stm32_is_enabled_gate(pll->gate_id)) { ++ if (stm32_gate_is_enabled(pll->gate_id)) { + io_clrbits32(priv->base + pll->reg_pllxcr, + RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN | + RCC_PLLNCR_DIVREN); + +- clk_stm32_gate_ready_endisable(pll->gate_id, false, true); ++ if (stm32_gate_rdy_disable(pll->gate_id)) ++ return -1; + } + + /* Configure PLLs source */ +@@ -1319,17 +1350,18 @@ static int clk_stm32_pll_init(struct clk_stm32_priv *priv, int pll_idx, + clk_stm32_pll_config_csg(priv, pll, &pll_conf->vco); + } + +- if (!clk_stm32_is_enabled_gate(pll->gate_id)) { +- ret =clk_stm32_gate_ready_endisable(pll->gate_id, true, true); +- if (ret != 0) +- return ret; ++ if (!stm32_gate_is_enabled(pll->gate_id)) { ++ if (stm32_gate_rdy_enable(pll->gate_id)) ++ return -1; + + clk_stm32_pll_restore_output_diven(pll, save_div_pqr_en); + } + +- if (config_on_the_fly == -1) ++ if ((config_on_the_fly == -1) && (mux_sys != -1)) { + /* Restore to backup parent */ +- clk_stm32_pll_init_restore_clk_system(mux_sys, sel); ++ if (stm32_mux_set_parent(mux_sys, sel)) ++ return -1; ++ } + + return 0; + } +@@ -1338,7 +1370,7 @@ static int stm32_clk_pll_configure(struct clk_stm32_priv *priv) + { + struct stm32_pll_dt_cfg *pll_conf = NULL; + size_t i = 0; +- int plls[] = { PLL1_ID, PLL3_ID, PLL4_ID }; ++ const int plls[] = { PLL1_ID, PLL3_ID, PLL4_ID }; + + for (i = 0; i < ARRAY_SIZE(plls); i++) { + pll_conf = clk_stm32_pll_get_pdata(plls[i]); +@@ -1388,24 +1420,26 @@ static int stm32mp1_init_clock_tree(struct clk_stm32_priv *priv, + if (ret != 0) + panic(); + +- /* Configure LSE css after RTC source configuration */ ++ /* Configure LSE CSS after RTC source configuration */ + ret = stm32_clk_oscillators_lse_set_css(priv, pdata); + if (ret != 0) + panic(); + + /* Software Self-Refresh mode (SSR) during DDR initilialization */ + io_clrsetbits32(priv->base + RCC_DDRITFCR, RCC_DDRITFCR_DDRCKMOD_MASK, +- RCC_DDRITFCR_DDRCKMOD_SSR << RCC_DDRITFCR_DDRCKMOD_SHIFT); ++ RCC_DDRITFCR_DDRCKMOD_SSR << ++ RCC_DDRITFCR_DDRCKMOD_SHIFT); + + return 0; + } + + #ifdef CFG_STM32_CLK_DEBUG +-static void clk_stm32_debug_display_pll_cfg(int pll_id, struct stm32_pll_dt_cfg *pll) ++static void clk_stm32_debug_display_pll_cfg(int pll_id, ++ struct stm32_pll_dt_cfg *pll) + { + struct stm32_pll_vco *vco = &pll->vco; + struct stm32_pll_output *out = &pll->output; +- size_t j = 0; ++ unsigned int j = 0; + + printf("PLL%d : %s", pll_id + 1, vco->status ? "" : "disabled"); + +@@ -1416,9 +1450,9 @@ static void clk_stm32_debug_display_pll_cfg(int pll_id, struct stm32_pll_dt_cfg + + printf(" vco = < "); + +- for (j = 0; j < PLL_DIV_MN_NB; j++) { ++ for (j = 0; j < PLL_DIV_MN_NB; j++) + printf("%d ", vco->div_mn[j]); +- } ++ + printf("> "); + + printf("frac = 0x%x ", vco->frac); +@@ -1428,34 +1462,33 @@ static void clk_stm32_debug_display_pll_cfg(int pll_id, struct stm32_pll_dt_cfg + if (vco->csg_enabled) { + printf("csg = < "); + +- for (j = 0; j < PLL_CSG_NB; j++) { ++ for (j = 0; j < PLL_CSG_NB; j++) + printf("%d ", vco->csg[j]); +- } + + printf("> "); + } + + printf("output = < "); + +- for (j = 0; j < PLL_DIV_PQR_NB; j++) { ++ for (j = 0; j < PLL_DIV_PQR_NB; j++) + printf("%d ", out->output[j]); +- } ++ + printf(">\n"); + } + + static void clk_stm32_debug_display_opp_cfg(const char *opp_name, + struct stm32_clk_opp_cfg *opp_cfg) + { +- size_t i = 0; ++ unsigned int i = 0; + + printf("\nOPP %s :\n", opp_name); + + for (i = 0; i < MAX_OPP; i++) { +- if(opp_cfg->frq == 0UL) ++ if (opp_cfg->frq == 0UL) + break; + +- printf("frequence = %d src = 0x%x div = 0x%x ", opp_cfg->frq, +- opp_cfg->src, opp_cfg->div); ++ printf("frequency = %d src = 0x%x div = 0x%x ", opp_cfg->frq, ++ opp_cfg->src, opp_cfg->div); + + clk_stm32_debug_display_pll_cfg(PLL1_ID, &opp_cfg->pll_cfg); + +@@ -1494,7 +1527,9 @@ static void clk_stm32_debug_display_osc_dt_cfg(struct clk_stm32_priv *priv) + + for (i = 0; i < nb; i++) { + struct stm32_osci_dt_cfg *osc = &pdata->osci[i]; +- struct clk_oscillator_data *osc_data = clk_oscillator_get_data(i); ++ struct clk_oscillator_data *osc_data = NULL; ++ ++ osc_data = clk_oscillator_get_data(i); + + printf("%s %ld bypass = %d digbyp = %d css = %d drive = %d\n", + osc_data->name, +@@ -1556,16 +1591,13 @@ static int clk_stm32_parse_oscillator_fdt(const void *fdt, int node, + { + int subnode = 0; + +- /* default value oscillator not found, freq=0 */ +- osci->freq = 0; +- + fdt_for_each_subnode(subnode, fdt, node) { + const char *cchar = NULL; + const fdt32_t *cuint = NULL; + int ret = 0; + + cchar = fdt_get_name(fdt, subnode, &ret); +- if (cchar == NULL) ++ if (!cchar) + return ret; + + if (strncmp(cchar, name, (size_t)ret) || +@@ -1573,18 +1605,18 @@ static int clk_stm32_parse_oscillator_fdt(const void *fdt, int node, + continue; + + cuint = fdt_getprop(fdt, subnode, "clock-frequency", &ret); +- if (cuint == NULL) +- return ret; ++ if (!cuint) ++ panic(); + + osci->freq = fdt32_to_cpu(*cuint); + +- if (fdt_getprop(fdt, subnode, "st,bypass", NULL) != NULL) ++ if (fdt_getprop(fdt, subnode, "st,bypass", NULL)) + osci->bypass = true; + +- if (fdt_getprop(fdt, subnode, "st,digbypass", NULL) != NULL) ++ if (fdt_getprop(fdt, subnode, "st,digbypass", NULL)) + osci->digbyp = true; + +- if (fdt_getprop(fdt, subnode, "st,css", NULL) != NULL) ++ if (fdt_getprop(fdt, subnode, "st,css", NULL)) + osci->css = true; + + osci->drive = _fdt_read_uint32_default(fdt, subnode, "st,drive", +@@ -1593,11 +1625,11 @@ static int clk_stm32_parse_oscillator_fdt(const void *fdt, int node, + return 0; + } + +- return 0; ++ return -FDT_ERR_NOTFOUND; + } + + static int stm32_clk_parse_fdt_all_oscillator(const void *fdt, +- __maybe_unused int node, ++ int node __maybe_unused, + struct stm32_clk_platdata *pdata) + { + int fdt_err = 0; +@@ -1609,40 +1641,40 @@ static int stm32_clk_parse_fdt_all_oscillator(const void *fdt, + return -FDT_ERR_NOTFOUND; + + for (i = 0; i < NB_OSCILLATOR; i++) { +- struct clk_oscillator_data *osc_data = clk_oscillator_get_data(i); + struct stm32_osci_dt_cfg *osci = &pdata->osci[i]; +- const char *name = osc_data->name; ++ struct clk_oscillator_data *osc_data = NULL; ++ ++ osc_data = clk_oscillator_get_data(i); + +- fdt_err = clk_stm32_parse_oscillator_fdt(fdt, osc_node, name, osci); ++ fdt_err = clk_stm32_parse_oscillator_fdt(fdt, osc_node, ++ osc_data->name, osci); + if (fdt_err < 0) +- panic(); ++ osci->freq = 0UL; + } + + return 0; + } + +-#define RCC_PLL_NAME_SIZE 12 +- +-static int clk_stm32_load_vco_config(const void *fdt, int subnode, +- struct stm32_pll_vco *vco) ++static int clk_stm32_load_vco_config_fdt(const void *fdt, int subnode, ++ struct stm32_pll_vco *vco) + { +- int err = 0; ++ int ret = 0; + +- err = _fdt_read_uint32_array(fdt, subnode, "divmn", vco->div_mn, +- (int)PLL_DIV_MN_NB); +- if (err != 0) +- return err; ++ ret = _fdt_read_uint32_array(fdt, subnode, "divmn", vco->div_mn, ++ PLL_DIV_MN_NB); ++ if (ret != 0) ++ return ret; + +- err = _fdt_read_uint32_array(fdt, subnode, "csg", vco->csg, +- (int)PLL_CSG_NB); ++ ret = _fdt_read_uint32_array(fdt, subnode, "csg", vco->csg, ++ PLL_CSG_NB); + +- vco->csg_enabled = (err == 0); ++ vco->csg_enabled = (ret == 0); + +- if (err == -FDT_ERR_NOTFOUND) +- err = 0; ++ if (ret == -FDT_ERR_NOTFOUND) ++ ret = 0; + +- if (err != 0) +- return err; ++ if (ret != 0) ++ return ret; + + vco->status = RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN | + RCC_PLLNCR_DIVREN | RCC_PLLNCR_PLLON; +@@ -1654,8 +1686,8 @@ static int clk_stm32_load_vco_config(const void *fdt, int subnode, + return 0; + } + +-static int clk_stm32_load_output_config(const void *fdt, int subnode, +- struct stm32_pll_output *output) ++static int clk_stm32_load_output_config_fdt(const void *fdt, int subnode, ++ struct stm32_pll_output *output) + { + return _fdt_read_uint32_array(fdt, subnode, "st,pll_div_pqr", + output->output, (int)PLL_DIV_PQR_NB); +@@ -1685,11 +1717,11 @@ static int clk_stm32_parse_pll_fdt(const void *fdt, int subnode, + if (subnode_vco < 0) + return -FDT_ERR_NOTFOUND; + +- err = clk_stm32_load_vco_config(fdt, subnode_vco, &pll->vco); ++ err = clk_stm32_load_vco_config_fdt(fdt, subnode_vco, &pll->vco); + if (err != 0) + return err; + +- err = clk_stm32_load_output_config(fdt, subnode_pll, &pll->output); ++ err = clk_stm32_load_output_config_fdt(fdt, subnode_pll, &pll->output); + if (err != 0) + return err; + +@@ -1699,19 +1731,18 @@ static int clk_stm32_parse_pll_fdt(const void *fdt, int subnode, + static int stm32_clk_parse_fdt_all_pll(const void *fdt, int node, + struct stm32_clk_platdata *pdata) + { +- + size_t i = 0; + + for (i = PLL1_ID; i < pdata->npll; i++) { + struct stm32_pll_dt_cfg *pll = pdata->pll + i; +- char name[RCC_PLL_NAME_SIZE]; ++ char name[RCC_PLL_NAME_SIZE] = { 0 }; + int subnode = 0; + int err = 0; + + snprintf(name, sizeof(name), "st,pll@%d", i); + + subnode = fdt_subnode_offset(fdt, node, name); +- if (!_fdt_check_node(fdt, subnode)) ++ if (subnode < 0) + continue; + + err = clk_stm32_parse_pll_fdt(fdt, subnode, pll); +@@ -1728,22 +1759,35 @@ static int stm32_clk_parse_fdt_opp(const void *fdt, int node, + { + int subnode = 0; + int nb_opp = 0; ++ int ret = 0; + + node = fdt_subnode_offset(fdt, node, opp_name); +- if (!_fdt_check_node(fdt, node)) ++ if (node == -FDT_ERR_NOTFOUND) + return 0; ++ if (node < 0) ++ return node; + + fdt_for_each_subnode(subnode, fdt, node) { + if (nb_opp >= MAX_OPP) { +- EMSG("%d MAX opp in %s !", MAX_OPP, opp_name); +- break; ++ EMSG("%d MAX opp in %s", MAX_OPP, opp_name); ++ panic(); + } + +- opp_cfg->frq = _fdt_read_uint32_default(fdt, subnode,"hz", UINT32_MAX); +- opp_cfg->src = _fdt_read_uint32_default(fdt, subnode, "st,clksrc", UINT32_MAX); +- opp_cfg->div = _fdt_read_uint32_default(fdt, subnode, "st,clkdiv", UINT32_MAX); ++ opp_cfg->frq = _fdt_read_uint32_default(fdt, subnode, ++ "hz", ++ UINT32_MAX); ++ ++ opp_cfg->src = _fdt_read_uint32_default(fdt, subnode, ++ "st,clksrc", ++ UINT32_MAX); ++ ++ opp_cfg->div = _fdt_read_uint32_default(fdt, subnode, ++ "st,clkdiv", ++ UINT32_MAX); + +- clk_stm32_parse_pll_fdt(fdt, subnode, &opp_cfg->pll_cfg); ++ ret = clk_stm32_parse_pll_fdt(fdt, subnode, &opp_cfg->pll_cfg); ++ if (ret) ++ return ret; + + opp_cfg++; + nb_opp++; +@@ -1756,15 +1800,27 @@ static int stm32_clk_parse_fdt_all_opp(const void *fdt, int node, + struct stm32_clk_platdata *pdata) + { + struct stm32_clk_opp_dt_cfg *opp = pdata->opp; ++ int ret = 0; + + node = fdt_subnode_offset(fdt, node, "st,clk_opp"); +- if (!_fdt_check_node(fdt, node)) +- /* no opp are defined */ ++ /* No opp are defined */ ++ if (node == -FDT_ERR_NOTFOUND) + return 0; ++ if (node < 0) ++ return node; + +- stm32_clk_parse_fdt_opp(fdt, node, "st,ck_mpu", opp->mpu_opp); +- stm32_clk_parse_fdt_opp(fdt, node, "st,ck_axi", opp->axi_opp); +- stm32_clk_parse_fdt_opp(fdt, node, "st,ck_mlahbs", opp->mlahbs_opp); ++ ret = stm32_clk_parse_fdt_opp(fdt, node, "st,ck_mpu", opp->mpu_opp); ++ if (ret) ++ return ret; ++ ++ ret = stm32_clk_parse_fdt_opp(fdt, node, "st,ck_axi", opp->axi_opp); ++ if (ret) ++ return ret; ++ ++ ret = stm32_clk_parse_fdt_opp(fdt, node, "st,ck_mlahbs", ++ opp->mlahbs_opp); ++ if (ret) ++ return ret; + + return 0; + } +@@ -1809,7 +1865,7 @@ static size_t clk_stm32_pll_get_parent(struct clk *clk) + { + struct clk_stm32_pll_cfg *cfg = clk->priv; + +- return clk_stm32_get_parent_mux(cfg->mux_id);; ++ return stm32_mux_get_parent(cfg->mux_id); + } + + static unsigned long clk_stm32_pll_get_rate(struct clk *clk, +@@ -1857,7 +1913,7 @@ static bool clk_stm32_pll_is_enabled(struct clk *clk) + { + struct clk_stm32_pll_cfg *cfg = clk->priv; + +- return clk_stm32_is_enabled_gate(cfg->gate_id); ++ return stm32_gate_is_enabled(cfg->gate_id); + } + + static TEE_Result clk_stm32_pll_enable(struct clk *clk) +@@ -1865,9 +1921,9 @@ static TEE_Result clk_stm32_pll_enable(struct clk *clk) + struct clk_stm32_pll_cfg *cfg = clk->priv; + + if (clk_stm32_pll_is_enabled(clk)) +- return 0; ++ return TEE_SUCCESS; + +- return clk_stm32_gate_ready_endisable(cfg->gate_id, true, true); ++ return stm32_gate_rdy_enable(cfg->gate_id); + } + + static void clk_stm32_pll_disable(struct clk *clk) +@@ -1883,7 +1939,7 @@ static void clk_stm32_pll_disable(struct clk *clk) + io_clrbits32(pll_base, RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN | + RCC_PLLNCR_DIVREN); + +- clk_stm32_gate_ready_endisable(cfg->gate_id, false, true); ++ stm32_gate_rdy_disable(cfg->gate_id); + } + + static const struct clk_ops clk_stm32_pll_ops = { +@@ -1898,7 +1954,7 @@ static TEE_Result clk_stm32_composite_get_duty_cycle(struct clk *clk, + struct clk_duty *duty) + { + struct clk_stm32_composite_cfg *cfg = clk->priv; +- uint32_t val = clk_stm32_div_get_value(cfg->div_id); ++ uint32_t val = stm32_div_get_value(cfg->div_id); + + duty->num = (val + 1) / 2; + duty->den = (val + 1); +@@ -1906,7 +1962,7 @@ static TEE_Result clk_stm32_composite_get_duty_cycle(struct clk *clk, + return 0; + } + +-static unsigned long clk_stm32_composite_round_rate(__maybe_unused struct clk *clk, ++static unsigned long clk_stm32_composite_round_rate(struct clk *clk __unused, + unsigned long rate, + unsigned long prate) + { +@@ -1929,10 +1985,11 @@ static const struct clk_ops clk_stm32_composite_duty_cycle_ops = { + .get_duty_cycle = clk_stm32_composite_get_duty_cycle, + }; + +-static struct stm32_clk_opp_cfg *clk_stm32_get_opp_config(struct stm32_clk_opp_cfg *opp_cfg, +- unsigned long rate) ++static struct ++stm32_clk_opp_cfg *clk_stm32_get_opp_config(struct stm32_clk_opp_cfg *opp_cfg, ++ unsigned long rate) + { +- size_t i = 0; ++ unsigned int i = 0; + + for (i = 0; i < MAX_OPP; i++, opp_cfg++) { + if (opp_cfg->frq == 0UL) +@@ -1945,9 +2002,9 @@ static struct stm32_clk_opp_cfg *clk_stm32_get_opp_config(struct stm32_clk_opp_c + return NULL; + } + +-static TEE_Result clk_stm32_pll1_set_rate(__maybe_unused struct clk *clk, ++static TEE_Result clk_stm32_pll1_set_rate(struct clk *clk __maybe_unused, + unsigned long rate, +- __maybe_unused unsigned long prate) ++ unsigned long prate __maybe_unused) + { + const struct stm32_clk_pll *pll = clk_stm32_pll_data(PLL1_ID); + struct clk_stm32_priv *priv = clk_stm32_get_priv(); +@@ -1956,40 +2013,44 @@ static TEE_Result clk_stm32_pll1_set_rate(__maybe_unused struct clk *clk, + struct stm32_clk_opp_cfg *opp = NULL; + int config_on_the_fly = -1; + int err = 0; +- size_t sel = clk_stm32_get_parent_mux(MUX_MPU); ++ size_t sel = stm32_mux_get_parent(MUX_MPU); + + opp = clk_stm32_get_opp_config(pdata->opp->mpu_opp, rate); +- if (opp == NULL) { +- return 0; +- } ++ if (!opp) ++ return TEE_ERROR_GENERIC; + + pll_conf = &opp->pll_cfg; + + err = clk_stm32_is_pll_config_on_the_fly(priv, pll, pll_conf, + &config_on_the_fly); +- if (err != 0) +- return err; ++ if (err) ++ return TEE_ERROR_GENERIC; + +- if (config_on_the_fly == 1) { +- return 0; +- } ++ if (config_on_the_fly == 1) ++ return TEE_SUCCESS; + + if (config_on_the_fly == -1) { + /* Switch to HSI and stop PLL1 before reconfiguration */ +- clk_stm32_set_parent_mux(MUX_MPU, 0); +- clk_stm32_disable_gate(GATE_PLL1_DIVP); +- clk_stm32_gate_rdy_disable(GATE_PLL1); ++ if (stm32_mux_set_parent(MUX_MPU, 0)) ++ return TEE_ERROR_GENERIC; ++ ++ stm32_gate_disable(GATE_PLL1_DIVP); ++ stm32_gate_rdy_disable(GATE_PLL1); + clk_stm32_pll_config_vco(priv, pll, &pll_conf->vco); + } + + clk_stm32_pll_config_out(priv, pll, &pll_conf->output); +- clk_stm32_gate_rdy_enable(GATE_PLL1); +- clk_stm32_enable_gate(GATE_PLL1_DIVP); ++ if (stm32_gate_rdy_enable(GATE_PLL1)) { ++ EMSG("timeout to enable PLL1 clock"); ++ panic(); ++ } ++ stm32_gate_enable(GATE_PLL1_DIVP); + + /* Restore MPU source */ +- clk_stm32_set_parent_mux(MUX_MPU, sel); ++ if (stm32_mux_set_parent(MUX_MPU, sel)) ++ return TEE_ERROR_GENERIC; + +- return 0; ++ return TEE_SUCCESS; + } + + static const struct clk_ops clk_stm32_pll1_ops = { +@@ -2019,7 +2080,7 @@ static TEE_Result clk_stm32_mpu_determine_rate(struct clk *clk, + int index = 0; + + opp = clk_stm32_get_opp_config(pdata->opp->mpu_opp, rate); +- if (opp == NULL) ++ if (!opp) + return 0; + + index = (opp->src & MUX_SEL_MASK) >> MUX_SEL_SHIFT; +@@ -2049,7 +2110,7 @@ static TEE_Result clk_stm32_axi_determine_rate(struct clk *clk, + int index = 0; + + opp = clk_stm32_get_opp_config(pdata->opp->axi_opp, rate); +- if (opp == NULL) ++ if (!opp) + return 0; + + index = (opp->src & MUX_SEL_MASK) >> MUX_SEL_SHIFT; +@@ -2069,7 +2130,8 @@ static const struct clk_ops clk_stm32_axi_ops = { + .get_rate = clk_stm32_composite_get_rate, + }; + +-static TEE_Result clk_stm32_mlahb_determine_rate(struct clk *clk, struct clk_rate_request *req) ++static TEE_Result clk_stm32_mlahb_determine_rate(struct clk *clk, ++ struct clk_rate_request *req) + { + struct clk_stm32_priv *priv = clk_stm32_get_priv(); + struct stm32_clk_platdata *pdata = priv->pdata; +@@ -2079,7 +2141,7 @@ static TEE_Result clk_stm32_mlahb_determine_rate(struct clk *clk, struct clk_rat + int index = 0; + + opp = clk_stm32_get_opp_config(pdata->opp->mlahbs_opp, rate); +- if (opp == NULL) ++ if (!opp) + return 0; + + index = (opp->src & MUX_SEL_MASK) >> MUX_SEL_SHIFT; +@@ -2124,68 +2186,115 @@ const struct clk_ops ck_timer_ops = { + }; + + #define STM32_TIMER(_name, _parent, _flags, _apbdiv, _timpre)\ +-struct clk _name = {\ +- .ops = &ck_timer_ops,\ +- .priv = &(struct clk_stm32_timer_cfg) {\ +- .apbdiv = (_apbdiv),\ +- .timpre = (_timpre),\ +- },\ +- .name = #_name,\ +- .flags = (0 | (_flags)),\ +- .num_parents = 1,\ +- .parents = { _parent },\ +-} ++ struct clk _name = {\ ++ .ops = &ck_timer_ops,\ ++ .priv = &(struct clk_stm32_timer_cfg) {\ ++ .apbdiv = (_apbdiv),\ ++ .timpre = (_timpre),\ ++ },\ ++ .name = #_name,\ ++ .flags = (_flags),\ ++ .num_parents = 1,\ ++ .parents = { _parent },\ ++ } + + #define STM32_KCLK(_name, _nb_parents, _parents, _flags, _gate_id, _mux_id)\ +-struct clk _name = {\ +- .ops = &clk_stm32_composite_ops,\ +- .priv = &(struct clk_stm32_composite_cfg) {\ +- .gate_id = (_gate_id),\ +- .div_id = (NO_DIV),\ +- .mux_id = (_mux_id),\ +- },\ +- .name = #_name,\ +- .flags = (_flags),\ +- .num_parents = (_nb_parents),\ +- .parents = _parents,\ +-} ++ struct clk _name = {\ ++ .ops = &clk_stm32_composite_ops,\ ++ .priv = &(struct clk_stm32_composite_cfg) {\ ++ .gate_id = (_gate_id),\ ++ .div_id = (NO_DIV),\ ++ .mux_id = (_mux_id),\ ++ },\ ++ .name = #_name,\ ++ .flags = (_flags),\ ++ .num_parents = (_nb_parents),\ ++ .parents = _parents,\ ++ } + + #define STM32_PLL_VCO(_name, _nb_parents, _parents, _flags, _reg,\ + _gate_id, _mux_id)\ +-struct clk _name = {\ +- .ops = &clk_stm32_pll_ops,\ +- .priv = &(struct clk_stm32_pll_cfg) {\ +- .reg_pllxcr = (_reg),\ +- .gate_id = (_gate_id),\ +- .mux_id = (_mux_id),\ +- },\ +- .name = #_name,\ +- .flags = (_flags),\ +- .num_parents = (_nb_parents),\ +- .parents = _parents,\ +-} ++ struct clk _name = {\ ++ .ops = &clk_stm32_pll_ops,\ ++ .priv = &(struct clk_stm32_pll_cfg) {\ ++ .reg_pllxcr = (_reg),\ ++ .gate_id = (_gate_id),\ ++ .mux_id = (_mux_id),\ ++ },\ ++ .name = #_name,\ ++ .flags = (_flags),\ ++ .num_parents = (_nb_parents),\ ++ .parents = _parents,\ ++ } + + #define STM32_PLL_OUPUT(_name, _nb_parents, _parents, _flags,\ + _gate_id, _div_id, _mux_id)\ +-struct clk _name = {\ +- .ops = &clk_stm32_composite_duty_cycle_ops,\ +- .priv = &(struct clk_stm32_composite_cfg) {\ +- .gate_id = (_gate_id),\ +- .div_id = (_div_id),\ +- .mux_id = (_mux_id),\ +- },\ +- .name = #_name,\ +- .flags = (_flags),\ +- .num_parents = (_nb_parents),\ +- .parents = _parents,\ +-} ++ struct clk _name = {\ ++ .ops = &clk_stm32_composite_duty_cycle_ops,\ ++ .priv = &(struct clk_stm32_composite_cfg) {\ ++ .gate_id = (_gate_id),\ ++ .div_id = (_div_id),\ ++ .mux_id = (_mux_id),\ ++ },\ ++ .name = #_name,\ ++ .flags = (_flags),\ ++ .num_parents = (_nb_parents),\ ++ .parents = _parents,\ ++ } + + /* Oscillator clocks */ +-static STM32_GATE_READY(ck_hsi, NULL, 0, GATE_HSI); +-static STM32_GATE_READY(ck_hse, NULL, 0, GATE_HSE); +-static STM32_GATE_READY(ck_csi, NULL, 0, GATE_CSI); +-static STM32_GATE_READY(ck_lsi, NULL, 0, GATE_LSI); +-static STM32_GATE_READY(ck_lse, NULL, 0, GATE_LSE); ++ ++static TEE_Result clk_stm32_oscillator_enable(struct clk *clk) ++{ ++ struct clk_stm32_gate_cfg *cfg = clk->priv; ++ ++ if (clk->rate == 0U) ++ return 0; ++ ++ return stm32_gate_rdy_enable(cfg->gate_id); ++} ++ ++static void clk_stm32_oscillator_disable(struct clk *clk) ++{ ++ struct clk_stm32_gate_cfg *cfg = clk->priv; ++ ++ if (clk->rate == 0U) ++ return; ++ ++ if (stm32_gate_rdy_disable(cfg->gate_id)) ++ panic(); ++} ++ ++static bool clk_stm32_oscillator_is_enabled(struct clk *clk) ++{ ++ struct clk_stm32_gate_cfg *cfg = clk->priv; ++ ++ return stm32_gate_is_enabled(cfg->gate_id); ++} ++ ++static const struct clk_ops clk_stm32_oscillator_ops = { ++ .enable = clk_stm32_oscillator_enable, ++ .disable = clk_stm32_oscillator_disable, ++ .is_enabled = clk_stm32_oscillator_is_enabled, ++}; ++ ++#define STM32_OSCILLATOR(_name, _parent, _flags, _gate_id)\ ++ struct clk _name = {\ ++ .ops = &clk_stm32_oscillator_ops,\ ++ .priv = &(struct clk_stm32_gate_cfg) {\ ++ .gate_id = _gate_id,\ ++ },\ ++ .name = #_name,\ ++ .flags = (_flags),\ ++ .num_parents = 1,\ ++ .parents = { _parent },\ ++ } ++ ++static STM32_OSCILLATOR(ck_hsi, NULL, 0, GATE_HSI); ++static STM32_OSCILLATOR(ck_hse, NULL, 0, GATE_HSE); ++static STM32_OSCILLATOR(ck_csi, NULL, 0, GATE_CSI); ++static STM32_OSCILLATOR(ck_lsi, NULL, 0, GATE_LSI); ++static STM32_OSCILLATOR(ck_lse, NULL, 0, GATE_LSE); + + static STM32_FIXED_FACTOR(ck_i2sckin, NULL, 0, 1, 1); + static STM32_FIXED_FACTOR(ck_hse_div2, &ck_hse, 0, 1, 2); +@@ -2213,9 +2322,9 @@ static struct clk ck_pll1p = { + .gate_id = GATE_PLL1_DIVP, + .div_id = DIV_PLL1DIVP, + .mux_id = NO_MUX, +- },\ ++ }, + .name = "ck_pll1p", +- .flags = CLK_SET_RATE_PARENT,\ ++ .flags = CLK_SET_RATE_PARENT, + .num_parents = 1, + .parents = { &ck_pll1_vco }, + }; +@@ -2241,12 +2350,11 @@ static STM32_PLL_VCO(ck_pll2_vco, 2, PARENT(&ck_hsi, &ck_hse), + + static STM32_PLL_VCO(ck_pll3_vco, 3, + PARENT(&ck_hsi, &ck_hse, &ck_csi), +- 0, RCC_PLL3CR, GATE_PLL3, MUX_PLL3); ++ 0, RCC_PLL3CR, GATE_PLL3, MUX_PLL3); + + static STM32_PLL_VCO(ck_pll4_vco, 4, +- PARENT(&ck_hsi, &ck_hse, &ck_csi, +- &ck_i2sckin), +- 0, RCC_PLL4CR, GATE_PLL4, MUX_PLL4); ++ PARENT(&ck_hsi, &ck_hse, &ck_csi, &ck_i2sckin), ++ 0, RCC_PLL4CR, GATE_PLL4, MUX_PLL4); + + static STM32_PLL_OUPUT(ck_pll2p, 1, PARENT(&ck_pll2_vco), 0, + GATE_PLL2_DIVP, DIV_PLL2DIVP, NO_MUX); +@@ -2290,8 +2398,8 @@ static struct clk ck_mpu = { + static struct clk ck_axi = { + .ops = &clk_stm32_axi_ops, + .priv = &(struct clk_stm32_composite_cfg) { +- .mux_id = MUX_AXI, +- .div_id = DIV_AXI, ++ .mux_id = MUX_AXI, ++ .div_id = DIV_AXI, + }, + .name = "ck_axi", + .flags = 0, +@@ -2302,8 +2410,8 @@ static struct clk ck_axi = { + static struct clk ck_mlahb = { + .ops = &clk_stm32_mlahb_ops, + .priv = &(struct clk_stm32_composite_cfg) { +- .mux_id = MUX_MLAHB, +- .div_id = DIV_MLAHB, ++ .mux_id = MUX_MLAHB, ++ .div_id = DIV_MLAHB, + }, + .name = "ck_mlahb", + .flags = 0, +@@ -2311,8 +2419,8 @@ static struct clk ck_mlahb = { + .parents = { &ck_hsi, &ck_hse, &ck_csi, &ck_pll3p }, + }; + +-static STM32_MUX(ck_per, 4, PARENT(&ck_hsi, &ck_csi, &ck_hse, +- &ck_off), 0, MUX_CKPER); ++static STM32_MUX(ck_per, 4, PARENT(&ck_hsi, &ck_csi, &ck_hse, &ck_off), ++ 0, MUX_CKPER); + + /* Bus clocks */ + static STM32_DIVIDER(ck_pclk1, &ck_mlahb, 0, DIV_APB1); +@@ -2323,9 +2431,9 @@ static STM32_DIVIDER(ck_pclk5, &ck_axi, 0, DIV_APB5); + static STM32_DIVIDER(ck_pclk6, &ck_mlahb, 0, DIV_APB6); + + /* Timer Clocks */ +-static STM32_TIMER(ck_timg1, &ck_pclk1, 0, RCC_APB1DIVR, RCC_TIMG1PRER ); +-static STM32_TIMER(ck_timg2, &ck_pclk2, 0, RCC_APB2DIVR, RCC_TIMG2PRER ); +-static STM32_TIMER(ck_timg3, &ck_pclk6, 0, RCC_APB6DIVR, RCC_TIMG3PRER ); ++static STM32_TIMER(ck_timg1, &ck_pclk1, 0, RCC_APB1DIVR, RCC_TIMG1PRER); ++static STM32_TIMER(ck_timg2, &ck_pclk2, 0, RCC_APB2DIVR, RCC_TIMG2PRER); ++static STM32_TIMER(ck_timg3, &ck_pclk6, 0, RCC_APB6DIVR, RCC_TIMG3PRER); + + /* Peripheral and Kernel Clocks */ + static STM32_GATE(ck_ddrc1, &ck_axi, 0, GATE_DDRC1); +@@ -2377,6 +2485,8 @@ static STM32_GATE(ck_tim17_k, &ck_timg3, 0, GATE_TIM17); + static STM32_GATE(ck_ltdc_px, &ck_pll4q, 0, GATE_LTDC); + static STM32_GATE(ck_dma1, &ck_mlahb, 0, GATE_DMA1); + static STM32_GATE(ck_dma2, &ck_mlahb, 0, GATE_DMA2); ++static STM32_GATE(ck_adc1, &ck_mlahb, 0, GATE_ADC1); ++static STM32_GATE(ck_adc2, &ck_mlahb, 0, GATE_ADC2); + static STM32_GATE(ck_mdma, &ck_axi, 0, GATE_MDMA); + static STM32_GATE(ck_eth1mac, &ck_axi, 0, GATE_ETH1MAC); + static STM32_GATE(ck_usbh, &ck_axi, 0, GATE_USBH); +@@ -2411,97 +2521,81 @@ static STM32_KCLK(ck_stgen_k, 2, + + static STM32_KCLK(ck_usart1_k, 6, + PARENT(&ck_pclk6, &ck_pll3q, &ck_hsi, +- &ck_csi, &ck_pll4q, &ck_hse), ++ &ck_csi, &ck_pll4q, &ck_hse), + 0, GATE_USART1, MUX_UART1); + + static STM32_KCLK(ck_usart2_k, 6, +- PARENT(&ck_pclk6, &ck_pll3q, &ck_hsi, +- &ck_csi, &ck_pll4q, &ck_hse), ++ PARENT(&ck_pclk6, &ck_pll3q, &ck_hsi, &ck_csi, &ck_pll4q, ++ &ck_hse), + 0, GATE_USART2, MUX_UART2); + + static STM32_KCLK(ck_i2c4_k, 4, +- PARENT(&ck_pclk6, &ck_pll4r, &ck_hsi, +- &ck_csi), ++ PARENT(&ck_pclk6, &ck_pll4r, &ck_hsi, &ck_csi), + 0, GATE_I2C4, MUX_I2C4); + + static STM32_KCLK(ck_rtc, 4, +- PARENT(&ck_off, &ck_lse, &ck_lsi, +- &ck_hse), ++ PARENT(&ck_off, &ck_lse, &ck_lsi, &ck_hse), + 0, GATE_RTCCK, MUX_RTC); + + static STM32_KCLK(ck_saes_k, 4, +- PARENT(&ck_axi, &ck_per, &ck_pll4r, +- &ck_lsi), ++ PARENT(&ck_axi, &ck_per, &ck_pll4r, &ck_lsi), + 0, GATE_SAES, MUX_SAES); + + static STM32_KCLK(ck_rng1_k, 4, +- PARENT(&ck_csi, &ck_pll4r, &ck_off, +- &ck_lsi), ++ PARENT(&ck_csi, &ck_pll4r, &ck_lse, &ck_lsi), + 0, GATE_RNG1, MUX_RNG1); + + static STM32_KCLK(ck_sdmmc1_k, 4, +- PARENT(&ck_axi, &ck_pll3r, &ck_pll4p, +- &ck_hsi), ++ PARENT(&ck_axi, &ck_pll3r, &ck_pll4p, &ck_hsi), + 0, GATE_SDMMC1, MUX_SDMMC1); + + static STM32_KCLK(ck_sdmmc2_k, 4, +- PARENT(&ck_axi, &ck_pll3r, &ck_pll4p, +- &ck_hsi), ++ PARENT(&ck_axi, &ck_pll3r, &ck_pll4p, &ck_hsi), + 0, GATE_SDMMC2, MUX_SDMMC2); + + static STM32_KCLK(ck_usart3_k, 5, +- PARENT(&ck_pclk1, &ck_pll4q, &ck_hsi, &ck_csi, +- &ck_hse), ++ PARENT(&ck_pclk1, &ck_pll4q, &ck_hsi, &ck_csi, &ck_hse), + 0, GATE_USART3, MUX_UART35); + + static STM32_KCLK(ck_uart4_k, 5, +- PARENT(&ck_pclk1, &ck_pll4q, &ck_hsi, &ck_csi, +- &ck_hse), ++ PARENT(&ck_pclk1, &ck_pll4q, &ck_hsi, &ck_csi, &ck_hse), + 0, GATE_UART4, MUX_UART4); + + static STM32_KCLK(ck_uart5_k, 5, +- PARENT(&ck_pclk1, &ck_pll4q, &ck_hsi, &ck_csi, +- &ck_hse), ++ PARENT(&ck_pclk1, &ck_pll4q, &ck_hsi, &ck_csi, &ck_hse), + 0, GATE_UART5, MUX_UART35); + + static STM32_KCLK(ck_uart7_k, 5, +- PARENT(&ck_pclk1, &ck_pll4q, &ck_hsi, &ck_csi, +- &ck_hse), ++ PARENT(&ck_pclk1, &ck_pll4q, &ck_hsi, &ck_csi, &ck_hse), + 0, GATE_UART7, MUX_UART78); + + static STM32_KCLK(ck_uart8_k, 5, +- PARENT(&ck_pclk1, &ck_pll4q, &ck_hsi, &ck_csi, +- &ck_hse), ++ PARENT(&ck_pclk1, &ck_pll4q, &ck_hsi, &ck_csi, &ck_hse), + 0, GATE_UART8, MUX_UART78); + + static STM32_KCLK(ck_usart6_k, 5, +- PARENT(&ck_pclk2, &ck_pll4q, &ck_hsi, &ck_csi, +- &ck_hse), ++ PARENT(&ck_pclk2, &ck_pll4q, &ck_hsi, &ck_csi, &ck_hse), + 0, GATE_USART6, MUX_UART6); + + static STM32_KCLK(ck_fmc_k, 4, +- PARENT(&ck_axi, &ck_pll3r, &ck_pll4p, +- &ck_per), ++ PARENT(&ck_axi, &ck_pll3r, &ck_pll4p, &ck_per), + 0, GATE_FMC, MUX_FMC); + + static STM32_KCLK(ck_qspi_k, 4, +- PARENT(&ck_axi, &ck_pll3r, &ck_pll4p, +- &ck_per), ++ PARENT(&ck_axi, &ck_pll3r, &ck_pll4p, &ck_per), + 0, GATE_QSPI, MUX_QSPI); + + static STM32_KCLK(ck_lptim1_k, 6, +- PARENT(&ck_pclk1, &ck_pll4p, &ck_pll3q, +- &ck_lse, &ck_lsi, &ck_per), ++ PARENT(&ck_pclk1, &ck_pll4p, &ck_pll3q, &ck_lse, &ck_lsi, ++ &ck_per), + 0, GATE_LPTIM1, MUX_LPTIM1); + + static STM32_KCLK(ck_spi2_k, 5, +- PARENT(&ck_pll4p, &ck_pll3q, &ck_i2sckin, +- &ck_per, &ck_pll3r), ++ PARENT(&ck_pll4p, &ck_pll3q, &ck_i2sckin, &ck_per, &ck_pll3r), + 0, GATE_SPI2, MUX_SPI23); + + static STM32_KCLK(ck_spi3_k, 5, +- PARENT(&ck_pll4p, &ck_pll3q, &ck_i2sckin, +- &ck_per, &ck_pll3r), ++ PARENT(&ck_pll4p, &ck_pll3q, &ck_i2sckin, &ck_per, &ck_pll3r), + 0, GATE_SPI3, MUX_SPI23); + + static STM32_KCLK(ck_spdif_k, 3, +@@ -2509,95 +2603,81 @@ static STM32_KCLK(ck_spdif_k, 3, + 0, GATE_SPDIF, MUX_SPDIF); + + static STM32_KCLK(ck_spi1_k, 5, +- PARENT(&ck_pll4p, &ck_pll3q, &ck_i2sckin, +- &ck_per, &ck_pll3r), ++ PARENT(&ck_pll4p, &ck_pll3q, &ck_i2sckin, &ck_per, &ck_pll3r), + 0, GATE_SPI1, MUX_SPI1); + + static STM32_KCLK(ck_spi4_k, 6, +- PARENT(&ck_pclk6, &ck_pll4q, &ck_hsi, &ck_csi, +- &ck_hse, &ck_i2sckin), ++ PARENT(&ck_pclk6, &ck_pll4q, &ck_hsi, &ck_csi, &ck_hse, ++ &ck_i2sckin), + 0, GATE_SPI4, MUX_SPI4); + + static STM32_KCLK(ck_spi5_k, 5, +- PARENT(&ck_pclk6, &ck_pll4q, &ck_hsi, &ck_csi, +- &ck_hse), ++ PARENT(&ck_pclk6, &ck_pll4q, &ck_hsi, &ck_csi, &ck_hse), + 0, GATE_SPI5, MUX_SPI5); + + static STM32_KCLK(ck_sai1_k, 5, +- PARENT(&ck_pll4q, &ck_pll3q, &ck_i2sckin, +- &ck_per, &ck_pll3r), ++ PARENT(&ck_pll4q, &ck_pll3q, &ck_i2sckin, &ck_per, &ck_pll3r), + 0, GATE_SAI1, MUX_SAI1); + + static STM32_KCLK(ck_sai2_k, 6, +- PARENT(&ck_pll4q, &ck_pll3q, &ck_i2sckin, +- &ck_per, &ck_off, &ck_pll3r), ++ PARENT(&ck_pll4q, &ck_pll3q, &ck_i2sckin, &ck_per, &ck_off, ++ &ck_pll3r), + 0, GATE_SAI2, MUX_SAI2); + + static STM32_KCLK(ck_dfsdm_k, 5, +- PARENT(&ck_pll4q, &ck_pll3q, &ck_i2sckin, +- &ck_per, &ck_pll3r), ++ PARENT(&ck_pll4q, &ck_pll3q, &ck_i2sckin, &ck_per, &ck_pll3r), + 0, GATE_DFSDM, MUX_SAI1); + + static STM32_KCLK(ck_fdcan_k, 4, +- PARENT(&ck_hse, &ck_pll3q, &ck_pll4q, +- &ck_pll4r), ++ PARENT(&ck_hse, &ck_pll3q, &ck_pll4q, &ck_pll4r), + 0, GATE_FDCAN, MUX_FDCAN); + + static STM32_KCLK(ck_i2c1_k, 4, +- PARENT(&ck_pclk1, &ck_pll4r, &ck_hsi, +- &ck_csi), ++ PARENT(&ck_pclk1, &ck_pll4r, &ck_hsi, &ck_csi), + 0, GATE_I2C1, MUX_I2C12); + + static STM32_KCLK(ck_i2c2_k, 4, +- PARENT(&ck_pclk1, &ck_pll4r, &ck_hsi,&ck_csi), ++ PARENT(&ck_pclk1, &ck_pll4r, &ck_hsi, &ck_csi), + 0, GATE_I2C2, MUX_I2C12); + + static STM32_KCLK(ck_adfsdm_k, 5, +- PARENT(&ck_pll4q, &ck_pll3q, &ck_i2sckin, +- &ck_per, &ck_pll3r), ++ PARENT(&ck_pll4q, &ck_pll3q, &ck_i2sckin, &ck_per, &ck_pll3r), + 0, GATE_ADFSDM, MUX_SAI1); + + static STM32_KCLK(ck_lptim2_k, 5, +- PARENT(&ck_pclk3, &ck_pll4q, &ck_per, &ck_lse, +- &ck_lsi), ++ PARENT(&ck_pclk3, &ck_pll4q, &ck_per, &ck_lse, &ck_lsi), + 0, GATE_LPTIM2, MUX_LPTIM2); + + static STM32_KCLK(ck_lptim3_k, 5, +- PARENT(&ck_pclk3, &ck_pll4q, &ck_per, &ck_lse, +- &ck_lsi), ++ PARENT(&ck_pclk3, &ck_pll4q, &ck_per, &ck_lse, &ck_lsi), + 0, GATE_LPTIM3, MUX_LPTIM3); + + static STM32_KCLK(ck_lptim4_k, 6, +- PARENT(&ck_pclk3, &ck_pll4p, &ck_pll3q, +- &ck_lse,&ck_lsi, &ck_per), ++ PARENT(&ck_pclk3, &ck_pll4p, &ck_pll3q, &ck_lse, &ck_lsi, ++ &ck_per), + 0, GATE_LPTIM4, MUX_LPTIM45); + + static STM32_KCLK(ck_lptim5_k, 6, +- PARENT(&ck_pclk3, &ck_pll4p, &ck_pll3q, +- &ck_lse, &ck_lsi, &ck_per), ++ PARENT(&ck_pclk3, &ck_pll4p, &ck_pll3q, &ck_lse, &ck_lsi, ++ &ck_per), + 0, GATE_LPTIM5, MUX_LPTIM45); + + static STM32_KCLK(ck_i2c3_k, 4, +- PARENT(&ck_pclk6, &ck_pll4r, &ck_hsi, +- &ck_csi), ++ PARENT(&ck_pclk6, &ck_pll4r, &ck_hsi, &ck_csi), + 0, GATE_I2C3, MUX_I2C3); + + static STM32_KCLK(ck_i2c5_k, 4, +- PARENT(&ck_pclk6, &ck_pll4r, &ck_hsi, +- &ck_csi), ++ PARENT(&ck_pclk6, &ck_pll4r, &ck_hsi, &ck_csi), + 0, GATE_I2C5, MUX_I2C5); + + static STM32_KCLK(ck_dcmipp_k, 4, +- PARENT(&ck_axi, &ck_pll2q, &ck_pll4p, +- &ck_per), ++ PARENT(&ck_axi, &ck_pll2q, &ck_pll4p, &ck_per), + 0, GATE_DCMIPP, MUX_DCMIPP); + +-static STM32_KCLK(ck_adc1_k, 3, PARENT(&ck_pll4r, &ck_per, +- &ck_pll3q), ++static STM32_KCLK(ck_adc1_k, 3, PARENT(&ck_pll4r, &ck_per, &ck_pll3q), + 0, GATE_ADC1, MUX_ADC1); + +-static STM32_KCLK(ck_adc2_k, 3, PARENT(&ck_pll4r, &ck_per, +- &ck_pll3q), ++static STM32_KCLK(ck_adc2_k, 3, PARENT(&ck_pll4r, &ck_per, &ck_pll3q), + 0, GATE_ADC2, MUX_ADC2); + + static STM32_KCLK(ck_eth1ck_k, 2, PARENT(&ck_pll4p, &ck_pll3q), +@@ -2607,13 +2687,12 @@ static STM32_KCLK(ck_eth2ck_k, 2, PARENT(&ck_pll4p, &ck_pll3q), + 0, GATE_ETH2CK, MUX_ETH2); + + static STM32_COMPOSITE(ck_mco1, 5, +- PARENT(&ck_hsi, &ck_hse, &ck_csi, +- &ck_lsi, &ck_lse), 0, +- GATE_MCO1, DIV_MCO1, MUX_MCO1); ++ PARENT(&ck_hsi, &ck_hse, &ck_csi, &ck_lsi, &ck_lse), ++ 0, GATE_MCO1, DIV_MCO1, MUX_MCO1); + + static STM32_COMPOSITE(ck_mco2, 6, + PARENT(&ck_mpu, &ck_axi, &ck_mlahb, +- &ck_pll4p, &ck_hse, &ck_hsi), ++ &ck_pll4p, &ck_hse, &ck_hsi), + 0, GATE_MCO2, DIV_MCO2, MUX_MCO2); + + static STM32_COMPOSITE(ck_trace, 1, PARENT(&ck_axi), +@@ -2627,7 +2706,7 @@ enum { + STM32MP13_ALL_CLK_NB + }; + +-struct clk *stm32mp13_clk_provided[STM32MP13_ALL_CLK_NB] = { ++static struct clk *stm32mp13_clk_provided[STM32MP13_ALL_CLK_NB] = { + [CK_HSE] = &ck_hse, + [CK_CSI] = &ck_csi, + [CK_LSI] = &ck_lsi, +@@ -2722,6 +2801,8 @@ struct clk *stm32mp13_clk_provided[STM32MP13_ALL_CLK_NB] = { + [LTDC_PX] = &ck_ltdc_px, + [DMA1] = &ck_dma1, + [DMA2] = &ck_dma2, ++ [ADC1] = &ck_adc1, ++ [ADC2] = &ck_adc2, + [MDMA] = &ck_mdma, + [ETH1MAC] = &ck_eth1mac, + [USBH] = &ck_usbh, +@@ -2788,22 +2869,22 @@ static uint32_t clk_stm32_backup_div[DIV_NB]; + + static void clk_stm32_pm_save_mux(uint16_t mux_id) + { +- clk_stm32_backup_mux[mux_id] = clk_stm32_get_parent_mux(mux_id); ++ clk_stm32_backup_mux[mux_id] = stm32_mux_get_parent(mux_id); + } + + static void clk_stm32_pm_restore_mux(uint16_t mux_id) + { +- clk_stm32_set_parent_mux(mux_id, clk_stm32_backup_mux[mux_id]); ++ stm32_mux_set_parent(mux_id, clk_stm32_backup_mux[mux_id]); + } + + static void clk_stm32_pm_restore_div(uint16_t div_id) + { +- clk_stm32_set_div_value(div_id, clk_stm32_backup_div[div_id]); ++ stm32_div_set_value(div_id, clk_stm32_backup_div[div_id]); + } + + static void clk_stm32_pm_save_div(uint16_t div_id) + { +- clk_stm32_backup_div[div_id] = clk_stm32_div_get_value(div_id); ++ clk_stm32_backup_div[div_id] = stm32_div_get_value(div_id); + } + + static void clk_stm32_pm_force_set_all_oscillators(bool cmd) +@@ -2849,16 +2930,20 @@ static void clk_stm32_pm_backup_iwdg(void) + { + uintptr_t rcc_base = stm32_rcc_base(); + +- apb_iwdg1 = (io_read32(rcc_base + RCC_MP_APB5ENSETR) & RCC_MP_APB5ENSETR_IWDG1APBEN); +- apb_iwdg2 = (io_read32(rcc_base + RCC_MP_APB4ENSETR) & RCC_MP_APB4ENSETR_IWDG2APBEN); ++ apb_iwdg1 = (io_read32(rcc_base + RCC_MP_APB5ENSETR) & ++ RCC_MP_APB5ENSETR_IWDG1APBEN); ++ apb_iwdg2 = (io_read32(rcc_base + RCC_MP_APB4ENSETR) & ++ RCC_MP_APB4ENSETR_IWDG2APBEN); + } + + static void clk_stm32_pm_restore_iwdg(void) + { + uintptr_t rcc_base = stm32_rcc_base(); + +- io_clrsetbits32(rcc_base + RCC_MP_APB5ENSETR, RCC_MP_APB5ENSETR_IWDG1APBEN, apb_iwdg1); +- io_clrsetbits32(rcc_base + RCC_MP_APB4ENSETR, RCC_MP_APB4ENSETR_IWDG2APBEN, apb_iwdg2); ++ io_clrsetbits32(rcc_base + RCC_MP_APB5ENSETR, ++ RCC_MP_APB5ENSETR_IWDG1APBEN, apb_iwdg1); ++ io_clrsetbits32(rcc_base + RCC_MP_APB4ENSETR, ++ RCC_MP_APB4ENSETR_IWDG2APBEN, apb_iwdg2); + } + + static void clk_stm32_pm_backup_all_mux(void) +@@ -2867,7 +2952,7 @@ static void clk_stm32_pm_backup_all_mux(void) + uint32_t mux_id = 0U; + + for (mux_id = 0U; mux_id < priv->nb_muxes; mux_id++) +- clk_stm32_backup_mux[mux_id] = clk_stm32_get_parent_mux(mux_id); ++ clk_stm32_backup_mux[mux_id] = stm32_mux_get_parent(mux_id); + } + + static void clk_stm32_pm_restore_all_kernel_mux(void) +@@ -2885,7 +2970,7 @@ static void clk_stm32_pm_backup_all_div(void) + size_t div_id = 0; + + for (div_id = 0; div_id < priv->nb_div; div_id++) +- clk_stm32_backup_div[div_id] = clk_stm32_div_get_value(div_id); ++ clk_stm32_backup_div[div_id] = stm32_div_get_value(div_id); + } + + static void clk_stm32_pm_restore_div_system(void) +@@ -2903,8 +2988,8 @@ static void clk_stm32_pm_restore_div_system(void) + }; + + for (div_id = 0; div_id < ARRAY_SIZE(div_tab); div_id++) +- clk_stm32_set_div_value(div_tab[div_id], +- clk_stm32_backup_div[div_tab[div_id]]); ++ stm32_div_set_value(div_tab[div_id], ++ clk_stm32_backup_div[div_tab[div_id]]); + } + + /* Structure is used for set/clear registers and for regular registers */ +@@ -3052,31 +3137,38 @@ static void clk_stm32_pm_pll_backup_vco(struct clk_stm32_priv *priv, + int sel = 0; + + mux_id = pll->mux_id; +- sel = clk_stm32_get_parent_mux(mux_id); ++ sel = stm32_mux_get_parent(mux_id); + + vco->src = CMD_MUX << CMD_SHIFT | mux_id << MUX_ID_SHIFT | sel; + + /* Read N / M / IFREGE fields */ + value = io_read32(pll_base + RCC_OFFSET_PLLXCFGR1); + +- vco->div_mn[PLL_CFG_M] = (value & RCC_PLLNCFGR1_DIVM_MASK) >> RCC_PLLNCFGR1_DIVM_SHIFT; +- vco->div_mn[PLL_CFG_N] = (value & RCC_PLLNCFGR1_DIVN_MASK) >> RCC_PLLNCFGR1_DIVN_SHIFT; ++ vco->div_mn[PLL_CFG_M] = (value & RCC_PLLNCFGR1_DIVM_MASK) >> ++ RCC_PLLNCFGR1_DIVM_SHIFT; ++ vco->div_mn[PLL_CFG_N] = (value & RCC_PLLNCFGR1_DIVN_MASK) >> ++ RCC_PLLNCFGR1_DIVN_SHIFT; + + /* Read Frac */ +- vco->frac = io_read32(pll_base + RCC_OFFSET_PLLXFRACR) & RCC_PLLNFRACR_FRACV_MASK; ++ vco->frac = io_read32(pll_base + RCC_OFFSET_PLLXFRACR) & ++ RCC_PLLNFRACR_FRACV_MASK; + vco->frac = vco->frac >> RCC_PLLNFRACR_FRACV_SHIFT; + + /* Read CSG */ + value = io_read32(pll_base + RCC_OFFSET_PLLXCSGR); +- mod_per = (value & RCC_PLLNCSGR_MOD_PER_MASK) >> RCC_PLLNCSGR_MOD_PER_SHIFT; +- inc_step = (value & RCC_PLLNCSGR_INC_STEP_MASK) >> RCC_PLLNCSGR_INC_STEP_SHIFT; +- sscg_mode = (value & RCC_PLLNCSGR_SSCG_MODE_MASK) >> RCC_PLLNCSGR_SSCG_MODE_SHIFT; ++ mod_per = (value & RCC_PLLNCSGR_MOD_PER_MASK) >> ++ RCC_PLLNCSGR_MOD_PER_SHIFT; ++ inc_step = (value & RCC_PLLNCSGR_INC_STEP_MASK) >> ++ RCC_PLLNCSGR_INC_STEP_SHIFT; ++ sscg_mode = (value & RCC_PLLNCSGR_SSCG_MODE_MASK) >> ++ RCC_PLLNCSGR_SSCG_MODE_SHIFT; + + vco->csg[PLL_CSG_MOD_PER] = mod_per; + vco->csg[PLL_CSG_INC_STEP] = inc_step; + vco->csg[PLL_CSG_SSCG_MODE] = sscg_mode; + +- vco->csg_enabled = io_read32(pll_base + RCC_OFFSET_PLLXCSGR) && RCC_PLLNCR_SSCG_CTRL; ++ vco->csg_enabled = io_read32(pll_base + RCC_OFFSET_PLLXCSGR) && ++ RCC_PLLNCR_SSCG_CTRL; + } + + static void clk_stm32_pm_pll_backup_output(struct clk_stm32_priv *priv, +@@ -3088,9 +3180,12 @@ static void clk_stm32_pm_pll_backup_output(struct clk_stm32_priv *priv, + + value = io_read32(pll_base + RCC_OFFSET_PLLXCFGR2); + +- out->output[PLL_CFG_P] = (value & RCC_PLLNCFGR2_DIVP_MASK) >> RCC_PLLNCFGR2_DIVP_SHIFT; +- out->output[PLL_CFG_Q] = (value & RCC_PLLNCFGR2_DIVQ_MASK) >> RCC_PLLNCFGR2_DIVQ_SHIFT; +- out->output[PLL_CFG_R] = (value & RCC_PLLNCFGR2_DIVR_MASK) >> RCC_PLLNCFGR2_DIVR_SHIFT; ++ out->output[PLL_CFG_P] = (value & RCC_PLLNCFGR2_DIVP_MASK) >> ++ RCC_PLLNCFGR2_DIVP_SHIFT; ++ out->output[PLL_CFG_Q] = (value & RCC_PLLNCFGR2_DIVQ_MASK) >> ++ RCC_PLLNCFGR2_DIVQ_SHIFT; ++ out->output[PLL_CFG_R] = (value & RCC_PLLNCFGR2_DIVR_MASK) >> ++ RCC_PLLNCFGR2_DIVR_SHIFT; + } + + static void stm32_clk_pm_pll_backup(int pll_idx) +@@ -3111,7 +3206,6 @@ static void clk_stm32_pm_backup_pll34(void) + + for (i = 0; i < ARRAY_SIZE(plls); i++) + clk_stm32_pm_pll_backup_status(plls[i]); +- + } + + static void clk_stm32_pm_restore_pll34(void) +@@ -3121,13 +3215,20 @@ static void clk_stm32_pm_restore_pll34(void) + + for (i = 0; i < ARRAY_SIZE(plls); i++) { + int pll_id = plls[i]; +- struct stm32_pll_dt_cfg *pll_conf = &stm32_pll_backup_state[pll_id]; + const struct stm32_clk_pll *pll = clk_stm32_pll_data(pll_id); +- bool pll_status = clk_stm32_is_enabled_gate(pll->gate_id); ++ bool pll_status = stm32_gate_is_enabled(pll->gate_id); ++ struct stm32_pll_dt_cfg *conf = NULL; ++ ++ conf = &stm32_pll_backup_state[pll_id]; + +- if ((pll_conf->vco.status & RCC_PLLNCR_PLLON) && !pll_status) { +- clk_stm32_gate_ready_endisable(pll->gate_id, true, true); +- clk_stm32_pll_restore_output_diven(pll, pll_conf->vco.status); ++ if ((conf->vco.status & RCC_PLLNCR_PLLON) && !pll_status) { ++ if (stm32_gate_rdy_enable(pll->gate_id)) { ++ EMSG("timeout to enable pll %d", pll_id); ++ panic(); ++ } ++ ++ clk_stm32_pll_restore_output_diven(pll, ++ conf->vco.status); + } + } + } +@@ -3146,11 +3247,8 @@ static void clk_stm32_pm_restore_all_pll(void) + int pll_id = 0; + + for (pll_id = PLL1_ID; pll_id < PLL_NB; pll_id++) { +- struct stm32_pll_dt_cfg *pll_conf = &stm32_pll_backup_state[pll_id]; +- int err = 0; +- +- err = clk_stm32_pll_init(priv, pll_id, pll_conf); +- if (err) { ++ if (clk_stm32_pll_init(priv, pll_id, ++ &stm32_pll_backup_state[pll_id])) { + EMSG("Failed to restore PLL"); + panic(); + } +@@ -3159,26 +3257,29 @@ static void clk_stm32_pm_restore_all_pll(void) + + static void clk_stm32_pm_restore_pll_output_status(void) + { +- const uint32_t mask = RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN | RCC_PLLNCR_DIVREN; + struct clk_stm32_priv *priv = clk_stm32_get_priv(); ++ uint32_t mask = 0; + int pll_id = 0; + ++ mask = RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN | RCC_PLLNCR_DIVREN; ++ + for (pll_id = PLL1_ID; pll_id < PLL_NB; pll_id++) { +- struct stm32_pll_dt_cfg *pll_conf = &stm32_pll_backup_state[pll_id]; ++ struct stm32_pll_dt_cfg *conf = NULL; + const struct stm32_clk_pll *pll = clk_stm32_pll_data(pll_id); + uintptr_t pllxcr = priv->base + pll->reg_pllxcr; + ++ conf = &stm32_pll_backup_state[pll_id]; ++ + /* If pll was on */ +- if (pll_conf->vco.status & RCC_PLLNCR_PLLON) { ++ if (conf->vco.status & RCC_PLLNCR_PLLON) { + /* Set output */ +- io_clrsetbits32(pllxcr, mask, pll_conf->vco.status & mask); +- } +- else { ++ io_clrsetbits32(pllxcr, mask, ++ conf->vco.status & mask); ++ } else { + /* Stop all output */ + io_clrbits32(pllxcr, RCC_PLLNCR_DIVPEN | + RCC_PLLNCR_DIVQEN | RCC_PLLNCR_DIVREN); +- clk_stm32_gate_ready_endisable(pll->gate_id, false, true); +- ++ stm32_gate_rdy_disable(pll->gate_id); + } + } + } +@@ -3266,7 +3367,7 @@ static TEE_Result stm32_clock_pm(enum pm_op op __unused, + + #endif + +-static bool clk_stm32_clock_is_critical(__maybe_unused struct clk *clk) ++static bool clk_stm32_clock_is_critical(struct clk *clk __maybe_unused) + { + struct clk *clk_criticals[] = { + &ck_hsi, +@@ -3294,7 +3395,8 @@ static bool clk_stm32_clock_is_critical(__maybe_unused struct clk *clk) + &ck_bkpsram, + &ck_mce, + &ck_mco1, +- &ck_rng1_k ++ &ck_rng1_k, ++ &ck_mlahb, + }; + size_t i = 0; + +@@ -3329,30 +3431,27 @@ static bool clk_stm32_clock_is_ignored_unused(__maybe_unused struct clk *clk) + static void clk_stm32_init_oscillators(const void *fdt, int node) + { + size_t i = 0; +- TEE_Result __maybe_unused res = TEE_ERROR_GENERIC; +- const char *name[] = { "clk-hse", "clk-hsi", "clk-lse", +- "clk-lsi", "clk-csi", "clk-i2sin"}; +- struct clk *clks[] = { &ck_hse, &ck_hsi, &ck_lse, +- &ck_lsi, &ck_csi, &ck_i2sckin }; ++ const char *name[6] = { "clk-hse", "clk-hsi", "clk-lse", ++ "clk-lsi", "clk-csi", "clk-i2sin" }; ++ struct clk *clks[6] = { &ck_hse, &ck_hsi, &ck_lse, ++ &ck_lsi, &ck_csi, &ck_i2sckin }; + + for (i = 0; i < ARRAY_SIZE(clks); i++) { + struct clk *clk = NULL; +- res = clk_dt_get_by_name(fdt, node, name[i], &clk); + +- DMSG("%s: %s : clk = 0x%p, res = %d, rate = %ld\n", __func__, +- name[i], clk, res, clk_get_rate(clk)); ++ clk_dt_get_by_name(fdt, node, name[i], &clk); + + clks[i]->parents[0] = clk; + } + } + +-struct stm32_pll_dt_cfg mp13_pll[PLL_NB]; +-struct stm32_clk_opp_dt_cfg mp13_clk_opp; +-struct stm32_osci_dt_cfg mp13_osci[NB_OSCILLATOR]; +-uint32_t mp13_clksrc[MUX_NB]; +-uint32_t mp13_clkdiv[DIV_NB]; ++static struct stm32_pll_dt_cfg mp13_pll[PLL_NB]; ++static struct stm32_clk_opp_dt_cfg mp13_clk_opp; ++static struct stm32_osci_dt_cfg mp13_osci[NB_OSCILLATOR]; ++static uint32_t mp13_clksrc[MUX_NB]; ++static uint32_t mp13_clkdiv[DIV_NB]; + +-struct stm32_clk_platdata stm32mp13_clock_pdata = { ++static struct stm32_clk_platdata stm32mp13_clock_pdata = { + .osci = mp13_osci, + .nosci = NB_OSCILLATOR, + .pll = mp13_pll, +@@ -3381,6 +3480,7 @@ static struct clk_stm32_priv stm32mp13_clock_data = { + static TEE_Result stm32mp13_clk_probe(const void *fdt, int node, + const void *compat_data __unused) + { ++ TEE_Result res = TEE_ERROR_GENERIC; + int fdt_rc = 0; + int rc = 0; + struct clk_stm32_priv *priv = &stm32mp13_clock_data; +@@ -3388,24 +3488,26 @@ static TEE_Result stm32mp13_clk_probe(const void *fdt, int node, + + fdt_rc = stm32_clk_parse_fdt(fdt, node, pdata); + if (fdt_rc) { +- EMSG("Failed to parse clock FDT node: %d", fdt_rc); ++ EMSG("Failed to parse clock node: %d", fdt_rc); + return TEE_ERROR_GENERIC; + } + +- rc = clk_stm32_init(priv, stm32_rcc_base()); +- if (rc) +- return TEE_ERROR_GENERIC; ++ res = clk_stm32_init(priv, stm32_rcc_base()); ++ if (res) ++ return res; + + #ifdef CFG_STM32_CLK_DEBUG + clk_stm32_debug_display_pdata(); + #endif + +- clk_stm32_init_oscillators(fdt, node); + rc = stm32mp1_init_clock_tree(priv, pdata); + if (rc) + return TEE_ERROR_GENERIC; + ++ clk_stm32_init_oscillators(fdt, node); ++ + register_pm_core_service_cb(stm32_clock_pm, NULL, "stm32-rcc_pm"); ++ + stm32mp_clk_provider_probe_final(fdt, node, priv); + + #ifdef CFG_STM32_CLK_DEBUG +diff --git a/core/drivers/crypto/stm32/stm32_hash.c b/core/drivers/crypto/stm32/stm32_hash.c +index 3b590cffc4..c57458b44c 100644 +--- a/core/drivers/crypto/stm32/stm32_hash.c ++++ b/core/drivers/crypto/stm32/stm32_hash.c +@@ -598,11 +598,16 @@ TEE_Result stm32_hash_update(struct stm32_hash_context *c, + memcpy(((uint8_t *)c->remain.buf) + c->remain.len, buffer, + len); + c->remain.len += len; ++ ++ next_queue_size = c->queue_size; ++ + /* + * We don't need to save status as we didn't change IP + * internal state. + */ + goto exit; ++ } else { ++ next_queue_size = c->block_size; + } + + /* +@@ -630,6 +635,7 @@ TEE_Result stm32_hash_update(struct stm32_hash_context *c, + res = hash_write_data(hash_base(c), c->remain.buf[i]); + if (res) + goto exit; ++ + c->remain.buf[i] = 0; /* Reset to 0 */ + } + +@@ -638,14 +644,14 @@ TEE_Result stm32_hash_update(struct stm32_hash_context *c, + } + + /* +- * We will fill the queue for the first time, now queue flush will +- * happen exactly after block_size bytes. ++ * Here, the data should be written to the FIFO until we cannot ++ * guarantee anymore that the data that we write will trigger a ++ * process of data. Then we write the remaining data until DINIS ++ * is set to 1 by hardware, meaning that a complete block can be ++ * sent. Data written will be saved during save_context() and ++ * remaining data not written (if there's any) will be saved in ++ * c->remain.buf. + */ +- if (len >= c->queue_size) +- next_queue_size = c->block_size; +- else +- next_queue_size = c->queue_size; +- + while (len >= c->queue_size || + !(io_read32(hash_base(c) + _HASH_SR) & _HASH_SR_DINIS)) { + uint32_t tmp_buf = 0; +diff --git a/core/drivers/crypto/stm32/stm32_saes.c b/core/drivers/crypto/stm32/stm32_saes.c +index ce04b7473d..9453d0d52a 100644 +--- a/core/drivers/crypto/stm32/stm32_saes.c ++++ b/core/drivers/crypto/stm32/stm32_saes.c +@@ -10,11 +10,13 @@ + #include + #include + #include ++#include + #include + #include + #include + #include + #include ++#include + #include + #include + +@@ -256,6 +258,30 @@ static void clear_computation_completed(uintptr_t base) + io_setbits32(base + _SAES_ICR, _SAES_I_CC); + } + ++static TEE_Result wait_key_valid(vaddr_t base) ++{ ++ uint64_t timeout_ref = timeout_init_us(SAES_TIMEOUT_US); ++ ++ while ((io_read32(base + _SAES_SR) & _SAES_SR_KEYVALID) != ++ _SAES_SR_KEYVALID) ++ if (timeout_elapsed(timeout_ref)) ++ break; ++ ++ if ((io_read32(base + _SAES_SR) & _SAES_SR_KEYVALID) != ++ _SAES_SR_KEYVALID) { ++ DMSG("CCF timeout"); ++ return TEE_ERROR_BUSY; ++ } ++ ++ return TEE_SUCCESS; ++} ++ ++static void saes_select_instance(struct stm32_saes_context *ctx) ++{ ++ ctx->lock = &saes_lock; ++ ctx->base = io_pa_or_va(&saes_pdata.base, 1); ++} ++ + static TEE_Result saes_start(struct stm32_saes_context *ctx) + { + uint64_t timeout_ref = 0; +@@ -350,6 +376,8 @@ static void saes_write_key(struct stm32_saes_context *ctx) + + static TEE_Result saes_prepare_key(struct stm32_saes_context *ctx) + { ++ TEE_Result res = TEE_ERROR_GENERIC; ++ + /* Disable the SAES peripheral */ + io_clrbits32(ctx->base + _SAES_CR, _SAES_CR_EN); + +@@ -361,13 +389,16 @@ static TEE_Result saes_prepare_key(struct stm32_saes_context *ctx) + + saes_write_key(ctx); + ++ res = wait_key_valid(ctx->base); ++ if (res) ++ return res; ++ + /* + * For ECB/CBC decryption, key preparation mode must be selected + * to populate the key. + */ + if ((IS_CHAINING_MODE(ECB, ctx->cr) || + IS_CHAINING_MODE(CBC, ctx->cr)) && is_decrypt(ctx->cr)) { +- TEE_Result res = TEE_SUCCESS; + + /* Select Mode 2 */ + io_clrsetbits32(ctx->base + _SAES_CR, _SAES_CR_MODE_MASK, +@@ -544,12 +575,12 @@ TEE_Result stm32_saes_init(struct stm32_saes_context *ctx, bool is_dec, + if (!ctx) + return TEE_ERROR_BAD_PARAMETERS; + ++ saes_select_instance(ctx); ++ + ctx->assoc_len = 0U; + ctx->load_len = 0U; + ctx->extra_size = 0U; +- ctx->lock = &saes_lock; + +- ctx->base = io_pa_or_va(&saes_pdata.base, 1); + ctx->cr = _SAES_CR_RESET_VALUE; + + /* We want buffer to be u32 aligned */ +@@ -1121,6 +1152,239 @@ out: + return res; + } + ++static void xor_block(uint8_t *b1, uint8_t *b2, size_t size) ++{ ++ size_t i = 0; ++ ++ for (i = 0; i < size; i++) ++ b1[i] ^= b2[i]; ++} ++ ++static TEE_Result stm32_saes_cmac_prf_128(struct stm32_saes_context *ctx, ++ enum stm32_saes_key_selection key_select, ++ const void *key, size_t key_size, uint8_t *data, ++ size_t data_size, uint8_t *out) ++{ ++ TEE_Result res = TEE_ERROR_GENERIC; ++ uint8_t l[AES_BLOCK_SIZE] = { 0 }; ++ uint8_t k1[AES_BLOCK_SIZE] = { 0 }; ++ uint8_t k2[AES_BLOCK_SIZE] = { 0 }; ++ uint8_t block[AES_BLOCK_SIZE] = { 0 }; ++ int i = 0; ++ uint8_t bit = 0; ++ size_t processed = 0; ++ ++ if (!ctx) ++ return TEE_ERROR_BAD_PARAMETERS; ++ ++ /* Get K1 and K2 */ ++ res = stm32_saes_init(ctx, false, STM32_SAES_MODE_ECB, key_select, ++ key, key_size, NULL, 0); ++ if (res) ++ goto out; ++ ++ res = stm32_saes_update(ctx, true, l, l, sizeof(l)); ++ if (res) ++ goto out; ++ ++ /* MSB(L) == 0 => K1 = L << 1 */ ++ bit = 0; ++ for (i = sizeof(l) - 1; i >= 0; i--) { ++ k1[i] = (l[i] << 1) | bit; ++ bit = (l[i] & 0x80) >> 7; ++ } ++ /* MSB(L) == 1 => K1 = (L << 1) XOR const_Rb */ ++ if ((l[0] & 0x80)) ++ k1[sizeof(k1) - 1] = k1[sizeof(k1) - 1] ^ 0x87; ++ ++ /* MSB(K1) == 0 => K2 = K1 << 1 */ ++ bit = 0; ++ for (i = sizeof(k1) - 1; i >= 0; i--) { ++ k2[i] = (k1[i] << 1) | bit; ++ bit = (k1[i] & 0x80) >> 7; ++ } ++ ++ /* MSB(K1) == 1 => K2 = (K1 << 1) XOR const_Rb */ ++ if ((k1[0] & 0x80)) ++ k2[sizeof(k2) - 1] = k2[sizeof(k2) - 1] ^ 0x87; ++ ++ memset(block, 0, sizeof(block)); ++ ++ if (data_size > AES_BLOCK_SIZE) { ++ uint8_t *data_out; ++ ++ /* all block but last in CBC mode */ ++ res = stm32_saes_init(ctx, false, STM32_SAES_MODE_CBC, ++ key_select, key, key_size, block, ++ sizeof(block)); ++ if (res) ++ goto out; ++ ++ processed = ROUNDDOWN(data_size - 1, AES_BLOCK_SIZE); ++ data_out = malloc(processed); ++ if (!data_out) ++ return TEE_ERROR_OUT_OF_MEMORY; ++ ++ res = stm32_saes_update(ctx, true, data, data_out, processed); ++ if (res) { ++ free(data_out); ++ goto out; ++ } ++ ++ /* Copy last out block or keep block as { 0 } */ ++ memcpy(block, data_out + processed - AES_BLOCK_SIZE, ++ AES_BLOCK_SIZE); ++ ++ free(data_out); ++ } ++ ++ /* Manage last block */ ++ xor_block(block, data + processed, data_size - processed); ++ if (data_size - processed == AES_BLOCK_SIZE) { ++ xor_block(block, k1, AES_BLOCK_SIZE); ++ } else { ++ /* xor with padding = 0b100... */ ++ block[data_size - processed] ^= 0x80; ++ xor_block(block, k2, AES_BLOCK_SIZE); ++ } ++ ++ /* ++ * AES last block. ++ * We need to use same chaining mode to keep same key if DHUK is ++ * selected. ++ * So we reuse l as a { 0 } iv ++ */ ++ memset(l, 0, sizeof(l)); ++ res = stm32_saes_init(ctx, false, STM32_SAES_MODE_CBC, key_select, key, ++ key_size, l, sizeof(l)); ++ if (res) ++ goto out; ++ ++ res = stm32_saes_update(ctx, true, block, out, AES_BLOCK_SIZE); ++ ++out: ++ return res; ++} ++ ++TEE_Result stm32_saes_kdf_aes_cmac_prf(struct stm32_saes_context *ctx, ++ enum stm32_saes_key_selection key_select, ++ const void *key, size_t key_size, ++ const void *input, size_t input_size, ++ uint8_t *subkey, size_t subkey_size) ++ ++{ ++ TEE_Result res = TEE_SUCCESS; ++ uint32_t index = 0; ++ uint32_t index_be = 0; ++ uint8_t *data = NULL; ++ size_t data_index = 0; ++ size_t subkey_index = 0; ++ size_t data_size = input_size + sizeof(index_be); ++ uint8_t cmac[AES_BLOCK_SIZE] = { 0 }; ++ ++ if (!ctx || !input || !input_size) ++ return TEE_ERROR_BAD_PARAMETERS; ++ ++ /* For each K(i) we will add an index */ ++ data = malloc(data_size); ++ if (!data) ++ return TEE_ERROR_OUT_OF_MEMORY; ++ ++ data_index = 0; ++ index_be = TOBE32(index); ++ memcpy(data + data_index, &index_be, sizeof(index_be)); ++ data_index += sizeof(index_be); ++ memcpy(data + data_index, input, input_size); ++ data_index += input_size; ++ ++ /* ++ * K(i) computation. ++ */ ++ index = 0; ++ while (subkey_index < subkey_size) { ++ /* update index */ ++ index++; ++ index_be = TOBE32(index); ++ memcpy(data, &index_be, sizeof(index_be)); ++ ++ res = stm32_saes_cmac_prf_128(ctx, key_select, key, key_size, ++ data, data_size, cmac); ++ if (res) ++ goto out; ++ ++ memcpy(subkey + subkey_index, cmac, MIN(subkey_size - ++ subkey_index, ++ sizeof(cmac))); ++ subkey_index += sizeof(cmac); ++ } ++ ++out: ++ free(data); ++ if (res) ++ memzero_explicit(subkey, subkey_size); ++ ++ return res; ++} ++ ++TEE_Result huk_subkey_derive(enum huk_subkey_usage usage, ++ const void *const_data, size_t const_data_len, ++ uint8_t *subkey, size_t subkey_len) ++{ ++ TEE_Result res = TEE_ERROR_GENERIC; ++ uint8_t *input = NULL; ++ size_t input_index = 0; ++ size_t subkey_bitlen = 0; ++ struct stm32_saes_context ctx = { }; ++ uint8_t separator = 0; ++ ++ input = malloc(const_data_len + sizeof(separator) + sizeof(usage) + ++ sizeof(subkey_bitlen) + AES_BLOCK_SIZE); ++ if (!input) ++ return TEE_ERROR_OUT_OF_MEMORY; ++ ++ input_index = 0; ++ if (const_data) { ++ memcpy(input + input_index, const_data, const_data_len); ++ input_index += const_data_len; ++ ++ memcpy(input + input_index, &separator, sizeof(separator)); ++ input_index += sizeof(separator); ++ } ++ ++ memcpy(input + input_index, &usage, sizeof(usage)); ++ input_index += sizeof(usage); ++ ++ /* ++ * We should add the subkey_len in bits at end of input. ++ * And we choose to put in a MSB first uint32_t. ++ */ ++ subkey_bitlen = TOBE32(subkey_len * INT8_BIT); ++ memcpy(input + input_index, &subkey_bitlen, sizeof(subkey_bitlen)); ++ input_index += sizeof(subkey_bitlen); ++ ++ /* ++ * We get K(0) to avoid some key control attack ++ * and store it at end of input. ++ */ ++ res = stm32_saes_cmac_prf_128(&ctx, STM32_SAES_KEY_DHU, NULL, ++ AES_KEYSIZE_128, ++ input, input_index, ++ input + input_index); ++ if (res) ++ goto out; ++ ++ /* We just added K(0) to input */ ++ input_index += AES_BLOCK_SIZE; ++ ++ res = stm32_saes_kdf_aes_cmac_prf(&ctx, STM32_SAES_KEY_DHU, NULL, ++ AES_KEYSIZE_128, input, input_index, ++ subkey, subkey_len); ++ ++out: ++ free(input); ++ return res; ++} ++ + #ifdef CFG_EMBED_DTB + static TEE_Result stm32_saes_parse_fdt(struct stm32_saes_platdata *pdata, + const void *fdt, int node) +diff --git a/core/drivers/crypto/stm32/stm32_saes.h b/core/drivers/crypto/stm32/stm32_saes.h +index d11ff98103..14fe3f4ae3 100644 +--- a/core/drivers/crypto/stm32/stm32_saes.h ++++ b/core/drivers/crypto/stm32/stm32_saes.h +@@ -65,4 +65,10 @@ TEE_Result stm32_saes_update_load(struct stm32_saes_context *ctx, + uint8_t *data_out, size_t data_len); + TEE_Result stm32_saes_final(struct stm32_saes_context *ctx, uint8_t *tag, + size_t tag_len); ++ ++TEE_Result stm32_saes_kdf_aes_cmac_prf(struct stm32_saes_context *ctx, ++ enum stm32_saes_key_selection key_select, ++ const void *key, size_t key_size, ++ const void *input, size_t input_size, ++ uint8_t *subkey, size_t subkey_size); + #endif +diff --git a/core/drivers/regulator/stm32_regulator_gpio.c b/core/drivers/regulator/stm32_regulator_gpio.c +index e45f9a8f6c..e121c327db 100644 +--- a/core/drivers/regulator/stm32_regulator_gpio.c ++++ b/core/drivers/regulator/stm32_regulator_gpio.c +@@ -1,54 +1,75 @@ + // SPDX-License-Identifier: BSD-2-Clause + /* +- * Copyright (c) 2020-2021, STMicroelectronics ++ * Copyright (c) 2020-2022, STMicroelectronics + */ + +-#include +-#include + #include + #include + #include +-#include +-#include +-#include + #include +-#include + #include +-#include + #include + #include + #include + +-#define GPIO_REGULATOR_NAME_LEN 16 +-#define GPIO_REGULATOR_MAX_STATES 2 ++#define GPIO_REGULATOR_NAME_LEN U(16) ++#define MAX_VOLTAGE_LIST_SIZE U(2) + + struct gpio_regul { +- char name[GPIO_REGULATOR_NAME_LEN]; + struct regul_desc desc; +- struct stm32_pinctrl_list *pinctrl; +- uint16_t gpio_low_mv; +- uint16_t gpio_high_mv; +- uint16_t gpio_voltage_table[2]; ++ struct regul_ops ops; ++ unsigned int enable_gpio; ++ unsigned int voltage_gpio; ++ uint16_t gpio_voltage_table[MAX_VOLTAGE_LIST_SIZE]; ++ char name[GPIO_REGULATOR_NAME_LEN]; ++ bool enable_active_high; ++ bool voltage_level_high; + }; + +-static TEE_Result gpio_get_voltage(const struct regul_desc *desc, uint16_t *mv) ++static TEE_Result set_state_always_on(const struct regul_desc *desc __unused, ++ bool enable __unused) ++{ ++ return TEE_SUCCESS; ++} ++ ++static TEE_Result get_state_always_on(const struct regul_desc *desc __unused, ++ bool *enabled __unused) ++{ ++ *enabled = true; ++ ++ return TEE_SUCCESS; ++} ++ ++static TEE_Result gpio_set_state(const struct regul_desc *desc, bool enable) + { + struct gpio_regul *gr = (struct gpio_regul *)desc->driver_data; +- struct stm32_pinctrl *pinctrl = NULL; +- unsigned int gpio = 0; ++ enum gpio_level level = GPIO_LEVEL_LOW; + +- FMSG("%s: get volt", desc->node_name); ++ FMSG("regul %s set state=%u\n", desc->node_name, enable); + +- pinctrl = STAILQ_FIRST(gr->pinctrl); +- if (!pinctrl) +- panic(); ++ if (!gr->enable_active_high) ++ enable = !enable; + +- gpio = stm32_pinctrl_get_gpio_id(pinctrl); ++ level = enable ? GPIO_LEVEL_HIGH : GPIO_LEVEL_LOW; + +- if (stm32_gpio_get_ops()->get_value(NULL, gpio) == GPIO_LEVEL_HIGH) +- *mv = gr->gpio_high_mv; +- else +- *mv = gr->gpio_low_mv; ++ stm32_gpio_get_ops()->set_value(NULL, gr->enable_gpio, level); ++ ++ return TEE_SUCCESS; ++} ++ ++static TEE_Result gpio_get_state(const struct regul_desc *desc, bool *enabled) ++{ ++ struct gpio_regul *gr = (struct gpio_regul *)desc->driver_data; ++ enum gpio_level level = GPIO_LEVEL_LOW; ++ ++ level = stm32_gpio_get_ops()->get_value(NULL, gr->enable_gpio); ++ ++ *enabled = level == GPIO_LEVEL_HIGH; ++ ++ if (!gr->enable_active_high) ++ *enabled = !(*enabled); ++ ++ FMSG("regul %s get state=%u\n", desc->node_name, *enabled); + + return TEE_SUCCESS; + } +@@ -56,24 +77,39 @@ static TEE_Result gpio_get_voltage(const struct regul_desc *desc, uint16_t *mv) + static TEE_Result gpio_set_voltage(const struct regul_desc *desc, uint16_t mv) + { + struct gpio_regul *gr = (struct gpio_regul *)desc->driver_data; +- struct stm32_pinctrl *pinctrl = NULL; +- unsigned int gpio = 0; + enum gpio_level level = GPIO_LEVEL_LOW; + + FMSG("%s: set volt", desc->node_name); + +- pinctrl = STAILQ_FIRST(gr->pinctrl); +- if (!pinctrl) +- panic(); ++ if (mv == gr->gpio_voltage_table[1]) ++ level = GPIO_LEVEL_HIGH; ++ else if (mv != gr->gpio_voltage_table[0]) ++ return TEE_ERROR_BAD_PARAMETERS; + +- gpio = stm32_pinctrl_get_gpio_id(pinctrl); ++ if (!gr->voltage_level_high) ++ level = level == GPIO_LEVEL_HIGH ? GPIO_LEVEL_LOW : ++ GPIO_LEVEL_HIGH; + +- if (mv == gr->gpio_high_mv) +- level = GPIO_LEVEL_HIGH; +- else if (mv != gr->gpio_low_mv) +- panic(); ++ stm32_gpio_get_ops()->set_value(NULL, gr->voltage_gpio, level); ++ ++ return TEE_SUCCESS; ++} ++ ++static TEE_Result gpio_get_voltage(const struct regul_desc *desc, uint16_t *mv) ++{ ++ struct gpio_regul *gr = (struct gpio_regul *)desc->driver_data; ++ enum gpio_level level = GPIO_LEVEL_LOW; ++ ++ FMSG("%s: get volt", desc->node_name); + +- stm32_gpio_get_ops()->set_value(NULL, gpio, level); ++ level = stm32_gpio_get_ops()->get_value(NULL, gr->voltage_gpio); ++ ++ if (!gr->voltage_level_high) ++ level = level == GPIO_LEVEL_HIGH ? GPIO_LEVEL_LOW : ++ GPIO_LEVEL_HIGH; ++ ++ *mv = level == GPIO_LEVEL_HIGH ? gr->gpio_voltage_table[1] : ++ gr->gpio_voltage_table[0]; + + return TEE_SUCCESS; + } +@@ -89,23 +125,33 @@ static TEE_Result gpio_list_voltages(const struct regul_desc *desc, + return TEE_SUCCESS; + } + +-static struct regul_ops gpio_regul_ops = { +- .set_voltage = gpio_set_voltage, +- .get_voltage = gpio_get_voltage, +- .list_voltages = gpio_list_voltages, +-}; +-DECLARE_KEEP_PAGER(gpio_regul_ops); ++static TEE_Result get_pinctrl_from_index(struct stm32_pinctrl_list *list, ++ size_t index, ++ struct stm32_pinctrl **pin) ++{ ++ size_t count = 0; ++ ++ STAILQ_FOREACH(*pin, list, link) { ++ if (count == index) ++ return TEE_SUCCESS; ++ ++ count++; ++ } ++ ++ EMSG("Pin index not found in pinctrl list"); ++ ++ return TEE_ERROR_GENERIC; ++} + + static TEE_Result gpio_regulator_probe(const void *fdt, int node, + const void *compat_data __unused) + { + TEE_Result res = TEE_ERROR_GENERIC; +- + size_t len = 0; + struct gpio_regul *gr = NULL; + const char *reg_name = NULL; + const fdt32_t *cuint = NULL; +- struct stm32_pinctrl *pinctrl = NULL; ++ struct stm32_pinctrl_list *list = NULL; + + gr = calloc(1, sizeof(*gr)); + if (!gr) +@@ -113,38 +159,89 @@ static TEE_Result gpio_regulator_probe(const void *fdt, int node, + + reg_name = fdt_get_name(fdt, node, NULL); + len = snprintf(gr->name, sizeof(gr->name) - 1, "%s", reg_name); +- assert(len > 0 && len < (sizeof(gr->name) - 1)); ++ if ((len <= 0) || (len >= (sizeof(gr->name) - 1))) { ++ res = TEE_ERROR_BAD_PARAMETERS; ++ goto err; ++ } ++ ++ FMSG("%s: probe", gr->name); + + gr->desc.node_name = gr->name; + gr->desc.driver_data = gr; +- gr->desc.ops = &gpio_regul_ops; +- +- res = stm32_pinctrl_dt_get_by_index(fdt, node, 0, &gr->pinctrl); +- if(res) +- return res; + +- len = 0; +- +- STAILQ_FOREACH(pinctrl, gr->pinctrl, link) +- len++; ++ gr->ops.set_state = set_state_always_on; ++ gr->ops.get_state = get_state_always_on; ++ gr->desc.ops = &gr->ops; ++ ++ res = stm32_pinctrl_dt_get_by_index(fdt, node, 0, &list); ++ if (res) ++ goto err; ++ ++ cuint = fdt_getprop(fdt, node, "st,voltage_pin_index", NULL); ++ if (cuint) { ++ size_t index = (size_t)fdt32_to_cpu(*cuint); ++ struct stm32_pinctrl *pin = NULL; ++ const uint32_t *prop = NULL; ++ int count = 0; ++ ++ res = get_pinctrl_from_index(list, index, &pin); ++ if (res) ++ goto err; ++ ++ gr->voltage_gpio = stm32_pinctrl_get_gpio_id(pin); ++ ++ prop = fdt_getprop(fdt, node, "states", &count); ++ if (!prop) { ++ res = TEE_ERROR_ITEM_NOT_FOUND; ++ goto err; ++ } ++ ++ /* The driver support only one GPIO to adjust voltage */ ++ if ((count / sizeof(uint32_t)) != 4) { ++ res = TEE_ERROR_BAD_PARAMETERS; ++ goto err; ++ } ++ ++ gr->gpio_voltage_table[0] = (uint16_t)(fdt32_to_cpu(prop[0]) / ++ U(1000)); ++ gr->gpio_voltage_table[1] = (uint16_t)(fdt32_to_cpu(prop[2]) / ++ U(1000)); ++ ++ /* Low voltage should be lower than high voltage.*/ ++ if (gr->gpio_voltage_table[0] >= gr->gpio_voltage_table[1]) { ++ res = TEE_ERROR_BAD_PARAMETERS; ++ goto err; ++ } ++ ++ /* GPIO should have different states for both voltages */ ++ if (fdt32_to_cpu(prop[1]) == fdt32_to_cpu(prop[3])) { ++ res = TEE_ERROR_BAD_PARAMETERS; ++ goto err; ++ } ++ ++ if ((uint32_t)fdt32_to_cpu(prop[1]) == 0U) ++ gr->voltage_level_high = true; ++ ++ gr->ops.set_voltage = gpio_set_voltage; ++ gr->ops.get_voltage = gpio_get_voltage; ++ gr->ops.list_voltages = gpio_list_voltages; ++ } + +- if (len > 1) +- panic("Too many PINCTRLs found"); ++ cuint = fdt_getprop(fdt, node, "st,enable_pin_index", NULL); ++ if (cuint) { ++ size_t index = (size_t)fdt32_to_cpu(*cuint); ++ struct stm32_pinctrl *pin = NULL; + +- cuint = fdt_getprop(fdt, node, "low-level-microvolt", NULL); +- if (cuint) +- gr->gpio_low_mv = (uint16_t)(fdt32_to_cpu(*cuint) / 1000U); ++ res = get_pinctrl_from_index(list, index, &pin); ++ if (res) ++ goto err; + +- cuint = fdt_getprop(fdt, node, "high-level-microvolt", NULL); +- if (cuint) +- gr->gpio_high_mv = (uint16_t)(fdt32_to_cpu(*cuint) / 1000U); ++ gr->enable_gpio = stm32_pinctrl_get_gpio_id(pin); ++ if (fdt_getprop(fdt, node, "enable-active-high", NULL)) ++ gr->enable_active_high = true; + +- if (gr->gpio_low_mv < gr->gpio_high_mv) { +- gr->gpio_voltage_table[0] = gr->gpio_low_mv; +- gr->gpio_voltage_table[1] = gr->gpio_high_mv; +- } else { +- gr->gpio_voltage_table[0] = gr->gpio_high_mv; +- gr->gpio_voltage_table[1] = gr->gpio_low_mv; ++ gr->ops.set_state = gpio_set_state; ++ gr->ops.get_state = gpio_get_state; + } + + res = regulator_register(&gr->desc, node); +@@ -154,7 +251,10 @@ static TEE_Result gpio_regulator_probe(const void *fdt, int node, + panic(); + } + +- return TEE_SUCCESS; ++err: ++ if (res) ++ free(gr); ++ return res; + } + + static const struct dt_device_match gpio_regulator_match_table[] = { +diff --git a/core/drivers/regulator/stm32mp1_regulator_iod.c b/core/drivers/regulator/stm32mp1_regulator_iod.c +new file mode 100644 +index 0000000000..de28fbc89f +--- /dev/null ++++ b/core/drivers/regulator/stm32mp1_regulator_iod.c +@@ -0,0 +1,323 @@ ++// SPDX-License-Identifier: BSD-2-Clause ++/* ++ * Copyright (c) 2021-2022, STMicroelectronics ++ */ ++ ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++ ++#define TIMEOUT_US_10MS U(10000) ++ ++#define IO_VOLTAGE_THRESHOLD U(2700) ++ ++/* ++ * struct iod_regul - IO domain regulator instance ++ * @enable_reg: PWR register offset for the IO domain ++ * @enable_mask: Domain enable register bit mask in PWR register ++ * @ready_mask: Domain ready bit mask in PWR register ++ * @valid_mask: Domain valid bit mask in PWR register ++ * @hslv_index: Index of HSLV register for the IO domain ++ * @suspend_state: True if regulator is enabled before suspend, false otherwise ++ * @suspend_mv: Voltage level before suspend in millivolts ++ * @supply: Regulator supplier for the IO domain ++ */ ++struct iod_regul { ++ uint32_t enable_reg; ++ uint32_t enable_mask; ++ uint32_t ready_mask; ++ uint32_t valid_mask; ++ ++ uint32_t hslv_index; ++ ++ bool suspend_state; ++ uint16_t suspend_mv; ++ ++ struct rdev *supply; ++}; ++ ++static TEE_Result iod_set_speed(const struct regul_desc *desc, bool state) ++{ ++ struct iod_regul *iod = (struct iod_regul *)desc->driver_data; ++ ++ FMSG("%s: set speed=%u", desc->node_name, state); ++ ++ stm32mp_set_hslv_by_index(iod->hslv_index, state); ++ ++ return TEE_SUCCESS; ++} ++ ++static TEE_Result iod_set_state(const struct regul_desc *desc, bool enable) ++{ ++ struct iod_regul *iod = (struct iod_regul *)desc->driver_data; ++ uintptr_t pwr_reg = stm32_pwr_base() + iod->enable_reg; ++ ++ FMSG("%s: set state %u", desc->node_name, enable); ++ ++ if (enable) { ++ uint64_t to = timeout_init_us(TIMEOUT_US_10MS); ++ ++ io_setbits32(pwr_reg, iod->enable_mask); ++ ++ while (!timeout_elapsed(to)) ++ if (io_read32(pwr_reg) & iod->ready_mask) ++ break; ++ ++ if (!(io_read32(pwr_reg) & iod->ready_mask)) ++ return TEE_ERROR_GENERIC; ++ ++ io_setbits32(pwr_reg, iod->valid_mask); ++ io_clrbits32(pwr_reg, iod->enable_mask); ++ } else { ++ io_clrbits32(pwr_reg, iod->enable_mask | iod->valid_mask); ++ } ++ ++ return TEE_SUCCESS; ++} ++ ++static TEE_Result iod_get_state(const struct regul_desc *desc, bool *enabled) ++{ ++ struct iod_regul *iod = (struct iod_regul *)desc->driver_data; ++ uintptr_t pwr_reg = stm32_pwr_base() + iod->enable_reg; ++ ++ FMSG("%s: get state", desc->node_name); ++ ++ *enabled = io_read32(pwr_reg) & (iod->enable_mask | iod->valid_mask); ++ ++ return TEE_SUCCESS; ++} ++ ++static TEE_Result iod_get_voltage(const struct regul_desc *desc, uint16_t *mv) ++{ ++ struct iod_regul *iod = (struct iod_regul *)desc->driver_data; ++ ++ FMSG("%s: get volt", desc->node_name); ++ ++ return regulator_get_voltage(iod->supply, mv); ++} ++ ++static TEE_Result iod_set_voltage(const struct regul_desc *desc, uint16_t mv) ++{ ++ struct iod_regul *iod = (struct iod_regul *)desc->driver_data; ++ TEE_Result res = TEE_ERROR_GENERIC; ++ ++ FMSG("%s: set volt to %"PRIu16" mv", desc->node_name, mv); ++ ++ /* ++ * Set IO to low speed ++ * Setting high voltage with IOs in high speed mode may damage the IOs ++ */ ++ res = iod_set_speed(desc, false); ++ if (res) ++ return res; ++ ++ /* Forward set voltage request to the power supply */ ++ res = regulator_set_voltage(iod->supply, mv); ++ if (res) { ++ EMSG("regulator %s set voltage failed", desc->node_name); ++ return res; ++ } ++ ++ if (mv < IO_VOLTAGE_THRESHOLD) ++ res = iod_set_speed(desc, true); ++ ++ return res; ++} ++ ++static TEE_Result iod_list_voltages(const struct regul_desc *desc, ++ uint16_t **levels, size_t *count) ++{ ++ struct iod_regul *iod = (struct iod_regul *)desc->driver_data; ++ ++ /* Returns parent voltage list */ ++ return regulator_list_voltages(iod->supply, levels, count); ++} ++ ++/* ++ * Power management suspend/resume operations ++ * To protect the IOs, we switch to low speed before entering suspend state ++ * and restore the configuration when resuming. ++ */ ++static TEE_Result iod_pm(enum pm_op op, unsigned int pm_hint __unused, ++ const struct pm_callback_handle *hdl) ++{ ++ struct regul_desc *desc = (struct regul_desc *)hdl->handle; ++ struct iod_regul *iod = (struct iod_regul *)desc->driver_data; ++ TEE_Result res = TEE_ERROR_GENERIC; ++ ++ assert(op == PM_OP_SUSPEND || op == PM_OP_RESUME); ++ ++ if (op == PM_OP_SUSPEND) { ++ FMSG("%s: suspend", desc->node_name); ++ ++ res = iod_get_state(desc, &iod->suspend_state); ++ if (res) ++ return res; ++ ++ res = iod_get_voltage(desc, &iod->suspend_mv); ++ if (res) ++ return res; ++ ++ /* Force to low speed */ ++ res = iod_set_speed(desc, false); ++ if (res) ++ return res; ++ } else { ++ FMSG("%s: resume", desc->node_name); ++ ++ res = iod_set_voltage(desc, iod->suspend_mv); ++ if (res) ++ return res; ++ ++ res = iod_set_state(desc, iod->suspend_state); ++ if (res) ++ return res; ++ } ++ ++ return TEE_SUCCESS; ++} ++DECLARE_KEEP_PAGER(iod_pm); ++ ++static const struct regul_ops iod_ops = { ++ .set_state = iod_set_state, ++ .get_state = iod_get_state, ++ .set_voltage = iod_set_voltage, ++ .get_voltage = iod_get_voltage, ++ .list_voltages = iod_list_voltages, ++ .lock = stm32mp1_pwr_regul_lock, ++ .unlock = stm32mp1_pwr_regul_unlock, ++}; ++DECLARE_KEEP_PAGER(iod_ops); ++ ++static const struct regul_ops iod_fixed_ops = { ++ .set_state = iod_set_state, ++ .get_state = iod_get_state, ++ .get_voltage = iod_get_voltage, ++ .lock = stm32mp1_pwr_regul_lock, ++ .unlock = stm32mp1_pwr_regul_unlock, ++}; ++DECLARE_KEEP_PAGER(iod_fixed_ops); ++ ++#define DEFINE_REG(id, name, supply) { \ ++ .node_name = name, \ ++ .ops = &iod_ops, \ ++ .driver_data = &iod_regulators[id], \ ++ .supply_name = supply, \ ++} ++ ++enum iod_regulator { ++ IOD_SDMMC1, ++ IOD_SDMMC2, ++ IOD_REGU_COUNT ++}; ++ ++static struct iod_regul iod_regulators[] = { ++ [IOD_SDMMC1] = { ++ .enable_reg = PWR_CR3_OFF, ++ .enable_mask = PWR_CR3_VDDSD1EN, ++ .ready_mask = PWR_CR3_VDDSD1RDY, ++ .valid_mask = PWR_CR3_VDDSD1VALID, ++ .hslv_index = SYSCFG_HSLV_IDX_SDMMC1, ++ }, ++ [IOD_SDMMC2] = { ++ .enable_reg = PWR_CR3_OFF, ++ .enable_mask = PWR_CR3_VDDSD2EN, ++ .ready_mask = PWR_CR3_VDDSD2RDY, ++ .valid_mask = PWR_CR3_VDDSD2VALID, ++ .hslv_index = SYSCFG_HSLV_IDX_SDMMC2, ++ }, ++}; ++ ++static struct regul_desc iod_regul_desc[] = { ++ [IOD_SDMMC1] = DEFINE_REG(IOD_SDMMC1, "sdmmc1_io", "vddsd1"), ++ [IOD_SDMMC2] = DEFINE_REG(IOD_SDMMC2, "sdmmc2_io", "vddsd2"), ++}; ++DECLARE_KEEP_PAGER(iod_regul_desc); ++ ++static TEE_Result iod_regulator_probe(const void *fdt, int node, ++ const void *compat_data __unused) ++{ ++ TEE_Result res = TEE_ERROR_GENERIC; ++ struct regul_desc *desc = NULL; ++ const char *regu_name = NULL; ++ struct iod_regul *iod = NULL; ++ uint16_t mv = 0; ++ size_t i = 0; ++ ++ regu_name = fdt_get_name(fdt, node, NULL); ++ ++ FMSG("iod probe with regu_name=%s", regu_name); ++ ++ /* Look for matching regul */ ++ for (i = 0; i < IOD_REGU_COUNT; i++) ++ if (!strcmp(iod_regul_desc[i].node_name, regu_name)) ++ break; ++ ++ if (i == IOD_REGU_COUNT) { ++ EMSG("regu_name=%s not found", regu_name); ++ return TEE_ERROR_GENERIC; ++ } ++ ++ desc = &iod_regul_desc[i]; ++ iod = (struct iod_regul *)desc->driver_data; ++ ++ iod->supply = regulator_get_by_supply_name(fdt, node, ++ desc->supply_name); ++ if (!iod->supply) { ++ DMSG("desc->supply_name=%s not found", desc->supply_name); ++ return TEE_ERROR_DEFER_DRIVER_INIT; ++ } ++ ++ /* If the supply voltage is fixed, do not handle voltage ops */ ++ if (iod->supply->min_mv == iod->supply->max_mv) ++ desc->ops = &iod_fixed_ops; ++ ++ res = regulator_register(desc, node); ++ if (res) { ++ EMSG("regulator_register(%s) failed with %#"PRIx32, ++ regu_name, res); ++ return res; ++ } ++ ++ res = iod_get_voltage(desc, &mv); ++ if (res) ++ return res; ++ ++ if (mv < IO_VOLTAGE_THRESHOLD) { ++ res = iod_set_speed(desc, true); ++ if (res) ++ return res; ++ } ++ ++ register_pm_driver_cb(iod_pm, (void *)desc, "iod-regu"); ++ ++ FMSG("regu_name=%s probed", regu_name); ++ ++ return TEE_SUCCESS; ++} ++ ++static const struct dt_device_match iod_regulator_match_table[] = { ++ { .compatible = "st,stm32mp13-iod" }, ++ { } ++}; ++ ++DEFINE_DT_DRIVER(stm32mp1_regulator_iod_dt_driver) = { ++ .name = "stm32mp1-iod-regulator", ++ .match_table = iod_regulator_match_table, ++ .probe = iod_regulator_probe, ++}; +diff --git a/core/drivers/regulator/sub.mk b/core/drivers/regulator/sub.mk +index 8e18c5dbec..8d28387405 100644 +--- a/core/drivers/regulator/sub.mk ++++ b/core/drivers/regulator/sub.mk +@@ -1,4 +1,5 @@ + srcs-y += core.c + srcs-$(CFG_REGULATOR_FIXED) += regulator_fixed.c + srcs-$(CFG_STM32_REGULATOR_GPIO) += stm32_regulator_gpio.c ++srcs-$(CFG_STM32MP1_REGULATOR_IOD) += stm32mp1_regulator_iod.c + srcs-$(CFG_STM32_VREFBUF) += stm32_vrefbuf.c +diff --git a/core/drivers/stm32_bsec.c b/core/drivers/stm32_bsec.c +index 69b8a84cc2..6190c32909 100644 +--- a/core/drivers/stm32_bsec.c ++++ b/core/drivers/stm32_bsec.c +@@ -731,7 +731,7 @@ static void bsec_dt_otp_nsec_access(void *fdt, int bsec_node) + fdt_for_each_subnode(bsec_subnode, fdt, bsec_node) { + const fdt32_t *cuint = NULL; + unsigned int otp_id = 0; +- unsigned int i = 0; ++ unsigned int i, j = 0; + size_t size = 0; + uint32_t offset = 0; + uint32_t length = 0; +@@ -770,26 +770,31 @@ static void bsec_dt_otp_nsec_access(void *fdt, int bsec_node) + /* Handle different kinds of non-secure accesses */ + if (fdt_getprop(fdt, bsec_subnode, + "st,non-secure-otp-provisioning", NULL)) { +- uint32_t read_otp_value = 0; +- bool locked = false; +- ++ bool locked, locked_next = false; + /* Check if write of OTP is locked */ +- if (stm32_bsec_read_sw_lock(otp_id, &locked)) +- panic("BSEC: Couldn't read sw lock at init"); ++ if (stm32_bsec_read_permanent_lock(otp_id, &locked)) ++ panic("BSEC: Couldn't read permanent lock at init"); ++ ++ /* ++ * Check if fuses of the subnode ++ * have the same lock status ++ */ ++ for (j = 1; j < (length / sizeof(uint32_t)); j++) { ++ stm32_bsec_read_permanent_lock(otp_id + j, ++ &locked_next); ++ if (locked != locked_next) { ++ EMSG("Inconsistent status OTP id %u", ++ otp_id + j); ++ locked = true; ++ continue; ++ } ++ } + + if (locked) { + DMSG("BSEC: OTP locked"); + continue; + } + +- /* Check if fuses are empty */ +- if (stm32_bsec_read_otp(&read_otp_value, otp_id)) +- panic("Couldn't check if fuses are empty"); +- +- if (read_otp_value) { +- DMSG("Fuses not empty"); +- continue; +- } + } else if (!fdt_getprop(fdt, bsec_subnode, "st,non-secure-otp", + NULL)) { + continue; +diff --git a/core/drivers/stm32_huk.c b/core/drivers/stm32_huk.c +new file mode 100644 +index 0000000000..ec310bc8f0 +--- /dev/null ++++ b/core/drivers/stm32_huk.c +@@ -0,0 +1,80 @@ ++// SPDX-License-Identifier: BSD-3-Clause ++/* ++ * Copyright (c) 2022, STMicroelectronics ++ */ ++ ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++ ++#ifdef CFG_OTP_HW_TESTKEY ++static const uint8_t otp_hw_test_key[] = { ++ 0xD8, 0x30, 0xA1, 0x88, 0x14, 0xE0, 0x2F, 0xE9, ++ 0x43, 0x6B, 0xB3, 0x8E, 0x03, 0x02, 0xC4, 0x8C ++}; ++ ++TEE_Result tee_otp_get_hw_unique_key(struct tee_hw_unique_key *hwkey) ++{ ++ COMPILE_TIME_ASSERT(sizeof(otp_hw_test_key) >= sizeof(hwkey->data)); ++ ++ memcpy(hwkey->data, otp_hw_test_key, sizeof(hwkey->data)); ++ ++ return TEE_SUCCESS; ++} ++ ++#else ++ ++TEE_Result tee_otp_get_hw_unique_key(struct tee_hw_unique_key *hwkey) ++{ ++ uint32_t otp_id = 0; ++ size_t otp_bit_len = 0; ++ uint32_t tmp = 0; ++ bool provisioned = true; ++ size_t i = 0; ++ TEE_Result res = TEE_ERROR_GENERIC; ++ ++ if (!hwkey || !hwkey->data) ++ return TEE_ERROR_BAD_PARAMETERS; ++ ++ res = stm32_bsec_find_otp_in_nvmem_layout("huk_otp", &otp_id, ++ &otp_bit_len); ++ if (res) ++ goto out; ++ ++ if (otp_bit_len != (HW_UNIQUE_KEY_LENGTH * CHAR_BIT)) { ++ res = TEE_ERROR_BAD_PARAMETERS; ++ goto out; ++ } ++ ++ for (i = 0; i < HW_UNIQUE_KEY_LENGTH / sizeof(uint32_t); i++) { ++ res = stm32_bsec_read_permanent_lock(otp_id + i, &provisioned); ++ if (res) ++ goto out; ++ ++ if (!provisioned) { ++ res = TEE_ERROR_SECURITY; ++ goto out; ++ } ++ ++ res = stm32_bsec_shadow_read_otp(&tmp, otp_id + i); ++ if (res) ++ goto out; ++ ++ memcpy(hwkey->data + i * sizeof(uint32_t), &tmp, ++ sizeof(uint32_t)); ++ } ++ ++out: ++ if (res) { ++ EMSG("HUK not found"); ++ memzero_explicit(hwkey->data, HW_UNIQUE_KEY_LENGTH); ++ } ++ ++ return res; ++} ++#endif +diff --git a/core/drivers/stm32_rng.c b/core/drivers/stm32_rng.c +index 4e1da48c44..ef5862308e 100644 +--- a/core/drivers/stm32_rng.c ++++ b/core/drivers/stm32_rng.c +@@ -47,7 +47,7 @@ + #define RNG_NIST_CONFIG_B U(0x1801000) + #define RNG_NIST_CONFIG_MASK GENMASK_32(25, 8) + +-#define RNG_MAX_CLOCK_FREQ U(750000) ++#define RNG_MAX_NOISE_CLK_FREQ U(3000000) + + struct stm32_rng_driver_data { + bool has_cond_reset; +@@ -64,6 +64,7 @@ struct stm32_rng_device { + + /* Expect a single RNG device */ + static struct stm32_rng_device stm32_rng; ++DECLARE_KEEP_PAGER(stm32_rng); + + static vaddr_t get_base(struct stm32_rng_device *dev) + { +@@ -162,7 +163,7 @@ static uint32_t stm32_rng_clock_freq_restrain(struct stm32_rng_device *dev) + * No need to handle the case when clock-div > 0xF as it is physically + * impossible + */ +- while ((clock_rate >> clock_div) > RNG_MAX_CLOCK_FREQ) ++ while ((clock_rate >> clock_div) > RNG_MAX_NOISE_CLK_FREQ) + clock_div++; + + DMSG("RNG clk rate : %lu", clk_get_rate(dev->pdata.clock) >> clock_div); +@@ -307,7 +308,23 @@ uint8_t hw_get_random_byte(void) + + static TEE_Result stm32_rng_pm_resume(struct stm32_rng_device *dev) + { +- io_write32(get_base(dev) + RNG_CR, RNG_CR_RNGEN | dev->pm_cr); ++ /* Clean error indications */ ++ io_write32(get_base(dev) + RNG_SR, 0); ++ ++ if (dev->ddata->has_cond_reset) { ++ /* ++ * Correct configuration in bits [29:4] must be set in the same ++ * access that set RNG_CR_CONDRST bit. Else config setting is ++ * not taken into account. CONFIGLOCK bit must also be unset but ++ * it is not handled at the moment. ++ */ ++ io_write32(get_base(dev) + RNG_CR, dev->pm_cr | RNG_CR_CONDRST); ++ ++ io_clrsetbits32(get_base(dev) + RNG_CR, RNG_CR_CONDRST, ++ RNG_CR_RNGEN); ++ } else { ++ io_write32(get_base(dev) + RNG_CR, RNG_CR_RNGEN | dev->pm_cr); ++ } + + return TEE_SUCCESS; + } +@@ -452,6 +469,7 @@ static const struct stm32_rng_driver_data mp13_data[] = { + static const struct stm32_rng_driver_data mp15_data[] = { + { .has_cond_reset = false }, + }; ++DECLARE_KEEP_PAGER(mp15_data); + + static const struct dt_device_match rng_match_table[] = { + { .compatible = "st,stm32mp13-rng", .compat_data = &mp13_data }, +diff --git a/core/drivers/sub.mk b/core/drivers/sub.mk +index 38f03282ef..ba5cd5a7c8 100644 +--- a/core/drivers/sub.mk ++++ b/core/drivers/sub.mk +@@ -29,6 +29,7 @@ srcs-$(CFG_MVEBU_UART) += mvebu_uart.c + srcs-$(CFG_STM32_BSEC) += stm32_bsec.c + srcs-$(CFG_STM32_EXTI) += stm32_exti.c + srcs-$(CFG_STM32_GPIO) += stm32_gpio.c ++srcs-$(CFG_STM32_HUK) += stm32_huk.c + srcs-$(CFG_STM32_IWDG) += stm32_iwdg.c + srcs-$(CFG_STM32_I2C) += stm32_i2c.c + srcs-$(CFG_STM32_RNG) += stm32_rng.c +@@ -55,6 +56,7 @@ srcs-$(CFG_ZYNQMP_HUK) += zynqmp_huk.c + subdirs-y += counter + subdirs-y += crypto + subdirs-y += firewall ++subdirs-$(CFG_DRIVERS_ADC) += adc + subdirs-$(CFG_BNXT_FW) += bnxt + subdirs-$(CFG_DRIVERS_CLK) += clk + subdirs-$(CFG_DRIVERS_RSTCTRL) += rstctrl +diff --git a/core/include/drivers/adc.h b/core/include/drivers/adc.h +new file mode 100644 +index 0000000000..906857940b +--- /dev/null ++++ b/core/include/drivers/adc.h +@@ -0,0 +1,240 @@ ++/* SPDX-License-Identifier: BSD-2-Clause */ ++/* ++ * Copyright (c) 2022, STMicroelectronics ++ */ ++ ++#ifndef __DRIVERS_ADC_H ++#define __DRIVERS_ADC_H ++ ++#include ++#include ++ ++struct adc_ops; ++ ++/** ++ * struct adc_evt - event configuration filled by ADC user ++ * @id: ADC event identifier ++ * @lt: ADC event low threshold (raw value) ++ * @ht: ADC event high threshold (raw value) ++ */ ++struct adc_evt { ++ unsigned int id; ++ uint32_t lt; ++ uint32_t ht; ++}; ++ ++/** ++ * struct adc_chan - ADC channel descriptor ++ * @id: ADC channel identifier ++ * @name: ADC channel name ++ * @smpt: ADC channel minimum sampling time ++ */ ++struct adc_chan { ++ unsigned int id; ++ const char *name; ++ uint32_t smpt; ++}; ++ ++/** ++ * struct adc_device - ADC device ++ * @ops: ADC device operations pointer ++ * @name: ADC device instance name ++ * @state: ADC state (set when ADC is active) ++ * @lock: Protect ADC device against concurrent accessesw ++ * @channels: ADC channels table ++ * @channels_nb: ADC channels number ++ * @channel_mask: Bit mask of available channels ++ * @data_mask: Conversion output data mask ++ * @data: Pointer to private data ++ * @vref_mv: Reference supply voltage level in mV ++ */ ++struct adc_device { ++ struct adc_ops *ops; ++ char *name; ++ int state; ++ struct mutex lock; /* Protection against concurrent accesses */ ++ struct adc_chan *channels; ++ size_t channels_nb; ++ uint32_t channel_mask; ++ uint32_t data_mask; ++ void *data; ++ uint16_t vref_mv; ++}; ++ ++/** ++ * struct adc_consumer - ADC consumer ++ * @dev: Pointer to ADC device provider ++ * @evt: Pointer to event descriptor ++ * @channel: ADC consumer channel ++ * @link: ADC consumer chained list pointer ++ */ ++struct adc_consumer { ++ struct adc_device *dev; ++ struct adc_evt *evt; ++ uint32_t channel; ++ SLIST_ENTRY(adc_consumer) link; ++}; ++ ++/** ++ * struct adc_ops - ADC driver callbacks ++ * @read_channel: function to read a data from an ADC device channel ++ */ ++struct adc_ops { ++ TEE_Result (*read_channel)(struct adc_device *dev, uint32_t channel, ++ uint32_t *data); ++ TEE_Result (*set_event)(struct adc_device *dev, struct adc_evt *evt, ++ uint32_t channel); ++ TEE_Result (*clr_event)(struct adc_device *dev, struct adc_evt *evt, ++ uint32_t channel); ++ TEE_Result (*start_conv)(struct adc_device *dev, uint32_t channel_mask); ++ TEE_Result (*stop_conv)(struct adc_device *dev); ++}; ++ ++static inline void *adc_get_drv_data(struct adc_device *dev) ++{ ++ return dev->data; ++} ++ ++static inline void adc_set_drv_data(struct adc_device *dev, void *data) ++{ ++ dev->data = data; ++} ++ ++static inline void adc_register(struct adc_device *dev, struct adc_ops *ops) ++{ ++ dev->ops = ops; ++ mutex_init(&dev->lock); ++} ++ ++static inline void adc_unregister(struct adc_device *dev) ++{ ++ dev->ops = NULL; ++} ++ ++/** ++ * adc_get_by_name() - get ADC device handle from ADC name ++ * ++ * @adc_name: ADC instance name ++ * @adc_dev: [out] Pointer to the ADC device handle ++ * Return: TEE_SUCCESS on success, error code otherwise ++ */ ++TEE_Result adc_get_by_name(const char *adc_name, struct adc_device **adc_dev); ++ ++/** ++ * adc_trylock() - try to get lock on an ADC device ++ * ++ * @dev: ADC device ++ * Return: TEE_SUCCESS on success, error code otherwise ++ */ ++TEE_Result adc_trylock(struct adc_device *dev); ++ ++/** ++ * adc_unlock() - try to get lock on an ADC device ++ * ++ * @dev: ADC device ++ */ ++void adc_unlock(struct adc_device *dev); ++ ++/** ++ * adc_clr_event_all() - clear an event on all consumers of an ADC device ++ * ++ * @adc_name: ADC instance name ++ * @id: event identifier ++ * @Return: TEE_SUCCESS on success, error code otherwise ++ */ ++TEE_Result adc_clr_event_all(const char *adc_name, unsigned int id); ++ ++/** ++ * adc_consumer_get_by_name() - get consumer from ADC name and channel index ++ * ++ * @adc_name: ADC instance name ++ * @channel: ADC channel number ++ * @cons: [out] Pointer to the ADC consumer handle ++ * Return: TEE_SUCCESS on success, error code otherwise ++ */ ++TEE_Result adc_consumer_get_by_name(const char *adc_name, uint32_t channel, ++ struct adc_consumer **cons); ++ ++/** ++ * adc_consumer_print_list() - display the list of registered ADC consumers ++ */ ++void adc_consumer_print_list(void); ++ ++/** ++ * adc_print_info() - display ADC device info ++ * ++ * @adc_name: ADC instance name ++ * Return: TEE_SUCCESS on success, error code otherwise ++ */ ++TEE_Result adc_print_info(const char *adc_name); ++ ++/** ++ * adc_consumer_register() - register an ADC device consumer in ADC framework ++ * ++ * @dev: ADC device ++ * @channel: ADC channel number ++ * @cons: [out] Pointer to the registered ADC consumer handle ++ * Return: TEE_SUCCESS on success, error code otherwise ++ */ ++TEE_Result adc_consumer_register(struct adc_device *dev, uint32_t channel, ++ struct adc_consumer **cons); ++ ++/** ++ * adc_consumer_get_all() - get all ADC consumers for node ++ * ++ * @fdt: Pointer to the device tree blob ++ * @node: Offset of a tree node ++ * @nb_cons: [out] Pointer to ADC consumers number in @cons array ++ * @cons: [out] Pointer to consumers pointer array ++ * Return: TEE_SUCCESS on success, error code otherwise ++ */ ++TEE_Result adc_consumer_get_all(const void *fdt, int node, size_t *nb_cons, ++ struct adc_consumer ***cons); ++ ++/** ++ * adc_consumer_read_processed() - launch a single conversion on an ADC channel ++ * ++ * @cons: ADC consumer handle ++ * @uv: [out] Pointer to the converted data in uV ++ * Return: TEE_SUCCESS on success, error code otherwise ++ */ ++TEE_Result adc_consumer_read_processed(struct adc_consumer *cons, int32_t *uv); ++ ++/** ++ * adc_consumer_set_event() - configure an event for an ADC consumer ++ * ++ * @cons: ADC consumer handle ++ * @evt: Event descriptor pointer ++ * @Return: TEE_SUCCESS on success, error code otherwise ++ */ ++TEE_Result adc_consumer_set_event(struct adc_consumer *cons, ++ struct adc_evt *evt); ++ ++/** ++ * adc_consumer_clr_event() - clear an event for an ADC consumer ++ * ++ * @cons: ADC consumer handle ++ * @evt: Event descriptor pointer ++ * @Return: TEE_SUCCESS on success, error code otherwise ++ */ ++TEE_Result adc_consumer_clr_event(struct adc_consumer *cons, ++ struct adc_evt *evt); ++ ++/** ++ * adc_consumer_start_conv() - launch channel(s) conversions on an ADC device ++ * ++ * @dev: ADC device handle ++ * @channel_mask: ADC channel mask ++ * @Return: TEE_SUCCESS on success, error code otherwise ++ */ ++TEE_Result adc_consumer_start_conv(struct adc_device *dev, ++ uint32_t channel_mask); ++ ++/** ++ * adc_consumer_stop_conv() - stop channel(s) conversions on an ADC device ++ * ++ * @dev: ADC device handle ++ * @Return: TEE_SUCCESS on success, error code otherwise ++ */ ++TEE_Result adc_consumer_stop_conv(struct adc_device *dev); ++#endif /* __DRIVERS_ADC_H */ +diff --git a/core/include/drivers/stm32_adc_core.h b/core/include/drivers/stm32_adc_core.h +new file mode 100644 +index 0000000000..e6b2a1011e +--- /dev/null ++++ b/core/include/drivers/stm32_adc_core.h +@@ -0,0 +1,37 @@ ++/* SPDX-License-Identifier: BSD-2-Clause */ ++/* ++ * Copyright (c) 2022, STMicroelectronics ++ */ ++ ++#ifndef __DRIVERS_STM32_ADC_CORE_H ++#define __DRIVERS_STM32_ADC_CORE_H ++ ++#define STM32_ADCX_COMN_OFFSET U(0x300) ++ ++/* STM32MP13 - common registers for all ADC instances */ ++#define STM32MP13_ADC_CSR (STM32_ADCX_COMN_OFFSET + 0x00) ++#define STM32MP13_ADC_CCR (STM32_ADCX_COMN_OFFSET + 0x08) ++ ++/* STM32MP13_ADC_CSR - bit fields */ ++#define STM32MP13_AWD3 BIT(9) ++#define STM32MP13_AWD2 BIT(8) ++#define STM32MP13_AWD1 BIT(7) ++ ++/* STM32MP13_ADC_CCR - bit fields */ ++#define STM32MP13_VBATEN BIT(24) ++#define STM32MP13_VREFEN BIT(22) ++#define STM32MP13_PRESC_SHIFT U(18) ++#define STM32MP13_PRESC_MASK GENMASK_32(21, 18) ++#define STM32MP13_CKMODE_SHIFT U(16) ++#define STM32MP13_CKMODE_MASK GENMASK_32(17, 16) ++ ++struct stm32_adc_core_device; ++ ++struct stm32_adc_common { ++ struct stm32_adc_core_device *dev; ++ vaddr_t regs; ++ unsigned long rate; ++ uint16_t vref_mv; ++}; ++ ++#endif /* __DRIVERS_STM32_ADC_CORE_H */ +diff --git a/core/include/drivers/stm32mp13_rcc.h b/core/include/drivers/stm32mp13_rcc.h +index 892890f709..447b7010c9 100644 +--- a/core/include/drivers/stm32mp13_rcc.h ++++ b/core/include/drivers/stm32mp13_rcc.h +@@ -6,207 +6,207 @@ + #ifndef __DRIVERS_STM32MP13_RCC_H__ + #define __DRIVERS_STM32MP13_RCC_H__ + +-#define RCC_SECCFGR U(0X0) +-#define RCC_MP_SREQSETR U(0X100) +-#define RCC_MP_SREQCLRR U(0X104) +-#define RCC_MP_APRSTCR U(0X108) +-#define RCC_MP_APRSTSR U(0X10C) +-#define RCC_PWRLPDLYCR U(0X110) +-#define RCC_MP_GRSTCSETR U(0X114) +-#define RCC_BR_RSTSCLRR U(0X118) +-#define RCC_MP_RSTSSETR U(0X11C) +-#define RCC_MP_RSTSCLRR U(0X120) +-#define RCC_MP_IWDGFZSETR U(0X124) +-#define RCC_MP_IWDGFZCLRR U(0X128) +-#define RCC_MP_CIER U(0X200) +-#define RCC_MP_CIFR U(0X204) +-#define RCC_BDCR U(0X400) +-#define RCC_RDLSICR U(0X404) +-#define RCC_OCENSETR U(0X420) +-#define RCC_OCENCLRR U(0X424) +-#define RCC_OCRDYR U(0X428) +-#define RCC_HSICFGR U(0X440) +-#define RCC_CSICFGR U(0X444) +-#define RCC_MCO1CFGR U(0X460) +-#define RCC_MCO2CFGR U(0X464) +-#define RCC_DBGCFGR U(0X468) +-#define RCC_RCK12SELR U(0X480) +-#define RCC_RCK3SELR U(0X484) +-#define RCC_RCK4SELR U(0X488) +-#define RCC_PLL1CR U(0X4A0) +-#define RCC_PLL1CFGR1 U(0X4A4) +-#define RCC_PLL1CFGR2 U(0X4A8) +-#define RCC_PLL1FRACR U(0X4AC) +-#define RCC_PLL1CSGR U(0X4B0) +-#define RCC_PLL2CR U(0X4D0) +-#define RCC_PLL2CFGR1 U(0X4D4) +-#define RCC_PLL2CFGR2 U(0X4D8) +-#define RCC_PLL2FRACR U(0X4DC) +-#define RCC_PLL2CSGR U(0X4E0) +-#define RCC_PLL3CR U(0X500) +-#define RCC_PLL3CFGR1 U(0X504) +-#define RCC_PLL3CFGR2 U(0X508) +-#define RCC_PLL3FRACR U(0X50C) +-#define RCC_PLL3CSGR U(0X510) +-#define RCC_PLL4CR U(0X520) +-#define RCC_PLL4CFGR1 U(0X524) +-#define RCC_PLL4CFGR2 U(0X528) +-#define RCC_PLL4FRACR U(0X52C) +-#define RCC_PLL4CSGR U(0X530) +-#define RCC_MPCKSELR U(0X540) +-#define RCC_ASSCKSELR U(0X544) +-#define RCC_MSSCKSELR U(0X548) +-#define RCC_CPERCKSELR U(0X54C) +-#define RCC_RTCDIVR U(0X560) +-#define RCC_MPCKDIVR U(0X564) +-#define RCC_AXIDIVR U(0X568) +-#define RCC_MLAHBDIVR U(0X56C) +-#define RCC_APB1DIVR U(0X570) +-#define RCC_APB2DIVR U(0X574) +-#define RCC_APB3DIVR U(0X578) +-#define RCC_APB4DIVR U(0X57C) +-#define RCC_APB5DIVR U(0X580) +-#define RCC_APB6DIVR U(0X584) +-#define RCC_TIMG1PRER U(0X5A0) +-#define RCC_TIMG2PRER U(0X5A4) +-#define RCC_TIMG3PRER U(0X5A8) +-#define RCC_DDRITFCR U(0X5C0) +-#define RCC_I2C12CKSELR U(0X600) +-#define RCC_I2C345CKSELR U(0X604) +-#define RCC_SPI2S1CKSELR U(0X608) +-#define RCC_SPI2S23CKSELR U(0X60C) +-#define RCC_SPI45CKSELR U(0X610) +-#define RCC_UART12CKSELR U(0X614) +-#define RCC_UART35CKSELR U(0X618) +-#define RCC_UART4CKSELR U(0X61C) +-#define RCC_UART6CKSELR U(0X620) +-#define RCC_UART78CKSELR U(0X624) +-#define RCC_LPTIM1CKSELR U(0X628) +-#define RCC_LPTIM23CKSELR U(0X62C) +-#define RCC_LPTIM45CKSELR U(0X630) +-#define RCC_SAI1CKSELR U(0X634) +-#define RCC_SAI2CKSELR U(0X638) +-#define RCC_FDCANCKSELR U(0X63C) +-#define RCC_SPDIFCKSELR U(0X640) +-#define RCC_ADC12CKSELR U(0X644) +-#define RCC_SDMMC12CKSELR U(0X648) +-#define RCC_ETH12CKSELR U(0X64C) +-#define RCC_USBCKSELR U(0X650) +-#define RCC_QSPICKSELR U(0X654) +-#define RCC_FMCCKSELR U(0X658) +-#define RCC_RNG1CKSELR U(0X65C) +-#define RCC_STGENCKSELR U(0X660) +-#define RCC_DCMIPPCKSELR U(0X664) +-#define RCC_SAESCKSELR U(0X668) +-#define RCC_APB1RSTSETR U(0X6A0) +-#define RCC_APB1RSTCLRR U(0X6A4) +-#define RCC_APB2RSTSETR U(0X6A8) +-#define RCC_APB2RSTCLRR U(0X6AC) +-#define RCC_APB3RSTSETR U(0X6B0) +-#define RCC_APB3RSTCLRR U(0X6B4) +-#define RCC_APB4RSTSETR U(0X6B8) +-#define RCC_APB4RSTCLRR U(0X6BC) +-#define RCC_APB5RSTSETR U(0X6C0) +-#define RCC_APB5RSTCLRR U(0X6C4) +-#define RCC_APB6RSTSETR U(0X6C8) +-#define RCC_APB6RSTCLRR U(0X6CC) +-#define RCC_AHB2RSTSETR U(0X6D0) +-#define RCC_AHB2RSTCLRR U(0X6D4) +-#define RCC_AHB4RSTSETR U(0X6E0) +-#define RCC_AHB4RSTCLRR U(0X6E4) +-#define RCC_AHB5RSTSETR U(0X6E8) +-#define RCC_AHB5RSTCLRR U(0X6EC) +-#define RCC_AHB6RSTSETR U(0X6F0) +-#define RCC_AHB6RSTCLRR U(0X6F4) +-#define RCC_MP_APB1ENSETR U(0X700) +-#define RCC_MP_APB1ENCLRR U(0X704) +-#define RCC_MP_APB2ENSETR U(0X708) +-#define RCC_MP_APB2ENCLRR U(0X70C) +-#define RCC_MP_APB3ENSETR U(0X710) +-#define RCC_MP_APB3ENCLRR U(0X714) +-#define RCC_MP_S_APB3ENSETR U(0X718) +-#define RCC_MP_S_APB3ENCLRR U(0X71C) +-#define RCC_MP_NS_APB3ENSETR U(0X720) +-#define RCC_MP_NS_APB3ENCLRR U(0X724) +-#define RCC_MP_APB4ENSETR U(0X728) +-#define RCC_MP_APB4ENCLRR U(0X72C) +-#define RCC_MP_S_APB4ENSETR U(0X730) +-#define RCC_MP_S_APB4ENCLRR U(0X734) +-#define RCC_MP_NS_APB4ENSETR U(0X738) +-#define RCC_MP_NS_APB4ENCLRR U(0X73C) +-#define RCC_MP_APB5ENSETR U(0X740) +-#define RCC_MP_APB5ENCLRR U(0X744) +-#define RCC_MP_APB6ENSETR U(0X748) +-#define RCC_MP_APB6ENCLRR U(0X74C) +-#define RCC_MP_AHB2ENSETR U(0X750) +-#define RCC_MP_AHB2ENCLRR U(0X754) +-#define RCC_MP_AHB4ENSETR U(0X760) +-#define RCC_MP_AHB4ENCLRR U(0X764) +-#define RCC_MP_S_AHB4ENSETR U(0X768) +-#define RCC_MP_S_AHB4ENCLRR U(0X76C) +-#define RCC_MP_NS_AHB4ENSETR U(0X770) +-#define RCC_MP_NS_AHB4ENCLRR U(0X774) +-#define RCC_MP_AHB5ENSETR U(0X778) +-#define RCC_MP_AHB5ENCLRR U(0X77C) +-#define RCC_MP_AHB6ENSETR U(0X780) +-#define RCC_MP_AHB6ENCLRR U(0X784) +-#define RCC_MP_S_AHB6ENSETR U(0X788) +-#define RCC_MP_S_AHB6ENCLRR U(0X78C) +-#define RCC_MP_NS_AHB6ENSETR U(0X790) +-#define RCC_MP_NS_AHB6ENCLRR U(0X794) +-#define RCC_MP_APB1LPENSETR U(0X800) +-#define RCC_MP_APB1LPENCLRR U(0X804) +-#define RCC_MP_APB2LPENSETR U(0X808) +-#define RCC_MP_APB2LPENCLRR U(0X80C) +-#define RCC_MP_APB3LPENSETR U(0X810) +-#define RCC_MP_APB3LPENCLRR U(0X814) +-#define RCC_MP_S_APB3LPENSETR U(0X818) +-#define RCC_MP_S_APB3LPENCLRR U(0X81C) +-#define RCC_MP_NS_APB3LPENSETR U(0X820) +-#define RCC_MP_NS_APB3LPENCLRR U(0X824) +-#define RCC_MP_APB4LPENSETR U(0X828) +-#define RCC_MP_APB4LPENCLRR U(0X82C) +-#define RCC_MP_S_APB4LPENSETR U(0X830) +-#define RCC_MP_S_APB4LPENCLRR U(0X834) +-#define RCC_MP_NS_APB4LPENSETR U(0X838) +-#define RCC_MP_NS_APB4LPENCLRR U(0X83C) +-#define RCC_MP_APB5LPENSETR U(0X840) +-#define RCC_MP_APB5LPENCLRR U(0X844) +-#define RCC_MP_APB6LPENSETR U(0X848) +-#define RCC_MP_APB6LPENCLRR U(0X84C) +-#define RCC_MP_AHB2LPENSETR U(0X850) +-#define RCC_MP_AHB2LPENCLRR U(0X854) +-#define RCC_MP_AHB4LPENSETR U(0X858) +-#define RCC_MP_AHB4LPENCLRR U(0X85C) +-#define RCC_MP_S_AHB4LPENSETR U(0X868) +-#define RCC_MP_S_AHB4LPENCLRR U(0X86C) +-#define RCC_MP_NS_AHB4LPENSETR U(0X870) +-#define RCC_MP_NS_AHB4LPENCLRR U(0X874) +-#define RCC_MP_AHB5LPENSETR U(0X878) +-#define RCC_MP_AHB5LPENCLRR U(0X87C) +-#define RCC_MP_AHB6LPENSETR U(0X880) +-#define RCC_MP_AHB6LPENCLRR U(0X884) +-#define RCC_MP_S_AHB6LPENSETR U(0X888) +-#define RCC_MP_S_AHB6LPENCLRR U(0X88C) +-#define RCC_MP_NS_AHB6LPENSETR U(0X890) +-#define RCC_MP_NS_AHB6LPENCLRR U(0X894) +-#define RCC_MP_S_AXIMLPENSETR U(0X898) +-#define RCC_MP_S_AXIMLPENCLRR U(0X89C) +-#define RCC_MP_NS_AXIMLPENSETR U(0X8A0) +-#define RCC_MP_NS_AXIMLPENCLRR U(0X8A4) +-#define RCC_MP_MLAHBLPENSETR U(0X8A8) +-#define RCC_MP_MLAHBLPENCLRR U(0X8AC) +-#define RCC_APB3SECSR U(0X8C0) +-#define RCC_APB4SECSR U(0X8C4) +-#define RCC_APB5SECSR U(0X8C8) +-#define RCC_APB6SECSR U(0X8CC) +-#define RCC_AHB2SECSR U(0X8D0) +-#define RCC_AHB4SECSR U(0X8D4) +-#define RCC_AHB5SECSR U(0X8D8) +-#define RCC_AHB6SECSR U(0X8DC) +-#define RCC_VERR U(0XFF4) +-#define RCC_IDR U(0XFF8) +-#define RCC_SIDR U(0XFFC) ++#define RCC_SECCFGR U(0x0) ++#define RCC_MP_SREQSETR U(0x100) ++#define RCC_MP_SREQCLRR U(0x104) ++#define RCC_MP_APRSTCR U(0x108) ++#define RCC_MP_APRSTSR U(0x10C) ++#define RCC_PWRLPDLYCR U(0x110) ++#define RCC_MP_GRSTCSETR U(0x114) ++#define RCC_BR_RSTSCLRR U(0x118) ++#define RCC_MP_RSTSSETR U(0x11C) ++#define RCC_MP_RSTSCLRR U(0x120) ++#define RCC_MP_IWDGFZSETR U(0x124) ++#define RCC_MP_IWDGFZCLRR U(0x128) ++#define RCC_MP_CIER U(0x200) ++#define RCC_MP_CIFR U(0x204) ++#define RCC_BDCR U(0x400) ++#define RCC_RDLSICR U(0x404) ++#define RCC_OCENSETR U(0x420) ++#define RCC_OCENCLRR U(0x424) ++#define RCC_OCRDYR U(0x428) ++#define RCC_HSICFGR U(0x440) ++#define RCC_CSICFGR U(0x444) ++#define RCC_MCO1CFGR U(0x460) ++#define RCC_MCO2CFGR U(0x464) ++#define RCC_DBGCFGR U(0x468) ++#define RCC_RCK12SELR U(0x480) ++#define RCC_RCK3SELR U(0x484) ++#define RCC_RCK4SELR U(0x488) ++#define RCC_PLL1CR U(0x4A0) ++#define RCC_PLL1CFGR1 U(0x4A4) ++#define RCC_PLL1CFGR2 U(0x4A8) ++#define RCC_PLL1FRACR U(0x4AC) ++#define RCC_PLL1CSGR U(0x4B0) ++#define RCC_PLL2CR U(0x4D0) ++#define RCC_PLL2CFGR1 U(0x4D4) ++#define RCC_PLL2CFGR2 U(0x4D8) ++#define RCC_PLL2FRACR U(0x4DC) ++#define RCC_PLL2CSGR U(0x4E0) ++#define RCC_PLL3CR U(0x500) ++#define RCC_PLL3CFGR1 U(0x504) ++#define RCC_PLL3CFGR2 U(0x508) ++#define RCC_PLL3FRACR U(0x50C) ++#define RCC_PLL3CSGR U(0x510) ++#define RCC_PLL4CR U(0x520) ++#define RCC_PLL4CFGR1 U(0x524) ++#define RCC_PLL4CFGR2 U(0x528) ++#define RCC_PLL4FRACR U(0x52C) ++#define RCC_PLL4CSGR U(0x530) ++#define RCC_MPCKSELR U(0x540) ++#define RCC_ASSCKSELR U(0x544) ++#define RCC_MSSCKSELR U(0x548) ++#define RCC_CPERCKSELR U(0x54C) ++#define RCC_RTCDIVR U(0x560) ++#define RCC_MPCKDIVR U(0x564) ++#define RCC_AXIDIVR U(0x568) ++#define RCC_MLAHBDIVR U(0x56C) ++#define RCC_APB1DIVR U(0x570) ++#define RCC_APB2DIVR U(0x574) ++#define RCC_APB3DIVR U(0x578) ++#define RCC_APB4DIVR U(0x57C) ++#define RCC_APB5DIVR U(0x580) ++#define RCC_APB6DIVR U(0x584) ++#define RCC_TIMG1PRER U(0x5A0) ++#define RCC_TIMG2PRER U(0x5A4) ++#define RCC_TIMG3PRER U(0x5A8) ++#define RCC_DDRITFCR U(0x5C0) ++#define RCC_I2C12CKSELR U(0x600) ++#define RCC_I2C345CKSELR U(0x604) ++#define RCC_SPI2S1CKSELR U(0x608) ++#define RCC_SPI2S23CKSELR U(0x60C) ++#define RCC_SPI45CKSELR U(0x610) ++#define RCC_UART12CKSELR U(0x614) ++#define RCC_UART35CKSELR U(0x618) ++#define RCC_UART4CKSELR U(0x61C) ++#define RCC_UART6CKSELR U(0x620) ++#define RCC_UART78CKSELR U(0x624) ++#define RCC_LPTIM1CKSELR U(0x628) ++#define RCC_LPTIM23CKSELR U(0x62C) ++#define RCC_LPTIM45CKSELR U(0x630) ++#define RCC_SAI1CKSELR U(0x634) ++#define RCC_SAI2CKSELR U(0x638) ++#define RCC_FDCANCKSELR U(0x63C) ++#define RCC_SPDIFCKSELR U(0x640) ++#define RCC_ADC12CKSELR U(0x644) ++#define RCC_SDMMC12CKSELR U(0x648) ++#define RCC_ETH12CKSELR U(0x64C) ++#define RCC_USBCKSELR U(0x650) ++#define RCC_QSPICKSELR U(0x654) ++#define RCC_FMCCKSELR U(0x658) ++#define RCC_RNG1CKSELR U(0x65C) ++#define RCC_STGENCKSELR U(0x660) ++#define RCC_DCMIPPCKSELR U(0x664) ++#define RCC_SAESCKSELR U(0x668) ++#define RCC_APB1RSTSETR U(0x6A0) ++#define RCC_APB1RSTCLRR U(0x6A4) ++#define RCC_APB2RSTSETR U(0x6A8) ++#define RCC_APB2RSTCLRR U(0x6AC) ++#define RCC_APB3RSTSETR U(0x6B0) ++#define RCC_APB3RSTCLRR U(0x6B4) ++#define RCC_APB4RSTSETR U(0x6B8) ++#define RCC_APB4RSTCLRR U(0x6BC) ++#define RCC_APB5RSTSETR U(0x6C0) ++#define RCC_APB5RSTCLRR U(0x6C4) ++#define RCC_APB6RSTSETR U(0x6C8) ++#define RCC_APB6RSTCLRR U(0x6CC) ++#define RCC_AHB2RSTSETR U(0x6D0) ++#define RCC_AHB2RSTCLRR U(0x6D4) ++#define RCC_AHB4RSTSETR U(0x6E0) ++#define RCC_AHB4RSTCLRR U(0x6E4) ++#define RCC_AHB5RSTSETR U(0x6E8) ++#define RCC_AHB5RSTCLRR U(0x6EC) ++#define RCC_AHB6RSTSETR U(0x6F0) ++#define RCC_AHB6RSTCLRR U(0x6F4) ++#define RCC_MP_APB1ENSETR U(0x700) ++#define RCC_MP_APB1ENCLRR U(0x704) ++#define RCC_MP_APB2ENSETR U(0x708) ++#define RCC_MP_APB2ENCLRR U(0x70C) ++#define RCC_MP_APB3ENSETR U(0x710) ++#define RCC_MP_APB3ENCLRR U(0x714) ++#define RCC_MP_S_APB3ENSETR U(0x718) ++#define RCC_MP_S_APB3ENCLRR U(0x71C) ++#define RCC_MP_NS_APB3ENSETR U(0x720) ++#define RCC_MP_NS_APB3ENCLRR U(0x724) ++#define RCC_MP_APB4ENSETR U(0x728) ++#define RCC_MP_APB4ENCLRR U(0x72C) ++#define RCC_MP_S_APB4ENSETR U(0x730) ++#define RCC_MP_S_APB4ENCLRR U(0x734) ++#define RCC_MP_NS_APB4ENSETR U(0x738) ++#define RCC_MP_NS_APB4ENCLRR U(0x73C) ++#define RCC_MP_APB5ENSETR U(0x740) ++#define RCC_MP_APB5ENCLRR U(0x744) ++#define RCC_MP_APB6ENSETR U(0x748) ++#define RCC_MP_APB6ENCLRR U(0x74C) ++#define RCC_MP_AHB2ENSETR U(0x750) ++#define RCC_MP_AHB2ENCLRR U(0x754) ++#define RCC_MP_AHB4ENSETR U(0x760) ++#define RCC_MP_AHB4ENCLRR U(0x764) ++#define RCC_MP_S_AHB4ENSETR U(0x768) ++#define RCC_MP_S_AHB4ENCLRR U(0x76C) ++#define RCC_MP_NS_AHB4ENSETR U(0x770) ++#define RCC_MP_NS_AHB4ENCLRR U(0x774) ++#define RCC_MP_AHB5ENSETR U(0x778) ++#define RCC_MP_AHB5ENCLRR U(0x77C) ++#define RCC_MP_AHB6ENSETR U(0x780) ++#define RCC_MP_AHB6ENCLRR U(0x784) ++#define RCC_MP_S_AHB6ENSETR U(0x788) ++#define RCC_MP_S_AHB6ENCLRR U(0x78C) ++#define RCC_MP_NS_AHB6ENSETR U(0x790) ++#define RCC_MP_NS_AHB6ENCLRR U(0x794) ++#define RCC_MP_APB1LPENSETR U(0x800) ++#define RCC_MP_APB1LPENCLRR U(0x804) ++#define RCC_MP_APB2LPENSETR U(0x808) ++#define RCC_MP_APB2LPENCLRR U(0x80C) ++#define RCC_MP_APB3LPENSETR U(0x810) ++#define RCC_MP_APB3LPENCLRR U(0x814) ++#define RCC_MP_S_APB3LPENSETR U(0x818) ++#define RCC_MP_S_APB3LPENCLRR U(0x81C) ++#define RCC_MP_NS_APB3LPENSETR U(0x820) ++#define RCC_MP_NS_APB3LPENCLRR U(0x824) ++#define RCC_MP_APB4LPENSETR U(0x828) ++#define RCC_MP_APB4LPENCLRR U(0x82C) ++#define RCC_MP_S_APB4LPENSETR U(0x830) ++#define RCC_MP_S_APB4LPENCLRR U(0x834) ++#define RCC_MP_NS_APB4LPENSETR U(0x838) ++#define RCC_MP_NS_APB4LPENCLRR U(0x83C) ++#define RCC_MP_APB5LPENSETR U(0x840) ++#define RCC_MP_APB5LPENCLRR U(0x844) ++#define RCC_MP_APB6LPENSETR U(0x848) ++#define RCC_MP_APB6LPENCLRR U(0x84C) ++#define RCC_MP_AHB2LPENSETR U(0x850) ++#define RCC_MP_AHB2LPENCLRR U(0x854) ++#define RCC_MP_AHB4LPENSETR U(0x858) ++#define RCC_MP_AHB4LPENCLRR U(0x85C) ++#define RCC_MP_S_AHB4LPENSETR U(0x868) ++#define RCC_MP_S_AHB4LPENCLRR U(0x86C) ++#define RCC_MP_NS_AHB4LPENSETR U(0x870) ++#define RCC_MP_NS_AHB4LPENCLRR U(0x874) ++#define RCC_MP_AHB5LPENSETR U(0x878) ++#define RCC_MP_AHB5LPENCLRR U(0x87C) ++#define RCC_MP_AHB6LPENSETR U(0x880) ++#define RCC_MP_AHB6LPENCLRR U(0x884) ++#define RCC_MP_S_AHB6LPENSETR U(0x888) ++#define RCC_MP_S_AHB6LPENCLRR U(0x88C) ++#define RCC_MP_NS_AHB6LPENSETR U(0x890) ++#define RCC_MP_NS_AHB6LPENCLRR U(0x894) ++#define RCC_MP_S_AXIMLPENSETR U(0x898) ++#define RCC_MP_S_AXIMLPENCLRR U(0x89C) ++#define RCC_MP_NS_AXIMLPENSETR U(0x8A0) ++#define RCC_MP_NS_AXIMLPENCLRR U(0x8A4) ++#define RCC_MP_MLAHBLPENSETR U(0x8A8) ++#define RCC_MP_MLAHBLPENCLRR U(0x8AC) ++#define RCC_APB3SECSR U(0x8C0) ++#define RCC_APB4SECSR U(0x8C4) ++#define RCC_APB5SECSR U(0x8C8) ++#define RCC_APB6SECSR U(0x8CC) ++#define RCC_AHB2SECSR U(0x8D0) ++#define RCC_AHB4SECSR U(0x8D4) ++#define RCC_AHB5SECSR U(0x8D8) ++#define RCC_AHB6SECSR U(0x8DC) ++#define RCC_VERR U(0xFF4) ++#define RCC_IDR U(0xFF8) ++#define RCC_SIDR U(0xFFC) + + /* RCC_SECCFGR register fields */ + #define RCC_SECCFGR_HSISEC BIT(0) +diff --git a/core/include/dt-bindings/clock/stm32mp13-clks.h b/core/include/dt-bindings/clock/stm32mp13-clks.h +index 3e8d8eeb9f..c84ba2c7a7 100644 +--- a/core/include/dt-bindings/clock/stm32mp13-clks.h ++++ b/core/include/dt-bindings/clock/stm32mp13-clks.h +@@ -1,7 +1,7 @@ + /* SPDX-License-Identifier: GPL-2.0+ or BSD-3-Clause */ + /* +- * Copyright (C) STMicroelectronics 2020 - All Rights Reserved +- * Author: Gabriel Fernandez for STMicroelectronics. ++ * Copyright (C) STMicroelectronics 2022 - All Rights Reserved ++ * Author: Gabriel Fernandez + */ + + #ifndef _DT_BINDINGS_STM32MP13_CLKS_H_ +@@ -48,8 +48,6 @@ + #define PCLK5 30 + #define PCLK6 31 + +- +- + /* BASE TIMER */ + #define CK_TIMG1 32 + #define CK_TIMG2 33 +@@ -222,9 +220,9 @@ + #define CK_SCMI_PCLK4 21 + #define CK_SCMI_PCLK5 22 + #define CK_SCMI_PCLK6 23 +-#define CK_SCMI_CKTIMG1 24 +-#define CK_SCMI_CKTIMG2 25 +-#define CK_SCMI_CKTIMG3 26 ++#define CK_SCMI_CKTIMG1 24 ++#define CK_SCMI_CKTIMG2 25 ++#define CK_SCMI_CKTIMG3 26 + #define CK_SCMI_RTC 27 + #define CK_SCMI_RTCAPB 28 + #define CK_SCMI_BSEC 29 +diff --git a/core/include/dt-bindings/clock/stm32mp13-clksrc.h b/core/include/dt-bindings/clock/stm32mp13-clksrc.h +index d3ff566b0b..19c18335b6 100644 +--- a/core/include/dt-bindings/clock/stm32mp13-clksrc.h ++++ b/core/include/dt-bindings/clock/stm32mp13-clksrc.h +@@ -1,7 +1,7 @@ ++/* SPDX-License-Identifier: GPL-2.0+ or BSD-3-Clause */ + /* +- * Copyright (C) 2018-2020, STMicroelectronics - All Rights Reserved +- * +- * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause ++ * Copyright (C) STMicroelectronics 2022 - All Rights Reserved ++ * Author: Gabriel Fernandez + */ + + #ifndef _DT_BINDINGS_CLOCK_STM32MP13_CLKSRC_H_ +@@ -10,7 +10,6 @@ + #define CMD_DIV 0 + #define CMD_MUX 1 + #define CMD_CLK 2 +-#define CMD_RESERVED1 3 + + #define CMD_SHIFT 26 + #define CMD_MASK 0xFC000000 +@@ -66,25 +65,17 @@ + #define DIV_NB 26 + + #define DIV(div_id, div) ((CMD_DIV << CMD_SHIFT) |\ +- ((div_id) << DIV_ID_SHIFT |\ +- (div))) ++ ((div_id) << DIV_ID_SHIFT) |\ ++ (div)) + + #define CLKSRC(mux_id, sel) ((CMD_MUX << CMD_SHIFT) |\ +- ((mux_id) << MUX_ID_SHIFT |\ +- (sel))) +- +-/* MCO output is enable */ +-#define MCO_SRC(mco_id, sel) ((CMD_CLK << CMD_SHIFT) |\ +- (((mco_id) << CLK_ID_SHIFT) |\ +- (sel)) | CLK_ON_MASK) +- +-#define MCO_DISABLED(mco_id) ((CMD_CLK << CMD_SHIFT) |\ +- ((mco_id) << CLK_ID_SHIFT)) ++ ((mux_id) << MUX_ID_SHIFT) |\ ++ (sel)) + + /* CLK output is enable */ + #define CLK_SRC(clk_id, sel) ((CMD_CLK << CMD_SHIFT) |\ +- (((clk_id) << CLK_ID_SHIFT) |\ +- (sel)) | CLK_ON_MASK) ++ ((clk_id) << CLK_ID_SHIFT) |\ ++ (sel) | CLK_ON_MASK) + + #define CLK_DISABLED(clk_id) ((CMD_CLK << CMD_SHIFT) |\ + ((clk_id) << CLK_ID_SHIFT)) +@@ -380,9 +371,6 @@ + #define CLK_SAES_PLL4R CLKSRC(MUX_SAES, 2) + #define CLK_SAES_LSI CLKSRC(MUX_SAES, 3) + +-/* PLL output is enable when x=1, with x=p,q or r */ +-#define PQR(p, q, r) (((p) & 1) | (((q) & 1) << 1) | (((r) & 1) << 2)) +- + /* define for st,pll /csg */ + #define SSCG_MODE_CENTER_SPREAD 0 + #define SSCG_MODE_DOWN_SPREAD 1 +diff --git a/core/include/dt-bindings/reset/stm32mp13-resets.h b/core/include/dt-bindings/reset/stm32mp13-resets.h +index 934864e90d..3dfc731495 100644 +--- a/core/include/dt-bindings/reset/stm32mp13-resets.h ++++ b/core/include/dt-bindings/reset/stm32mp13-resets.h +@@ -1,7 +1,7 @@ + /* SPDX-License-Identifier: GPL-2.0 or BSD-3-Clause */ + /* +- * Copyright (C) STMicroelectronics 2018 - All Rights Reserved +- * Author: Gabriel Fernandez for STMicroelectronics. ++ * Copyright (C) STMicroelectronics 2022 - All Rights Reserved ++ * Author: Gabriel Fernandez + */ + + #ifndef _DT_BINDINGS_STM32MP13_RESET_H_ +diff --git a/core/include/kernel/dt.h b/core/include/kernel/dt.h +index 4cd195cb12..45a0554888 100644 +--- a/core/include/kernel/dt.h ++++ b/core/include/kernel/dt.h +@@ -69,6 +69,7 @@ enum dt_driver_type { + DT_DRIVER_RSTCTRL, + DT_DRIVER_PINCTRL, + DT_DRIVER_I2C, ++ DT_DRIVER_ADC, + }; + + /* +diff --git a/core/include/kernel/notif.h b/core/include/kernel/notif.h +index 71b53d8eac..28c9fc9c81 100644 +--- a/core/include/kernel/notif.h ++++ b/core/include/kernel/notif.h +@@ -38,6 +38,11 @@ + + #define NOTIF_VALUE_DO_BOTTOM_HALF 0 + ++/* NOTIF_VALUE_DO_IT notify that an IT is pending */ ++#define NOTIF_VALUE_DO_IT 1 ++ ++#define NOTIF_IT_VALUE_MAX U(31) ++ + /* + * enum notif_event - Notification of an event + * @NOTIF_EVENT_STARTED: Delivered in an atomic context to inform +@@ -111,10 +116,15 @@ TEE_Result notif_wait(uint32_t value); + */ + #if defined(CFG_CORE_ASYNC_NOTIF) + void notif_send_async(uint32_t value); ++void notif_send_it(uint32_t it_value); + #else + static inline void notif_send_async(uint32_t value __unused) + { + } ++ ++static inline void notif_send_it(uint32_t value __unused) ++{ ++} + #endif + + /* +@@ -143,6 +153,8 @@ static inline void notif_unregister_driver(struct notif_driver *ndrv __unused) + /* This is called from a fast call */ + #if defined(CFG_CORE_ASYNC_NOTIF) + uint32_t notif_get_value(bool *value_valid, bool *value_pending); ++uint32_t it_get_value(bool *value_valid, bool *value_pending); ++uint32_t it_set_mask(uint32_t it_value, bool masked); + #else + static inline uint32_t notif_get_value(bool *value_valid, bool *value_pending) + { +@@ -150,6 +162,19 @@ static inline uint32_t notif_get_value(bool *value_valid, bool *value_pending) + *value_pending = false; + return UINT32_MAX; + } ++ ++static inline uint32_t it_get_value(bool *value_valid, bool *value_pending) ++{ ++ *value_valid = false; ++ *value_pending = false; ++ return UINT32_MAX; ++} ++ ++static inline uint32_t it_set_mask(uint32_t it_value __unused, ++ bool masked __unused) ++{ ++ return 0; ++} + #endif + + /* +diff --git a/core/kernel/dt.c b/core/kernel/dt.c +index 5dfcf6b792..15d71dfe6e 100644 +--- a/core/kernel/dt.c ++++ b/core/kernel/dt.c +@@ -147,9 +147,6 @@ static paddr_t _fdt_read_paddr(const uint32_t *cell, int n) + #endif + } + +- if (!addr) +- goto bad; +- + return addr; + bad: + return DT_INFO_INVALID_REG; +diff --git a/core/kernel/dt_driver.c b/core/kernel/dt_driver.c +index e4d1417098..e54c60cc9e 100644 +--- a/core/kernel/dt_driver.c ++++ b/core/kernel/dt_driver.c +@@ -107,6 +107,7 @@ static void assert_type_is_valid(enum dt_driver_type type) + case DT_DRIVER_UART: + case DT_DRIVER_PINCTRL: + case DT_DRIVER_I2C: ++ case DT_DRIVER_ADC: + return; + default: + assert(0); +@@ -128,6 +129,7 @@ TEE_Result dt_driver_register_provider(const void *fdt, int nodeoffset, + assert_type_is_valid(type); + switch (type) { + case DT_DRIVER_CLK: ++ case DT_DRIVER_ADC: + provider_cells = fdt_get_dt_driver_cells(fdt, nodeoffset, type); + if (provider_cells < 0) { + DMSG("Failed to find provider cells: %d", +@@ -136,19 +138,26 @@ TEE_Result dt_driver_register_provider(const void *fdt, int nodeoffset, + } + + phandle = fdt_get_phandle(fdt, nodeoffset); +- if (!phandle || phandle == (uint32_t)-1) { ++ if (!phandle) ++ return TEE_SUCCESS; ++ ++ if (phandle == (uint32_t)-1) { + DMSG("Failed to find provide phandle"); + return TEE_ERROR_GENERIC; + } + break; + case DT_DRIVER_PINCTRL: + phandle = fdt_get_phandle(fdt, nodeoffset); +- if (!phandle || phandle == (uint32_t)-1) { ++ if (!phandle) ++ return TEE_SUCCESS; ++ ++ if (phandle == (uint32_t)-1) { + DMSG("Failed to find provide phandle"); + return TEE_ERROR_GENERIC; + } + break; + case DT_DRIVER_I2C: ++ case DT_DRIVER_NOTYPE: + break; + default: + panic("Trying to register unknown type of provider"); +@@ -182,6 +191,9 @@ int fdt_get_dt_driver_cells(const void *fdt, int nodeoffset, + int len = 0; + + switch (type) { ++ case DT_DRIVER_ADC: ++ cells_name = "#io-channel-cells"; ++ break; + case DT_DRIVER_CLK: + cells_name = "#clock-cells"; + break; +diff --git a/core/kernel/huk_subkey.c b/core/kernel/huk_subkey.c +index 03088c9228..5481120b61 100644 +--- a/core/kernel/huk_subkey.c ++++ b/core/kernel/huk_subkey.c +@@ -58,6 +58,7 @@ static TEE_Result huk_compat(void *ctx, enum huk_subkey_usage usage) + } + #endif /*CFG_CORE_HUK_SUBKEY_COMPAT*/ + ++__weak + TEE_Result huk_subkey_derive(enum huk_subkey_usage usage, + const void *const_data, size_t const_data_len, + uint8_t *subkey, size_t subkey_len) +diff --git a/core/kernel/notif.c b/core/kernel/notif.c +index 62ce180f78..ca4f8a0fa7 100644 +--- a/core/kernel/notif.c ++++ b/core/kernel/notif.c +@@ -26,6 +26,10 @@ static bitstr_t bit_decl(notif_values, NOTIF_ASYNC_VALUE_MAX + 1); + static bitstr_t bit_decl(notif_alloc_values, NOTIF_ASYNC_VALUE_MAX + 1); + static bool notif_started; + ++static bitstr_t bit_decl(it_pending, NOTIF_IT_VALUE_MAX + 1); ++static bitstr_t bit_decl(it_masked, NOTIF_IT_VALUE_MAX + 1); ++static unsigned int it_lock = SPINLOCK_UNLOCK; ++ + TEE_Result notif_alloc_async_value(uint32_t *val) + { + static bool alloc_values_inited; +@@ -108,6 +112,78 @@ void notif_send_async(uint32_t value) + cpu_spin_unlock_xrestore(¬if_lock, old_itr_status); + } + ++uint32_t it_get_value(bool *value_valid, bool *value_pending) ++{ ++ uint32_t old_itr_status = 0; ++ uint32_t res = 0; ++ int bit = 0; ++ ++ old_itr_status = cpu_spin_lock_xsave(&it_lock); ++ ++ bit_ffs(it_pending, (int)NOTIF_IT_VALUE_MAX, &bit); ++ ++ *value_valid = (bit >= 0); ++ if (!*value_valid) { ++ *value_pending = false; ++ goto out; ++ } ++ ++ res = bit; ++ bit_clear(it_pending, res); ++ bit_ffs(it_pending, (int)NOTIF_IT_VALUE_MAX, &bit); ++ *value_pending = (bit >= 0); ++out: ++ cpu_spin_unlock_xrestore(&it_lock, old_itr_status); ++ ++ return res; ++} ++ ++uint32_t it_set_mask(uint32_t it_number, bool masked) ++{ ++ uint32_t old_itr_status = 0; ++ uint32_t res = 0; ++ ++ old_itr_status = cpu_spin_lock_xsave(&it_lock); ++ ++ DMSG("it_number=%u masked=%u", it_number, masked); ++ ++ if (it_number >= NOTIF_IT_VALUE_MAX) ++ goto out; ++ ++ if (masked) { ++ bit_set(it_masked, it_number); ++ } else { ++ bit_clear(it_masked, it_number); ++ ++ if (bit_test(it_pending, it_number)) ++ notif_send_async(NOTIF_VALUE_DO_IT); ++ } ++ ++out: ++ cpu_spin_unlock_xrestore(&it_lock, old_itr_status); ++ ++ return res; ++} ++ ++void notif_send_it(uint32_t it_number) ++{ ++ uint32_t old_itr_status = 0; ++ ++ COMPILE_TIME_ASSERT(NOTIF_VALUE_DO_IT == ++ OPTEE_SMC_ASYNC_NOTIF_VALUE_DO_IT); ++ ++ assert(it_number <= NOTIF_IT_VALUE_MAX); ++ old_itr_status = cpu_spin_lock_xsave(&it_lock); ++ ++ DMSG("0x%"PRIx32, it_number); ++ bit_set(it_pending, it_number); ++ ++ if (!bit_test(it_masked, it_number)) ++ notif_send_async(NOTIF_VALUE_DO_IT); ++ ++ cpu_spin_unlock_xrestore(&it_lock, old_itr_status); ++} ++ + bool notif_async_is_started(void) + { + uint32_t old_itr_status = 0; +@@ -129,6 +205,12 @@ void notif_register_driver(struct notif_driver *ndrv) + SLIST_INSERT_HEAD(¬if_driver_head, ndrv, link); + + cpu_spin_unlock_xrestore(¬if_lock, old_itr_status); ++ ++ old_itr_status = cpu_spin_lock_xsave(&it_lock); ++ ++ bit_nset(it_masked, 0, (int)NOTIF_IT_VALUE_MAX); ++ ++ cpu_spin_unlock_xrestore(&it_lock, old_itr_status); + } + + void notif_unregister_driver(struct notif_driver *ndrv) +diff --git a/core/tee/entry_std.c b/core/tee/entry_std.c +index 9ba766293d..b95f15d032 100644 +--- a/core/tee/entry_std.c ++++ b/core/tee/entry_std.c +@@ -358,6 +358,7 @@ static void entry_open_session(struct optee_msg_arg *arg, uint32_t num_params) + TEE_UUID uuid; + struct tee_ta_param param; + size_t num_meta; ++ size_t num_sess_params = 0; + uint64_t saved_attr[TEE_NUM_PARAMS] = { 0 }; + + res = get_open_session_meta(num_params, arg->params, &num_meta, &uuid, +@@ -365,7 +366,13 @@ static void entry_open_session(struct optee_msg_arg *arg, uint32_t num_params) + if (res != TEE_SUCCESS) + goto out; + +- res = copy_in_params(arg->params + num_meta, num_params - num_meta, ++ if (SUB_OVERFLOW(num_params, num_meta, &num_sess_params) || ++ num_sess_params > TEE_NUM_PARAMS) { ++ res = TEE_ERROR_BAD_PARAMETERS; ++ goto out; ++ } ++ ++ res = copy_in_params(arg->params + num_meta, num_sess_params, + ¶m, saved_attr); + if (res != TEE_SUCCESS) + goto cleanup_shm_refs; +@@ -374,7 +381,7 @@ static void entry_open_session(struct optee_msg_arg *arg, uint32_t num_params) + &clnt_id, TEE_TIMEOUT_INFINITE, ¶m); + if (res != TEE_SUCCESS) + s = NULL; +- copy_out_param(¶m, num_params - num_meta, arg->params + num_meta, ++ copy_out_param(¶m, num_sess_params, arg->params + num_meta, + saved_attr); + + /* +@@ -386,7 +393,7 @@ static void entry_open_session(struct optee_msg_arg *arg, uint32_t num_params) + &session_pnum); + + cleanup_shm_refs: +- cleanup_shm_refs(saved_attr, ¶m, num_params - num_meta); ++ cleanup_shm_refs(saved_attr, ¶m, num_sess_params); + + out: + if (s) +@@ -427,14 +434,19 @@ static void entry_invoke_command(struct optee_msg_arg *arg, uint32_t num_params) + + bm_timestamp(); + ++ if (num_params > TEE_NUM_PARAMS) { ++ res = TEE_ERROR_BAD_PARAMETERS; ++ goto out; ++ } ++ + res = copy_in_params(arg->params, num_params, ¶m, saved_attr); + if (res != TEE_SUCCESS) +- goto out; ++ goto cleanup_shm_refs; + + s = tee_ta_get_session(arg->session, true, &tee_open_sessions); + if (!s) { + res = TEE_ERROR_BAD_PARAMETERS; +- goto out; ++ goto cleanup_shm_refs; + } + + res = tee_ta_invoke_command(&err_orig, s, NSAPP_IDENTITY, +@@ -446,9 +458,10 @@ static void entry_invoke_command(struct optee_msg_arg *arg, uint32_t num_params) + + copy_out_param(¶m, num_params, arg->params, saved_attr); + +-out: ++cleanup_shm_refs: + cleanup_shm_refs(saved_attr, ¶m, num_params); + ++out: + arg->ret = res; + arg->ret_origin = err_orig; + } +diff --git a/documentation/devicetree/bindings/regulator/st,stm32-regulator-gpio.yaml b/documentation/devicetree/bindings/regulator/st,stm32-regulator-gpio.yaml +new file mode 100644 +index 0000000000..9fedfddcc1 +--- /dev/null ++++ b/documentation/devicetree/bindings/regulator/st,stm32-regulator-gpio.yaml +@@ -0,0 +1,76 @@ ++# SPDX-License-Identifier: GPL-2.0 ++%YAML 1.2 ++--- ++$id: http://devicetree.org/schemas/regulator/gpio-regulator.yaml# ++$schema: http://devicetree.org/meta-schemas/core.yaml# ++ ++title: STM32 GPIO controlled regulators ++ ++maintainers: ++ - Pascal Paillet ++ ++description: ++ Any property defined as part of the core regulator binding, defined in ++ regulator.yaml, can also be used. ++ ++allOf: ++ - $ref: "regulator.yaml#" ++ ++properties: ++ compatible: ++ const: st,stm32-regulator-gpio ++ ++ regulator-name: true ++ ++ st,enable_pin_index: ++ description: GPIO used to control enable/disable of the regulator. ++ The GPIO is referred as index number in the pinctrl table. ++ maxItems: 1 ++ ++ enable-active-high: ++ description: Polarity of "enable-gpio" GPIO is active HIGH. Default is ++ active LOW. ++ type: boolean ++ ++ st,voltage_pin_index: ++ description: GPIO used to change the output voltage of the regulator. ++ The GPIO is referred as index number in the pinctrl table. ++ maxItems: 1 ++ ++ states: ++ description: Selection of available voltages in ascending order ++ provided by this regulator and matching GPIO configurations to ++ achieve them. ++ $ref: /schemas/types.yaml#/definitions/uint32-matrix ++ maxItems: 2 ++ items: ++ items: ++ - description: Voltage in microvolts ++ - description: GPIO state value ++ ++required: ++ - compatible ++ - regulator-name ++ ++unevaluatedProperties: false ++ ++examples: ++ - | ++ gpio-regulator { ++ compatible = "st,stm32-regulator-gpio"; ++ regulator-name = "vddcpu"; ++ ++ regulator-min-microvolt = <1200000>; ++ regulator-max-microvolt = <1350000>; ++ ++ pinctrl-names = "default"; ++ pinctrl-0 = <&vddcpu_pins_a>; ++ ++ st,enable_pin_index = <1>; ++ enable-active-high; ++ ++ st,voltage_pin_index = <0>; ++ states = <1200000 0x1>, ++ <1350000 0x0>; ++ }; ++... +diff --git a/scripts/sign_rproc_fw.py b/scripts/sign_rproc_fw.py +index 4248ed2801..48adda6f34 100755 +--- a/scripts/sign_rproc_fw.py ++++ b/scripts/sign_rproc_fw.py +@@ -7,7 +7,7 @@ + try: + from elftools.elf.elffile import ELFFile + from elftools.elf.sections import SymbolTableSection +- from elftools.elf.enums import ENUM_P_TYPE_BASE ++ from elftools.elf.enums import ENUM_P_TYPE_ARM + from elftools.elf.enums import * + except ImportError: + print(""" +@@ -124,7 +124,7 @@ class SegmentHash(object): + logging.debug("hash computed: %s" % seg.hash) + del h + struct.pack_into('