1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3 * Copyright (c) 2016, Linaro Limited
4 */
5
6 #include <kernel/misc.h>
7 #include <kernel/msg_param.h>
8 #include <kernel/pseudo_ta.h>
9 #include <kernel/user_ta.h>
10 #include <kernel/thread.h>
11 #include <mm/core_memprot.h>
12 #include <mm/mobj.h>
13 #include <optee_rpc_cmd.h>
14 #include <pta_gprof.h>
15 #include <string.h>
16
gprof_send_rpc(TEE_UUID * uuid,void * buf,size_t len,uint32_t * id)17 static TEE_Result gprof_send_rpc(TEE_UUID *uuid, void *buf, size_t len,
18 uint32_t *id)
19 {
20 struct thread_param params[3] = { };
21 struct mobj *mobj;
22 TEE_Result res = TEE_ERROR_GENERIC;
23 char *va;
24
25 mobj = thread_rpc_alloc_payload(sizeof(*uuid) + len);
26 if (!mobj)
27 return TEE_ERROR_OUT_OF_MEMORY;
28
29 va = mobj_get_va(mobj, 0, sizeof(*uuid) + len);
30 if (!va)
31 goto exit;
32
33 memcpy(va, uuid, sizeof(*uuid));
34 memcpy(va + sizeof(*uuid), buf, len);
35
36 params[0] = THREAD_PARAM_VALUE(INOUT, *id, 0, 0);
37 params[1] = THREAD_PARAM_MEMREF(IN, mobj, 0, sizeof(*uuid));
38 params[2] = THREAD_PARAM_MEMREF(IN, mobj, sizeof(*uuid), len);
39
40 res = thread_rpc_cmd(OPTEE_RPC_CMD_GPROF, 3, params);
41 if (res != TEE_SUCCESS)
42 goto exit;
43
44 *id = (uint32_t)params[0].u.value.a;
45 exit:
46 thread_rpc_free_payload(mobj);
47 return res;
48 }
49
gprof_send(struct ts_session * s,uint32_t param_types,TEE_Param params[TEE_NUM_PARAMS])50 static TEE_Result gprof_send(struct ts_session *s, uint32_t param_types,
51 TEE_Param params[TEE_NUM_PARAMS])
52 {
53 uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INOUT,
54 TEE_PARAM_TYPE_MEMREF_INPUT,
55 TEE_PARAM_TYPE_NONE,
56 TEE_PARAM_TYPE_NONE);
57
58 if (exp_pt != param_types)
59 return TEE_ERROR_BAD_PARAMETERS;
60
61 return gprof_send_rpc(&s->ctx->uuid, params[1].memref.buffer,
62 params[1].memref.size, ¶ms[0].value.a);
63 }
64
gprof_start_pc_sampling(struct ts_session * s,uint32_t param_types,TEE_Param params[TEE_NUM_PARAMS])65 static TEE_Result gprof_start_pc_sampling(struct ts_session *s,
66 uint32_t param_types,
67 TEE_Param params[TEE_NUM_PARAMS])
68 {
69 uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_MEMREF_INOUT,
70 TEE_PARAM_TYPE_VALUE_INPUT,
71 TEE_PARAM_TYPE_NONE,
72 TEE_PARAM_TYPE_NONE);
73 struct sample_buf *sbuf = NULL;
74 uint32_t offset = 0;
75 uint32_t scale = 0;
76 uint32_t len = 0;
77 uaddr_t buf = 0;
78
79 if (exp_pt != param_types)
80 return TEE_ERROR_BAD_PARAMETERS;
81
82 if (s->sbuf) {
83 DMSG("PC sampling already started");
84 return TEE_ERROR_BAD_STATE;
85 }
86
87 buf = (uaddr_t)params[0].memref.buffer;
88 len = params[0].memref.size;
89 offset = params[1].value.a;
90 scale = params[1].value.b;
91
92 sbuf = calloc(1, sizeof(*sbuf));
93 if (!sbuf)
94 return TEE_ERROR_OUT_OF_MEMORY;
95
96 sbuf->samples = (uint16_t *)buf;
97 sbuf->nsamples = len / sizeof(*sbuf->samples);
98 sbuf->offset = offset;
99 sbuf->scale = scale;
100 sbuf->freq = read_cntfrq();
101 sbuf->enabled = true;
102 s->sbuf = sbuf;
103
104 return TEE_SUCCESS;
105 }
106
gprof_stop_pc_sampling(struct ts_session * s,uint32_t param_types,TEE_Param params[TEE_NUM_PARAMS])107 static TEE_Result gprof_stop_pc_sampling(struct ts_session *s,
108 uint32_t param_types,
109 TEE_Param params[TEE_NUM_PARAMS])
110 {
111 uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_OUTPUT,
112 TEE_PARAM_TYPE_NONE,
113 TEE_PARAM_TYPE_NONE,
114 TEE_PARAM_TYPE_NONE);
115 struct sample_buf *sbuf = NULL;
116 uint32_t rate = 0;
117
118 if (exp_pt != param_types)
119 return TEE_ERROR_BAD_PARAMETERS;
120
121 sbuf = s->sbuf;
122 if (!sbuf)
123 return TEE_ERROR_BAD_STATE;
124 assert(sbuf->samples);
125
126 /* Stop sampling */
127 if (sbuf->enabled)
128 sbuf->enabled = false;
129
130 rate = ((uint64_t)sbuf->count * sbuf->freq) / sbuf->usr;
131 params[0].value.a = rate;
132
133 DMSG("TA sampling stats: sample count=%" PRIu32 " user time=%" PRIu64
134 " cntfrq=%" PRIu32 " rate=%" PRIu32, sbuf->count, sbuf->usr,
135 sbuf->freq, rate);
136
137 free(sbuf);
138 s->sbuf = NULL;
139
140 return TEE_SUCCESS;
141 }
142
143 /*
144 * Trusted Application Entry Points
145 */
146
open_session(uint32_t param_types __unused,TEE_Param params[TEE_NUM_PARAMS]__unused,void ** sess_ctx __unused)147 static TEE_Result open_session(uint32_t param_types __unused,
148 TEE_Param params[TEE_NUM_PARAMS] __unused,
149 void **sess_ctx __unused)
150 {
151 struct ts_session *s = ts_get_calling_session();
152
153 /* Check that we're called from a user TA */
154 if (!s)
155 return TEE_ERROR_ACCESS_DENIED;
156 if (!is_user_ta_ctx(s->ctx))
157 return TEE_ERROR_ACCESS_DENIED;
158
159 return TEE_SUCCESS;
160 }
161
invoke_command(void * sess_ctx __unused,uint32_t cmd_id,uint32_t param_types,TEE_Param params[TEE_NUM_PARAMS])162 static TEE_Result invoke_command(void *sess_ctx __unused, uint32_t cmd_id,
163 uint32_t param_types,
164 TEE_Param params[TEE_NUM_PARAMS])
165 {
166 struct ts_session *s = ts_get_calling_session();
167
168 switch (cmd_id) {
169 case PTA_GPROF_SEND:
170 return gprof_send(s, param_types, params);
171 case PTA_GPROF_START_PC_SAMPLING:
172 return gprof_start_pc_sampling(s, param_types, params);
173 case PTA_GPROF_STOP_PC_SAMPLING:
174 return gprof_stop_pc_sampling(s, param_types, params);
175 default:
176 break;
177 }
178 return TEE_ERROR_NOT_IMPLEMENTED;
179 }
180
181 pseudo_ta_register(.uuid = PTA_GPROF_UUID, .name = "gprof",
182 .flags = PTA_DEFAULT_FLAGS,
183 .open_session_entry_point = open_session,
184 .invoke_command_entry_point = invoke_command);
185