From 46dec2e356b14245e929050c1768dc109e8abc64 Mon Sep 17 00:00:00 2001
From: ian <ian@FreeBSD.org>
Date: Fri, 23 Jan 2015 16:05:47 +0000
Subject: [PATCH] Add pinctrl driver support for the encoded input register
 config words that the linux guys made up on the fly (but didn't document)
 last August. This type of encoded config now appears in the imx6 fdt data.

---
 sys/arm/freescale/imx/imx_iomux.c | 31 ++++++++++++++++++++++++++++---
 1 file changed, 28 insertions(+), 3 deletions(-)

diff --git a/sys/arm/freescale/imx/imx_iomux.c b/sys/arm/freescale/imx/imx_iomux.c
index 7786b76e0a4f..d1ac6cbc2657 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) {