1 /* 2 * Copyright (c) 2020, ARM Limited and Contributors. All rights reserved. 3 * 4 * SPDX-License-Identifier: BSD-3-Clause 5 */ 6 7 #include <assert.h> 8 #include <errno.h> 9 #include <lib/el3_runtime/context_mgmt.h> 10 #include "spmd_private.h" 11 12 struct spmd_pm_secondary_ep_t { 13 uintptr_t entry_point; 14 uintptr_t context; 15 bool locked; 16 }; 17 18 static struct spmd_pm_secondary_ep_t spmd_pm_secondary_ep[PLATFORM_CORE_COUNT]; 19 20 /******************************************************************************* 21 * spmd_build_spmc_message 22 * 23 * Builds an SPMD to SPMC direct message request. 24 ******************************************************************************/ 25 static void spmd_build_spmc_message(gp_regs_t *gpregs, unsigned long long message) 26 { 27 write_ctx_reg(gpregs, CTX_GPREG_X0, FFA_MSG_SEND_DIRECT_REQ_SMC32); 28 write_ctx_reg(gpregs, CTX_GPREG_X1, 29 (SPMD_DIRECT_MSG_ENDPOINT_ID << FFA_DIRECT_MSG_SOURCE_SHIFT) | 30 spmd_spmc_id_get()); 31 write_ctx_reg(gpregs, CTX_GPREG_X2, FFA_PARAM_MBZ); 32 write_ctx_reg(gpregs, CTX_GPREG_X3, message); 33 } 34 35 /******************************************************************************* 36 * spmd_pm_secondary_core_set_ep 37 ******************************************************************************/ 38 int spmd_pm_secondary_core_set_ep(unsigned long long mpidr, 39 uintptr_t entry_point, unsigned long long context) 40 { 41 int id = plat_core_pos_by_mpidr(mpidr); 42 43 if ((id < 0) || (id >= PLATFORM_CORE_COUNT)) { 44 ERROR("%s inconsistent MPIDR (%llx)\n", __func__, mpidr); 45 return -EINVAL; 46 } 47 48 if (spmd_pm_secondary_ep[id].locked) { 49 ERROR("%s entry locked (%llx)\n", __func__, mpidr); 50 return -EINVAL; 51 } 52 53 /* 54 * Check entry_point address is a PA within 55 * load_address <= entry_point < load_address + binary_size 56 */ 57 if (!spmd_check_address_in_binary_image(entry_point)) { 58 ERROR("%s entry point is not within image boundaries (%llx)\n", 59 __func__, mpidr); 60 return -EINVAL; 61 } 62 63 /* Fill new entry to corresponding secondary core id and lock it */ 64 spmd_pm_secondary_ep[id].entry_point = entry_point; 65 spmd_pm_secondary_ep[id].context = context; 66 spmd_pm_secondary_ep[id].locked = true; 67 68 VERBOSE("%s %d %llx %lx %llx\n", 69 __func__, id, mpidr, entry_point, context); 70 71 return 0; 72 } 73 74 /******************************************************************************* 75 * This CPU has been turned on. Enter SPMC to initialise S-EL1 or S-EL2. As part 76 * of the SPMC initialization path, they will initialize any SPs that they 77 * manage. Entry into SPMC is done after initialising minimal architectural 78 * state that guarantees safe execution. 79 ******************************************************************************/ 80 static void spmd_cpu_on_finish_handler(u_register_t unused) 81 { 82 entry_point_info_t *spmc_ep_info = spmd_spmc_ep_info_get(); 83 spmd_spm_core_context_t *ctx = spmd_get_context(); 84 unsigned int linear_id = plat_my_core_pos(); 85 int rc; 86 87 assert(ctx != NULL); 88 assert(ctx->state != SPMC_STATE_ON); 89 assert(spmc_ep_info != NULL); 90 91 /* 92 * TODO: this might require locking the spmc_ep_info structure, 93 * or provisioning one structure per cpu 94 */ 95 if (spmd_pm_secondary_ep[linear_id].entry_point == 0) { 96 goto exit; 97 } 98 99 spmc_ep_info->pc = spmd_pm_secondary_ep[linear_id].entry_point; 100 cm_setup_context(&ctx->cpu_ctx, spmc_ep_info); 101 write_ctx_reg(get_gpregs_ctx(&ctx->cpu_ctx), CTX_GPREG_X0, 102 spmd_pm_secondary_ep[linear_id].context); 103 104 /* Mark CPU as initiating ON operation */ 105 ctx->state = SPMC_STATE_ON_PENDING; 106 107 rc = spmd_spm_core_sync_entry(ctx); 108 if (rc != 0) { 109 ERROR("%s failed failed (%d) on CPU%u\n", __func__, rc, 110 linear_id); 111 ctx->state = SPMC_STATE_OFF; 112 return; 113 } 114 115 exit: 116 ctx->state = SPMC_STATE_ON; 117 118 VERBOSE("CPU %u on!\n", linear_id); 119 } 120 121 /******************************************************************************* 122 * spmd_cpu_off_handler 123 ******************************************************************************/ 124 static int32_t spmd_cpu_off_handler(u_register_t unused) 125 { 126 spmd_spm_core_context_t *ctx = spmd_get_context(); 127 unsigned int linear_id = plat_my_core_pos(); 128 int32_t rc; 129 130 assert(ctx != NULL); 131 assert(ctx->state != SPMC_STATE_OFF); 132 133 if (spmd_pm_secondary_ep[linear_id].entry_point == 0) { 134 goto exit; 135 } 136 137 /* Build an SPMD to SPMC direct message request. */ 138 spmd_build_spmc_message(get_gpregs_ctx(&ctx->cpu_ctx), PSCI_CPU_OFF); 139 140 rc = spmd_spm_core_sync_entry(ctx); 141 if (rc != 0) { 142 ERROR("%s failed (%d) on CPU%u\n", __func__, rc, linear_id); 143 } 144 145 /* TODO expect FFA_DIRECT_MSG_RESP returned from SPMC */ 146 147 exit: 148 ctx->state = SPMC_STATE_OFF; 149 150 VERBOSE("CPU %u off!\n", linear_id); 151 152 return 0; 153 } 154 155 /******************************************************************************* 156 * Structure populated by the SPM Dispatcher to perform any bookkeeping before 157 * PSCI executes a power mgmt. operation. 158 ******************************************************************************/ 159 const spd_pm_ops_t spmd_pm = { 160 .svc_on_finish = spmd_cpu_on_finish_handler, 161 .svc_off = spmd_cpu_off_handler 162 }; 163