mtip32xx: convert to use blk-mq

This rips out timeout handling, requeueing, etc in converting
it to use blk-mq instead.

Acked-by: Asai Thambi S P <asamymuthupa@micron.com>
Signed-off-by: Jens Axboe <axboe@fb.com>

2 files changed, 297 insertions, 616 deletions

diff --git a/drivers/block/mtip32xx/mtip32xx.c b/drivers/block/mtip32xx/mtip32xx.c
index fb624469d0ee..3a0882ee1642 100644
--- a/drivers/block/mtip32xx/mtip32xx.c
+++ b/drivers/block/mtip32xx/mtip32xx.c
@@ -31,6 +31,7 @@
 #include <linux/module.h>
 #include <linux/genhd.h>
 #include <linux/blkdev.h>
+#include <linux/blk-mq.h>
 #include <linux/bio.h>
 #include <linux/dma-mapping.h>
 #include <linux/idr.h>
@@ -173,60 +174,36 @@ static bool mtip_check_surprise_removal(struct pci_dev *pdev)
         return false; /* device present */
 }
 
-/*
- * Obtain an empty command slot.
- *
- * This function needs to be reentrant since it could be called
- * at the same time on multiple CPUs. The allocation of the
- * command slot must be atomic.
- *
- * @port Pointer to the port data structure.
- *
- * return value
- *      >= 0    Index of command slot obtained.
- *      -1      No command slots available.
- */
-static int get_slot(struct mtip_port *port)
+static struct mtip_cmd *mtip_get_int_command(struct driver_data *dd)
 {
-        int slot, i;
-        unsigned int num_command_slots = port->dd->slot_groups * 32;
+        struct request *rq;
 
-        /*
-         * Try 10 times, because there is a small race here.
-         *  that's ok, because it's still cheaper than a lock.
-         *
-         * Race: Since this section is not protected by lock, same bit
-         * could be chosen by different process contexts running in
-         * different processor. So instead of costly lock, we are going
-         * with loop.
-         */
-        for (i = 0; i < 10; i++) {
-                slot = find_next_zero_bit(port->allocated,
-                                         num_command_slots, 1);
-                if ((slot < num_command_slots) &&
-                    (!test_and_set_bit(slot, port->allocated)))
-                        return slot;
-        }
-        dev_warn(&port->dd->pdev->dev, "Failed to get a tag.\n");
+        rq = blk_mq_alloc_reserved_request(dd->queue, 0, __GFP_WAIT);
+        return blk_mq_rq_to_pdu(rq);
+}
 
-        mtip_check_surprise_removal(port->dd->pdev);
-        return -1;
+static void mtip_put_int_command(struct driver_data *dd, struct mtip_cmd *cmd)
+{
+        blk_put_request(blk_mq_rq_from_pdu(cmd));
 }
 
 /*
- * Release a command slot.
- *
- * @port Pointer to the port data structure.
- * @tag  Tag of command to release
- *
- * return value
- *      None
+ * Once we add support for one hctx per mtip group, this will change a bit
  */
