1 /* 2 * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved. 3 * 4 * SPDX-License-Identifier: BSD-3-Clause 5 */ 6 7 #include <assert.h> 8 #include <inttypes.h> 9 #include <stdint.h> 10 11 #include "../../services/std_svc/spm/el3_spmc/spmc.h" 12 #include "../../services/std_svc/spm/el3_spmc/spmc_shared_mem.h" 13 #include <arch_features.h> 14 #include <arch_helpers.h> 15 #include <bl32/tsp/tsp.h> 16 #include <common/bl_common.h> 17 #include <common/debug.h> 18 #include <lib/psci/psci.h> 19 #include <lib/spinlock.h> 20 #include <plat/common/platform.h> 21 #include <platform_tsp.h> 22 #include <services/ffa_svc.h> 23 #include "tsp_private.h" 24 25 #include <platform_def.h> 26 27 extern void tsp_cpu_on_entry(void); 28 29 static ffa_endpoint_id16_t tsp_id, spmc_id; 30 31 static smc_args_t *send_ffa_pm_success(void) 32 { 33 return set_smc_args(FFA_MSG_SEND_DIRECT_RESP_SMC32, 34 tsp_id << FFA_DIRECT_MSG_SOURCE_SHIFT | 35 spmc_id, 36 FFA_FWK_MSG_BIT | 37 (FFA_PM_MSG_PM_RESP & FFA_FWK_MSG_MASK), 38 0, 0, 0, 0, 0); 39 } 40 41 /******************************************************************************* 42 * This function performs any remaining book keeping in the test secure payload 43 * before this cpu is turned off in response to a psci cpu_off request. 44 ******************************************************************************/ 45 smc_args_t *tsp_cpu_off_main(uint64_t arg0, 46 uint64_t arg1, 47 uint64_t arg2, 48 uint64_t arg3, 49 uint64_t arg4, 50 uint64_t arg5, 51 uint64_t arg6, 52 uint64_t arg7) 53 { 54 uint32_t linear_id = plat_my_core_pos(); 55 56 /* 57 * This cpu is being turned off, so disable the timer to prevent the 58 * secure timer interrupt from interfering with power down. A pending 59 * interrupt will be lost but we do not care as we are turning off. 60 */ 61 tsp_generic_timer_stop(); 62 63 /* Update this cpu's statistics. */ 64 tsp_stats[linear_id].smc_count++; 65 tsp_stats[linear_id].eret_count++; 66 tsp_stats[linear_id].cpu_off_count++; 67 68 #if LOG_LEVEL >= LOG_LEVEL_INFO 69 spin_lock(&console_lock); 70 INFO("TSP: cpu 0x%lx off request\n", read_mpidr()); 71 INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu off requests\n", 72 read_mpidr(), 73 tsp_stats[linear_id].smc_count, 74 tsp_stats[linear_id].eret_count, 75 tsp_stats[linear_id].cpu_off_count); 76 spin_unlock(&console_lock); 77 #endif 78 79 return send_ffa_pm_success(); 80 } 81 82 /******************************************************************************* 83 * This function performs any book keeping in the test secure payload before 84 * this cpu's architectural state is saved in response to an earlier psci 85 * cpu_suspend request. 86 ******************************************************************************/ 87 smc_args_t *tsp_cpu_suspend_main(uint64_t arg0, 88 uint64_t arg1, 89 uint64_t arg2, 90 uint64_t arg3, 91 uint64_t arg4, 92 uint64_t arg5, 93 uint64_t arg6, 94 uint64_t arg7) 95 { 96 uint32_t linear_id = plat_my_core_pos(); 97 98 /* 99 * Save the time context and disable it to prevent the secure timer 100 * interrupt from interfering with wakeup from the suspend state. 101 */ 102 tsp_generic_timer_save(); 103 tsp_generic_timer_stop(); 104 105 /* Update this cpu's statistics. */ 106 tsp_stats[linear_id].smc_count++; 107 tsp_stats[linear_id].eret_count++; 108 tsp_stats[linear_id].cpu_suspend_count++; 109 110 #if LOG_LEVEL >= LOG_LEVEL_INFO 111 spin_lock(&console_lock); 112 INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu suspend requests\n", 113 read_mpidr(), 114 tsp_stats[linear_id].smc_count, 115 tsp_stats[linear_id].eret_count, 116 tsp_stats[linear_id].cpu_suspend_count); 117 spin_unlock(&console_lock); 118 #endif 119 120 return send_ffa_pm_success(); 121 } 122 123 /******************************************************************************* 124 * This function performs any bookkeeping in the test secure payload after this 125 * cpu's architectural state has been restored after wakeup from an earlier psci 126 * cpu_suspend request. 127 ******************************************************************************/ 128 smc_args_t *tsp_cpu_resume_main(uint64_t max_off_pwrlvl, 129 uint64_t arg1, 130 uint64_t arg2, 131 uint64_t arg3, 132 uint64_t arg4, 133 uint64_t arg5, 134 uint64_t arg6, 135 uint64_t arg7) 136 { 137 uint32_t linear_id = plat_my_core_pos(); 138 139 /* Restore the generic timer context. */ 140 tsp_generic_timer_restore(); 141 142 /* Update this cpu's statistics. */ 143 tsp_stats[linear_id].smc_count++; 144 tsp_stats[linear_id].eret_count++; 145 tsp_stats[linear_id].cpu_resume_count++; 146 147 #if LOG_LEVEL >= LOG_LEVEL_INFO 148 spin_lock(&console_lock); 149 INFO("TSP: cpu 0x%lx resumed. maximum off power level %" PRId64 "\n", 150 read_mpidr(), max_off_pwrlvl); 151 INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu resume requests\n", 152 read_mpidr(), 153 tsp_stats[linear_id].smc_count, 154 tsp_stats[linear_id].eret_count, 155 tsp_stats[linear_id].cpu_resume_count); 156 spin_unlock(&console_lock); 157 #endif 158 159 return send_ffa_pm_success(); 160 } 161 162 /******************************************************************************* 163 * This function handles framework messages. Currently only PM. 164 ******************************************************************************/ 165 static smc_args_t *handle_framework_message(uint64_t arg0, 166 uint64_t arg1, 167 uint64_t arg2, 168 uint64_t arg3, 169 uint64_t arg4, 170 uint64_t arg5, 171 uint64_t arg6, 172 uint64_t arg7) 173 { 174 /* Check if it is a power management message from the SPMC. */ 175 if (ffa_endpoint_source(arg1) != spmc_id) { 176 goto err; 177 } 178 179 /* Check if it is a PM request message. */ 180 if ((arg2 & FFA_FWK_MSG_MASK) == FFA_FWK_MSG_PSCI) { 181 /* Check if it is a PSCI CPU_OFF request. */ 182 if (arg3 == PSCI_CPU_OFF) { 183 return tsp_cpu_off_main(arg0, arg1, arg2, arg3, 184 arg4, arg5, arg6, arg7); 185 } else if (arg3 == PSCI_CPU_SUSPEND_AARCH64) { 186 return tsp_cpu_suspend_main(arg0, arg1, arg2, arg3, 187 arg4, arg5, arg6, arg7); 188 } 189 } else if ((arg2 & FFA_FWK_MSG_MASK) == FFA_PM_MSG_WB_REQ) { 190 /* Check it is a PSCI Warm Boot request. */ 191 if (arg3 == FFA_WB_TYPE_NOTS2RAM) { 192 return tsp_cpu_resume_main(arg0, arg1, arg2, arg3, 193 arg4, arg5, arg6, arg7); 194 } 195 } 196 197 err: 198 ERROR("%s: Unknown framework message!\n", __func__); 199 panic(); 200 } 201 202 /******************************************************************************* 203 * This function implements the event loop for handling FF-A ABI invocations. 204 ******************************************************************************/ 205 static smc_args_t *tsp_event_loop(uint64_t smc_fid, 206 uint64_t arg1, 207 uint64_t arg2, 208 uint64_t arg3, 209 uint64_t arg4, 210 uint64_t arg5, 211 uint64_t arg6, 212 uint64_t arg7) 213 { 214 /* Panic if the SPMC did not forward an FF-A call. */ 215 if (!is_ffa_fid(smc_fid)) { 216 ERROR("%s: Unknown SMC FID (0x%lx)\n", __func__, smc_fid); 217 panic(); 218 } 219 220 switch (smc_fid) { 221 case FFA_INTERRUPT: 222 /* 223 * IRQs were enabled upon re-entry into the TSP. The interrupt 224 * must have been handled by now. Return to the SPMC indicating 225 * the same. 226 */ 227 return set_smc_args(FFA_MSG_WAIT, 0, 0, 0, 0, 0, 0, 0); 228 229 case FFA_MSG_SEND_DIRECT_REQ_SMC32: 230 /* Check if a framework message, handle accordingly. */ 231 if ((arg2 & FFA_FWK_MSG_BIT)) { 232 return handle_framework_message(smc_fid, arg1, arg2, arg3, 233 arg4, arg5, arg6, arg7); 234 } 235 default: 236 break; 237 } 238 239 ERROR("%s: Unsupported FF-A FID (0x%lx)\n", __func__, smc_fid); 240 panic(); 241 } 242 243 static smc_args_t *tsp_loop(smc_args_t *args) 244 { 245 smc_args_t ret; 246 247 do { 248 /* -------------------------------------------- 249 * Mask FIQ interrupts to avoid preemption 250 * in case EL3 SPMC delegates an IRQ next or a 251 * managed exit. Lastly, unmask IRQs so that 252 * they can be handled immediately upon re-entry 253 * --------------------------------------------- 254 */ 255 write_daifset(DAIF_FIQ_BIT); 256 write_daifclr(DAIF_IRQ_BIT); 257 ret = smc_helper(args->_regs[0], args->_regs[1], args->_regs[2], 258 args->_regs[3], args->_regs[4], args->_regs[5], 259 args->_regs[6], args->_regs[7]); 260 args = tsp_event_loop(ret._regs[0], ret._regs[1], ret._regs[2], 261 ret._regs[3], ret._regs[4], ret._regs[5], 262 ret._regs[6], ret._regs[7]); 263 } while (1); 264 265 /* Not Reached. */ 266 return NULL; 267 } 268 269 /******************************************************************************* 270 * TSP main entry point where it gets the opportunity to initialize its secure 271 * state/applications. Once the state is initialized, it must return to the 272 * SPD with a pointer to the 'tsp_vector_table' jump table. 273 ******************************************************************************/ 274 uint64_t tsp_main(void) 275 { 276 smc_args_t smc_args = {0}; 277 278 NOTICE("TSP: %s\n", version_string); 279 NOTICE("TSP: %s\n", build_message); 280 INFO("TSP: Total memory base : 0x%lx\n", (unsigned long) BL32_BASE); 281 INFO("TSP: Total memory size : 0x%lx bytes\n", BL32_TOTAL_SIZE); 282 uint32_t linear_id = plat_my_core_pos(); 283 284 /* Initialize the platform. */ 285 tsp_platform_setup(); 286 287 /* Initialize secure/applications state here. */ 288 tsp_generic_timer_start(); 289 290 /* Register secondary entrypoint with the SPMC. */ 291 smc_args = smc_helper(FFA_SECONDARY_EP_REGISTER_SMC64, 292 (uint64_t) tsp_cpu_on_entry, 293 0, 0, 0, 0, 0, 0); 294 if (smc_args._regs[SMC_ARG0] != FFA_SUCCESS_SMC32) { 295 ERROR("TSP could not register secondary ep (0x%lx)\n", 296 smc_args._regs[2]); 297 panic(); 298 } 299 /* Get TSP's endpoint id */ 300 smc_args = smc_helper(FFA_ID_GET, 0, 0, 0, 0, 0, 0, 0); 301 if (smc_args._regs[SMC_ARG0] != FFA_SUCCESS_SMC32) { 302 ERROR("TSP could not get own ID (0x%lx) on core%d\n", 303 smc_args._regs[2], linear_id); 304 panic(); 305 } 306 307 tsp_id = smc_args._regs[2]; 308 INFO("TSP FF-A endpoint id = 0x%x\n", tsp_id); 309 /* Get the SPMC ID */ 310 smc_args = smc_helper(FFA_SPM_ID_GET, 0, 0, 0, 0, 0, 0, 0); 311 if (smc_args._regs[SMC_ARG0] != FFA_SUCCESS_SMC32) { 312 ERROR("TSP could not get SPMC ID (0x%lx) on core%d\n", 313 smc_args._regs[2], linear_id); 314 panic(); 315 } 316 317 spmc_id = smc_args._regs[2]; 318 319 /* Update this cpu's statistics */ 320 tsp_stats[linear_id].smc_count++; 321 tsp_stats[linear_id].eret_count++; 322 tsp_stats[linear_id].cpu_on_count++; 323 324 INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu on requests\n", 325 read_mpidr(), 326 tsp_stats[linear_id].smc_count, 327 tsp_stats[linear_id].eret_count, 328 tsp_stats[linear_id].cpu_on_count); 329 330 /* Tell SPMD that we are done initialising. */ 331 tsp_loop(set_smc_args(FFA_MSG_WAIT, 0, 0, 0, 0, 0, 0, 0)); 332 333 /* Not reached. */ 334 return 0; 335 } 336 337 /******************************************************************************* 338 * This function performs any remaining book keeping in the test secure payload 339 * after this cpu's architectural state has been setup in response to an earlier 340 * psci cpu_on request. 341 ******************************************************************************/ 342 smc_args_t *tsp_cpu_on_main(void) 343 { 344 uint32_t linear_id = plat_my_core_pos(); 345 346 /* Initialize secure/applications state here. */ 347 tsp_generic_timer_start(); 348 349 /* Update this cpu's statistics. */ 350 tsp_stats[linear_id].smc_count++; 351 tsp_stats[linear_id].eret_count++; 352 tsp_stats[linear_id].cpu_on_count++; 353 #if LOG_LEVEL >= LOG_LEVEL_INFO 354 spin_lock(&console_lock); 355 INFO("TSP: cpu 0x%lx turned on\n", read_mpidr()); 356 INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu on requests\n", 357 read_mpidr(), 358 tsp_stats[linear_id].smc_count, 359 tsp_stats[linear_id].eret_count, 360 tsp_stats[linear_id].cpu_on_count); 361 spin_unlock(&console_lock); 362 #endif 363 /* --------------------------------------------- 364 * Jump to the main event loop to return to EL3 365 * and be ready for the next request on this cpu. 366 * --------------------------------------------- 367 */ 368 return tsp_loop(set_smc_args(FFA_MSG_WAIT, 0, 0, 0, 0, 0, 0, 0)); 369 } 370