Add support for user TA profiling with gprof (-pg)

Adds the infrastructure to collect profiling information from Trusted
Applications running in user mode and instrumented with -pg.
Enable with: CFG_TA_GPROF_SUPPORT=y.

Profiling support in itself adds no significant performance overhead.
Instrumented applications however may run 1.3x - 2x slower, and have a
larger .bss section (+1.36 times .text size for 32-bit TAs, +1.77 times
.text size for 64-bit ones).

Signed-off-by: Jerome Forissier <jerome.forissier@linaro.org>
Tested-by: Jerome Forissier <jerome.forissier@linaro.org> (D02 64-bit)
Tested-by: Jerome Forissier <jerome.forissier@linaro.org> (QEMU 32-bit)
Reviewed-by: Jens Wiklander <jens.wiklander@linaro.org>

27 files changed, 1565 insertions, 37 deletions

diff --git a/core/arch/arm/kernel/thread.c b/core/arch/arm/kernel/thread.c
index d3358b2d..7a0dd3f8 100644
--- a/core/arch/arm/kernel/thread.c
+++ b/core/arch/arm/kernel/thread.c
@@ -498,6 +498,30 @@ static void copy_a0_to_a5(struct thread_ctx_regs *regs,
 }
 #endif /*ARM64*/
 
+#ifdef ARM32
+static bool is_from_user(uint32_t cpsr)
+{
+	return (cpsr & ARM32_CPSR_MODE_MASK) == ARM32_CPSR_MODE_USR;
+}
+#endif
+
+#ifdef ARM64
+static bool is_from_user(uint32_t cpsr)
+{
+	if (cpsr & (SPSR_MODE_RW_32 << SPSR_MODE_RW_SHIFT))
+		return true;
+	if (((cpsr >> SPSR_64_MODE_EL_SHIFT) & SPSR_64_MODE_EL_MASK) ==
+	     SPSR_64_MODE_EL0)
+		return true;
+	return false;
+}
+#endif
+
+static bool is_user_mode(struct thread_ctx_regs *regs)
+{
+	return is_from_user((uint32_t)regs->cpsr);
+}
+
 static void thread_resume_from_rpc(struct thread_smc_args *args)
 {
 	size_t n = args->a3; /* thread id */
@@ -524,6 +548,9 @@ 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);
 
@@ -646,25 +673,6 @@ void thread_state_free(void)
 	unlock_global();
 }
 
-#ifdef ARM32
-static bool is_from_user(uint32_t cpsr)
-{
-	return (cpsr & ARM32_CPSR_MODE_MASK) == ARM32_CPSR_MODE_USR;
-}
-#endif
-
-#ifdef ARM64
-static bool is_from_user(uint32_t cpsr)
-{
-	if (cpsr & (SPSR_MODE_RW_32 << SPSR_MODE_RW_SHIFT))
-		return true;
-	if (((cpsr >> SPSR_64_MODE_EL_SHIFT) & SPSR_64_MODE_EL_MASK) ==
-	     SPSR_64_MODE_EL0)
-		return true;
-	return false;
-}
-#endif
-
 #ifdef CFG_WITH_PAGER
 static void release_unused_kernel_stack(struct thread_ctx *thr)
 {
@@ -691,8 +699,11 @@ int thread_state_suspend(uint32_t flags, uint32_t cpsr, vaddr_t pc)
 
 	release_unused_kernel_stack(threads + ct);
 
-	if (is_from_user(cpsr))
+	if (is_from_user(cpsr)) {
 		thread_user_save_vfp();
+		tee_ta_update_session_utime_suspend();
+		tee_ta_gprof_sample_pc(pc);
+	}
 	thread_lazy_restore_ns_vfp();
 
 	lock_global();
@@ -1081,6 +1092,8 @@ uint32_t thread_enter_user_mode(unsigned long a0, unsigned long a1,
 {
 	uint32_t spsr;
 
+	tee_ta_update_session_utime_resume();
+
 	if (!get_spsr(is_32bit, entry_func, &spsr)) {
 		*exit_status0 = 1; /* panic */
 		*exit_status1 = 0xbadbadba;
diff --git a/core/arch/arm/kernel/user_ta.c b/core/arch/arm/kernel/user_ta.c
index e52cc07d..db757dd6 100644
--- a/core/arch/arm/kernel/user_ta.c
+++ b/core/arch/arm/kernel/user_ta.c
@@ -783,7 +783,6 @@ static void user_ta_ctx_destroy(struct tee_ta_ctx *ctx)
 	tee_obj_close_all(utc);
 	/* Free emums created by this TA */
 	tee_svc_storage_close_all_enum(utc);
-
 	free(utc);
 }
 
diff --git a/core/arch/arm/sta/gprof.c b/core/arch/arm/sta/gprof.c
new file mode 100644
index 00000000..ae461a79
--- /dev/null
+++ b/core/arch/arm/sta/gprof.c
@@ -0,0 +1,236 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arm.h>
+#include <kernel/misc.h>
+#include <kernel/static_ta.h>
+#include <kernel/user_ta.h>
+#include <kernel/thread.h>
+#include <mm/core_memprot.h>
+#include <mm/tee_mmu.h>
+#include <optee_msg_supplicant.h>
+#include <pta_gprof.h>
+#include <string.h>
+
+static TEE_Result gprof_send_rpc(TEE_UUID *uuid, void *buf, size_t len,
+				 uint32_t *id)
+{
+	struct optee_msg_param params[3];
+	TEE_Result res = TEE_ERROR_GENERIC;
+	uint64_t c = 0;
+	paddr_t pa;
+	char *va;
+
+	thread_rpc_alloc_payload(sizeof(*uuid) + len, &pa, &c);
+	if (!pa)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	va = phys_to_virt(pa, MEM_AREA_NSEC_SHM);
+	if (!va)
+		goto exit;
+
+	memcpy(va, uuid, sizeof(*uuid));
+	memcpy(va + sizeof(*uuid), buf, len);
+
+	memset(params, 0, sizeof(params));
+	params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INOUT;
+	params[0].u.value.a = *id;
+
+	params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
+	params[1].u.tmem.buf_ptr = pa;
+	params[1].u.tmem.size = sizeof(*uuid);
+	params[1].u.tmem.shm_ref = c;
+
+	params[2].attr = OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
+	params[2].u.tmem.buf_ptr = pa + sizeof(*uuid);
+	params[2].u.tmem.size = len;
+	params[2].u.tmem.shm_ref = c;
+
+	res = thread_rpc_cmd(OPTEE_MSG_RPC_CMD_GPROF, 3, params);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	*id = (uint32_t)params[0].u.value.a;
+exit:
+	thread_rpc_free_payload(c);
+	return res;
+}
+
+static TEE_Result gprof_send(struct tee_ta_session *s,
+			     uint32_t param_types,
+			     TEE_Param params[TEE_NUM_PARAMS])
+{
+	uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INOUT,
+					  TEE_PARAM_TYPE_MEMREF_INPUT,
+					  TEE_PARAM_TYPE_NONE,
+					  TEE_PARAM_TYPE_NONE);
+
+	if (exp_pt != param_types)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	return gprof_send_rpc(&s->ctx->uuid, params[1].memref.buffer,
+			      params[1].memref.size, &params[0].value.a);
+}
+
+static TEE_Result gprof_start_pc_sampling(struct tee_ta_session *s,
+					  uint32_t param_types,
+					  TEE_Param params[TEE_NUM_PARAMS])
+{
+	uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_MEMREF_INOUT,
+					  TEE_PARAM_TYPE_VALUE_INPUT,
+					  TEE_PARAM_TYPE_NONE,
+					  TEE_PARAM_TYPE_NONE);
+	struct sample_buf *sbuf;
+	uint32_t offset;
+	uint32_t scale;
+	TEE_Result res;
+	uint32_t len;
+	uaddr_t buf;
+
+	if (exp_pt != param_types)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	buf = (uaddr_t)params[0].memref.buffer;
+	len = params[0].memref.size;
+	offset = params[1].value.a;
+	scale = params[1].value.b;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(s->ctx),
+					  TEE_MEMORY_ACCESS_WRITE |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  buf, len);
+	if (res != TEE_SUCCESS)
+		return res;
+	sbuf = calloc(1, sizeof(*sbuf));
+	if (!sbuf)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	sbuf->samples = (uint16_t *)buf;
+	sbuf->nsamples = len / sizeof(*sbuf->samples);
+	sbuf->offset = offset;
+	sbuf->scale = scale;
+	sbuf->freq = read_cntfrq();
+	sbuf->enabled = true;
+	s->sbuf = sbuf;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result gprof_stop_pc_sampling(struct tee_ta_session *s,
+					 uint32_t param_types,
+					 TEE_Param params[TEE_NUM_PARAMS])
+{
+	uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_OUTPUT,
+					  TEE_PARAM_TYPE_NONE,
+					  TEE_PARAM_TYPE_NONE,
+					  TEE_PARAM_TYPE_NONE);
+	struct sample_buf *sbuf;
+	uint32_t rate;
+
+	if (exp_pt != param_types)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	sbuf = s->sbuf;
+	if (!sbuf)
+		return TEE_ERROR_BAD_STATE;
+	assert(sbuf->samples);
+
+	/* Stop sampling */
+	if (sbuf->enabled)
+		sbuf->enabled = false;
+
+	rate = ((uint64_t)sbuf->count * sbuf->freq) / sbuf->usr;
+	params[0].value.a = rate;
+
+	DMSG("TA sampling stats: sample count=%" PRIu32 " user time=%" PRIu64
+	     " cntfrq=%" PRIu32 " rate=%" PRIu32, sbuf->count, sbuf->usr,
+	     sbuf->freq, rate);
+
+	free(sbuf);
+	s->sbuf = NULL;
+
+	return TEE_SUCCESS;
+}
+
+/*
+ * Trusted Application Entry Points
+ */
+
+static TEE_Result create_ta(void)
+{
+	return TEE_SUCCESS;
+}
+
+static void destroy_ta(void)
+{
+}
+
+static TEE_Result open_session(uint32_t param_types __unused,
+			       TEE_Param params[TEE_NUM_PARAMS] __unused,
+			       void **sess_ctx __unused)
+{
+	struct tee_ta_session *s;
+
+	/* Check that we're called from a user TA */
+	s = tee_ta_get_calling_session();
+	if (!s)
+		return TEE_ERROR_ACCESS_DENIED;
+	if (is_static_ta_ctx(s->ctx))
+		return TEE_ERROR_ACCESS_DENIED;
+
+	return TEE_SUCCESS;
+}
+
+static void close_session(void *sess_ctx __unused)
+{
+}
+
+static TEE_Result invoke_command(void *sess_ctx __unused, uint32_t cmd_id,
+				 uint32_t param_types,
+				 TEE_Param params[TEE_NUM_PARAMS])
+{
+	struct tee_ta_session *s = tee_ta_get_calling_session();
+
+	switch (cmd_id) {
+	case PTA_GPROF_SEND:
+		return gprof_send(s, param_types, params);
+	case PTA_GPROF_START_PC_SAMPLING:
+		return gprof_start_pc_sampling(s, param_types, params);
+	case PTA_GPROF_STOP_PC_SAMPLING:
+		return gprof_stop_pc_sampling(s, param_types, params);
+	default:
+		break;
+	}
+	return TEE_ERROR_NOT_IMPLEMENTED;
+}
+
+static_ta_register(.uuid = PTA_GPROF_UUID, .name = "gprof",
+		   .create_entry_point = create_ta,
+		   .destroy_entry_point = destroy_ta,
+		   .open_session_entry_point = open_session,
+		   .close_session_entry_point = close_session,
+		   .invoke_command_entry_point = invoke_command);
diff --git a/core/arch/arm/sta/sub.mk b/core/arch/arm/sta/sub.mk
index 3cea21f5..cbe9a4e0 100644
--- a/core/arch/arm/sta/sub.mk
+++ b/core/arch/arm/sta/sub.mk
@@ -2,6 +2,7 @@ srcs-$(CFG_TEE_CORE_EMBED_INTERNAL_TESTS) += sta_self_tests.c
 srcs-$(CFG_TEE_CORE_EMBED_INTERNAL_TESTS) += core_self_tests.c
 srcs-$(CFG_TEE_CORE_EMBED_INTERNAL_TESTS) += interrupt_tests.c
 srcs-$(CFG_WITH_STATS) += stats.c