-static inline void release_slot(struct mtip_port *port, int tag)
+static struct request *mtip_rq_from_tag(struct driver_data *dd,
+                                        unsigned int tag)
+{
+        struct blk_mq_hw_ctx *hctx = dd->queue->queue_hw_ctx[0];
+
+        return blk_mq_tag_to_rq(hctx->tags, tag);
+}
+
+static struct mtip_cmd *mtip_cmd_from_tag(struct driver_data *dd,
+                                          unsigned int tag)
 {
-        smp_mb__before_clear_bit();
-        clear_bit(tag, port->allocated);
-        smp_mb__after_clear_bit();
+        struct request *rq = mtip_rq_from_tag(dd, tag);
+
+        return blk_mq_rq_to_pdu(rq);
 }
 
 /*
@@ -248,93 +225,28 @@ static inline void release_slot(struct mtip_port *port, int tag)
  *      None
  */
 static void mtip_async_complete(struct mtip_port *port,
-                                int tag,
-                                void *data,
-                                int status)
+                                int tag, struct mtip_cmd *cmd, int status)
 {
-        struct mtip_cmd *cmd;
-        struct driver_data *dd = data;
-        int unaligned, cb_status = status ? -EIO : 0;
-        void (*func)(void *, int);
+        struct driver_data *dd = port->dd;
+        struct request *rq;
 
         if (unlikely(!dd) || unlikely(!port))
                 return;
 
-        cmd = &port->commands[tag];
-
         if (unlikely(status == PORT_IRQ_TF_ERR)) {
                 dev_warn(&port->dd->pdev->dev,
                         "Command tag %d failed due to TFE\n", tag);
         }
 
-        /* Clear the active flag */
-        atomic_set(&port->commands[tag].active, 0);
-
-        /* Upper layer callback */
-        func = cmd->async_callback;
-        if (likely(func && cmpxchg(&cmd->async_callback, func, 0) == func)) {
-
-                /* Unmap the DMA scatter list entries */
-                dma_unmap_sg(&dd->pdev->dev,
-                        cmd->sg,
-                        cmd->scatter_ents,
-                        cmd->direction);
-
-                func(cmd->async_data, cb_status);
-                unaligned = cmd->unaligned;
-
-                /* Clear the allocated bit for the command */
-                release_slot(port, tag);
-
-                if (unlikely(unaligned))
-                        up(&port->cmd_slot_unal);
-                else
-                        up(&port->cmd_slot);
-        }
-}
-
-/*
- * This function is called for clean the pending command in the
- * command slot during the surprise removal of device and return
- * error to the upper layer.
- *
- * @dd Pointer to the DRIVER_DATA structure.
- *
- * return value
- *      None
- */
-static void mtip_command_cleanup(struct driver_data *dd)
-{
-        int tag = 0;
-        struct mtip_cmd *cmd;
-        struct mtip_port *port = dd->port;
-        unsigned int num_cmd_slots = dd->slot_groups * 32;
-
-        if (!test_bit(MTIP_DDF_INIT_DONE_BIT, &dd->dd_flag))
-                return;
-
-        if (!port)
-                return;
-
-        cmd = &port->commands[MTIP_TAG_INTERNAL];
-        if (atomic_read(&cmd->active))
-                if (readl(port->cmd_issue[MTIP_TAG_INTERNAL]) &
-                                        (1 << MTIP_TAG_INTERNAL))
-                        if (cmd->comp_func)
-                                cmd->comp_func(port, MTIP_TAG_INTERNAL,
-                                         cmd->comp_data, -ENODEV);
+        /* Unmap the DMA scatter list entries */
+        dma_unmap_sg(&dd->pdev->dev, cmd->sg, cmd->scatter_ents, cmd->direction);
 
-        while (1) {
-                tag = find_next_bit(port->allocated, num_cmd_slots, tag);
-                if (tag >= num_cmd_slots)
-                        break;
+        rq = mtip_rq_from_tag(dd, tag);
 
-                cmd = &port->commands[tag];
-                if (atomic_read(&cmd->active))
-                        mtip_async_complete(port, tag, dd, -ENODEV);
-        }
+        if (unlikely(cmd->unaligned))
+                up(&port->cmd_slot_unal);
 
-        set_bit(MTIP_DDF_CLEANUP_BIT, &dd->dd_flag);
+        blk_mq_end_io(rq, status ? -EIO : 0);
 }
 
 /*
@@ -388,8 +300,6 @@ static inline void mtip_issue_ncq_command(struct mtip_port *port, int tag)
 {
         int group = tag >> 5;
 
-        atomic_set(&port->commands[tag].active, 1);
-
         /* guard SACT and CI registers */
         spin_lock(&port->cmd_issue_lock[group]);
         writel((1 << MTIP_TAG_BIT(tag)),
@@ -397,10 +307,6 @@ static inline void mtip_issue_ncq_command(struct mtip_port *port, int tag)
         writel((1 << MTIP_TAG_BIT(tag)),
                         port->cmd_issue[MTIP_TAG_INDEX(tag)]);
         spin_unlock(&port->cmd_issue_lock[group]);
-
-        /* Set the command's timeout value.*/
-        port->commands[tag].comp_time = jiffies + msecs_to_jiffies(
-                                        MTIP_NCQ_COMMAND_TIMEOUT_MS);
 }
 
 /*
@@ -648,132 +554,13 @@ static void print_tags(struct driver_data *dd,
 
         memset(tagmap, 0, sizeof(tagmap));
         for (group = SLOTBITS_IN_LONGS; group > 0; group--)
-                tagmap_len = sprintf(tagmap + tagmap_len, "%016lX ",
+                tagmap_len += sprintf(tagmap + tagmap_len, "%016lX ",
                                                 tagbits[group-1]);
         dev_warn(&dd->pdev->dev,
                         "%d command(s) %s: tagmap [%s]", cnt, msg, tagmap);
 }
 
 /*
- * Called periodically to see if any read/write commands are
- * taking too long to complete.
- *
- * @data Pointer to the PORT data structure.
- *
- * return value
- *      None
- */
-static void mtip_timeout_function(unsigned long int data)
-{
-        struct mtip_port *port = (struct mtip_port *) data;
-        struct host_to_dev_fis *fis;
-        struct mtip_cmd *cmd;
-        int unaligned, tag, cmdto_cnt = 0;
-        unsigned int bit, group;
-        unsigned int num_command_slots;
-        unsigned long to, tagaccum[SLOTBITS_IN_LONGS];
-        void (*func)(void *, int);
-
-        if (unlikely(!port))
-                return;
-
-        if (unlikely(port->dd->sr))
-                return;
-
-        if (test_bit(MTIP_DDF_RESUME_BIT, &port->dd->dd_flag)) {
-                mod_timer(&port->cmd_timer,
-                        jiffies + msecs_to_jiffies(30000));
-                return;
-        }
-        /* clear the tag accumulator */
-        memset(tagaccum, 0, SLOTBITS_IN_LONGS * sizeof(long));
-        num_command_slots = port->dd->slot_groups * 32;
-
-        for (tag = 0; tag < num_command_slots; tag++) {
-                /*
-                 * Skip internal command slot as it has
-                 * its own timeout mechanism
-                 */
-                if (tag == MTIP_TAG_INTERNAL)
-                        continue;
-
-                if (atomic_read(&port->commands[tag].active) &&
-                   (time_after(jiffies, port->commands[tag].comp_time))) {
-                        group = tag >> 5;
-                        bit = tag & 0x1F;
-
-                        cmd = &port->commands[tag];
-                        fis = (struct host_to_dev_fis *) cmd->command;
-
-                        set_bit(tag, tagaccum);
-                        cmdto_cnt++;
-                        if (cmdto_cnt == 1)
-                                set_bit(MTIP_PF_EH_ACTIVE_BIT, &port->flags);
-
-                        /*
-                         * Clear the completed bit. This should prevent
-                         *  any interrupt handlers from trying to retire
-                         *  the command.
-                         */
-                        writel(1 << bit, port->completed[group]);
-
-                        /* Clear the active flag for the command */
-                        atomic_set(&port->commands[tag].active, 0);
-
-                        func = cmd->async_callback;
-                        if (func &&
-                            cmpxchg(&cmd->async_callback, func, 0) == func) {
-
-                                /* Unmap the DMA scatter list entries */
-                                dma_unmap_sg(&port->dd->pdev->dev,
-                                                cmd->sg,
-                                                cmd->scatter_ents,
-                                                cmd->direction);
-
-                                func(cmd->async_data, -EIO);
-                                unaligned = cmd->unaligned;
-
-                                /* Clear the allocated bit for the command. */
-                                release_slot(port, tag);
-
-                                if (unaligned)
-                                        up(&port->cmd_slot_unal);
-                                else
-                                        up(&port->cmd_slot);
-                        }
-                }
-        }
-
-        if (cmdto_cnt) {
-                print_tags(port->dd, "timed out", tagaccum, cmdto_cnt);
-                if (!test_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags)) {
-                        mtip_device_reset(port->dd);
-                        wake_up_interruptible(&port->svc_wait);
-                }
-                clear_bit(MTIP_PF_EH_ACTIVE_BIT, &port->flags);
-        }
-
-        if (port->ic_pause_timer) {
-                to  = port->ic_pause_timer + msecs_to_jiffies(1000);
-                if (time_after(jiffies, to)) {
-                        if (!test_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags)) {
-                                port->ic_pause_timer = 0;
-                                clear_bit(MTIP_PF_SE_ACTIVE_BIT, &port->flags);
-                                clear_bit(MTIP_PF_DM_ACTIVE_BIT, &port->flags);
-                                clear_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags);
-                                wake_up_interruptible(&port->svc_wait);
-                        }
-
-
-                }
-        }
-
-        /* Restart the timer */
-        mod_timer(&port->cmd_timer,
-                jiffies + msecs_to_jiffies(MTIP_TIMEOUT_CHECK_PERIOD));
-}
-
-/*
  * Internal command completion callback function.
  *
  * This function is normally called by the driver ISR when an internal
@@ -789,28 +576,19 @@ static void mtip_timeout_function(unsigned long int data)
  *      None
  */
 static void mtip_completion(struct mtip_port *port,
-                            int tag,
-                            void *data,
-                            int status)
+                            int tag, struct mtip_cmd *command, int status)
 {
-        struct mtip_cmd *command = &port->commands[tag];
-        struct completion *waiting = data;
+        struct completion *waiting = command->comp_data;
         if (unlikely(status == PORT_IRQ_TF_ERR))
                 dev_warn(&port->dd->pdev->dev,
                         "Internal command %d completed with TFE\n", tag);
 
-        command->async_callback = NULL;
-        command->comp_func = NULL;
-
         complete(waiting);
 }
 
 static void mtip_null_completion(struct mtip_port *port,
-                            int tag,
-                            void *data,
-                            int status)
+                            int tag, struct mtip_cmd *command, int status)
 {
-        return;
 }
 
 static int mtip_read_log_page(struct mtip_port *port, u8 page, u16 *buffer,
@@ -843,18 +621,16 @@ static void mtip_handle_tfe(struct driver_data *dd)
         port = dd->port;
 
         /* Stop the timer to prevent command timeouts. */
-        del_timer(&port->cmd_timer);
         set_bit(MTIP_PF_EH_ACTIVE_BIT, &port->flags);
 
         if (test_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags) &&
                         test_bit(MTIP_TAG_INTERNAL, port->allocated)) {
-                cmd = &port->commands[MTIP_TAG_INTERNAL];
+                cmd = mtip_cmd_from_tag(dd, MTIP_TAG_INTERNAL);
                 dbg_printk(MTIP_DRV_NAME " TFE for the internal command\n");
 
-                atomic_inc(&cmd->active); /* active > 1 indicates error */
                 if (cmd->comp_data && cmd->comp_func) {
                         cmd->comp_func(port, MTIP_TAG_INTERNAL,
-                                        cmd->comp_data, PORT_IRQ_TF_ERR);
+                                        cmd, PORT_IRQ_TF_ERR);
                 }
                 goto handle_tfe_exit;
         }
