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 void release_secondary_early_hpen(size_t __unused pos) 116 { 117 118 write32(TEE_LOAD_ADDR, 119 stm32mp_bkpreg(BCKR_CORE1_BRANCH_ADDRESS)); 120 write32(BOOT_API_A7_CORE1_MAGIC_NUMBER, 121 stm32mp_bkpreg(BCKR_CORE1_MAGIC_NUMBER)); 122 123 dmb(); 124 isb(); 125 itr_raise_sgi(GIC_SEC_SGI_0, BIT(pos)); 126 } 127 128 /* Override default psci_cpu_on() with platform specific sequence */ 129 int psci_cpu_on(uint32_t core_id, uint32_t entry, uint32_t context_id) 130 { 131 size_t pos = get_core_pos_mpidr(core_id); 132 uint32_t excep; 133 int rc; 134 135 if (!pos || pos >= CFG_TEE_CORE_NB_CORE) 136 return PSCI_RET_INVALID_PARAMETERS; 137 138 DMSG("core %zu, ns_entry 0x%" PRIx32 ", state %u", 139 pos, entry, core_state[pos]); 140 141 excep = lock_state_access(); 142 143 switch (core_state[pos]) { 144 case CORE_ON: 145 rc = PSCI_RET_ALREADY_ON; 146 break; 147 case CORE_AWAKE: 148 rc = PSCI_RET_ON_PENDING; 149 break; 150 case CORE_RET: 151 rc = PSCI_RET_DENIED; 152 break; 153 case CORE_OFF: 154 core_state[pos] = CORE_AWAKE; 155 rc = PSCI_RET_SUCCESS; 156 break; 157 default: 158 panic(); 159 } 160 161 unlock_state_access(excep); 162 163 if (rc == PSCI_RET_SUCCESS) { 164 generic_boot_set_core_ns_entry(pos, entry, context_id); 165 release_secondary_early_hpen(pos); 166 } 167 168 return rc; 169 } 170 #endif 171 172 /* Override default psci_cpu_on() with platform supported features */ 173 int psci_features(uint32_t psci_fid) 174 { 175 switch (psci_fid) { 176 case PSCI_PSCI_FEATURES: 177 case PSCI_VERSION: 178 #if CFG_TEE_CORE_NB_CORE > 1 179 case PSCI_CPU_ON: 180 #endif 181 return PSCI_RET_SUCCESS; 182 default: 183 return PSCI_RET_NOT_SUPPORTED; 184 } 185 } 186 187 /* Override default psci_version() to enable PSCI_VERSION_1_0 API */ 188 uint32_t psci_version(void) 189 { 190 return PSCI_VERSION_1_0; 191 } 192