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