ftrace: Add function execution time support

core/arch/arm/include/sm/sm.h

@@ -37,6 +37,10 @@ struct sm_unbanked_regs {
 #ifdef CFG_SM_NO_CYCLE_COUNTING
 	uint32_t pmcr;
 #endif
+#ifdef CFG_TA_FTRACE_SUPPORT
+	uint32_t cntkctl;
+	uint32_t pad;
+#endif
 };
 
 struct sm_nsec_ctx {

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.
@@ -1075,6 +1075,15 @@ void thread_init_per_cpu(void)
 	set_abt_stack(l, GET_STACK(stack_abt[pos]));
 
 	thread_init_vbar(get_excp_vect());
+
+#ifdef CFG_TA_FTRACE_SUPPORT
+	/*
+	 * Enable accesses to frequency register and physical counter
+	 * register in EL0/PL0 required for timestamping during
+	 * function tracing.
+	 */
+	write_cntkctl(read_cntkctl() | CNTKCTL_PL0PCTEN);
+#endif
 }
 
 struct thread_specific_data *thread_get_tsd(void)

core/arch/arm/kernel/user_ta.c

@@ -252,6 +252,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;
 #endif
 
 out:

core/arch/arm/sm/sm_a32.S

@@ -44,6 +44,11 @@ UNWIND(	.cantunwind)
 	read_pmcr r2
 	stm	r0!, {r2}
 #endif
+
+#ifdef CFG_TA_FTRACE_SUPPORT
+	read_cntkctl r2
+	stm	r0!, {r2}
+#endif
 	cps	#CPSR_MODE_MON
 	bx	lr
 UNWIND(	.fnend)
@@ -76,6 +81,11 @@ UNWIND(	.cantunwind)
 	ldm	r0!, {r2}
 	write_pmcr r2
 #endif
+
+#ifdef CFG_TA_FTRACE_SUPPORT
+	ldm	r0!, {r2}
+	write_cntkctl r2
+#endif
 	cps	#CPSR_MODE_MON
 	bx	lr
 UNWIND(	.fnend)

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,11 +7,12 @@
 #define FTRACE_H
 
 #include <types_ext.h>
+#include <user_ta_header.h>
 
-bool ftrace_init(void);
+#ifdef CFG_TA_FTRACE_SUPPORT
+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
 void ftrace_map_lr(uint64_t *lr);
 #else
 static inline void ftrace_map_lr(uint64_t *lr __unused)

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

@@ -143,7 +143,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/arm32_user_sysreg.txt

@@ -0,0 +1,13 @@
+# Format of file
+# <reg-name> <CRn> <opc1> <CRm> <opc2> <Type> <Description>
+# lines beginning with '@' will be printed as additional comments
+
+@ Based on register description in
+@ ARM Architecture Reference Manual
+@ ARMv7-A and ARMv7-R edition
+@ Issue C.c
+@
+
+@ B8.2 Generic Timer registers summary
+CNTFRQ    c14 0 c0  0 RW Counter Frequency register
+CNTPCT    -   0 c14 - RO Physical Count register