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 ******************************************************************************/ 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 *****************************************************************************/ 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 ******************************************************************************/ 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 ******************************************************************************/ 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 *****************************************************************************/ 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
