Index: arm/xilinx/zy7_devcfg.c
===================================================================
--- arm/xilinx/zy7_devcfg.c	(revision 278739)
+++ arm/xilinx/zy7_devcfg.c	(working copy)
@@ -72,10 +72,23 @@
 	bus_dmamap_t	dma_map;
 
 	int		is_open;
+
+	struct sysctl_ctx_list sysctl_tree;
+	struct sysctl_oid *sysctl_tree_top;
 };
 
 static struct zy7_devcfg_softc *zy7_devcfg_softc_p;
 
+#define	FCLK_NUM	4
+
+struct zy7_fclk_config {
+	int		source;
+	int		frequency;
+	int		actual_frequency;
+};
+
+static struct zy7_fclk_config fclk_configs[FCLK_NUM];
+
 #define DEVCFG_SC_LOCK(sc)		mtx_lock(&(sc)->sc_mtx)
 #define	DEVCFG_SC_UNLOCK(sc)		mtx_unlock(&(sc)->sc_mtx)
 #define DEVCFG_SC_LOCK_INIT(sc) \
@@ -103,6 +116,11 @@
 SYSCTL_INT(_hw, OID_AUTO, ps_vers, CTLFLAG_RD, &zy7_ps_vers, 0,
 	   "Zynq-7000 PS version");
 
+static int zy7_devcfg_fclk_sysctl_level_shifters(SYSCTL_HANDLER_ARGS);
+SYSCTL_PROC(_hw_fpga, OID_AUTO, level_shifters, 
+		    CTLFLAG_RW | CTLTYPE_INT, 
+		    NULL, 0, zy7_devcfg_fclk_sysctl_level_shifters,
+		    "I", "Enable/disable level shifters");
 
 /* cdev entry points. */
 static int zy7_devcfg_open(struct cdev *, int, int, struct thread *);
@@ -109,7 +127,6 @@
 static int zy7_devcfg_write(struct cdev *, struct uio *, int);
 static int zy7_devcfg_close(struct cdev *, int, int, struct thread *);
 
