1 files changed, 61 insertions, 7 deletions

diff --git a/drivers/ata/libata-core.c b/drivers/ata/libata-core.c
index 622350d9b2e3..4214bfb13bbd 100644
--- a/drivers/ata/libata-core.c
+++ b/drivers/ata/libata-core.c
@@ -612,7 +612,7 @@ u64 ata_tf_read_block(struct ata_taskfile *tf, struct ata_device *dev)
                 if (tf->flags & ATA_TFLAG_LBA48) {
                         block |= (u64)tf->hob_lbah << 40;
                         block |= (u64)tf->hob_lbam << 32;
-                        block |= tf->hob_lbal << 24;
+                        block |= (u64)tf->hob_lbal << 24;
                 } else
                         block |= (tf->device & 0xf) << 24;
 
@@ -1712,6 +1712,8 @@ unsigned ata_exec_internal_sg(struct ata_device *dev,
         else
                 tag = 0;
 
+        if (test_and_set_bit(tag, &ap->qc_allocated))
+                BUG();
         qc = __ata_qc_from_tag(ap, tag);
 
         qc->tag = tag;
@@ -4563,6 +4565,37 @@ void swap_buf_le16(u16 *buf, unsigned int buf_words)
 }
 
 /**
+ *      ata_qc_new - Request an available ATA command, for queueing
+ *      @ap: Port associated with device @dev
+ *      @dev: Device from whom we request an available command structure
+ *
+ *      LOCKING:
+ *      None.
+ */
+
+static struct ata_queued_cmd *ata_qc_new(struct ata_port *ap)
+{
+        struct ata_queued_cmd *qc = NULL;
+        unsigned int i;
+
+        /* no command while frozen */
+        if (unlikely(ap->pflags & ATA_PFLAG_FROZEN))
+                return NULL;
+
+        /* the last tag is reserved for internal command. */
+        for (i = 0; i < ATA_MAX_QUEUE - 1; i++)
+                if (!test_and_set_bit(i, &ap->qc_allocated)) {
+                        qc = __ata_qc_from_tag(ap, i);
+                        break;
+                }
+
+        if (qc)
+                qc->tag = i;
+
+        return qc;
+}
+
+/**
  *      ata_qc_new_init - Request an available ATA command, and initialize it
  *      @dev: Device from whom we request an available command structure
  *      @tag: command tag
@@ -4571,20 +4604,16 @@ void swap_buf_le16(u16 *buf, unsigned int buf_words)
  *      None.
  */
 
-struct ata_queued_cmd *ata_qc_new_init(struct ata_device *dev, int tag)
+struct ata_queued_cmd *ata_qc_new_init(struct ata_device *dev)
 {
         struct ata_port *ap = dev->link->ap;
         struct ata_queued_cmd *qc;
 
-        if (unlikely(ap->pflags & ATA_PFLAG_FROZEN))
-                return NULL;
-
-        qc = __ata_qc_from_tag(ap, tag);
+        qc = ata_qc_new(ap);
         if (qc) {
                 qc->scsicmd = NULL;
                 qc->ap = ap;
                 qc->dev = dev;
-                qc->tag = tag;
 
                 ata_qc_reinit(qc);
         }
@@ -4592,6 +4621,31 @@ struct ata_queued_cmd *ata_qc_new_init(struct ata_device *dev, int tag)
         return qc;
 }
 
+/**
+ *      ata_qc_free - free unused ata_queued_cmd
+ *      @qc: Command to complete
+ *
+ *      Designed to free unused ata_queued_cmd object
+ *      in case something prevents using it.
+ *
+ *      LOCKING:
+ *      spin_lock_irqsave(host lock)
+ */
+void ata_qc_free(struct ata_queued_cmd *qc)
+{
+        struct ata_port *ap = qc->ap;
+        unsigned int tag;
+
+        WARN_ON(qc == NULL);    /* ata_qc_from_tag _might_ return NULL */
+
+        qc->flags = 0;
+        tag = qc->tag;
+        if (likely(ata_tag_valid(tag))) {
+                qc->tag = ATA_TAG_POISON;
+                clear_bit(tag, &ap->qc_allocated);
+        }
+}
+
 void __ata_qc_complete(struct ata_queued_cmd *qc)
 {
         struct ata_port *ap = qc->ap;