ftrace: Add function execution time support

Add support to dump function execution time at corresponding function
exit points in output ftrace buffer.

To achieve proper function execution time we need to exclude TA suspend
time from timestamps, so add corresponding support in TEE core.

Also user mapping must be active to access ftrace buffer, so do that
during TA resume.

Signed-off-by: Sumit Garg <sumit.garg@linaro.org>

core/arch/arm/kernel/thread.c

@@ -533,12 +533,12 @@ static void thread_resume_from_rpc(struct thread_smc_args *args)
 
 	l->curr_thread = n;
 
-	if (is_user_mode(&threads[n].regs))
-		tee_ta_update_session_utime_resume();
-
 	if (threads[n].have_user_map)
 		core_mmu_set_user_map(&threads[n].user_map);
 
+	if (is_user_mode(&threads[n].regs))
+		tee_ta_update_session_utime_resume();
+
 	/*
 	 * Return from RPC to request service of a foreign interrupt must not
 	 * get parameters from non-secure world.

core/arch/arm/kernel/user_ta.c

@@ -303,6 +303,7 @@ static TEE_Result init_with_ldelf(struct tee_ta_session *sess,
 	utc->dump_entry_func = arg->dump_entry;
 #ifdef CFG_TA_FTRACE_SUPPORT
 	utc->ftrace_entry_func = arg->ftrace_entry;
+	sess->fbuf = arg->fbuf;
 	/*
 	 * Enable accesses to frequency register and physical counter
 	 * register in el0 mode required for timestamping during

core/include/kernel/tee_ta_manager.h

@@ -107,6 +107,9 @@ struct tee_ta_session {
 #if defined(CFG_TA_GPROF_SUPPORT)
 	struct sample_buf *sbuf; /* Profiling data (PC sampling) */
 #endif
+#if defined(CFG_TA_FTRACE_SUPPORT)
+	struct ftrace_buf *fbuf; /* ftrace buffer */
+#endif
 };
 
 /* Registered contexts */
@@ -160,14 +163,17 @@ struct tee_ta_session *tee_ta_get_session(uint32_t id, bool exclusive,
 
 void tee_ta_put_session(struct tee_ta_session *sess);
 
-#if defined(CFG_TA_GPROF_SUPPORT)
-void tee_ta_gprof_sample_pc(vaddr_t pc);
+#if defined(CFG_TA_GPROF_SUPPORT) || defined(CFG_TA_FTRACE_SUPPORT)
 void tee_ta_update_session_utime_suspend(void);
 void tee_ta_update_session_utime_resume(void);
 #else
-static inline void tee_ta_gprof_sample_pc(vaddr_t pc __unused) {}
 static inline void tee_ta_update_session_utime_suspend(void) {}
 static inline void tee_ta_update_session_utime_resume(void) {}
 #endif
+#if defined(CFG_TA_GPROF_SUPPORT)
+void tee_ta_gprof_sample_pc(vaddr_t pc);
+#else
+static inline void tee_ta_gprof_sample_pc(vaddr_t pc __unused) {}
+#endif
 
 #endif

core/kernel/tee_ta_manager.c

@@ -29,6 +29,7 @@
 #include <tee/tee_svc_storage.h>
 #include <tee_api_types.h>
 #include <trace.h>
+#include <user_ta_header.h>
 #include <utee_types.h>
 #include <util.h>
 
@@ -876,12 +877,14 @@ struct tee_ta_session *tee_ta_get_calling_session(void)
 	return s;
 }
 
+#if defined(CFG_TA_GPROF_SUPPORT) || defined(CFG_TA_FTRACE_SUPPORT)
+
 #if defined(CFG_TA_GPROF_SUPPORT)
 void tee_ta_gprof_sample_pc(vaddr_t pc)
 {
-	struct tee_ta_session *s;
-	struct sample_buf *sbuf;
-	size_t idx;
+	struct tee_ta_session *s = NULL;
+	struct sample_buf *sbuf = NULL;
+	size_t idx = 0;
 
 	if (tee_ta_get_current_session(&s) != TEE_SUCCESS)
 		return;
@@ -895,23 +898,15 @@ void tee_ta_gprof_sample_pc(vaddr_t pc)
 	sbuf->count++;
 }
 
-/*
- * Update user-mode CPU time for the current session
- * @suspend: true if session is being suspended (leaving user mode), false if
- * it is resumed (entering user mode)
- */
-static void tee_ta_update_session_utime(bool suspend)
+static void gprof_update_session_utime(bool suspend, struct tee_ta_session *s,
+				       uint64_t now)
 {
-	struct tee_ta_session *s;
-	struct sample_buf *sbuf;
-	uint64_t now;
+	struct sample_buf *sbuf = NULL;
 
-	if (tee_ta_get_current_session(&s) != TEE_SUCCESS)
-		return;
 	sbuf = s->sbuf;
 	if (!sbuf)
 		return;
-	now = read_cntpct();
+
 	if (suspend) {
 		assert(sbuf->usr_entered);
 		sbuf->usr += now - sbuf->usr_entered;
@@ -923,6 +918,50 @@ static void tee_ta_update_session_utime(bool suspend)
 		sbuf->usr_entered = now;
 	}
 }