-
 struct cdevsw zy7_devcfg_cdevsw = {
 	.d_version =	D_VERSION,
 	.d_open =	zy7_devcfg_open,
@@ -230,7 +247,152 @@
 #define ZY7_DEVCFG_XADCIF_RD_FIFO	0x114
 #define ZY7_DEVCFG_XADCIF_MCTL		0x118
 
+static int
+zy7_devcfg_fclk_sysctl_source(SYSCTL_HANDLER_ARGS)
+{
+	char buf[4];
+	struct zy7_fclk_config *cfg;
+	int unit;
+	int error;
 
+	cfg = arg1;
+	unit = arg2;
+
+	switch (cfg->source) {
+		case ZY7_PL_FCLK_SRC_IO:
+		case ZY7_PL_FCLK_SRC_IO_ALT:
+			strncpy(buf, "IO", sizeof(buf));
+			break;
+		case ZY7_PL_FCLK_SRC_DDR:
+			strncpy(buf, "DDR", sizeof(buf));
+			break;
+		case ZY7_PL_FCLK_SRC_ARM:
+			strncpy(buf, "ARM", sizeof(buf));
+			break;
+		default:
+			strncpy(buf, "???", sizeof(buf));
+			break;
+	}
+
+	error = sysctl_handle_string(oidp, buf, sizeof(buf), req);
+	if (error != 0 || req->newptr == NULL)
+		return (error);
+
+	if (strcasecmp(buf, "io") == 0)
+		cfg->source = ZY7_PL_FCLK_SRC_IO;
+	else if (strcasecmp(buf, "ddr") == 0)
+		cfg->source = ZY7_PL_FCLK_SRC_DDR;
+	else if (strcasecmp(buf, "arm") == 0)
+		cfg->source = ZY7_PL_FCLK_SRC_ARM;
+	else
+		return (EINVAL);
+
+	zy7_pl_fclk_set_source(unit, cfg->source);
+	if (cfg->frequency > 0)
+		cfg->actual_frequency = zy7_pl_fclk_get_freq(unit);
+
+	return (0);
+}
+
+static int
+zy7_devcfg_fclk_sysctl_freq(SYSCTL_HANDLER_ARGS)
+{
+	struct zy7_fclk_config *cfg;
+	int unit;
+	int error;
+	int freq;
+	int new_actual_freq;
+
+	cfg = arg1;
+	unit = arg2;
+
+	freq = cfg->frequency;
+
+	error = sysctl_handle_int(oidp, &freq, 0, req);
+	if (error != 0 || req->newptr == NULL)
+		return (error);
+
+	if (freq > 0) {
+		new_actual_freq = zy7_pl_fclk_set_freq(unit, freq);
+		if (new_actual_freq < 0)
+			return (EINVAL);
+		if (!zy7_pl_fclk_enabled(unit))
+			zy7_pl_fclk_enable(unit);
+	}
+	else {
+		zy7_pl_fclk_disable(unit);
+		new_actual_freq = 0;
+	}
+
+	cfg->frequency = freq;
+	cfg->actual_frequency = new_actual_freq;
+
+	return (0);
+}
+
+static int
+zy7_devcfg_fclk_sysctl_level_shifters(SYSCTL_HANDLER_ARGS)
+{
+	int error, enabled;
+
+	enabled = zy7_pl_level_shifters_enabled();
+
+	error = sysctl_handle_int(oidp, &enabled, 0, req);
+	if (error != 0 || req->newptr == NULL)
+		return (error);
+
+	if (enabled)
+		zy7_pl_level_shifters_enable();
+	else
+		zy7_pl_level_shifters_disable();
+
+	return (0);
+}
+
+static int
+zy7_devcfg_init_fclk_sysctl(struct zy7_devcfg_softc *sc)
+{
+	struct sysctl_oid *fclk_node;
+	char fclk_num[4];
+	int i;
+
+	sysctl_ctx_init(&sc->sysctl_tree);
+	sc->sysctl_tree_top = SYSCTL_ADD_NODE(&sc->sysctl_tree,
+	    SYSCTL_STATIC_CHILDREN(_hw_fpga), OID_AUTO, "fclk",
+	    CTLFLAG_RD, 0, "");
+	if (sc->sysctl_tree_top == NULL) {
+		sysctl_ctx_free(&sc->sysctl_tree);
+		return (-1);
+	}
+
+	for (i = 0; i < FCLK_NUM; i++) {
+		snprintf(fclk_num, sizeof(fclk_num), "%d", i);
+		fclk_node = SYSCTL_ADD_NODE(&sc->sysctl_tree,
+		    SYSCTL_CHILDREN(sc->sysctl_tree_top), OID_AUTO, fclk_num,
+		    CTLFLAG_RD, 0, "");
+
+		SYSCTL_ADD_INT(&sc->sysctl_tree,
+		    SYSCTL_CHILDREN(fclk_node), OID_AUTO,
+		    "actual_freq", CTLFLAG_RD, 
+		    &fclk_configs[i].actual_frequency, i,
+		    "Actual frequency");
+		SYSCTL_ADD_PROC(&sc->sysctl_tree,
+		    SYSCTL_CHILDREN(fclk_node), OID_AUTO,
+		    "freq", CTLFLAG_RW | CTLTYPE_INT, 
+		    &fclk_configs[i], i,
+		    zy7_devcfg_fclk_sysctl_freq,
+		    "I", "Configured frequency");
+		SYSCTL_ADD_PROC(&sc->sysctl_tree,
+		    SYSCTL_CHILDREN(fclk_node), OID_AUTO,
+		    "source", CTLFLAG_RW | CTLTYPE_STRING, 
+		    &fclk_configs[i], i, 
+		    zy7_devcfg_fclk_sysctl_source,
+		    "A", "Clock source");
+	}
+
+	return (0);
+}
+
 /* Enable programming the PL through PCAP. */
 static void
 zy7_devcfg_init_hw(struct zy7_devcfg_softc *sc)
@@ -334,7 +496,6 @@
 		*(bus_addr_t *)arg = seg[0].ds_addr;
 }
 
