From 41c79775f08ad5836ae875cd21ea7ed952fa1237 Mon Sep 17 00:00:00 2001 From: Patrick Delaunay Date: Mon, 16 Apr 2018 10:13:24 +0200 Subject: [PATCH] arm: stm32mp1: add PSCI support 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 Reviewed-by: CITOOLS --- arch/arm/mach-stm32mp/Kconfig | 3 + arch/arm/mach-stm32mp/Makefile | 1 + arch/arm/mach-stm32mp/include/mach/stm32.h | 2 + arch/arm/mach-stm32mp/psci.c | 180 +++++++++++++++++++++++++++++ include/configs/stm32mp1.h | 5 + 5 files changed, 191 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 4d59480..6e9b508 100644 --- a/arch/arm/mach-stm32mp/Kconfig +++ b/arch/arm/mach-stm32mp/Kconfig @@ -24,7 +24,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 select SYS_ARCH_TIMER diff --git a/arch/arm/mach-stm32mp/Makefile b/arch/arm/mach-stm32mp/Makefile index cdb087c..a9b523d 100644 --- a/arch/arm/mach-stm32mp/Makefile +++ b/arch/arm/mach-stm32mp/Makefile @@ -8,3 +8,4 @@ obj-y += dram_init.o obj-y += syscon.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 2090402..afcab29 100644 --- a/arch/arm/mach-stm32mp/include/mach/stm32.h +++ b/arch/arm/mach-stm32mp/include/mach/stm32.h @@ -73,6 +73,8 @@ enum boot_device { /* 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) #define TAMP_BOOT_CONTEXT TAMP_BACKUP_REGISTER(20) #define TAMP_BOOT_MODE_MASK GENMASK(15, 8) diff --git a/arch/arm/mach-stm32mp/psci.c b/arch/arm/mach-stm32mp/psci.c new file mode 100644 index 0000000..6ed2482 --- /dev/null +++ b/arch/arm/mach-stm32mp/psci.c @@ -0,0 +1,180 @@ +// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause +/* + * Copyright (C) 2018, STMicroelectronics - All Rights Reserved + */ + +#include +#include +#include +#include +#include +#include +#include + +#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 aea0d98..09ec429 100644 --- a/include/configs/stm32mp1.h +++ b/include/configs/stm32mp1.h @@ -17,6 +17,11 @@ */ #define CONFIG_SYS_HZ 1000 +/* 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 */