diff options
| author | Aaron Nyholm <aaron.nyholm@southerninnovation.com> | 2023-06-15 15:35:48 +1000 |
|---|---|---|
| committer | Chris Johns <chrisj@rtems.org> | 2023-06-20 10:16:10 +1000 |
| commit | 5de608c58234b893093c6dbed5f73a580f032db0 (patch) | |
| tree | 25a2e805dc28c542af5059458120762bedce90c2 | |
| parent | ipsec-tools: Fix copying fd_set prior to select (diff) | |
| download | rtems-libbsd-5de608c58234b893093c6dbed5f73a580f032db0.tar.bz2 | |
rtemsbsd/versal_slcr: Fix Versal GEM clock set
| -rw-r--r-- | rtemsbsd/sys/arm64/xilinx/versal_slcr.c | 30 | ||||
| -rw-r--r-- | rtemsbsd/sys/arm64/xilinx/versal_slcr.h | 6 |
2 files changed, 32 insertions, 4 deletions
diff --git a/rtemsbsd/sys/arm64/xilinx/versal_slcr.c b/rtemsbsd/sys/arm64/xilinx/versal_slcr.c index 74ebde91..fea3abab 100644 --- a/rtemsbsd/sys/arm64/xilinx/versal_slcr.c +++ b/rtemsbsd/sys/arm64/xilinx/versal_slcr.c @@ -80,8 +80,9 @@ 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 +127,36 @@ 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 |