blob: c0d29f242e76918b125eb29d2dc56ac0f2fa274e [file] [log] [blame]
Achin Gupta4f6ad662013-10-25 09:08:21 +01001/*
Dan Handleye83b0ca2014-01-14 18:17:09 +00002 * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
Achin Gupta4f6ad662013-10-25 09:08:21 +01003 *
4 * Redistribution and use in source and binary forms, with or without
5 * modification, are permitted provided that the following conditions are met:
6 *
7 * Redistributions of source code must retain the above copyright notice, this
8 * list of conditions and the following disclaimer.
9 *
10 * Redistributions in binary form must reproduce the above copyright notice,
11 * this list of conditions and the following disclaimer in the documentation
12 * and/or other materials provided with the distribution.
13 *
14 * Neither the name of ARM nor the names of its contributors may be used
15 * to endorse or promote products derived from this software without specific
16 * prior written permission.
17 *
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
22 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28 * POSSIBILITY OF SUCH DAMAGE.
29 */
30
31#include <stdio.h>
32#include <string.h>
33#include <assert.h>
34#include <arch_helpers.h>
35#include <console.h>
36#include <platform.h>
37#include <psci_private.h>
38
39/*******************************************************************************
40 * Routines for retrieving the node corresponding to an affinity level instance
41 * in the mpidr. The first one uses binary search to find the node corresponding
42 * to the mpidr (key) at a particular affinity level. The second routine decides
43 * extents of the binary search at each affinity level.
44 ******************************************************************************/
45static int psci_aff_map_get_idx(unsigned long key,
46 int min_idx,
47 int max_idx)
48{
49 int mid;
50
51 /*
52 * Terminating condition: If the max and min indices have crossed paths
53 * during the binary search then the key has not been found.
54 */
55 if (max_idx < min_idx)
56 return PSCI_E_INVALID_PARAMS;
57
58 /*
59 * Bisect the array around 'mid' and then recurse into the array chunk
60 * where the key is likely to be found. The mpidrs in each node in the
61 * 'psci_aff_map' for a given affinity level are stored in an ascending
62 * order which makes the binary search possible.
63 */
64 mid = min_idx + ((max_idx - min_idx) >> 1); /* Divide by 2 */
65 if (psci_aff_map[mid].mpidr > key)
66 return psci_aff_map_get_idx(key, min_idx, mid - 1);
67 else if (psci_aff_map[mid].mpidr < key)
68 return psci_aff_map_get_idx(key, mid + 1, max_idx);
69 else
70 return mid;
71}
72
73aff_map_node *psci_get_aff_map_node(unsigned long mpidr, int aff_lvl)
74{
75 int rc;
76
77 /* Right shift the mpidr to the required affinity level */
78 mpidr = mpidr_mask_lower_afflvls(mpidr, aff_lvl);
79
80 rc = psci_aff_map_get_idx(mpidr,
81 psci_aff_limits[aff_lvl].min,
82 psci_aff_limits[aff_lvl].max);
83 if (rc >= 0)
84 return &psci_aff_map[rc];
85 else
86 return NULL;
87}
88
89/*******************************************************************************
Achin Gupta0959db52013-12-02 17:33:04 +000090 * This function populates an array with nodes corresponding to a given range of
91 * affinity levels in an mpidr. It returns successfully only when the affinity
92 * levels are correct, the mpidr is valid i.e. no affinity level is absent from
93 * the topology tree & the affinity instance at level 0 is not absent.
94 ******************************************************************************/
95int psci_get_aff_map_nodes(unsigned long mpidr,
96 int start_afflvl,
97 int end_afflvl,
98 mpidr_aff_map_nodes mpidr_nodes)
99{
100 int rc = PSCI_E_INVALID_PARAMS, level;
101 aff_map_node *node;
102
103 rc = psci_check_afflvl_range(start_afflvl, end_afflvl);
104 if (rc != PSCI_E_SUCCESS)
105 return rc;
106
107 for (level = start_afflvl; level <= end_afflvl; level++) {
108
109 /*
110 * Grab the node for each affinity level. No affinity level
111 * can be missing as that would mean that the topology tree
112 * is corrupted.
113 */
114 node = psci_get_aff_map_node(mpidr, level);
115 if (node == NULL) {
116 rc = PSCI_E_INVALID_PARAMS;
117 break;
118 }
119
120 /*
121 * Skip absent affinity levels unless it's afffinity level 0.
122 * An absent cpu means that the mpidr is invalid. Save the
123 * pointer to the node for the present affinity level
124 */
125 if (!(node->state & PSCI_AFF_PRESENT)) {
126 if (level == MPIDR_AFFLVL0) {
127 rc = PSCI_E_INVALID_PARAMS;
128 break;
129 }
130
131 mpidr_nodes[level] = NULL;
132 } else
133 mpidr_nodes[level] = node;
134 }
135
136 return rc;
137}
138
139/*******************************************************************************
Achin Gupta4f6ad662013-10-25 09:08:21 +0100140 * Function which initializes the 'aff_map_node' corresponding to an affinity
141 * level instance. Each node has a unique mpidr, level and bakery lock. The data
142 * field is opaque and holds affinity level specific data e.g. for affinity
143 * level 0 it contains the index into arrays that hold the secure/non-secure
144 * state for a cpu that's been turned on/off
145 ******************************************************************************/
146static void psci_init_aff_map_node(unsigned long mpidr,
147 int level,
148 unsigned int idx)
149{
150 unsigned char state;
151 psci_aff_map[idx].mpidr = mpidr;
152 psci_aff_map[idx].level = level;
153 bakery_lock_init(&psci_aff_map[idx].lock);
154
155 /*
156 * If an affinity instance is present then mark it as OFF to begin with.
157 */
158 state = plat_get_aff_state(level, mpidr);
159 psci_aff_map[idx].state = state;
Achin Gupta4f6ad662013-10-25 09:08:21 +0100160
161 if (level == MPIDR_AFFLVL0) {
Achin Gupta75f73672013-12-05 16:33:10 +0000162
163 /*
164 * Mark the cpu as OFF. Higher affinity level reference counts
165 * have already been memset to 0
166 */
167 if (state & PSCI_AFF_PRESENT)
168 psci_set_state(&psci_aff_map[idx], PSCI_STATE_OFF);
169
Achin Gupta4f6ad662013-10-25 09:08:21 +0100170 /* Ensure that we have not overflowed the psci_ns_einfo array */
171 assert(psci_ns_einfo_idx < PSCI_NUM_AFFS);
172
173 psci_aff_map[idx].data = psci_ns_einfo_idx;
174 psci_ns_einfo_idx++;
175 }
176
177 return;
178}
179
180/*******************************************************************************
181 * Core routine used by the Breadth-First-Search algorithm to populate the
182 * affinity tree. Each level in the tree corresponds to an affinity level. This
183 * routine's aim is to traverse to the target affinity level and populate nodes
184 * in the 'psci_aff_map' for all the siblings at that level. It uses the current
185 * affinity level to keep track of how many levels from the root of the tree
186 * have been traversed. If the current affinity level != target affinity level,
187 * then the platform is asked to return the number of children that each
188 * affinity instance has at the current affinity level. Traversal is then done
189 * for each child at the next lower level i.e. current affinity level - 1.
190 *
191 * CAUTION: This routine assumes that affinity instance ids are allocated in a
192 * monotonically increasing manner at each affinity level in a mpidr starting
193 * from 0. If the platform breaks this assumption then this code will have to
194 * be reworked accordingly.
195 ******************************************************************************/
196static unsigned int psci_init_aff_map(unsigned long mpidr,
197 unsigned int affmap_idx,
198 int cur_afflvl,
199 int tgt_afflvl)
200{
201 unsigned int ctr, aff_count;
202
203 assert(cur_afflvl >= tgt_afflvl);
204
205 /*
206 * Find the number of siblings at the current affinity level &
207 * assert if there are none 'cause then we have been invoked with
208 * an invalid mpidr.
209 */
210 aff_count = plat_get_aff_count(cur_afflvl, mpidr);
211 assert(aff_count);
212
213 if (tgt_afflvl < cur_afflvl) {
214 for (ctr = 0; ctr < aff_count; ctr++) {
215 mpidr = mpidr_set_aff_inst(mpidr, ctr, cur_afflvl);
216 affmap_idx = psci_init_aff_map(mpidr,
217 affmap_idx,
218 cur_afflvl - 1,
219 tgt_afflvl);
220 }
221 } else {
222 for (ctr = 0; ctr < aff_count; ctr++, affmap_idx++) {
223 mpidr = mpidr_set_aff_inst(mpidr, ctr, cur_afflvl);
224 psci_init_aff_map_node(mpidr, cur_afflvl, affmap_idx);
225 }
226
227 /* affmap_idx is 1 greater than the max index of cur_afflvl */
228 psci_aff_limits[cur_afflvl].max = affmap_idx - 1;
229 }
230
231 return affmap_idx;
232}
233
234/*******************************************************************************
235 * This function initializes the topology tree by querying the platform. To do
236 * so, it's helper routines implement a Breadth-First-Search. At each affinity
237 * level the platform conveys the number of affinity instances that exist i.e.
238 * the affinity count. The algorithm populates the psci_aff_map recursively
239 * using this information. On a platform that implements two clusters of 4 cpus
240 * each, the populated aff_map_array would look like this:
241 *
242 * <- cpus cluster0 -><- cpus cluster1 ->
243 * ---------------------------------------------------
244 * | 0 | 1 | 0 | 1 | 2 | 3 | 0 | 1 | 2 | 3 |
245 * ---------------------------------------------------
246 * ^ ^
247 * cluster __| cpu __|
248 * limit limit
249 *
250 * The first 2 entries are of the cluster nodes. The next 4 entries are of cpus
251 * within cluster 0. The last 4 entries are of cpus within cluster 1.
252 * The 'psci_aff_limits' array contains the max & min index of each affinity
253 * level within the 'psci_aff_map' array. This allows restricting search of a
254 * node at an affinity level between the indices in the limits array.
255 ******************************************************************************/
256void psci_setup(unsigned long mpidr)
257{
258 int afflvl, affmap_idx, rc, max_afflvl;
259 aff_map_node *node;
260
261 /* Initialize psci's internal state */
262 memset(psci_aff_map, 0, sizeof(psci_aff_map));
263 memset(psci_aff_limits, 0, sizeof(psci_aff_limits));
264 memset(psci_ns_entry_info, 0, sizeof(psci_ns_entry_info));
265 psci_ns_einfo_idx = 0;
266 psci_plat_pm_ops = NULL;
267
268 /* Find out the maximum affinity level that the platform implements */
269 max_afflvl = get_max_afflvl();
270 assert(max_afflvl <= MPIDR_MAX_AFFLVL);
271
272 /*
273 * This call traverses the topology tree with help from the platform and
274 * populates the affinity map using a breadth-first-search recursively.
275 * We assume that the platform allocates affinity instance ids from 0
276 * onwards at each affinity level in the mpidr. FIRST_MPIDR = 0.0.0.0
277 */
278 affmap_idx = 0;
279 for (afflvl = max_afflvl; afflvl >= MPIDR_AFFLVL0; afflvl--) {
280 affmap_idx = psci_init_aff_map(FIRST_MPIDR,
281 affmap_idx,
282 max_afflvl,
283 afflvl);
284 }
285
286 /*
287 * Set the bounds for the affinity counts of each level in the map. Also
288 * flush out the entire array so that it's visible to subsequent power
289 * management operations. The 'psci_aff_map' array is allocated in
290 * coherent memory so does not need flushing. The 'psci_aff_limits'
291 * array is allocated in normal memory. It will be accessed when the mmu
292 * is off e.g. after reset. Hence it needs to be flushed.
293 */
294 for (afflvl = MPIDR_AFFLVL0; afflvl < max_afflvl; afflvl++) {
295 psci_aff_limits[afflvl].min =
296 psci_aff_limits[afflvl + 1].max + 1;
297 }
298
299 flush_dcache_range((unsigned long) psci_aff_limits,
300 sizeof(psci_aff_limits));
301
302 /*
303 * Mark the affinity instances in our mpidr as ON. No need to lock as
304 * this is the primary cpu.
305 */
306 mpidr &= MPIDR_AFFINITY_MASK;
Achin Gupta75f73672013-12-05 16:33:10 +0000307 for (afflvl = MPIDR_AFFLVL0; afflvl <= max_afflvl; afflvl++) {
Achin Gupta4f6ad662013-10-25 09:08:21 +0100308
309 node = psci_get_aff_map_node(mpidr, afflvl);
310 assert(node);
311
312 /* Mark each present node as ON. */
Achin Gupta75f73672013-12-05 16:33:10 +0000313 if (node->state & PSCI_AFF_PRESENT)
314 psci_set_state(node, PSCI_STATE_ON);
Achin Gupta4f6ad662013-10-25 09:08:21 +0100315 }
316
317 rc = platform_setup_pm(&psci_plat_pm_ops);
318 assert(rc == 0);
319 assert(psci_plat_pm_ops);
320
321 return;
322}