bpmpfw: cleanup makefile and split hpp/cpp

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

@@ -0,0 +1,84 @@
+/*
+ * 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/>.
+ *
+ */
+
+#include "pllmb.hpp"
+
+namespace pllmb {
+
+    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;
+    }
+
+    u64 MeasureRamClockHz() {
+        const u64 pllmbHz = MeasurePtoHz(PtoPllmb, 2, PllmMisc2, PllmMisc2PllmbBit);
+        if (pllmbHz != 0) {
+            return pllmbHz;
+        }
+        return MeasurePtoHz(PtoPllm, 2, PllmMisc2, PllmMisc2PllmBit);
+    }
+
+} // namespace pllmb