Add PMF instrumentation points in TF

In order to quantify the overall time spent in the PSCI software
implementation, an initial collection of PMF instrumentation points
has been added.

Instrumentation has been added to the following code paths:

- Entry to PSCI SMC handler.  The timestamp is captured as early
  as possible during the runtime exception and stored in memory
  before entering the PSCI SMC handler.

- Exit from PSCI SMC handler.  The timestamp is captured after
  normal return from the PSCI SMC handler or if a low power state
  was requested it is captured in the bl31 warm boot path before
  return to normal world.

- Entry to low power state.  The timestamp is captured before entry
  to a low power state which implies either standby or power down.
  As these power states are mutually exclusive, only one timestamp
  is defined to describe both.  It is possible to differentiate between
  the two power states using the PSCI STAT interface.

- Exit from low power state.  The timestamp is captured after a standby
  or power up operation has completed.

To calculate the number of cycles spent running code in Trusted Firmware
one can perform the following calculation:

(exit_psci - enter_psci) - (exit_low_pwr - enter_low_pwr).

The resulting number of cycles can be converted to time given the
frequency of the counter.

Change-Id: Ie3b8f3d16409b6703747093b3a2d5c7429ad0166
Signed-off-by: dp-arm <dimitris.papastamos@arm.com>

3 files changed, 55 insertions, 0 deletions

diff --git a/bl31/aarch64/bl31_entrypoint.S b/bl31/aarch64/bl31_entrypoint.S
index 4c3a515a..d14a68d0 100644
--- a/bl31/aarch64/bl31_entrypoint.S
+++ b/bl31/aarch64/bl31_entrypoint.S
@@ -31,6 +31,8 @@
 #include <arch.h>
 #include <bl_common.h>
 #include <el3_common_macros.S>
+#include <pmf_asm_macros.S>
+#include <runtime_instr.h>
 #include <xlat_tables.h>
 
 	.globl	bl31_entrypoint
@@ -141,6 +143,18 @@ endfunc bl31_entrypoint
 	 * --------------------------------------------------------------------
 	 */
 func bl31_warm_entrypoint
+#if ENABLE_RUNTIME_INSTRUMENTATION
+
+	/*
+	 * This timestamp update happens with cache off.  The next
+	 * timestamp collection will need to do cache maintenance prior
+	 * to timestamp update.
+	 */
+	pmf_calc_timestamp_addr rt_instr_svc RT_INSTR_EXIT_HW_LOW_PWR
+	mrs	x1, cntpct_el0
+	str	x1, [x0]
+#endif
+
 	/*
 	 * On the warm boot path, most of the EL3 initialisations performed by
 	 * 'el3_entrypoint_common' must be skipped:
@@ -188,5 +202,23 @@ func bl31_warm_entrypoint
 
 	bl	psci_warmboot_entrypoint
 
+#if ENABLE_RUNTIME_INSTRUMENTATION
+	pmf_calc_timestamp_addr rt_instr_svc RT_INSTR_EXIT_PSCI
+	mov	x19, x0
+
+	/*
+	 * Invalidate before updating timestamp to ensure previous timestamp
+	 * updates on the same cache line with caches disabled are properly
+	 * seen by the same core. Without the cache invalidate, the core might
+	 * write into a stale cache line.
+	 */
+	mov	x1, #PMF_TS_SIZE
+	mov	x20, x30
+	bl	inv_dcache_range
+	mov	x30, x20
+
+	mrs	x0, cntpct_el0
+	str	x0, [x19]
+#endif
 	b	el3_exit
 endfunc bl31_warm_entrypoint
diff --git a/bl31/aarch64/runtime_exceptions.S b/bl31/aarch64/runtime_exceptions.S
index 799062ef..f333bf16 100644
--- a/bl31/aarch64/runtime_exceptions.S
+++ b/bl31/aarch64/runtime_exceptions.S
@@ -31,6 +31,7 @@
 #include <arch.h>
 #include <asm_macros.S>
 #include <context.h>
+#include <cpu_data.h>
 #include <interrupt_mgmt.h>
 #include <platform_def.h>
 #include <runtime_svc.h>
@@ -47,6 +48,21 @@
 	msr	daifclr, #DAIF_ABT_BIT
 
 	str	x30, [sp, #CTX_GPREGS_OFFSET + CTX_GPREG_LR]
+
+#if ENABLE_RUNTIME_INSTRUMENTATION
+
+	/*
+	 * Read the timestamp value and store it in per-cpu data.
+	 * The value will be extracted from per-cpu data by the
+	 * C level SMC handler and saved to the PMF timestamp region.
+	 */
+	mrs	x30, cntpct_el0
+	str	x29, [sp, #CTX_GPREGS_OFFSET + CTX_GPREG_X29]
+	mrs	x29, tpidr_el3
+	str	x30, [x29, #CPU_DATA_PMF_TS0_OFFSET]
+	ldr	x29, [sp, #CTX_GPREGS_OFFSET + CTX_GPREG_X29]
+#endif
+
 	mrs	x30, esr_el3
 	ubfx	x30, x30, #ESR_EC_SHIFT, #ESR_EC_LENGTH
 
diff --git a/bl31/bl31_main.c b/bl31/bl31_main.c
index fae5ee4e..85b3ea1e 100644
--- a/bl31/bl31_main.c
+++ b/bl31/bl31_main.c
@@ -36,9 +36,16 @@
 #include <context_mgmt.h>
 #include <debug.h>
 #include <platform.h>
+#include <pmf.h>
+#include <runtime_instr.h>
 #include <runtime_svc.h>
 #include <string.h>
 
+#if ENABLE_RUNTIME_INSTRUMENTATION
+PMF_REGISTER_SERVICE_SMC(rt_instr_svc, PMF_RT_INSTR_SVC_ID,
+	RT_INSTR_TOTAL_IDS, PMF_STORE_ENABLE)
+#endif
+
 /*******************************************************************************
  * This function pointer is used to initialise the BL32 image. It's initialized
  * by SPD calling bl31_register_bl32_init after setting up all things necessary