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 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 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 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 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 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 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