1 // SPDX-License-Identifier: BSD-2-Clause 2 /* 3 * Copyright (c) 2017-2018, STMicroelectronics 4 */ 5 6 #include <arm.h> 7 #include <boot_api.h> 8 #include <drivers/stm32mp1_rcc.h> 9 #include <io.h> 10 #include <kernel/cache_helpers.h> 11 #include <kernel/generic_boot.h> 12 #include <kernel/interrupt.h> 13 #include <kernel/misc.h> 14 #include <kernel/panic.h> 15 #include <kernel/spinlock.h> 16 #include <mm/core_memprot.h> 17 #include <platform_config.h> 18 #include <sm/psci.h> 19 #include <stm32_util.h> 20 #include <trace.h> 21 22 /* 23 * SMP boot support and access to the mailbox 24 */ 25 26 enum core_state_id { 27 CORE_OFF = 0, 28 CORE_RET, 29 CORE_AWAKE, 30 CORE_ON, 31 }; 32 33 static enum core_state_id core_state[CFG_TEE_CORE_NB_CORE]; 34 static unsigned int __maybe_unused state_lock = SPINLOCK_UNLOCK; 35 36 static uint32_t __maybe_unused lock_state_access(void) 37 { 38 return may_spin_lock(&state_lock); 39 } 40 41 static void __maybe_unused unlock_state_access(uint32_t exceptions) 42 { 43 may_spin_unlock(&state_lock, exceptions); 44 } 45 46 int psci_affinity_info(uint32_t affinity, uint32_t lowest_affinity_level) 47 { 48 unsigned int pos = get_core_pos_mpidr(affinity); 49 50 DMSG("core %zu, state %u", pos, core_state[pos]); 51 52 if ((pos >= CFG_TEE_CORE_NB_CORE) || 53 (lowest_affinity_level > PSCI_AFFINITY_LEVEL_ON)) { 54 return PSCI_RET_INVALID_PARAMETERS; 55 } 56 57 switch (core_state[pos]) { 58 case CORE_OFF: 59 case CORE_RET: 60 return PSCI_AFFINITY_LEVEL_OFF; 61 case CORE_AWAKE: 62 return PSCI_AFFINITY_LEVEL_ON_PENDING; 63 case CORE_ON: 64 return PSCI_AFFINITY_LEVEL_ON; 65 default: 66 panic(); 67 } 68 } 69 70 #if CFG_TEE_CORE_NB_CORE == 1 71 /* 72 * Function called when a CPU is booted through the OP-TEE. 73 * All cores shall register when online. 74 */ 75 void stm32mp_register_online_cpu(void) 76 { 77 assert(core_state[0] == CORE_OFF); 78 core_state[0] = CORE_ON; 79 } 80 #else 81 static void __noreturn stm32_pm_cpu_power_down_wfi(void) 82 { 83 dcache_op_level1(DCACHE_OP_CLEAN); 84 85 write32(RCC_MP_GRSTCSETR_MPUP1RST, stm32_rcc_base() + RCC_MP_GRSTCSETR); 86 87 dsb(); 88 isb(); 89 wfi(); 90 panic(); 91 } 92 93 void stm32mp_register_online_cpu(void) 94 { 95 size_t pos = get_core_pos(); 96 uint32_t excep = lock_state_access(); 97 98 if (pos == 0) { 99 assert(core_state[pos] == CORE_OFF); 100 } else { 101 if (core_state[pos] != CORE_AWAKE) { 102 core_state[pos] = CORE_OFF; 103 unlock_state_access(excep); 104 stm32_pm_cpu_power_down_wfi(); 105 panic(); 106 } 107 } 108 109 core_state[pos] = CORE_ON; 110 unlock_state_access(excep); 111 } 112 113 #define GIC_SEC_SGI_0 8 114 115 static vaddr_t bckreg_base(void) 116 { 117 static void *va; 118 119 if (!cpu_mmu_enabled()) 120 return BKP_REGS_BASE + BKP_REGISTER_OFF; 121 122 if (!va) 123 va = phys_to_virt(BKP_REGS_BASE + BKP_REGISTER_OFF, 124 MEM_AREA_IO_SEC); 125 126 return (vaddr_t)va; 127 } 128 129 static uint32_t *bckreg_address(unsigned int idx) 130 { 131 return (uint32_t *)bckreg_base() + idx; 132 } 133 134 static void release_secondary_early_hpen(size_t __unused pos) 135 { 136 uint32_t *p_entry = bckreg_address(BCKR_CORE1_BRANCH_ADDRESS); 137 uint32_t *p_magic = bckreg_address(BCKR_CORE1_MAGIC_NUMBER); 138 139 *p_entry = TEE_LOAD_ADDR; 140 *p_magic = BOOT_API_A7_CORE1_MAGIC_NUMBER; 141 142 dmb(); 143 isb(); 144 itr_raise_sgi(GIC_SEC_SGI_0, BIT(pos)); 145 } 146 147 /* Override default psci_cpu_on() with platform specific sequence */ 148 int psci_cpu_on(uint32_t core_id, uint32_t entry, uint32_t context_id) 149 { 150 size_t pos = get_core_pos_mpidr(core_id); 151 uint32_t excep; 152 int rc; 153 154 if (!pos || pos >= CFG_TEE_CORE_NB_CORE) 155 return PSCI_RET_INVALID_PARAMETERS; 156 157 DMSG("core %zu, ns_entry 0x%" PRIx32 ", state %u", 158 pos, entry, core_state[pos]); 159 160 excep = lock_state_access(); 161 162 switch (core_state[pos]) { 163 case CORE_ON: 164 rc = PSCI_RET_ALREADY_ON; 165 break; 166 case CORE_AWAKE: 167 rc = PSCI_RET_ON_PENDING; 168 break; 169 case CORE_RET: 170 rc = PSCI_RET_DENIED; 171 break; 172 case CORE_OFF: 173 core_state[pos] = CORE_AWAKE; 174 rc = PSCI_RET_SUCCESS; 175 break; 176 default: 177 panic(); 178 } 179 180 unlock_state_access(excep); 181 182 if (rc == PSCI_RET_SUCCESS) { 183 generic_boot_set_core_ns_entry(pos, entry, context_id); 184 release_secondary_early_hpen(pos); 185 } 186 187 return rc; 188 } 189 #endif 190 191 /* Override default psci_cpu_on() with platform supported features */ 192 int psci_features(uint32_t psci_fid) 193 { 194 switch (psci_fid) { 195 case PSCI_PSCI_FEATURES: 196 case PSCI_VERSION: 197 #if CFG_TEE_CORE_NB_CORE > 1 198 case PSCI_CPU_ON: 199 #endif 200 return PSCI_RET_SUCCESS; 201 default: 202 return PSCI_RET_NOT_SUPPORTED; 203 } 204 } 205 206 /* Override default psci_version() to enable PSCI_VERSION_1_0 API */ 207 uint32_t psci_version(void) 208 { 209 return PSCI_VERSION_1_0; 210 } 211