From 4ea6ce31566638ea6d66b7b0a571078e65b00f2e Mon Sep 17 00:00:00 2001
From: Michael Scire <SciresM@gmail.com>
Date: Thu, 2 May 2019 18:10:07 -0700
Subject: [PATCH] boot: implement CheckClock

---
 stratosphere/boot/source/boot_check_clock.cpp | 48 +++++++++++++++++++
 stratosphere/boot/source/boot_functions.hpp   | 11 +++--
 stratosphere/boot/source/boot_main.cpp        |  3 +-
 .../boot/source/boot_reboot_manager.cpp       |  7 +++
 4 files changed, 65 insertions(+), 4 deletions(-)
 create mode 100644 stratosphere/boot/source/boot_check_clock.cpp

diff --git a/stratosphere/boot/source/boot_check_clock.cpp b/stratosphere/boot/source/boot_check_clock.cpp
new file mode 100644
index 000000000..e8561ca94
--- /dev/null
+++ b/stratosphere/boot/source/boot_check_clock.cpp
@@ -0,0 +1,48 @@
+/*
+ * Copyright (c) 2018-2019 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 "boot_functions.hpp"
+
+static constexpr u32 ExpectedPlluDivP = (1 << 16);
+static constexpr u32 ExpectedPlluDivN = (25 << 8);
+static constexpr u32 ExpectedPlluDivM = (2  << 0);
+static constexpr u32 ExpectedPlluVal  = (ExpectedPlluDivP | ExpectedPlluDivN | ExpectedPlluDivM);
+static constexpr u32 ExpectedPlluMask = 0x1FFFFF;
+
+static constexpr u32 ExpectedUtmipDivN = (25 << 16);
+static constexpr u32 ExpectedUtmipDivM = (1 << 8);
+static constexpr u32 ExpectedUtmipVal  = (ExpectedUtmipDivN | ExpectedUtmipDivM);
+static constexpr u32 ExpectedUtmipMask = 0xFFFF00;
+
+static bool IsUsbClockValid() {
+    u64 _vaddr;
+    if (R_FAILED(svcQueryIoMapping(&_vaddr, 0x60006000ul, 0x1000))) {
+        std::abort();
+    }
+    volatile u32 *car_regs = reinterpret_cast<volatile u32 *>(_vaddr);
+
+    const u32 pllu = car_regs[0xC0 >> 2];
+    const u32 utmip = car_regs[0x480 >> 2];
+    return ((pllu & ExpectedPlluMask) == ExpectedPlluVal) && ((utmip & ExpectedUtmipMask) == ExpectedUtmipVal);
+}
+
+void Boot::CheckClock() {
+    if (!IsUsbClockValid()) {
+        /* Sleep for 1s, then reboot. */
+        svcSleepThread(1'000'000'000ul);
+        Boot::RebootSystem();
+    }
+}
diff --git a/stratosphere/boot/source/boot_functions.hpp b/stratosphere/boot/source/boot_functions.hpp
index 03ba21e52..4430cc08a 100644
--- a/stratosphere/boot/source/boot_functions.hpp
+++ b/stratosphere/boot/source/boot_functions.hpp
@@ -13,7 +13,7 @@
  * 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 <switch.h>
 #include <stratosphere.hpp>
@@ -28,16 +28,21 @@ class Boot {
         /* Functions for actually booting. */
         static void ChangeGpioVoltageTo1_8v();
         static void SetInitialGpioConfiguration();
+        static void CheckClock();
+
+        /* Power utilities. */
+        static void RebootSystem();
+        static void ShutdownSystem();
 
         /* Register Utilities. */
         static u32 ReadPmcRegister(u32 phys_addr);
         static void WritePmcRegister(u32 phys_addr, u32 value, u32 mask = UINT32_MAX);
-        
+
         /* GPIO Utilities. */
         static u32 GpioConfigure(u32 gpio_pad_name);
         static u32 GpioSetDirection(u32 gpio_pad_name, GpioDirection dir);
         static u32 GpioSetValue(u32 gpio_pad_name, GpioValue val);
-        
+
         /* SPL Utilities. */
         static HardwareType GetHardwareType();
 
diff --git a/stratosphere/boot/source/boot_main.cpp b/stratosphere/boot/source/boot_main.cpp
index e714a1c3b..114c5e939 100644
--- a/stratosphere/boot/source/boot_main.cpp
+++ b/stratosphere/boot/source/boot_main.cpp
@@ -116,7 +116,8 @@ int main(int argc, char **argv)
     /* Setup GPIO. */
     Boot::SetInitialGpioConfiguration();
     
-    /* TODO: CheckClock(); */
+    /* Check USB PLL/UTMIP clock. */
+    Boot::CheckClock();
     
     /* TODO: DetectBootReason(); */
     
diff --git a/stratosphere/boot/source/boot_reboot_manager.cpp b/stratosphere/boot/source/boot_reboot_manager.cpp
index e6311c3c9..7c9a414bc 100644
--- a/stratosphere/boot/source/boot_reboot_manager.cpp
+++ b/stratosphere/boot/source/boot_reboot_manager.cpp
@@ -17,6 +17,7 @@
 #include <switch.h>
 #include <stratosphere.hpp>
 #include <strings.h>
+#include "boot_functions.hpp"
 #include "boot_reboot_manager.hpp"
 #include "fusee-primary_bin.h"
 
@@ -65,4 +66,10 @@ void BootRebootManager::RebootForFatalError(AtmosphereFatalErrorContext *ctx) {
     CopyToIram(IRAM_PAYLOAD_BASE + IRAM_PAYLOAD_MAX_SIZE, g_work_page, sizeof(g_work_page));
 
     RebootToIramPayload();
+}
+
+void Boot::RebootSystem() {
+    if (R_FAILED(BootRebootManager::PerformReboot())) {
+        std::abort();
+    }
 }
\ No newline at end of file