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 <sys/queue.h>
9 #include <types_ext.h>
10 #include <util.h>
11
12 #include "ftrace.h"
13 #include "ta_elf.h"
14
15 #define MIN_FTRACE_BUF_SIZE 1024
16 #define MAX_HEADER_STRLEN 128
17
18 static struct ftrace_buf *fbuf;
19
ftrace_init(struct ftrace_buf ** fbuf_ptr)20 bool ftrace_init(struct ftrace_buf **fbuf_ptr)
21 {
22 struct __ftrace_info *finfo = NULL;
23 struct ta_elf *elf = TAILQ_FIRST(&main_elf_queue);
24 TEE_Result res = TEE_SUCCESS;
25 vaddr_t val = 0;
26 int count = 0;
27 size_t fbuf_size = 0;
28
29 res = ta_elf_resolve_sym("__ftrace_info", &val, NULL, NULL);
30 if (res)
31 return false;
32
33 finfo = (struct __ftrace_info *)val;
34
35 assert(elf && elf->is_main);
36
37 if (SUB_OVERFLOW(finfo->buf_end.ptr64, finfo->buf_start.ptr64,
38 &fbuf_size))
39 return false;
40
41 if (fbuf_size < MIN_FTRACE_BUF_SIZE) {
42 DMSG("ftrace buffer too small");
43 return false;
44 }
45
46 fbuf = (struct ftrace_buf *)(vaddr_t)finfo->buf_start.ptr64;
47 fbuf->head_off = sizeof(struct ftrace_buf);
48 count = snprintk((char *)fbuf + fbuf->head_off, MAX_HEADER_STRLEN,
49 "Function graph for TA: %pUl @ %lx\n",
50 (void *)&elf->uuid, elf->load_addr);
51 assert(count < MAX_HEADER_STRLEN);
52
53 fbuf->ret_func_ptr = finfo->ret_ptr.ptr64;
54 fbuf->ret_idx = 0;
55 fbuf->lr_idx = 0;
56 fbuf->suspend_time = 0;
57 fbuf->buf_off = fbuf->head_off + count;
58 fbuf->curr_size = 0;
59 fbuf->max_size = fbuf_size - sizeof(struct ftrace_buf) - count;
60 fbuf->syscall_trace_enabled = false;
61 fbuf->syscall_trace_suspended = false;
62
63 *fbuf_ptr = fbuf;
64
65 return true;
66 }
67
ftrace_copy_buf(void * pctx,void (* copy_func)(void * pctx,void * b,size_t bl))68 void ftrace_copy_buf(void *pctx, void (*copy_func)(void *pctx, void *b,
69 size_t bl))
70 {
71 if (fbuf) {
72 struct ta_elf *elf = TAILQ_FIRST(&main_elf_queue);
73 size_t dump_size = fbuf->buf_off - fbuf->head_off +
74 fbuf->curr_size;
75
76 assert(elf && elf->is_main);
77 copy_func(pctx, (char *)fbuf + fbuf->head_off, dump_size);
78 }
79 }
80
ftrace_map_lr(uint64_t * lr)81 void ftrace_map_lr(uint64_t *lr)
82 {
83 if (fbuf) {
84 if (*lr == fbuf->ret_func_ptr &&
85 fbuf->lr_idx < fbuf->ret_idx) {
86 fbuf->lr_idx++;
87 *lr = fbuf->ret_stack[fbuf->ret_idx - fbuf->lr_idx];
88 }
89 }
90 }
91