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 ******************************************************************************/
24extern uintptr_t __PMF_SVC_DESCS_START__;
25extern 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__))
28extern void *__PERCPU_TIMESTAMP_SIZE__;
29#define PMF_PERCPU_TIMESTAMP_SIZE	((uintptr_t)&__PERCPU_TIMESTAMP_SIZE__)
30extern uintptr_t __PMF_TIMESTAMP_START__;
31#define PMF_TIMESTAMP_ARRAY_START	((uintptr_t)&__PMF_TIMESTAMP_START__)
32extern 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 */
40static pmf_svc_desc_t *pmf_svc_descs;
41
42/*
43 * This array is used to store registered PMF services in sorted order.
44 */
45static 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 */
50static 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 */
56int 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 */
118static 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 */
157int 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 */
182void __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 */
192static 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 */
214void __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 */
228void __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 */
243unsigned 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