3 files changed, 73 insertions, 19 deletions

diff --git a/sys/dev/mps/mps.c b/sys/dev/mps/mps.c
index bde7f43a8e4f..6fb5231a9e25 100644
--- a/sys/dev/mps/mps.c
+++ b/sys/dev/mps/mps.c
@@ -50,6 +50,7 @@ __FBSDID("$FreeBSD$");
 #include <sys/malloc.h>
 #include <sys/uio.h>
 #include <sys/sysctl.h>
+#include <sys/smp.h>
 #include <sys/queue.h>
 #include <sys/kthread.h>
 #include <sys/taskqueue.h>
@@ -700,6 +701,10 @@ mps_iocfacts_free(struct mps_softc *sc)
 	}
 	if (sc->buffer_dmat != NULL)
 		bus_dma_tag_destroy(sc->buffer_dmat);
+
+	mps_pci_free_interrupts(sc);
+	free(sc->queues, M_MPT2);
+	sc->queues = NULL;
 }
 
 /* 
@@ -1109,8 +1114,24 @@ static int
 mps_alloc_queues(struct mps_softc *sc)
 {
 	bus_addr_t queues_busaddr;
+	struct mps_queue *q;
 	uint8_t *queues;
-	int qsize, fqsize, pqsize;
+	int qsize, fqsize, pqsize, nq, i;
+
+	nq = MIN(sc->msi_msgs, mp_ncpus);
+	sc->msi_msgs = nq;
+	mps_dprint(sc, MPS_INIT|MPS_XINFO, "Allocating %d I/O queues\n", nq);
+
+	sc->queues = malloc(sizeof(struct mps_queue) * nq, M_MPT2, M_NOWAIT|M_ZERO);
+	if (sc->queues == NULL)
+		return (ENOMEM);
+
+	for (i = 0; i < nq; i++) {
+		q = &sc->queues[i];
+		mps_dprint(sc, MPS_INIT, "Configuring queue %d %p\n", i, q);
+		q->sc = sc;
+		q->qnum = i;
+	}
 
 	/*
 	 * The reply free queue contains 4 byte entries in multiples of 16 and
diff --git a/sys/dev/mps/mps_pci.c b/sys/dev/mps/mps_pci.c
index 2e95b42cc22d..e052317ed2aa 100644
--- a/sys/dev/mps/mps_pci.c
+++ b/sys/dev/mps/mps_pci.c
@@ -275,6 +275,7 @@ int
 mps_pci_setup_interrupts(struct mps_softc *sc)
 {
 	device_t dev;
+	struct mps_queue *q;
 	void *ihandler;
 	int i, error, rid, initial_rid;
 
@@ -294,18 +295,19 @@ mps_pci_setup_interrupts(struct mps_softc *sc)
 	}
 
 	for (i = 0; i < sc->msi_msgs; i++) {
+		q = &sc->queues[i];
 		rid = i + initial_rid;
-		sc->mps_irq_rid[i] = rid;
-		sc->mps_irq[i] = bus_alloc_resource_any(dev, SYS_RES_IRQ,
-		    &sc->mps_irq_rid[i], RF_ACTIVE);
-		if (sc->mps_irq[i] == NULL) {
+		q->irq_rid = rid;
+		q->irq = bus_alloc_resource_any(dev, SYS_RES_IRQ,
+		    &q->irq_rid, RF_ACTIVE);
+		if (q->irq == NULL) {
 			mps_dprint(sc, MPS_ERROR|MPS_INIT,
 			    "Cannot allocate interrupt RID%d\n", rid);
 			break;
 		}
-		error = bus_setup_intr(dev, sc->mps_irq[i],
+		error = bus_setup_intr(dev, q->irq,
 		    INTR_TYPE_BIO | INTR_MPSAFE, NULL, ihandler,
-		    sc, &sc->mps_intrhand[i]);
+		    sc, &q->intrhand);
 		if (error) {
 			mps_dprint(sc, MPS_ERROR|MPS_INIT,
 			    "Cannot setup interrupt RID %d\n", rid);
@@ -333,23 +335,35 @@ mps_pci_detach(device_t dev)
 	return (0);
 }
 
-static void
-mps_pci_free(struct mps_softc *sc)
+void
+mps_pci_free_interrupts(struct mps_softc *sc)
 {
+	struct mps_queue *q;
 	int i;
 
-	if (sc->mps_parent_dmat != NULL) {
-		bus_dma_tag_destroy(sc->mps_parent_dmat);
-	}
+	if (sc->queues == NULL)
+		return;
 
 	for (i = 0; i < sc->msi_msgs; i++) {
-		if (sc->mps_irq[i] != NULL) {
-			bus_teardown_intr(sc->mps_dev, sc->mps_irq[i],
-			    sc->mps_intrhand[i]);
+		q = &sc->queues[i];
+		if (q->irq != NULL) {
+			bus_teardown_intr(sc->mps_dev, q->irq,
+			    q->intrhand);
 			bus_release_resource(sc->mps_dev, SYS_RES_IRQ,
-			    sc->mps_irq_rid[i], sc->mps_irq[i]);
+			    q->irq_rid, q->irq);
 		}
 	}
+}
+
+static void
+mps_pci_free(struct mps_softc *sc)
+{
+
+	if (sc->mps_parent_dmat != NULL) {
+		bus_dma_tag_destroy(sc->mps_parent_dmat);
+	}
+
+	mps_pci_free_interrupts(sc);
 
 	if (sc->mps_flags & MPS_FLAGS_MSI)
 		pci_release_msi(sc->mps_dev);
diff --git a/sys/dev/mps/mpsvar.h b/sys/dev/mps/mpsvar.h
index b8d074a8e2bb..faadde7e928c 100644
--- a/sys/dev/mps/mpsvar.h
+++ b/sys/dev/mps/mpsvar.h
@@ -260,6 +260,26 @@ struct mps_event_handle {
 	u32				mask[MPI2_EVENT_NOTIFY_EVENTMASK_WORDS];
 };
 
+struct mps_queue {
+	struct mps_softc		*sc;
+	int				qnum;
+	MPI2_REPLY_DESCRIPTORS_UNION	*post_queue;
+	int				replypostindex;
+#ifdef notyet
+	ck_ring_buffer_t		*ringmem;
+	ck_ring_buffer_t		*chainmem;
+	ck_ring_t			req_ring;
+	ck_ring_t			chain_ring;
+#endif
+	bus_dma_tag_t			buffer_dmat;
+	int				io_cmds_highwater;
+	int				chain_free_lowwater;
+	int				chain_alloc_fail;
+	struct resource			*irq;
+	void				*intrhand;
+	int				irq_rid;
+};
+
 struct mps_softc {
 	device_t			mps_dev;
 	struct cdev			*mps_cdev;
@@ -294,6 +314,7 @@ struct mps_softc {
 	struct mps_chain		*chains;
 	struct callout			periodic;
 	struct callout			device_check_callout;
+	struct mps_queue		*queues;
 
 	struct mpssas_softc		*sassc;
 	char            tmp_string[MPS_STRING_LENGTH];
@@ -324,9 +345,6 @@ struct mps_softc {
 
 	struct mtx			mps_mtx;
 	struct intr_config_hook		mps_ich;
-	struct resource			*mps_irq[MPS_MSI_COUNT];
-	void				*mps_intrhand[MPS_MSI_COUNT];
-	int				mps_irq_rid[MPS_MSI_COUNT];
 
 	uint8_t				*req_frames;
 	bus_addr_t			req_busaddr;
@@ -671,6 +689,7 @@ mps_unmask_intr(struct mps_softc *sc)
 }
 
 int mps_pci_setup_interrupts(struct mps_softc *sc);
+void mps_pci_free_interrupts(struct mps_softc *sc);
 int mps_pci_restore(struct mps_softc *sc);
 
 void mps_get_tunables(struct mps_softc *sc);