-
 static int
 zy7_devcfg_open(struct cdev *dev, int oflags, int devtype, struct thread *td)
 {
@@ -474,10 +635,11 @@
 	bus_dma_tag_destroy(sc->dma_tag);
 	DEVCFG_SC_UNLOCK(sc);
 
+	zy7_slcr_postload_pl(zy7_en_level_shifters);
+
 	return (0);
 }
 
-
 static void
 zy7_devcfg_intr(void *arg)
 {
@@ -549,6 +711,7 @@
 zy7_devcfg_attach(device_t dev)
 {
 	struct zy7_devcfg_softc *sc = device_get_softc(dev);
+	int i;
 	int rid, err;
 
 	/* Allow only one attach. */
@@ -612,6 +775,17 @@
 		       ZY7_DEVCFG_MCTRL_PS_VERS_MASK) >>
 		ZY7_DEVCFG_MCTRL_PS_VERS_SHIFT;
 
+	for (i = 0; i < FCLK_NUM; i++) {
+		fclk_configs[i].source = zy7_pl_fclk_get_source(i);
+		fclk_configs[i].actual_frequency = 
+			zy7_pl_fclk_enabled(i) ? zy7_pl_fclk_get_freq(i) : 0;
+		/* Initially assume actual frequency is the configure one */
+		fclk_configs[i].frequency = fclk_configs[i].actual_frequency;
+	}
+
+	if (zy7_devcfg_init_fclk_sysctl(sc) < 0)
+		device_printf(dev, "failed to initialized sysctl tree\n");
+
 	return (0);
 }
 
@@ -620,6 +794,11 @@
 {
 	struct zy7_devcfg_softc *sc = device_get_softc(dev);
 
+	if (sc->sysctl_tree_top != NULL) {
+		sysctl_ctx_free(&sc->sysctl_tree);
+		sc->sysctl_tree_top = NULL;
+	}
+
 	if (device_is_attached(dev))
 		bus_generic_detach(dev);
 
Index: arm/xilinx/zy7_slcr.c
===================================================================
--- arm/xilinx/zy7_slcr.c	(revision 278739)
+++ arm/xilinx/zy7_slcr.c	(working copy)
@@ -79,7 +79,6 @@
 
 #define ZYNQ_DEFAULT_PS_CLK_FREQUENCY	33333333	/* 33.3 Mhz */
 
-
 SYSCTL_NODE(_hw, OID_AUTO, zynq, CTLFLAG_RD, 0, "Xilinx Zynq-7000");
 
 static char zynq_bootmode[64];
@@ -126,7 +125,6 @@
 	WR4(sc, ZY7_SLCR_LOCK, ZY7_SLCR_LOCK_MAGIC);
 }
 
-
 static void
 zy7_slcr_cpu_reset(void)
 {
@@ -255,6 +253,296 @@
 	return (0);
 }
 
