The added function psci_arch_cpu_entry() is called during psci_cpu_entry() and can be used by arch to handle PSCI state transition from ON_PENDING to ON.

The default weak function is empty: not behavior change.

Signed-off-by: Patrick Delaunay patrick.delaunay@st.com ---

Changes in v2: None

arch/arm/cpu/armv7/psci.S | 7 +++++++ 1 file changed, 7 insertions(+)

diff --git a/arch/arm/cpu/armv7/psci.S b/arch/arm/cpu/armv7/psci.S index 35fd955..08b5088 100644 --- a/arch/arm/cpu/armv7/psci.S +++ b/arch/arm/cpu/armv7/psci.S @@ -321,11 +321,18 @@ ENTRY(psci_arch_init) ENDPROC(psci_arch_init) .weak psci_arch_init

+ENTRY(psci_arch_cpu_entry) + mov pc, lr +ENDPROC(psci_arch_cpu_entry) +.weak psci_arch_cpu_entry + ENTRY(psci_cpu_entry) bl psci_enable_smp

bl _nonsec_init

+ bl psci_arch_cpu_entry + bl psci_get_cpu_id @ CPU ID => r0 mov r2, r0 @ CPU ID => r2 bl psci_get_context_id @ context id => r0

Add PSCI v1.0 support for Linux and manage PSCI state for each CPU (affinity 0 level) with all mandatory functions: - PSCI_VERSION - CPU_SUSPEND - CPU_OFF - CPU_ON - AFFINITY_INFO - SYSTEM_OFF - SYSTEM_RESET - PSCI_FEATURES and 1 optional to avoid Linux warning - MIGRATE_INFO_TYPE

Signed-off-by: Patrick Delaunay patrick.delaunay@st.com Reviewed-by: CITOOLS smet-aci-reviews@lists.codex.cro.st.com --- it is the v2 for http://patchwork.ozlabs.org/patch/888210

Linux boot and cpu1 hoot-plug is OK

[ 0.000000] psci: probing for conduit method from DT. [ 0.000000] psci: PSCIv1.0 detected in firmware. [ 0.000000] psci: Using standard PSCI v0.2 function IDs [ 0.000000] psci: Trusted OS migration not required ..... [ 0.153066] smp: Bringing up secondary CPUs ... [ 0.203301] CPU1: thread -1, cpu 1, socket 0, mpidr 80000001 [ 0.203467] smp: Brought up 1 node, 2 CPUs [ 0.213500] SMP: Total of 2 processors activated (96.00 BogoMIPS). .... root@stm32mp1:~# echo 0 > /sys/devices/system/cpu/cpu1/online [ 76.498342] Retrying again to check for CPU kill [ 76.501500] CPU1 killed. root@stm32mp1:~# cat /sys/devices/system/cpu/online 0 root@stm32mp1:~# echo 1 > /sys/devices/system/cpu/cpu1/online root@stm32mp1:~# cat /sys/devices/system/cpu/online 0-1

Changes in v2: - major update after Mark Rutland review - add missing mandatory functions (AFFINITY_INFO, SYSTEM_OFF) - add optional MIGRATE_INFO_TYPE for avoid Linux warning - manage PSCI states transition - save and use context_id for CPU_ON - manage errors for all function

arch/arm/mach-stm32mp/Kconfig | 3 + arch/arm/mach-stm32mp/Makefile | 1 + arch/arm/mach-stm32mp/include/mach/stm32.h | 5 + arch/arm/mach-stm32mp/psci.c | 181 +++++++++++++++++++++++++++++ include/configs/stm32mp1.h | 5 + 5 files changed, 195 insertions(+) create mode 100644 arch/arm/mach-stm32mp/psci.c

diff --git a/arch/arm/mach-stm32mp/Kconfig b/arch/arm/mach-stm32mp/Kconfig index 8c755f8..503bce1 100644 --- a/arch/arm/mach-stm32mp/Kconfig +++ b/arch/arm/mach-stm32mp/Kconfig @@ -23,7 +23,10 @@ config SYS_SOC

