1 // SPDX-License-Identifier: BSD-2-Clause 2 /* 3 * Copyright (c) 2019, Linaro Limited 4 */ 5 6 #include <assert.h> 7 #include <printk.h> 8 #include <string.h> 9 #include <sys/queue.h> 10 #include <types_ext.h> 11 #include <user_ta_header.h> 12 #include <util.h> 13 14 #include "ftrace.h" 15 #include "ta_elf.h" 16 17 #define MIN_FTRACE_BUF_SIZE 1024 18 #define MAX_HEADER_STRLEN 128 19 20 static struct ftrace_buf *fbuf; 21 22 bool ftrace_init(struct ftrace_buf **fbuf_ptr) 23 { 24 struct __ftrace_info *finfo = NULL; 25 struct ta_elf *elf = TAILQ_FIRST(&main_elf_queue); 26 TEE_Result res = TEE_SUCCESS; 27 vaddr_t val = 0; 28 int count = 0; 29 size_t fbuf_size = 0; 30 size_t pad = 0; 31 char *p = NULL; 32 char magic[] = { 'F', 'T', 'R', 'A', 'C', 'E', 0x00, 0x01 }; 33 34 res = ta_elf_resolve_sym("__ftrace_info", &val, NULL, NULL); 35 if (res) 36 return false; 37 38 finfo = (struct __ftrace_info *)val; 39 40 assert(elf && elf->is_main); 41 42 if (SUB_OVERFLOW(finfo->buf_end.ptr64, finfo->buf_start.ptr64, 43 &fbuf_size)) 44 return false; 45 46 if (fbuf_size < MIN_FTRACE_BUF_SIZE) { 47 DMSG("ftrace buffer too small"); 48 return false; 49 } 50 51 fbuf = (struct ftrace_buf *)(vaddr_t)finfo->buf_start.ptr64; 52 fbuf->head_off = sizeof(struct ftrace_buf); 53 p = (char *)fbuf + fbuf->head_off; 54 count = snprintk(p, MAX_HEADER_STRLEN, 55 "Function graph for TA: %pUl @ %lx\n", 56 (void *)&elf->uuid, elf->load_addr); 57 assert(count < MAX_HEADER_STRLEN); 58 p += count; 59 60 fbuf->ret_func_ptr = finfo->ret_ptr.ptr64; 61 fbuf->ret_idx = 0; 62 fbuf->lr_idx = 0; 63 fbuf->suspend_time = 0; 64 fbuf->buf_off = fbuf->head_off + count; 65 /* For proper alignment of uint64_t values in the ftrace buffer */ 66 pad = 8 - (vaddr_t)p % 8; 67 if (pad == 8) 68 pad = 0; 69 while (pad--) { 70 *p++ = 0; 71 fbuf->buf_off++; 72 count++; 73 } 74 /* Delimiter for easier decoding */ 75 memcpy(p, magic, sizeof(magic)); 76 fbuf->buf_off += sizeof(magic); 77 count += sizeof(magic); 78 fbuf->curr_idx = 0; 79 fbuf->max_size = fbuf_size - sizeof(struct ftrace_buf) - count; 80 fbuf->syscall_trace_enabled = false; 81 fbuf->syscall_trace_suspended = false; 82 83 *fbuf_ptr = fbuf; 84 85 return true; 86 } 87 88 void ftrace_copy_buf(void *pctx, void (*copy_func)(void *pctx, void *b, 89 size_t bl)) 90 { 91 if (fbuf) { 92 struct ta_elf *elf = TAILQ_FIRST(&main_elf_queue); 93 char *hstart = (char *)fbuf + fbuf->head_off; 94 char *cstart = (char *)fbuf + fbuf->buf_off; 95 char *ccurr = cstart + fbuf->curr_idx * sizeof(uint64_t); 96 size_t csize = 0; 97 size_t dump_size = 0; 98 char *end = NULL; 99 100 assert(elf && elf->is_main); 101 102 if (fbuf->overflow) 103 csize = fbuf->max_size; 104 else 105 csize = fbuf->curr_idx * sizeof(uint64_t); 106 dump_size = fbuf->buf_off - fbuf->head_off + csize; 107 end = hstart + dump_size; 108 109 /* Header */ 110 copy_func(pctx, hstart, fbuf->buf_off - fbuf->head_off); 111 if (fbuf->overflow) { 112 /* From current index to end of circular buffer */ 113 copy_func(pctx, ccurr, end - ccurr); 114 } 115 /* From start of circular buffer to current index */ 116 copy_func(pctx, cstart, ccurr - cstart); 117 } 118 } 119 120 void ftrace_map_lr(uint64_t *lr) 121 { 122 if (fbuf) { 123 if (*lr == fbuf->ret_func_ptr && 124 fbuf->lr_idx < fbuf->ret_idx) { 125 fbuf->lr_idx++; 126 *lr = fbuf->ret_stack[fbuf->ret_idx - fbuf->lr_idx]; 127 } 128 } 129 } 130