exo/mariko fatal: halt other cores when beginning fatal program

exosphere/program/source/secmon_error.cpp

@@ -21,17 +21,6 @@ namespace ams {
     namespace {
 
         constexpr bool SaveSystemStateForDebug = false;
-        constexpr bool LogSystemStateForDebug = false;
-
-        void LogU64(u64 value) {
-            char buffer[2 * sizeof(value)];
-            for (size_t i = 0; i < sizeof(value); ++i) {
-                buffer[sizeof(buffer) - 1 - (2 * i) - 0] = "0123456789ABCDEF"[(value >> 0) & 0xF];
-                buffer[sizeof(buffer) - 1 - (2 * i) - 1] = "0123456789ABCDEF"[(value >> 4) & 0xF];
-                value >>= 8;
-            }
-            log::SendText(buffer, sizeof(buffer));
-        }
 
     }
 }
@@ -114,57 +103,6 @@ namespace ams::secmon {
             util::WaitMicroSeconds(1000);
         }
 
-        ALWAYS_INLINE void LogSystemStateForDebugErrorReboot(u64 lr, u64 sp) {
-            log::SendText("*** Error Reboot ***\n", 21);
-            log::Flush();
-
-            u64 temp_reg;
-
-            __asm__ __volatile__("mrs %0, esr_el3" : "=r"(temp_reg) :: "memory");
-            log::SendText("ESR_EL3: ", 9);
-            LogU64(temp_reg);
-            log::SendText("\n", 1);
-            log::Flush();
-
-            __asm__ __volatile__("mrs %0, elr_el3" : "=r"(temp_reg) :: "memory");
-            log::SendText("ELR_EL3: ", 9);
-            LogU64(temp_reg);
-            log::SendText("\n", 1);
-            log::Flush();
-
-            __asm__ __volatile__("mrs %0, far_el3" : "=r"(temp_reg) :: "memory");
-            log::SendText("FAR_EL3: ", 9);
-            LogU64(temp_reg);
-            log::SendText("\n", 1);
-            log::Flush();
-
-            log::SendText("LR:      ", 9);
-            LogU64(lr);
-            log::SendText("\n", 1);
-            log::Flush();
-
-            log::SendText("SP:      ", 9);
-            LogU64(sp);
-            log::SendText("\n", 1);
-            log::Flush();
-
-            log::SendText("Stack:\n", 7);
-            log::Flush();
-
-            char buf[2];
-            for (int i = 0; i < 0x100; ++i) {
-                const u8 byte = *(volatile u8 *)(sp + i);
-                buf[0] = "0123456789ABCDEF"[(byte >> 4) & 0xF];
-                buf[1] = "0123456789ABCDEF"[(byte >> 0) & 0xF];
-                log::SendText(buf, 2);
-                log::Flush();
-                if (util::IsAligned(i + 1, 0x10)) {
-                    log::SendText("\n", 1);
-                    log::Flush();
-                }
-            }
-        }
-
     }
 
     void SetError(pkg1::ErrorInfo info) {
@@ -181,14 +119,6 @@ namespace ams::secmon {
             SaveSystemStateForDebugErrorReboot();
         }
 
-        if constexpr (LogSystemStateForDebug) {
-            u64 lr, sp;
-            __asm__ __volatile__("mov %0, lr" : "=r"(lr) :: "memory");
-            __asm__ __volatile__("mov %0, sp" : "=r"(sp) :: "memory");
-
-            LogSystemStateForDebugErrorReboot(lr, sp);
-        }
-
         /* Lockout the security engine. */
         se::Lockout();
 

exosphere/program/source/secmon_exception_vectors.s

@@ -163,22 +163,22 @@ vector_entry irq_a64
     check_vector_size irq_a64
 
 vector_entry fiq_a64
-    /* Save X29, X30. */
+    /* Save x18, x26-x30. */
     stp x29, x30, [sp, #-0x10]!
-
-    /* Get the current core ID, ensure it's core 3. */
-    mrs x29, mpidr_el1
-    and x29, x29, #3
-    cmp x29, #3
-    b.ne _ZN3ams6secmon26UnexpectedExceptionHandlerEv
-
-    /* Save x26-x28, x18. */
     stp x28, x18, [sp, #-0x10]!
     stp x26, x27, [sp, #-0x10]!
 
     /* Set x18 to the global data region. */
     ldr x18, =0x1F01FA000
 
+    /* Get the current core. */
+    mrs x29, mpidr_el1
+    and x29, x29, #3
+
+    /* If we're not on core 3, take the core0-2 handler. */
+    cmp x29, #3
+    b.ne _ZN3ams6secmon25HandleFiqExceptionCore012Ev
+
     /* Handle the fiq exception. */
     bl _ZN3ams6secmon18HandleFiqExceptionEv
 
@@ -326,7 +326,41 @@ _ZN3ams6secmon18HandleFiqExceptionEv:
 vector_entry serror_a32
     /* An unexpected exception was taken. */
     b _ZN3ams6secmon26UnexpectedExceptionHandlerEv
-    check_vector_size serror_a32
+    .endfunc
+    .cfi_endproc
+_ZN3ams6secmon25HandleFiqExceptionCore012Ev:
+   /* Acquire exclusive access to the common smc stack. */
+    stp x4, x5, [sp, #-0x10]!
+    stp x2, x3, [sp, #-0x10]!
+    stp x0, x1, [sp, #-0x10]!
+    bl _ZN3ams6secmon25AcquireCommonSmcStackLockEv
+    ldp x0, x1, [sp], #0x10
+    ldp x2, x3, [sp], #0x10
+    ldp x4, x5, [sp], #0x10
+
+    /* Pivot to use the common smc stack. */
+    mov x30, sp
+    ldr x29, =0x1F01F6E80
+    mov sp, x29
+    stp x29, x30, [sp, #-0x10]!
+
+    /* Handle the fiq exception. */
+    bl _ZN3ams6secmon18HandleFiqExceptionEv
+
+    /* Restore our core-specific stack. */
+    ldp x29, x30, [sp], #0x10
+    mov sp, x30
+
+    /* Release our exclusive access to the common smc stack. */
+    stp x0, x1, [sp, #-0x10]!
+    bl _ZN3ams6secmon25ReleaseCommonSmcStackLockEv
+    ldp x0, x1, [sp], #0x10
+
+    /* Return. */
+    ldp x26, x27, [sp], #0x10
+    ldp x28, x18, [sp], #0x10
+    ldp x29, x30, [sp], #0x10
+    eret
 
     /* Instantiate the literal pool for the exception vectors. */
     .ltorg

exosphere/program/source/secmon_interrupt_handler.cpp

@@ -25,14 +25,16 @@ namespace ams::secmon {
 
         constinit InterruptHandler g_handlers[InterruptHandlersMax] = {};
         constinit int g_interrupt_ids[InterruptHandlersMax]         = {};
+        constinit u8 g_interrupt_core_masks[InterruptHandlersMax]   = {};
 
     }
 
-    void SetInterruptHandler(int interrupt_id, InterruptHandler handler) {
+    void SetInterruptHandler(int interrupt_id, u8 core_mask, InterruptHandler handler) {
         for (int i = 0; i < InterruptHandlersMax; ++i) {
             if (g_interrupt_ids[i] == 0) {
                 g_interrupt_ids[i]        = interrupt_id;
                 g_handlers[i]             = handler;
+                g_interrupt_core_masks[i] = core_mask;
                 return;
             }
         }
@@ -51,6 +53,9 @@ namespace ams::secmon {
         /* Check each handler. */
         for (int i = 0; i < InterruptHandlersMax; ++i) {
             if (g_interrupt_ids[i] == interrupt_id) {
+                /* Validate that we can invoke the handler. */
+                AMS_ABORT_UNLESS((g_interrupt_core_masks[i] & (1u << hw::GetCurrentCoreId())) != 0);
+
                 /* Invoke the handler. */
                 g_handlers[i]();
                 gic::SetEndOfInterrupt(interrupt_id);

exosphere/program/source/secmon_interrupt_handler.hpp

@@ -20,6 +20,6 @@ namespace ams::secmon {
 
     using InterruptHandler = void (*)();
 
-    void SetInterruptHandler(int interrupt_id, InterruptHandler handler);
+    void SetInterruptHandler(int interrupt_id, u8 core_mask, InterruptHandler handler);
 
 }

exosphere/program/source/secmon_mariko_fatal_error.cpp

@@ -0,0 +1,90 @@
+/*
+ * Copyright (c) 2018-2020 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <exosphere.hpp>
+#include "secmon_cpu_context.hpp"
+#include "secmon_map.hpp"
+#include "secmon_page_mapper.hpp"
+#include "secmon_mariko_fatal_error.hpp"
+#include "smc/secmon_smc_power_management.hpp"
+
+namespace ams::secmon {
+
+    namespace {
+
+        constinit u8 g_fatal_error_mask = 0;
+
+    }
+
+    void HandleMarikoFatalErrorInterrupt() {
+        /* This interrupt handler doesn't return, so mark that we're at end of interrupt. */
+        gic::SetEndOfInterrupt(MarikoFatalErrorInterruptId);
+
+        /* Get the current core id. */
+        const auto core_id = hw::GetCurrentCoreId();
+
+        /* Set that we received the fatal on the current core. */
+        g_fatal_error_mask |= (1u << core_id);
+        hw::FlushDataCache(std::addressof(g_fatal_error_mask), sizeof(g_fatal_error_mask));
+        hw::DataSynchronizationBarrier();
+
+        /* If not all cores have received the fatal, we need to trigger the interrupt on other cores. */
+        if (g_fatal_error_mask != (1u << NumCores) - 1) {
+
+            /* Configure and send the interrupt to the next core. */
+            const auto next_core = __builtin_ctz(~g_fatal_error_mask);
+            gic::SetSpiTargetCpu(MarikoFatalErrorInterruptId, (1u << next_core));
+            gic::SetPending(MarikoFatalErrorInterruptId);
+        }
+
+        /* If current core is not 3, kill ourselves. */
+        if (core_id != NumCores - 1) {
+            smc::PowerOffCpu();
+        } else {
+            /* Wait for all cores to kill themselves. */
+            while (g_fatal_error_mask != (1u << NumCores) - 1) {
+                util::WaitMicroSeconds(100);
+            }
+        }
+
+        /* Copy the fatal error context to mariko tzram. */
+        {
+            /* Map the iram page. */
+            constexpr uintptr_t FatalErrorPhysicalAddress = MemoryRegionPhysicalIramFatalErrorContext.GetAddress();
+            AtmosphereIramPageMapper mapper(FatalErrorPhysicalAddress);
+            if (mapper.Map()) {
+                /* Copy the fatal error context. */
+                      void *dst = MemoryRegionVirtualTzramMarikoProgramFatalErrorContext.GetPointer<void>();
+                const void *src = mapper.GetPointerTo(FatalErrorPhysicalAddress, sizeof(ams::impl::FatalErrorContext));
+                std::memcpy(dst, src, sizeof(ams::impl::FatalErrorContext));
+            }
+        }
+
+        /* Map Dram for the mariko program. */
+        MapDramForMarikoProgram();
+
+        AMS_SECMON_LOG("%s\n", "Jumping to Mariko Fatal.");
+        AMS_LOG_FLUSH();
+
+        /* Jump to the mariko fatal program. */
+        reinterpret_cast<void (*)()>(secmon::MemoryRegionVirtualTzramMarikoProgram.GetAddress())();
+
+        /* The mariko fatal program never returns. */
+        __builtin_unreachable();
+
+        AMS_INFINITE_LOOP();
+    }
+
+}

exosphere/program/source/secmon_mariko_fatal_error.hpp

@@ -0,0 +1,25 @@
+/*
+ * Copyright (c) 2018-2020 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+#include <exosphere.hpp>
+
+namespace ams::secmon {
+
+    constexpr inline int MarikoFatalErrorInterruptId = 198;
+
+    NORETURN void HandleMarikoFatalErrorInterrupt();
+
+}

exosphere/program/source/secmon_setup.cpp

@@ -18,6 +18,7 @@
 #include "secmon_error.hpp"
 #include "secmon_map.hpp"
 #include "secmon_cpu_context.hpp"
+#include "secmon_mariko_fatal_error.hpp"
 #include "secmon_interrupt_handler.hpp"
 #include "secmon_misc.hpp"
 #include "smc/secmon_random_cache.hpp"
@@ -1082,25 +1083,36 @@ namespace ams::secmon {
 
         /* Setup the security engine interrupt. */
         constexpr int SecurityEngineInterruptId = 90;
+        constexpr u8  SecurityEngineInterruptCoreMask = (1 << 3);
         gic::SetPriority      (SecurityEngineInterruptId,            gic::HighestPriority);
         gic::SetInterruptGroup(SecurityEngineInterruptId,                               0);
         gic::SetEnable        (SecurityEngineInterruptId,                            true);
-        gic::SetSpiTargetCpu  (SecurityEngineInterruptId,                 (1 << 3));
+        gic::SetSpiTargetCpu  (SecurityEngineInterruptId, SecurityEngineInterruptCoreMask);
         gic::SetSpiMode       (SecurityEngineInterruptId,        gic::InterruptMode_Level);
 
         /* Setup the activity monitor interrupt. */
         constexpr int ActivityMonitorInterruptId = 77;
+        constexpr u8  ActivityMonitorInterruptCoreMask = (1 << 3);
         gic::SetPriority      (ActivityMonitorInterruptId,            gic::HighestPriority);
         gic::SetInterruptGroup(ActivityMonitorInterruptId,                               0);
         gic::SetEnable        (ActivityMonitorInterruptId,                            true);
-        gic::SetSpiTargetCpu  (ActivityMonitorInterruptId,                 (1 << 3));
+        gic::SetSpiTargetCpu  (ActivityMonitorInterruptId, ActivityMonitorInterruptCoreMask);
         gic::SetSpiMode       (ActivityMonitorInterruptId,        gic::InterruptMode_Level);
 
+        /* Setup the mariko fatal error interrupt. */
+        constexpr u8 MarikoFatalInterruptCoreMask = 0b1111;
+        gic::SetPriority      (MarikoFatalErrorInterruptId,         gic::HighestPriority);
+        gic::SetInterruptGroup(MarikoFatalErrorInterruptId,                            0);
+        gic::SetEnable        (MarikoFatalErrorInterruptId,                         true);
+        gic::SetSpiTargetCpu  (MarikoFatalErrorInterruptId,                            0);
+        gic::SetSpiMode       (MarikoFatalErrorInterruptId,     gic::InterruptMode_Level);
+
         /* If we're coldboot, perform one-time setup. */
         if (g_is_cold_boot) {
-            /* Register both interrupt handlers. */
+            /* Register all interrupt handlers. */
-            SetInterruptHandler(SecurityEngineInterruptId,      se::HandleInterrupt);
+            SetInterruptHandler(SecurityEngineInterruptId,   SecurityEngineInterruptCoreMask,      se::HandleInterrupt);
-            SetInterruptHandler(ActivityMonitorInterruptId, actmon::HandleInterrupt);
+            SetInterruptHandler(ActivityMonitorInterruptId,  ActivityMonitorInterruptCoreMask, actmon::HandleInterrupt);
+            SetInterruptHandler(MarikoFatalErrorInterruptId, MarikoFatalInterruptCoreMask,     secmon::HandleMarikoFatalErrorInterrupt);
 
             /* We're expecting the other cores to come out of reset. */
             for (int i = 1; i < NumCores; ++i) {

exosphere/program/source/secmon_user_power_management.cpp

@@ -14,8 +14,9 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 #include <exosphere.hpp>
-#include "secmon_map.hpp"
+#include "secmon_cpu_context.hpp"
 #include "secmon_page_mapper.hpp"
+#include "secmon_mariko_fatal_error.hpp"
 #include "secmon_user_power_management.hpp"
 
 #include "rebootstub_bin.h"
@@ -90,34 +91,11 @@ namespace ams::secmon {
             /* On Erista, we reboot to fatal error by jumping to fusee primary's handler. */
             return PerformUserRebootToPayload();
         } else /* if (fuse::GetSocType() == fuse::SocType_Mariko) */ {
-            /* TODO: Send a SGI FIQ to the other CPUs, so that user code stops executing. */
+            /* Call the fatal error handler. */
+            HandleMarikoFatalErrorInterrupt();
 
-            /* TODO: On cores other than 3, halt/wfi. */
+            /* We should never get to this point. */
-
+            AMS_ABORT("Returned from Mariko Fatal handler?\n");
-            /* Copy the fatal error context to mariko tzram. */
-            {
-                /* Map the iram page. */
-                constexpr uintptr_t FatalErrorPhysicalAddress = MemoryRegionPhysicalIramFatalErrorContext.GetAddress();
-                AtmosphereIramPageMapper mapper(FatalErrorPhysicalAddress);
-                if (mapper.Map()) {
-                    /* Copy the fatal error context. */
-                          void *dst = MemoryRegionVirtualTzramMarikoProgramFatalErrorContext.GetPointer<void>();
-                    const void *src = mapper.GetPointerTo(FatalErrorPhysicalAddress, sizeof(ams::impl::FatalErrorContext));
-                    std::memcpy(dst, src, sizeof(ams::impl::FatalErrorContext));
-                }
-            }
-
-            /* Map Dram for the mariko program. */
-            MapDramForMarikoProgram();
-
-            AMS_SECMON_LOG("%s\n", "Jumping to Mariko Fatal.");
-            AMS_LOG_FLUSH();
-
-            /* Jump to the mariko fatal program. */
-            reinterpret_cast<void (*)()>(secmon::MemoryRegionVirtualTzramMarikoProgram.GetAddress())();
-
-            /* The mariko fatal program never returns. */
-            __builtin_unreachable();
         }
     }
 

exosphere/program/source/smc/secmon_smc_power_management.cpp

@@ -133,7 +133,7 @@ namespace ams::secmon::smc {
                                                                         REG_BITS_VALUE(which_core + 0x10, 1, 1)); /* CORERESETn */
         }
 
-        void PowerOffCpu() {
+        void PowerOffCpuImpl() {
             /* Get the current core id. */
             const auto core_id = hw::GetCurrentCoreId();
 
@@ -503,9 +503,7 @@ namespace ams::secmon::smc {
 
     }
 
-    SmcResult SmcPowerOffCpu(SmcArguments &args) {
+    void PowerOffCpu() {
-        AMS_UNUSED(args);
-
         /* Get the current core id. */
         const auto core_id = hw::GetCurrentCoreId();
 
@@ -514,15 +512,21 @@ namespace ams::secmon::smc {
 
         /* If we're on the final core, shut down directly. Otherwise, invoke with special stack. */
         if (core_id == NumCores - 1) {
-            PowerOffCpu();
+            PowerOffCpuImpl();
         } else {
-            PivotStackAndInvoke(GetCoreExceptionStackVirtual(), PowerOffCpu);
+            PivotStackAndInvoke(GetCoreExceptionStackVirtual(), PowerOffCpuImpl);
         }
 
         /* This code will never be reached. */
         __builtin_unreachable();
     }
 
+    SmcResult SmcPowerOffCpu(SmcArguments &args) {
+        AMS_UNUSED(args);
+
+        PowerOffCpu();
+    }
+
     SmcResult SmcPowerOnCpu(SmcArguments &args) {
         /* Get and validate the core to power on. */
         const int which_core = args.r[1];

exosphere/program/source/smc/secmon_smc_power_management.hpp

@@ -19,6 +19,8 @@
 
 namespace ams::secmon::smc {
 
+    NORETURN void PowerOffCpu();
+
     SmcResult SmcPowerOffCpu(SmcArguments &args);
     SmcResult SmcPowerOnCpu(SmcArguments &args);