1 /*
2 * Copyright (c) 2017, Linaro Limited
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 *
8 * 1. Redistributions of source code must retain the above copyright notice,
9 * this list of conditions and the following disclaimer.
10 *
11 * 2. Redistributions in binary form must reproduce the above copyright notice,
12 * this list of conditions and the following disclaimer in the documentation
13 * and/or other materials provided with the distribution.
14 *
15 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
19 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 * POSSIBILITY OF SUCH DAMAGE.
26 */
27 #include <err.h>
28 #include <fcntl.h>
29 #include <math.h>
30 #include <pthread.h>
31 #include <sched.h>
32 #include <stdbool.h>
33 #include <stdio.h>
34 #include <stdlib.h>
35 #include <string.h>
36 #include <sys/mman.h>
37 #include <sys/stat.h>
38 #include <tee_bench.h>
39 #include <tee_client_api.h>
40 #include <unistd.h>
41
42 #include "teec_benchmark.h"
43
44 struct tee_ts_global *bench_ts_global;
45 static const TEEC_UUID pta_benchmark_uuid = PTA_BENCHMARK_UUID;
46
47 static TEEC_Context bench_ctx;
48 static TEEC_Session bench_sess;
49
50 static pthread_mutex_t teec_bench_mu = PTHREAD_MUTEX_INITIALIZER;
51
52 /* Cycle counter */
read_ccounter(void)53 static inline uint64_t read_ccounter(void)
54 {
55 uint64_t ccounter = 0;
56 #ifdef __aarch64__
57 asm volatile("mrs %0, PMCCNTR_EL0" : "=r"(ccounter));
58 #else
59 asm volatile("mrc p15, 0, %0, c9, c13, 0" : "=r"(ccounter));
60 #endif
61 return ccounter * TEE_BENCH_DIVIDER;
62 }
63
benchmark_pta_open(void)64 static TEEC_Result benchmark_pta_open(void)
65 {
66 TEEC_Result res = TEEC_ERROR_GENERIC;
67 uint32_t ret_orig = 0;
68
69 res = TEEC_InitializeContext(NULL, &bench_ctx);
70 if (res != TEEC_SUCCESS)
71 return res;
72
73 res = TEEC_OpenSession(&bench_ctx, &bench_sess,
74 &pta_benchmark_uuid,
75 TEEC_LOGIN_PUBLIC, NULL, NULL, &ret_orig);
76 if (res != TEEC_SUCCESS) {
77 TEEC_FinalizeContext(&bench_ctx);
78 return res;
79 }
80
81 return res;
82 }
83
benchmark_pta_close(void)84 static void benchmark_pta_close(void)
85 {
86 TEEC_CloseSession(&bench_sess);
87 TEEC_FinalizeContext(&bench_ctx);
88 }
89
benchmark_get_bench_buf_paddr(uint64_t * paddr_ts_buf,uint64_t * size)90 static TEEC_Result benchmark_get_bench_buf_paddr(uint64_t *paddr_ts_buf,
91 uint64_t *size)
92 {
93 TEEC_Result res = TEEC_ERROR_GENERIC;
94 uint32_t ret_orig = 0;
95 TEEC_Operation op;
96
97 memset(&op, 0, sizeof(op));
98
99 res = benchmark_pta_open();
100 if (res != TEEC_SUCCESS)
101 return res;
102
103 op.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_OUTPUT, TEEC_NONE,
104 TEEC_NONE, TEEC_NONE);
105
106 res = TEEC_InvokeCommand(&bench_sess, BENCHMARK_CMD_GET_MEMREF,
107 &op, &ret_orig);
108 if (res != TEEC_SUCCESS)
109 return res;
110
111 *paddr_ts_buf = op.params[0].value.a;
112 *size = op.params[0].value.b;
113
114 benchmark_pta_close();
115
116 return res;
117 }
118
mmap_paddr(intptr_t paddr,uint64_t size)119 static void *mmap_paddr(intptr_t paddr, uint64_t size)
120 {
121 int devmem = 0;
122 off_t offset = 0;
123 off_t page_addr = 0;
124 intptr_t *hw_addr = NULL;
125
126 devmem = open("/dev/mem", O_RDWR);
127 if (!devmem)
128 return NULL;
129
130 offset = (off_t)paddr % getpagesize();
131 page_addr = (off_t)(paddr - offset);
132
133 hw_addr = (intptr_t *)mmap(0, size + offset, PROT_READ|PROT_WRITE,
134 MAP_SHARED, devmem, page_addr);
135 if (hw_addr == MAP_FAILED) {
136 close(devmem);
137 return NULL;
138 }
139
140 close(devmem);
141 return (hw_addr + offset);
142 }
143
144 /* check if we are in benchmark mode */
benchmark_check_mode(void)145 static bool benchmark_check_mode(void)
146 {
147 uint64_t ts_buf_raw = 0;
148 uint64_t ts_buf_size = 0;
149 bool res = true;
150
151 if (!bench_ts_global) {
152 /* receive buffer from Benchmark PTA and register it */
153 benchmark_get_bench_buf_paddr(&ts_buf_raw, &ts_buf_size);
154 if (ts_buf_raw && ts_buf_size) {
155 bench_ts_global = mmap_paddr(ts_buf_raw, ts_buf_size);
156 res = (bench_ts_global) ? true : false;
157 } else {
158 res = false;
159 }
160 }
161
162 return res;
163 }
164
165 /* Adding timestamp to buffer */
bm_timestamp(void)166 void bm_timestamp(void)
167 {
168 struct tee_ts_cpu_buf *cpu_buf = NULL;
169 uint64_t ts_i = 0;
170 void *ret_addr = NULL;
171 uint32_t cur_cpu = 0;
172 int ret = 0;
173 cpu_set_t cpu_set_old;
174 cpu_set_t cpu_set_tmp;
175 struct tee_time_st ts_data;
176
177 memset(&cpu_set_old, 0, sizeof(cpu_set_old));
178 memset(&cpu_set_tmp, 0, sizeof(cpu_set_tmp));
179 memset(&ts_data, 0, sizeof(ts_data));
180
181 if (pthread_mutex_trylock(&teec_bench_mu))
182 return;
183
184 if (!benchmark_check_mode())
185 goto error;
186
187 CPU_ZERO(&cpu_set_old);
188 ret = sched_getaffinity(0, sizeof(cpu_set_old), &cpu_set_old);
189 if (ret)
190 goto error;
191
192 /* stick to the same core while putting timestamp */
193 cur_cpu = sched_getcpu();
194 CPU_ZERO(&cpu_set_tmp);
195 CPU_SET(cur_cpu, &cpu_set_tmp);
196 ret = sched_setaffinity(0, sizeof(cpu_set_tmp), &cpu_set_tmp);
197 if (ret)
198 goto error;
199
200 /* fill timestamp data */
201 if (cur_cpu >= bench_ts_global->cores) {
202 ret = sched_setaffinity(0, sizeof(cpu_set_old), &cpu_set_old);
203 goto error;
204 }
205
206 ret_addr = __builtin_return_address(0);
207
208 cpu_buf = &bench_ts_global->cpu_buf[cur_cpu];
209 ts_i = __sync_fetch_and_add(&cpu_buf->head, 1);
210 ts_data.cnt = read_ccounter();
211 ts_data.addr = (uintptr_t)ret_addr;
212 ts_data.src = TEE_BENCH_CLIENT;
213 cpu_buf->stamps[ts_i & TEE_BENCH_MAX_MASK] = ts_data;
214
215 /* set back affinity mask */
216 sched_setaffinity(0, sizeof(cpu_set_old), &cpu_set_old);
217
218 error:
219 pthread_mutex_unlock(&teec_bench_mu);
220 }
221
222