dvfs: fix bugs and edge cases with cpu dvfs

Source/hoc-clk/sysmodule/src/board/board.cpp

@@ -51,7 +51,8 @@
 #include "../mapping/mem_map.hpp"
 #include "../soc/dram_mrr.hpp"
 #include "../i2c/max77620_wdt.hpp"
-
+#include "../mgr/clock_manager.hpp"
+#include "../file/config.hpp"
 namespace board {
 
     u64 clkVirtAddr, dsiVirtAddr, apbVirtAddr, fuseVirtAddr;
@@ -95,15 +96,45 @@ namespace board {
             u32 flags;
             rc = pscPmModuleGetRequest(&s_pscModule, &state, &flags);
             if (R_SUCCEEDED(rc)) {
-                if (state == PscPmState_Awake) {
+                if (state == PscPmState_ReadyAwaken) {
                     i2c::wdt::ResetWdtEnableState();
                     i2c::wdt::Pet();
+
+                    if(board::GetSocType() == HocClkSocType_Mariko && 
+                       file::config::GetConfigValue(HocClkConfigValue_CPUDVFSMode) == CPUDVFSMode_Modify) {
+                        s32 cpuDvfsOffset = file::config::GetConfigValue(HocClkConfigValue_CPUDVFSOffset);
+                        cpuDvfsOffset = std::max(cpuDvfsOffset, -80);
+                        u32 cpuVmin = GetMinimumCpuVmin(mgr::GetNearestOverrideHz(HocClkModule_MEM) / 1000000, GetCpuSpeedoBracket());
+
+                        if (cpuVmin) {
+                            cpuVmin += cpuDvfsOffset;
+                        }
+                        
+                        ApplyCpuMinVolt(cpuVmin, true);
+                    }
+
+                    file::utils::LogLine("[brd]: Ready to awaken");
+
                     s_isAwake = true;
                 } else if (state == PscPmState_ReadySleep) {
                     i2c::wdt::Disarm();
+
+                    ResetToStockMem();
+                    ResetToStockCpu();
+                    ApplyCpuMinVolt(0, true);
+
+                    file::utils::LogLine("[brd]: Ready to sleep");
+
                     s_isAwake = false;
                 } else if (state == PscPmState_ReadyShutdown) {
                     i2c::wdt::Disarm();
+
+                    ResetToStockMem();
+                    ResetToStockCpu();
+                    ApplyCpuMinVolt(0, true);
+
+                    file::utils::LogLine("[brd]: Ready to shutdown");
+
                     s_isAwake = false;
                 }
 
@@ -254,6 +285,8 @@ namespace board {
 
         rc = threadStart(&s_pscThread);
         ASSERT_RESULT_OK(rc, "threadStart");
+
+        s_isAwake = true;
     }
 
     void Exit() {
@@ -345,4 +378,8 @@ namespace board {
         return cpuSpeedoBracket;
     }
 
+    bool IsAwake() {
+        return s_isAwake;
+    }
+
 }  // namespace board

Source/hoc-clk/sysmodule/src/board/board.hpp

@@ -61,5 +61,5 @@ namespace board {
     bool IsDram8GB();
     void SetDisplayRefreshDockedState(bool docked);
     FuseData *GetFuseData();
-    
+    bool IsAwake();
 }  // namespace board

Source/hoc-clk/sysmodule/src/board/board_volt.cpp

@@ -640,7 +640,7 @@ namespace board {
             return mv <= PmicVmin ? 0 : (mv - PmicVmin) / PmicStep;
         }
 
-        u32 GetLutVolt(u32 code) {
+        [[maybe_unused]] u32 GetLutVolt(u32 code) {
             return code * PmicStep + PmicVmin;
         }
 
@@ -652,14 +652,17 @@ namespace board {
             VolatileCopyDwords(cpuVoltData.table, cpuVoltData.lut, LutSize);
         }
 
-        /* todo debug edge cases of this. */
+        /* TODO: Dynamically get this from the PCV LUT */
         void UnstuckFreqWar() {
+            constexpr u32 KNOWN_GOOD_WAR_FREQUENCY = 1'785'000'000;
             u32 hz = board::GetHz(HocClkModule_CPU);
-            board::SetHz(HocClkModule_CPU, ~0);
+            board::SetHz(HocClkModule_CPU, KNOWN_GOOD_WAR_FREQUENCY);
+            svcSleepThread(5'000'000);
             board::SetHz(HocClkModule_CPU, hz);
         }
 
         void RestoreCpuLut() {
+            cpuVoltData.lastVmin = 0;
             VolatileCopyDwords(cpuVoltData.lut, cpuVoltData.table, LutSize);
             UnstuckFreqWar();
         }
@@ -762,16 +765,22 @@ namespace board {
         return baseVolt;
     }
 
-    void ApplyCpuMinVolt(u32 vmin) {
-        if (!cpuVoltData.initialized || vmin == cpuVoltData.lastVmin) {
-            return;
+    void ApplyCpuMinVolt(u32 vmin, bool force) {
+        if(!force) {
+            if (!cpuVoltData.initialized || (vmin == cpuVoltData.lastVmin)) {
+                return;
+            }
         }
 
+        file::utils::LogLine("[dvfs]: %setting cpu vmin to %dmV", force ? "Forcefully s" : "S", vmin);
+
         if (vmin == 0) {
             RestoreCpuLut();
+            file::utils::LogLine("[dvfs]: Restored LUT");
             return;
         }
 
+
         u32 tmpLut[LutSize];
         std::memcpy(tmpLut, cpuVoltData.table, sizeof(tmpLut));
 
@@ -780,6 +789,7 @@ namespace board {
         const u32 floored = FloorLutTable(tmpLut, minCode);
         if (floored == 0) {
             RestoreCpuLut();
+            file::utils::LogLine("[dvfs]: Floored, restored LUT");
             return;
         }
 
@@ -794,9 +804,7 @@ namespace board {
 
         VolatileCopyDwords(cpuVoltData.lut, tmpLut, LutSize);
 
-        if (vmin < cpuVoltData.lastVmin) {
-            UnstuckFreqWar();
-        }
+        UnstuckFreqWar();
 
         cpuVoltData.lastVmin = vmin;
     }

Source/hoc-clk/sysmodule/src/board/board_volt.hpp

@@ -61,7 +61,7 @@ namespace board {
     u32 GetMinimumGpuVmin(u32 freqMhz, u32 bracket);
 
     void InitializeCpuLut();
-    void ApplyCpuMinVolt(u32 vmin);
+    void ApplyCpuMinVolt(u32 vmin, bool force);
     u32 GetMinimumCpuVmin(u32 freqMhz, u32 bracket);
 
 }  // namespace board

Source/hoc-clk/sysmodule/src/mgr/clock_manager.cpp

@@ -468,7 +468,7 @@ namespace mgr {
                 cpuVmin += cpuDvfsOffset;
             }
 
-            board::ApplyCpuMinVolt(cpuVmin);
+            board::ApplyCpuMinVolt(cpuVmin, false);
         }
     }
 
@@ -484,7 +484,7 @@ namespace mgr {
         }
 
         if (IsCpuDvfsEnabled()) {
-            board::ApplyCpuMinVolt(0);
+            board::ApplyCpuMinVolt(0, false);
         }
     }
 
@@ -684,7 +684,7 @@ namespace mgr {
             }
 
             if (IsCpuDvfsEnabled()) {
-                board::ApplyCpuMinVolt(0);
+                board::ApplyCpuMinVolt(0, false);
             }
             WaitForNextTick();
         }

Source/hoc-clk/sysmodule/src/mgr/clock_manager.hpp

@@ -66,6 +66,8 @@ namespace mgr {
 
     void ApplyGpuFreqVoltRequest(u32 voltage, u32 hz);
 
+    u32 GetNearestOverrideHz(HocClkModule module);
+
     void Tick();
     void WaitForNextTick();
 }  // namespace mgr