diff --git a/include/bl32/payloads/tlk.h b/include/bl32/payloads/tlk.h index b6299a8..0ad1ac0 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 65941e6..ceb88e4 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, 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 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 5944174..b19e27d 100644 --- a/services/spd/tlkd/tlkd_common.c +++ b/services/spd/tlkd/tlkd_common.c @@ -35,6 +35,64 @@ #include #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 8d2d437..eb6b89d 100644 --- a/services/spd/tlkd/tlkd_main.c +++ b/services/spd/tlkd/tlkd_main.c @@ -188,6 +188,7 @@ { 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 @@ 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 88e720a..271c24c 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 @@ /******************************************************************************* * 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);