[PATCH] rtemsbsd/versal_slcr: Fix Versal GEM clock set

[aaron.nyholm at unfoldedeffective.com]

From: Aaron Nyholm <aaron.nyholm at southerninnovation.com>

---
 rtemsbsd/sys/arm64/xilinx/versal_slcr.c | 34 ++++++++++++++++++++++---
 rtemsbsd/sys/arm64/xilinx/versal_slcr.h |  6 +++++
 2 files changed, 36 insertions(+), 4 deletions(-)

diff --git a/rtemsbsd/sys/arm64/xilinx/versal_slcr.c b/rtemsbsd/sys/arm64/xilinx/versal_slcr.c
index 74ebde91..1f4d48bc 100644
--- a/rtemsbsd/sys/arm64/xilinx/versal_slcr.c
+++ b/rtemsbsd/sys/arm64/xilinx/versal_slcr.c
@@ -78,10 +78,13 @@ SYSCTL_NODE(_hw, OID_AUTO, versal, CTLFLAG_RD, 0, "Xilinx Versal ACAP SLCR");
 int
 cgem_set_ref_clk(int unit, int frequency)
 {
+
 	struct versal_slcr_softc *sc = versal_slcr_softc_p;
 	int div, last_error = 0;
-	uint64_t clk_ctrl, pll_ctrl;
+	uint64_t clk_ctrl, pll_ctrl, to_xpd_ctrl;
 	uint32_t clk_ctrl_val, pll_ctrl_val, pll_freq, pll_reset, pll_bypass;
+	uint32_t clk_src_sel, to_xpd_ctrl_val, to_xpd_div, to_xpd_freq;
+
 
 	if (!sc)
 		return (-1);
@@ -126,15 +129,38 @@ cgem_set_ref_clk(int unit, int frequency)
 	}
 
 	/* Apply divider */
-  pll_freq >>= (pll_ctrl_val & VERSAL_SLCR_PLL_CTRL_DIV_MASK) >> VERSAL_SLCR_PLL_CTRL_DIV_SHIFT;
+	pll_freq >>= (pll_ctrl_val & VERSAL_SLCR_PLL_CTRL_DIV_MASK) >> VERSAL_SLCR_PLL_CTRL_DIV_SHIFT;
+
+	/* Check if routed through {X}PLL_TO_XPD_CLK to GEM{unit}_REF_CLK and adjust */
+	clk_src_sel = (clk_ctrl_val & VERSAL_SLCR_GEM_CLK_CTRL_SRCSEL_MASK);
+	to_xpd_ctrl = 0;
+	if (clk_src_sel == VERSAL_SLCR_GEM_CLK_CTRL_SRCSEL_P_PLL)
+	{
+		to_xpd_ctrl = VERSAL_SLCR_PPLL_TO_XPD_CTRL;
+	} else if (clk_src_sel == VERSAL_SLCR_GEM_CLK_CTRL_SRCSEL_N_PLL)
+	{
+		to_xpd_ctrl = VERSAL_SLCR_NPLL_TO_XPD_CTRL;
+	}
+
+	if (to_xpd_ctrl != 0) {
+		to_xpd_ctrl_val = RD4(sc, to_xpd_ctrl);
+		to_xpd_div = (to_xpd_ctrl_val & VERSAL_SLCR_XPD_CLK_CTRL_DIVISOR_MASK);
+		to_xpd_div = to_xpd_div >> VERSAL_SLCR_XPD_CTRL_DIV_SHIFT;
+		if (to_xpd_div == 0) {
+			to_xpd_div = 1;
+		}
+		to_xpd_freq = pll_freq / to_xpd_div;
+	} else {
+		to_xpd_freq = pll_freq;
+	}
 
 	/* Find suitable divisor. Linear search, not the fastest method but hey.
 	 */
 	for (div = 1; div <= VERSAL_SLCR_GEM_CLK_CTRL_DIVISOR_MAX; div++) {
-    int div_freq = pll_freq / div;
+		int div_freq = to_xpd_freq / div;
 		int error = abs(frequency - div_freq);
 		if (error >= last_error && last_error != 0) {
-      div--;
+			div--;
 			break;
 		}
 		last_error = error;
diff --git a/rtemsbsd/sys/arm64/xilinx/versal_slcr.h b/rtemsbsd/sys/arm64/xilinx/versal_slcr.h
index e1c967ac..121c1e0a 100644
--- a/rtemsbsd/sys/arm64/xilinx/versal_slcr.h
+++ b/rtemsbsd/sys/arm64/xilinx/versal_slcr.h
@@ -78,6 +78,12 @@
 #define   VERSAL_SLCR_GEM_CLK_CTRL_SRCSEL_R_PLL		(1<<0)
 #define   VERSAL_SLCR_GEM_CLK_CTRL_SRCSEL_N_PLL		(3<<0)
 
+#define VERSAL_SLCR_PPLL_TO_XPD_CTRL		(VERSAL_SLCR_CRF_OFFSET + 0x100)
+#define VERSAL_SLCR_NPLL_TO_XPD_CTRL		(VERSAL_SLCR_CRF_OFFSET + 0x104)
+#define   VERSAL_SLCR_XPD_CLK_CTRL_DIVISOR_MAX		0x3ff
+#define   VERSAL_SLCR_XPD_CLK_CTRL_DIVISOR_MASK	(VERSAL_SLCR_XPD_CLK_CTRL_DIVISOR_MAX<<8)
+#define   VERSAL_SLCR_XPD_CTRL_DIV_SHIFT		8
+
 #define VERSAL_DEFAULT_PS_CLK_FREQUENCY 33333333
 
 #ifdef _KERNEL
-- 
2.25.1