+srcs-$(CFG_TA_GPROF_SUPPORT) += gprof.c
 
 ifeq ($(CFG_SE_API),y)
 srcs-$(CFG_SE_API_SELF_TEST) += se_api_self_tests.c
diff --git a/core/arch/arm/tee/arch_svc.c b/core/arch/arm/tee/arch_svc.c
index 0ac8d0c5..8a89ce93 100644
--- a/core/arch/arm/tee/arch_svc.c
+++ b/core/arch/arm/tee/arch_svc.c
@@ -198,6 +198,9 @@ void tee_svc_handler(struct thread_svc_regs *regs)
 
 	thread_user_save_vfp();
 
+	/* TA has just entered kernel mode */
+	tee_ta_update_session_utime_suspend();
+
 	/* Restore IRQ which are disabled on exception entry */
 	thread_restore_irq();
 
@@ -217,6 +220,11 @@ void tee_svc_handler(struct thread_svc_regs *regs)
 		scf = tee_svc_syscall_table[scn].fn;
 
 	set_svc_retval(regs, tee_svc_do_call(regs, scf));
+
+	if (scn != TEE_SCN_RETURN) {
+		/* We're about to switch back to user mode */
+		tee_ta_update_session_utime_resume();
+	}
 }
 
 #ifdef ARM32
diff --git a/core/include/kernel/tee_ta_manager.h b/core/include/kernel/tee_ta_manager.h
index 279b6f8c..ecff2312 100644
--- a/core/include/kernel/tee_ta_manager.h
+++ b/core/include/kernel/tee_ta_manager.h
@@ -80,6 +80,20 @@ struct tee_ta_ops {
 	void (*destroy)(struct tee_ta_ctx *ctx);
 };
 
