1 /* 2 * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved. 3 * 4 * SPDX-License-Identifier: BSD-3-Clause 5 */ 6 #include <arch.h> 7 #include <arch_helpers.h> 8 #include <assert.h> 9 #include <debug.h> 10 #include <errno.h> 11 #include <platform.h> 12 #include <pmf.h> 13 #include <string.h> 14 #include <utils_def.h> 15 16 /******************************************************************************* 17 * The 'pmf_svc_descs' array holds the PMF service descriptors exported by 18 * services by placing them in the 'pmf_svc_descs' linker section. 19 * The 'pmf_svc_descs_indices' array holds the index of a descriptor in the 20 * 'pmf_svc_descs' array. The TIF[15:10] bits in the time-stamp id are used 21 * to get an index into the 'pmf_svc_descs_indices' array. This gives the 22 * index of the descriptor in the 'pmf_svc_descs' array which contains the 23 * service function pointers. 24 ******************************************************************************/ 25 26 IMPORT_SYM(uintptr_t, __PMF_SVC_DESCS_START__, PMF_SVC_DESCS_START); 27 IMPORT_SYM(uintptr_t, __PMF_SVC_DESCS_END__, PMF_SVC_DESCS_END); 28 IMPORT_SYM(uintptr_t, __PERCPU_TIMESTAMP_SIZE__, PMF_PERCPU_TIMESTAMP_SIZE); 29 IMPORT_SYM(intptr_t, __PMF_TIMESTAMP_START__, PMF_TIMESTAMP_ARRAY_START); 30 IMPORT_SYM(uintptr_t, __PMF_TIMESTAMP_END__, PMF_TIMESTAMP_ARRAY_END); 31 32 #define PMF_SVC_DESCS_MAX 10 33 34 /* 35 * This is used to traverse through registered PMF services. 36 */ 37 static pmf_svc_desc_t *pmf_svc_descs; 38 39 /* 40 * This array is used to store registered PMF services in sorted order. 41 */ 42 static int pmf_svc_descs_indices[PMF_SVC_DESCS_MAX]; 43 44 /* 45 * This is used to track total number of successfully registered PMF services. 46 */ 47 static int pmf_num_services; 48 49 /* 50 * This is the main PMF function that initialize registered 51 * PMF services and also sort them in ascending order. 52 */ 53 int pmf_setup(void) 54 { 55 int rc, ii, jj = 0; 56 int pmf_svc_descs_num, temp_val; 57 58 /* If no PMF services are registered then simply bail out */ 59 pmf_svc_descs_num = (PMF_SVC_DESCS_END - PMF_SVC_DESCS_START)/ 60 sizeof(pmf_svc_desc_t); 61 if (pmf_svc_descs_num == 0) 62 return 0; 63 64 assert(pmf_svc_descs_num < PMF_SVC_DESCS_MAX); 65 66 pmf_svc_descs = (pmf_svc_desc_t *) PMF_SVC_DESCS_START; 67 for (ii = 0; ii < pmf_svc_descs_num; ii++) { 68 69 assert(pmf_svc_descs[ii].get_ts); 70 71 /* 72 * Call the initialization routine for this 73 * PMF service, if it is defined. 74 */ 75 if (pmf_svc_descs[ii].init) { 76 rc = pmf_svc_descs[ii].init(); 77 if (rc) { 78 WARN("Could not initialize PMF" 79 "service %s - skipping \n", 80 pmf_svc_descs[ii].name); 81 continue; 82 } 83 } 84 85 /* Update the pmf_svc_descs_indices array */ 86 pmf_svc_descs_indices[jj++] = ii; 87 } 88 89 pmf_num_services = jj; 90 91 /* 92 * Sort the successfully registered PMF services 93 * according to service ID 94 */ 95 for (ii = 1; ii < pmf_num_services; ii++) { 96 for (jj = 0; jj < (pmf_num_services - ii); jj++) { 97 if ((pmf_svc_descs[jj].svc_config & PMF_SVC_ID_MASK) > 98 (pmf_svc_descs[jj + 1].svc_config & 99 PMF_SVC_ID_MASK)) { 100 temp_val = pmf_svc_descs_indices[jj]; 101 pmf_svc_descs_indices[jj] = 102 pmf_svc_descs_indices[jj+1]; 103 pmf_svc_descs_indices[jj+1] = temp_val; 104 } 105 } 106 } 107 108 return 0; 109 } 110 111 /* 112 * This function implements binary search to find registered 113 * PMF service based on Service ID provided in `tid` argument. 114 */ 115 static pmf_svc_desc_t *get_service(unsigned int tid) 116 { 117 int low = 0; 118 int mid; 119 int high = pmf_num_services; 120 unsigned int svc_id = tid & PMF_SVC_ID_MASK; 121 int index; 122 unsigned int desc_svc_id; 123 124 if (pmf_num_services == 0) 125 return NULL; 126 127 assert(pmf_svc_descs); 128 129 do { 130 mid = (low + high) / 2; 131 index = pmf_svc_descs_indices[mid]; 132 133 desc_svc_id = pmf_svc_descs[index].svc_config & PMF_SVC_ID_MASK; 134 if (svc_id < desc_svc_id) 135 high = mid - 1; 136 if (svc_id > desc_svc_id) 137 low = mid + 1; 138 } while ((svc_id != desc_svc_id) && (low <= high)); 139 140 /* 141 * Make sure the Service found supports the tid range. 142 */ 143 if ((svc_id == desc_svc_id) && ((tid & PMF_TID_MASK) < 144 (pmf_svc_descs[index].svc_config & PMF_TID_MASK))) 145 return (pmf_svc_desc_t *)&pmf_svc_descs[index]; 146 147 return NULL; 148 } 149 150 /* 151 * This function gets the time-stamp value for the PMF services 152 * registered for SMC interface based on `tid` and `mpidr`. 153 */ 154 int pmf_get_timestamp_smc(unsigned int tid, 155 u_register_t mpidr, 156 unsigned int flags, 157 unsigned long long *ts_value) 158 { 159 pmf_svc_desc_t *svc_desc; 160 assert(ts_value); 161 162 /* Search for registered service. */ 163 svc_desc = get_service(tid); 164 165 if ((svc_desc == NULL) || (plat_core_pos_by_mpidr(mpidr) < 0)) { 166 *ts_value = 0; 167 return -EINVAL; 168 } else { 169 /* Call the service time-stamp handler. */ 170 *ts_value = svc_desc->get_ts(tid, mpidr, flags); 171 return 0; 172 } 173 } 174 175 /* 176 * This function can be used to dump `ts` value for given `tid`. 177 * Assumption is that the console is already initialized. 178 */ 179 void __pmf_dump_timestamp(unsigned int tid, unsigned long long ts) 180 { 181 printf("PMF:cpu %u tid %u ts %llu\n", 182 plat_my_core_pos(), tid, ts); 183 } 184 185 /* 186 * This function calculate the address identified by 187 * `base_addr`, `tid` and `cpuid`. 188 */ 189 static inline uintptr_t calc_ts_addr(uintptr_t base_addr, 190 unsigned int tid, 191 unsigned int cpuid) 192 { 193 assert(cpuid < PLATFORM_CORE_COUNT); 194 assert(base_addr >= PMF_TIMESTAMP_ARRAY_START); 195 assert(base_addr < ((PMF_TIMESTAMP_ARRAY_START + 196 PMF_PERCPU_TIMESTAMP_SIZE) - ((tid & PMF_TID_MASK) * 197 sizeof(unsigned long long)))); 198 199 base_addr += ((cpuid * PMF_PERCPU_TIMESTAMP_SIZE) + 200 ((tid & PMF_TID_MASK) * sizeof(unsigned long long))); 201 202 return base_addr; 203 } 204 205 /* 206 * This function stores the `ts` value to the storage identified by 207 * `base_addr`, `tid` and current cpu id. 208 * Note: The timestamp addresses are cache line aligned per cpu 209 * and only the owning CPU would ever write into it. 210 */ 211 void __pmf_store_timestamp(uintptr_t base_addr, 212 unsigned int tid, 213 unsigned long long ts) 214 { 215 unsigned long long *ts_addr = (unsigned long long *)calc_ts_addr(base_addr, 216 tid, plat_my_core_pos()); 217 *ts_addr = ts; 218 } 219 220 /* 221 * This is the cached version of `pmf_store_my_timestamp` 222 * Note: The timestamp addresses are cache line aligned per cpu 223 * and only the owning CPU would ever write into it. 224 */ 225 void __pmf_store_timestamp_with_cache_maint(uintptr_t base_addr, 226 unsigned int tid, 227 unsigned long long ts) 228 { 229 unsigned long long *ts_addr = (unsigned long long *)calc_ts_addr(base_addr, 230 tid, plat_my_core_pos()); 231 *ts_addr = ts; 232 flush_dcache_range((uintptr_t)ts_addr, sizeof(unsigned long long)); 233 } 234 235 /* 236 * This function retrieves the `ts` value from the storage identified by 237 * `base_addr`, `tid` and `cpuid`. 238 * Note: The timestamp addresses are cache line aligned per cpu. 239 */ 240 unsigned long long __pmf_get_timestamp(uintptr_t base_addr, 241 unsigned int tid, 242 unsigned int cpuid, 243 unsigned int flags) 244 { 245 assert(cpuid < PLATFORM_CORE_COUNT); 246 unsigned long long *ts_addr = (unsigned long long *)calc_ts_addr(base_addr, 247 tid, cpuid); 248 249 if (flags & PMF_CACHE_MAINT) 250 inv_dcache_range((uintptr_t)ts_addr, sizeof(unsigned long long)); 251 252 return *ts_addr; 253 } 254