From 40b838c8966f01bce4363ff2c2a6e9c574d348e2 Mon Sep 17 00:00:00 2001
From: Michael Scire <SciresM@gmail.com>
Date: Tue, 28 May 2019 02:56:15 -0700
Subject: [PATCH] exo: add extension smc to write to user address

---
 exosphere/src/smc_ams.c | 66 +++++++++++++++++++++++++++++++++++++++++
 exosphere/src/smc_ams.h |  1 +
 exosphere/src/smc_api.c |  6 ++++
 3 files changed, 73 insertions(+)

diff --git a/exosphere/src/smc_ams.c b/exosphere/src/smc_ams.c
index a84c44e82..e291236e7 100644
--- a/exosphere/src/smc_ams.c
+++ b/exosphere/src/smc_ams.c
@@ -155,3 +155,69 @@ uint32_t ams_iram_copy(smc_args_t *args) {
     
     return 0;
 }
+
+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[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 dram_page_offset = (dram_address & 0xFFFULL);
+    const size_t size = (size_t)(args->X[3]);
+
+    /* Validate addresses. */
+    if (!ams_is_user_addr_valid(dram_address)) {
+        return 2;
+    }
+
+    /* Validate size. */
+    switch (size) {
+        case 1:
+        case 2:
+        case 4:
+        case 8:
+            if ((size + dram_page_offset) > 0x1000) {
+                return 2;
+            }
+            break;
+        default:
+            return 2;
+    }
+
+    /* Validate alignment. */
+    if (dram_page_offset % size) {
+        return 2;
+    }
+
+    /* Map pages. */
+    ams_map_userpage(dram_address);
+
+    /* Write data. */
+    uintptr_t dram_ptr = (uintptr_t)(AMS_USER_PAGE_SECURE_MONITOR_ADDR + dram_page_offset);
+    switch (size) {
+        case 1:
+            *((volatile uint8_t *)dram_ptr) = (uint8_t)(args->X[2]);
+            break;
+        case 2:
+            *((volatile uint16_t *)dram_ptr) = (uint16_t)(args->X[2]);
+            break;
+        case 4:
+            *((volatile uint32_t *)dram_ptr) = (uint32_t)(args->X[2]);
+            break;
+        case 8:
+            *((volatile uint64_t *)dram_ptr) = args->X[2];
+            break;
+        default:
+            generic_panic();
+    }
+
+    /* Flush! */
+    flush_dcache_range((void *)dram_ptr, (void *)(dram_ptr + size));
+
+    /* Unmap pages. */
+    ams_unmap_userpage();
+
+    return 0;
+}
diff --git a/exosphere/src/smc_ams.h b/exosphere/src/smc_ams.h
index 01ec5a07a..cbae64d63 100644
--- a/exosphere/src/smc_ams.h
+++ b/exosphere/src/smc_ams.h
@@ -20,6 +20,7 @@
 #include "smc_api.h"
 
 uint32_t ams_iram_copy(smc_args_t *args);
+uint32_t ams_write_address(smc_args_t *args);
 
 void ams_map_irampage(uintptr_t iram_address);
 void ams_unmap_irampage(void);
diff --git a/exosphere/src/smc_api.c b/exosphere/src/smc_api.c
index 8e8f798da..01ee63ac5 100644
--- a/exosphere/src/smc_api.c
+++ b/exosphere/src/smc_api.c
@@ -77,6 +77,7 @@ uint32_t smc_read_write_register(smc_args_t *args);
 
 /* Atmosphere SMC prototypes */
 uint32_t smc_ams_iram_copy(smc_args_t *args);
+uint32_t smc_ams_write_address(smc_args_t *args);
 
 /* TODO: Provide a way to set this. It's 0 on non-recovery boot anyway... */
 static uint32_t g_smc_blacklist_mask = 0;
@@ -133,6 +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},
 };
 #define SMC_AMS_HANDLERS (sizeof(g_smc_ams_table) / sizeof(g_smc_ams_table[0]))
 
@@ -718,3 +720,7 @@ uint32_t smc_panic(smc_args_t *args) {
 uint32_t smc_ams_iram_copy(smc_args_t *args) {
     return smc_wrapper_sync(args, ams_iram_copy);
 }
+
+uint32_t smc_ams_write_address(smc_args_t *args) {
+    return smc_wrapper_sync(args, ams_write_address);
+}