hoc-clk: add (half finished) cpu dvfs

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);
+    }
+
+}