From 6dd366cb22c9d2cbf7530d3e6b4f92bd4c91ef2e Mon Sep 17 00:00:00 2001
From: Michael Scire <SciresM@gmail.com>
Date: Tue, 28 May 2019 13:25:12 -0700
Subject: [PATCH] Fix emunand SMC behaviors

---
 exosphere/src/configitem.c | 3 ++-
 exosphere/src/smc_ams.c    | 5 +++--
 exosphere/src/smc_api.c    | 2 +-
 exosphere/src/utils.h      | 7 +++++++
 4 files changed, 13 insertions(+), 4 deletions(-)

diff --git a/exosphere/src/configitem.c b/exosphere/src/configitem.c
index dc6cbf2dc..4adb878b3 100644
--- a/exosphere/src/configitem.c
+++ b/exosphere/src/configitem.c
@@ -286,7 +286,8 @@ uint32_t configitem_get(bool privileged, ConfigItem item, uint64_t *p_outvalue)
             break;
         case CONFIGITEM_EMUNAND_CONFIG:
             /* UNOFFICIAL: Gets configuration meta for emunand. */
-            *p_outvalue = exosphere_get_emunand_config();
+            *p_outvalue = 1; //exosphere_get_emunand_config();
+            break;
         default:
             result = 2;
             break;
diff --git a/exosphere/src/smc_ams.c b/exosphere/src/smc_ams.c
index e291236e7..f71d947a6 100644
--- a/exosphere/src/smc_ams.c
+++ b/exosphere/src/smc_ams.c
@@ -159,11 +159,12 @@ uint32_t ams_iram_copy(smc_args_t *args) {
 uint32_t ams_write_address(smc_args_t *args) {
     /* Implements a write to a DRAM page. */
     /* This operation can be used to write to read-only pages. */
-    /* args->X[1] = DRAM address (translated by kernel), must be size-bytes aligned. */
+    /* args->X[1] = Virtual address, must be size-bytes aligned and readable by EL0. */
     /* args->X[2] = Value. */
     /* args->X[3] = size (must be 1, 2, 4, or 8). */
 
-    const uintptr_t dram_address = (uintptr_t)args->X[1];
+    const uintptr_t el0_virtual_address = (uintptr_t)args->X[1];
+    const uintptr_t dram_address = get_physical_address_el0(el0_virtual_address);
     const uintptr_t dram_page_offset = (dram_address & 0xFFFULL);
     const size_t size = (size_t)(args->X[3]);
 
diff --git a/exosphere/src/smc_api.c b/exosphere/src/smc_api.c
index 01ee63ac5..65762233e 100644
--- a/exosphere/src/smc_api.c
+++ b/exosphere/src/smc_api.c
@@ -134,7 +134,7 @@ static smc_table_entry_t g_smc_ams_table[] = {
     {0, 4, NULL},
     {0xF0000201, 0, smc_ams_iram_copy},
     {0xF0000002, 0, smc_read_write_register},
-    {0xF0000203, 0, smc_ams_write_address},
+    {0xF0000003, 0, smc_ams_write_address},
 };
 #define SMC_AMS_HANDLERS (sizeof(g_smc_ams_table) / sizeof(g_smc_ams_table[0]))
 
diff --git a/exosphere/src/utils.h b/exosphere/src/utils.h
index 9cc801959..727713aeb 100644
--- a/exosphere/src/utils.h
+++ b/exosphere/src/utils.h
@@ -70,6 +70,13 @@ static inline uintptr_t get_physical_address(const void *vaddr) {
     return (PAR & 1) ? 0ull : (PAR & MASK2L(40, 12)) | ((uintptr_t)vaddr & MASKL(12));
 }
 
+static inline uintptr_t get_physical_address_el0(const uintptr_t el0_vaddr) {
+    uintptr_t PAR;
+    __asm__ __volatile__ ("at s1e0r, %0" :: "r"(el0_vaddr));
+    __asm__ __volatile__ ("mrs %0, par_el1" : "=r"(PAR));
+    return (PAR & 1) ? 0ull : (PAR & MASK2L(40, 12)) | ((uintptr_t)el0_vaddr & MASKL(12));
+}
+
 static inline uint32_t read32le(const volatile void *dword, size_t offset) {
     return *(uint32_t *)((uintptr_t)dword + offset);
 }