move pllmb measurement to bpmp

Source/hoc-clk/bpmpfw/source/main.cpp

@@ -23,6 +23,7 @@
 #include "bpmp_mmu.hpp"
 #include "freq.hpp"
 #include "libc_platform.hpp"
+#include "pllmb.hpp"
 #include "regs.hpp"
 #include "sensors.hpp"
 
@@ -55,6 +56,7 @@ void MainLoop(HocClkBpmpSharedInfo &info) {
         freq::Update(info);
         actmon::Update(info);
         aotag::Update(info);
+        info.freqMemPll = static_cast<u32>(pllmb::MeasureRamClockHz() / 1000);
 
         msleep(250);
     }

Source/hoc-clk/bpmpfw/source/pllmb.hpp

@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) Souldbminer
+ * Copyright (c) KazushiMe
+ * Copyright (c) Souldbminer, Lightos and Horizon OC Contributors
+ *
+ * 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/>.
+ *
+ */
+
+#pragma once
+#include "freq.hpp"
+#include "regs.hpp"
+
+namespace pllmb {
+
+    constexpr u32 PtoPllm  = 2;
+    constexpr u32 PtoPllmb = 37;
+
+    constexpr u32 PllmMisc2         = 0x9C;
+    constexpr u32 PllmMisc2PllmBit  = (1u << 8);
+    constexpr u32 PllmMisc2PllmbBit = (1u << 9);
+
+    constexpr u32 PtoCyclePeriod = (1u << 31); // PTO_CLK_CNT_STATUS busy bit
+
+    inline u64 MeasurePtoHz(u32 ptoId, u32 divider, u32 preselReg, u32 preselMask) {
+        u32 preselOrigBit = 0;
+
+        if (preselReg) {
+            u32 val = MMIO32(freq::CarBase + preselReg);
+            usleep(10);
+            preselOrigBit = val & preselMask;
+            val &= ~preselMask;
+            val |= preselMask;
+            MMIO32(freq::CarBase + preselReg) = val;
+            usleep(10);
+        }
+
+        constexpr u32 cycleCount = 16;
+        const u32 savedCntl = MMIO32(freq::CarBase + freq::CarPtoClkCntCntl);
+
+        u32 val = freq::PtoDivSelDiv1 | freq::PtoClkEnable | (cycleCount - 1);
+        val |= ptoId << freq::PtoSrcSelShift;
+
+        MMIO32(freq::CarBase + freq::CarPtoClkCntCntl) = val;
+        usleep(10);
+        MMIO32(freq::CarBase + freq::CarPtoClkCntCntl) = val | freq::PtoCntRst;
+        usleep(10);
+        MMIO32(freq::CarBase + freq::CarPtoClkCntCntl) = val;
+        usleep(10);
+        MMIO32(freq::CarBase + freq::CarPtoClkCntCntl) = val | freq::PtoCntEn;
+        usleep(500);
+
+        while (MMIO32(freq::CarBase + freq::CarPtoClkCntStatus) & PtoCyclePeriod)
+            ;
+
+        u32 cnt = MMIO32(freq::CarBase + freq::CarPtoClkCntStatus) & freq::PtoClkCntMask;
+        cnt *= divider;
+
+        const u64 rateHz = (static_cast<u64>(cnt) * 32768) / cycleCount;
+
+        usleep(10);
+        MMIO32(freq::CarBase + freq::CarPtoClkCntCntl) = savedCntl;
+        usleep(10);
+
+        if (preselReg) {
+            u32 val2 = MMIO32(freq::CarBase + preselReg);
+            usleep(10);
+            val2 &= ~preselMask;
+            val2 |= preselOrigBit;
+            MMIO32(freq::CarBase + preselReg) = val2;
+            usleep(10);
+        }
+
+        return rateHz;
+    }
+
+    inline u64 MeasureRamClockHz() {
+        const u64 pllmbHz = MeasurePtoHz(PtoPllmb, 2, PllmMisc2, PllmMisc2PllmbBit);
+        if (pllmbHz != 0) {
+            return pllmbHz;
+        }
+        return MeasurePtoHz(PtoPllm, 2, PllmMisc2, PllmMisc2PllmBit);
+    }
+
+} // namespace pllmb