From e6c5eb39286f87eff34aa948f4dc1c3042defff4 Mon Sep 17 00:00:00 2001
From: TuxSH <1922548+TuxSH@users.noreply.github.com>
Date: Thu, 1 Aug 2019 02:58:16 +0200
Subject: [PATCH] thermosphere: add shadow page table hooks

note: HCR.TVM not supported by qemu yet
---
 thermosphere/src/platform/qemu/memory_map.h  | 11 +++++
 thermosphere/src/platform/tegra/memory_map.h | 11 +++++
 thermosphere/src/shadow_page_tables.c        | 16 +++++--
 thermosphere/src/sysreg_traps.c              | 45 ++++++++++++++++++++
 thermosphere/src/traps.c                     |  4 +-
 5 files changed, 82 insertions(+), 5 deletions(-)

diff --git a/thermosphere/src/platform/qemu/memory_map.h b/thermosphere/src/platform/qemu/memory_map.h
index 331afbee9..8a0956e23 100644
--- a/thermosphere/src/platform/qemu/memory_map.h
+++ b/thermosphere/src/platform/qemu/memory_map.h
@@ -18,4 +18,15 @@
 
 #include "../../types.h"
 
+static inline u64 transformKernelAddress(u64 pa)
+{
+    switch (pa) {
+        // GICv2 CPU -> vCPU interface
+        case 0x08010000:
+            return 0x08040000;
+        default:
+            return pa;
+    }
+}
+
 uintptr_t configureMemoryMap(u32 *addrSpaceSize);
diff --git a/thermosphere/src/platform/tegra/memory_map.h b/thermosphere/src/platform/tegra/memory_map.h
index 331afbee9..20a2b176d 100644
--- a/thermosphere/src/platform/tegra/memory_map.h
+++ b/thermosphere/src/platform/tegra/memory_map.h
@@ -18,4 +18,15 @@
 
 #include "../../types.h"
 
+static inline u64 transformKernelAddress(u64 pa)
+{
+    switch (pa) {
+        // GICv2 CPU -> vCPU interface
+        case 0x50042000:
+            return 0x50046000;
+        default:
+            return pa;
+    }
+}
+
 uintptr_t configureMemoryMap(u32 *addrSpaceSize);
diff --git a/thermosphere/src/shadow_page_tables.c b/thermosphere/src/shadow_page_tables.c
index 963a54726..0e24120db 100644
--- a/thermosphere/src/shadow_page_tables.c
+++ b/thermosphere/src/shadow_page_tables.c
@@ -16,6 +16,8 @@
 
 
 #include "shadow_page_tables.h"
+#include "platform/memory_map_mmu_cfg.h"
+#include "debug_log.h"
 
 #ifdef A32_SUPPORTED
 static void replacePageTableShortL2(u32 *ttbl)
@@ -91,9 +93,11 @@ static void replacePageTableLongImpl(u64 *ttbl, u32 level, u32 nbits)
                     replacePageTableLongImpl((u64 *)addr, level + 1, 9);
                 } else {
                     u64 pa = ttbl[i] & MASK2L(47, 12);
-                    // FIXME
-                    if (pa == 0x50042000ull) {
-                        ttbl[i] = (ttbl[i] & ~MASK2L(47, 12)) | 0x50046000ull;
+                    u64 newPa = transformKernelAddress(pa);
+                    if (pa != newPa) {
+                        // Note: unreachable on QEMU yet
+                        DEBUG("EL1 in-place page-table entry swap: 0x%08llx => 0x%08llx\n", pa, newPa);
+                        ttbl[i] = (ttbl[i] & ~MASK2L(47, 12)) | newPa;
                     }
                 }
 
@@ -101,6 +105,7 @@ static void replacePageTableLongImpl(u64 *ttbl, u32 level, u32 nbits)
             }
 
             default:
+                __builtin_unreachable();
                 break;
         }
     }
