1 /*
2 * Copyright (c) 2020-2021, 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 <services/ffa_svc.h>
10 #include <lib/el3_runtime/context_mgmt.h>
11 #include <lib/spinlock.h>
12 #include <platform_def.h>
13 #include <plat/common/common_def.h>
14 #include <plat/common/platform.h>
15
16 #include "spmc.h"
17
18 /*******************************************************************************
19 * spmc_build_pm_message
20 *
21 * Builds an SPMC to SP direct message request.
22 ******************************************************************************/
spmc_build_pm_message(gp_regs_t * gpregs,unsigned long long message,uint8_t pm_msg_type,uint16_t sp_id)23 static void spmc_build_pm_message(gp_regs_t *gpregs,
24 unsigned long long message,
25 uint8_t pm_msg_type,
26 uint16_t sp_id)
27 {
28 write_ctx_reg(gpregs, CTX_GPREG_X0, FFA_MSG_SEND_DIRECT_REQ_SMC32);
29 write_ctx_reg(gpregs, CTX_GPREG_X1,
30 (FFA_SPMC_ID << FFA_DIRECT_MSG_SOURCE_SHIFT) |
31 sp_id);
32 write_ctx_reg(gpregs, CTX_GPREG_X2, FFA_DIRECT_FRAMEWORK_MSG_MASK |
33 (pm_msg_type & FFA_PM_MSG_MASK));
34 write_ctx_reg(gpregs, CTX_GPREG_X3, message);
35 }
36
37 /*******************************************************************************
38 * This CPU has been turned on. Enter the SP to initialise S-EL1.
39 ******************************************************************************/
spmc_cpu_on_finish_handler(u_register_t unused)40 static void spmc_cpu_on_finish_handler(u_register_t unused)
41 {
42 sp_desc_t *sp = spmc_get_current_sp_ctx();
43 sp_exec_ctx_t *ec;
44 unsigned int linear_id = plat_my_core_pos();
45 entry_point_info_t sec_ec_ep_info = {0};
46 uint64_t rc;
47
48 /* Sanity check for a NULL pointer dereference */
49 assert (NULL != sp);
50
51 /* Do nothing in case of a S-EL0 SP */
52 if (sp-> runtime_el == EL0)
53 return;
54
55 /* Initialize entry point information for the SP */
56 SET_PARAM_HEAD(&sec_ec_ep_info, PARAM_EP, VERSION_1, SECURE | EP_ST_ENABLE);
57
58 /*
59 * Check if the primary execution context registered an entry point else
60 * bail out early.
61 * TODO: Add support for boot reason in manifest to allow jumping to
62 * entrypoint into the primary execution context.
63 */
64 spin_lock(&sp->secondary_ep_lock);
65 if (0 == sp->secondary_ep) {
66 WARN("%s: No secondary ep on core%u \n", __func__, linear_id);
67 return;
68 }
69
70 sec_ec_ep_info.pc = sp->secondary_ep;
71 spin_unlock(&sp->secondary_ep_lock);
72
73 /*
74 * Setup and initialise the SP execution context on this physical cpu.
75 */
76 spmc_sp_common_setup(sp, &sec_ec_ep_info);
77 spmc_el1_sp_setup(sp, &sec_ec_ep_info);
78
79 /* Obtain a reference to the SP execution context */
80 ec = &sp->ec[get_ec_index(sp)];
81
82 /*
83 * TODO: Should we do some PM related state tracking of the SP execution
84 * context here?
85 */
86
87 /* Update the runtime model and state of the partition */
88 ec->rt_model = RT_MODEL_INIT;
89 ec->rt_state = RT_STATE_RUNNING;
90
91 INFO("SP (0x%x) init start on core%u.\n", sp->sp_id, linear_id);
92
93 rc = spmc_sp_synchronous_entry(ec);
94 if (rc != 0ULL) {
95 ERROR("%s failed (%llu) on CPU%u\n", __func__, rc, linear_id);
96 }
97
98 /* Update the runtime state of the partition */
99 ec->rt_state = RT_STATE_WAITING;
100
101 VERBOSE("CPU %u on!\n", linear_id);
102 }
103
104 /*******************************************************************************
105 * spmc_cpu_off_handler
106 ******************************************************************************/
spmc_cpu_off_handler(u_register_t unused)107 static int32_t spmc_cpu_off_handler(u_register_t unused)
108 {
109 sp_desc_t *sp = spmc_get_current_sp_ctx();
110 sp_exec_ctx_t *ec;
111 unsigned int linear_id = plat_my_core_pos();
112 u_register_t resp;
113 uint64_t rc;
114
115 /* Sanity check for a NULL pointer dereference */
116 assert (NULL != sp);
117
118 /* Do nothing in case of a S-EL0 SP */
119 if (sp-> runtime_el == EL0)
120 return 0;
121
122 /* Obtain a reference to the SP execution context */
123 ec = &sp->ec[get_ec_index(sp)];
124
125 /*
126 * TODO: Should we do some PM related state tracking of the SP execution
127 * context here?
128 */
129
130 /* Build an SPMC to SPMC direct message request. */
131 spmc_build_pm_message(get_gpregs_ctx(&ec->cpu_ctx),
132 PSCI_CPU_OFF,
133 FFA_PM_MSG_PSCI_REQ,
134 sp->sp_id);
135
136 /* Sanity check partition state */
137 assert(ec->rt_state == RT_STATE_WAITING);
138
139 /* Update the runtime model and state of the partition */
140 ec->rt_model = RT_MODEL_DIR_REQ;
141 ec->rt_state = RT_STATE_RUNNING;
142
143 rc = spmc_sp_synchronous_entry(ec);
144 if (rc != 0ULL) {
145 ERROR("%s failed (%llu) on CPU%u\n", __func__, rc, linear_id);
146 }
147
148 /* Expect a direct message response from the SP. */
149 resp = read_ctx_reg(get_gpregs_ctx(&ec->cpu_ctx), CTX_GPREG_X0);
150 if (resp != FFA_MSG_SEND_DIRECT_RESP_SMC32) {
151 ERROR("%s invalid SPMC response (%lx).\n", __func__, resp);
152 return -EINVAL;
153 }
154
155 /* Expect a PM message response from the SP. */
156 resp = read_ctx_reg(get_gpregs_ctx(&ec->cpu_ctx), CTX_GPREG_X2);
157 if (!(resp & FFA_DIRECT_FRAMEWORK_MSG_MASK) ||
158 ((resp & FFA_PM_MSG_MASK) != FFA_PM_MSG_PM_RESP)) {
159 ERROR("%s invalid SPMC response (%lx).\n", __func__, resp);
160 return -EINVAL;
161 }
162
163 /*
164 * TODO: Check w1 in case the SP has sent a malformed message. If the
165 * execution context failed to turn off then it might need to be marked
166 * as such.
167 */
168
169 /* Update the runtime state of the partition */
170 ec->rt_state = RT_STATE_WAITING;
171
172 VERBOSE("CPU %u off!\n", linear_id);
173
174 /* Return the status code returned by the SP */
175 return read_ctx_reg(get_gpregs_ctx(&ec->cpu_ctx), CTX_GPREG_X3);
176 }
177
178 /*******************************************************************************
179 * Structure populated by the SPM Core to perform any bookkeeping before
180 * PSCI executes a power mgmt. operation.
181 ******************************************************************************/
182 const spd_pm_ops_t spmc_pm = {
183 .svc_on_finish = spmc_cpu_on_finish_handler,
184 .svc_off = spmc_cpu_off_handler
185 };
186