From 213e1ad5fb5a5e230dfc42b12a7ffc846cab2850 Mon Sep 17 00:00:00 2001
From: ian <ian@ccf9f872-aa2e-dd11-9fc8-001c23d0bc1f>
Date: Fri, 13 Feb 2015 23:34:40 +0000
Subject: [PATCH] MFC r277555, r277568:

  Enable all sd device clocks on imx6.

  Add imx5/6 pinmux driver support for encoded input register configs.


git-svn-id: svn://svn.freebsd.org/base/stable/10@278732 ccf9f872-aa2e-dd11-9fc8-001c23d0bc1f
---
 sys/arm/freescale/imx/imx6_ccm.c  |  2 +-
 sys/arm/freescale/imx/imx_iomux.c | 31 ++++++++++++++++++++++++++++---
 2 files changed, 29 insertions(+), 4 deletions(-)

diff --git a/sys/arm/freescale/imx/imx6_ccm.c b/sys/arm/freescale/imx/imx6_ccm.c
index 769a84108..2c80bd9ee 100644
--- a/sys/arm/freescale/imx/imx6_ccm.c
+++ b/sys/arm/freescale/imx/imx6_ccm.c
@@ -95,7 +95,7 @@ ccm_init_gates(struct ccm_softc *sc)
 	WR4(sc, CCM_CCGR3, 0x3ff00000); /* DDR memory controller */
 	WR4(sc, CCM_CCGR4, 0x0000f300); /* pl301 bus crossbar */
 	WR4(sc, CCM_CCGR5, 0x0f000000); /* uarts */
-	WR4(sc, CCM_CCGR6, 0x000000cc); /* usdhc 1 & 3 */
+	WR4(sc, CCM_CCGR6, 0x000000ff); /* usdhc 1-4 */
 }
 
 static int
diff --git a/sys/arm/freescale/imx/imx_iomux.c b/sys/arm/freescale/imx/imx_iomux.c
index 7786b76e0..d1ac6cbc2 100644
--- a/sys/arm/freescale/imx/imx_iomux.c
+++ b/sys/arm/freescale/imx/imx_iomux.c
@@ -117,10 +117,36 @@ WR4(struct iomux_softc *sc, bus_size_t off, uint32_t val)
 	bus_write_4(sc->mem_res, off, val);
 }
 
+static void
+iomux_configure_input(struct iomux_softc *sc, uint32_t reg, uint32_t val)
+{
+	u_int select, mask, shift, width;
+
+	/* If register and value are zero, there is nothing to configure. */
+	if (reg == 0 && val == 0)
+		return;
+
+	/*
+	 * If the config value has 0xff in the high byte it is encoded:
+	 * 	31     23      15      7        0
+	 *      | 0xff | shift | width | select |
+	 * We need to mask out the old select value and OR in the new, using a
+	 * mask of the given width and shifting the values up by shift.
+	 */
+	if ((val & 0xff000000) == 0xff000000) {
+		select = val & 0x000000ff;
+		width = (val & 0x0000ff00) >> 8;
+		shift = (val & 0x00ff0000) >> 16;
+		mask  = ((1u << width) - 1) << shift;
+		val = (RD4(sc, reg) & ~mask) | (select << shift);
+	}
+	WR4(sc, reg, val);
+}
+
 static int
 iomux_configure_pins(device_t dev, phandle_t cfgxref)
 {
-	struct iomux_softc * sc;
+	struct iomux_softc *sc;
 	struct pincfg *cfgtuples, *cfg;
 	phandle_t cfgnode;
 	int i, ntuples;
@@ -137,8 +163,7 @@ iomux_configure_pins(device_t dev, phandle_t cfgxref)
 	for (i = 0, cfg = cfgtuples; i < ntuples; i++, cfg++) {
 		sion = (cfg->padconf_val & PADCONF_SION) ? PADMUX_SION : 0;
 		WR4(sc, cfg->mux_reg, cfg->mux_val | sion);
-		if (cfg->input_reg != 0)
-			WR4(sc, cfg->input_reg, cfg->input_val);
+		iomux_configure_input(sc, cfg->input_reg, cfg->input_val);
 		if ((cfg->padconf_val & PADCONF_NONE) == 0)
 			WR4(sc, cfg->padconf_reg, cfg->padconf_val);
 		if (bootverbose) {
-- 
2.45.0