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 */
pmf_setup(void)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 */
get_service(unsigned int tid)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 */
pmf_get_timestamp_smc(unsigned int tid,u_register_t mpidr,unsigned int flags,unsigned long long * ts_value)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) || (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 */
__pmf_dump_timestamp(unsigned int tid,unsigned long long ts)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 */
calc_ts_addr(uintptr_t base_addr,unsigned int tid,unsigned int cpuid)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 */
__pmf_store_timestamp(uintptr_t base_addr,unsigned int tid,unsigned long long ts)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 */
__pmf_store_timestamp_with_cache_maint(uintptr_t base_addr,unsigned int tid,unsigned long long ts)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 */
__pmf_get_timestamp(uintptr_t base_addr,unsigned int tid,unsigned int cpuid,unsigned int flags)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 }
257