@@ -125,4 +130,9 @@ void replacePageTableLong(u64 *ttbl, u32 txsz)
     } else if (startBit >= 12) {
         replacePageTableLongImpl(ttbl, 3, startBit - 11);
     }
+}
+
+void replaceKernelPageTables(void)
+{
+
 }
\ No newline at end of file
diff --git a/thermosphere/src/sysreg_traps.c b/thermosphere/src/sysreg_traps.c
index 61a339f65..64dae81a3 100644
--- a/thermosphere/src/sysreg_traps.c
+++ b/thermosphere/src/sysreg_traps.c
@@ -18,6 +18,8 @@
 #include "synchronization.h"
 #include "sysreg.h"
 #include "arm.h"
+#include "debug_log.h"
+#include "shadow_page_tables.h"
 
 static void doSystemRegisterRwImpl(u64 *val, u32 iss)
 {
@@ -86,6 +88,49 @@ void doSystemRegisterWrite(ExceptionStackFrame *frame, u32 iss, u32 reg1, u32 re
             val = frame->x[reg1];
         }
     }
+
+    // Hooks go here:
+    switch (iss) {
+        case ENCODE_SYSREG_ISS(SCTLR_EL1): {
+            DEBUG("Hooked sysreg write: SCTLR_EL1 = %0x016llx\n", val);
+            // Possible MMU (re)-enablement
+            if (val & 1) {
+                u64 tcr = GET_SYSREG(tcr_el1);
+                if (((tcr >> 14) & 3) == 0) {
+                    // 4KB granule
+                    replacePageTableLong((u64 *)(GET_SYSREG(ttbr0_el1) & MASK2L(47, 1)), (u32)(tcr & 0x3F));
+                    replacePageTableLong((u64 *)(GET_SYSREG(ttbr1_el1) & MASK2L(47, 1)), (u32)((tcr >> 16) & 0x3F));
+                }
+            }
+
+            break;
+        }
+        case ENCODE_SYSREG_ISS(TTBR1_EL1): {
+            DEBUG("Hooked sysreg write: TTBR1_EL1 = %0x016llx\n", val);
+            u64 tcr = GET_SYSREG(tcr_el1);
+            // MMU enabled & 4KB granule
+            if ((GET_SYSREG(sctlr_el1) & 1) && ((tcr >> 14) & 3) == 0) {
+                // Note: lack of ttbr0 intentional here
+                replacePageTableLong((u64 *)(val & MASK2L(47, 1)), (u32)((tcr >> 16) & 0x3F));
+            }
+            break;
+        }
+        case ENCODE_SYSREG_ISS(TCR_EL1): {
+            DEBUG("Hooked sysreg write: TCR_EL1 = %0x016llx\n", val);
+            u64 tcr = val;
+            // MMU enabled & 4KB granule
+            if ((GET_SYSREG(sctlr_el1) & 1) && ((tcr >> 14) & 3) == 0) {
+                // Note: lack of ttbr0 intentional here
+                replacePageTableLong((u64 *)(GET_SYSREG(ttbr1_el1) & MASK2L(47, 1)), (u32)((tcr >> 16) & 0x3F));
+            }
+            break;
+        }
+        // Note: TTBR0_EL1 deliberately not hooked
+
+        default:
+            break;
+    }
+
     doSystemRegisterRwImpl(&val, iss);
     skipFaultingInstruction(frame, 4);
 }
diff --git a/thermosphere/src/traps.c b/thermosphere/src/traps.c
index 33acbd687..b471d0d2a 100644
--- a/thermosphere/src/traps.c
+++ b/thermosphere/src/traps.c
@@ -36,8 +36,8 @@ void enableTraps(void)
     u64 hcr = GET_SYSREG(hcr_el2);
 
     // Trap *writes* to memory control registers
-    //hcr |= HCR_TVM;
-    // actually don't
+    // Note: QEMU doesn't support this bit as of August 2019...
+    hcr |= HCR_TVM;
 
     // Trap SMC instructions
     hcr |= HCR_TSC;