ldr: build 64 lut table ourselves instead of interpolating

Source/Atmosphere/stratosphere/loader/source/oc/oc_common.hpp

@@ -96,6 +96,8 @@ namespace ams::ldr {
     R_DEFINE_ERROR_RESULT(InvalidCpuLutMaxAsmPattern,   1033);
     R_DEFINE_ERROR_RESULT(InvalidCpuLutHook,            1034);
     R_DEFINE_ERROR_RESULT(InvalidDvcoRateConfig,        1035);
+    R_DEFINE_ERROR_RESULT(InvalidVoltCode,              1036);
+    R_DEFINE_ERROR_RESULT(InvalidVoltLutIndex,          1037);
 }
 
 namespace ams::ldr::hoc {

Source/Atmosphere/stratosphere/loader/source/oc/pcv/mariko/pcv_mariko.hpp

@@ -37,6 +37,7 @@ namespace ams::ldr::hoc::pcv::mariko {
         } busData;
         struct {
             uintptr_t orig; /* ClDvfsLut64ExpandImpl to CLDVFS LUT writer trampoline. */
+            u32 table[ClDvfsLutSize];
         } lut64;
 #if HOC_UART_LOG
         u32 verbosityLevel;

Source/Atmosphere/stratosphere/loader/source/oc/pcv/mariko/pcv_mariko_cpu.cpp

@@ -25,17 +25,183 @@
 
 namespace ams::ldr::hoc::pcv::mariko {
 
-    u32 CapCpuClock() {
-        u32 cpuCap = allowedCpuMaxFrequencies[0];
+    namespace {
+        u32 CapCpuClock() {
+            u32 cpuCap = allowedCpuMaxFrequencies[0];
 
-        for (u32 freq : allowedCpuMaxFrequencies) {
-            if (C.marikoCpuMaxClock >= freq) {
-                cpuCap = freq;
-            } else {
-                break;
+            for (u32 freq : allowedCpuMaxFrequencies) {
+                if (C.marikoCpuMaxClock >= freq) {
+                    cpuCap = freq;
+                } else {
+                    break;
+                }
+            }
+            return cpuCap;
+        }
+
+        /* TODO: Reduce code duplication. */
+        cvb_entry_t *GetDvfsTable() {
+            switch (C.tableConf) {
+                case TBREAK_1683: return const_cast<cvb_entry_t *>(C.marikoCpuDvfsTable1683Tbreak);
+                case TBREAK_1581: return const_cast<cvb_entry_t *>(C.marikoCpuDvfsTable1581Tbreak);
+                case EXTREME_TABLE: return const_cast<cvb_entry_t *>(C.marikoCpuDvfsTableExtreme);
+                case DEFAULT_TABLE:
+                default:
+                    return const_cast<cvb_entry_t *>(mariko::CpuCvbTableDefault);
             }
         }
-        return cpuCap;
+
+        /* Todo: Reduce code duplication of this. */
+        Result GetCpuSpeedo(u32 &cpuSpeedo) {
+            constexpr u64 FusePhysicalAddress = 0x7000F000;
+            u64 virtualAddress                = 0;
+            constexpr u64 Size                = 0x1000;
+
+            u64 outSize;
+            /* TODO: use svc::QueryMemoryMapping instead. */
+            R_TRY(svcQueryMemoryMapping(&virtualAddress, &outSize, FusePhysicalAddress, Size));
+
+            constexpr u32 FuseOffset      = 2048;
+            constexpr u32 CpuSpeedoOffset = 0x12C;
+            cpuSpeedo                     = *reinterpret_cast<u32 *>(virtualAddress + FuseOffset + CpuSpeedoOffset);
+
+            R_SUCCEED();
+        }
+
+        u32 CalculateCvbVoltage(cvb_entry_t *entry, u32 speedo, u32 speedoScale) {
+            float speedDiv = static_cast<float>(speedo) / speedoScale;
+            return ((entry->cvb_dfll_param.c2 * speedDiv + entry->cvb_dfll_param.c1) * speedDiv + entry->cvb_dfll_param.c0);
+        }
+
+        u32 DivRoundUp(u32 n, u32 d) {
+            return (n + d - 1) / d;
+        }
+
+        u32 RoundCvbVoltage(u32 mv, u32 vScale, u32 stepUv) {
+            u32 step = (stepUv ? stepUv : 1000) * vScale;
+
+            u32 uv = mv * 1000;
+            uv     = DivRoundUp(uv, step) * stepUv;
+            return uv / 1000;
+        }
+
+        u32 EffVmin() { return C.marikoCpuLowVmin ? C.marikoCpuLowVmin : CpuVminOfficial; }
+        u32 EffVmax() { return C.marikoCpuMaxVolt ? C.marikoCpuMaxVolt : CpuVoltOfficial; }
+
+        u32 ClampVoltage(u32 mv) {
+            return std::clamp(mv, EffVmin(), EffVmax());
+        }
+
+        constexpr u32 PmicBaseMv    = 250;
+        constexpr u32 PmicStepMv    = 5;
+        constexpr u32 PmicVoltCount = 256;
+
+        u32 GetExactVoltCode(u32 mv) {
+            u32 alignMult = mv / PmicStepMv;
+
+            for (u32 i = 0; i < PmicVoltCount; ++i) {
+                u32 regMult = (PmicBaseMv + i * PmicStepMv) / PmicStepMv;
+                if (alignMult == regMult) {
+                    return i;
+                }
+            }
+
+            return UINT32_MAX;
+        }
+
+        u32 GetVoltCode(u32 mv) {
+            u32 alignMult = DivRoundUp(mv, PmicStepMv);
+
+            for (u32 i = 0; i < PmicVoltCount; i++) {
+                u32 regMult = (PmicBaseMv + i * PmicStepMv) / PmicStepMv;
+                if (alignMult <= regMult) {
+                    return i;
+                }
+            }
+
+            return UINT32_MAX;
+        }
+
+        Result BuildLut(u32 *lut, const u32 *voltArray) {
+            const u32 vMin = EffVmin();
+            const u32 vMax = *std::max_element(voltArray, voltArray + ClDvfsLutSize);
+            R_UNLESS(vMin <= vMax, ldr::ResultInvalidCpuMinVolt());
+
+            u32 voltage = vMin;
+            u32 code    = GetExactVoltCode(voltage);
+            R_UNLESS(code != UINT32_MAX, ldr::ResultInvalidVoltCode());
+            lut[0] = code;
+
+            u32 j = 1;
+            for (u32 i = 0; i < ClDvfsLutSize; ++i) {
+                for (;;) {
+                    R_UNLESS(j < ClDvfsLutSize, ldr::ResultInvalidVoltLutIndex());
+
+                    voltage += std::max<u32>(1u, (vMax - voltage) / (ClDvfsLutSize - j));
+                    if (voltage >= voltArray[i]) {
+                        break;
+                    }
+
+                    code = GetVoltCode(voltage);
+                    R_UNLESS(code != UINT32_MAX, ldr::ResultInvalidVoltCode());
+                    if (code != lut[j - 1]) {
+                        lut[j++] = code;
+                    }
+                }
+
+                voltage = (j == ClDvfsLutSize - 1) ? vMax : voltArray[i];
+                code    = GetExactVoltCode(voltage);
+                R_UNLESS(code != UINT32_MAX, ldr::ResultInvalidVoltCode());
+
+                if (code != lut[j - 1]) {
+                    R_UNLESS(j < ClDvfsLutSize, ldr::ResultInvalidVoltLutIndex());
+                    lut[j++] = code;
+                }
+
+                if (voltage >= vMax) {
+                    break;
+                }
+            }
+
+            for (u32 k = j; k < ClDvfsLutSize; ++k) {
+                lut[k] = lut[j - 1];
+            }
+
+            R_SUCCEED();
+        }
+
+        /* Since doing the proper lut creation in pcv is wasteful, we'll do it ourselves */
+        Result FillLutTable(HookPayloadData *data) {
+            /* Step 1: Get the speedo. */
+            u32 cpuSpeedo = 0;
+            R_TRY(GetCpuSpeedo(cpuSpeedo));
+
+            /* Step 2: Select the appropriate table. */
+            cvb_entry_t *table = GetDvfsTable();
+            constexpr u32 SpeedoScale  = 100;
+            constexpr u32 VoltageScale = 1000;
+            constexpr u32 StepUv       = 5000;
+
+            /* Step 3: Fill the voltage array. */
+            u32 voltArray[ClDvfsLutSize]  = {};
+            for (u32 i = 0; i < DvfsTableEntryLimit; ++i) {
+                u32 voltage = CalculateCvbVoltage(&table[i], cpuSpeedo, SpeedoScale);
+                voltage = RoundCvbVoltage(voltage, VoltageScale, StepUv);
+                voltage = ClampVoltage(voltage);
+
+                voltArray[i] = voltage;
+            }
+
+            /* Step 4: Build the lut table. */
+            u32 lut[ClDvfsLutSize] = {};
+            R_TRY(BuildLut(lut, voltArray));
+
+            /* Step 5: Copy it. */
+            std::memcpy(data->lut64.table, lut, sizeof(lut));
+
+            /* Step 6: Finally done. */
+            R_SUCCEED();
+        }
     }
 
     Result CpuFreqVdd(u32 *ptr) {
@@ -291,19 +457,11 @@ namespace ams::ldr::hoc::pcv::mariko {
 
         /* *(param+8) = CLDVFS mapping */
         const u64 base = *reinterpret_cast<u64 *>(param + 8);
-        u32 *lut = reinterpret_cast<u32 *>(base + 0x200);
+        u32 *lut       = reinterpret_cast<u32 *>(base + 0x200);
 
-        /* Expand the LUT in place */
-        const u32 top = lut[32];
-
-        for (int i = 31; i >= 0; --i) {
-            const u32 a = lut[i];
-            const u32 b = lut[i + 1]; /* dst 2*(i+1) > src i+1 */
-            lut[2 * i]     = a;
-            lut[2 * i + 1] = (a + b) >> 1;
+        for (u32 i = 0; i < ClDvfsLutSize; ++i) {
+            lut[i] = data->lut64.table[i];
         }
-
-        lut[63] = top;
         __asm__ volatile("dsb st" ::: "memory");
     }
 
@@ -333,6 +491,8 @@ namespace ams::ldr::hoc::pcv::mariko {
     Result LutWriterInstallHooks(HookPayloadData *data) {
         R_UNLESS(lutWriterCache.site != nullptr, ldr::ResultInvalidCpuLutHook());
 
+        R_TRY(FillLutTable(data));
+
         uintptr_t orig = 0;
         R_TRY(INSTALL_IMPL_HOOK_ORIG(lutWriterCache.site, CpuLutWriterExpandImpl, &orig));
         data->lut64.orig = orig;

Source/Atmosphere/stratosphere/loader/source/oc/pcv/mariko/pcv_mariko_cpu.hpp

@@ -78,7 +78,7 @@ namespace ams::ldr::hoc::pcv::mariko {
     static_assert(sizeof(cpuVoltagePatchValues) == sizeof(cpuVoltagePatchOffsets), "Invalid cpuVoltagePatch size");
 
     static const u32 cpuVoltThermalData[] = { 620, 1120, 20000, 620, 1120, 70000, 950, 1132, 0, 950, 1227, 0 };
-    
+
     /* Refer to customize.cpp for more information */
     static const u32 allowedCpuMaxFrequencies[] = { 1'963'500, 2'091'000, 2'193'000, 2'295'000, 2'397'000, 2'499'000, 2'601'000, 2'703'000, 2'805'000 };
 
@@ -89,7 +89,7 @@ namespace ams::ldr::hoc::pcv::mariko {
     Result CpuLutMaxAsm(u32* ptr);
     Result CpuLutMaxAsm2(u32* ptr);
     Result CpuLutDvcoRateCfg(u32* ptr);
-    
+
     ALWAYS_INLINE bool CpuLutAsmPatternFn(u32* ptr) {
         /* Check if it is a DSB ST, a common indication of CL-DVFS related functions */
         return *ptr == _asm::Dsb(_asm::barrier::St);
@@ -107,4 +107,5 @@ namespace ams::ldr::hoc::pcv::mariko {
         return _asm::IsOp(*ptr, _asm::op::LdrImm64, _asm::field::Rt, _asm::field::Rn, _asm::field::Off8)
         && _asm::Get(*ptr, _asm::field::Off8) == 0x248;
     }
+
 }

Source/Atmosphere/stratosphere/loader/source/oc/pcv/mariko/pcv_mariko_soc.cpp

@@ -35,7 +35,7 @@ namespace ams::ldr::hoc::pcv::mariko {
             R_TRY(svcQueryMemoryMapping(&virtualAddress, &outSize, FusePhysicalAddress, Size));
 
             constexpr u32 FuseOffset      = 2048;
-            constexpr u32 SocSpeedoOffset = 308;
+            constexpr u32 SocSpeedoOffset = 0x134;
             socSpeedo                     = *reinterpret_cast<u32 *>(virtualAddress + FuseOffset + SocSpeedoOffset);
 
             R_SUCCEED();

Source/Atmosphere/stratosphere/loader/source/oc/pcv/pcv_common.hpp

@@ -194,6 +194,7 @@ namespace ams::ldr::hoc::pcv {
     #define R_SKIP() R_SUCCEED()
 
     // Count 32 / Index 31 is reserved to be empty
+    /* The actual lut table has a size of 33. */
     constexpr size_t DvfsTableEntryCount = 32;
     constexpr size_t DvfsTableEntryLimit = DvfsTableEntryCount - 1;
 
@@ -204,6 +205,9 @@ namespace ams::ldr::hoc::pcv {
     // The pcv SoC-voltage DVB table is a fixed 32-entry region. (it doesn't need to be larger)
     constexpr size_t DvbTableCapacity = 32;
 
+    /* Note: According to TRM the lut size is 33, but up to 64 LUT is possible. */
+    constexpr size_t ClDvfsLutSize = 64;
+
     inline uintptr_t g_pcv_cave      = 0;
     inline size_t    g_pcv_cave_size = 0;