git: d27ba3088424 - main - qcom_qup: add initial v1/v2 QUP SPI driver

From: Adrian Chadd <adrian_at_FreeBSD.org>
Date: Mon, 27 Dec 2021 23:35:59 UTC
The branch main has been updated by adrian:

URL: https://cgit.FreeBSD.org/src/commit/?id=d27ba3088424e53eabc0b0186ed122ec43119501

commit d27ba3088424e53eabc0b0186ed122ec43119501
Author:     Adrian Chadd <adrian@FreeBSD.org>
AuthorDate: 2021-12-27 23:27:29 +0000
Commit:     Adrian Chadd <adrian@FreeBSD.org>
CommitDate: 2021-12-27 23:27:29 +0000

    qcom_qup: add initial v1/v2 QUP SPI driver
    
    The Qualcomm Universal Peripherals Engine (QUP) is a unified SPI and I2C
    peripheral that ships with a variety of Qualcomm SoCs.
    
    It supports three transfer modes - single PIO, block PIO and DMA.
    
    This driver only supports the single PIO mode, which is enough to
    bootstrap the rest of the SPI NAND/NOR support and means I can do
    things like read the Wifi calibration data from NOR.  It has some
    hardware support code for the other transfer modes as well as
    some support for split transfers (ie, transfers with no read or
    write phase), but I haven't yet implemented those.
    
    This driver is based on four sources - the linux driver, the u-boot
    driver, some initial work done for APQ8064 by mmel@, and the APQ8064
    Technical Reference Manual which is surprisingly free and open to
    read.  The linux and u-boot drivers approach a variety of things
    completely differently, from how PIO is done, the hardware support
    for re-ordering bytes in a transfer word and how the CS lines
    are used.
    
    Tested:
    
    * IPQ4018, SPI to NAND/NOR flash, PIO only
---
 sys/arm/conf/std.qca              |   5 +
 sys/arm/qualcomm/std.ipq4018      |   3 +
 sys/dev/qcom_qup/qcom_qup_reg.h   | 115 +++++
 sys/dev/qcom_qup/qcom_spi.c       | 911 +++++++++++++++++++++++++++++++++++
 sys/dev/qcom_qup/qcom_spi_debug.h |  49 ++
 sys/dev/qcom_qup/qcom_spi_hw.c    | 985 ++++++++++++++++++++++++++++++++++++++
 sys/dev/qcom_qup/qcom_spi_reg.h   |  76 +++
 sys/dev/qcom_qup/qcom_spi_var.h   | 162 +++++++
 8 files changed, 2306 insertions(+)

diff --git a/sys/arm/conf/std.qca b/sys/arm/conf/std.qca
index a78e27074ea4..7caae2f645ca 100644
--- a/sys/arm/conf/std.qca
+++ b/sys/arm/conf/std.qca
@@ -24,6 +24,11 @@ device 		syscon
 # Random
 device 		qcom_rnd
 
+# SPI
+device 		spibus
+device 		qcom_qup_spi
+device 		mx25l
+
 # interrupt controller
 device 		gic
 
diff --git a/sys/arm/qualcomm/std.ipq4018 b/sys/arm/qualcomm/std.ipq4018
index 8a9dba579e48..2187e3f17516 100644
--- a/sys/arm/qualcomm/std.ipq4018
+++ b/sys/arm/qualcomm/std.ipq4018
@@ -5,6 +5,9 @@ arm/qualcomm/qcom_cpu_kpssv2.c		optional smp
 
 dev/qcom_rnd/qcom_rnd.c			optional qcom_rnd
 
+dev/qcom_qup/qcom_spi.c			optional qcom_qup_spi
+dev/qcom_qup/qcom_spi_hw.c		optional qcom_qup_spi
+
 dev/qcom_gcc/qcom_gcc_ipq4018.c		optional qcom_gcc_ipq4018
 dev/qcom_gcc/qcom_gcc_ipq4018_reset.c	optional qcom_gcc_ipq4018
 dev/qcom_gcc/qcom_gcc_ipq4018_clock.c	optional qcom_gcc_ipq4018
