nexus: Add ZynqMP SLCR driver

Add a System Level Control Register driver for the Xilinx Zynq
Ultrascale+ MPSoC with basic clock control functionality for use with
the Cadence GEM. This also removes the Zynq-7000 clock control weakref
from compilation depending on the BSP in use.

6 files changed, 349 insertions, 0 deletions

diff --git a/freebsd/sys/arm/xilinx/zy7_slcr.c b/freebsd/sys/arm/xilinx/zy7_slcr.c
index 79fccee5..4bbdb626 100644
--- a/freebsd/sys/arm/xilinx/zy7_slcr.c
+++ b/freebsd/sys/arm/xilinx/zy7_slcr.c
@@ -45,6 +45,7 @@ __FBSDID("$FreeBSD$");
 #include <sys/systm.h>
 #ifdef __rtems__
 #include <sys/bus.h>
+#include <bsp.h>
 #endif /* __rtems__ */
 #include <sys/conf.h>
 #include <sys/kernel.h>
@@ -217,6 +218,7 @@ zy7_slcr_postload_pl(int en_level_shifters)
 	ZSLCR_UNLOCK(sc);
 }
 
+#if defined(LIBBSP_ARM_XILINX_ZYNQ_BSP_H)
 /* Override cgem_set_refclk() in gigabit ethernet driver
  * (sys/dev/cadence/if_cgem.c).  This function is called to
  * request a change in the gem's reference clock speed.
@@ -264,6 +266,7 @@ cgem_set_ref_clk(int unit, int frequency)
 
 	return (0);
 }
+#endif /* LIBBSP_ARM_XILINX_ZYNQ_BSP_H */
 
 /* 
  * PL clocks management function
diff --git a/libbsd.py b/libbsd.py
index 28617d21..16e81565 100644
--- a/libbsd.py
+++ b/libbsd.py
@@ -1423,6 +1423,7 @@ class dev_net(builder.Module):
         self.addRTEMSKernelSourceFiles(
             [
                 'sys/dev/mii/ksz8091rnb_50MHz.c',
+                'sys/arm64/xilinx/zynqmp_slcr.c',
             ],
             mm.generator['source']()
         )
diff --git a/rtemsbsd/include/bsp/nexus-devices.h b/rtemsbsd/include/bsp/nexus-devices.h
index 7cdd5428..ea8fa7d7 100644
--- a/rtemsbsd/include/bsp/nexus-devices.h
+++ b/rtemsbsd/include/bsp/nexus-devices.h
@@ -107,6 +107,7 @@ RTEMS_BSD_DRIVER_MMC;
 
 #include <bsp/irq.h>
 
+RTEMS_BSD_DRIVER_XILINX_ZYNQMP_SLCR;
 /* Qemu only applies user-mode networking to the first interface by default, so
  * all 4 CGEM instances must be configured in the Qemu arguments using
  * "-nic user,model=cadence_gem" for each nic.
diff --git a/rtemsbsd/include/machine/rtems-bsd-nexus-bus.h b/rtemsbsd/include/machine/rtems-bsd-nexus-bus.h
index 5c95d2c3..bb5546a6 100644
--- a/rtemsbsd/include/machine/rtems-bsd-nexus-bus.h
+++ b/rtemsbsd/include/machine/rtems-bsd-nexus-bus.h
@@ -38,6 +38,7 @@
  *
  *  Devices:
  *   RTEMS_BSD_DRIVER_XILINX_ZYNQ_SLCR
+ *   RTEMS_BSD_DRIVER_XILINX_ZYNQMP_SLCR
  *   RTEMS_BSD_DRIVER_LPC32XX_PWR
  *   RTEMS_BSD_DRIVER_LPC32XX_TSC
  *
@@ -118,6 +119,26 @@ extern "C" {
 #endif /* RTEMS_BSD_DRIVER_XILINX_ZYNQ_SLCR */
 
 /*
+ * Xilinx ZynqMP System Level Control Registers (SLCR).
+ */
+#if !defined(RTEMS_BSD_DRIVER_XILINX_ZYNQMP_SLCR)
+  /*
+   * Hard IP part of the ZynqMP so a fixed address.
+   */
+  #define RTEMS_BSD_DRIVER_XILINX_ZYNQMP_SLCR                            \
+    static const rtems_bsd_device_resource zynqmp_slcr_res[] = {          \
+      {                                                                 \
+        .type = RTEMS_BSD_RES_MEMORY,                                   \
+        .start_request = 0,                                             \
+        .start_actual = 0xf0000000                                      \
+      }                                                                 \
+    };                                                                  \
+    RTEMS_BSD_DEFINE_NEXUS_DEVICE(zynqmp_slcr, 0,                          \
+                                  RTEMS_ARRAY_SIZE(zynqmp_slcr_res),      \
+                                  &zynqmp_slcr_res[0])
+#endif /* RTEMS_BSD_DRIVER_XILINX_ZYNQMP_SLCR */
+
+/*
  * Xilinx Zynq Arasan SDIO Driver.
  */
 #if !defined(RTEMS_BSD_DRIVER_XILINX_ZYNQ_SDHCI)
