exo2: implement warmboot through start of virtual exec

exosphere2/program/source/secmon_setup_warm.cpp

@@ -26,8 +26,12 @@ namespace ams::secmon {
 
     namespace {
 
+        constexpr inline uintptr_t MC = MemoryRegionPhysicalDeviceMemoryController.GetAddress();
+
         using namespace ams::mmu;
 
+        constexpr inline PageTableMappingAttribute MappingAttributesEl3SecureRwCode = AddMappingAttributeIndex(PageTableMappingAttributes_El3SecureRwCode, MemoryAttributeIndexNormal);
+
         void SetupCpuCommonControllers() {
             /* Set cpuactlr_el1. */
             {
@@ -146,6 +150,69 @@ namespace ams::secmon {
             hw::InstructionSynchronizationBarrier();
         }
 
+        bool IsExitLp0() {
+            return reg::Read(MC + MC_SECURITY_CFG3) == 0;
+        }
+
+        constexpr void AddPhysicalTzramIdentityMappingImpl(u64 *l1, u64 *l2, u64 *l3) {
+            /* Define extents. */
+            const uintptr_t start_address = MemoryRegionPhysicalTzram.GetAddress();
+            const size_t size             = MemoryRegionPhysicalTzram.GetSize();
+            const uintptr_t end_address   = start_address + size;
+
+            /* Flush cache for the L3 page table entries. */
+            {
+                const uintptr_t start = GetL3EntryIndex(start_address);
+                const uintptr_t end   = GetL3EntryIndex(end_address);
+                for (uintptr_t i = start; i < end; i += hw::DataCacheLineSize / sizeof(*l3)) {
+                    if (!std::is_constant_evaluated()) { hw::FlushDataCacheLine(l3 + i); }
+                }
+            }
+
+            /* Flush cache for the L2 page table entry. */
+            if (!std::is_constant_evaluated()) { hw::FlushDataCacheLine(l2 + GetL2EntryIndex(start_address)); }
+
+            /* Flush cache for the L1 page table entry. */
+            if (!std::is_constant_evaluated()) { hw::FlushDataCacheLine(l1 + GetL1EntryIndex(start_address)); }
+
+            /* Add the L3 mappings. */
+            SetL3BlockEntry(l3, start_address, start_address, size, MappingAttributesEl3SecureRwCode);
+
+            /* Add the L2 entry for the physical tzram region. */
+            SetL2TableEntry(l2, MemoryRegionPhysicalTzramL2.GetAddress(), MemoryRegionPhysicalTzramL2L3PageTable.GetAddress(), PageTableTableAttributes_El3SecureCode);
+
+            /* Add the L1 entry for the physical region. */
+            SetL1TableEntry(l1, MemoryRegionPhysical.GetAddress(), MemoryRegionPhysicalTzramL2L3PageTable.GetAddress(), PageTableTableAttributes_El3SecureCode);
+            static_assert(GetL1EntryIndex(MemoryRegionPhysical.GetAddress()) == 1);
+
+            /* Invalidate the data cache for the L3 page table entries. */
+            {
+                const uintptr_t start = GetL3EntryIndex(start_address);
+                const uintptr_t end   = GetL3EntryIndex(end_address);
+                for (uintptr_t i = start; i < end; i += hw::DataCacheLineSize / sizeof(*l3)) {
+                    if (!std::is_constant_evaluated()) { hw::InvalidateDataCacheLine(l3 + i); }
+                }
+            }
+
+            /* Flush cache for the L2 page table entry. */
+            if (!std::is_constant_evaluated()) { hw::InvalidateDataCacheLine(l2 + GetL2EntryIndex(start_address)); }
+
+            /* Flush cache for the L1 page table entry. */
+            if (!std::is_constant_evaluated()) { hw::InvalidateDataCacheLine(l1 + GetL1EntryIndex(start_address)); }
+        }
+
+        void AddPhysicalTzramIdentityMapping() {
+            /* Get page table extents. */
+            u64 * const l1    = MemoryRegionPhysicalTzramL1PageTable.GetPointer<u64>();
+            u64 * const l2_l3 = MemoryRegionPhysicalTzramL2L3PageTable.GetPointer<u64>();
+
+            /* Add the mapping. */
+            AddPhysicalTzramIdentityMappingImpl(l1, l2_l3, l2_l3);
+
+            /* Ensure that mappings are consistent. */
+            setup::EnsureMappingConsistency();
+        }
+
     }
 
     void SetupCpuMemoryControllersEnableMmu() {
@@ -204,7 +271,16 @@ namespace ams::secmon {
     }
 
     void SetupSocDmaControllersCpuMemoryControllersEnableMmuWarmboot() {
-        /* TODO */
+        /* If this is being called from lp0 exit, we want to setup the soc dma controllers. */
+        if (IsExitLp0()) {
+            SetupSocDmaControllers();
+        }
+
+        /* Add a physical TZRAM identity map. */
+        AddPhysicalTzramIdentityMapping();
+
+        /* Initialize cpu memory controllers and the MMU. */
+        SetupCpuMemoryControllersEnableMmu();
     }
 
 }

exosphere2/program/source/secmon_start_warm.s

@@ -95,10 +95,11 @@ _start_warm:
     ERRATUM_INVALIDATE_BTB_AT_BOOT
 
     /* Acquire exclusive access to the common warmboot stack. */
+    bl _ZN3ams6secmon26AcquireCommonWarmbootStackEv
 
     /* Set the stack pointer to the common warmboot stack address. */
     msr  spsel, #1
-    ldr x20, =0x1F01F67C0
+    ldr x20, =0x7C0107C0
     mov sp, x20
 
     /* Perform warmboot setup. */
@@ -108,3 +109,104 @@ _start_warm:
     b _ZN3ams6secmon20StartWarmbootVirtualEv
 
 
+/* void ams::secmon::AcquireCommonWarmbootStack() { */
+/* NOTE: This implements critical section enter via https://en.wikipedia.org/wiki/Lamport%27s_bakery_algorithm */
+/* This algorithm is used because the MMU is not awake yet, so exclusive load/store instructions are not usable. */
+/* NOTE: Nintendo attempted to implement this algorithm themselves, but did not really understand how it works. */
+/* They use the same ticket number for all cores; this can lead to starvation and other problems. */
+.section    .warmboot.text._ZN3ams6secmon26AcquireCommonWarmbootStackEv, "ax", %progbits
+.align      4
+.global     _ZN3ams6secmon26AcquireCommonWarmbootStackEv
+_ZN3ams6secmon26AcquireCommonWarmbootStackEv:
+    /* BakeryLock *lock = std::addressof(secmon::CommonWarmBootStackLock); */
+    ldr x0, =_ZN3ams6secmon23CommonWarmbootStackLockE
+
+    /* const u32 id = GetCurrentCoreId(); */
+    mrs x8, mpidr_el1
+    and x8, x8, #3
+
+    /* lock->customers[id].is_entering = true; */
+    ldrb w2, [x0, x8]
+    orr  w2, w2, #~0x7F
+    strb w2, [x0, x8]
+
+    /* const u8 ticket_0 = lock->customers[0].ticket_number; */
+    ldrb w4, [x0, #0]
+    and  w4, w4, #0x7F
+
+    /* const u8 ticket_1 = lock->customers[1].ticket_number; */
+    ldrb w5, [x0, #1]
+    and  w5, w5, #0x7F
+
+    /* const u8 ticket_2 = lock->customers[2].ticket_number; */
+    ldrb w6, [x0, #2]
+    and  w6, w6, #0x7F
+
+    /* const u8 ticket_3 = lock->customers[3].ticket_number; */
+    ldrb w7, [x0, #3]
+    and  w7, w7, #0x7F
+
+    /* u8 biggest_ticket = std::max(std::max(ticket_0, ticket_1), std::max(ticket_2, ticket_3)) */
+    cmp  w4, w5
+    csel w2, w4, w5, hi
+    cmp  w6, w7
+    csel w3, w6, w7, hi
+    cmp  w2, w3
+    csel w2, w2, w3, hi
+
+    /* NOTE: The biggest a ticket can ever be is 4, so the general increment is safe and 7-bit increment is not needed. */
+    /* lock->customers[id] = { .is_entering = false, .ticket_number = ++biggest_ticket }; */
+    add  w2, w2, #1
+    strb w2, [x0, x8]
+
+    /* Ensure instructions aren't reordered around this point. */
+    /* hw::DataSynchronizationBarrier(); */
+    dsb  sy
+
+    /* hw::SendEvent(); */
+    sev
+
+    /* for (unsigned int i = 0; i < 4; ++i) { */
+    mov  w3, #0
+1:
+    /*     hw::SendEventLocal(); */
+    sevl
+
+    /*     do { */
+2:
+    /*         hw::WaitForEvent(); */
+    wfe
+    /*     while (lock->customers[i].is_entering); */
+    ldrb w4, [x0, x3]
+    tbnz w4, #7, 2b
+
+    /*     u8 their_ticket; */
+
+    /*     hw::SendEventLocal(); */
+    sevl
+
+    /*    do { */
+2:
+    /*         hw::WaitForEvent(); */
+    wfe
+    /*         their_ticket = lock->customers[i].ticket_number; */
+    ldrb w4, [x0, x3]
+    ands w4, w4, #0x7F
+    /*         if (their_ticket == 0) { break; } */
+    b.eq 3f
+    /*    while ((their_ticket > my_ticket) || (their_ticket == my_ticket && id > i)); */
+    cmp  w2, w4
+    b.hi 2b
+    ccmp w8, w3, #0, eq
+    b.hi 2b
+
+    /* } */
+3:
+    add w3, w3, #1
+    cmp w3, #4
+    b.ne 1b
+
+    /* hw::DataMemoryBarrier(); */
+    dmb sy
+
+    ret