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