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