/*
|
* Copyright (c) 2017, Linaro Limited
|
* All rights reserved.
|
*
|
* Redistribution and use in source and binary forms, with or without
|
* modification, are permitted provided that the following conditions are met:
|
*
|
* 1. Redistributions of source code must retain the above copyright notice,
|
* this list of conditions and the following disclaimer.
|
*
|
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
* this list of conditions and the following disclaimer in the documentation
|
* and/or other materials provided with the distribution.
|
*
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
* POSSIBILITY OF SUCH DAMAGE.
|
*/
|
#include <err.h>
|
#include <fcntl.h>
|
#include <math.h>
|
#include <pthread.h>
|
#include <sched.h>
|
#include <stdbool.h>
|
#include <stdio.h>
|
#include <stdlib.h>
|
#include <string.h>
|
#include <sys/mman.h>
|
#include <sys/stat.h>
|
#include <tee_bench.h>
|
#include <tee_client_api.h>
|
#include <unistd.h>
|
|
#include "teec_benchmark.h"
|
|
struct tee_ts_global *bench_ts_global;
|
static const TEEC_UUID pta_benchmark_uuid = PTA_BENCHMARK_UUID;
|
|
static TEEC_Context bench_ctx;
|
static TEEC_Session bench_sess;
|
|
static pthread_mutex_t teec_bench_mu = PTHREAD_MUTEX_INITIALIZER;
|
|
/* Cycle counter */
|
static inline uint64_t read_ccounter(void)
|
{
|
uint64_t ccounter = 0;
|
#ifdef __aarch64__
|
asm volatile("mrs %0, PMCCNTR_EL0" : "=r"(ccounter));
|
#else
|
asm volatile("mrc p15, 0, %0, c9, c13, 0" : "=r"(ccounter));
|
#endif
|
return ccounter * TEE_BENCH_DIVIDER;
|
}
|
|
static TEEC_Result benchmark_pta_open(void)
|
{
|
TEEC_Result res = TEEC_ERROR_GENERIC;
|
uint32_t ret_orig = 0;
|
|
res = TEEC_InitializeContext(NULL, &bench_ctx);
|
if (res != TEEC_SUCCESS)
|
return res;
|
|
res = TEEC_OpenSession(&bench_ctx, &bench_sess,
|
&pta_benchmark_uuid,
|
TEEC_LOGIN_PUBLIC, NULL, NULL, &ret_orig);
|
if (res != TEEC_SUCCESS) {
|
TEEC_FinalizeContext(&bench_ctx);
|
return res;
|
}
|
|
return res;
|
}
|
|
static void benchmark_pta_close(void)
|
{
|
TEEC_CloseSession(&bench_sess);
|
TEEC_FinalizeContext(&bench_ctx);
|
}
|
|
static TEEC_Result benchmark_get_bench_buf_paddr(uint64_t *paddr_ts_buf,
|
uint64_t *size)
|
{
|
TEEC_Result res = TEEC_ERROR_GENERIC;
|
uint32_t ret_orig = 0;
|
TEEC_Operation op;
|
|
memset(&op, 0, sizeof(op));
|
|
res = benchmark_pta_open();
|
if (res != TEEC_SUCCESS)
|
return res;
|
|
op.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_OUTPUT, TEEC_NONE,
|
TEEC_NONE, TEEC_NONE);
|
|
res = TEEC_InvokeCommand(&bench_sess, BENCHMARK_CMD_GET_MEMREF,
|
&op, &ret_orig);
|
if (res != TEEC_SUCCESS)
|
return res;
|
|
*paddr_ts_buf = op.params[0].value.a;
|
*size = op.params[0].value.b;
|
|
benchmark_pta_close();
|
|
return res;
|
}
|
|
static void *mmap_paddr(intptr_t paddr, uint64_t size)
|
{
|
int devmem = 0;
|
off_t offset = 0;
|
off_t page_addr = 0;
|
intptr_t *hw_addr = NULL;
|
|
devmem = open("/dev/mem", O_RDWR);
|
if (!devmem)
|
return NULL;
|
|
offset = (off_t)paddr % getpagesize();
|
page_addr = (off_t)(paddr - offset);
|
|
hw_addr = (intptr_t *)mmap(0, size, PROT_READ|PROT_WRITE,
|
MAP_SHARED, devmem, page_addr);
|
if (hw_addr == MAP_FAILED) {
|
close(devmem);
|
return NULL;
|
}
|
|
close(devmem);
|
return (hw_addr + offset);
|
}
|
|
/* check if we are in benchmark mode */
|
static bool benchmark_check_mode(void)
|
{
|
uint64_t ts_buf_raw = 0;
|
uint64_t ts_buf_size = 0;
|
bool res = true;
|
|
if (!bench_ts_global) {
|
/* receive buffer from Benchmark PTA and register it */
|
benchmark_get_bench_buf_paddr(&ts_buf_raw, &ts_buf_size);
|
if (ts_buf_raw && ts_buf_size) {
|
bench_ts_global = mmap_paddr(ts_buf_raw, ts_buf_size);
|
res = (bench_ts_global) ? true : false;
|
} else {
|
res = false;
|
}
|
}
|
|
return res;
|
}
|
|
/* Adding timestamp to buffer */
|
void bm_timestamp(void)
|
{
|
struct tee_ts_cpu_buf *cpu_buf = NULL;
|
uint64_t ts_i = 0;
|
void *ret_addr = NULL;
|
uint32_t cur_cpu = 0;
|
int ret = 0;
|
cpu_set_t cpu_set_old;
|
cpu_set_t cpu_set_tmp;
|
struct tee_time_st ts_data;
|
|
memset(&cpu_set_old, 0, sizeof(cpu_set_old));
|
memset(&cpu_set_tmp, 0, sizeof(cpu_set_tmp));
|
memset(&ts_data, 0, sizeof(ts_data));
|
|
if (pthread_mutex_trylock(&teec_bench_mu))
|
return;
|
|
if (!benchmark_check_mode())
|
goto error;
|
|
CPU_ZERO(&cpu_set_old);
|
ret = sched_getaffinity(0, sizeof(cpu_set_old), &cpu_set_old);
|
if (ret)
|
goto error;
|
|
/* stick to the same core while putting timestamp */
|
cur_cpu = sched_getcpu();
|
CPU_ZERO(&cpu_set_tmp);
|
CPU_SET(cur_cpu, &cpu_set_tmp);
|
ret = sched_setaffinity(0, sizeof(cpu_set_tmp), &cpu_set_tmp);
|
if (ret)
|
goto error;
|
|
/* fill timestamp data */
|
if (cur_cpu >= bench_ts_global->cores) {
|
ret = sched_setaffinity(0, sizeof(cpu_set_old), &cpu_set_old);
|
goto error;
|
}
|
|
ret_addr = __builtin_return_address(0);
|
|
cpu_buf = &bench_ts_global->cpu_buf[cur_cpu];
|
ts_i = __sync_fetch_and_add(&cpu_buf->head, 1);
|
ts_data.cnt = read_ccounter();
|
ts_data.addr = (uintptr_t)ret_addr;
|
ts_data.src = TEE_BENCH_CLIENT;
|
cpu_buf->stamps[ts_i & TEE_BENCH_MAX_MASK] = ts_data;
|
|
/* set back affinity mask */
|
sched_setaffinity(0, sizeof(cpu_set_old), &cpu_set_old);
|
|
error:
|
pthread_mutex_unlock(&teec_bench_mu);
|
}
|
