1 /*
2 * Copyright (c) 2015-2017, ARM Limited and Contributors. All rights reserved.
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7 #include <arch_helpers.h>
8 #include <assert.h>
9 #include <platform.h>
10 #include <platform_def.h>
11 #include <psci.h>
12
13 /* The power domain tree descriptor */
14 static unsigned char power_domain_tree_desc
15 [PLATFORM_NUM_AFFS - PLATFORM_CORE_COUNT + 1];
16
17 /*******************************************************************************
18 * Simple routine to set the id of an affinity instance at a given level
19 * in the mpidr. The assumption is that the affinity level and the power
20 * domain level are the same.
21 ******************************************************************************/
mpidr_set_aff_inst(unsigned long mpidr,unsigned char aff_inst,int aff_lvl)22 unsigned long mpidr_set_aff_inst(unsigned long mpidr,
23 unsigned char aff_inst,
24 int aff_lvl)
25 {
26 unsigned long aff_shift;
27
28 assert(aff_lvl <= MPIDR_AFFLVL3);
29
30 /*
31 * Decide the number of bits to shift by depending upon
32 * the power level
33 */
34 aff_shift = get_afflvl_shift(aff_lvl);
35
36 /* Clear the existing power instance & set the new one*/
37 mpidr &= ~((unsigned long)MPIDR_AFFLVL_MASK << aff_shift);
38 mpidr |= (unsigned long)aff_inst << aff_shift;
39
40 return mpidr;
41 }
42
43 /******************************************************************************
44 * This function uses insertion sort to sort a given list of mpidr's in the
45 * ascending order of the index returned by platform_get_core_pos.
46 *****************************************************************************/
sort_mpidr_by_cpu_idx(unsigned int aff_count,unsigned long mpidr_list[])47 void sort_mpidr_by_cpu_idx(unsigned int aff_count, unsigned long mpidr_list[])
48 {
49 int i, j;
50 unsigned long temp_mpidr;
51
52 for (i = 1; i < aff_count; i++) {
53 temp_mpidr = mpidr_list[i];
54
55 for (j = i;
56 j > 0 &&
57 platform_get_core_pos(mpidr_list[j-1]) >
58 platform_get_core_pos(temp_mpidr);
59 j--)
60 mpidr_list[j] = mpidr_list[j-1];
61
62 mpidr_list[j] = temp_mpidr;
63 }
64 }
65
66 /*******************************************************************************
67 * The compatibility routine to construct the power domain tree description.
68 * The assumption made is that the power domains correspond to affinity
69 * instances on the platform. This routine's aim is to traverse to the target
70 * affinity level and populate the number of siblings at that level in
71 * 'power_domain_tree_desc' array. It uses the current affinity level to keep
72 * track of how many levels from the root of the tree have been traversed.
73 * If the current affinity level != target affinity level, then the platform
74 * is asked to return the number of children that each affinity instance has
75 * at the current affinity level. Traversal is then done for each child at the
76 * next lower level i.e. current affinity level - 1.
77 *
78 * The power domain description needs to be constructed in such a way that
79 * affinity instances containing CPUs with lower cpu indices need to be
80 * described first. Hence when traversing the power domain levels, the list
81 * of mpidrs at that power domain level is sorted in the ascending order of CPU
82 * indices before the lower levels are recursively described.
83 *
84 * CAUTION: This routine assumes that affinity instance ids are allocated in a
85 * monotonically increasing manner at each affinity level in a mpidr starting
86 * from 0. If the platform breaks this assumption then this code will have to
87 * be reworked accordingly.
88 ******************************************************************************/
init_pwr_domain_tree_desc(unsigned long mpidr,unsigned int affmap_idx,unsigned int cur_afflvl,unsigned int tgt_afflvl)89 static unsigned int init_pwr_domain_tree_desc(unsigned long mpidr,
90 unsigned int affmap_idx,
91 unsigned int cur_afflvl,
92 unsigned int tgt_afflvl)
93 {
94 unsigned int ctr, aff_count;
95
96 /*
97 * Temporary list to hold the MPIDR list at a particular power domain
98 * level so as to sort them.
99 */
100 unsigned long mpidr_list[PLATFORM_CORE_COUNT];
101
102 assert(cur_afflvl >= tgt_afflvl);
103
104 /*
105 * Find the number of siblings at the current power level &
106 * assert if there are none 'cause then we have been invoked with
107 * an invalid mpidr.
108 */
109 aff_count = plat_get_aff_count(cur_afflvl, mpidr);
110 assert(aff_count);
111
112 if (tgt_afflvl < cur_afflvl) {
113 for (ctr = 0; ctr < aff_count; ctr++) {
114 mpidr_list[ctr] = mpidr_set_aff_inst(mpidr, ctr,
115 cur_afflvl);
116 }
117
118 /* Need to sort mpidr list according to CPU index */
119 sort_mpidr_by_cpu_idx(aff_count, mpidr_list);
120 for (ctr = 0; ctr < aff_count; ctr++) {
121 affmap_idx = init_pwr_domain_tree_desc(mpidr_list[ctr],
122 affmap_idx,
123 cur_afflvl - 1,
124 tgt_afflvl);
125 }
126 } else {
127 power_domain_tree_desc[affmap_idx++] = aff_count;
128 }
129 return affmap_idx;
130 }
131
132
133 /*******************************************************************************
134 * This function constructs the topology tree description at runtime
135 * and returns it. The assumption made is that the power domains correspond
136 * to affinity instances on the platform.
137 ******************************************************************************/
plat_get_power_domain_tree_desc(void)138 const unsigned char *plat_get_power_domain_tree_desc(void)
139 {
140 int afflvl;
141 unsigned int affmap_idx;
142
143 /*
144 * We assume that the platform allocates affinity instance ids from
145 * 0 onwards at each affinity level in the mpidr. FIRST_MPIDR = 0.0.0.0
146 */
147 affmap_idx = 0;
148 for (afflvl = (int) PLATFORM_MAX_AFFLVL;
149 afflvl >= (int) MPIDR_AFFLVL0; afflvl--) {
150 affmap_idx = init_pwr_domain_tree_desc(FIRST_MPIDR,
151 affmap_idx,
152 PLATFORM_MAX_AFFLVL,
153 (unsigned int) afflvl);
154 }
155
156 assert(affmap_idx == (PLATFORM_NUM_AFFS - PLATFORM_CORE_COUNT + 1));
157
158 return power_domain_tree_desc;
159 }
160
161 /******************************************************************************
162 * The compatibility helper function for plat_core_pos_by_mpidr(). It
163 * validates the 'mpidr' by making sure that it is within acceptable bounds
164 * for the platform and queries the platform layer whether the CPU specified
165 * by the mpidr is present or not. If present, it returns the index of the
166 * core corresponding to the 'mpidr'. Else it returns -1.
167 *****************************************************************************/
plat_core_pos_by_mpidr(u_register_t mpidr)168 int plat_core_pos_by_mpidr(u_register_t mpidr)
169 {
170 unsigned long shift, aff_inst;
171 int i;
172
173 /* Ignore the Reserved bits and U bit in MPIDR */
174 mpidr &= MPIDR_AFFINITY_MASK;
175
176 /*
177 * Check if any affinity field higher than
178 * the PLATFORM_MAX_AFFLVL is set.
179 */
180 shift = get_afflvl_shift(PLATFORM_MAX_AFFLVL + 1);
181 if (mpidr >> shift)
182 return -1;
183
184 for (i = PLATFORM_MAX_AFFLVL; i >= 0; i--) {
185 shift = get_afflvl_shift(i);
186 aff_inst = ((mpidr &
187 ((unsigned long)MPIDR_AFFLVL_MASK << shift)) >> shift);
188 if (aff_inst >= plat_get_aff_count(i, mpidr))
189 return -1;
190 }
191
192 if (plat_get_aff_state(0, mpidr) == PSCI_AFF_ABSENT)
193 return -1;
194
195 return platform_get_core_pos(mpidr);
196 }
197