Translate secure/non-secure virtual addresses

This patch adds functionality to translate virtual addresses from
secure or non-secure worlds. This functionality helps Trusted Apps
to share virtual addresses directly and allows the NS world to
pass virtual addresses to TLK directly.

Change-Id: I77b0892963e0e839c448b5d0532920fb7e54dc8e
Signed-off-by: Varun Wadekar <vwadekar@nvidia.com>

5 files changed, 93 insertions, 0 deletions

diff --git a/include/bl32/payloads/tlk.h b/include/bl32/payloads/tlk.h
index b6299a8b5..0ad1ac06e 100644
--- a/include/bl32/payloads/tlk.h
+++ b/include/bl32/payloads/tlk.h
@@ -48,6 +48,7 @@
  */
 #define TLK_REQUEST_DONE	(0x32000001 | (1 << 31))
 #define TLK_ENTRY_DONE		(0x32000003 | (1 << 31))
+#define TLK_VA_TRANSLATE	(0x32000004 | (1 << 31))
 #define TLK_FID_SHARED_MEMBUF	(0x32000005 | (1 << 31))
 
 /*
diff --git a/include/lib/aarch64/arch_helpers.h b/include/lib/aarch64/arch_helpers.h
index 65941e6cf..ceb88e477 100644
--- a/include/lib/aarch64/arch_helpers.h
+++ b/include/lib/aarch64/arch_helpers.h
@@ -136,6 +136,14 @@ DEFINE_SYSOP_TYPE_PARAM_FUNC(dc, civac)
 DEFINE_SYSOP_TYPE_PARAM_FUNC(dc, cvau)
 DEFINE_SYSOP_TYPE_PARAM_FUNC(dc, zva)
 
+/*******************************************************************************
+ * Address translation accessor prototypes
+ ******************************************************************************/
+DEFINE_SYSOP_TYPE_PARAM_FUNC(at, s12e1r)
+DEFINE_SYSOP_TYPE_PARAM_FUNC(at, s12e1w)
+DEFINE_SYSOP_TYPE_PARAM_FUNC(at, s12e0r)
+DEFINE_SYSOP_TYPE_PARAM_FUNC(at, s12e0w)
+
 void flush_dcache_range(uint64_t, uint64_t);
 void inv_dcache_range(uint64_t, uint64_t);
 void dcsw_op_louis(uint32_t);
@@ -160,6 +168,7 @@ DEFINE_SYSREG_WRITE_CONST_FUNC(daifclr)
 #define disable_serror()		write_daifset(DAIF_ABT_BIT)
 #define disable_debug_exceptions()	write_daifset(DAIF_DBG_BIT)
 
+DEFINE_SYSREG_READ_FUNC(par_el1)
 DEFINE_SYSREG_READ_FUNC(id_pfr1_el1)
 DEFINE_SYSREG_READ_FUNC(id_aa64pfr0_el1)
 DEFINE_SYSREG_READ_FUNC(CurrentEl)
diff --git a/services/spd/tlkd/tlkd_common.c b/services/spd/tlkd/tlkd_common.c
index 5944174d5..b19e27d06 100644
--- a/services/spd/tlkd/tlkd_common.c
+++ b/services/spd/tlkd/tlkd_common.c
@@ -35,6 +35,64 @@
 #include <string.h>
 #include "tlkd_private.h"
 