@@ -866,6 +642,8 @@ static void mtip_handle_tfe(struct driver_data *dd)
         for (group = 0; group < dd->slot_groups; group++) {
                 completed = readl(port->completed[group]);
 
+                dev_warn(&dd->pdev->dev, "g=%u, comp=%x\n", group, completed);
+
                 /* clear completed status register in the hardware.*/
                 writel(completed, port->completed[group]);
 
@@ -879,15 +657,11 @@ static void mtip_handle_tfe(struct driver_data *dd)
                         if (tag == MTIP_TAG_INTERNAL)
                                 continue;
 
-                        cmd = &port->commands[tag];
+                        cmd = mtip_cmd_from_tag(dd, tag);
                         if (likely(cmd->comp_func)) {
                                 set_bit(tag, tagaccum);
                                 cmd_cnt++;
-                                atomic_set(&cmd->active, 0);
-                                cmd->comp_func(port,
-                                         tag,
-                                         cmd->comp_data,
-                                         0);
+                                cmd->comp_func(port, tag, cmd, 0);
                         } else {
                                 dev_err(&port->dd->pdev->dev,
                                         "Missing completion func for tag %d",
@@ -947,11 +721,7 @@ static void mtip_handle_tfe(struct driver_data *dd)
                 for (bit = 0; bit < 32; bit++) {
                         reissue = 1;
                         tag = (group << 5) + bit;
-                        cmd = &port->commands[tag];
-
-                        /* If the active bit is set re-issue the command */
-                        if (atomic_read(&cmd->active) == 0)
-                                continue;
+                        cmd = mtip_cmd_from_tag(dd, tag);
 
                         fis = (struct host_to_dev_fis *)cmd->command;
 
@@ -970,11 +740,9 @@ static void mtip_handle_tfe(struct driver_data *dd)
                                         tag,
                                         fail_reason != NULL ?
                                                 fail_reason : "unknown");
-                                        atomic_set(&cmd->active, 0);
                                         if (cmd->comp_func) {
                                                 cmd->comp_func(port, tag,
-                                                        cmd->comp_data,
-                                                        -ENODATA);
+                                                        cmd, -ENODATA);
                                         }
                                         continue;
                                 }
@@ -997,14 +765,9 @@ static void mtip_handle_tfe(struct driver_data *dd)
                         /* Retire a command that will not be reissued */
                         dev_warn(&port->dd->pdev->dev,
                                 "retiring tag %d\n", tag);
-                        atomic_set(&cmd->active, 0);
 
                         if (cmd->comp_func)
-                                cmd->comp_func(
-                                        port,
-                                        tag,
-                                        cmd->comp_data,
-                                        PORT_IRQ_TF_ERR);
+                                cmd->comp_func(port, tag, cmd, PORT_IRQ_TF_ERR);
                         else
                                 dev_warn(&port->dd->pdev->dev,
                                         "Bad completion for tag %d\n",
@@ -1017,9 +780,6 @@ handle_tfe_exit:
         /* clear eh_active */
         clear_bit(MTIP_PF_EH_ACTIVE_BIT, &port->flags);
         wake_up_interruptible(&port->svc_wait);
-
-        mod_timer(&port->cmd_timer,
-                 jiffies + msecs_to_jiffies(MTIP_TIMEOUT_CHECK_PERIOD));
 }
 
 /*
@@ -1048,15 +808,10 @@ static inline void mtip_workq_sdbfx(struct mtip_port *port, int group,
                         if (unlikely(tag == MTIP_TAG_INTERNAL))
                                 continue;
 
-                        command = &port->commands[tag];
-                        /* make internal callback */
-                        if (likely(command->comp_func)) {
-                                command->comp_func(
-                                        port,
-                                        tag,
-                                        command->comp_data,
-                                        0);
-                        } else {
+                        command = mtip_cmd_from_tag(dd, tag);
+                        if (likely(command->comp_func))
+                                command->comp_func(port, tag, command, 0);
+                        else {
                                 dev_dbg(&dd->pdev->dev,
                                         "Null completion for tag %d",
                                         tag);
@@ -1081,16 +836,13 @@ static inline void mtip_workq_sdbfx(struct mtip_port *port, int group,
 static inline void mtip_process_legacy(struct driver_data *dd, u32 port_stat)
 {
         struct mtip_port *port = dd->port;
-        struct mtip_cmd *cmd = &port->commands[MTIP_TAG_INTERNAL];
+        struct mtip_cmd *cmd = mtip_cmd_from_tag(dd, MTIP_TAG_INTERNAL);
 
         if (test_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags) &&
             (cmd != NULL) && !(readl(port->cmd_issue[MTIP_TAG_INTERNAL])
                 & (1 << MTIP_TAG_INTERNAL))) {
                 if (cmd->comp_func) {
-                        cmd->comp_func(port,
-                                MTIP_TAG_INTERNAL,
-                                cmd->comp_data,
-                                0);
+                        cmd->comp_func(port, MTIP_TAG_INTERNAL, cmd, 0);
                         return;
                 }
         }
@@ -1222,7 +974,6 @@ static irqreturn_t mtip_irq_handler(int irq, void *instance)
 
 static void mtip_issue_non_ncq_command(struct mtip_port *port, int tag)
 {
-        atomic_set(&port->commands[tag].active, 1);
         writel(1 << MTIP_TAG_BIT(tag),
                 port->cmd_issue[MTIP_TAG_INDEX(tag)]);
 }
@@ -1335,10 +1086,9 @@ static int mtip_exec_internal_command(struct mtip_port *port,
 {
         struct mtip_cmd_sg *command_sg;
         DECLARE_COMPLETION_ONSTACK(wait);
-        int rv = 0, ready2go = 1;
-        struct mtip_cmd *int_cmd = &port->commands[MTIP_TAG_INTERNAL];
-        unsigned long to;
+        struct mtip_cmd *int_cmd;
         struct driver_data *dd = port->dd;
+        int rv = 0;
 
         /* Make sure the buffer is 8 byte aligned. This is asic specific. */
         if (buffer & 0x00000007) {
@@ -1346,19 +1096,8 @@ static int mtip_exec_internal_command(struct mtip_port *port,
                 return -EFAULT;
         }
 
-        to = jiffies + msecs_to_jiffies(timeout);
-        do {
-                ready2go = !test_and_set_bit(MTIP_TAG_INTERNAL,
-                                                port->allocated);
-                if (ready2go)
-                        break;
-                mdelay(100);
-        } while (time_before(jiffies, to));
-        if (!ready2go) {
-                dev_warn(&dd->pdev->dev,
-                        "Internal cmd active. new cmd [%02X]\n", fis->command);
-                return -EBUSY;
-        }
+        int_cmd = mtip_get_int_command(dd);
+
         set_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags);
         port->ic_pause_timer = 0;
 
@@ -1371,7 +1110,7 @@ static int mtip_exec_internal_command(struct mtip_port *port,
                         if (mtip_quiesce_io(port, 5000) < 0) {
                                 dev_warn(&dd->pdev->dev,
                                         "Failed to quiesce IO\n");
-                                release_slot(port, MTIP_TAG_INTERNAL);
+                                mtip_put_int_command(dd, int_cmd);
                                 clear_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags);
                                 wake_up_interruptible(&port->svc_wait);
                                 return -EBUSY;
@@ -1497,8 +1236,7 @@ static int mtip_exec_internal_command(struct mtip_port *port,
         }
 exec_ic_exit:
         /* Clear the allocated and active bits for the internal command. */
-        atomic_set(&int_cmd->active, 0);
-        release_slot(port, MTIP_TAG_INTERNAL);
+        mtip_put_int_command(dd, int_cmd);
         if (rv >= 0 && mtip_pause_ncq(port, fis)) {
                 /* NCQ paused */
                 return rv;
@@ -2610,22 +2348,21 @@ static int mtip_hw_ioctl(struct driver_data *dd, unsigned int cmd,
  * return value
  *      None
  */
-static void mtip_hw_submit_io(struct driver_data *dd, sector_t sector,
-                              int nsect, int nents, int tag, void *callback,
-                              void *data, int dir, int unaligned)
+static void mtip_hw_submit_io(struct driver_data *dd, struct request *rq,
+                              struct mtip_cmd *command, int nents,
+                              struct blk_mq_hw_ctx *hctx)
 {
         struct host_to_dev_fis  *fis;
         struct mtip_port *port = dd->port;
-        struct mtip_cmd *command = &port->commands[tag];
-        int dma_dir = (dir == READ) ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
-        u64 start = sector;
+        int dma_dir = rq_data_dir(rq) == READ ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
+        u64 start = blk_rq_pos(rq);
+        unsigned int nsect = blk_rq_sectors(rq);
 
         /* Map the scatter list for DMA access */
         nents = dma_map_sg(&dd->pdev->dev, command->sg, nents, dma_dir);
 
         command->scatter_ents = nents;
 
-        command->unaligned = unaligned;
         /*
          * The number of retries for this command before it is
          * reported as a failure to the upper layers.
@@ -2636,8 +2373,10 @@ static void mtip_hw_submit_io(struct driver_data *dd, sector_t sector,
         fis = command->command;
         fis->type        = 0x27;
         fis->opts        = 1 << 7;
-        fis->command     =
-                (dir == READ ? ATA_CMD_FPDMA_READ : ATA_CMD_FPDMA_WRITE);
+        if (rq_data_dir(rq) == READ)
+                fis->command = ATA_CMD_FPDMA_READ;
+        else
+                fis->command = ATA_CMD_FPDMA_WRITE;
         fis->lba_low     = start & 0xFF;
         fis->lba_mid     = (start >> 8) & 0xFF;
         fis->lba_hi      = (start >> 16) & 0xFF;
@@ -2647,14 +2386,14 @@ static void mtip_hw_submit_io(struct driver_data *dd, sector_t sector,
         fis->device      = 1 << 6;
         fis->features    = nsect & 0xFF;
         fis->features_ex = (nsect >> 8) & 0xFF;
-        fis->sect_count  = ((tag << 3) | (tag >> 5));
+        fis->sect_count  = ((rq->tag << 3) | (rq->tag >> 5));
         fis->sect_cnt_ex = 0;
         fis->control     = 0;
         fis->res2        = 0;
         fis->res3        = 0;
         fill_command_sg(dd, command, nents);
 
-        if (unaligned)
+        if (command->unaligned)
                 fis->device |= 1 << 7;
 
         /* Populate the command header */
@@ -2672,81 +2411,17 @@ static void mtip_hw_submit_io(struct driver_data *dd, sector_t sector,
         command->direction = dma_dir;
 
         /*
-         * Set the completion function and data for the command passed
-         * from the upper layer.
-         */
-        command->async_data = data;
-        command->async_callback = callback;
-
-        /*
          * To prevent this command from being issued
          * if an internal command is in progress or error handling is active.
          */
         if (port->flags & MTIP_PF_PAUSE_IO) {
-                set_bit(tag, port->cmds_to_issue);
+                set_bit(rq->tag, port->cmds_to_issue);
                 set_bit(MTIP_PF_ISSUE_CMDS_BIT, &port->flags);
                 return;
         }
 
         /* Issue the command to the hardware */
-        mtip_issue_ncq_command(port, tag);
-
-        return;
-}
-
-/*
- * Release a command slot.
- *
- * @dd  Pointer to the driver data structure.
- * @tag Slot tag
- *
- * return value
- *      None
- */
-static void mtip_hw_release_scatterlist(struct driver_data *dd, int tag,
-                                                                int unaligned)
-{
-        struct semaphore *sem = unaligned ? &dd->port->cmd_slot_unal :
-                                                        &dd->port->cmd_slot;
-        release_slot(dd->port, tag);
-        up(sem);
-}
-
-/*
- * Obtain a command slot and return its associated scatter list.
- *
- * @dd  Pointer to the driver data structure.
- * @tag Pointer to an int that will receive the allocated command
- *            slot tag.
- *
- * return value
- *      Pointer to the scatter list for the allocated command slot
- *      or NULL if no command slots are available.
- */
-static struct scatterlist *mtip_hw_get_scatterlist(struct driver_data *dd,
-                                                   int *tag, int unaligned)
-{
-        struct semaphore *sem = unaligned ? &dd->port->cmd_slot_unal :
-                                                        &dd->port->cmd_slot;
-
-        /*
-         * It is possible that, even with this semaphore, a thread
-         * may think that no command slots are available. Therefore, we
-         * need to make an attempt to get_slot().
-         */
-        down(sem);
-        *tag = get_slot(dd->port);
-
-        if (unlikely(test_bit(MTIP_DDF_REMOVE_PENDING_BIT, &dd->dd_flag))) {
-                up(sem);
-                return NULL;
-        }
-        if (unlikely(*tag < 0)) {
-                up(sem);
-                return NULL;
-        }
-
-        return dd->port->commands[*tag].sg;
+        mtip_issue_ncq_command(port, rq->tag);
 }
 
 /*
@@ -3117,6 +2792,7 @@ static int mtip_free_orphan(struct driver_data *dd)
                 if (dd->queue) {
                         dd->queue->queuedata = NULL;
                         blk_cleanup_queue(dd->queue);
+                        blk_mq_free_tag_set(&dd->tags);
                         dd->queue = NULL;
                 }
         }
@@ -3369,7 +3045,6 @@ st_out:
  */
 static void mtip_dma_free(struct driver_data *dd)
 {
-        int i;
         struct mtip_port *port = dd->port;
 
         if (port->block1)
@@ -3380,13 +3055,6 @@ static void mtip_dma_free(struct driver_data *dd)
                 dmam_free_coherent(&dd->pdev->dev, AHCI_CMD_TBL_SZ,
                                 port->command_list, port->command_list_dma);
         }
-
-        for (i = 0; i < MTIP_MAX_COMMAND_SLOTS; i++) {
-                if (port->commands[i].command)
-                        dmam_free_coherent(&dd->pdev->dev, CMD_DMA_ALLOC_SZ,
-                                port->commands[i].command,
-                                port->commands[i].command_dma);
-        }
 }
 
 /*
@@ -3400,8 +3068,6 @@ static void mtip_dma_free(struct driver_data *dd)
 static int mtip_dma_alloc(struct driver_data *dd)
 {
         struct mtip_port *port = dd->port;
-        int i, rv = 0;
-        u32 host_cap_64 = readl(dd->mmio + HOST_CAP) & HOST_CAP_64;
 
         /* Allocate dma memory for RX Fis, Identify, and Sector Bufffer */
         port->block1 =
@@ -3434,41 +3100,63 @@ static int mtip_dma_alloc(struct driver_data *dd)
         port->smart_buf     = port->block1 + AHCI_SMARTBUF_OFFSET;
         port->smart_buf_dma = port->block1_dma + AHCI_SMARTBUF_OFFSET;
 
-        /* Setup per command SGL DMA region */
-
-        /* Point the command headers at the command tables */
-        for (i = 0; i < MTIP_MAX_COMMAND_SLOTS; i++) {
-                port->commands[i].command =
-                        dmam_alloc_coherent(&dd->pdev->dev, CMD_DMA_ALLOC_SZ,
-                                &port->commands[i].command_dma, GFP_KERNEL);
-                if (!port->commands[i].command) {
-                        rv = -ENOMEM;
-                        mtip_dma_free(dd);
-                        return rv;
-                }
-                memset(port->commands[i].command, 0, CMD_DMA_ALLOC_SZ);
-
-                port->commands[i].command_header = port->command_list +
-                                        (sizeof(struct mtip_cmd_hdr) * i);
-                port->commands[i].command_header_dma =
-                                        dd->port->command_list_dma +
-                                        (sizeof(struct mtip_cmd_hdr) * i);
+        return 0;
+}
 
-                if (host_cap_64)
-                        port->commands[i].command_header->ctbau =
-                                __force_bit2int cpu_to_le32(
-                                (port->commands[i].command_dma >> 16) >> 16);
+static int mtip_hw_get_identify(struct driver_data *dd)
+{
+        struct smart_attr attr242;
+        unsigned char *buf;
+        int rv;
 
-                port->commands[i].command_header->ctba =
-                                __force_bit2int cpu_to_le32(
-                                port->commands[i].command_dma & 0xFFFFFFFF);
+        if (mtip_get_identify(dd->port, NULL) < 0)
+                return -EFAULT;
 
-                sg_init_table(port->commands[i].sg, MTIP_MAX_SG);
+        if (*(dd->port->identify + MTIP_FTL_REBUILD_OFFSET) ==
+                MTIP_FTL_REBUILD_MAGIC) {
+                set_bit(MTIP_PF_REBUILD_BIT, &dd->port->flags);
+                return MTIP_FTL_REBUILD_MAGIC;
+        }
+        mtip_dump_identify(dd->port);
 
-                /* Mark command as currently inactive */
-                atomic_set(&dd->port->commands[i].active, 0);
+        /* check write protect, over temp and rebuild statuses */
+        rv = mtip_read_log_page(dd->port, ATA_LOG_SATA_NCQ,
+                                dd->port->log_buf,
+                                dd->port->log_buf_dma, 1);
+        if (rv) {
+                dev_warn(&dd->pdev->dev,
+                        "Error in READ LOG EXT (10h) command\n");
+                /* non-critical error, don't fail the load */
+        } else {
+                buf = (unsigned char *)dd->port->log_buf;
+                if (buf[259] & 0x1) {
+                        dev_info(&dd->pdev->dev,
+                                "Write protect bit is set.\n");
+                        set_bit(MTIP_DDF_WRITE_PROTECT_BIT, &dd->dd_flag);
+                }
+                if (buf[288] == 0xF7) {
+                        dev_info(&dd->pdev->dev,
+                                "Exceeded Tmax, drive in thermal shutdown.\n");
+                        set_bit(MTIP_DDF_OVER_TEMP_BIT, &dd->dd_flag);
+                }
+                if (buf[288] == 0xBF) {
+                        dev_info(&dd->pdev->dev,
+                                "Drive indicates rebuild has failed.\n");
+                        /* TODO */
+                }
         }
-        return 0;
+
+        /* get write protect progess */
+        memset(&attr242, 0, sizeof(struct smart_attr));
+        if (mtip_get_smart_attr(dd->port, 242, &attr242))
+                dev_warn(&dd->pdev->dev,
+                                "Unable to check write protect progress\n");
+        else
+                dev_info(&dd->pdev->dev,
+                                "Write protect progress: %u%% (%u blocks)\n",
+                                attr242.cur, le32_to_cpu(attr242.data));
+
+        return rv;
 }
 
 /*
@@ -3485,8 +3173,6 @@ static int mtip_hw_init(struct driver_data *dd)
         int rv;
         unsigned int num_command_slots;
         unsigned long timeout, timetaken;
-        unsigned char *buf;
-        struct smart_attr attr242;
 
         dd->mmio = pcim_iomap_table(dd->pdev)[MTIP_ABAR];
 
@@ -3517,8 +3203,6 @@ static int mtip_hw_init(struct driver_data *dd)
         else
                 dd->unal_qdepth = 0;
 
-        /* Counting semaphore to track command slot usage */
-        sema_init(&dd->port->cmd_slot, num_command_slots - 1 - dd->unal_qdepth);
         sema_init(&dd->port->cmd_slot_unal, dd->unal_qdepth);
 
         /* Spinlock to prevent concurrent issue */
@@ -3603,73 +3287,16 @@ static int mtip_hw_init(struct driver_data *dd)
         writel(readl(dd->mmio + HOST_CTL) | HOST_IRQ_EN,
                                         dd->mmio + HOST_CTL);
 
-        init_timer(&dd->port->cmd_timer);
         init_waitqueue_head(&dd->port->svc_wait);
 
-        dd->port->cmd_timer.data = (unsigned long int) dd->port;
-        dd->port->cmd_timer.function = mtip_timeout_function;
-        mod_timer(&dd->port->cmd_timer,
-                jiffies + msecs_to_jiffies(MTIP_TIMEOUT_CHECK_PERIOD));
-
-
         if (test_bit(MTIP_DDF_REMOVE_PENDING_BIT, &dd->dd_flag)) {
                 rv = -EFAULT;
                 goto out3;
         }
 
-        if (mtip_get_identify(dd->port, NULL) < 0) {
-                rv = -EFAULT;
-                goto out3;
-        }
-        mtip_dump_identify(dd->port);
-
-        if (*(dd->port->identify + MTIP_FTL_REBUILD_OFFSET) ==
-                MTIP_FTL_REBUILD_MAGIC) {
-                set_bit(MTIP_PF_REBUILD_BIT, &dd->port->flags);
-                return MTIP_FTL_REBUILD_MAGIC;
-        }
-
-        /* check write protect, over temp and rebuild statuses */
-        rv = mtip_read_log_page(dd->port, ATA_LOG_SATA_NCQ,
-                                dd->port->log_buf,
-                                dd->port->log_buf_dma, 1);
-        if (rv) {
-                dev_warn(&dd->pdev->dev,
-                        "Error in READ LOG EXT (10h) command\n");
-                /* non-critical error, don't fail the load */
-        } else {
-                buf = (unsigned char *)dd->port->log_buf;
-                if (buf[259] & 0x1) {
-                        dev_info(&dd->pdev->dev,
-                                "Write protect bit is set.\n");
-                        set_bit(MTIP_DDF_WRITE_PROTECT_BIT, &dd->dd_flag);
-                }
-                if (buf[288] == 0xF7) {
-                        dev_info(&dd->pdev->dev,
-                                "Exceeded Tmax, drive in thermal shutdown.\n");
-                        set_bit(MTIP_DDF_OVER_TEMP_BIT, &dd->dd_flag);
-                }
-                if (buf[288] == 0xBF) {
-                        dev_info(&dd->pdev->dev,
-                                "Drive is in security locked state.\n");
-                        set_bit(MTIP_DDF_SEC_LOCK_BIT, &dd->dd_flag);
-                }
-        }
-
-        /* get write protect progess */
-        memset(&attr242, 0, sizeof(struct smart_attr));
-        if (mtip_get_smart_attr(dd->port, 242, &attr242))
-                dev_warn(&dd->pdev->dev,
-                                "Unable to check write protect progress\n");
-        else
-                dev_info(&dd->pdev->dev,
-                                "Write protect progress: %u%% (%u blocks)\n",
-                                attr242.cur, le32_to_cpu(attr242.data));
         return rv;
 
 out3:
-        del_timer_sync(&dd->port->cmd_timer);
-
         /* Disable interrupts on the HBA. */
         writel(readl(dd->mmio + HOST_CTL) & ~HOST_IRQ_EN,
                         dd->mmio + HOST_CTL);
@@ -3689,6 +3316,22 @@ out1:
         return rv;
 }
 
+static void mtip_standby_drive(struct driver_data *dd)
+{
+        if (dd->sr)
+                return;
+
+        /*
+         * Send standby immediate (E0h) to the drive so that it
+         * saves its state.
+         */
+        if (!test_bit(MTIP_PF_REBUILD_BIT, &dd->port->flags) &&
+            !test_bit(MTIP_DDF_SEC_LOCK_BIT, &dd->dd_flag))
+                if (mtip_standby_immediate(dd->port))
+                        dev_warn(&dd->pdev->dev,
+                                "STANDBY IMMEDIATE failed\n");
+}
+
 /*
  * Called to deinitialize an interface.
  *
@@ -3704,12 +3347,6 @@ static int mtip_hw_exit(struct driver_data *dd)
          * saves its state.
          */
         if (!dd->sr) {
-                if (!test_bit(MTIP_PF_REBUILD_BIT, &dd->port->flags) &&
-                    !test_bit(MTIP_DDF_SEC_LOCK_BIT, &dd->dd_flag))
-                        if (mtip_standby_immediate(dd->port))
-                                dev_warn(&dd->pdev->dev,
-                                        "STANDBY IMMEDIATE failed\n");
-
                 /* de-initialize the port. */
                 mtip_deinit_port(dd->port);
 
@@ -3718,8 +3355,6 @@ static int mtip_hw_exit(struct driver_data *dd)
                                 dd->mmio + HOST_CTL);
         }
 
-        del_timer_sync(&dd->port->cmd_timer);
-
         /* Release the IRQ. */
         irq_set_affinity_hint(dd->pdev->irq, NULL);
         devm_free_irq(&dd->pdev->dev, dd->pdev->irq, dd);
@@ -4036,100 +3671,140 @@ static const struct block_device_operations mtip_block_ops = {
  *
  * @queue Pointer to the request queue. Unused other than to obtain
  *              the driver data structure.
- * @bio   Pointer to the BIO.
+ * @rq    Pointer to the request.
  *
  */
-static void mtip_make_request(struct request_queue *queue, struct bio *bio)
+static int mtip_submit_request(struct blk_mq_hw_ctx *hctx, struct request *rq)
 {
-        struct driver_data *dd = queue->queuedata;
-        struct scatterlist *sg;
-        struct bio_vec bvec;
-        struct bvec_iter iter;
-        int nents = 0;
-        int tag = 0, unaligned = 0;
+        struct driver_data *dd = hctx->queue->queuedata;
+        struct mtip_cmd *cmd = blk_mq_rq_to_pdu(rq);
+        unsigned int nents;
 
         if (unlikely(dd->dd_flag & MTIP_DDF_STOP_IO)) {
                 if (unlikely(test_bit(MTIP_DDF_REMOVE_PENDING_BIT,
                                                         &dd->dd_flag))) {
-                        bio_endio(bio, -ENXIO);
-                        return;
+                        return -ENXIO;
                 }
                 if (unlikely(test_bit(MTIP_DDF_OVER_TEMP_BIT, &dd->dd_flag))) {
-                        bio_endio(bio, -ENODATA);
-                        return;
+                        return -ENODATA;
                 }
                 if (unlikely(test_bit(MTIP_DDF_WRITE_PROTECT_BIT,
                                                         &dd->dd_flag) &&
-                                bio_data_dir(bio))) {
-                        bio_endio(bio, -ENODATA);
-                        return;
-                }
-                if (unlikely(test_bit(MTIP_DDF_SEC_LOCK_BIT, &dd->dd_flag))) {
-                        bio_endio(bio, -ENODATA);
-                        return;
-                }
-                if (test_bit(MTIP_DDF_REBUILD_FAILED_BIT, &dd->dd_flag)) {
-                        bio_endio(bio, -ENXIO);
-                        return;
+                                rq_data_dir(rq))) {
+                        return -ENODATA;
                 }
+                if (unlikely(test_bit(MTIP_DDF_SEC_LOCK_BIT, &dd->dd_flag)))
+                        return -ENODATA;
+                if (test_bit(MTIP_DDF_REBUILD_FAILED_BIT, &dd->dd_flag))
+                        return -ENXIO;
         }
 
-        if (unlikely(bio->bi_rw & REQ_DISCARD)) {
-                bio_endio(bio, mtip_send_trim(dd, bio->bi_iter.bi_sector,
-                                                bio_sectors(bio)));
-                return;
-        }
+        if (rq->cmd_flags & REQ_DISCARD) {
+                int err;
 
-        if (unlikely(!bio_has_data(bio))) {
-                blk_queue_flush(queue, 0);
-                bio_endio(bio, 0);
-                return;
+                err = mtip_send_trim(dd, blk_rq_pos(rq), blk_rq_sectors(rq));
+                blk_mq_end_io(rq, err);
+                return 0;
         }
 
-        if (bio_data_dir(bio) == WRITE && bio_sectors(bio) <= 64 &&
-                                                        dd->unal_qdepth) {
-                if (bio->bi_iter.bi_sector % 8 != 0)
-                        /* Unaligned on 4k boundaries */
-                        unaligned = 1;
-                else if (bio_sectors(bio) % 8 != 0) /* Aligned but not 4k/8k */
-                        unaligned = 1;
+        /* Create the scatter list for this request. */
+        nents = blk_rq_map_sg(hctx->queue, rq, cmd->sg);
+
+        /* Issue the read/write. */
+        mtip_hw_submit_io(dd, rq, cmd, nents, hctx);
+        return 0;
+}
+
+static bool mtip_check_unal_depth(struct blk_mq_hw_ctx *hctx,
+                                  struct request *rq)
+{
+        struct driver_data *dd = hctx->queue->queuedata;
+        struct mtip_cmd *cmd = blk_mq_rq_to_pdu(rq);
+
+        if (!dd->unal_qdepth || rq_data_dir(rq) == READ)
+                return false;
+
+        /*
+         * If unaligned depth must be limited on this controller, mark it
+         * as unaligned if the IO isn't on a 4k boundary (start of length).
+         */
+        if (blk_rq_sectors(rq) <= 64) {
+                if ((blk_rq_pos(rq) & 7) || (blk_rq_sectors(rq) & 7))
+                        cmd->unaligned = 1;
         }
 
-        sg = mtip_hw_get_scatterlist(dd, &tag, unaligned);
-        if (likely(sg != NULL)) {
-                blk_queue_bounce(queue, &bio);
+        if (cmd->unaligned && down_trylock(&dd->port->cmd_slot_unal))
+                return true;
 
-                if (unlikely((bio)->bi_vcnt > MTIP_MAX_SG)) {
-                        dev_warn(&dd->pdev->dev,
-                                "Maximum number of SGL entries exceeded\n");
-                        bio_io_error(bio);
-                        mtip_hw_release_scatterlist(dd, tag, unaligned);
-                        return;
-                }
+        return false;
+}
 
-                /* Create the scatter list for this bio. */
-                bio_for_each_segment(bvec, bio, iter) {
-                        sg_set_page(&sg[nents],
-                                        bvec.bv_page,
-                                        bvec.bv_len,
-                                        bvec.bv_offset);
-                        nents++;
-                }
+static int mtip_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *rq)
+{
+        int ret;
 
-                /* Issue the read/write. */
-                mtip_hw_submit_io(dd,
-                                bio->bi_iter.bi_sector,
-                                bio_sectors(bio),
-                                nents,
-                                tag,
-                                bio_endio,
-                                bio,
-                                bio_data_dir(bio),
-                                unaligned);
-        } else
-                bio_io_error(bio);
+        if (mtip_check_unal_depth(hctx, rq))
+                return BLK_MQ_RQ_QUEUE_BUSY;
+
+        ret = mtip_submit_request(hctx, rq);
+        if (!ret)
+                return BLK_MQ_RQ_QUEUE_OK;
+
+        rq->errors = ret;
+        return BLK_MQ_RQ_QUEUE_ERROR;
 }
 
+static void mtip_free_cmd(void *data, struct request *rq,
+                          unsigned int hctx_idx, unsigned int request_idx)
+{
+        struct driver_data *dd = data;
+        struct mtip_cmd *cmd = blk_mq_rq_to_pdu(rq);
+
+        if (!cmd->command)
+                return;
+
+        dmam_free_coherent(&dd->pdev->dev, CMD_DMA_ALLOC_SZ,
+                                cmd->command, cmd->command_dma);
+}
+
+static int mtip_init_cmd(void *data, struct request *rq, unsigned int hctx_idx,
+                         unsigned int request_idx, unsigned int numa_node)
+{
+        struct driver_data *dd = data;
+        struct mtip_cmd *cmd = blk_mq_rq_to_pdu(rq);
+        u32 host_cap_64 = readl(dd->mmio + HOST_CAP) & HOST_CAP_64;
+
+        cmd->command = dmam_alloc_coherent(&dd->pdev->dev, CMD_DMA_ALLOC_SZ,
+                        &cmd->command_dma, GFP_KERNEL);
+        if (!cmd->command)
+                return -ENOMEM;
+
+        memset(cmd->command, 0, CMD_DMA_ALLOC_SZ);
+
+        /* Point the command headers at the command tables. */
+        cmd->command_header = dd->port->command_list +
+                                (sizeof(struct mtip_cmd_hdr) * request_idx);
+        cmd->command_header_dma = dd->port->command_list_dma +
+                                (sizeof(struct mtip_cmd_hdr) * request_idx);
+
+        if (host_cap_64)
+                cmd->command_header->ctbau = __force_bit2int cpu_to_le32((cmd->command_dma >> 16) >> 16);
+
+        cmd->command_header->ctba = __force_bit2int cpu_to_le32(cmd->command_dma & 0xFFFFFFFF);
+
+        sg_init_table(cmd->sg, MTIP_MAX_SG);
+        return 0;
+}
+
+static struct blk_mq_ops mtip_mq_ops = {
+        .queue_rq       = mtip_queue_rq,
+        .map_queue      = blk_mq_map_queue,
+        .alloc_hctx     = blk_mq_alloc_single_hw_queue,
+        .free_hctx      = blk_mq_free_single_hw_queue,
+        .init_request   = mtip_init_cmd,
+        .exit_request   = mtip_free_cmd,
+};
+
 /*
  * Block layer initialization function.
  *
@@ -4152,11 +3827,7 @@ static int mtip_block_initialize(struct driver_data *dd)
         if (dd->disk)
                 goto skip_create_disk; /* hw init done, before rebuild */
 
-        /* Initialize the protocol layer. */
-        wait_for_rebuild = mtip_hw_init(dd);
-        if (wait_for_rebuild < 0) {
-                dev_err(&dd->pdev->dev,
-                        "Protocol layer initialization failed\n");
+        if (mtip_hw_init(dd)) {
                 rv = -EINVAL;
                 goto protocol_init_error;
         }
@@ -4198,16 +3869,27 @@ static int mtip_block_initialize(struct driver_data *dd)
 
         mtip_hw_debugfs_init(dd);
 
-        /*
-         * if rebuild pending, start the service thread, and delay the block
-         * queue creation and add_disk()
-         */
-        if (wait_for_rebuild == MTIP_FTL_REBUILD_MAGIC)
-                goto start_service_thread;
-
 skip_create_disk:
+        memset(&dd->tags, 0, sizeof(dd->tags));
+        dd->tags.ops = &mtip_mq_ops;
+        dd->tags.nr_hw_queues = 1;
+        dd->tags.queue_depth = MTIP_MAX_COMMAND_SLOTS;
+        dd->tags.reserved_tags = 1;
+        dd->tags.cmd_size = sizeof(struct mtip_cmd);
+        dd->tags.numa_node = dd->numa_node;
+        dd->tags.flags = BLK_MQ_F_SHOULD_MERGE;
+        dd->tags.driver_data = dd;
+
+        rv = blk_mq_alloc_tag_set(&dd->tags);
+        if (rv) {
+                dev_err(&dd->pdev->dev,
+                        "Unable to allocate request queue\n");
+                rv = -ENOMEM;
+                goto block_queue_alloc_init_error;
+        }
+
         /* Allocate the request queue. */
-        dd->queue = blk_alloc_queue_node(GFP_KERNEL, dd->numa_node);
+        dd->queue = blk_mq_init_queue(&dd->tags);
         if (dd->queue == NULL) {
                 dev_err(&dd->pdev->dev,
                         "Unable to allocate request queue\n");
@@ -4215,12 +3897,25 @@ skip_create_disk:
                 goto block_queue_alloc_init_error;
         }
 
-        /* Attach our request function to the request queue. */
-        blk_queue_make_request(dd->queue, mtip_make_request);
-
         dd->disk->queue         = dd->queue;
         dd->queue->queuedata    = dd;
 
+        /* Initialize the protocol layer. */
+        wait_for_rebuild = mtip_hw_get_identify(dd);
+        if (wait_for_rebuild < 0) {
+                dev_err(&dd->pdev->dev,
+                        "Protocol layer initialization failed\n");
+                rv = -EINVAL;
+                goto init_hw_cmds_error;
+        }
+
+        /*
+         * if rebuild pending, start the service thread, and delay the block
+         * queue creation and add_disk()
+         */
+        if (wait_for_rebuild == MTIP_FTL_REBUILD_MAGIC)
+                goto start_service_thread;
+
         /* Set device limits. */
         set_bit(QUEUE_FLAG_NONROT, &dd->queue->queue_flags);
         blk_queue_max_segments(dd->queue, MTIP_MAX_SG);
@@ -4299,8 +3994,9 @@ kthread_run_error:
         del_gendisk(dd->disk);
 
 read_capacity_error:
+init_hw_cmds_error:
         blk_cleanup_queue(dd->queue);
-
+        blk_mq_free_tag_set(&dd->tags);
 block_queue_alloc_init_error:
         mtip_hw_debugfs_exit(dd);
 disk_index_error:
@@ -4349,6 +4045,9 @@ static int mtip_block_remove(struct driver_data *dd)
                                 kobject_put(kobj);
                         }
                 }
+
+                mtip_standby_drive(dd);
+
                 /*
                  * Delete our gendisk structure. This also removes the device
                  * from /dev
@@ -4361,6 +4060,7 @@ static int mtip_block_remove(struct driver_data *dd)
                         if (dd->disk->queue) {
                                 del_gendisk(dd->disk);
                                 blk_cleanup_queue(dd->queue);
+                                blk_mq_free_tag_set(&dd->tags);
                                 dd->queue = NULL;
                         } else
                                 put_disk(dd->disk);
@@ -4395,6 +4095,8 @@ static int mtip_block_remove(struct driver_data *dd)
  */
 static int mtip_block_shutdown(struct driver_data *dd)
 {
+        mtip_hw_shutdown(dd);
+
         /* Delete our gendisk structure, and cleanup the blk queue. */
         if (dd->disk) {
                 dev_info(&dd->pdev->dev,
@@ -4403,6 +4105,7 @@ static int mtip_block_shutdown(struct driver_data *dd)
                 if (dd->disk->queue) {
                         del_gendisk(dd->disk);
                         blk_cleanup_queue(dd->queue);
+                        blk_mq_free_tag_set(&dd->tags);
                 } else
                         put_disk(dd->disk);
                 dd->disk  = NULL;
@@ -4412,8 +4115,6 @@ static int mtip_block_shutdown(struct driver_data *dd)
         spin_lock(&rssd_index_lock);
         ida_remove(&rssd_index_ida, dd->index);
         spin_unlock(&rssd_index_lock);
-
-        mtip_hw_shutdown(dd);
         return 0;
 }
 
@@ -4767,8 +4468,6 @@ static void mtip_pci_remove(struct pci_dev *pdev)
                 dev_warn(&dd->pdev->dev,
                         "Completion workers still active!\n");
         }
-        /* Cleanup the outstanding commands */
-        mtip_command_cleanup(dd);
 
         /* Clean up the block layer. */
         mtip_block_remove(dd);
diff --git a/drivers/block/mtip32xx/mtip32xx.h b/drivers/block/mtip32xx/mtip32xx.h
index ffb955e7ccb9..982a88fe1ab2 100644
--- a/drivers/block/mtip32xx/mtip32xx.h
+++ b/drivers/block/mtip32xx/mtip32xx.h
@@ -331,12 +331,8 @@ struct mtip_cmd {
          */
         void (*comp_func)(struct mtip_port *port,
                                 int tag,
-                                void *data,
+                                struct mtip_cmd *cmd,
                                 int status);
-        /* Additional callback function that may be called by comp_func() */
-        void (*async_callback)(void *data, int status);
-
-        void *async_data; /* Addl. data passed to async_callback() */
 
         int scatter_ents; /* Number of scatter list entries used */
 
@@ -347,10 +343,6 @@ struct mtip_cmd {
         int retries; /* The number of retries left for this command. */
 
         int direction; /* Data transfer direction */
-
-        unsigned long comp_time; /* command completion time, in jiffies */
-
-        atomic_t active; /* declares if this command sent to the drive. */
 };
 
 /* Structure used to describe a port. */
@@ -436,12 +428,6 @@ struct mtip_port {
          * or error handling is active
          */
         unsigned long cmds_to_issue[SLOTBITS_IN_LONGS];
-        /*
-         * Array of command slots. Structure includes pointers to the
-         * command header and command table, and completion function and data
-         * pointers.
-         */
-        struct mtip_cmd commands[MTIP_MAX_COMMAND_SLOTS];
         /* Used by mtip_service_thread to wait for an event */
         wait_queue_head_t svc_wait;
         /*
@@ -452,13 +438,7 @@ struct mtip_port {
         /*
          * Timer used to complete commands that have been active for too long.
          */
-        struct timer_list cmd_timer;
         unsigned long ic_pause_timer;
-        /*
-         * Semaphore used to block threads if there are no
-         * command slots available.
-         */
-        struct semaphore cmd_slot;
 
         /* Semaphore to control queue depth of unaligned IOs */
         struct semaphore cmd_slot_unal;
@@ -485,6 +465,8 @@ struct driver_data {
 
         struct request_queue *queue; /* Our request queue. */
 
+        struct blk_mq_tag_set tags; /* blk_mq tags */
+
         struct mtip_port *port; /* Pointer to the port data structure. */
 
         unsigned product_type; /* magic value declaring the product type */