| 1 |
/* |
| 2 |
* Copyright (c) 2016-2018, Arm Limited and Contributors. All rights reserved. |
| 3 |
* |
| 4 |
* SPDX-License-Identifier: BSD-3-Clause |
| 5 |
*/ |
| 6 |
|
| 7 |
#include <assert.h> |
| 8 |
#include <errno.h> |
| 9 |
#include <string.h> |
| 10 |
|
| 11 |
#include <arch.h> |
| 12 |
#include <arch_helpers.h> |
| 13 |
#include <common/debug.h> |
| 14 |
#include <lib/pmf/pmf.h> |
| 15 |
#include <lib/utils_def.h> |
| 16 |
#include <plat/common/platform.h> |
| 17 |
|
| 18 |
/******************************************************************************* |
| 19 |
* The 'pmf_svc_descs' array holds the PMF service descriptors exported by |
| 20 |
* services by placing them in the '.pmf_svc_descs' linker section. |
| 21 |
* The 'pmf_svc_descs_indices' array holds the index of a descriptor in the |
| 22 |
* 'pmf_svc_descs' array. The TIF[15:10] bits in the time-stamp id are used |
| 23 |
* to get an index into the 'pmf_svc_descs_indices' array. This gives the |
| 24 |
* index of the descriptor in the 'pmf_svc_descs' array which contains the |
| 25 |
* service function pointers. |
| 26 |
******************************************************************************/ |
| 27 |
|
| 28 |
IMPORT_SYM(uintptr_t, __PMF_SVC_DESCS_START__, PMF_SVC_DESCS_START); |
| 29 |
IMPORT_SYM(uintptr_t, __PMF_SVC_DESCS_END__, PMF_SVC_DESCS_END); |
| 30 |
IMPORT_SYM(uintptr_t, __PMF_PERCPU_TIMESTAMP_END__, PMF_PERCPU_TIMESTAMP_END); |
| 31 |
IMPORT_SYM(uintptr_t, __PMF_TIMESTAMP_START__, PMF_TIMESTAMP_ARRAY_START); |
| 32 |
|
| 33 |
#define PMF_PERCPU_TIMESTAMP_SIZE (PMF_PERCPU_TIMESTAMP_END - PMF_TIMESTAMP_ARRAY_START) |
| 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 != NULL); |
| 73 |
|
| 74 |
/* |
| 75 |
* Call the initialization routine for this |
| 76 |
* PMF service, if it is defined. |
| 77 |
*/ |
| 78 |
if (pmf_svc_descs[ii].init != NULL) { |
| 79 |
rc = pmf_svc_descs[ii].init(); |
| 80 |
if (rc != 0) { |
| 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 != NULL); |
| 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 != NULL); |
| 164 |
|
| 165 |
/* Search for registered service. */ |
| 166 |
svc_desc = get_service(tid); |
| 167 |
|
| 168 |
if (svc_desc == NULL) { |
| 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 |
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) != 0U) |
| 253 |
inv_dcache_range((uintptr_t)ts_addr, sizeof(unsigned long long)); |
| 254 |
|
| 255 |
return *ts_addr; |
| 256 |
} |