hoc-clk: add (half finished) cpu dvfs

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

@@ -61,6 +61,7 @@ namespace board {
     HocClkConsoleType gConsoleType = HocClkConsoleType_Icosa;
     FuseData fuseData;
     u8 speedoBracket;
+    u8 cpuSpeedoBracket;
     PwmChannelSession iCon;
 
     u32 fd = 0, fd2 = 0;
@@ -104,17 +105,17 @@ namespace board {
                 } else if (state == PscPmState_ReadyShutdown) {
                     i2c::wdt::Disarm();
                     s_isAwake = false;
-
                 }
 
                 pscPmModuleAcknowledge(&s_pscModule, state);
             }
         }
     }
-    
+
     void FetchHardwareInfos() {
         ReadFuses(fuseData, fuseVirtAddr);
         SetGpuBracket(fuseData.gpuSpeedo, speedoBracket);
+        SetCpuBracket(fuseData.cpuSpeedo, cpuSpeedoBracket);
 
         u32 hidrev = *(u32 *)(apbVirtAddr + APB_MISC_GP_HIDREV);
         if (((hidrev >> 4) & 0xF) >= GP_HIDREV_MAJOR_T210B01) {
@@ -239,6 +240,7 @@ namespace board {
         display::Initialize(&cfg);
 
         CacheDfllData();
+        InitializeCpuLut();
         CacheGpuVoltTable();
 
         rc = pscmInitialize();
@@ -249,7 +251,7 @@ namespace board {
 
         rc = threadCreate(&s_pscThread, PscThreadFunc, nullptr, NULL, 0x1000, 0x10, 3);
         ASSERT_RESULT_OK(rc, "threadCreate");
-        
+
         rc = threadStart(&s_pscThread);
         ASSERT_RESULT_OK(rc, "threadStart");
     }
@@ -282,7 +284,7 @@ namespace board {
         batteryInfoExit();
         pmdmntExit();
         nvExit();
-        
+
         eventFire(&s_pscModule.event);
         threadWaitForExit(&s_pscThread);
         threadClose(&s_pscThread);
@@ -339,4 +341,8 @@ namespace board {
         return speedoBracket;
     }
 
+    u8 GetCpuSpeedoBracket() {
+        return cpuSpeedoBracket;
+    }
+
 }  // namespace board

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

@@ -48,6 +48,7 @@ namespace board {
     extern HocClkConsoleType gConsoleType;
     extern FuseData fuseData;
     extern u8 speedoBracket;
+    extern u8 cpuSpeedoBracket;
 
     void Initialize();
     void Exit();
@@ -56,6 +57,7 @@ namespace board {
     u8 GetFuseDramId();
     u8 GetDramID();
     u8 GetGpuSpeedoBracket();
+    u8 GetCpuSpeedoBracket();
     bool IsDram8GB();
     void SetDisplayRefreshDockedState(bool docked);
     FuseData *GetFuseData();

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

@@ -60,6 +60,36 @@ namespace board {
         }
     }
 
+    void SetCpuBracket(u16 speedo, u8 &cpuBracket) {
+        switch (speedo) {
+            // Mariko
+            case 1451 ... 1500:
+                cpuBracket = 6;
+                break;
+            case 1501 ... 1550:
+                cpuBracket = 5;
+                break;
+            case 1551 ... 1600:
+                cpuBracket = 4;
+                break;
+            case 1601 ... 1650:
+                cpuBracket = 3;
+                break;
+            case 1651 ... 1700:
+                cpuBracket = 2;
+                break;
+            case 1701 ... 1750:
+                cpuBracket = 1;
+                break;
+            case 1751 ... 1800:
+                cpuBracket = 0;
+                break;
+            default:
+                cpuBracket = 6;
+                break;
+        }
+    }
+
     void ReadFuses(FuseData &speedo, u64 fuseVa) {
         constexpr u32 FuseOffset = 0x800;
         u8 *fusePtr = reinterpret_cast<u8 *>(fuseVa) + FuseOffset;

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

@@ -36,5 +36,6 @@ namespace board {
 
     void ReadFuses(FuseData &speedo, u64 fuseVa);
     void SetGpuBracket(u16 gpuSpeedo, u8 &gpuBracket);
+    void SetCpuBracket(u16 cpuSpeedo, u8 &cpuBracket);
 
 }  // namespace board

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

@@ -290,7 +290,6 @@ namespace board {
         return true;
     }
 
-
     void CacheGpuVoltTable() {
         // Likely CPU regulator?
         UnkRegulator reg = {
@@ -610,4 +609,189 @@ namespace board {
             return baseVolt;
         }
     }
-}  // namespace board
+
+    namespace {
+        constexpr size_t LutSize = 33;
+        constexpr u32 PmicVmin   = 250;
+        constexpr u32 PmicStep   = 5;
+
+        struct {
+            volatile u32 *lut = nullptr;
+            u32 table[LutSize] = {};
+            bool initialized = false;
+        } cpuVoltData;
+    }
+
+    namespace {
+        inline void DataSyncBarrier() {
+            __asm__ volatile("dsb sy" ::: "memory");
+        }
+
+        void VolatileCopyDwords(volatile u32 *dst, const volatile u32 *src, size_t dwords) {
+            for (size_t i = 0; i < dwords; ++i) {
+                dst[i] = src[i];
+            }
+
+            DataSyncBarrier();
+        }
+
+        u32 GetLutCode(u32 mv) {
+            return mv <= PmicVmin ? 0 : (mv - PmicVmin) / PmicStep;
+        }
+
+        u32 GetLutVolt(u32 code) {
+            return code * PmicStep + PmicVmin;
+        }
+
+        void InitializeLutPtr() {
+            cpuVoltData.lut = reinterpret_cast<volatile u32 *>(cldvfs + CL_DVFS_LUT_TABLE_0);
+        }
+
+        void CacheCpuLut() {
+            VolatileCopyDwords(cpuVoltData.table, cpuVoltData.lut, LutSize);
+        }
+
+        /* todo debug edge cases of this. */
+        void UnstuckFreqWar() {
+            u32 hz = board::GetHz(HocClkModule_CPU);
+            board::SetHz(HocClkModule_CPU, ~0);
+            board::SetHz(HocClkModule_CPU, hz);
+        }
+
+        void RestoreCpuLut() {
+            VolatileCopyDwords(cpuVoltData.lut, cpuVoltData.table, LutSize);
+            UnstuckFreqWar();
+        }
+
+        u32 FloorLutTable(u32 *lut, u32 minCode) {
+            u32 floored = 0;
+            while (floored < LutSize && lut[floored] < minCode) {
+                lut[floored++] = minCode;
+            }
+            return floored;
+        }
+
+        u32 StripRedundantLutEntries(u32 *lut, u32 redundant) {
+            const u32 strippedCount = LutSize - redundant;
+            std::memmove(lut, lut + redundant, sizeof(u32) * strippedCount);
+
+            return strippedCount;
+        }
+
+        /* Naiive interpolation. */
+        u32 InterpolateLut(u32 low, u32 high) {
+            return low + (high - low) / 2;
+        }
+
+        /* Find biggest deltas, shift to create a new slot in between it, interpolate later. */
+        u32 SplitAndInterpolateDeltas(u32 *lut, u32 validCount) {
+            while (validCount < LutSize) {
+                u32 biggestDelta = 0;
+                u32 upperIdx = 0;
+
+                for (u32 i = 0; i + 1 < validCount; ++i) {
+                    const u32 delta = lut[i + 1] - lut[i];
+                    if (delta > biggestDelta) {
+                        biggestDelta = delta;
+                        upperIdx = i + 1;
+                    }
+                }
+
+                if (biggestDelta < 2) {
+                    break;
+                }
+
+                const u32 mid = InterpolateLut(lut[upperIdx - 1], lut[upperIdx]);
+                std::memmove(lut + upperIdx + 1, lut + upperIdx, (validCount - upperIdx) * sizeof(u32));
+                lut[upperIdx] = mid;
+                ++validCount;
+            }
+
+            return validCount;
+        }
+
+        void PadLutTail(u32 *lut, u32 validCount) {
+            for (u32 i = validCount; i < LutSize; ++i) {
+                lut[i] = lut[validCount - 1];
+            }
+        }
+    }
+
+    void InitializeCpuLut() {
+        InitializeLutPtr();
+        CacheCpuLut();
+
+        cpuVoltData.initialized = true;
+    }
+
+    /* TODO: Verify table on consoles */
+    u32 GetMinimumCpuVmin(u32 freqMhz, u32 bracket) {
+        struct CpuVminEntry {
+            u32 freq;
+            u32 volt;
+        };
+
+        static const CpuVminEntry cpuVminRamTable[/* Bracket */ 7][/* Entry */ 12] = {
+            { {1866, 550}, {1996, 560}, {2133, 570}, {2400, 580}, {2533, 590}, {2666, 600}, {2800, 610}, {2933, 620}, {3066, 630}, {3200, 640}, {3333, 660}, {3466, 680} },  // Bracket 0 (Speedo 1751-1800)
+            { {1866, 555}, {1996, 565}, {2133, 575}, {2400, 585}, {2533, 595}, {2666, 605}, {2800, 615}, {2933, 625}, {3066, 635}, {3200, 645}, {3333, 665}, {3466, 685} },  // Bracket 1 (Speedo 1701-1750)
+            { {1866, 560}, {1996, 570}, {2133, 580}, {2400, 590}, {2533, 600}, {2666, 610}, {2800, 620}, {2933, 630}, {3066, 640}, {3200, 650}, {3333, 670}, {3466, 690} },  // Bracket 2 (Speedo 1651-1700)
+            { {1866, 565}, {1996, 575}, {2133, 585}, {2400, 595}, {2533, 605}, {2666, 615}, {2800, 625}, {2933, 635}, {3066, 645}, {3200, 655}, {3333, 675}, {3466, 695} },  // Bracket 3 (Speedo 1601-1650)
+            { {1866, 570}, {1996, 580}, {2133, 590}, {2400, 600}, {2533, 610}, {2666, 620}, {2800, 630}, {2933, 640}, {3066, 650}, {3200, 660}, {3333, 680}, {3466, 700} },  // Bracket 4 (Speedo 1551-1600)
+            { {1866, 575}, {1996, 585}, {2133, 595}, {2400, 605}, {2533, 615}, {2666, 625}, {2800, 635}, {2933, 645}, {3066, 655}, {3200, 665}, {3333, 685}, {3466, 705} },  // Bracket 5 (Speedo 1501-1550)
+            { {1866, 580}, {1996, 590}, {2133, 600}, {2400, 610}, {2533, 620}, {2666, 630}, {2800, 640}, {2933, 650}, {3066, 660}, {3200, 670}, {3333, 690}, {3466, 710} },  // Bracket 6 (Speedo 1451-1500)
+        };
+
+        if (freqMhz <= 1600) {
+            return 0;  // DVFS doesnt work below 1600MHz, it will just use vMin
+        }
+
+        if (bracket >= std::size(cpuVminRamTable)) {
+            bracket = 0;
+        }
+
+        const auto &entries = cpuVminRamTable[bracket];
+        u32 baseVolt = entries[std::size(entries) - 1].volt;
+        for (const auto &entry : entries) {
+            if (freqMhz <= entry.freq) {
+                baseVolt = entry.volt;
+                break;
+            }
+        }
+
+        return baseVolt;
+    }
+
+    void ApplyCpuMinVolt(u32 vmin) {
+        if (!cpuVoltData.initialized) {
+            return;
+        }
+
+        if (vmin == 0) {
+            RestoreCpuLut();
+            return;
+        }
+
+        u32 tmpLut[LutSize];
+        std::memcpy(tmpLut, cpuVoltData.table, sizeof(tmpLut));
+
+        const u32 minCode = std::min(GetLutCode(vmin), tmpLut[LutSize - 1]);
+
+        const u32 floored = FloorLutTable(tmpLut, minCode);
+        if (floored == 0) {
+            RestoreCpuLut();
+            return;
+        }
+
+        u32 redundant = floored - 1;
+        if (floored < LutSize && tmpLut[floored] == minCode) {
+            redundant = floored;
+        }
+
+        u32 valid = StripRedundantLutEntries(tmpLut, redundant);
+        valid     = SplitAndInterpolateDeltas(tmpLut, valid);
+        PadLutTail(tmpLut, valid);
+
+        VolatileCopyDwords(cpuVoltData.lut, tmpLut, LutSize);
+    }
+
+}

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

@@ -60,4 +60,8 @@ namespace board {
     void PcvHijackGpuFrequency(u32 voltage, u32 hz);
     u32 GetMinimumGpuVmin(u32 freqMhz, u32 bracket);
 
+    void InitializeCpuLut();
+    void ApplyCpuMinVolt(u32 vmin);
+    u32 GetMinimumCpuVmin(u32 freqMhz, u32 bracket);
+
 }  // namespace board

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

@@ -426,32 +426,54 @@ namespace mgr {
         board::SetHz(HocClkModule_GPU, currentFreq);
     }
 
-    void ApplyGpuDvfs(u32 targetHz) {
-        s32 dvfsOffset = file::config::GetConfigValue(HocClkConfigValue_DVFSOffset);
-        dvfsOffset = std::max(dvfsOffset, -80);
-        u32 vmin = board::GetMinimumGpuVmin(targetHz / 1000000, board::GetGpuSpeedoBracket());
+    bool IsGpuDvfsEnabled() {
+        return file::config::GetConfigValue(HocClkConfigValue_GPUDVFSMode) == DVFSMode_Hijack;
+    }
 
-        if (vmin) {
-            vmin += dvfsOffset;
+    bool IsCpuDvfsEnabled() {
+        return board::GetSocType() == HocClkSocType_Mariko && file::config::GetConfigValue(HocClkConfigValue_CPUDVFSMode) == CPUDVFSMode_Modify;
+    }
+
+    void ApplyRamOcDvfs(u32 targetHz) {
+        if (IsGpuDvfsEnabled()) {
+            s32 gpuDvfsOffset = file::config::GetConfigValue(HocClkConfigValue_GPUDVFSOffset);
+            gpuDvfsOffset = std::max(gpuDvfsOffset, -80);
+            u32 gpuVmin = board::GetMinimumGpuVmin(targetHz / 1000000, board::GetGpuSpeedoBracket());
+
+            if (gpuVmin) {
+                gpuVmin += gpuDvfsOffset;
+            }
+
+            /* Prevent console from combusting if for some reason bad shit happens :P */
+            gpuVmin = ClampGpuVoltage(gpuVmin);
+
+            /* Hijack gpu volt table. */
+            board::PcvHijackGpuVolts(gpuVmin);
+
+            /* Update gpu frequency to actually use the voltage. */
+            if (targetHz) {
+                board::SetHz(HocClkModule_GPU, GetCurrentNearestFrequency(HocClkModule_GPU));
+            } else {
+                /* If the target frequency is zero, we reset the frequency to ensure it gets updated even without any frequency override. */
+                board::ResetToStockGpu();
+            }
         }
 
-        /* Prevent console from combusting if for some reason bad shit happens :P */
-        vmin = ClampGpuVoltage(vmin);
+        if (IsCpuDvfsEnabled()) {
+            s32 cpuDvfsOffset = file::config::GetConfigValue(HocClkConfigValue_CPUDVFSOffset);
+            cpuDvfsOffset = std::max(cpuDvfsOffset, -80);
+            u32 cpuVmin = board::GetMinimumCpuVmin(targetHz / 1000000, board::GetCpuSpeedoBracket());
 
-        /* Hijack gpu volt table. */
-        board::PcvHijackGpuVolts(vmin);
+            if (cpuVmin) {
+                cpuVmin += cpuDvfsOffset;
+            }
 
-        /* Update gpu frequency to actually use the voltage. */
-        if (targetHz) {
-            board::SetHz(HocClkModule_GPU, GetCurrentNearestFrequency(HocClkModule_GPU));
-        } else {
-            /* If the target frequency is zero, we reset the frequency to ensure it gets updated even without any frequency override. */
-            board::ResetToStockGpu();
+            board::ApplyCpuMinVolt(cpuVmin);
         }
     }
 
     void DVFSReset() {
-        if (file::config::GetConfigValue(HocClkConfigValue_DVFSMode) == DVFSMode_Hijack) {
+        if (IsGpuDvfsEnabled()) {
             board::PcvHijackGpuVolts(0);  // Reset to vMin
 
             u32 targetHz = GetNearestOverrideHz(HocClkModule_GPU);
@@ -460,6 +482,10 @@ namespace mgr {
             if (targetHz)
                 board::SetHz(HocClkModule_GPU, targetHz);
         }
+
+        if (IsCpuDvfsEnabled()) {
+            board::ApplyCpuMinVolt(0);
+        }
     }
 
     void HandleFreqReset(HocClkModule module, bool isBoost, bool didHijackPcv) {
@@ -506,9 +532,7 @@ namespace mgr {
         u32 nearestFreq = GetCurrentNearestFrequency(HocClkModule_MEM);
 
         if (targetRamHz != nearestFreq) {
-            if (file::config::GetConfigValue(HocClkConfigValue_DVFSMode) == DVFSMode_Hijack) {
-                ApplyGpuDvfs(targetRamHz);
-            }
+            ApplyRamOcDvfs(targetRamHz);
 
             board::SetHz(HocClkModule_MEM, targetRamHz);
         }
@@ -605,9 +629,10 @@ namespace mgr {
                                        targetHz / 100000 - targetHz / 1000000 * 10);
 
                     // The logic MUST be done in this order otherwise you WILL get crashes
-                    if (module == HocClkModule_MEM && targetHz > oldHz && file::config::GetConfigValue(HocClkConfigValue_DVFSMode) == DVFSMode_Hijack) {
-                        ApplyGpuDvfs(targetHz);
+                    if (module == HocClkModule_MEM && targetHz > oldHz) {
+                        ApplyRamOcDvfs(targetHz);
                     }
+
                     board::SetHz((HocClkModule)module, nearestHz);
                     gContext.freqs[module] = nearestHz;
 
@@ -615,12 +640,13 @@ namespace mgr {
                         gContext.stable.freqs[module] = nearestHz;
                     }
 
-                    if (module == HocClkModule_MEM && targetHz < oldHz && file::config::GetConfigValue(HocClkConfigValue_DVFSMode) == DVFSMode_Hijack) {
-                        ApplyGpuDvfs(targetHz);
+                    if (module == HocClkModule_MEM && targetHz < oldHz) {
+                        ApplyRamOcDvfs(targetHz);
                     }
 
-                    if (module == HocClkModule_MEM && file::config::GetConfigValue(HocClkConfigValue_DVFSMode) == DVFSMode_Hijack)
+                    if (module == HocClkModule_MEM && file::config::GetConfigValue(HocClkConfigValue_GPUDVFSMode) == DVFSMode_Hijack)
                         didHijackPcv = false;
+
                 }
             } else {
                 HandleFreqReset((HocClkModule)module, isBoost, didHijackPcv);
@@ -652,10 +678,14 @@ namespace mgr {
         // restore clocks to stock values on app or profile change
         if (hasChanged) {
             board::ResetToStock();
-            if (file::config::GetConfigValue(HocClkConfigValue_DVFSMode) == DVFSMode_Hijack) {
+            if (IsGpuDvfsEnabled()) {
                 board::PcvHijackGpuVolts(0);
                 board::ResetToStockGpu();
             }
+
+            if (IsCpuDvfsEnabled()) {
+                board::ApplyCpuMinVolt(0);
+            }
             WaitForNextTick();
         }
 
@@ -780,7 +810,7 @@ namespace mgr {
             gContext.resolutionHeight = 0;  // N/A
 
         gContext.isWdtEnabled = i2c::wdt::IsWdtEnabled();
-        
+
         return hasChanged;
     }
 

Source/hoc-clk/sysmodule/src/soc/registers.h

@@ -528,6 +528,8 @@
 
 #define CLK_SOURCE_EMC 0x19c
 
+#define CL_DVFS_LUT_TABLE_0 0x200
+
 #define PLLC_BASE 0x080
 #define PLLM_BASE 0x090
 #define PLLP_BASE 0x0a0