diff --git a/rtemsbsd/sys/arm64/xilinx/zynqmp_slcr.c b/rtemsbsd/sys/arm64/xilinx/zynqmp_slcr.c
new file mode 100644
index 00000000..ac52d234
--- /dev/null
+++ b/rtemsbsd/sys/arm64/xilinx/zynqmp_slcr.c
@@ -0,0 +1,233 @@
+#include <machine/rtems-bsd-kernel-space.h>
+
+/*-
+ * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
+ *
+ * Copyright (c) 2021 Kinsey Moore
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ */
+
+/*
+ * Zynq Ultrascale+ MPSoC SLCR driver.  Provides hook for CGEM clocks.
+ *
+ * Reference: Zynq Ultrascale+ MPSoC Technical Reference Manual.
+ * (v2.2) December 4, 2020.  Xilinx doc UG1085.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <bsp.h>
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/conf.h>
+#include <sys/kernel.h>
+#include <sys/module.h>
+#include <sys/lock.h>
+#include <sys/mutex.h>
+#include <rtems/bsd/sys/resource.h>
+#include <sys/sysctl.h>
+#include <sys/rman.h>
+
+#include <machine/bus.h>
+#include <machine/resource.h>
+#include <machine/stdarg.h>
+
+#include <arm64/xilinx/zynqmp_slcr.h>
+
+struct zynqmp_slcr_softc {
+	device_t	dev;
+	struct mtx	sc_mtx;
+	struct resource	*mem_res;
+};
+
+static struct zynqmp_slcr_softc *zynqmp_slcr_softc_p;
+
+#define RD4(sc, off) 		(bus_read_4((sc)->mem_res, (off)))
+#define WR4(sc, off, val) 	(bus_write_4((sc)->mem_res, (off), (val)))
+
+SYSCTL_NODE(_hw, OID_AUTO, zynqmp, CTLFLAG_RD, 0, "Xilinx Zynq Ultrascale+ MPSoC");
+
+#if defined(LIBBSP_AARCH64_XILINX_ZYNQMP_BSP_H)
+/* Override cgem_set_refclk() in gigabit ethernet driver
+ * (sys/dev/cadence/if_cgem.c).  This function is called to
+ * request a change in the gem's reference clock speed.
+ */
+int
+cgem_set_ref_clk(int unit, int frequency)
+{
+	struct zynqmp_slcr_softc *sc = zynqmp_slcr_softc_p;
+	int div0, div1;
+	uint64_t clk_ctrl, pll_ctrl;
+	uint32_t clk_ctrl_val, pll_ctrl_val, pll_freq, pll_bypass;
+
+	if (!sc)
+		return (-1);
+
+	switch (unit) {
+		case 0:
+			clk_ctrl = ZYNQMP_SLCR_GEM0_CLK_CTRL;
+			break;
+		case 1:
+			clk_ctrl = ZYNQMP_SLCR_GEM1_CLK_CTRL;
+			break;
+		case 2:
+			clk_ctrl = ZYNQMP_SLCR_GEM2_CLK_CTRL;
+			break;
+		case 3:
+			clk_ctrl = ZYNQMP_SLCR_GEM3_CLK_CTRL;
+			break;
+		default:
+			return (-1);
+	}
+
+	clk_ctrl_val = RD4(sc, clk_ctrl);
+	switch (clk_ctrl_val & ZYNQMP_SLCR_GEM_CLK_CTRL_SRCSEL_MASK) {
+		case 0:
+			pll_ctrl = ZYNQMP_SLCR_IO_PLL_CTRL;
+			break;
+		case 2:
+			pll_ctrl = ZYNQMP_SLCR_R_PLL_CTRL;
+			break;
+		case 3:
+			pll_ctrl = ZYNQMP_SLCR_D_PLL_CTRL;
+			break;
+		default:
+			return (-1);
+	}
+
+	/* Get PLL frequency */
+	pll_ctrl_val = RD4(sc, pll_ctrl);
+	pll_bypass = pll_ctrl_val & ZYNQMP_SLCR_PLL_CTRL_BYPASS;
+	if ((pll_ctrl_val & ZYNQMP_SLCR_PLL_CTRL_RESET) && !pll_bypass) {
+		return 0;
+	}
+
+	pll_freq = ZYNQMP_DEFAULT_PS_CLK_FREQUENCY;
+	if (!pll_bypass) {
+		pll_freq *= (pll_ctrl_val & ZYNQMP_SLCR_PLL_CTRL_FBDIV_MASK) >> ZYNQMP_SLCR_PLL_CTRL_FBDIV_SHIFT;
+	}
+
+	/* Divide by 2 if necessary */
+	pll_freq >>= !!(pll_ctrl_val & ZYNQMP_SLCR_PLL_CTRL_DIV2);
+
+	/* Find suitable divisor pairs.  Round result to nearest khz
+	 * to test for match.
+	 */
+	for (div1 = 1; div1 <= ZYNQMP_SLCR_GEM_CLK_CTRL_DIVISOR1_MAX; div1++) {
+		div0 = (pll_freq + div1 * frequency / 2) /
+			div1 / frequency;
+		if (div0 > 0 && div0 <= ZYNQMP_SLCR_GEM_CLK_CTRL_DIVISOR0_MAX &&
+		    ((pll_freq / div0 / div1) + 500) / 1000 ==
+		    (frequency + 500) / 1000)
+			break;
+	}
+
+	if (div1 > ZYNQMP_SLCR_GEM_CLK_CTRL_DIVISOR1_MAX)
+		return (-1);
+
+	/* Modify GEM reference clock. */
+	clk_ctrl_val &= ~ZYNQMP_SLCR_GEM_CLK_CTRL_DIVISOR1_MASK;
+	clk_ctrl_val &= ~ZYNQMP_SLCR_GEM_CLK_CTRL_DIVISOR0_MASK;
+	clk_ctrl_val |= div1 << ZYNQMP_SLCR_GEM_CLK_CTRL_DIVISOR1_SHIFT;
+	clk_ctrl_val |= div0 << ZYNQMP_SLCR_GEM_CLK_CTRL_DIVISOR0_SHIFT;
+	WR4(sc, clk_ctrl, clk_ctrl_val);
+
+	return (0);
+}
+#endif
+
+static int
+zynqmp_slcr_probe(device_t dev)
+{
+
+	device_set_desc(dev, "Zynq Ultrascale+ MPSoC SLCR block");
+	return (0);
+}
+
+static int
+zynqmp_slcr_attach(device_t dev)
+{
+	struct zynqmp_slcr_softc *sc = device_get_softc(dev);
+	int rid;
+
+	/* Allow only one attach. */
+	if (zynqmp_slcr_softc_p != NULL)
+		return (ENXIO);
+
+	sc->dev = dev;
+
+	/* Get memory resource. */
+	rid = 0;
+	sc->mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+					     RF_ACTIVE);
+	if (sc->mem_res == NULL) {
+		device_printf(dev, "could not allocate memory resources.\n");
+		return (ENOMEM);
+	}
+
+	/* For use with CGEM clock setting */
+	zynqmp_slcr_softc_p = sc;
+
+	return (0);
+}
+
+static int
+zynqmp_slcr_detach(device_t dev)
+{
+	struct zynqmp_slcr_softc *sc = device_get_softc(dev);
+
+	bus_generic_detach(dev);
+
+	/* Release memory resource. */
+	if (sc->mem_res != NULL)
+		bus_release_resource(dev, SYS_RES_MEMORY,
+			     rman_get_rid(sc->mem_res), sc->mem_res);
+
+	zynqmp_slcr_softc_p = NULL;
+
+	return (0);
+}
+
+static device_method_t zynqmp_slcr_methods[] = {
+	/* device_if */
+	DEVMETHOD(device_probe, 	zynqmp_slcr_probe),
+	DEVMETHOD(device_attach, 	zynqmp_slcr_attach),
+	DEVMETHOD(device_detach, 	zynqmp_slcr_detach),
+
+	DEVMETHOD_END
+};
+
+static driver_t zynqmp_slcr_driver = {
+	"zynqmp_slcr",
+	zynqmp_slcr_methods,
+	sizeof(struct zynqmp_slcr_softc),
+};
+static devclass_t zynqmp_slcr_devclass;
+
+DRIVER_MODULE(zynqmp_slcr, nexus, zynqmp_slcr_driver, zynqmp_slcr_devclass, 0, 0);
+MODULE_VERSION(zynqmp_slcr, 1);
diff --git a/rtemsbsd/sys/arm64/xilinx/zynqmp_slcr.h b/rtemsbsd/sys/arm64/xilinx/zynqmp_slcr.h
new file mode 100644
index 00000000..d5c7f38e
--- /dev/null
+++ b/rtemsbsd/sys/arm64/xilinx/zynqmp_slcr.h
@@ -0,0 +1,90 @@
+/*-
+ * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
+ *
+ * Copyright (c) 2021 Kinsey Moore
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ */
+
+/*
+ * Defines for Zynq Ultrascale+ MPSoC SLCR registers.
+ *
+ * Reference: Zynq Ultrascale+ MPSoC Technical Reference Manual.
+ * (v2.2) December 4, 2020.  Xilinx doc UG1085.
+ *
+ * Additional Reference: Zynq Ultrascale+ MPSoC Register Reference.
+ * (v1.7) February 8, 2019.  Xilinx doc UG1087.
+ */
+
+#ifndef _ZYNQMP_SLCR_H_
+#define _ZYNQMP_SLCR_H_
+
+#define ZYNQMP_SLCR_CRF_OFFSET		0x0d1a0000
+#define ZYNQMP_SLCR_CRL_OFFSET		0x0f5e0000
+
+/* PLL controls. */
+#define ZYNQMP_SLCR_IO_PLL_CTRL		(ZYNQMP_SLCR_CRL_OFFSET + 0x20)
+#define ZYNQMP_SLCR_R_PLL_CTRL		(ZYNQMP_SLCR_CRL_OFFSET + 0x30)
+#define ZYNQMP_SLCR_D_PLL_CTRL		(ZYNQMP_SLCR_CRF_OFFSET + 0x2c)
+#define   ZYNQMP_SLCR_PLL_CTRL_RESET		(1<<0)
+#define   ZYNQMP_SLCR_PLL_CTRL_BYPASS		(1<<3)
+#define   ZYNQMP_SLCR_PLL_CTRL_FBDIV_SHIFT	8
+#define   ZYNQMP_SLCR_PLL_CTRL_FBDIV_MASK	(0x7f<<8)
+#define   ZYNQMP_SLCR_PLL_CTRL_FBDIV_MAX	0x7f
+#define   ZYNQMP_SLCR_PLL_CTRL_DIV2		(1<<16)
+#define   ZYNQMP_SLCR_PLL_CTRL_PRE_SRC_SHIFT	20
+#define   ZYNQMP_SLCR_PLL_CTRL_PRE_SRC_MASK	(0x7<<20)
+#define   ZYNQMP_SLCR_PLL_CTRL_POST_SRC_SHIFT	24
+#define   ZYNQMP_SLCR_PLL_CTRL_POST_SRC_MASK	(0x7<<24)
+#define     ZYNQMP_SLCR_PLL_CTRL_SRC_PS		0x0
+#define     ZYNQMP_SLCR_PLL_CTRL_SRC_VIDEO	0x4
+#define     ZYNQMP_SLCR_PLL_CTRL_SRC_ALT	0x5
+#define     ZYNQMP_SLCR_PLL_CTRL_SRC_AUX	0x6
+#define     ZYNQMP_SLCR_PLL_CTRL_SRC_GT		0x7
+
+/* Clock controls. */
+#define ZYNQMP_SLCR_GEM0_CLK_CTRL		(ZYNQMP_SLCR_CRL_OFFSET + 0x50)
+#define ZYNQMP_SLCR_GEM1_CLK_CTRL		(ZYNQMP_SLCR_CRL_OFFSET + 0x54)
+#define ZYNQMP_SLCR_GEM2_CLK_CTRL		(ZYNQMP_SLCR_CRL_OFFSET + 0x58)
+#define ZYNQMP_SLCR_GEM3_CLK_CTRL		(ZYNQMP_SLCR_CRL_OFFSET + 0x5c)
+#define   ZYNQMP_SLCR_GEM_CLK_CTRL_RX_CLKACT		(1<<26)
+#define   ZYNQMP_SLCR_GEM_CLK_CTRL_CLKACT		(1<<25)
+#define   ZYNQMP_SLCR_GEM_CLK_CTRL_DIVISOR1_MASK	(0x3f<<16)
+#define   ZYNQMP_SLCR_GEM_CLK_CTRL_DIVISOR1_SHIFT	16
+#define   ZYNQMP_SLCR_GEM_CLK_CTRL_DIVISOR1_MAX		0x3f
+#define   ZYNQMP_SLCR_GEM_CLK_CTRL_DIVISOR0_MASK	(0x3f<<8)
+#define   ZYNQMP_SLCR_GEM_CLK_CTRL_DIVISOR0_SHIFT	8
+#define   ZYNQMP_SLCR_GEM_CLK_CTRL_DIVISOR0_MAX		0x3f
+#define   ZYNQMP_SLCR_GEM_CLK_CTRL_SRCSEL_MASK		(7<<0)
+#define   ZYNQMP_SLCR_GEM_CLK_CTRL_SRCSEL_IO_PLL	(0<<0)
+#define   ZYNQMP_SLCR_GEM_CLK_CTRL_SRCSEL_R_PLL		(2<<0)
+#define   ZYNQMP_SLCR_GEM_CLK_CTRL_SRCSEL_D_PLL		(3<<0)
+
+#define ZYNQMP_DEFAULT_PS_CLK_FREQUENCY 33333333
+
+#ifdef _KERNEL
+extern int cgem_set_ref_clk(int unit, int frequency);
+#endif
+#endif /* _ZYNQMP_SLCR_H_ */