1 // SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause 2 /* 3 * Copyright (C) 2018, STMicroelectronics - All Rights Reserved 4 */ 5 6 #include <config.h> 7 #include <common.h> 8 #include <asm/armv7.h> 9 #include <asm/gic.h> 10 #include <asm/io.h> 11 #include <asm/psci.h> 12 #include <asm/secure.h> 13 14 #define BOOT_API_A7_CORE0_MAGIC_NUMBER 0xCA7FACE0 15 #define BOOT_API_A7_CORE1_MAGIC_NUMBER 0xCA7FACE1 16 17 #define MPIDR_AFF0 GENMASK(7, 0) 18 19 #define RCC_MP_GRSTCSETR (STM32_RCC_BASE + 0x0404) 20 #define RCC_MP_GRSTCSETR_MPUP1RST BIT(5) 21 #define RCC_MP_GRSTCSETR_MPUP0RST BIT(4) 22 #define RCC_MP_GRSTCSETR_MPSYSRST BIT(0) 23 24 #define STM32MP1_PSCI_NR_CPUS 2 25 #if STM32MP1_PSCI_NR_CPUS > CONFIG_ARMV7_PSCI_NR_CPUS 26 #error "invalid value for CONFIG_ARMV7_PSCI_NR_CPUS" 27 #endif 28 29 u8 psci_state[STM32MP1_PSCI_NR_CPUS] __secure_data = { 30 PSCI_AFFINITY_LEVEL_ON, 31 PSCI_AFFINITY_LEVEL_OFF}; 32 33 void __secure psci_set_state(int cpu, u8 state) 34 { 35 psci_state[cpu] = state; 36 dsb(); 37 isb(); 38 } 39 40 static u32 __secure stm32mp_get_gicd_base_address(void) 41 { 42 u32 periphbase; 43 44 /* get the GIC base address from the CBAR register */ 45 asm("mrc p15, 4, %0, c15, c0, 0\n" : "=r" (periphbase)); 46 47 return (periphbase & CBAR_MASK) + GIC_DIST_OFFSET; 48 } 49 50 static void __secure stm32mp_smp_kick_all_cpus(void) 51 { 52 u32 gic_dist_addr; 53 54 gic_dist_addr = stm32mp_get_gicd_base_address(); 55 56 /* kick all CPUs (except this one) by writing to GICD_SGIR */ 57 writel(1U << 24, gic_dist_addr + GICD_SGIR); 58 } 59 60 void __secure psci_arch_cpu_entry(void) 61 { 62 u32 cpu = psci_get_cpu_id(); 63 64 psci_set_state(cpu, PSCI_AFFINITY_LEVEL_ON); 65 } 66 67 int __secure psci_features(u32 function_id, u32 psci_fid) 68 { 69 switch (psci_fid) { 70 case ARM_PSCI_0_2_FN_PSCI_VERSION: 71 case ARM_PSCI_0_2_FN_CPU_OFF: 72 case ARM_PSCI_0_2_FN_CPU_ON: 73 case ARM_PSCI_0_2_FN_AFFINITY_INFO: 74 case ARM_PSCI_0_2_FN_MIGRATE_INFO_TYPE: 75 case ARM_PSCI_0_2_FN_SYSTEM_OFF: 76 case ARM_PSCI_0_2_FN_SYSTEM_RESET: 77 return 0x0; 78 } 79 return ARM_PSCI_RET_NI; 80 } 81 82 unsigned int __secure psci_version(u32 function_id) 83 { 84 return ARM_PSCI_VER_1_0; 85 } 86 87 int __secure psci_affinity_info(u32 function_id, u32 target_affinity, 88 u32 lowest_affinity_level) 89 { 90 u32 cpu = target_affinity & MPIDR_AFF0; 91 92 if (lowest_affinity_level > 0) 93 return ARM_PSCI_RET_INVAL; 94 95 if (target_affinity & ~MPIDR_AFF0) 96 return ARM_PSCI_RET_INVAL; 97 98 if (cpu >= STM32MP1_PSCI_NR_CPUS) 99 return ARM_PSCI_RET_INVAL; 100 101 return psci_state[cpu]; 102 } 103 104 int __secure psci_migrate_info_type(u32 function_id) 105 { 106 /* Trusted OS is either not present or does not require migration */ 107 return 2; 108 } 109 110 int __secure psci_cpu_on(u32 function_id, u32 target_cpu, u32 pc, 111 u32 context_id) 112 { 113 u32 cpu = target_cpu & MPIDR_AFF0; 114 115 if (target_cpu & ~MPIDR_AFF0) 116 return ARM_PSCI_RET_INVAL; 117 118 if (cpu >= STM32MP1_PSCI_NR_CPUS) 119 return ARM_PSCI_RET_INVAL; 120 121 if (psci_state[cpu] == PSCI_AFFINITY_LEVEL_ON) 122 return ARM_PSCI_RET_ALREADY_ON; 123 124 /* store target PC and context id*/ 125 psci_save(cpu, pc, context_id); 126 127 /* write entrypoint in backup RAM register */ 128 writel((u32)&psci_cpu_entry, TAMP_BACKUP_BRANCH_ADDRESS); 129 psci_set_state(cpu, PSCI_AFFINITY_LEVEL_ON_PENDING); 130 131 /* write magic number in backup register */ 132 if (cpu == 0x01) 133 writel(BOOT_API_A7_CORE1_MAGIC_NUMBER, 134 TAMP_BACKUP_MAGIC_NUMBER); 135 else 136 writel(BOOT_API_A7_CORE0_MAGIC_NUMBER, 137 TAMP_BACKUP_MAGIC_NUMBER); 138 139 stm32mp_smp_kick_all_cpus(); 140 141 return ARM_PSCI_RET_SUCCESS; 142 } 143 144 int __secure psci_cpu_off(u32 function_id) 145 { 146 u32 cpu; 147 148 cpu = psci_get_cpu_id(); 149 150 psci_cpu_off_common(); 151 psci_set_state(cpu, PSCI_AFFINITY_LEVEL_OFF); 152 153 /* reset core: wfi is managed by BootRom */ 154 if (cpu == 0x01) 155 writel(RCC_MP_GRSTCSETR_MPUP1RST, RCC_MP_GRSTCSETR); 156 else 157 writel(RCC_MP_GRSTCSETR_MPUP0RST, RCC_MP_GRSTCSETR); 158 159 /* just waiting reset */ 160 while (1) 161 wfi(); 162 } 163 164 void __secure psci_system_reset(u32 function_id) 165 { 166 /* System reset */ 167 writel(RCC_MP_GRSTCSETR_MPSYSRST, RCC_MP_GRSTCSETR); 168 /* just waiting reset */ 169 while (1) 170 wfi(); 171 } 172 173 void __secure psci_system_off(u32 function_id) 174 { 175 /* System Off is not managed, waiting user power off 176 * TODO: handle I2C write in PMIC Main Control register bit 0 = SWOFF 177 */ 178 while (1) 179 wfi(); 180 } 181