ldelf: support TA ftrace

Adds support in ldelf to dump ftrace data.

Reviewed-by: Sumit Garg <sumit.garg@linaro.org>
Acked-by: Jerome Forissier <jerome.forissier@linaro.org>
Signed-off-by: Jens Wiklander <jens.wiklander@linaro.org>

9 files changed, 242 insertions, 23 deletions

diff --git a/ldelf/ftrace.c b/ldelf/ftrace.c
new file mode 100644
index 00000000..690e0e1a
--- /dev/null
+++ b/ldelf/ftrace.c
@@ -0,0 +1,85 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (c) 2019, Linaro Limited
+ */
+
+#include <assert.h>
+#include <printk.h>
+#include <sys/queue.h>
+#include <types_ext.h>
+#include <util.h>
+#include <user_ta_header.h>
+
+#include "ftrace.h"
+#include "ta_elf.h"
+
+#define MIN_FTRACE_BUF_SIZE	1024
+#define MAX_HEADER_STRLEN	128
+
+static struct __ftrace_info *finfo;
+static struct ftrace_buf *fbuf;
+
+bool ftrace_init(void)
+{
+	struct ta_elf *elf = TAILQ_FIRST(&main_elf_queue);
+	TEE_Result res = TEE_SUCCESS;
+	vaddr_t val = 0;
+	int count = 0;
+	size_t fbuf_size = 0;
+
+	res = ta_elf_resolve_sym("__ftrace_info", &val);
+	if (res)
+		return false;
+
+	finfo = (struct __ftrace_info *)val;
+
+	assert(elf && elf->is_main);
+
+	if (SUB_OVERFLOW(finfo->buf_end, finfo->buf_start, &fbuf_size))
+		return false;
+
+	if (fbuf_size < MIN_FTRACE_BUF_SIZE) {
+		DMSG("ftrace buffer too small");
+		return false;
+	}
+
+	fbuf = (struct ftrace_buf *)finfo->buf_start;
+	fbuf->head_off = sizeof(struct ftrace_buf);
+	count = snprintk((char *)fbuf + fbuf->head_off, MAX_HEADER_STRLEN,
+			 "Function graph for TA: %pUl @ %lx\n",
+			 (void *)&elf->uuid, elf->load_addr);
+	assert(count < MAX_HEADER_STRLEN);
+
+	fbuf->ret_func_ptr = finfo->ret_ptr;
+	fbuf->ret_idx = 0;
+	fbuf->lr_idx = 0;
+	fbuf->buf_off = fbuf->head_off + count;
+	fbuf->curr_size = 0;
+	fbuf->max_size = fbuf_size - sizeof(struct ftrace_buf) - count;
+
+	return true;
+}
+
+void ftrace_copy_buf(void *pctx, void (*copy_func)(void *pctx, void *b,
+						   size_t bl))
+{
+	if (fbuf) {
+		struct ta_elf *elf = TAILQ_FIRST(&main_elf_queue);
+		size_t dump_size = fbuf->buf_off - fbuf->head_off +
+				   fbuf->curr_size;
+
+		assert(elf && elf->is_main);
+		copy_func(pctx, (char *)fbuf + fbuf->head_off, dump_size);
+	}
+}
+
+void ftrace_map_lr(uint64_t *lr)
+{
+	if (fbuf) {
+		if (*lr == fbuf->ret_func_ptr &&
+		    fbuf->lr_idx < fbuf->ret_idx) {
+			fbuf->lr_idx++;
+			*lr = fbuf->ret_stack[fbuf->ret_idx - fbuf->lr_idx];
+		}
+	}
+}
diff --git a/ldelf/ftrace.h b/ldelf/ftrace.h
new file mode 100644
index 00000000..95299845
--- /dev/null
+++ b/ldelf/ftrace.h
@@ -0,0 +1,23 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (c) 2019, Linaro Limited
+ */
+
+#ifndef FTRACE_H
+#define FTRACE_H
+
+#include <types_ext.h>
+
+bool ftrace_init(void);
+void ftrace_copy_buf(void *pctx, void (*copy_func)(void *pctx, void *b,
+						   size_t bl));
+#ifdef CFG_TA_FTRACE_SUPPORT
+void ftrace_map_lr(uint64_t *lr);
+#else
+static inline void ftrace_map_lr(uint64_t *lr __unused)
+{
+}
+#endif
+
+#endif /*FTRACE_H*/
+
diff --git a/ldelf/include/ldelf.h b/ldelf/include/ldelf.h
index 20654e88..e2c207bb 100644
--- a/ldelf/include/ldelf.h
+++ b/ldelf/include/ldelf.h
@@ -14,12 +14,13 @@
 
 /*
  * struct ldelf_arg - argument for ldelf
- * @uuid:	[in] UUID of TA to load
- * @is_32bit:	[out] 1 if a 32bit TA or 0 if a 64bit TA
- * @flags:	[out] Flags field of TA header
- * @entry_func:	[out] TA entry function
- * @stack_ptr:	[out] TA stack pointer
- * @dump_entry: [out] Dump TA mappings and stack trace
+ * @uuid:	  [in] UUID of TA to load
+ * @is_32bit:	  [out] 1 if a 32bit TA or 0 if a 64bit TA
+ * @flags:	  [out] Flags field of TA header
+ * @entry_func:	  [out] TA entry function
+ * @stack_ptr:	  [out] TA stack pointer
+ * @dump_entry:	  [out] Dump TA mappings and stack trace
+ * @ftrace_entry: [out] Dump TA mappings and ftrace buffer
  */
 struct ldelf_arg {
 	TEE_UUID uuid;
@@ -28,6 +29,7 @@ struct ldelf_arg {
 	uint64_t entry_func;
 	uint64_t stack_ptr;
 	uint64_t dump_entry;
+	uint64_t ftrace_entry;
 };
 
 #define DUMP_MAP_READ	BIT(0)
diff --git a/ldelf/main.c b/ldelf/main.c
index d5128479..ecc07adb 100644
--- a/ldelf/main.c
+++ b/ldelf/main.c
@@ -6,17 +6,27 @@
 #include <assert.h>
 #include <ldelf.h>
 #include <malloc.h>
+#include <printk.h>
+#include <string.h>
 #include <sys/queue.h>
 #include <tee_api_types.h>
 #include <trace.h>
 #include <types_ext.h>
+#include <util.h>
 
-#include "ta_elf.h"
+#include "ftrace.h"
 #include "sys.h"
+#include "ta_elf.h"
 
 static size_t mpool_size = 2 * SMALL_PAGE_SIZE;
 static vaddr_t mpool_base;
 
+static void __printf(2, 0) print_to_console(void *pctx __unused,
+					    const char *fmt, va_list ap)
+{
+	trace_vprintf(NULL, 0, TRACE_ERROR, true, fmt, ap);
+}
+
 static void __noreturn __maybe_unused dump_ta_state(struct dump_entry_arg *arg)
 {
 	struct ta_elf *elf = TAILQ_FIRST(&main_elf_queue);
@@ -26,8 +36,8 @@ static void __noreturn __maybe_unused dump_ta_state(struct dump_entry_arg *arg)
 	EMSG_RAW(" arch: %s", elf->is_32bit ? "arm" : "aarch64");
 
 
-	ta_elf_print_mappings(&main_elf_queue, arg->num_maps, arg->maps,
-			      mpool_base);
+	ta_elf_print_mappings(NULL, print_to_console, &main_elf_queue,
+			      arg->num_maps, arg->maps, mpool_base);
 
 	if (arg->is_arm32)
 		ta_elf_stack_trace_a32(arg->arm32.regs);
@@ -38,6 +48,60 @@ static void __noreturn __maybe_unused dump_ta_state(struct dump_entry_arg *arg)
 	sys_return_cleanup();
 }
 
+#ifdef CFG_TA_FTRACE_SUPPORT
+struct print_buf_ctx {
+	char *buf;
+	size_t blen;
+	size_t ret;
+};
+
+static void __printf(2, 0) print_to_pbuf(void *pctx, const char *fmt,
+					 va_list ap)
+{
+	struct print_buf_ctx *pbuf = pctx;
+	char *buf = NULL;
+	size_t blen = 0;
+	int ret = 0;
+
+	if (pbuf->buf && pbuf->blen > pbuf->ret) {
+		buf = pbuf->buf + pbuf->ret;
+		blen = pbuf->blen - pbuf->ret;
+	}
+
+	ret = vsnprintk(buf, blen, fmt, ap);
+	assert(ret >= 0);
+
+	pbuf->ret += ret;
+}
+
+static void copy_to_pbuf(void *pctx, void *b, size_t bl)
+{
+	struct print_buf_ctx *pbuf = pctx;
+	char *buf = NULL;
+	size_t blen = 0;
+
+	if (pbuf->buf && pbuf->blen > pbuf->ret) {
+		buf = pbuf->buf + pbuf->ret;
+		blen = pbuf->blen - pbuf->ret;
+		memcpy(buf, b, MIN(blen, bl));
+	}
+
+	pbuf->ret += bl;
+
+}
+
+static void __noreturn ftrace_dump(void *buf, size_t *blen)
+{
+	struct print_buf_ctx pbuf = { .buf = buf, .blen = *blen };
+
+	ta_elf_print_mappings(&pbuf, print_to_pbuf, &main_elf_queue,
+			      0, NULL, mpool_base);
+	ftrace_copy_buf(&pbuf, copy_to_pbuf);
+	*blen = pbuf.ret;
+	sys_return_cleanup();
+}
+#endif
+
 /*
  * ldelf()- Loads ELF into memory
  * @arg:	Argument passing to/from TEE Core
@@ -75,6 +139,12 @@ void ldelf(struct ldelf_arg *arg)
 		ta_elf_finalize_mappings(elf);
 	}
 
+	arg->ftrace_entry = 0;
+#ifdef CFG_TA_FTRACE_SUPPORT
+	if (ftrace_init())
+		arg->ftrace_entry = (vaddr_t)(void *)ftrace_dump;
+#endif
+
 	TAILQ_FOREACH(elf, &main_elf_queue, link)
 		DMSG("ELF (%pUl) at %#"PRIxVA,
 		     (void *)&elf->uuid, elf->load_addr);
diff --git a/ldelf/sub.mk b/ldelf/sub.mk
index 4ac9bd3e..89abc469 100644
--- a/ldelf/sub.mk
+++ b/ldelf/sub.mk
@@ -7,3 +7,4 @@ srcs-y += ta_elf.c
 srcs-y += ta_elf_rel.c
 srcs-$(CFG_UNWIND) += unwind_arm32.c
 srcs-$(CFG_UNWIND) += unwind_arm64.c
+srcs-$(CFG_TA_FTRACE_SUPPORT) += ftrace.c
diff --git a/ldelf/ta_elf.c b/ldelf/ta_elf.c
index fa8c06a2..9d187c7f 100644
--- a/ldelf/ta_elf.c
+++ b/ldelf/ta_elf.c
@@ -948,7 +948,18 @@ void ta_elf_finalize_mappings(struct ta_elf *elf)
 	}
 }
 
-static void print_seg(size_t idx __maybe_unused, int elf_idx __maybe_unused,
+static void __printf(3, 4) print_wrapper(void *pctx, print_func_t print_func,
+					 const char *fmt, ...)
+{
+	va_list ap;
+
+	va_start(ap, fmt);
+	print_func(pctx, fmt, ap);
+	va_end(ap);
+}
+
+static void print_seg(void *pctx, print_func_t print_func,
+		      size_t idx __maybe_unused, int elf_idx __maybe_unused,
 		      vaddr_t va __maybe_unused, paddr_t pa __maybe_unused,
 		      size_t sz __maybe_unused, uint32_t flags)
 {
@@ -976,8 +987,9 @@ static void print_seg(size_t idx __maybe_unused, int elf_idx __maybe_unused,
 	if (flags & DUMP_MAP_SECURE)
 		flags_str[3] = 's';
 
-	EMSG_RAW("region %2zu: va 0x%0*"PRIxVA" pa 0x%0*"PRIxPA" size 0x%06zx flags %s%s",
-		 idx, width, va, width, pa, sz, flags_str, desc);
+	print_wrapper(pctx, print_func,
+		      "region %2zu: va 0x%0*"PRIxVA" pa 0x%0*"PRIxPA" size 0x%06zx flags %s%s\n",
+		      idx, width, va, width, pa, sz, flags_str, desc);
 }
 
 static bool get_next_in_order(struct ta_elf_queue *elf_queue,
@@ -1027,7 +1039,8 @@ static bool get_next_in_order(struct ta_elf_queue *elf_queue,
 	return true;
 }
 
-void ta_elf_print_mappings(struct ta_elf_queue *elf_queue, size_t num_maps,
+void ta_elf_print_mappings(void *pctx, print_func_t print_func,
+			   struct ta_elf_queue *elf_queue, size_t num_maps,
 			   struct dump_map *maps, vaddr_t mpool_base)
 {
 	struct segment *seg = NULL;
@@ -1085,8 +1098,9 @@ void ta_elf_print_mappings(struct ta_elf_queue *elf_queue, size_t num_maps,
 			} else if (maps[map_idx].va < va) {
 				if (maps[map_idx].va == mpool_base)
 					f |= DUMP_MAP_LDELF;
-				print_seg(idx, -1, maps[map_idx].va,
-					  maps[map_idx].pa, maps[map_idx].sz,
+				print_seg(pctx, print_func, idx, -1,
+					  maps[map_idx].va, maps[map_idx].pa,
+					  maps[map_idx].sz,
 					  maps[map_idx].flags | f);
 				idx++;
 			}
@@ -1104,7 +1118,7 @@ void ta_elf_print_mappings(struct ta_elf_queue *elf_queue, size_t num_maps,
 		if (seg->flags & PF_X)
 			flags |= DUMP_MAP_EXEC;
 
-		print_seg(idx, elf_idx, va, offs, sz, flags);
+		print_seg(pctx, print_func, idx, elf_idx, va, offs, sz, flags);
 		idx++;
 
 		if (!get_next_in_order(elf_queue, &elf, &seg, &elf_idx))
@@ -1113,8 +1127,9 @@ void ta_elf_print_mappings(struct ta_elf_queue *elf_queue, size_t num_maps,
 
 	elf_idx = 0;
 	TAILQ_FOREACH(elf, elf_queue, link) {
-		EMSG_RAW(" [%zu] %pUl @ 0x%0*" PRIxVA,
-			 elf_idx, (void *)&elf->uuid, 8, elf->load_addr);
+		print_wrapper(pctx, print_func,
+			      " [%zu] %pUl @ 0x%0*"PRIxVA"\n",
+			      elf_idx, (void *)&elf->uuid, 8, elf->load_addr);
 		elf_idx++;
 	}
 }
diff --git a/ldelf/ta_elf.h b/ldelf/ta_elf.h
index 45b23b60..f3e3d3eb 100644
--- a/ldelf/ta_elf.h
+++ b/ldelf/ta_elf.h
@@ -71,6 +71,9 @@ struct ta_elf {
 
 TAILQ_HEAD(ta_elf_queue, ta_elf);
 
+typedef void (*print_func_t)(void *pctx, const char *fmt, va_list ap)
+	__printf(2, 0);
+
 extern struct ta_elf_queue main_elf_queue;
 
 void ta_elf_load_main(const TEE_UUID *uuid, uint32_t *is_32bit,
@@ -79,8 +82,10 @@ void ta_elf_load_dependency(struct ta_elf *elf, bool is_32bit);
 void ta_elf_relocate(struct ta_elf *elf);
 void ta_elf_finalize_mappings(struct ta_elf *elf);
 
-void ta_elf_print_mappings(struct ta_elf_queue *elf_queue, size_t num_maps,
+void ta_elf_print_mappings(void *pctx, print_func_t print_func,
+			   struct ta_elf_queue *elf_queue, size_t num_maps,
 			   struct dump_map *maps, vaddr_t mpool_base);
+
 #ifdef CFG_UNWIND
 void ta_elf_stack_trace_a32(uint32_t regs[16]);
 void ta_elf_stack_trace_a64(uint64_t fp, uint64_t sp, uint64_t pc);
@@ -90,4 +95,7 @@ static inline void ta_elf_stack_trace_a64(uint64_t fp __unused,
 					  uint64_t sp __unused,
 					  uint64_t pc __unused) { }
 #endif /*CFG_UNWIND*/
+
+TEE_Result ta_elf_resolve_sym(const char *name, vaddr_t *val);
+
 #endif /*TA_ELF_H*/
diff --git a/ldelf/ta_elf_rel.c b/ldelf/ta_elf_rel.c
index c8d80b31..6aca0504 100644
--- a/ldelf/ta_elf_rel.c
+++ b/ldelf/ta_elf_rel.c
@@ -50,7 +50,7 @@ static bool __resolve_sym(struct ta_elf *elf, unsigned int bind,
 	return true;
 }
 
-static void resolve_sym(const char *name, vaddr_t *val)
+TEE_Result ta_elf_resolve_sym(const char *name, vaddr_t *val)
 {
 	uint32_t hash = elf_hash(name);
 	struct ta_elf *elf = NULL;
@@ -77,7 +77,7 @@ static void resolve_sym(const char *name, vaddr_t *val)
 						  sym[n].st_shndx,
 						  sym[n].st_name,
 						  sym[n].st_value, name, val))