diff --git a/sys/dev/qcom_qup/qcom_qup_reg.h b/sys/dev/qcom_qup/qcom_qup_reg.h
new file mode 100644
index 000000000000..18931e560959
--- /dev/null
+++ b/sys/dev/qcom_qup/qcom_qup_reg.h
@@ -0,0 +1,115 @@
+/*-
+ * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
+ *
+ * Copyright (c) 2021 Adrian Chadd <adrian@FreeBSD.org>
+ *
+ * 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$
+ */
+
+#ifndef	__QCOM_QUP_REG_H__
+#define	__QCOM_QUP_REG_H__
+
+#define	QUP_CONFIG			0x0000
+#define		QUP_CONFIG_N			0x001f
+#define		QUP_CONFIG_SPI_MODE		(1U << 8)
+#define		QUP_CONFIG_MINI_CORE_I2C_MASTER	(2U << 8)
+#define		QUP_CONFIG_MINI_CORE_I2C_SLAVE	(3U << 8)
+
+#define		QUP_CONFIG_NO_OUTPUT		(1U << 6)
+#define		QUP_CONFIG_NO_INPUT		(1U << 7)
+#define		QUP_CONFIG_APP_CLK_ON_EN	(1 << 12)
+#define		QUP_CONFIG_CLOCK_AUTO_GATE	(1U << 13)
+
+#define	QUP_STATE			0x0004
+#define		QUP_STATE_VALID		(1U << 2)
+#define		QUP_STATE_RESET		0
+#define		QUP_STATE_RUN		1
+#define		QUP_STATE_PAUSE		3
+#define		QUP_STATE_MASK		3
+#define		QUP_STATE_CLEAR		2
+
+#define	QUP_IO_M_MODES			0x0008
+#define		QUP_IO_M_OUTPUT_BLOCK_SIZE_MASK		0x3
+#define		QUP_IO_M_OUTPUT_BLOCK_SIZE_SHIFT	0
+
+#define		QUP_IO_M_OUTPUT_FIFO_SIZE_MASK		0x7
+#define		QUP_IO_M_OUTPUT_FIFO_SIZE_SHIFT		2
+
+#define		QUP_IO_M_INPUT_BLOCK_SIZE_MASK		0x3
+#define		QUP_IO_M_INPUT_BLOCK_SIZE_SHIFT		5
+
+#define		QUP_IO_M_INPUT_FIFO_SIZE_MASK		0x7
+#define		QUP_IO_M_INPUT_FIFO_SIZE_SHIFT		7
+
+#define		QUP_IO_M_PACK_EN		(1U << 15)
+#define		QUP_IO_M_UNPACK_EN		(1U << 14)
+#define		QUP_IO_M_INPUT_MODE_SHIFT	12
+#define		QUP_IO_M_OUTPUT_MODE_SHIFT	10
+#define		QUP_IO_M_INPUT_MODE_MASK	0x3
+#define		QUP_IO_M_OUTPUT_MODE_MASK	0x3
+
+#define		QUP_IO_M_MODE_FIFO		0
+#define		QUP_IO_M_MODE_BLOCK		1
+#define		QUP_IO_M_MODE_DMOV		2
+#define		QUP_IO_M_MODE_BAM		3
+
+#define	QUP_SW_RESET			0x000c
+
+#define	QUP_OPERATIONAL			0x0018
+#define		QUP_OP_IN_BLOCK_READ_REQ	(1U << 13)
+#define		QUP_OP_OUT_BLOCK_WRITE_REQ	(1U << 12)
+#define		QUP_OP_MAX_INPUT_DONE_FLAG	(1U << 11)
+#define		QUP_OP_MAX_OUTPUT_DONE_FLAG	(1U << 10)
+#define		QUP_OP_IN_SERVICE_FLAG		(1U << 9)
+#define		QUP_OP_OUT_SERVICE_FLAG		(1U << 8)
+#define		QUP_OP_IN_FIFO_FULL		(1U << 7)
+#define		QUP_OP_OUT_FIFO_FULL		(1U << 6)
+#define		QUP_OP_IN_FIFO_NOT_EMPTY	(1U << 5)
+#define		QUP_OP_OUT_FIFO_NOT_EMPTY	(1U << 4)
+
+#define	QUP_ERROR_FLAGS			0x001c
+#define	QUP_ERROR_FLAGS_EN		0x0020
+#define		QUP_ERROR_OUTPUT_OVER_RUN	(1U << 5)
+#define		QUP_ERROR_INPUT_UNDER_RUN	(1U << 4)
+#define		QUP_ERROR_OUTPUT_UNDER_RUN	(1U << 3)
+#define		QUP_ERROR_INPUT_OVER_RUN	(1U << 2)
+
+#define	QUP_OPERATIONAL_MASK		0x0028
+
+#define	QUP_HW_VERSION			0x0030
+#define		QUP_HW_VERSION_2_1_1		0x20010001
+
+#define	QUP_MX_OUTPUT_CNT		0x0100
+#define	QUP_MX_OUTPUT_CNT_CURRENT	0x0104
+#define	QUP_OUTPUT_FIFO			0x0110
+#define	QUP_MX_WRITE_CNT		0x0150
+#define	QUP_MX_WRITE_CNT_CURRENT	0x0154
+#define	QUP_MX_INPUT_CNT		0x0200
+#define	QUP_MX_INPUT_CNT_CURRENT	0x0204
+#define	QUP_MX_READ_CNT			0x0208
+#define	QUP_MX_READ_CNT_CURRENT		0x020c
+#define	QUP_INPUT_FIFO			0x0218
+
+#endif	/* __QCOM_QUP_REG_H__ */
+
diff --git a/sys/dev/qcom_qup/qcom_spi.c b/sys/dev/qcom_qup/qcom_spi.c
new file mode 100644
index 000000000000..e16a03db46dc
--- /dev/null
+++ b/sys/dev/qcom_qup/qcom_spi.c
@@ -0,0 +1,911 @@
+/*-
+ * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
+ *
+ * Copyright (c) 2021, Adrian Chadd <adrian@FreeBSD.org>
+ *
+ * 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 unmodified, 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.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/systm.h>
+
+#include <sys/bus.h>
+#include <sys/interrupt.h>
+#include <sys/malloc.h>
+#include <sys/lock.h>
+#include <sys/mutex.h>
+#include <sys/kernel.h>
+#include <sys/module.h>
+#include <sys/rman.h>
+#include <sys/gpio.h>
+
+#include <vm/vm.h>
+#include <vm/pmap.h>
+#include <vm/vm_extern.h>
+
+#include <machine/bus.h>
+#include <machine/cpu.h>
+
+#include <dev/fdt/fdt_common.h>
+#include <dev/fdt/fdt_pinctrl.h>
+
+#include <dev/gpio/gpiobusvar.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include <dev/extres/clk/clk.h>
+#include <dev/extres/hwreset/hwreset.h>
+
+#include <dev/spibus/spi.h>
+#include <dev/spibus/spibusvar.h>
+#include "spibus_if.h"
+
+#include <dev/qcom_qup/qcom_spi_var.h>
+#include <dev/qcom_qup/qcom_qup_reg.h>
+#include <dev/qcom_qup/qcom_spi_reg.h>
+#include <dev/qcom_qup/qcom_spi_debug.h>
+
+static struct ofw_compat_data compat_data[] = {
+	{ "qcom,spi-qup-v1.1.1",	QCOM_SPI_HW_QPI_V1_1 },
+	{ "qcom,spi-qup-v2.1.1",	QCOM_SPI_HW_QPI_V2_1 },
+	{ "qcom,spi-qup-v2.2.1",	QCOM_SPI_HW_QPI_V2_2 },
+	{ NULL,				0 }
+};
+
+/*
+ * Flip the CS GPIO line either active or inactive.
+ *
+ * Actually listen to the CS polarity.
+ */
+static void
+qcom_spi_set_chipsel(struct qcom_spi_softc *sc, int cs, bool active)
+{
+	bool pinactive;
+	bool invert = !! (cs & SPIBUS_CS_HIGH);
+
+	cs = cs & ~SPIBUS_CS_HIGH;
+
+	if (sc->cs_pins[cs] == NULL) {
+		device_printf(sc->sc_dev,
+		    "%s: cs=%u, active=%u, invert=%u, no gpio?\n",
+		    __func__, cs, active, invert);
+		return;
+	}
+
+	QCOM_SPI_DPRINTF(sc, QCOM_SPI_DEBUG_CHIPSELECT,
+	    "%s: cs=%u active=%u\n", __func__, cs, active);
+
+	/*
+	 * Default rule here is CS is active low.
+	 */
+	if (active)
+		pinactive = false;
+	else
+		pinactive = true;
+
+	/*
+	 * Invert the CS line if required.
+	 */
+	if (invert)
+		pinactive = !! pinactive;
+
+	gpio_pin_set_active(sc->cs_pins[cs], pinactive);
+	gpio_pin_is_active(sc->cs_pins[cs], &pinactive);
+}
+
+static void
+qcom_spi_intr(void *arg)
+{
+	struct qcom_spi_softc *sc = arg;
+	int ret;
+
+	QCOM_SPI_DPRINTF(sc, QCOM_SPI_DEBUG_INTR, "%s: called\n", __func__);
+
+
+	QCOM_SPI_LOCK(sc);
+	ret = qcom_spi_hw_interrupt_handle(sc);
+	if (ret != 0) {
+		device_printf(sc->sc_dev,
+		    "ERROR: failed to read intr status\n");
+		goto done;
+	}
+
+	/*
+	 * Handle spurious interrupts outside of an actual
+	 * transfer.
+	 */
+	if (sc->transfer.active == false) {
+		device_printf(sc->sc_dev,
+		    "ERROR: spurious interrupt\n");
+		qcom_spi_hw_ack_opmode(sc);
+		goto done;
+	}
+
+	/* Now, handle interrupts */
+	if (sc->intr.error) {
+		sc->intr.error = false;
+		device_printf(sc->sc_dev, "ERROR: intr\n");
+	}
+
+	if (sc->intr.do_rx) {
+		sc->intr.do_rx = false;
+		QCOM_SPI_DPRINTF(sc, QCOM_SPI_DEBUG_INTR,
+		    "%s: PIO_READ\n", __func__);
+		if (sc->state.transfer_mode == QUP_IO_M_MODE_FIFO)
+			ret = qcom_spi_hw_read_pio_fifo(sc);
+		else
+			ret = qcom_spi_hw_read_pio_block(sc);
+		if (ret != 0) {
+			device_printf(sc->sc_dev,
+			    "ERROR: qcom_spi_hw_read failed (%u)\n", ret);
+			goto done;
+		}
+	}
+	if (sc->intr.do_tx) {
+		sc->intr.do_tx = false;
+		QCOM_SPI_DPRINTF(sc, QCOM_SPI_DEBUG_INTR,
+		    "%s: PIO_WRITE\n", __func__);
+		/*
+		 * For FIFO operations we do not do a write here, we did
+		 * it at the beginning of the transfer.
+		 *
+		 * For BLOCK operations yes, we call the routine.
+		 */
+
+		if (sc->state.transfer_mode == QUP_IO_M_MODE_FIFO)
+			ret = qcom_spi_hw_ack_write_pio_fifo(sc);
+		else
+			ret = qcom_spi_hw_write_pio_block(sc);
+		if (ret != 0) {
+			device_printf(sc->sc_dev,
+			    "ERROR: qcom_spi_hw_write failed (%u)\n", ret);
+			goto done;
+		}
+	}
+
+	/*
+	 * Do this last.  We may actually have completed the
+	 * transfer in the PIO receive path above and it will
+	 * set the done flag here.
+	 */
+	if (sc->intr.done) {
+		sc->intr.done = false;
+		sc->transfer.done = true;
+		QCOM_SPI_DPRINTF(sc, QCOM_SPI_DEBUG_INTR,
+		    "%s: transfer done\n", __func__);
+		wakeup(sc);
+	}
+
+done:
+	QCOM_SPI_DPRINTF(sc, QCOM_SPI_DEBUG_INTR,
+	    "%s: done\n", __func__);
+	QCOM_SPI_UNLOCK(sc);
+}
+
+static int
+qcom_spi_probe(device_t dev)
+{
+
+	if (!ofw_bus_status_okay(dev))
+		return (ENXIO);
+
+	if (!ofw_bus_search_compatible(dev, compat_data)->ocd_data)
+		return (ENXIO);
+
+	device_set_desc(dev, "Qualcomm SPI Interface");
+	return (BUS_PROBE_DEFAULT);
+}
+
+/*
+ * Allocate GPIOs if provided in the SPI controller block.
+ *
+ * Some devices will use GPIO lines for chip select.
+ * It's also quite annoying because some devices will want to use
+ * the hardware provided CS gating for say, the first chipselect block,
+ * and then use GPIOs for the later ones.
+ *
+ * So here we just assume for now that SPI index 0 uses the hardware
+ * lines, and >0 use GPIO lines.  Revisit this if better hardware
+ * shows up.
+ *
+ * And finally, iterating over the cs-gpios list to allocate GPIOs
+ * doesn't actually tell us what the polarity is.  For that we need
+ * to actually iterate over the list of child nodes and check what
+ * their properties are (and look for "spi-cs-high".)
+ */
+static void
+qcom_spi_attach_gpios(struct qcom_spi_softc *sc)
+{
+	phandle_t node;
+	int idx, err;
+
+	/* Allocate gpio pins for configured chip selects. */
+	node = ofw_bus_get_node(sc->sc_dev);
+	for (idx = 0; idx < nitems(sc->cs_pins); idx++) {
+		err = gpio_pin_get_by_ofw_propidx(sc->sc_dev, node,
+		    "cs-gpios", idx, &sc->cs_pins[idx]);
+		if (err == 0) {
+			err = gpio_pin_setflags(sc->cs_pins[idx],
+			    GPIO_PIN_OUTPUT);
+			if (err != 0) {
+				device_printf(sc->sc_dev,
+				    "error configuring gpio for"
+				    " cs %u (%d)\n", idx, err);
+			}
+			/*
+			 * We can't set this HIGH right now because
+			 * we don't know if it needs to be set to
+			 * high for inactive or low for inactive
+			 * based on the child SPI device flags.
+			 */
+#if 0
+			gpio_pin_set_active(sc->cs_pins[idx], 1);
+			gpio_pin_is_active(sc->cs_pins[idx], &tmp);
+#endif
+		} else {
+			device_printf(sc->sc_dev,
+			    "cannot configure gpio for chip select %u\n", idx);
+			sc->cs_pins[idx] = NULL;
+		}
+	}
+}
+
+static void
+qcom_spi_sysctl_attach(struct qcom_spi_softc *sc)
+{
+	struct sysctl_ctx_list *ctx = device_get_sysctl_ctx(sc->sc_dev);
+	struct sysctl_oid *tree = device_get_sysctl_tree(sc->sc_dev);
+
+	SYSCTL_ADD_UINT(ctx, SYSCTL_CHILDREN(tree), OID_AUTO,
+	    "debug", CTLFLAG_RW, &sc->sc_debug, 0,
+	    "control debugging printfs");
+}
+
+static int
+qcom_spi_attach(device_t dev)
+{
+	struct qcom_spi_softc *sc = device_get_softc(dev);
+	int rid, ret, i, val;
+
+	sc->sc_dev = dev;
+
+	/*
+	 * Hardware version is stored in the ofw_compat_data table.
+	 */
+	sc->hw_version =
+	    ofw_bus_search_compatible(dev, compat_data)->ocd_data;
+
+	mtx_init(&sc->sc_mtx, device_get_nameunit(dev), NULL, MTX_DEF);
+
+	rid = 0;
+	sc->sc_mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid,
+	    RF_ACTIVE);
+	if (!sc->sc_mem_res) {
+		device_printf(dev, "ERROR: Could not map memory\n");
+		ret = ENXIO;
+		goto error;
+	}
+
+	rid = 0;
+	sc->sc_irq_res = bus_alloc_resource_any(dev, SYS_RES_IRQ, &rid,
+	    RF_ACTIVE | RF_SHAREABLE);
+	if (!sc->sc_irq_res) {
+		device_printf(dev, "ERROR: Could not map interrupt\n");
+		ret = ENXIO;
+		goto error;
+	}
+
+	ret = bus_setup_intr(dev, sc->sc_irq_res,
+	    INTR_TYPE_MISC | INTR_MPSAFE,
+	    NULL, qcom_spi_intr, sc, &sc->sc_irq_h);
+	if (ret != 0) {
+		device_printf(dev, "ERROR: could not configure interrupt "
+		    "(%d)\n",
+		    ret);
+		goto error;
+	}
+
+	qcom_spi_attach_gpios(sc);
+
+	ret = clk_get_by_ofw_name(dev, 0, "core", &sc->clk_core);
+	if (ret != 0) {
+		device_printf(dev, "ERROR: could not get %s clock (%d)\n",
+		    "core", ret);
+		goto error;
+	}
+	ret = clk_get_by_ofw_name(dev, 0, "iface", &sc->clk_iface);
+	if (ret != 0) {
+		device_printf(dev, "ERROR: could not get %s clock (%d)\n",
+		    "iface", ret);
+		goto error;
+	}
+
+	/* Bring up initial clocks if they're off */
+	ret = clk_enable(sc->clk_core);
+	if (ret != 0) {
+		device_printf(dev, "ERROR: couldn't enable core clock (%u)\n",
+		    ret);
+		goto error;
+	}
+	ret = clk_enable(sc->clk_iface);
+	if (ret != 0) {
+		device_printf(dev, "ERROR: couldn't enable iface clock (%u)\n",
+		    ret);
+		goto error;
+	}
+
+	/*
+	 * Read optional spi-max-frequency
+	 */
+	if (OF_getencprop(ofw_bus_get_node(dev), "spi-max-frequency",
+	    &val, sizeof(val)) > 0)
+		sc->config.max_frequency = val;
+	else
+		sc->config.max_frequency = SPI_MAX_RATE;
+
+	/*
+	 * Read optional cs-select
+	 */
+	if (OF_getencprop(ofw_bus_get_node(dev), "cs-select",
+	    &val, sizeof(val)) > 0)
+		sc->config.cs_select = val;
+	else
+		sc->config.cs_select = 0;
+
+	/*
+	 * Read optional num-cs
+	 */
+	if (OF_getencprop(ofw_bus_get_node(dev), "num-cs",
+	    &val, sizeof(val)) > 0)
+		sc->config.num_cs = val;
+	else
+		sc->config.num_cs = SPI_NUM_CHIPSELECTS;
+
+	ret = fdt_pinctrl_configure_by_name(dev, "default");
+	if (ret != 0) {
+		device_printf(dev,
+		    "ERROR: could not configure default pinmux\n");
+		goto error;
+	}
+
+	ret = qcom_spi_hw_read_controller_transfer_sizes(sc);
+	if (ret != 0) {
+		device_printf(dev, "ERROR: Could not read transfer config\n");
+		goto error;
+	}
+
+
+	device_printf(dev, "BLOCK: input=%u bytes, output=%u bytes\n",
+	    sc->config.input_block_size,
+	    sc->config.output_block_size);
+	device_printf(dev, "FIFO: input=%u bytes, output=%u bytes\n",
+	    sc->config.input_fifo_size,
+	    sc->config.output_fifo_size);
+
+	/* QUP config */
+	QCOM_SPI_LOCK(sc);
+	ret = qcom_spi_hw_qup_init_locked(sc);
+	if (ret != 0) {
+		device_printf(dev, "ERROR: QUP init failed (%d)\n", ret);
+		QCOM_SPI_UNLOCK(sc);
+		goto error;
+	}
+
+	/* Initial SPI config */
+	ret = qcom_spi_hw_spi_init_locked(sc);
+	if (ret != 0) {
+		device_printf(dev, "ERROR: SPI init failed (%d)\n", ret);
+		QCOM_SPI_UNLOCK(sc);
+		goto error;
+	}
+	QCOM_SPI_UNLOCK(sc);
+
+	sc->spibus = device_add_child(dev, "spibus", -1);
+
+	/* We're done, so shut down the interface clock for now */
+	device_printf(dev, "DONE: shutting down interface clock for now\n");
+	clk_disable(sc->clk_iface);
+
+	/* Register for debug sysctl */
+	qcom_spi_sysctl_attach(sc);
+
+	return (bus_generic_attach(dev));
+error:
+	if (sc->sc_irq_h)
+		bus_teardown_intr(dev, sc->sc_irq_res, sc->sc_irq_h);
+	if (sc->sc_mem_res)
+		bus_release_resource(dev, SYS_RES_MEMORY, 0, sc->sc_mem_res);
+	if (sc->sc_irq_res)
+		bus_release_resource(dev, SYS_RES_IRQ, 0, sc->sc_irq_res);
+	if (sc->clk_core) {
+		clk_disable(sc->clk_core);
+		clk_release(sc->clk_core);
+	}
+	if (sc->clk_iface) {
+		clk_disable(sc->clk_iface);
+		clk_release(sc->clk_iface);
+	}
+	for (i = 0; i < CS_MAX; i++) {
+		if (sc->cs_pins[i] != NULL)
+			gpio_pin_release(sc->cs_pins[i]);
+	}
+	mtx_destroy(&sc->sc_mtx);
+	return (ret);
+}
+
+/*
+ * Do a PIO transfer.
+ *
+ * Note that right now the TX/RX lens need to match, I'm not doing
+ * dummy reads / dummy writes as required if they're not the same
+ * size.  The QUP hardware supports doing multi-phase transactions
+ * where the FIFO isn't engaged for transmit or receive, but it's
+ * not yet being done here.
+ */
+static int
+qcom_spi_transfer_pio_block(struct qcom_spi_softc *sc, int mode,
+    char *tx_buf, int tx_len, char *rx_buf, int rx_len)
+{
+	int ret = 0;
+
+	QCOM_SPI_DPRINTF(sc, QCOM_SPI_DEBUG_TRANSFER, "%s: start\n",
+	    __func__);
+
+	if (rx_len != tx_len) {
+		device_printf(sc->sc_dev,
+		    "ERROR: tx/rx len doesn't match (%d/%d)\n",
+		    tx_len, rx_len);
+		return (ENXIO);
+	}
+
+	QCOM_SPI_ASSERT_LOCKED(sc);
+
+	/*
+	 * Make initial choices for transfer configuration.
+	 */
+	ret = qcom_spi_hw_setup_transfer_selection(sc, tx_len);
+	if (ret != 0) {
+		device_printf(sc->sc_dev,
+		    "ERROR: failed to setup transfer selection (%d)\n",
+		    ret);
+		return (ret);
+	}
+
+	/* Now set suitable buffer/lengths */
+	sc->transfer.tx_buf = tx_buf;
+	sc->transfer.tx_len = tx_len;
+	sc->transfer.rx_buf = rx_buf;
+	sc->transfer.rx_len = rx_len;
+	sc->transfer.done = false;
+	sc->transfer.active = false;
+
+	/*
+	 * Loop until the full transfer set is done.
+	 *
+	 * qcom_spi_hw_setup_current_transfer() will take care of
+	 * setting a maximum transfer size for the hardware and choose
+	 * a suitable operating mode.
+	 */
+	while (sc->transfer.tx_offset < sc->transfer.tx_len) {
+		/*
+		 * Set transfer to false early; this covers
+		 * it also finishing a sub-transfer and we're
+		 * about the put the block into RESET state before
+		 * starting a new transfer.
+		 */
+		sc->transfer.active = false;
+
+		QCOM_SPI_DPRINTF(sc, QCOM_SPI_DEBUG_TRANSFER,
+		    "%s: tx=%d of %d bytes, rx=%d of %d bytes\n",
+		    __func__,
+		    sc->transfer.tx_offset, sc->transfer.tx_len,
+		    sc->transfer.rx_offset, sc->transfer.rx_len);
+
+		/*
+		 * Set state to RESET before doing anything.
+		 *
+		 * Otherwise the second sub-transfer that we queue up
+		 * will generate interrupts immediately when we start
+		 * configuring it here and it'll start underflowing.
+		 */
+		ret = qcom_spi_hw_qup_set_state_locked(sc, QUP_STATE_RESET);
+		if (ret != 0) {
+			device_printf(sc->sc_dev,
+			    "ERROR: can't transition to RESET (%u)\n", ret);
+			goto done;
+		}
+		/* blank interrupt state; we'll do a RESET below */
+		bzero(&sc->intr, sizeof(sc->intr));
+		sc->transfer.done = false;
+
+		/*
+		 * Configure what the transfer configuration for this
+		 * sub-transfer will be.
+		 */
+		ret = qcom_spi_hw_setup_current_transfer(sc);
+		if (ret != 0) {
+			device_printf(sc->sc_dev,
+			    "ERROR: failed to setup sub transfer (%d)\n",
+			    ret);
+			goto done;
+		}
+
+		/*
+		 * For now since we're configuring up PIO, we only setup
+		 * the PIO transfer size.
+		 */
+		ret = qcom_spi_hw_setup_pio_transfer_cnt(sc);
+		if (ret != 0) {
+			device_printf(sc->sc_dev,
+			    "ERROR: qcom_spi_hw_setup_pio_transfer_cnt failed"
+			    " (%u)\n", ret);
+			goto done;
+		}
+
+#if 0
+		/*
+		 * This is what we'd do to setup the block transfer sizes.
+		 */
+		ret = qcom_spi_hw_setup_block_transfer_cnt(sc);
+		if (ret != 0) {
+			device_printf(sc->sc_dev,
+			    "ERROR: qcom_spi_hw_setup_block_transfer_cnt failed"
+			    " (%u)\n", ret);
+			goto done;
+		}
+#endif
+
+		ret = qcom_spi_hw_setup_io_modes(sc);
+		if (ret != 0) {
+			device_printf(sc->sc_dev,
+			    "ERROR: qcom_spi_hw_setup_io_modes failed"
+			    " (%u)\n", ret);
+			goto done;
+		}
+
+		ret = qcom_spi_hw_setup_spi_io_clock_polarity(sc,
+		    !! (mode & SPIBUS_MODE_CPOL));
+		if (ret != 0) {
+			device_printf(sc->sc_dev,
+			    "ERROR: qcom_spi_hw_setup_spi_io_clock_polarity"
+			    "    failed (%u)\n", ret);
+			goto done;
+		}
+
+		ret = qcom_spi_hw_setup_spi_config(sc, sc->state.frequency,
+		    !! (mode & SPIBUS_MODE_CPHA));
+		if (ret != 0) {
+			device_printf(sc->sc_dev,
+			    "ERROR: qcom_spi_hw_setup_spi_config failed"
+			    " (%u)\n", ret);
+			goto done;
+		}
+
+		ret = qcom_spi_hw_setup_qup_config(sc, !! (tx_len > 0),
+		    !! (rx_len > 0));
+		if (ret != 0) {
+			device_printf(sc->sc_dev,
+			    "ERROR: qcom_spi_hw_setup_qup_config failed"
+			    " (%u)\n", ret);
+			goto done;
+		}
+
+		ret = qcom_spi_hw_setup_operational_mask(sc);
+		if (ret != 0) {
+			device_printf(sc->sc_dev,
+			    "ERROR: qcom_spi_hw_setup_operational_mask failed"
+			    " (%u)\n", ret);
+			goto done;
+		}
+
+		/*
+		 * Setup is done; reset the controller and start the PIO
+		 * write.
+		 */
+
+		/*
+		 * Set state to RUN; we may start getting interrupts that
+		 * are valid and we need to handle.
+		 */
+		sc->transfer.active = true;
+		ret = qcom_spi_hw_qup_set_state_locked(sc, QUP_STATE_RUN);
+		if (ret != 0) {
+			device_printf(sc->sc_dev,
+			    "ERROR: can't transition to RUN (%u)\n", ret);
+			goto done;
+		}
+
+		/*
+		 * Set state to PAUSE
+		 */
+		ret = qcom_spi_hw_qup_set_state_locked(sc, QUP_STATE_PAUSE);
+		if (ret != 0) {
+			device_printf(sc->sc_dev,
+			    "ERROR: can't transition to PAUSE (%u)\n", ret);
+			goto done;
+		}
+
+		/*
+		 * If FIFO mode, write data now.  Else, we'll get an
+		 * interrupt when it's time to populate more data
+		 * in BLOCK mode.
+		 */
+		if (sc->state.transfer_mode == QUP_IO_M_MODE_FIFO)
+			ret = qcom_spi_hw_write_pio_fifo(sc);
+		else
+			ret = qcom_spi_hw_write_pio_block(sc);
+		if (ret != 0) {
+			device_printf(sc->sc_dev,
+			    "ERROR: qcom_spi_hw_write failed (%u)\n", ret);
+			goto done;
+		}
+
+		/*
+		 * Set state to RUN
+		 */
+		ret = qcom_spi_hw_qup_set_state_locked(sc, QUP_STATE_RUN);
+		if (ret != 0) {
+			device_printf(sc->sc_dev,
+			    "ERROR: can't transition to RUN (%u)\n", ret);
+			goto done;
+		}
+
+		/*
+		 * Wait for an interrupt notification (which will
+		 * continue to drive the state machine for this
+		 * sub-transfer) or timeout.
+		 */
+		ret = 0;
+		while (ret == 0 && sc->transfer.done == false) {
+			QCOM_SPI_DPRINTF(sc, QCOM_SPI_DEBUG_TRANSFER,
+			    "%s: waiting\n", __func__);
+			ret = msleep(sc, &sc->sc_mtx, 0, "qcom_spi", 0);
+		}
+	}
+done:
+	/*
+	 * Complete; put controller into reset.
+	 *
+	 * Don't worry about return value here; if we errored out above then
+	 * we want to communicate that value to the caller.
+	 */
+	(void) qcom_spi_hw_qup_set_state_locked(sc, QUP_STATE_RESET);
+	QCOM_SPI_DPRINTF(sc, QCOM_SPI_DEBUG_TRANSFER,
+	    "%s: completed\n", __func__);
+
+	 /*
+	  * Blank the transfer state so we don't use an old transfer
+	  * state in a subsequent interrupt.
+	  */
+	(void) qcom_spi_hw_complete_transfer(sc);
+	sc->transfer.active = false;
+
+	return (ret);
+}
+
+static int
+qcom_spi_transfer(device_t dev, device_t child, struct spi_command *cmd)
+{
+	struct qcom_spi_softc *sc = device_get_softc(dev);
+	uint32_t cs_val, mode_val, clock_val;
+	uint32_t ret = 0;
+
+	spibus_get_cs(child, &cs_val);
+	spibus_get_clock(child, &clock_val);
+	spibus_get_mode(child, &mode_val);
+
+	QCOM_SPI_DPRINTF(sc, QCOM_SPI_DEBUG_TRANSFER,
+	    "%s: called; child cs=0x%08x, clock=%u, mode=0x%08x, "
+	    "cmd=%u/%u bytes; data=%u/%u bytes\n",
+	    __func__,
+	    cs_val,
+	    clock_val,
+	    mode_val,
+	    cmd->tx_cmd_sz, cmd->rx_cmd_sz,
+	    cmd->tx_data_sz, cmd->rx_data_sz);
+
+	QCOM_SPI_LOCK(sc);
+
+	/*
+	 * wait until the controller isn't busy
+	 */
+	while (sc->sc_busy == true)
+		mtx_sleep(sc, &sc->sc_mtx, 0, "qcom_spi_wait", 0);
+
+	/*
+	 * it's ours now!
+	 */
+	sc->sc_busy = true;
+
+	sc->state.cs_high = !! (cs_val & SPIBUS_CS_HIGH);
+	sc->state.frequency = clock_val;
+
+	/*
+	 * We can't set the clock frequency and enable it
+	 * with the driver lock held, as the SPI lock is non-sleepable
+	 * and the clock framework is sleepable.
+	 *
+	 * No other transaction is going on right now, so we can
+	 * unlock here and do the clock related work.
+	 */
+	QCOM_SPI_UNLOCK(sc);
+
+	/*
+	 * Set the clock frequency
+	 */
+	ret = clk_set_freq(sc->clk_iface, sc->state.frequency, 0);
+	if (ret != 0) {
+		device_printf(sc->sc_dev,
+		    "ERROR: failed to set frequency to %u\n",
+		    sc->state.frequency);
+		goto done2;
+	}
+	clk_enable(sc->clk_iface);
+
+	QCOM_SPI_LOCK(sc);
+
+	/*
+	 * Set state to RESET
+	 */
+	ret = qcom_spi_hw_qup_set_state_locked(sc, QUP_STATE_RESET);
+	if (ret != 0) {
+		device_printf(sc->sc_dev,
+		    "ERROR: can't transition to RESET (%u)\n", ret);
+		goto done;
+	}
+
+	/* Assert hardware CS if set, else GPIO */
+	if (sc->cs_pins[cs_val & ~SPIBUS_CS_HIGH] == NULL)
+		qcom_spi_hw_spi_cs_force(sc, cs_val & SPIBUS_CS_HIGH, true);
+	else
+		qcom_spi_set_chipsel(sc, cs_val & ~SPIBUS_CS_HIGH, true);
*** 1423 LINES SKIPPED ***