config TARGET_STM32MP1 bool "Support stm32mp1xx" + select ARCH_SUPPORT_PSCI select CPU_V7 + select CPU_V7_HAS_NONSEC + select CPU_V7_HAS_VIRT select PINCTRL_STM32 select STM32_RESET help diff --git a/arch/arm/mach-stm32mp/Makefile b/arch/arm/mach-stm32mp/Makefile index 4620869..9aac2da 100644 --- a/arch/arm/mach-stm32mp/Makefile +++ b/arch/arm/mach-stm32mp/Makefile @@ -8,3 +8,4 @@ obj-y += cpu.o obj-y += dram_init.o

obj-$(CONFIG_SPL_BUILD) += spl.o +obj-$(CONFIG_ARMV7_PSCI) += psci.o diff --git a/arch/arm/mach-stm32mp/include/mach/stm32.h b/arch/arm/mach-stm32mp/include/mach/stm32.h index ffbe0b1..dd6f030 100644 --- a/arch/arm/mach-stm32mp/include/mach/stm32.h +++ b/arch/arm/mach-stm32mp/include/mach/stm32.h @@ -24,4 +24,9 @@ #define STM32_DDR_BASE 0xC0000000 #define STM32_DDR_SIZE SZ_1G

+/* TAMP registers */ +#define TAMP_BACKUP_REGISTER(x) (STM32_TAMP_BASE + 0x100 + 4 * x) +#define TAMP_BACKUP_MAGIC_NUMBER TAMP_BACKUP_REGISTER(4) +#define TAMP_BACKUP_BRANCH_ADDRESS TAMP_BACKUP_REGISTER(5) + #endif /* _MACH_STM32_H_ */ diff --git a/arch/arm/mach-stm32mp/psci.c b/arch/arm/mach-stm32mp/psci.c new file mode 100644 index 0000000..b31c13e --- /dev/null +++ b/arch/arm/mach-stm32mp/psci.c @@ -0,0 +1,181 @@ +/* + * Copyright (C) 2018, STMicroelectronics - All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ BSD-3-Clause + */ + +#include <config.h> +#include <common.h> +#include <asm/armv7.h> +#include <asm/gic.h> +#include <asm/io.h> +#include <asm/psci.h> +#include <asm/secure.h> + +#define BOOT_API_A7_CORE0_MAGIC_NUMBER 0xCA7FACE0 +#define BOOT_API_A7_CORE1_MAGIC_NUMBER 0xCA7FACE1 + +#define MPIDR_AFF0 GENMASK(7, 0) + +#define RCC_MP_GRSTCSETR (STM32_RCC_BASE + 0x0404) +#define RCC_MP_GRSTCSETR_MPUP1RST BIT(5) +#define RCC_MP_GRSTCSETR_MPUP0RST BIT(4) +#define RCC_MP_GRSTCSETR_MPSYSRST BIT(0) + +#define STM32MP1_PSCI_NR_CPUS 2 +#if STM32MP1_PSCI_NR_CPUS > CONFIG_ARMV7_PSCI_NR_CPUS +#error "invalid value for CONFIG_ARMV7_PSCI_NR_CPUS" +#endif + +u8 psci_state[STM32MP1_PSCI_NR_CPUS] __secure_data = { + PSCI_AFFINITY_LEVEL_ON, + PSCI_AFFINITY_LEVEL_OFF}; + +void __secure psci_set_state(int cpu, u8 state) +{ + psci_state[cpu] = state; + dsb(); + isb(); +} + +static u32 __secure stm32mp_get_gicd_base_address(void) +{ + u32 periphbase; + + /* get the GIC base address from the CBAR register */ + asm("mrc p15, 4, %0, c15, c0, 0\n" : "=r" (periphbase)); + + return (periphbase & CBAR_MASK) + GIC_DIST_OFFSET; +} + +static void __secure stm32mp_smp_kick_all_cpus(void) +{ + u32 gic_dist_addr; + + gic_dist_addr = stm32mp_get_gicd_base_address(); + + /* kick all CPUs (except this one) by writing to GICD_SGIR */ + writel(1U << 24, gic_dist_addr + GICD_SGIR); +} + +void __secure psci_arch_cpu_entry(void) +{ + u32 cpu = psci_get_cpu_id(); + + psci_set_state(cpu, PSCI_AFFINITY_LEVEL_ON); +} + +int __secure psci_features(u32 function_id, u32 psci_fid) +{ + switch (psci_fid) { + case ARM_PSCI_0_2_FN_PSCI_VERSION: + case ARM_PSCI_0_2_FN_CPU_OFF: + case ARM_PSCI_0_2_FN_CPU_ON: + case ARM_PSCI_0_2_FN_AFFINITY_INFO: + case ARM_PSCI_0_2_FN_MIGRATE_INFO_TYPE: + case ARM_PSCI_0_2_FN_SYSTEM_OFF: + case ARM_PSCI_0_2_FN_SYSTEM_RESET: + return 0x0; + } + return ARM_PSCI_RET_NI; +} + +unsigned int __secure psci_version(u32 function_id) +{ + return ARM_PSCI_VER_1_0; +} + +int __secure psci_affinity_info(u32 function_id, u32 target_affinity, + u32 lowest_affinity_level) +{ + u32 cpu = target_affinity & MPIDR_AFF0; + + if (lowest_affinity_level > 0) + return ARM_PSCI_RET_INVAL; + + if (target_affinity & ~MPIDR_AFF0) + return ARM_PSCI_RET_INVAL; + + if (cpu >= STM32MP1_PSCI_NR_CPUS) + return ARM_PSCI_RET_INVAL; + + return psci_state[cpu]; +} + +int __secure psci_migrate_info_type(u32 function_id) +{ + /* Trusted OS is either not present or does not require migration */ + return 2; +} + +int __secure psci_cpu_on(u32 function_id, u32 target_cpu, u32 pc, + u32 context_id) +{ + u32 cpu = target_cpu & MPIDR_AFF0; + + if (target_cpu & ~MPIDR_AFF0) + return ARM_PSCI_RET_INVAL; + + if (cpu >= STM32MP1_PSCI_NR_CPUS) + return ARM_PSCI_RET_INVAL; + + if (psci_state[cpu] == PSCI_AFFINITY_LEVEL_ON) + return ARM_PSCI_RET_ALREADY_ON; + + /* store target PC and context id*/ + psci_save(cpu, pc, context_id); + + /* write entrypoint in backup RAM register */ + writel((u32)&psci_cpu_entry, TAMP_BACKUP_BRANCH_ADDRESS); + psci_set_state(cpu, PSCI_AFFINITY_LEVEL_ON_PENDING); + + /* write magic number in backup register */ + if (cpu == 0x01) + writel(BOOT_API_A7_CORE1_MAGIC_NUMBER, + TAMP_BACKUP_MAGIC_NUMBER); + else + writel(BOOT_API_A7_CORE0_MAGIC_NUMBER, + TAMP_BACKUP_MAGIC_NUMBER); + + stm32mp_smp_kick_all_cpus(); + + return ARM_PSCI_RET_SUCCESS; +} + +int __secure psci_cpu_off(u32 function_id) +{ + u32 cpu; + + cpu = psci_get_cpu_id(); + + psci_cpu_off_common(); + psci_set_state(cpu, PSCI_AFFINITY_LEVEL_OFF); + + /* reset core: wfi is managed by BootRom */ + if (cpu == 0x01) + writel(RCC_MP_GRSTCSETR_MPUP1RST, RCC_MP_GRSTCSETR); + else + writel(RCC_MP_GRSTCSETR_MPUP0RST, RCC_MP_GRSTCSETR); + + /* just waiting reset */ + while (1) + wfi(); +} + +void __secure psci_system_reset(u32 function_id) +{ + /* System reset */ + writel(RCC_MP_GRSTCSETR_MPSYSRST, RCC_MP_GRSTCSETR); + /* just waiting reset */ + while (1) + wfi(); +} + +void __secure psci_system_off(u32 function_id) +{ + /* System Off is not managed, waiting user power off + * TODO: handle I2C write in PMIC Main Control register bit 0 = SWOFF + */ + while (1) + wfi(); +} diff --git a/include/configs/stm32mp1.h b/include/configs/stm32mp1.h index aae2cb8..b0aeb86 100644 --- a/include/configs/stm32mp1.h +++ b/include/configs/stm32mp1.h @@ -20,6 +20,11 @@ #define CONFIG_SYS_ARCH_TIMER #define CONFIG_SYS_HZ_CLOCK 64000000

+/* PSCI support */ +#define CONFIG_ARMV7_PSCI_1_0 +#define CONFIG_ARMV7_SECURE_BASE STM32_SYSRAM_BASE +#define CONFIG_ARMV7_SECURE_MAX_SIZE STM32_SYSRAM_SIZE + /* * malloc() pool size */