+#define AT_MASK		3
+
+/*******************************************************************************
+ * This function helps the SP to translate NS/S virtual addresses.
+ ******************************************************************************/
+uint64_t tlkd_va_translate(uintptr_t va, int type)
+{
+	uint64_t pa;
+
+	if (type & TLK_TRANSLATE_NS_VADDR) {
+
+		/* save secure context */
+		cm_el1_sysregs_context_save(SECURE);
+
+		/* restore non-secure context */
+		cm_el1_sysregs_context_restore(NON_SECURE);
+
+		/* switch NS bit to start using 64-bit, non-secure mappings */
+		write_scr(cm_get_scr_el3(NON_SECURE));
+		isb();
+	}
+
+	int at = type & AT_MASK;
+	switch (at) {
+	case 0:
+		ats12e1r(va);
+		break;
+	case 1:
+		ats12e1w(va);
+		break;
+	case 2:
+		ats12e0r(va);
+		break;
+	case 3:
+		ats12e0w(va);
+		break;
+	default:
+		assert(0);
+	}
+
+	/* get the (NS/S) physical address */
+	isb();
+	pa = read_par_el1();
+
+	/* Restore secure state */
+	if (type & TLK_TRANSLATE_NS_VADDR) {
+
+		/* restore secure context */
+		cm_el1_sysregs_context_restore(SECURE);
+
+		/* switch NS bit to start using 32-bit, secure mappings */
+		write_scr(cm_get_scr_el3(SECURE));
+		isb();
+	}
+
+	return pa;
+}
+
 /*******************************************************************************
  * Given a secure payload entrypoint, register width, cpu id & pointer to a
  * context data structure, this function will create a secure context ready for
diff --git a/services/spd/tlkd/tlkd_main.c b/services/spd/tlkd/tlkd_main.c
index 8d2d437ed..eb6b89de5 100644
--- a/services/spd/tlkd/tlkd_main.c
+++ b/services/spd/tlkd/tlkd_main.c
@@ -188,6 +188,7 @@ uint64_t tlkd_smc_handler(uint32_t smc_fid,
 {
 	cpu_context_t *ns_cpu_context;
 	uint32_t ns;
+	uint64_t vaddr, type, par;
 
 	/* Passing a NULL context is a critical programming error */
 	assert(handle);
@@ -247,6 +248,24 @@ uint64_t tlkd_smc_handler(uint32_t smc_fid,
 		SMC_RET0(&tlk_ctx.cpu_ctx);
 
 	/*
+	 * Translate NS/EL1-S virtual addresses
+	 */
+	case TLK_VA_TRANSLATE:
+		if (ns || !tlk_args_results_buf)
+			SMC_RET1(handle, SMC_UNK);
+
+		/* virtual address and type: ns/s */
+		vaddr = tlk_args_results_buf->args[0];
+		type = tlk_args_results_buf->args[1];
+
+		par = tlkd_va_translate(vaddr, type);
+
+		/* Save PA for use by the SP on return */
+		store_tlk_args_results(par, 0, 0, 0);
+
+		SMC_RET0(handle);
+
+	/*
 	 * This is a request from the SP to mark completion of
 	 * a standard function ID.
 	 */
diff --git a/services/spd/tlkd/tlkd_private.h b/services/spd/tlkd/tlkd_private.h
index 88e720a7e..271c24ca2 100644
--- a/services/spd/tlkd/tlkd_private.h
+++ b/services/spd/tlkd/tlkd_private.h
@@ -57,6 +57,11 @@
 					   << STD_SMC_ACTIVE_FLAG_SHIFT))
 
 /*******************************************************************************
+ * Translate virtual address received from the NS world
+ ******************************************************************************/
+#define TLK_TRANSLATE_NS_VADDR		4
+
+/*******************************************************************************
  * Secure Payload execution state information i.e. aarch32 or aarch64
  ******************************************************************************/
 #define SP_AARCH32		MODE_RW_32
@@ -124,6 +129,7 @@ typedef struct tlk_context {
 /*******************************************************************************
  * Function & Data prototypes
  ******************************************************************************/
+uint64_t tlkd_va_translate(uintptr_t va, int type);
 uint64_t tlkd_enter_sp(uint64_t *c_rt_ctx);
 void __dead2 tlkd_exit_sp(uint64_t c_rt_ctx, uint64_t ret);
 uint64_t tlkd_synchronous_sp_entry(tlk_context_t *tlk_ctx);