Snap for 4998801 from 0a6e4024f3580dad7adf564f71142236a914300f to oc-mr1-1.2-iot-releaseandroid-o-mr1-iot-release-smart-display-r4android-o-mr1-iot-release-smart-display-r3oreo-mr1-1.2-iot-release

Change-Id: I6a116de40f3ce0c0516b9196c899e98f7e339e78

1 files changed, 33 insertions, 16 deletions

diff --git a/lib/trusty/ql-tipc/arch/arm/trusty_mem.c b/lib/trusty/ql-tipc/arch/arm/trusty_mem.c
index e980b958ee..56d8348d3c 100644
--- a/lib/trusty/ql-tipc/arch/arm/trusty_mem.c
+++ b/lib/trusty/ql-tipc/arch/arm/trusty_mem.c
@@ -79,9 +79,28 @@ typedef uintptr_t paddr_t;
 /*
  * ARM64
  */
-static void arm64_write_ATS1E1W(uint64_t vaddr)
+
+/* Note: this will crash if called from user space */
+static void arm64_write_ATS1ExW(uint64_t vaddr)
 {
-    __asm__ volatile("at S1E1W, %0" :: "r" (vaddr));
+    uint64_t _current_el;
+
+    __asm__ volatile("mrs %0, CurrentEL" : "=r" (_current_el));
+
+    _current_el = (_current_el >> 2) & 0x3;
+    switch (_current_el) {
+    case 0x1:
+        __asm__ volatile("at S1E1W, %0" :: "r" (vaddr));
+        break;
+    case 0x2:
+        __asm__ volatile("at S1E2W, %0" :: "r" (vaddr));
+        break;
+    case 0x3:
+    default:
+        trusty_fatal("Unsupported execution state: EL%u\n", _current_el );
+        break;
+    }
+
     __asm__ volatile("isb" ::: "memory");
 }
 
@@ -99,7 +118,7 @@ static uint64_t va2par(vaddr_t va)
     unsigned long irq_state;
 
     trusty_local_irq_disable(&irq_state);
-    arm64_write_ATS1E1W(va);
+    arm64_write_ATS1ExW(va);
     par = arm64_read_par64();
     trusty_local_irq_restore(&irq_state);
 
@@ -136,20 +155,18 @@ static uint64_t par2attr(uint64_t par)
 /*
  * ARM32
  */
-static void arm_write_ATS1CPW(uint64_t vaddr)
-{
-    __asm__ volatile(
-        "mcr    p15, 0, %0, c7, c8, 1   \n"
-        : : "r"(vaddr)
-    );
-}
 
-static void arm_write_ATS1HW(uint64_t vaddr)
+/* Note: this will crash if called from user space */
+static void arm_write_ATS1xW(uint64_t vaddr)
 {
-    __asm__ volatile(
-        "mcr    p15, 4, %0, c7, c8, 1   \n"
-        : : "r"(vaddr)
-    );
+    uint32_t _cpsr;
+
+    __asm__ volatile("mrs %0, cpsr" : "=r"(_cpsr));
+
+    if ((_cpsr & 0xF) == 0xa)
+        __asm__ volatile("mcr    p15, 4, %0, c7, c8, 1" : : "r"(vaddr));
+    else
+        __asm__ volatile("mcr    p15, 0, %0, c7, c8, 1" : : "r"(vaddr));
 }
 
 static uint64_t arm_read_par64(void)
@@ -242,7 +259,7 @@ static uint64_t va2par(vaddr_t va)
     unsigned long irq_state;
 
     trusty_local_irq_disable(&irq_state);
-    arm_write_ATS1CPW(va); /* need to call the right one */
+    arm_write_ATS1xW(va);
     par = arm_read_par64();
     trusty_local_irq_restore(&irq_state);