+#if defined(CFG_TA_GPROF_SUPPORT)
+struct sample_buf {
+	uint32_t nsamples;	/* Size of @samples array in uint16_t */
+	uint32_t offset;	/* Passed from user mode */
+	uint32_t scale;		/* Passed from user mode */
+	uint32_t count;		/* Number of samples taken */
+	bool enabled;		/* Sampling enabled? */
+	uint16_t *samples;
+	uint64_t usr;		/* Total user CPU time for this session */
+	uint64_t usr_entered;	/* When this session last entered user mode */
+	uint32_t freq;		/* @usr divided by @freq is in seconds */
+};
+#endif
+
 /* Context of a loaded TA */
 struct tee_ta_ctx {
 	TEE_UUID uuid;
@@ -107,6 +121,9 @@ struct tee_ta_session {
 	struct condvar lock_cv;	/* CV used to wait for lock */
 	int lock_thread;	/* Id of thread holding the lock */
 	bool unlink;		/* True if session is to be unlinked */
+#if defined(CFG_TA_GPROF_SUPPORT)
+	struct sample_buf *sbuf; /* Profiling data (PC sampling) */
+#endif
 };
 
 /* Registered contexts */
@@ -161,4 +178,14 @@ void tee_ta_put_session(struct tee_ta_session *sess);
 
 void tee_ta_dump_current(void);
 
+#if defined(CFG_TA_GPROF_SUPPORT)
+void tee_ta_gprof_sample_pc(vaddr_t pc);
+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
+
 #endif
diff --git a/core/include/optee_msg_supplicant.h b/core/include/optee_msg_supplicant.h
index 14e3632b..d3892739 100644
--- a/core/include/optee_msg_supplicant.h
+++ b/core/include/optee_msg_supplicant.h
@@ -178,4 +178,22 @@
  * .cmd == OPTEE_MSG_RPC_CMD_SQL_FS
  */
 
+/*
+ * Send TA profiling information to normal world
+ *
+ * [in/out] param[0].u.value.a		File identifier. Must be set to 0 on
+ *					first call. A value >= 1 will be
+ *					returned on success. Re-use this value
+ *					to append data to the same file.
+ *
+ * [in] param[1].u.tmem.buf_ptr		Physical address of TA UUID
+ * [in] param[1].u.tmem.size		Size of UUID
+ * [in] param[1].u.tmem.shm_ref		Shared memory reference
+ *
+ * [in] param[2].u.tmem.buf_ptr		Physical address of profile data buffer
+ * [in] param[2].u.tmem.size		Buffer size
+ * [in] param[2].u.tmem.shm_ref		Shared memory reference
+ */
+#define OPTEE_MSG_RPC_CMD_GPROF		9
+
 #endif /*__OPTEE_MSG_SUPPLICANT_H*/
diff --git a/core/kernel/tee_ta_manager.c b/core/kernel/tee_ta_manager.c
index 4b1a130c..6e81b114 100644
--- a/core/kernel/tee_ta_manager.c
+++ b/core/kernel/tee_ta_manager.c
@@ -344,6 +344,9 @@ TEE_Result tee_ta_close_session(struct tee_ta_session *csess,
 	}
 
 	tee_ta_unlink_session(sess, open_sessions);
+#if defined(CFG_TA_GPROF_SUPPORT)
+	free(sess->sbuf);
+#endif
 	free(sess);
 
 	tee_ta_clear_busy(ctx);
@@ -401,7 +404,6 @@ static TEE_Result tee_ta_init_session_with_context(struct tee_ta_ctx *ctx,
 }
 
 
-
 static TEE_Result tee_ta_init_session(TEE_ErrorOrigin *err,
 				struct tee_ta_session_head *open_sessions,
 				const TEE_UUID *uuid,
@@ -703,3 +705,62 @@ void tee_ta_dump_current(void)
 
 	dump_state(s->ctx);
 }
+
+#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;
+
+	if (tee_ta_get_current_session(&s) != TEE_SUCCESS)
+		return;
+	sbuf = s->sbuf;
+	if (!sbuf || !sbuf->enabled)
+		return; /* PC sampling is not enabled */
+
+	idx = (((uint64_t)pc - sbuf->offset)/2 * sbuf->scale)/65536;
+	if (idx < sbuf->nsamples)
+		sbuf->samples[idx]++;
+	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)
+{
+	struct tee_ta_session *s;
+	struct sample_buf *sbuf;
+	uint64_t now;
+
+	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;
+		sbuf->usr_entered = 0;
+	} else {
+		assert(!sbuf->usr_entered);
+		if (!now)
+			now++; /* 0 is reserved */
+		sbuf->usr_entered = now;
+	}
+}
+
+void tee_ta_update_session_utime_suspend(void)
+{
+	tee_ta_update_session_utime(true);
+}
+
+void tee_ta_update_session_utime_resume(void)
+{
+	tee_ta_update_session_utime(false);
+}
+#endif
diff --git a/lib/libmpa/sub.mk b/lib/libmpa/sub.mk
index 87a6b72a..38c1610a 100644
--- a/lib/libmpa/sub.mk
+++ b/lib/libmpa/sub.mk
@@ -1,7 +1,7 @@
 global-incdirs-y += include
 
 ifneq ($(sm),core) # User-mode
-cflags-lib-$(CFG_U_LIBMPA_GPROF) += -pg
+cflags-lib-$(CFG_ULIBS_GPROF) += -pg
 endif
 
 srcs-y += mpa_misc.c
