lib/pmf/pmf_main.c - filogic/atf - Gitiles

 /*
  * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 #include <arch.h>
 #include <arch_helpers.h>
 #include <assert.h>
 #include <debug.h>
 #include <errno.h>
 #include <platform.h>
 #include <pmf.h>
 #include <string.h>
 #include <utils_def.h>

 /*******************************************************************************
  * The 'pmf_svc_descs' array holds the PMF service descriptors exported by
  * services by placing them in the 'pmf_svc_descs' linker section.
  * The 'pmf_svc_descs_indices' array holds the index of a descriptor in the
  * 'pmf_svc_descs' array. The TIF[15:10] bits in the time-stamp id are used
  * to get an index into the 'pmf_svc_descs_indices' array. This gives the
  * index of the descriptor in the 'pmf_svc_descs' array  which contains the
  * service function pointers.
  ******************************************************************************/

 IMPORT_SYM(uintptr_t, __PMF_SVC_DESCS_START__,		PMF_SVC_DESCS_START);
 IMPORT_SYM(uintptr_t, __PMF_SVC_DESCS_END__,		PMF_SVC_DESCS_END);
 IMPORT_SYM(uintptr_t, __PMF_PERCPU_TIMESTAMP_END__,	PMF_PERCPU_TIMESTAMP_END);
 IMPORT_SYM(intptr_t,  __PMF_TIMESTAMP_START__,		PMF_TIMESTAMP_ARRAY_START);

 #define PMF_PERCPU_TIMESTAMP_SIZE	(PMF_PERCPU_TIMESTAMP_END - PMF_TIMESTAMP_ARRAY_START)

 #define PMF_SVC_DESCS_MAX		10

 /*
  * This is used to traverse through registered PMF services.
  */
 static pmf_svc_desc_t *pmf_svc_descs;

 /*
  * This array is used to store registered PMF services in sorted order.
  */
 static int pmf_svc_descs_indices[PMF_SVC_DESCS_MAX];

 /*
  * This is used to track total number of successfully registered PMF services.
  */
 static int pmf_num_services;

 /*
  * This is the main PMF function that initialize registered
  * PMF services and also sort them in ascending order.
  */
 int pmf_setup(void)
 {
 	int rc, ii, jj = 0;
 	int pmf_svc_descs_num, temp_val;

 	/* If no PMF services are registered then simply bail out */
 	pmf_svc_descs_num = (PMF_SVC_DESCS_END - PMF_SVC_DESCS_START)/
 				 sizeof(pmf_svc_desc_t);
 	if (pmf_svc_descs_num == 0)
 		return 0;

 	assert(pmf_svc_descs_num < PMF_SVC_DESCS_MAX);

 	pmf_svc_descs = (pmf_svc_desc_t *) PMF_SVC_DESCS_START;
 	for (ii = 0; ii < pmf_svc_descs_num; ii++) {

 		assert(pmf_svc_descs[ii].get_ts);

 		/*
 		 * Call the initialization routine for this
 		 * PMF service, if it is defined.
 		 */
 		if (pmf_svc_descs[ii].init) {
 			rc = pmf_svc_descs[ii].init();
 			if (rc) {
 				WARN("Could not initialize PMF"
 					"service %s - skipping \n",
 					pmf_svc_descs[ii].name);
 				continue;
 			}
 		}

 		/* Update the pmf_svc_descs_indices array */
 		pmf_svc_descs_indices[jj++] = ii;
 	}

 	pmf_num_services = jj;

 	/*
 	 * Sort the successfully registered PMF services
 	 * according to service ID
 	 */
 	for (ii = 1; ii < pmf_num_services; ii++) {
 		for (jj = 0; jj < (pmf_num_services - ii); jj++) {
 			if ((pmf_svc_descs[jj].svc_config & PMF_SVC_ID_MASK) >
 				(pmf_svc_descs[jj + 1].svc_config &
 						PMF_SVC_ID_MASK)) {
 				temp_val = pmf_svc_descs_indices[jj];
 				pmf_svc_descs_indices[jj] =
 						pmf_svc_descs_indices[jj+1];
 				pmf_svc_descs_indices[jj+1] = temp_val;
 			}
 		}
 	}

 	return 0;
 }

 /*
  * This function implements binary search to find registered
  * PMF service based on Service ID provided in `tid` argument.
  */
 static pmf_svc_desc_t *get_service(unsigned int tid)
 {
 	int low = 0;
 	int mid;
 	int high = pmf_num_services;
 	unsigned int svc_id = tid & PMF_SVC_ID_MASK;
 	int index;
 	unsigned int desc_svc_id;

 	if (pmf_num_services == 0)
 		return NULL;

 	assert(pmf_svc_descs);

 	do {
 		mid = (low + high) / 2;
 		index = pmf_svc_descs_indices[mid];

 		desc_svc_id = pmf_svc_descs[index].svc_config & PMF_SVC_ID_MASK;
 		if (svc_id < desc_svc_id)
 			high = mid - 1;
 		if (svc_id > desc_svc_id)
 			low = mid + 1;
 	} while ((svc_id != desc_svc_id) && (low <= high));

 	/*
 	 * Make sure the Service found supports the tid range.
 	 */
 	if ((svc_id == desc_svc_id) && ((tid & PMF_TID_MASK) <
 		(pmf_svc_descs[index].svc_config & PMF_TID_MASK)))
 		return (pmf_svc_desc_t *)&pmf_svc_descs[index];

 	return NULL;
 }

 /*
  * This function gets the time-stamp value for the PMF services
  * registered for SMC interface based on `tid` and `mpidr`.
  */
 int pmf_get_timestamp_smc(unsigned int tid,
 		u_register_t mpidr,
 		unsigned int flags,
 		unsigned long long *ts_value)
 {
 	pmf_svc_desc_t *svc_desc;
 	assert(ts_value);

 	/* Search for registered service. */
 	svc_desc = get_service(tid);

 	if ((svc_desc == NULL) || (plat_core_pos_by_mpidr(mpidr) < 0)) {
 		*ts_value = 0;
 		return -EINVAL;
 	} else {
 		/* Call the service time-stamp handler. */
 		*ts_value = svc_desc->get_ts(tid, mpidr, flags);
 		return 0;
 	}
 }

 /*
  * This function can be used to dump `ts` value for given `tid`.
  * Assumption is that the console is already initialized.
  */
 void __pmf_dump_timestamp(unsigned int tid, unsigned long long ts)
 {
 	printf("PMF:cpu %u	tid %u	ts %llu\n",
 		plat_my_core_pos(), tid, ts);
 }

 /*
  * This function calculate the address identified by
  * `base_addr`, `tid` and `cpuid`.
  */
 static inline uintptr_t calc_ts_addr(uintptr_t base_addr,
 		unsigned int tid,
 		unsigned int cpuid)
 {
 	assert(cpuid < PLATFORM_CORE_COUNT);
 	assert(base_addr >= PMF_TIMESTAMP_ARRAY_START);
 	assert(base_addr < ((PMF_TIMESTAMP_ARRAY_START +
 		PMF_PERCPU_TIMESTAMP_SIZE) - ((tid & PMF_TID_MASK) *
 		sizeof(unsigned long long))));

 	base_addr += ((cpuid * PMF_PERCPU_TIMESTAMP_SIZE) +
 		((tid & PMF_TID_MASK) * sizeof(unsigned long long)));

 	return base_addr;
 }

 /*
  * This function stores the `ts` value to the storage identified by
  * `base_addr`, `tid` and current cpu id.
  * Note: The timestamp addresses are cache line aligned per cpu
  * and only the owning CPU would ever write into it.
  */
 void __pmf_store_timestamp(uintptr_t base_addr,
 			unsigned int tid,
 			unsigned long long ts)
 {
 	unsigned long long *ts_addr = (unsigned long long *)calc_ts_addr(base_addr,
 				 tid, plat_my_core_pos());
 	*ts_addr = ts;
 }

 /*
  * This is the cached version of `pmf_store_my_timestamp`
  * Note: The timestamp addresses are cache line aligned per cpu
  * and only the owning CPU would ever write into it.
  */
 void __pmf_store_timestamp_with_cache_maint(uintptr_t base_addr,
 			unsigned int tid,
 			unsigned long long ts)
 {
 	unsigned long long *ts_addr = (unsigned long long *)calc_ts_addr(base_addr,
 				 tid, plat_my_core_pos());
 	*ts_addr = ts;
 	flush_dcache_range((uintptr_t)ts_addr, sizeof(unsigned long long));
 }

 /*
  * This function retrieves the `ts` value from the storage identified by
  * `base_addr`, `tid` and `cpuid`.
  * Note: The timestamp addresses are cache line aligned per cpu.
  */
 unsigned long long __pmf_get_timestamp(uintptr_t base_addr,
 			unsigned int tid,
 			unsigned int cpuid,
 			unsigned int flags)
 {
 	assert(cpuid < PLATFORM_CORE_COUNT);
 	unsigned long long *ts_addr = (unsigned long long *)calc_ts_addr(base_addr,
 				tid, cpuid);

 	if (flags & PMF_CACHE_MAINT)
 		inv_dcache_range((uintptr_t)ts_addr, sizeof(unsigned long long));

 	return *ts_addr;
 }
	/*
	* Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved.
	*
	* SPDX-License-Identifier: BSD-3-Clause
	*/
	#include <arch.h>
	#include <arch_helpers.h>
	#include <assert.h>
	#include <debug.h>
	#include <errno.h>
	#include <platform.h>
	#include <pmf.h>
	#include <string.h>
	#include <utils_def.h>

	/*******************************************************************************
	* The 'pmf_svc_descs' array holds the PMF service descriptors exported by
	* services by placing them in the 'pmf_svc_descs' linker section.
	* The 'pmf_svc_descs_indices' array holds the index of a descriptor in the
	* 'pmf_svc_descs' array. The TIF[15:10] bits in the time-stamp id are used
	* to get an index into the 'pmf_svc_descs_indices' array. This gives the
	* index of the descriptor in the 'pmf_svc_descs' array which contains the
	* service function pointers.
	******************************************************************************/

	IMPORT_SYM(uintptr_t, __PMF_SVC_DESCS_START__, PMF_SVC_DESCS_START);
	IMPORT_SYM(uintptr_t, __PMF_SVC_DESCS_END__, PMF_SVC_DESCS_END);
	IMPORT_SYM(uintptr_t, __PMF_PERCPU_TIMESTAMP_END__, PMF_PERCPU_TIMESTAMP_END);
	IMPORT_SYM(intptr_t, __PMF_TIMESTAMP_START__, PMF_TIMESTAMP_ARRAY_START);

	#define PMF_PERCPU_TIMESTAMP_SIZE (PMF_PERCPU_TIMESTAMP_END - PMF_TIMESTAMP_ARRAY_START)

	#define PMF_SVC_DESCS_MAX 10

	/*
	* This is used to traverse through registered PMF services.
	*/
	static pmf_svc_desc_t *pmf_svc_descs;

	/*
	* This array is used to store registered PMF services in sorted order.
	*/
	static int pmf_svc_descs_indices[PMF_SVC_DESCS_MAX];

	/*
	* This is used to track total number of successfully registered PMF services.
	*/
	static int pmf_num_services;

	/*
	* This is the main PMF function that initialize registered
	* PMF services and also sort them in ascending order.
	*/
	int pmf_setup(void)
	{
	int rc, ii, jj = 0;
	int pmf_svc_descs_num, temp_val;

	/* If no PMF services are registered then simply bail out */
	pmf_svc_descs_num = (PMF_SVC_DESCS_END - PMF_SVC_DESCS_START)/
	sizeof(pmf_svc_desc_t);
	if (pmf_svc_descs_num == 0)
	return 0;

	assert(pmf_svc_descs_num < PMF_SVC_DESCS_MAX);

	pmf_svc_descs = (pmf_svc_desc_t *) PMF_SVC_DESCS_START;
	for (ii = 0; ii < pmf_svc_descs_num; ii++) {

	assert(pmf_svc_descs[ii].get_ts);

	/*
	* Call the initialization routine for this
	* PMF service, if it is defined.
	*/
	if (pmf_svc_descs[ii].init) {
	rc = pmf_svc_descs[ii].init();
	if (rc) {
	WARN("Could not initialize PMF"
	"service %s - skipping \n",
	pmf_svc_descs[ii].name);
	continue;
	}
	}

	/* Update the pmf_svc_descs_indices array */
	pmf_svc_descs_indices[jj++] = ii;
	}

	pmf_num_services = jj;

	/*
	* Sort the successfully registered PMF services
	* according to service ID
	*/
	for (ii = 1; ii < pmf_num_services; ii++) {
	for (jj = 0; jj < (pmf_num_services - ii); jj++) {
	if ((pmf_svc_descs[jj].svc_config & PMF_SVC_ID_MASK) >
	(pmf_svc_descs[jj + 1].svc_config &
	PMF_SVC_ID_MASK)) {
	temp_val = pmf_svc_descs_indices[jj];
	pmf_svc_descs_indices[jj] =
	pmf_svc_descs_indices[jj+1];
	pmf_svc_descs_indices[jj+1] = temp_val;
	}
	}
	}

	return 0;
	}

	/*
	* This function implements binary search to find registered
	* PMF service based on Service ID provided in `tid` argument.
	*/
	static pmf_svc_desc_t *get_service(unsigned int tid)
	{
	int low = 0;
	int mid;
	int high = pmf_num_services;
	unsigned int svc_id = tid & PMF_SVC_ID_MASK;
	int index;
	unsigned int desc_svc_id;

	if (pmf_num_services == 0)
	return NULL;

	assert(pmf_svc_descs);

	do {
	mid = (low + high) / 2;
	index = pmf_svc_descs_indices[mid];

	desc_svc_id = pmf_svc_descs[index].svc_config & PMF_SVC_ID_MASK;
	if (svc_id < desc_svc_id)
	high = mid - 1;
	if (svc_id > desc_svc_id)
	low = mid + 1;
	} while ((svc_id != desc_svc_id) && (low <= high));

	/*
	* Make sure the Service found supports the tid range.
	*/
	if ((svc_id == desc_svc_id) && ((tid & PMF_TID_MASK) <
	(pmf_svc_descs[index].svc_config & PMF_TID_MASK)))
	return (pmf_svc_desc_t *)&pmf_svc_descs[index];

	return NULL;
	}

	/*
	* This function gets the time-stamp value for the PMF services
	* registered for SMC interface based on `tid` and `mpidr`.
	*/
	int pmf_get_timestamp_smc(unsigned int tid,
	u_register_t mpidr,
	unsigned int flags,
	unsigned long long *ts_value)
	{
	pmf_svc_desc_t *svc_desc;
	assert(ts_value);

	/* Search for registered service. */
	svc_desc = get_service(tid);

	if ((svc_desc == NULL) \|\| (plat_core_pos_by_mpidr(mpidr) < 0)) {
	*ts_value = 0;
	return -EINVAL;
	} else {
	/* Call the service time-stamp handler. */
	*ts_value = svc_desc->get_ts(tid, mpidr, flags);
	return 0;
	}
	}

	/*
	* This function can be used to dump `ts` value for given `tid`.
	* Assumption is that the console is already initialized.
	*/
	void __pmf_dump_timestamp(unsigned int tid, unsigned long long ts)
	{
	printf("PMF:cpu %u tid %u ts %llu\n",
	plat_my_core_pos(), tid, ts);
	}

	/*
	* This function calculate the address identified by
	* `base_addr`, `tid` and `cpuid`.
	*/
	static inline uintptr_t calc_ts_addr(uintptr_t base_addr,
	unsigned int tid,
	unsigned int cpuid)
	{
	assert(cpuid < PLATFORM_CORE_COUNT);
	assert(base_addr >= PMF_TIMESTAMP_ARRAY_START);
	assert(base_addr < ((PMF_TIMESTAMP_ARRAY_START +
	PMF_PERCPU_TIMESTAMP_SIZE) - ((tid & PMF_TID_MASK) *
	sizeof(unsigned long long))));

	base_addr += ((cpuid * PMF_PERCPU_TIMESTAMP_SIZE) +
	((tid & PMF_TID_MASK) * sizeof(unsigned long long)));

	return base_addr;
	}

	/*
	* This function stores the `ts` value to the storage identified by
	* `base_addr`, `tid` and current cpu id.
	* Note: The timestamp addresses are cache line aligned per cpu
	* and only the owning CPU would ever write into it.
	*/
	void __pmf_store_timestamp(uintptr_t base_addr,
	unsigned int tid,
	unsigned long long ts)
	{
	unsigned long long ts_addr = (unsigned long long )calc_ts_addr(base_addr,
	tid, plat_my_core_pos());
	*ts_addr = ts;
	}

	/*
	* This is the cached version of `pmf_store_my_timestamp`
	* Note: The timestamp addresses are cache line aligned per cpu
	* and only the owning CPU would ever write into it.
	*/
	void __pmf_store_timestamp_with_cache_maint(uintptr_t base_addr,
	unsigned int tid,
	unsigned long long ts)
	{
	unsigned long long ts_addr = (unsigned long long )calc_ts_addr(base_addr,
	tid, plat_my_core_pos());
	*ts_addr = ts;
	flush_dcache_range((uintptr_t)ts_addr, sizeof(unsigned long long));
	}

	/*
	* This function retrieves the `ts` value from the storage identified by
	* `base_addr`, `tid` and `cpuid`.
	* Note: The timestamp addresses are cache line aligned per cpu.
	*/
	unsigned long long __pmf_get_timestamp(uintptr_t base_addr,
	unsigned int tid,
	unsigned int cpuid,
	unsigned int flags)
	{
	assert(cpuid < PLATFORM_CORE_COUNT);
	unsigned long long ts_addr = (unsigned long long )calc_ts_addr(base_addr,
	tid, cpuid);

	if (flags & PMF_CACHE_MAINT)
	inv_dcache_range((uintptr_t)ts_addr, sizeof(unsigned long long));

	return *ts_addr;
	}