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

[aaron . nyholm]

From: Aaron Nyholm <aaron.nyh...@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

_______________________________________________
devel mailing list
devel@rtems.org
http://lists.rtems.org/mailman/listinfo/devel