diff --git a/lib/libutee/arch/arm/gprof/gmon.h b/lib/libutee/arch/arm/gprof/gmon.h
new file mode 100644
index 00000000..f4ab98d9
--- /dev/null
+++ b/lib/libutee/arch/arm/gprof/gmon.h
@@ -0,0 +1,201 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * This file is adapted from glibc' gmon/sys/gmon.h.
+ *-
+ * Copyright (c) 1982, 1986, 1992, 1993
+ *	The Regents of the University of California.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 4. Neither the name of the University nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+/*
+ * See gmon_out.h for gmon.out format.
+ */
+
+#ifndef GMON_H
+#define GMON_H
+
+#include <stdint.h>
+
+/* Exported by the TA linker script */
+extern uint8_t __text_start[];
+extern uint8_t __text_end[];
+
+void __mcount_internal(unsigned long frompc, unsigned long selfpc);
+
+
+/*
+ * Histogram counters are unsigned shorts (according to the kernel).
+ */
+#define	HISTCOUNTER	unsigned short
+
+/*
+ * Fraction of text space to allocate for histogram counters here, 1/2
+ */
+#define	HISTFRACTION	2
+
+/*
+ * Fraction of text space to allocate for from hash buckets.
+ * The value of HASHFRACTION is based on the minimum number of bytes
+ * of separation between two subroutine call points in the object code.
+ * Given MIN_SUBR_SEPARATION bytes of separation the value of
+ * HASHFRACTION is calculated as:
+ *
+ *	HASHFRACTION = MIN_SUBR_SEPARATION / (2 * sizeof(short) - 1);
+ *
+ * For example, on the VAX, the shortest two call sequence is:
+ *
+ *	calls	$0,(r0)
+ *	calls	$0,(r0)
+ *
+ * which is separated by only three bytes, thus HASHFRACTION is
+ * calculated as:
+ *
+ *	HASHFRACTION = 3 / (2 * 2 - 1) = 1
+ *
+ * Note that the division above rounds down, thus if MIN_SUBR_FRACTION
+ * is less than three, this algorithm will not work!
+ *
+ * In practice, however, call instructions are rarely at a minimal
+ * distance.  Hence, we will define HASHFRACTION to be 2 across all
+ * architectures.  This saves a reasonable amount of space for
+ * profiling data structures without (in practice) sacrificing
+ * any granularity.
+ */
+#define	HASHFRACTION	2
+
+/*
+ * Percent of text space to allocate for tostructs.
+ * This is a heuristic; we will fail with a warning when profiling programs
+ * with a very large number of very small functions, but that's
+ * normally OK.
+ * 2 is probably still a good value for normal programs.
+ * Profiling a test case with 64000 small functions will work if
+ * you raise this value to 3 and link statically (which bloats the
+ * text size, thus raising the number of arcs expected by the heuristic).
+ */
+#define ARCDENSITY	3
+
+/*
+ * Always allocate at least this many tostructs.  This
+ * hides the inadequacy of the ARCDENSITY heuristic, at least
+ * for small programs.
+ */
+#define MINARCS		50
+
+/*
+ * The type used to represent indices into gmonparam.tos[].
+ */
+#define	ARCINDEX	unsigned long
+
+/*
+ * Maximum number of arcs we want to allow.
+ * Used to be max representable value of ARCINDEX minus 2, but now
+ * that ARCINDEX is a long, that's too large; we don't really want
+ * to allow a 48 gigabyte table.
+ * The old value of 1<<16 wasn't high enough in practice for large C++
+ * programs; will 1<<20 be adequate for long?  FIXME
+ */
+#define MAXARCS		(1 << 20)
+
+struct tostruct {
+	unsigned long selfpc;
+	long count;
+	ARCINDEX link;
+};
+
+/*
+ * A raw arc, with pointers to the calling site and the called site and a
+ * count.
+ */
+struct rawarc {
+	unsigned long	raw_frompc;
+	unsigned long	raw_selfpc;
+	long		raw_count;
+};
+
+/*
+ * General rounding functions.
+ */
+#define ROUNDDOWN(x, y)	(((x)/(y))*(y))
+#define ROUNDUP(x, y)	((((x)+(y)-1)/(y))*(y))
+
+/*
+ * The profiling data structures are housed in this structure.
+ */
+struct gmonparam {
+	long int	state;
+	unsigned short	*kcount;
+	unsigned long	kcountsize;
+	ARCINDEX	*froms;
+	unsigned long	fromssize;
+	struct tostruct	*tos;
+	unsigned long	tossize;
+	unsigned long	tolimit;
+	unsigned long	lowpc;
+	unsigned long	highpc;
+	unsigned long	textsize;
+	unsigned long	hashfraction;
+	long		log_hashfraction;
+	/* */
+	uint32_t	prof_rate; /* PC sampling frequency */
+};
+
+/*
+ * Possible states of profiling.
+ */
+#define	GMON_PROF_ON		0
+#define	GMON_PROF_BUSY		1
+#define	GMON_PROF_ERROR		2
+#define	GMON_PROF_OFF		3
+#define	GMON_PROF_OFF_EXITING	4
+
+#endif /* GMON_H */
diff --git a/lib/libutee/arch/arm/gprof/gmon_out.h b/lib/libutee/arch/arm/gprof/gmon_out.h
new file mode 100644
index 00000000..1f169dab
--- /dev/null
+++ b/lib/libutee/arch/arm/gprof/gmon_out.h
@@ -0,0 +1,118 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * gmon.out file format
+ *
+ * This file is adapted from glibc's gmon/sys/gmon_out.h
+ * Although gmon/sys/gmon_out.h is covered by the LGPL v2.1 license or later
+ * as stated below, please note the following:
+ * (https://www.gnu.org/licenses/lgpl-3.0.en.html#section3)
+ *
+ * "3. Object Code Incorporating Material from Library Header Files.
+ *  The object code form of an Application may incorporate material from a
+ *  header file that is part of the Library. You may convey such object code
+ *  under terms of your choice, provided that, if the incorporated material
+ *  is not limited to numerical parameters, data structure layouts and
+ *  accessors, or small macros, inline functions and templates (ten or fewer
+ *  lines in length), you do both of the following: [...]"
+ *
+ * The code below is indeed limited to data structure layouts.
+ */
+
+/*
+ * Copyright (C) 1996-2016 Free Software Foundation, Inc.
+ * This file is part of the GNU C Library.
+ * Contributed by David Mosberger <davidm@cs.arizona.edu>.
+ *
+ * The GNU C Library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * The GNU C Library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with the GNU C Library; if not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ * This file specifies the format of gmon.out files.  It should have
+ * as few external dependencies as possible as it is going to be included
+ * in many different programs.  That is, minimize the number of #include's.
+ *
+ * A gmon.out file consists of a header (defined by gmon_hdr) followed by
+ * a sequence of records.  Each record starts with a one-byte tag
+ * identifying the type of records, followed by records specific data.
+ */
+
+#ifndef GMON_OUT_H
+#define GMON_OUT_H
+
+#define	GMON_MAGIC	"gmon"	/* magic cookie */
+#define GMON_VERSION	1	/* version number */
+
+/*
+ * Raw header as it appears on file (without padding).  This header
+ * always comes first in gmon.out and is then followed by a series
+ * records defined below.
+ * Virtual addresses are stored as uintptr_t, gprof knows which size to expect
+ * because the executable file is provided.
+ */
+struct gmon_hdr {
+	char cookie[4];
+	int32_t version;
+	char spare[3 * 4];
+} __packed;
+
+/* types of records in this file: */
+enum gmon_record_tag {
+	GMON_TAG_TIME_HIST = 0,
+	GMON_TAG_CG_ARC = 1,
+	GMON_TAG_BB_COUNT = 2
+};
+
+struct gmon_hist_hdr {
+	uintptr_t low_pc;	/* base pc address of sample buffer */
+	uintptr_t high_pc;	/* max pc address of sampled buffer */
+	uint32_t hist_size;	/* size of sample buffer */
+	uint32_t prof_rate;	/* profiling clock rate */
+	char dimen[15];		/* phys. dim., usually "seconds" */
+	char dimen_abbrev;	/* usually 's' for "seconds" */
+} __packed;
+
+struct gmon_cg_arc_record {
+	uintptr_t from_pc;	/* address within caller's body */
+	uintptr_t self_pc;	/* address within callee's body */
+	int32_t count;		/* number of arc traversals */
+} __packed;
+
+#endif /* GMON_OUT_H */
diff --git a/lib/libutee/arch/arm/gprof/gprof.c b/lib/libutee/arch/arm/gprof/gprof.c
new file mode 100644
index 00000000..cf431489
--- /dev/null
+++ b/lib/libutee/arch/arm/gprof/gprof.c
@@ -0,0 +1,407 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * Portions of this file are adapted from glibc:
+ *   gmon/gmon.c
+ *   gmon/mcount.c
+ *
+ *-
+ * Copyright (c) 1983, 1992, 1993, 2011
+ *	The Regents of the University of California.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 4. Neither the name of the University nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <compiler.h>
+#include <inttypes.h>
+#include <malloc.h>
+#include <stdint.h>
+#include <string.h>
+#include <tee_api_private.h>
+#include <trace.h>
+#include <user_ta_header.h>
+#include <utee_types.h>
+#include "gmon.h"
+#include "gmon_out.h"
+#include "gprof_pta.h"
+
+/* Defined by the linker script */
+extern uint8_t __gprof_buf_end[];
+extern uint8_t __gprof_buf_start[];
+
+static bool ta_instrumented(void)
+{
+	return (__gprof_buf_end != __gprof_buf_start);
+}
+
+static void *gprof_alloc(size_t len)
+{
+	if (len > (size_t)(__gprof_buf_end - __gprof_buf_start))
+		return NULL;
+	return __gprof_buf_start;
+}
+
+static struct gmonparam _gmonparam = { GMON_PROF_OFF };
+
+static uint32_t _gprof_file_id; /* File id returned by tee-supplicant */
+
+static int _gprof_s_scale;
+#define SCALE_1_TO_1 0x10000L
+
+/* Adjust PC so that gprof can locate it in the TA ELF file */
+static unsigned long __noprof adjust_pc(unsigned long pc)
+{
+	return pc - (unsigned long)__text_start + sizeof(struct ta_head);
+}
+
+void __utee_gprof_init(void)
+{
+	unsigned long lowpc;
+	unsigned long highpc;
+	struct gmonparam *p = &_gmonparam;
+	size_t bufsize;
+	TEE_Result res;
+	char *cp;
+
+	if (!ta_instrumented())
+		return;
+
+	lowpc = adjust_pc((unsigned long)__text_start);
+	highpc = adjust_pc((unsigned long)__text_end);
+
+	/*
+	 * Round lowpc and highpc to multiples of the density we're using
+	 * so the rest of the scaling (here and in gprof) stays in ints.
+	 */
+	p->lowpc = ROUNDDOWN(lowpc, HISTFRACTION * sizeof(HISTCOUNTER));
+	p->highpc = ROUNDUP(highpc, HISTFRACTION * sizeof(HISTCOUNTER));
+	p->textsize = p->highpc - p->lowpc;
+	p->kcountsize = ROUNDUP(p->textsize / HISTFRACTION, sizeof(*p->froms));
+	p->hashfraction = HASHFRACTION;
+	p->log_hashfraction = -1;
+	/*
+	 * The following test must be kept in sync with the corresponding
+	 * test in __mcount_internal
+	 */
+	if ((HASHFRACTION & (HASHFRACTION - 1)) == 0) {
+		/*
+		 * If HASHFRACTION is a power of two, mcount can use shifting
+		 * instead of integer division. Precompute shift amount.
+		 */
+		p->log_hashfraction = __builtin_ffs(p->hashfraction *
+						    sizeof(*p->froms)) - 1;
+	}
+	p->fromssize = p->textsize / HASHFRACTION;
+	p->tolimit = p->textsize * ARCDENSITY / 100;
+	if (p->tolimit < MINARCS)
+		p->tolimit = MINARCS;
+	else if (p->tolimit > MAXARCS)
+		p->tolimit = MAXARCS;
+	p->tossize = p->tolimit * sizeof(struct tostruct);
+
+	bufsize = p->kcountsize + p->fromssize + p->tossize;
+
+	IMSG("gprof: initializing");
+	DMSG("TA text size: %zu, gprof buffer size: %zu",
+	     __text_end - __text_start, bufsize);
+
+	cp = gprof_alloc(bufsize);
+	if (!cp) {
+		EMSG("gprof: could not allocate profiling buffer");
+		p->tos = NULL;
+		p->state = GMON_PROF_ERROR;
+		return;
+	}
+
+	p->tos = (struct tostruct *)cp;
+	cp += p->tossize;
+	p->kcount = (HISTCOUNTER *)cp;
+	cp += p->kcountsize;
+	p->froms = (ARCINDEX *)cp;
+
+	p->tos[0].link = 0;
+
+	if (p->kcountsize < p->textsize)
+		_gprof_s_scale = ((float)p->kcountsize / p->textsize) *
+				  SCALE_1_TO_1;
+	else
+		_gprof_s_scale = SCALE_1_TO_1;
+
+	res = __pta_gprof_pc_sampling_start(p->kcount, p->kcountsize,
+					    p->lowpc +
+					    ((unsigned long)__text_start -
+						sizeof(struct ta_head)),
+					    _gprof_s_scale);
+	if (res != TEE_SUCCESS)
+		EMSG("gprof: could not start PC sampling (0x%08x)", res);
+
+	p->state = GMON_PROF_ON;
+}
+
+static void _gprof_write_buf(void *buf, size_t size)
+{
+	TEE_Result res;
+
+	res = __pta_gprof_send(buf, size, &_gprof_file_id);
+	if (res != TEE_SUCCESS)
+		EMSG("gprof: could not send gprof data (0x%08x)", res);
+}
+
+static void _gprof_write_header(void)
+{
+	struct gmon_hdr ghdr;
+	size_t size = sizeof(struct gmon_hdr);
+
+	memcpy(&ghdr.cookie[0], GMON_MAGIC, sizeof(ghdr.cookie));
+	ghdr.version = GMON_VERSION;
+	memset(ghdr.spare, '\0', sizeof(ghdr.spare));
+
+	_gprof_write_buf(&ghdr, size);
+}
+
+static void _gprof_write_hist(void)
+{
+	struct out_record {
+		uint8_t tag;
+		struct gmon_hist_hdr hist_hdr;
+	} __packed out = {
+		.tag = GMON_TAG_TIME_HIST,
+		.hist_hdr = {
+			.low_pc = _gmonparam.lowpc,
+			.high_pc = _gmonparam.highpc,
+			.hist_size = _gmonparam.kcountsize/sizeof(HISTCOUNTER),
+			.prof_rate = _gmonparam.prof_rate,
+			.dimen = "seconds",
+			.dimen_abbrev = 's',
+		}
+	};
+
+	_gprof_write_buf(&out, sizeof(out));
+	_gprof_write_buf(_gmonparam.kcount, _gmonparam.kcountsize);
+}
+
+static void _gprof_write_call_graph(void)
+{
+#define NARCS_PER_WRITE 16
+	struct out_record {
+		uint8_t tag;
+		uint8_t data[sizeof(struct gmon_cg_arc_record)];
+	} out[NARCS_PER_WRITE];
+	struct gmon_cg_arc_record arc;
+	ARCINDEX from_index, to_index;
+	unsigned long from_len;
+	unsigned long frompc;
+	int nfilled = 0;
+
+	from_len = _gmonparam.fromssize / sizeof(*_gmonparam.froms);
+
+	for (from_index = 0; from_index < from_len; ++from_index) {
+
+		if (_gmonparam.froms[from_index] == 0)
+			continue;
+
+		frompc = _gmonparam.lowpc;
+		frompc += (from_index * _gmonparam.hashfraction
+			   * sizeof(*_gmonparam.froms));
+		for (to_index = _gmonparam.froms[from_index];
+		     to_index != 0;
+		     to_index = _gmonparam.tos[to_index].link) {
+
+			arc.from_pc = frompc;
+			arc.self_pc = _gmonparam.tos[to_index].selfpc;
+			arc.count = _gmonparam.tos[to_index].count;
+
+			out[nfilled].tag = GMON_TAG_CG_ARC;
+			memcpy(out[nfilled].data, &arc, sizeof(arc));
+
+			if (++nfilled == NARCS_PER_WRITE) {
+				_gprof_write_buf(out, sizeof(out));
+				nfilled = 0;
+			}
+		}
+	}
+	if (nfilled > 0)
+		_gprof_write_buf(out, nfilled * sizeof(out[0]));
+}
+
+/* Stop profiling and send profile data in gmon.out format to Normal World */
+void __utee_gprof_fini(void)
+{
+	TEE_Result res;
+
+	if (_gmonparam.state != GMON_PROF_ON)
+		return;
+
+	/* Stop call graph tracing */
+	_gmonparam.state = GMON_PROF_OFF_EXITING;
+
+	/* Stop TA sampling */
+	res = __pta_gprof_pc_sampling_stop(&_gmonparam.prof_rate);
+
+	_gprof_write_header();
+	if (res == TEE_SUCCESS)
+		_gprof_write_hist();
+	_gprof_write_call_graph();
+
+	__pta_gprof_fini();
+}
+
+/*
+ * Called from the assembly stub (_mcount or __gnu_mcount_nc).
+ *
+ * __mcount_internal updates data structures that represent traversals of the
+ * program's call graph edges.  frompc and selfpc are the return
+ * address and function address that represents the given call graph edge.
+ */
+void __noprof __mcount_internal(unsigned long frompc, unsigned long selfpc)
+{
+	ARCINDEX *frompcindex;
+	struct tostruct *top, *prevtop;
+	struct gmonparam *p;
+	ARCINDEX toindex;
+	int i;
+
+	p = &_gmonparam;
+
+	/*
+	 * Check that we are profiling and that we aren't recursively invoked.
+	 */
+	if (p->state != GMON_PROF_ON)
+		return;
+	p->state = GMON_PROF_BUSY;
+
+	frompc = adjust_pc(frompc);
+	selfpc = adjust_pc(selfpc);
+
+	/* Check that frompcindex is a reasonable pc value. */
+	frompc -= p->lowpc;
+	if (frompc > p->textsize)
+		goto done;
+
+	/* Note: keep in sync. with the initialization function above */
+	if ((HASHFRACTION & (HASHFRACTION - 1)) == 0) {
+		/* Avoid integer divide if possible */
+		i = frompc >> p->log_hashfraction;
+	} else {
+		i = frompc / (p->hashfraction * sizeof(*p->froms));
+	}
+	frompcindex = &p->froms[i];
+	toindex = *frompcindex;
+	if (toindex == 0) {
+		/* First time traversing this arc */
+		toindex = ++p->tos[0].link;
+		if (toindex >= p->tolimit) {
+			/* Halt further profiling */
+			goto overflow;
+		}
+
+		*frompcindex = toindex;
+		top = &p->tos[toindex];
+		top->selfpc = selfpc;
+		top->count = 1;
+		top->link = 0;
+		goto done;
+	}
+	top = &p->tos[toindex];
+	if (top->selfpc == selfpc) {
+		/* Arc at front of chain; usual case */
+		top->count++;
+		goto done;
+	}
+	/*
+	 * Have to go looking down chain for it.
+	 * top points to what we are looking at,
+	 * prevtop points to previous top.
+	 * we know it is not at the head of the chain.
+	 */
+	for (;;) {
+		if (top->link == 0) {
+			/*
+			 * top is end of the chain and none of the chain
+			 * had top->selfpc == selfpc.
+			 * so we allocate a new tostruct
+			 * and link it to the head of the chain.
+			 */
+			toindex = ++p->tos[0].link;
+			if (toindex >= p->tolimit)
+				goto overflow;
+
+			top = &p->tos[toindex];
+			top->selfpc = selfpc;
+			top->count = 1;
+			top->link = *frompcindex;
+			*frompcindex = toindex;
+			goto done;
+		}
+		/*
+		 * Otherwise, check the next arc on the chain.
+		 */
+		prevtop = top;
+		top = &p->tos[top->link];
+		if (top->selfpc == selfpc) {
+			/*
+			 * There it is. Increment its count, move it to the
+			 * head of the chain.
+			 */
+			top->count++;
+			toindex = prevtop->link;
+			prevtop->link = top->link;
+			top->link = *frompcindex;
+			*frompcindex = toindex;
+			goto done;
+		}
+	}
+done:
+	p->state = GMON_PROF_ON;
+	return;
+overflow:
+	p->state = GMON_PROF_ERROR;
+}
diff --git a/lib/libutee/arch/arm/gprof/gprof_a32.S b/lib/libutee/arch/arm/gprof/gprof_a32.S
new file mode 100644
index 00000000..f92387f8
--- /dev/null
+++ b/lib/libutee/arch/arm/gprof/gprof_a32.S
@@ -0,0 +1,76 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <asm.S>
+
+#ifdef CFG_TA_GPROF_SUPPORT
+
+/*
+ * Convert return address to call site address by subtracting the size of the
+ * mcount call instruction (blx __gnu_mcount_nc).
+ */
+.macro mcount_adj_pc rd, rn
+	bic	\rd, \rn, #1	/* Clear thumb bit if present */
+	sub	\rd, \rd, #4
+.endm
+
+/*
+ * With the -pg option, GCC (4.4+) inserts a call to __gnu_mcount_nc into
+ * every function prologue.
+ * The caller of the instrumented function can be determined from the lr value
+ * stored on the top of the stack. The callee, i.e. the instrumented function
+ * itself, is determined from the current value of lr. Then we call:
+ *   void __mcount_internal(void *frompc, void *selfpc);
+ *
+ * __gnu_mcount_nc is defined and set to the value of this function by the
+ * TA linker script, only if__gnu_mcount_nc is referenced
+ */
+FUNC __utee_mcount, :
+	stmdb		sp!, {r0-r3, lr}
+	ldr		r0, [sp, #20]		/* lr of instrumented func */
+	mcount_adj_pc	r0, r0
+	mcount_adj_pc	r1, lr			/* instrumented func */
+	bl		__mcount_internal
+	ldmia		sp!, {r0-r3, ip, lr}
+	bx		ip
+END_FUNC __utee_mcount
+
+#else /* !CFG_TA_GPROF_SUPPORT */
+
+/*
+ * The TA linker script always references __utee_mcount so provide a version
+ * that just pops one register (lr) off the stack, since that's the ABI we must
+ * follow.
+ */
+	.weak __utee_mcount
+FUNC __utee_mcount, :
+	push		{lr}
+	pop		{ip, lr}
+	bx		ip
+END_FUNC __utee_mcount
+
+#endif /* CFG_TA_GPROF_SUPPORT */
diff --git a/lib/libutee/arch/arm/gprof/gprof_a64.S b/lib/libutee/arch/arm/gprof/gprof_a64.S
new file mode 100644
index 00000000..fd3b9871
--- /dev/null
+++ b/lib/libutee/arch/arm/gprof/gprof_a64.S
@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <asm.S>
+
+#ifdef CFG_TA_GPROF_SUPPORT
+
+
+/*
+ * Convert return address to call site address by subtracting the size of one
+ * instruction.
+ */
+.macro adjust_pc rd, rn
+	sub	\rd, \rn, #4
+.endm
+
+/*
+ * void __utee_mcount(void *return_address)
+ * @return_address: return address to instrumented function
+ *
+ * With the -pg option, the compiler inserts a call to _mcount into
+ * every function prologue.
+ * x0 contains the value of lr (x30) before the call, that is the return
+ * address to the caller of the instrumented function. The callee, i.e. the
+ * instrumented function itself, is determined from the current value of x30.
+ * Then we call:
+ *   void __mcount_internal(void *frompc, void *selfpc);
+ *
+ * _mcount is defined and set to the value of this function by the linker
+ * script if the TA is instrumented, i.e., if _mcount is referenced
+ */
+FUNC __utee_mcount, :
+	stp		x29, x30, [sp, #-16]!
+	mov		x29, sp
+	adjust_pc	x0, x0
+	adjust_pc	x1, x30
+	bl		__mcount_internal
+	ldp		x29, x30, [sp], #16
+	ret
+END_FUNC __utee_mcount
+
+#else /* !CFG_TA_GPROF_SUPPORT */
+
+/*
+ * The TA linker script always references __utee_mcount so provide a version
+ * that does nothing
+ */
+	.weak __utee_mcount
+FUNC __utee_mcount, :
+	ret
+END_FUNC __utee_mcount
+
+#endif /* CFG_TA_GPROF_SUPPORT */
diff --git a/lib/libutee/arch/arm/gprof/gprof_pta.c b/lib/libutee/arch/arm/gprof/gprof_pta.c
new file mode 100644
index 00000000..7f54e8f8
--- /dev/null
+++ b/lib/libutee/arch/arm/gprof/gprof_pta.c
@@ -0,0 +1,113 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <pta_gprof.h>
+#include <string.h>
+#include <tee_api.h>
+#include "gprof_pta.h"
+
+static TEE_TASessionHandle sess = TEE_HANDLE_NULL;
+
+static TEE_Result invoke_gprof_pta(uint32_t cmd_id, uint32_t param_types,
+				   TEE_Param params[TEE_NUM_PARAMS])
+{
+	static const TEE_UUID core_uuid = PTA_GPROF_UUID;
+	TEE_Result res;
+
+	if (!sess) {
+		res = TEE_OpenTASession(&core_uuid, 0, 0, NULL, &sess, NULL);
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+	res = TEE_InvokeTACommand(sess, 0, cmd_id, param_types, params, NULL);
+	return res;
+}
+
+TEE_Result __pta_gprof_send(void *buf, size_t len, uint32_t *id)
+{
+	TEE_Param params[TEE_NUM_PARAMS];
+	uint32_t param_types;
+	TEE_Result res;
+
+	param_types = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INOUT,
+				      TEE_PARAM_TYPE_MEMREF_INPUT,
+				      TEE_PARAM_TYPE_NONE,
+				      TEE_PARAM_TYPE_NONE);
+	memset(params, 0, sizeof(params));
+	params[0].value.a = *id;
+	params[1].memref.buffer = buf;
+	params[1].memref.size = len;
+	res = invoke_gprof_pta(PTA_GPROF_SEND, param_types, params);
+	if (res == TEE_SUCCESS)
+		*id = params[0].value.a;
+	return res;
+}
+
+TEE_Result __pta_gprof_pc_sampling_start(void *buf, size_t len, size_t offset,
+					 size_t scale)
+{
+	TEE_Param params[TEE_NUM_PARAMS];
+	uint32_t param_types;
+
+	param_types = TEE_PARAM_TYPES(TEE_PARAM_TYPE_MEMREF_INOUT,
+				      TEE_PARAM_TYPE_VALUE_INPUT,
+				      TEE_PARAM_TYPE_NONE,
+				      TEE_PARAM_TYPE_NONE);
+	memset(params, 0, sizeof(params));
+	params[0].memref.buffer = buf;
+	params[0].memref.size = len;
+	params[1].value.a = offset;
+	params[1].value.b = scale;
+	return invoke_gprof_pta(PTA_GPROF_START_PC_SAMPLING, param_types,
+				params);
+}
+
+TEE_Result __pta_gprof_pc_sampling_stop(uint32_t *rate)
+{
+	TEE_Param params[TEE_NUM_PARAMS];
+	uint32_t param_types;
+	TEE_Result res;
+
+	param_types = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_OUTPUT,
+				      TEE_PARAM_TYPE_NONE,
+				      TEE_PARAM_TYPE_NONE,
+				      TEE_PARAM_TYPE_NONE);
+	memset(params, 0, sizeof(params));
+	res = invoke_gprof_pta(PTA_GPROF_STOP_PC_SAMPLING, param_types,
+				params);
+	if (res != TEE_SUCCESS)
+		return res;
+	if (rate)
+		*rate = params[0].value.a;
+	return res;
+}
+
+void __pta_gprof_fini(void)
+{
+	if (sess)
+		TEE_CloseTASession(sess);
+}
diff --git a/lib/libutee/arch/arm/gprof/gprof_pta.h b/lib/libutee/arch/arm/gprof/gprof_pta.h
new file mode 100644
index 00000000..ff2e7a37
--- /dev/null
+++ b/lib/libutee/arch/arm/gprof/gprof_pta.h
@@ -0,0 +1,41 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __GPROF_PTA_H
+#define __GPROF_PTA_H
+
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+#include <tee_api_types.h>
+
+TEE_Result __pta_gprof_send(void *buf, size_t len, uint32_t *id);
+TEE_Result __pta_gprof_pc_sampling_start(void *buf, size_t len, size_t offset,
+					 size_t scale);
+TEE_Result __pta_gprof_pc_sampling_stop(uint32_t *rate);
+void __pta_gprof_fini(void);
+#endif /* __GPROF_PTA_H */
diff --git a/lib/libutee/arch/arm/gprof/sub.mk b/lib/libutee/arch/arm/gprof/sub.mk
new file mode 100644
index 00000000..e46e37c9
--- /dev/null
+++ b/lib/libutee/arch/arm/gprof/sub.mk
@@ -0,0 +1,7 @@
+cppflags-y += -I$(sub-dir)/../../..
+
+srcs-$(CFG_TA_GPROF_SUPPORT) += gprof.c
+srcs-$(CFG_TA_GPROF_SUPPORT) += gprof_pta.c
+cflags-remove-gprof.c-y += -Wcast-align
+srcs-$(CFG_ARM32_$(sm)) += gprof_a32.S
+srcs-$(CFG_ARM64_$(sm)) += gprof_a64.S
diff --git a/lib/libutee/arch/arm/sub.mk b/lib/libutee/arch/arm/sub.mk
index 0caede50..f0c8e8a6 100644
--- a/lib/libutee/arch/arm/sub.mk
+++ b/lib/libutee/arch/arm/sub.mk
@@ -4,3 +4,5 @@ srcs-y += user_ta_entry.c
 srcs-y += utee_misc.c
 srcs-$(CFG_ARM32_$(sm)) += utee_syscalls_a32.S
 srcs-$(CFG_ARM64_$(sm)) += utee_syscalls_a64.S
+
+subdirs-y += gprof
diff --git a/lib/libutee/arch/arm/user_ta_entry.c b/lib/libutee/arch/arm/user_ta_entry.c
index 197b436a..08842c79 100644
--- a/lib/libutee/arch/arm/user_ta_entry.c
+++ b/lib/libutee/arch/arm/user_ta_entry.c
@@ -38,6 +38,19 @@
 #include <malloc.h>
 #include "tee_api_private.h"
 
+/*
+ * Pull in symbol __utee_mcount.
+ * This symbol is implemented in assembly in its own compilation unit, and is
+ * never referenced except by the linker script (in a PROVIDE() command).
+ * Because the compilation units are packed into an archive (libutee.a), the
+ * linker will discard the compilation units that are not explicitly
+ * referenced. AFAICT this occurs *before* the linker processes the PROVIDE()
+ * command, resulting in an "undefined symbol" error. We avoid this by
+ * adding an explicit reference here.
+ */
+extern uint8_t __utee_mcount[];
+void *_ref__utee_mcount __unused = &__utee_mcount;
+
 struct ta_session {
 	uint32_t session_id;
 	void *session_ctx;
@@ -93,6 +106,7 @@ static TEE_Result ta_header_add_session(uint32_t session_id)
 
 		if (!context_init) {
 			trace_set_level(tahead_get_trace_level());
+			__utee_gprof_init();
 			malloc_add_pool(ta_heap, ta_heap_size);
 			_TEE_MathAPI_Init();
 			context_init = true;
@@ -124,8 +138,10 @@ static void ta_header_remove_session(uint32_t session_id)
 			TEE_Free(itr);
 
 			ta_ref_count--;
-			if (ta_ref_count == 0)
+			if (ta_ref_count == 0) {
+				__utee_gprof_fini();
 				TA_DestroyEntryPoint();
+			}
 
 			return;
 		}
diff --git a/lib/libutee/include/pta_gprof.h b/lib/libutee/include/pta_gprof.h
new file mode 100644
index 00000000..9e95fb08
--- /dev/null
+++ b/lib/libutee/include/pta_gprof.h
@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __PTA_GPROF_H
+#define __PTA_GPROF_H
+
+/*
+ * Interface to the gprof pseudo-TA, which is used by libutee to control TA
+ * profiling and forward data to tee-supplicant.
+ */
+
+#define PTA_GPROF_UUID { 0x2f6e0d48, 0xc574, 0x426d, { \
+			 0x82, 0x4e, 0x40, 0x19, 0x8c, 0xde, 0x5c, 0xac } }
+
+/*
+ * Send TA profiling data (gmon.out format) to tee-supplicant
+ * Data may be sent in several chunks: first set id to 0, then re-use the
+ * allocated value in subsequent calls.
+ *
+ * [in/out] value[0].a: id
+ * [in]     memref[1]: profiling data
+ */
+#define PTA_GPROF_SEND			0
+
+/*
+ * Start PC sampling of a user TA session
+ *
+ * [in/out] memref[0]: sampling buffer
+ * [in]     value[1].a: offset: the lowest PC value in the TA
+ * [in]     value[1].b: scale: histogram scaling factor
+ */
+#define PTA_GPROF_START_PC_SAMPLING	1
+
+/*
+ * Stop PC sampling of a user TA session and retrieve data
+ *
+ * [out] value[0].a: sampling frequency
+ */
+#define PTA_GPROF_STOP_PC_SAMPLING	2
+
+#endif /* __PTA_GPROF_H */
diff --git a/lib/libutee/include/utee_syscalls.h b/lib/libutee/include/utee_syscalls.h
index 39e96235..463ed1f2 100644
--- a/lib/libutee/include/utee_syscalls.h
+++ b/lib/libutee/include/utee_syscalls.h
@@ -294,4 +294,6 @@ TEE_Result utee_se_channel_close(unsigned long c);
 /* op is of type enum utee_cache_operation */
 TEE_Result utee_cache_operation(void *va, size_t l, unsigned long op);
 
+TEE_Result utee_gprof_send(void *buf, size_t size, uint32_t *id);
+
 #endif /* UTEE_SYSCALLS_H */
diff --git a/lib/libutee/sub.mk b/lib/libutee/sub.mk
index 690d8536..bc936159 100644
--- a/lib/libutee/sub.mk
+++ b/lib/libutee/sub.mk
@@ -16,4 +16,4 @@ srcs-y += tee_api_panic.c
 subdirs-y += tui
 subdirs-y += arch/$(ARCH)
 
-cflags-lib-$(CFG_LIBUTEE_GPROF) += -pg
+cflags-lib-$(CFG_ULIBS_GPROF) += -pg
diff --git a/lib/libutee/tee_api_private.h b/lib/libutee/tee_api_private.h
index efb5dd65..5907d53b 100644
--- a/lib/libutee/tee_api_private.h
+++ b/lib/libutee/tee_api_private.h
@@ -44,5 +44,13 @@ void __utee_entry(unsigned long func, unsigned long session_id,
 			struct utee_params *up, unsigned long cmd_id);
 
 
+#if defined(CFG_TA_GPROF_SUPPORT)
+void __utee_gprof_init(void);
+void __utee_gprof_fini(void);
+#else
+static inline void __utee_gprof_init(void) {}
+static inline void __utee_gprof_fini(void) {}
+#endif
+
 #endif /*TEE_API_PRIVATE*/
 
diff --git a/lib/libutils/ext/include/compiler.h b/lib/libutils/ext/include/compiler.h
index 215f964c..b5a17afb 100644
--- a/lib/libutils/ext/include/compiler.h
+++ b/lib/libutils/ext/include/compiler.h
@@ -53,6 +53,7 @@
 #define __rodata	__section(".rodata")
 #define __rodata_unpaged __section(".rodata.__unpaged")
 #define __early_bss	__section(".early_bss")
+#define __noprof	__attribute__((no_instrument_function))
 
 #define __compiler_bswap64(x)	__builtin_bswap64((x))
 #define __compiler_bswap32(x)	__builtin_bswap32((x))
diff --git a/lib/libutils/sub.mk b/lib/libutils/sub.mk
index e6502d1e..e88e67d0 100644
--- a/lib/libutils/sub.mk
+++ b/lib/libutils/sub.mk
@@ -2,5 +2,5 @@ subdirs-$(CFG_LIBUTILS_WITH_ISOC) += isoc
 subdirs-y += ext
 
 ifneq ($(sm),core) # User-mode
-cflags-lib-$(CFG_U_LIBUTILS_GPROF) += -pg
+cflags-lib-$(CFG_ULIBS_GPROF) += -pg
 endif
diff --git a/mk/config.mk b/mk/config.mk
index afe1f7f6..8e6c1119 100644
--- a/mk/config.mk
+++ b/mk/config.mk
@@ -210,16 +210,19 @@ CFG_BOOT_SECONDARY_REQUEST ?= n
 # Default heap size for Core, 64 kB
 CFG_CORE_HEAP_SIZE ?= 65536
 
-ifeq ($(CFG_TA_GPROF),y)
-# Build various user-mode libraries with profiling enabled (-pg)
-CFG_LIBUTEE_GPROF ?= y
-CFG_U_LIBUTILS_GPROF ?= y
-CFG_U_LIBMPA_GPROF ?= y
-endif
-
-ifeq ($(filter y,$(CFG_LIBUTEE_GPROF) $(CFG_U_LIBUTILS_GPROF) \
-		 $(CFG_U_LIBMPA_GPROF)),y)
+# TA profiling.
+# When this option is enabled, OP-TEE can execute Trusted Applications
+# instrumented with GCC's -pg flag and will output profiling information
+# in gmon.out format to /tmp/gmon-<ta_uuid>.out (path is defined in
+# tee-supplicant)
+CFG_TA_GPROF_SUPPORT ?= n
+
+# Enable to compile user TA libraries with profiling (-pg).
+# Depends on CFG_TA_GPROF_SUPPORT.
+CFG_ULIBS_GPROF ?= n
+
+ifeq ($(CFG_ULIBS_GPROF),y)
 ifneq ($(CFG_TA_GPROF_SUPPORT),y)
-$(error Cannot instrument user library if user mode profiling is disabled)
+$(error Cannot instrument user libraries if user mode profiling is disabled)
 endif
 endif
diff --git a/ta/arch/arm/ta.ld.S b/ta/arch/arm/ta.ld.S
index 8a318401..b80abc34 100644
--- a/ta/arch/arm/ta.ld.S
+++ b/ta/arch/arm/ta.ld.S
@@ -1,10 +1,18 @@
 #ifdef ARM32
 OUTPUT_FORMAT("elf32-littlearm")
 OUTPUT_ARCH(arm)
+#define MCOUNT_SYM __gnu_mcount_nc
+/*
+ * This magic value corresponds to the size requested by
+ * libutee/arch/arm/gprof/gprof.c
+ */
+#define GPROF_BUF_MULT(x) ((x) * 136) / 100
 #endif
 #ifdef ARM64
 OUTPUT_FORMAT("elf64-littleaarch64")
 OUTPUT_ARCH(aarch64)
+#define MCOUNT_SYM _mcount
+#define GPROF_BUF_MULT(x) ((x) * 177) / 100
 #endif
 
 PHDRS {
@@ -24,6 +32,7 @@ SECTIONS {
 	.ta_head : {*(.ta_head)} :exec
 
 	.text : {
+		__text_start = .;
 		*(.text .text.*)
 		*(.stub)
 		*(.glue_7)
@@ -31,6 +40,8 @@ SECTIONS {
 		*(.gnu.linkonce.t.*)
 		/* Workaround for an erratum in ARM's VFP11 coprocessor */
 		*(.vfp11_veneer)
+		PROVIDE(MCOUNT_SYM = __utee_mcount);
+		__text_end = .;
 	}
 	.eh_frame : { *(.eh_frame) } :rodata
 	.rodata : {
@@ -77,7 +88,24 @@ SECTIONS {
 
 	.data : { *(.data .data.* .gnu.linkonce.d.*) } :rwdata
 
-	.bss : { *(.bss .bss.* .gnu.linkonce.b.* COMMON) }
+	.bss : {
+		*(.bss .bss.* .gnu.linkonce.b.* COMMON)
+
+		/*
+		 * TA profiling with gprof
+		 * Reserve some space for the profiling buffer, only if the
+		 * TA is instrumented (i.e., some files were built with -pg).
+		 * Note that PROVIDE() above defines a symbol only if it is
+		 * referenced in the object files.
+		 * This also provides a way to detect at runtime if the TA is
+		 * instrumented or not.
+		 */
+		. = ALIGN(8);
+		__gprof_buf_start = .;
+		. += DEFINED(MCOUNT_SYM) ?
+			GPROF_BUF_MULT(__text_end - __text_start) : 0;
+		__gprof_buf_end = .;
+	}
 
 	/DISCARD/ : { *(.interp) }
 }