+#endif
+
+#if defined(CFG_TA_FTRACE_SUPPORT)
+static void ftrace_update_session_utime(bool suspend, struct tee_ta_session *s,
+					uint64_t now)
+{
+	struct ftrace_buf *fbuf = NULL;
+	uint32_t i = 0;
+
+	fbuf = s->fbuf;
+	if (!fbuf)
+		return;
+
+	if (suspend) {
+		fbuf->suspend_time = now;
+	} else {
+		for (i = 0; i <= fbuf->ret_idx; i++)
+			fbuf->begin_time[i] += now - fbuf->suspend_time;
+	}
+}
+#endif
+
+/*
+ * Update user-mode CPU time for the current session
+ * @suspend: true if session is being suspended (leaving user mode), false if
+ * it is resumed (entering user mode)
+ */
+static void tee_ta_update_session_utime(bool suspend)
+{
+	struct tee_ta_session *s = NULL;
+	uint64_t now = 0;
+
+	if (tee_ta_get_current_session(&s) != TEE_SUCCESS)
+		return;
+
+	now = read_cntpct();
+
+#if defined(CFG_TA_GPROF_SUPPORT)
+	gprof_update_session_utime(suspend, s, now);
+#endif
+#if defined(CFG_TA_FTRACE_SUPPORT)
+	ftrace_update_session_utime(suspend, s, now);
+#endif
+}
 
 void tee_ta_update_session_utime_suspend(void)
 {

ldelf/ftrace.c

@@ -8,19 +8,18 @@
 #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)
+bool ftrace_init(struct ftrace_buf **fbuf_ptr)
 {
+	struct __ftrace_info *finfo = NULL;
 	struct ta_elf *elf = TAILQ_FIRST(&main_elf_queue);
 	TEE_Result res = TEE_SUCCESS;
 	vaddr_t val = 0;
@@ -54,10 +53,13 @@ bool ftrace_init(void)
 	fbuf->ret_func_ptr = finfo->ret_ptr.ptr64;
 	fbuf->ret_idx = 0;
 	fbuf->lr_idx = 0;
+	fbuf->suspend_time = 0;
 	fbuf->buf_off = fbuf->head_off + count;
 	fbuf->curr_size = 0;
 	fbuf->max_size = fbuf_size - sizeof(struct ftrace_buf) - count;
 
+	*fbuf_ptr = fbuf;
+
 	return true;
 }
 

ldelf/ftrace.h

@@ -7,8 +7,9 @@
 #define FTRACE_H
 
 #include <types_ext.h>
+#include <user_ta_header.h>
 
-bool ftrace_init(void);
+bool ftrace_init(struct ftrace_buf **fbuf_ptr);
 void ftrace_copy_buf(void *pctx, void (*copy_func)(void *pctx, void *b,
 						   size_t bl));
 #ifdef CFG_TA_FTRACE_SUPPORT

ldelf/include/ldelf.h

@@ -8,6 +8,7 @@
 
 #include <types_ext.h>
 #include <tee_api_types.h>
+#include <user_ta_header.h>
 
 /* Size of stack for TEE Core to allocate */
 #define LDELF_STACK_SIZE	(4096 * 2)
@@ -21,6 +22,7 @@
  * @stack_ptr:	  [out] TA stack pointer
  * @dump_entry:	  [out] Dump TA mappings and stack trace
  * @ftrace_entry: [out] Dump TA mappings and ftrace buffer
+ * @fbuf:         [out] ftrace buffer pointer
  */
 struct ldelf_arg {
 	TEE_UUID uuid;
@@ -30,6 +32,7 @@ struct ldelf_arg {
 	uint64_t stack_ptr;
 	uint64_t dump_entry;
 	uint64_t ftrace_entry;
+	struct ftrace_buf *fbuf;
 };
 
 #define DUMP_MAP_READ	BIT(0)

ldelf/main.c

@@ -141,7 +141,7 @@ void ldelf(struct ldelf_arg *arg)
 
 	arg->ftrace_entry = 0;
 #ifdef CFG_TA_FTRACE_SUPPORT
-	if (ftrace_init())
+	if (ftrace_init(&arg->fbuf))
 		arg->ftrace_entry = (vaddr_t)(void *)ftrace_dump;
 #endif
 

lib/libutee/arch/arm/ftrace/ftrace.c

@@ -10,11 +10,14 @@
  * profiled too.
  */
 
+#include <arm_user_sysreg.h>
 #include <setjmp.h>
 #include <user_ta_header.h>
 #include <utee_syscalls.h>
 #include "ftrace.h"
 
+#define DURATION_MAX_LEN		16
+
 static const char hex_str[] = "0123456789abcdef";
 
 /*
@@ -40,8 +43,11 @@ static size_t __noprof to_func_enter_fmt(char *buf, uint32_t ret_idx,
 	uint32_t addr_size = 2 * sizeof(unsigned long);
 	uint32_t i = 0;
 
-	for (i = 0; i < ret_idx; i++)
-		*str++ = ' ';
+	for (i = 0; i < (DURATION_MAX_LEN + ret_idx); i++)
+		if (i == (DURATION_MAX_LEN - 2))
+			*str++ = '|';
+		else
+			*str++ = ' ';
 
 	*str++ = '0';
 	*str++ = 'x';
@@ -69,7 +75,8 @@ void __noprof ftrace_enter(unsigned long pc, unsigned long *lr)
 	if (!fbuf->buf_off || !fbuf->max_size)
 		return;
 
-	dump_size = fbuf->ret_idx + (2 * sizeof(unsigned long)) + 8;
+	dump_size = DURATION_MAX_LEN + fbuf->ret_idx +
+			(2 * sizeof(unsigned long)) + 8;
 
 	/*
 	 * Check if we have enough space in ftrace buffer. If not then just
@@ -85,6 +92,7 @@ void __noprof ftrace_enter(unsigned long pc, unsigned long *lr)
 
 	if (fbuf->ret_idx < FTRACE_RETFUNC_DEPTH) {
 		fbuf->ret_stack[fbuf->ret_idx] = *lr;
+		fbuf->begin_time[fbuf->ret_idx] = read_cntpct();
 		fbuf->ret_idx++;
 	} else {
 		/*
@@ -97,11 +105,42 @@ void __noprof ftrace_enter(unsigned long pc, unsigned long *lr)
 	*lr = (unsigned long)&__ftrace_return;
 }
 
+static void __noprof ftrace_duration(char *buf, uint64_t start, uint64_t end)
+{
+	uint32_t cntfrq = read_cntfrq();
+	uint64_t tics = end - start;
+	uint32_t us = 0;
+	uint32_t ns = 0;
+	uint32_t i = 0;
+
+	tics = tics * 1000000000 / cntfrq;
+
+	us = (tics % 1000000000) / 1000;
+	ns = tics % 1000;
+
+	*buf-- = 's';
+	*buf-- = 'u';
+	*buf-- = ' ';
+
+	for (i = 0; i < 3; i++) {
+		*buf-- = hex_str[ns % 10];
+		ns /= 10;
+	}
+
+	*buf-- = '.';
+
+	while (us) {
+		*buf-- = hex_str[us % 10];
+		us /= 10;
+	}
+}
+
 unsigned long __noprof ftrace_return(void)
 {
 	struct ftrace_buf *fbuf = NULL;
 	size_t dump_size = 0;
 	char *curr_buf = NULL;
+	char *dur_loc = NULL;
 	uint32_t i = 0;
 
 	fbuf = &__ftrace_buf_start;
@@ -124,21 +163,33 @@ unsigned long __noprof ftrace_return(void)
 		*(curr_buf - 2) = '\n';
 		*(curr_buf - 1) = '\0';
 		fbuf->curr_size -= 1;
+
+		dur_loc = curr_buf - (fbuf->ret_idx +
+				      (2 * sizeof(unsigned long)) + 11);
+		ftrace_duration(dur_loc, fbuf->begin_time[fbuf->ret_idx],
+				read_cntpct());
 	} else {
-		dump_size = fbuf->ret_idx + 3;
+		dump_size = DURATION_MAX_LEN + fbuf->ret_idx + 3;
 		if ((fbuf->curr_size + dump_size) > fbuf->max_size)
 			fbuf_shift(fbuf, dump_size);
 
 		curr_buf = (char *)fbuf + fbuf->buf_off + fbuf->curr_size;
 
-		for (i = 0; i < fbuf->ret_idx; i++)
-			*curr_buf++ = ' ';
+		for (i = 0; i < (DURATION_MAX_LEN + fbuf->ret_idx); i++)
+			if (i == (DURATION_MAX_LEN - 2))
+				*curr_buf++ = '|';
+			else
+				*curr_buf++ = ' ';
 
 		*curr_buf++ = '}';
 		*curr_buf++ = '\n';
 		*curr_buf = '\0';
 
 		fbuf->curr_size += dump_size - 1;
+
+		dur_loc = curr_buf - fbuf->ret_idx - 6;
+		ftrace_duration(dur_loc, fbuf->begin_time[fbuf->ret_idx],
+				read_cntpct());
 	}
 
 	return fbuf->ret_stack[fbuf->ret_idx];

lib/libutee/include/user_ta_header.h

@@ -55,6 +55,8 @@ struct ftrace_buf {
 	uint64_t ret_stack[FTRACE_RETFUNC_DEPTH]; /* Return stack */
 	uint32_t ret_idx;	/* Return stack index */
 	uint32_t lr_idx;	/* lr index used for stack unwinding */
+	uint64_t begin_time[FTRACE_RETFUNC_DEPTH]; /* Timestamp */
+	uint64_t suspend_time;	/* Suspend timestamp */
 	uint32_t curr_size;	/* Size of ftrace buffer */
 	uint32_t max_size;	/* Max allowed size of ftrace buffer */
 	uint32_t head_off;	/* Ftrace buffer header offset */