-					return;
+					return TEE_SUCCESS;
 			}
 		} else {
 			Elf64_Sym *sym = elf->dynsymtab;
@@ -89,11 +89,20 @@ static void resolve_sym(const char *name, vaddr_t *val)
 						  sym[n].st_shndx,
 						  sym[n].st_name,
 						  sym[n].st_value, name, val))
-					return;
+					return TEE_SUCCESS;
 			}
 		}
 	}
-	err(TEE_ERROR_ITEM_NOT_FOUND, "Symbol %s not found", name);
+
+	return TEE_ERROR_ITEM_NOT_FOUND;
+}
+
+static void resolve_sym(const char *name, vaddr_t *val)
+{
+	TEE_Result res = ta_elf_resolve_sym(name, val);
+
+	if (res)
+		err(res, "Symbol %s not found", name);
 }
 
 static void e32_process_dyn_rel(const Elf32_Sym *sym_tab, size_t num_syms,
diff --git a/ldelf/unwind_arm64.c b/ldelf/unwind_arm64.c
index 504a8f4a..74e0a438 100644
--- a/ldelf/unwind_arm64.c
+++ b/ldelf/unwind_arm64.c
@@ -33,6 +33,7 @@
 #include <trace.h>
 #include <types_ext.h>
 
+#include "ftrace.h"
 #include "unwind.h"
 
 static bool copy_in_reg(uint64_t *reg, vaddr_t addr)
@@ -63,6 +64,9 @@ static bool unwind_stack_arm64(struct unwind_state_arm64 *frame,
 	/* LR (X30) */
 	if (!copy_in_reg(&frame->pc, fp + 8))
 		return false;
+
+	ftrace_map_lr(&frame->pc);
+
 	frame->pc -= 4;
 
 	return true;
@@ -72,6 +76,8 @@ void print_stack_arm64(struct unwind_state_arm64 *state,
 		       vaddr_t stack, size_t stack_size)
 {
 	trace_printf_helper_raw(TRACE_ERROR, true, "Call stack:");
+
+	ftrace_map_lr(&state->pc);
 	do {
 		trace_printf_helper_raw(TRACE_ERROR, true, " 0x%016" PRIx64,
 					state->pc);