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
ftrace_init(struct ftrace_buf ** fbuf_ptr)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
ftrace_copy_buf(void * pctx,void (* copy_func)(void * pctx,void * b,size_t bl))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 if (!fbuf->dump_id)
111 copy_func(pctx, hstart, fbuf->buf_off - fbuf->head_off);
112 if (fbuf->overflow) {
113 /* From current index to end of circular buffer */
114 copy_func(pctx, ccurr, end - ccurr);
115 }
116 /* From start of circular buffer to current index */
117 copy_func(pctx, cstart, ccurr - cstart);
118 }
119 }
120
ftrace_get_dump_id(void)121 uint32_t ftrace_get_dump_id(void)
122 {
123 if (fbuf)
124 return fbuf->dump_id;
125 else
126 return 0;
127 }
128
ftrace_reset_buf(void)129 void ftrace_reset_buf(void)
130 {
131 if (fbuf) {
132 fbuf->curr_idx = 0;
133 fbuf->overflow = false;
134 }
135 }
136
ftrace_map_lr(uint64_t * lr)137 void ftrace_map_lr(uint64_t *lr)
138 {
139 if (fbuf) {
140 if (*lr == fbuf->ret_func_ptr &&
141 fbuf->lr_idx < fbuf->ret_idx) {
142 fbuf->lr_idx++;
143 *lr = fbuf->ret_stack[fbuf->ret_idx - fbuf->lr_idx];
144 }
145 }
146 }
147