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