+/* 
+ * PL clocks management function
+ */
+int 
+zy7_pl_fclk_set_source(int unit, int source)
+{
+	struct zy7_slcr_softc *sc = zy7_slcr_softc_p;
+	uint32_t reg;
+
+	if (!sc)
+		return (-1);
+
+	ZSLCR_LOCK(sc);
+
+	/* Unlock SLCR registers. */
+	zy7_slcr_unlock(sc);
+
+	/* Modify FPGAx source. */
+	reg = RD4(sc, ZY7_SLCR_FPGA_CLK_CTRL(unit));
+	reg &= ~(ZY7_SLCR_FPGA_CLK_CTRL_SRCSEL_MASK);
+	reg |= (source << ZY7_SLCR_FPGA_CLK_CTRL_SRCSEL_SHIFT);
+	WR4(sc, ZY7_SLCR_FPGA_CLK_CTRL(unit), reg);
+
+	/* Lock SLCR registers. */
+	zy7_slcr_lock(sc);
+
+	ZSLCR_UNLOCK(sc);
+
+	return (0);
+}
+
+int 
+zy7_pl_fclk_get_source(int unit)
+{
+	struct zy7_slcr_softc *sc = zy7_slcr_softc_p;
+	uint32_t reg;
+	int source;
+
+	if (!sc)
+		return (-1);
+
+	ZSLCR_LOCK(sc);
+
+	/* Modify GEM reference clock. */
+	reg = RD4(sc, ZY7_SLCR_FPGA_CLK_CTRL(unit));
+	source = (reg & ZY7_SLCR_FPGA_CLK_CTRL_SRCSEL_MASK) >> 
+	    ZY7_SLCR_FPGA_CLK_CTRL_SRCSEL_SHIFT;
+
+	/* ZY7_PL_FCLK_SRC_IO is actually b0x */
+	if ((source & 2) == 0)
+		source = ZY7_PL_FCLK_SRC_IO;
+
+	ZSLCR_UNLOCK(sc);
+
+	return (source);
+}
+
+int
+zy7_pl_fclk_set_freq(int unit, int frequency)
+{
+	struct zy7_slcr_softc *sc = zy7_slcr_softc_p;
+	int div0, div1;
+	int base_frequency;
+	uint32_t reg;
+	int source;
+
+	if (!sc)
+		return (-1);
+
+	source = zy7_pl_fclk_get_source(unit);
+	switch (source) {
+		case ZY7_PL_FCLK_SRC_IO:
+			base_frequency = io_pll_frequency;
+			break;
+
+		case ZY7_PL_FCLK_SRC_ARM:
+			base_frequency = arm_pll_frequency;
+			break;
+
+		case ZY7_PL_FCLK_SRC_DDR:
+			base_frequency = ddr_pll_frequency;
+			break;
+
+		default:
+			return (-1);
+	}
+
+	/* Find suitable divisor pairs.  Round result to nearest khz
+	 * to test for match.
+	 */
+	for (div1 = 1; div1 <= ZY7_SLCR_FPGA_CLK_CTRL_DIVISOR_MAX; div1++) {
+		div0 = (base_frequency + div1 * frequency / 2) /
+			div1 / frequency;
+		if (div0 > 0 && div0 <= ZY7_SLCR_FPGA_CLK_CTRL_DIVISOR_MAX &&
+		    ((base_frequency / div0 / div1) + 500) / 1000 ==
+		    (frequency + 500) / 1000)
+			break;
+	}
+
+	if (div1 > ZY7_SLCR_FPGA_CLK_CTRL_DIVISOR_MAX)
+		return (-1);
+
+	ZSLCR_LOCK(sc);
+
+	/* Unlock SLCR registers. */
+	zy7_slcr_unlock(sc);
+
+	/* Modify FPGAx reference clock. */
+	reg = RD4(sc, ZY7_SLCR_FPGA_CLK_CTRL(unit));
+	reg &= ~(ZY7_SLCR_FPGA_CLK_CTRL_DIVISOR1_MASK |
+	    ZY7_SLCR_FPGA_CLK_CTRL_DIVISOR0_MASK);
+	reg |= (div1 << ZY7_SLCR_FPGA_CLK_CTRL_DIVISOR1_SHIFT) |
+	    (div0 << ZY7_SLCR_FPGA_CLK_CTRL_DIVISOR0_SHIFT);
+	WR4(sc, ZY7_SLCR_FPGA_CLK_CTRL(unit), reg);
+
+	/* Lock SLCR registers. */
+	zy7_slcr_lock(sc);
+
+	ZSLCR_UNLOCK(sc);
+
+	return (base_frequency / div0 / div1);
+}
+
+int
+zy7_pl_fclk_get_freq(int unit)
+{
+	struct zy7_slcr_softc *sc = zy7_slcr_softc_p;
+	int div0, div1;
+	int base_frequency;
+	int frequency;
+	uint32_t reg;
+	int source;
+
+	if (!sc)
+		return (-1);
+
+	source = zy7_pl_fclk_get_source(unit);
+	switch (source) {
+		case ZY7_PL_FCLK_SRC_IO:
+			base_frequency = io_pll_frequency;
+			break;
+
+		case ZY7_PL_FCLK_SRC_ARM:
+			base_frequency = arm_pll_frequency;
+			break;
+
+		case ZY7_PL_FCLK_SRC_DDR:
+			base_frequency = ddr_pll_frequency;
+			break;
+
+		default:
+			return (-1);
+	}
+
+	ZSLCR_LOCK(sc);
+
+	/* Modify FPGAx reference clock. */
+	reg = RD4(sc, ZY7_SLCR_FPGA_CLK_CTRL(unit));
+	div1 = (reg & ZY7_SLCR_FPGA_CLK_CTRL_DIVISOR1_MASK) >>
+	    ZY7_SLCR_FPGA_CLK_CTRL_DIVISOR1_SHIFT;
+	div0 = (reg & ZY7_SLCR_FPGA_CLK_CTRL_DIVISOR0_MASK) >>
+	    ZY7_SLCR_FPGA_CLK_CTRL_DIVISOR0_SHIFT;
+
+	ZSLCR_UNLOCK(sc);
+
+	if (div0 == 0)
+		div0 = 1;
+
+	if (div1 == 0)
+		div1 = 1;
+
+	frequency = (base_frequency / div0 / div1);
+	/* Round to KHz */
+	frequency = (frequency + 500) / 1000;
+	frequency = frequency * 1000;
+
+	return (frequency);
+}
+
+int 
+zy7_pl_fclk_enable(int unit)
+{
+	struct zy7_slcr_softc *sc = zy7_slcr_softc_p;
+
+	if (!sc)
+		return (-1);
+
+	ZSLCR_LOCK(sc);
+
+	/* Unlock SLCR registers. */
+	zy7_slcr_unlock(sc);
+
+	WR4(sc, ZY7_SLCR_FPGA_THR_CTRL(unit), 0);
+	WR4(sc, ZY7_SLCR_FPGA_THR_CNT(unit), 0);
+
+	/* Lock SLCR registers. */
+	zy7_slcr_lock(sc);
+
+	ZSLCR_UNLOCK(sc);
+
+	return (0);
+}
+
+int 
+zy7_pl_fclk_disable(int unit)
+{
+	struct zy7_slcr_softc *sc = zy7_slcr_softc_p;
+
+	if (!sc)
+		return (-1);
+
+	ZSLCR_LOCK(sc);
+
+	/* Unlock SLCR registers. */
+	zy7_slcr_unlock(sc);
+
+	WR4(sc, ZY7_SLCR_FPGA_THR_CTRL(unit), 0);
+	WR4(sc, ZY7_SLCR_FPGA_THR_CNT(unit), 1);
+
+	/* Lock SLCR registers. */
+	zy7_slcr_lock(sc);
+
+	ZSLCR_UNLOCK(sc);
+
+	return (0);
+}
+
+int 
+zy7_pl_fclk_enabled(int unit)
+{
+	struct zy7_slcr_softc *sc = zy7_slcr_softc_p;
+	uint32_t reg;
+
+	if (!sc)
+		return (-1);
+
+	ZSLCR_LOCK(sc);
+	reg = RD4(sc, ZY7_SLCR_FPGA_THR_CNT(unit));
+	ZSLCR_UNLOCK(sc);
+
+	return !(reg & 1);
+}
+
+int
+zy7_pl_level_shifters_enabled()
+{
+	struct zy7_slcr_softc *sc = zy7_slcr_softc_p;
+
+	uint32_t reg;
+
+	if (!sc)
+		return (-1);
+
+	ZSLCR_LOCK(sc);
+	reg = RD4(sc, ZY7_SLCR_LVL_SHFTR_EN);
+	ZSLCR_UNLOCK(sc);
+
+	return (reg == ZY7_SLCR_LVL_SHFTR_EN_ALL);
+}
+
+void
+zy7_pl_level_shifters_enable()
+{
+	struct zy7_slcr_softc *sc = zy7_slcr_softc_p;
+
+	if (!sc)
+		return;
+
+	ZSLCR_LOCK(sc);
+	zy7_slcr_unlock(sc);
+	WR4(sc, ZY7_SLCR_LVL_SHFTR_EN, ZY7_SLCR_LVL_SHFTR_EN_ALL);
+	zy7_slcr_lock(sc);
+	ZSLCR_UNLOCK(sc);
+}
+
+void
+zy7_pl_level_shifters_disable()
+{
+	struct zy7_slcr_softc *sc = zy7_slcr_softc_p;
+
+	if (!sc)
+		return;
+
+	ZSLCR_LOCK(sc);
+	zy7_slcr_unlock(sc);
+	WR4(sc, ZY7_SLCR_LVL_SHFTR_EN, 0);
+	zy7_slcr_lock(sc);
+	ZSLCR_UNLOCK(sc);
+}
+
 static int
 zy7_slcr_probe(device_t dev)
 {
Index: arm/xilinx/zy7_slcr.h
===================================================================
--- arm/xilinx/zy7_slcr.h	(revision 278739)
+++ arm/xilinx/zy7_slcr.h	(working copy)
@@ -37,7 +37,6 @@
  * are in appendix B.28.
  */
 
-
 #ifndef _ZY7_SLCR_H_
 #define _ZY7_SLCR_H_
 
@@ -148,10 +147,19 @@
 #define ZY7_SLCR_DBG_CLK_CTRL		0x0164
 #define ZY7_SLCR_PCAP_CLK_CTRL		0x0168
 #define ZY7_SLCR_TOPSW_CLK_CTRL		0x016c	/* central intercnn clk ctrl */
-#define ZY7_SLCR_FPGA0_CLK_CTRL		0x0170
-#define ZY7_SLCR_FPGA1_CLK_CTRL		0x0180
-#define ZY7_SLCR_FPGA2_CLK_CTRL		0x0190
-#define ZY7_SLCR_FPGA3_CLK_CTRL		0x01a0
+#define ZY7_SLCR_FPGA_CLK_CTRL(unit)	(0x0170 + 0x10*(unit))
+#define	  ZY7_SLCR_FPGA_CLK_CTRL_DIVISOR1_SHIFT	20
+#define	  ZY7_SLCR_FPGA_CLK_CTRL_DIVISOR1_MASK	(0x3f << 20)
+#define	  ZY7_SLCR_FPGA_CLK_CTRL_DIVISOR0_SHIFT	8
+#define	  ZY7_SLCR_FPGA_CLK_CTRL_DIVISOR0_MASK	(0x3f << 8)
+#define	  ZY7_SLCR_FPGA_CLK_CTRL_DIVISOR_MAX	0x3f
+#define	  ZY7_SLCR_FPGA_CLK_CTRL_SRCSEL_SHIFT	4
+#define	  ZY7_SLCR_FPGA_CLK_CTRL_SRCSEL_MASK	(3 << 4)
+#define ZY7_SLCR_FPGA_THR_CTRL(unit)	(0x0174 + 0x10*(unit))
+#define ZY7_SLCR_FPGA_THR_CTRL_CNT_RST		(1 << 1)
+#define ZY7_SLCR_FPGA_THR_CTRL_CPU_START	(1 << 0)
+#define ZY7_SLCR_FPGA_THR_CNT(unit)	(0x0178 + 0x10*(unit))
+#define ZY7_SLCR_FPGA_THR_STA(unit)	(0x017c + 0x10*(unit))
 #define ZY7_SLCR_CLK_621_TRUE		0x01c4	/* cpu clock ratio mode */
 
 /* Reset controls. */
@@ -288,5 +296,23 @@
 extern void zy7_slcr_preload_pl(void);
 extern void zy7_slcr_postload_pl(int en_level_shifters);
 extern int cgem_set_ref_clk(int unit, int frequency);
+
+/* Should be consistent with SRCSEL field of FPGAx_CLK_CTRL */
+#define	ZY7_PL_FCLK_SRC_IO	0 
+#define	ZY7_PL_FCLK_SRC_IO_ALT	1 /* ZY7_PL_FCLK_SRC_IO is b0x */
+#define	ZY7_PL_FCLK_SRC_ARM	2
+#define	ZY7_PL_FCLK_SRC_DDR	3
+
+int zy7_pl_fclk_set_source(int unit, int source);
+int zy7_pl_fclk_get_source(int unit);
+int zy7_pl_fclk_set_freq(int unit, int freq);
+int zy7_pl_fclk_get_freq(int unit);
+int zy7_pl_fclk_enable(int unit);
+int zy7_pl_fclk_disable(int unit);
+int zy7_pl_fclk_enabled(int unit);
+int zy7_pl_level_shifters_enabled(void);
+void zy7_pl_level_shifters_enable(void);
+void zy7_pl_level_shifters_disable(void);
+
 #endif
 #endif /* _ZY7_SLCR_H_ */