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