@@ -49,6 +49,10 @@
 
 #include "libata.h"
 
+#define ata_for_each_tag(tag, mask) \
+	for (tag = find_first_bit(&mask, ATA_MAX_CMDS); tag < ATA_MAX_CMDS; \
+	     tag = find_next_bit(&mask, ATA_MAX_CMDS, tag + 1))
+
 static unsigned int ata_busy_sleep (struct ata_port *ap,
 				    unsigned long tmout_pat,
 			    	    unsigned long tmout);

@@ -59,7 +63,6 @@ static int fgb(u32 bitmap);
 static int ata_choose_xfer_mode(struct ata_port *ap,
 				u8 *xfer_mode_out,
 				unsigned int *xfer_shift_out);
-static int ata_qc_complete_noop(struct ata_queued_cmd *qc, u8 drv_stat);
 static void __ata_qc_complete(struct ata_queued_cmd *qc);
 
 static unsigned int ata_unique_id = 1;

@@ -70,6 +73,87 @@ MODULE_DESCRIPTION("Library module for A
 MODULE_LICENSE("GPL");
 MODULE_VERSION(DRV_VERSION);
 
+static void ata_qc_complete_noop(struct ata_queued_cmd *qc)
+{
+	/* noop */
+}
+
+static void ata_qc_exec_special_timeout(unsigned long data)
+{
+	struct completion *wait = (void *)data;
+	complete(wait);
+}
+
+/**
+ *	ata_qc_exec_special - execute libata internal special command
+ *	@qc: Command to execute
+ *	@tmout: timeout in jiffies
+ *
+ *	Executes libata internal command with timeout.  Timeout and
+ *	error conditions are reported via return value.  No recovery
+ *	action is taken after a command times out.  It's caller's duto
+ *	to clean up after timeout.
+ *
+ *	Also, note that error condition is checked after the qc is
+ *	completed, meaning that if another command is issued before
+ *	checking the condition, we get the wrong values.  As special
+ *	cmds are used only for initialization and recovery, this
+ *	won't cause any problem currently.
+ *
+ *	LOCKING:
+ *	None.  Should be called with kernel context, might sleep.
+ */
+
+static int ata_qc_exec_special(struct ata_queued_cmd *qc, unsigned long tmout)
+{
+	struct ata_port *ap = qc->ap;
+	DECLARE_COMPLETION(wait);
+	struct timer_list timer;
+	int rc;
+
+	might_sleep();
+
+	if (ata_busy_sleep(ap, ATA_TMOUT_SPECIAL_QUICK, ATA_TMOUT_SPECIAL))
+		return -ETIMEDOUT;
+
+	qc->complete_fn = ata_qc_complete_noop;
+	qc->waiting = &wait;
+
+	if (tmout) {
+		init_timer(&timer);
+		timer.function = ata_qc_exec_special_timeout;
+		timer.data = (unsigned long)&wait;
+		timer.expires = jiffies + tmout;
+		add_timer(&timer);
+	}
+
+	spin_lock_irq(&ap->host_set->lock);
+	rc = ata_qc_issue(qc);
+	spin_unlock_irq(&ap->host_set->lock);
+
+	if (rc) {
+		if (tmout)
+			del_timer(&timer);
+		return -EAGAIN;	/* any better value for issue failure? */
+	}
+
+	wait_for_completion(&wait);
+
+	if (tmout && !del_timer(&timer)) {
+		spin_lock_irq(&ap->host_set->lock);
+		if (qc->waiting == &wait) {
+			ata_qc_complete(qc);
+			rc = -ETIMEDOUT;
+		}
+		spin_unlock_irq(&ap->host_set->lock);
+	}
+
+	if (rc == 0 && !ata_ok(ata_chk_status(ap)))
+		rc = -EIO;
+
+	return rc;
+}
+
 /**
  *	ata_tf_load - send taskfile registers to host controller
  *	@ap: Port to which output is sent

@@ -1126,9 +1210,7 @@ static void ata_dev_identify(struct ata_
 	unsigned long xfer_modes;
 	u8 status;
 	unsigned int using_edd;
-	DECLARE_COMPLETION(wait);
 	struct ata_queued_cmd *qc;
-	unsigned long flags;
 	int rc;
 
 	if (!ata_dev_present(dev)) {

@@ -1166,20 +1248,19 @@ retry:
 		DPRINTK("do ATAPI identify\n");
 	}
 
-	qc->waiting = &wait;
-	qc->complete_fn = ata_qc_complete_noop;
+	rc = ata_qc_exec_special(qc, ATA_TMOUT_IDENTIFY);
 
-	spin_lock_irqsave(&ap->host_set->lock, flags);
-	rc = ata_qc_issue(qc);
-	spin_unlock_irqrestore(&ap->host_set->lock, flags);
+	switch (rc) {
+	case 0:
+		break;
+	case -EIO:
+		/* Device failed the command */
+		status = ata_chk_status(ap);
 
-	if (rc)
-		goto err_out;
-	else
-		wait_for_completion(&wait);
+		if (!(status & ATA_ERR))
+			/* Weird error condition, give up */
+			goto err_out;
 
-	status = ata_chk_status(ap);
-	if (status & ATA_ERR) {
 		/*
 		 * arg!  EDD works for all test cases, but seems to return
 		 * the ATA signature for some ATAPI devices.  Until the

@@ -1202,6 +1283,9 @@ retry:
 				goto retry;
 			}
 		}
+		/* Fall through */
+	default:
+		/* Issue failed or some other error */
 		goto err_out;
 	}
 

@@ -1326,10 +1410,7 @@ int ata_read_log_page(struct ata_port *a
 		      char *buffer, unsigned int sectors)
 {
 	struct ata_device *dev = &ap->device[device];
-	DECLARE_COMPLETION(wait);
 	struct ata_queued_cmd *qc;
-	unsigned long flags;
-	int rc;
 
 	assert(dev->class == ATA_DEV_ATA);
 

@@ -1347,18 +1428,53 @@ int ata_read_log_page(struct ata_port *a
 	qc->tf.lbal = page;
 	qc->flags |= ATA_QCFLAG_PREEMPT;
 
-	qc->waiting = &wait;
-	qc->complete_fn = ata_qc_complete_noop;
+	return ata_qc_exec_special(qc, ATA_TMOUT_READLOG);
+}
 
-	spin_lock_irqsave(&ap->host_set->lock, flags);
-	rc = ata_qc_issue(qc);
-	spin_unlock_irqrestore(&ap->host_set->lock, flags);
+/**
+ *	ata_eh_qc_complete - Complete an ATA command from EH
+ *	@qc: Command to complete
+ *	@drv_stat: ATA Status register contents
+ *	@drv_err: ATA Error register contents
+ *
+ *	This function is used in EH to complete commands.
+ *
+ *	HACK ALERT!  We cannot use the supplied completion function
+ *	from inside the ->eh_strategy_handler() thread.  libata is the
+ *	only user of ->eh_strategy_handler() in any kernel, so the
+ *	default scsi_done() assumes it is not being called from the
+ *	SCSI EH.
+ *
+ *	LOCKING:
+ *	None.  Called from EH.
+ */
 
-	if (rc)
-		return -EIO;
+void ata_eh_qc_complete(struct ata_queued_cmd *qc)
+{
+	BUG_ON(!(qc->ap->flags & ATA_FLAG_RECOVERY));
 
-	wait_for_completion(&wait);
-	return 0;
+	qc->scsidone = scsi_finish_command;
+	__ata_qc_complete(qc);
+}
+
+/**
+ *	ata_eh_qc_retry - Retry an ATA command from EH.
+ *	@qc: Command to complete
+ *
+ *	This function is used in EH to complete commands.
+ *
+ *	HACK ALERT!  See ata_eh_qc_complete.
+ *
+ *	LOCKING:
+ *	None.  Called from EH.
+ */
+
+void ata_eh_qc_retry(struct ata_queued_cmd *qc)
+{
+	BUG_ON(!(qc->ap->flags & ATA_FLAG_RECOVERY));
+
+	qc->scsidone = (void *)scsi_retry_command;
+	__ata_qc_complete(qc);
 }
 
 /**

@@ -2133,10 +2249,8 @@ static int ata_choose_xfer_mode(struct a
 
 static void ata_dev_set_xfermode(struct ata_port *ap, struct ata_device *dev)
 {
-	DECLARE_COMPLETION(wait);
 	struct ata_queued_cmd *qc;
 	int rc;
-	unsigned long flags;
 
 	/* set up set-features taskfile */
 	DPRINTK("set features - xfer mode\n");

@@ -2150,17 +2264,13 @@ static void ata_dev_set_xfermode(struct 
 	qc->tf.protocol = ATA_PROT_NODATA;
 	qc->tf.nsect = dev->xfer_mode;
 
-	qc->waiting = &wait;
-	qc->complete_fn = ata_qc_complete_noop;
-
-	spin_lock_irqsave(&ap->host_set->lock, flags);
-	rc = ata_qc_issue(qc);
-	spin_unlock_irqrestore(&ap->host_set->lock, flags);
-
-	if (rc)
+	rc = ata_qc_exec_special(qc, ATA_TMOUT_SET_XFERMODE);
+	if (rc) {
+		printk(KERN_ERR "ata%u: failed to set xfermode, disabling... "
+		       "(rc=%d stat=0x%x err=0x%x)\n",
+		       ap->id, rc, ata_chk_status(ap), ata_chk_err(ap));
 		ata_port_disable(ap);
-	else
-		wait_for_completion(&wait);
+	}
 
 	DPRINTK("EXIT\n");
 }

@@ -2530,7 +2640,7 @@ static void ata_pio_complete (struct ata
 
 	ata_irq_on(ap);
 
-	ata_qc_complete(qc, drv_stat);
+	ata_qc_complete(qc);
 }
 
 

@@ -2755,7 +2865,10 @@ static void ata_pio_block(struct ata_por
 
 			ata_irq_on(ap);
 
-			ata_qc_complete(qc, status);
+			if (!(status & (ATA_ERR | ATA_DRQ)))
+				ata_qc_complete(qc);
+			else
+				ata_qc_error(qc);
 			return;
 		}
 

@@ -2787,7 +2900,7 @@ static void ata_pio_error(struct ata_por
 
 	ata_irq_on(ap);
 
-	ata_qc_complete(qc, drv_stat | ATA_ERR);
+	ata_qc_error(qc);
 }
 
 static void ata_pio_task(void *_data)

@@ -2828,9 +2941,7 @@ static void ata_pio_task(void *_data)
 static void atapi_request_sense(struct ata_port *ap, struct ata_device *dev,
 				struct scsi_cmnd *cmd)
 {
-	DECLARE_COMPLETION(wait);
 	struct ata_queued_cmd *qc;
-	unsigned long flags;
 	int rc;
 
 	DPRINTK("ATAPI request sense\n");

@@ -2855,31 +2966,34 @@ static void atapi_request_sense(struct a
 	qc->tf.lbam = (8 * 1024) & 0xff;
 	qc->tf.lbah = (8 * 1024) >> 8;
 	qc->nbytes = SCSI_SENSE_BUFFERSIZE;
+	qc->flags |= ATA_QCFLAG_PREEMPT;
 
-	qc->waiting = &wait;
-	qc->complete_fn = ata_qc_complete_noop;
-
-	spin_lock_irqsave(&ap->host_set->lock, flags);
-	rc = ata_qc_issue(qc);
-	spin_unlock_irqrestore(&ap->host_set->lock, flags);
+	rc = ata_qc_exec_special(qc, ATA_TMOUT_REQUEST_SENSE);
+	if (rc) {
+		printk(KERN_WARNING "ata%u: failed to read sense data "
+		       "(rc=%d stat=0x%x err=0x%x)\n",
+		       ap->id, rc, ata_chk_status(ap), ata_chk_err(ap));
+		/*
+		 * If this happens, we're returning w/ NULL sense
+		 * data.  Upper layer can handle it.
+		 */
+	}
 
-	if (rc)
-		ata_port_disable(ap);
-	else
-		wait_for_completion(&wait);
+	cmd->result = SAM_STAT_CHECK_CONDITION;
 
 	DPRINTK("EXIT\n");
 }
 
 /**
- *	ata_qc_timeout - Handle timeout of queued command
- *	@qc: Command that timed out
+ *	ata_error_handler - Handle error or timeout of queued command
+ *	@ap: Port on which the command is active
  *
- *	Some part of the kernel (currently, only the SCSI layer)
- *	has noticed that the active command on port @ap has not
- *	completed after a specified length of time.  Handle this
- *	condition by disabling DMA (if necessary) and completing
- *	transactions, with error if necessary.
+ *	The driver reported error or some part of the kernel
+ *	(currently, only the SCSI layer) has noticed that the active
+ *	command on port @ap has not completed after a specified length
+ *	of time.  Handle this condition by disabling DMA (if
+ *	necessary) and completing transactions, with error if
+ *	necessary.
  *
  *	This also handles the case of the "lost interrupt", where
  *	for some reason (possibly hardware bug, possibly driver bug)

@@ -2890,102 +3004,65 @@ static void atapi_request_sense(struct a
  *	Inherited from SCSI layer (none, can sleep)
  */
 
-static void ata_qc_timeout(struct ata_queued_cmd *qc)
+void ata_error_handler(struct ata_port *ap)
 {
-	struct ata_port *ap = qc->ap;
-	struct ata_device *dev = qc->dev;
-	u8 host_stat = 0, drv_stat;
+	struct ata_queued_cmd *qc;
+	u8 host_stat = 0, drv_stat, drv_err;
 
 	DPRINTK("ENTER\n");
 
-	/* FIXME: doesn't this conflict with timeout handling? */
-	if (qc->dev->class == ATA_DEV_ATAPI && qc->scsicmd) {
-		struct scsi_cmnd *cmd = qc->scsicmd;
-
-		if (!scsi_eh_eflags_chk(cmd, SCSI_EH_CANCEL_CMD)) {
-
-			/* finish completing original command */
-			__ata_qc_complete(qc);
-
-			atapi_request_sense(ap, dev, cmd);
-
-			cmd->result = (CHECK_CONDITION << 1) | (DID_OK << 16);
-			scsi_finish_command(cmd);
-
-			goto out;
-		}
+	qc = ata_qc_from_tag(ap, ap->active_tag);
+	if (!qc) {
+		printk(KERN_ERR "ata%u: BUG: timeout without command\n",
+		       ap->id);
+		goto out;
 	}
 
-	/* hack alert!  We cannot use the supplied completion
-	 * function from inside the ->eh_strategy_handler() thread.
-	 * libata is the only user of ->eh_strategy_handler() in
-	 * any kernel, so the default scsi_done() assumes it is
-	 * not being called from the SCSI EH.
-	 */
-	qc->scsidone = scsi_finish_command;
-
-	switch (qc->tf.protocol) {
-
-	case ATA_PROT_DMA:
-	case ATA_PROT_ATAPI_DMA:
-		host_stat = ap->ops->bmdma_status(ap);
-
-		/* before we do anything else, clear DMA-Start bit */
-		ap->ops->bmdma_stop(ap);
-
-		/* fall through */
-
-	default:
-		ata_altstatus(ap);
+	if (qc->flags & ATA_QCFLAG_ERROR) {
 		drv_stat = ata_chk_status(ap);
+		drv_err = ata_chk_err(ap);
+	} else {
+		/*
+		 * Okay, command has timed out.  Currently all we do
+		 * is stopping the dma engine.  Maybe performing
+		 * ->phy_reset is useful to make the device online
+		 * again.  However, this was all that the original
+		 * code did, so, for now, leave it as it is.
+		 */
+		switch (qc->tf.protocol) {
 
-		/* ack bmdma irq events */
-		ap->ops->irq_clear(ap);
+		case ATA_PROT_DMA:
+		case ATA_PROT_ATAPI_DMA:
+			host_stat = ap->ops->bmdma_status(ap);
 
-		printk(KERN_ERR "ata%u: command 0x%x timeout, stat 0x%x host_stat 0x%x\n",
-		       ap->id, qc->tf.command, drv_stat, host_stat);
+			/* before we do anything else, clear DMA-Start bit */
+			ap->ops->bmdma_stop(ap);
 
-		/* complete taskfile transaction */
-		ata_qc_complete(qc, drv_stat);
-		break;
-	}
-out:
-	DPRINTK("EXIT\n");
-}
+			/* fall through */
 
-/**
- *	ata_eng_timeout - Handle timeout of queued command
- *	@ap: Port on which timed-out command is active
- *
- *	Some part of the kernel (currently, only the SCSI layer)
- *	has noticed that the active command on port @ap has not
- *	completed after a specified length of time.  Handle this
- *	condition by disabling DMA (if necessary) and completing
- *	transactions, with error if necessary.
- *
- *	This also handles the case of the "lost interrupt", where
- *	for some reason (possibly hardware bug, possibly driver bug)
- *	an interrupt was not delivered to the driver, even though the
- *	transaction completed successfully.
- *
- *	LOCKING:
- *	Inherited from SCSI layer (none, can sleep)
- */
+		default:
+			ata_altstatus(ap);
+			drv_stat = ata_chk_status(ap);
+			drv_err = ata_chk_err(ap);
 
-void ata_eng_timeout(struct ata_port *ap)
-{
-	struct ata_queued_cmd *qc;
+			/* ack bmdma irq events */
+			ap->ops->irq_clear(ap);
 
-	DPRINTK("ENTER\n");
+			printk(KERN_ERR "ata%u: command 0x%x timeout, stat 0x%x host_stat 0x%x\n",
+			       ap->id, qc->tf.command, drv_stat, host_stat);
+			break;
+		}
+	}
 
-	qc = ata_qc_from_tag(ap, ap->active_tag);
-	if (!qc) {
-		printk(KERN_ERR "ata%u: BUG: timeout without command\n",
-		       ap->id);
-		goto out;
+	if (qc->scsicmd) {
+		if (qc->dev->class == ATA_DEV_ATA ||
+		    !(qc->flags & ATA_QCFLAG_ERROR))
+			ata_to_sense_error(qc, drv_stat, drv_err);
+		else
+			atapi_request_sense(ap, qc->dev, qc->scsicmd);
 	}
 
-	ata_qc_timeout(qc);
+	ata_eh_qc_complete(qc);
 
 out:
 	DPRINTK("EXIT\n");

@@ -3049,100 +3126,142 @@ struct ata_queued_cmd *ata_qc_new_init(s
 	return qc;
 }
 
-static int ata_qc_complete_noop(struct ata_queued_cmd *qc, u8 drv_stat)
-{
-	return 0;
-}
+/**
+ *	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_set lock)
+ *
+ */
 
-static void __ata_qc_complete(struct ata_queued_cmd *qc)
+void ata_qc_free(struct ata_queued_cmd *qc)
 {
 	struct ata_port *ap = qc->ap;
-	unsigned int tag, do_clear = 0;
+	unsigned int tag = qc->tag;
+
+	assert(ata_tag_valid(qc->tag));
 
 	if (likely(qc->flags & ATA_QCFLAG_ACTIVE)) {
-		assert(ap->queue_depth);
-		ap->queue_depth--;
+		assert(ap->flags & ATA_FLAG_INFLIGHT);
 
-		if (!ap->queue_depth)
-			ap->flags &= ~ATA_FLAG_NCQ_QUEUED;
+		if (ap->active_tag == ATA_TAG_POISON) {
+			assert(ap->sactive & (1 << tag));
+			ap->sactive &= ~(1 << tag);
+			if (!ap->sactive)
+				ap->flags &= ~ATA_FLAG_INFLIGHT;
+		} else {
+			assert(ap->active_tag == tag &&
+			       (qc->flags & ATA_QCFLAG_PREEMPT || !ap->sactive));
+			ap->active_tag = ap->preempted_tag;
+			ap->preempted_tag = ATA_TAG_POISON;
+			if (ap->active_tag == ATA_TAG_POISON && !ap->sactive)
+				ap->flags &= ~ATA_FLAG_INFLIGHT;
+		}
 	}
 
 	qc->flags = 0;
-	tag = qc->tag;
-	if (likely(ata_tag_valid(tag))) {
-		if (tag == ap->active_tag)
-			ap->active_tag = ATA_TAG_POISON;
-		qc->tag = ATA_TAG_POISON;
-		do_clear = 1;
-	}
-
-	if (qc->waiting) {
-		struct completion *waiting = qc->waiting;
-		qc->waiting = NULL;
-		complete(waiting);
-	}
+	qc->tag = ATA_TAG_POISON;
 
-	if (likely(do_clear))
-		clear_bit(tag, &ap->qactive);
+	clear_bit(tag, &ap->qactive);
 	if (ap->cmd_waiters)
 		wake_up(&ap->cmd_wait_queue);
 }
 
+static void __ata_qc_complete(struct ata_queued_cmd *qc)
+{
+	struct completion *waiting = qc->waiting;
+
+	assert(qc != NULL);	/* ata_qc_from_tag _might_ return NULL */
+	assert(qc->flags & ATA_QCFLAG_ACTIVE);
+
+	if (likely(qc->flags & ATA_QCFLAG_DMAMAP))
+		ata_sg_clean(qc);
+
+	/* call completion callback */
+	qc->complete_fn(qc);
+	/* for special cmd timeout synchronization */
+	qc->waiting = NULL;
+
+	ata_qc_free(qc);
+
+	if (waiting)
+		complete(waiting);
+
+	VPRINTK("EXIT\n");
+}
+
 /**
- *	ata_qc_free - free unused ata_queued_cmd
+ *	ata_qc_complete - Complete an active ATA command
  *	@qc: Command to complete
  *
- *	Designed to free unused ata_queued_cmd object
- *	in case something prevents using it.
+ *	Indicate to the mid and upper layers that an ATA
+ *	command has completed, with either an ok or not-ok status.
  *
  *	LOCKING:
  *	spin_lock_irqsave(host_set lock)
  *
  */
-void ata_qc_free(struct ata_queued_cmd *qc)
+void ata_qc_complete(struct ata_queued_cmd *qc)
 {
-	assert(qc != NULL);	/* ata_qc_from_tag _might_ return NULL */
-	assert(qc->waiting == NULL);	/* nothing should be waiting */
+	if (unlikely((qc->flags & ATA_QCFLAG_ERROR ||
+		      qc->ap->flags & ATA_FLAG_RECOVERY) &&
+		     !(qc->flags & ATA_QCFLAG_PREEMPT))) {
+		printk(KERN_WARNING "ata%u: ignoring command completion for "
+		       "tag %u during recovery\n", qc->ap->id, qc->tag);
+		return;
+	}
 
 	__ata_qc_complete(qc);
 }
 
 /**
- *	ata_qc_complete - Complete an active ATA command
- *	@qc: Command to complete
- *	@drv_stat: ATA Status register contents
+ *	ata_qc_error - Invoke error handler
+ *	@qc: Command which failed
  *
- *	Indicate to the mid and upper layers that an ATA
- *	command has completed, with either an ok or not-ok status.
+ *	Invoke EH for a failed qc.
  *
  *	LOCKING:
  *	spin_lock_irqsave(host_set lock)
  *
  */
 
-void ata_qc_complete(struct ata_queued_cmd *qc, u8 drv_stat)
+void ata_qc_error(struct ata_queued_cmd *qc)
 {
-	int rc;
-
-	assert(qc != NULL);	/* ata_qc_from_tag _might_ return NULL */
-	assert(qc->flags & ATA_QCFLAG_ACTIVE);
-
-	if (likely(qc->flags & ATA_QCFLAG_DMAMAP))
-		ata_sg_clean(qc);
-
-	/* call completion callback */
-	rc = qc->complete_fn(qc, drv_stat);
-
-	/* if callback indicates not to complete command (non-zero),
-	 * return immediately
-	 */
-	if (unlikely(rc != 0))
+	if (unlikely((qc->flags & ATA_QCFLAG_ERROR ||
+		      qc->ap->flags & ATA_FLAG_RECOVERY) &&
+		     !(qc->flags & ATA_QCFLAG_PREEMPT))) {
+		printk(KERN_WARNING "ata%u: ignoring command error for "
+		       "tag %u during recovery\n", qc->ap->id, qc->tag);
 		return;
+	}
 
-	__ata_qc_complete(qc);
-	qc->flags &= ~ATA_QCFLAG_ACTIVE;
+	if (qc->scsicmd) {
+		/*
+		 *  SCSI command.  Invoke SCSI EH
+		 */
+		printk(KERN_WARNING "ata%u: requesting check condition for "
+		       "failed scmd %p tag %u\n",
+		       qc->ap->id, qc->scsicmd, qc->tag);
 
-	VPRINTK("EXIT\n");
+		qc->flags |= ATA_QCFLAG_ERROR;
+		qc->ap->flags |= ATA_FLAG_ERROR;
+
+		qc->scsicmd->result = SAM_STAT_CHECK_CONDITION;
+		qc->scsidone(qc->scsicmd);
+	} else {
+		/*
+		 * libata internal command.  No tender and kind error
+		 * handling yet.  Just finish'em.
+		 */
+		printk(KERN_WARNING "ata%u: finishing internal command tag %u "
+		       "with error\n", qc->ap->id, qc->tag);
+		BUG_ON(qc->flags & ATA_QCFLAG_NCQ);
+		ata_qc_complete(qc);
+	}
 }
 
 static inline int ata_should_dma_map(struct ata_queued_cmd *qc)

@@ -3179,6 +3298,14 @@ static inline int ata_qc_issue_ok(struct
 {
 	if (qc->flags & ATA_QCFLAG_PREEMPT)
 		return 1;
+
+	/*
+	 * If error handling is in progress, only preempt commands are
+	 * allowed.
+	 */
+	if (qc->ap->flags & (ATA_FLAG_ERROR | ATA_FLAG_RECOVERY))
+		return 0;
+
 	/*
 	 * if people are already waiting for a queue drain, don't allow a
 	 * new 'lucky' queuer to get in there

@@ -3189,7 +3316,7 @@ static inline int ata_qc_issue_ok(struct
 	/*
 	 * If nothing is queued, it's always ok to continue.
 	 */
-	if (!ap->queue_depth)
+	if (!(ap->flags & ATA_FLAG_INFLIGHT))
 		return 1;
 
 	/*

@@ -3203,7 +3330,7 @@ static inline int ata_qc_issue_ok(struct
 	 * Command is NCQ, allow it to be queued if the commands that are
 	 * currently running are also NCQ
 	 */
-	if (ap->flags & ATA_FLAG_NCQ_QUEUED)
+	if (ap->sactive)
 		return 1;
 
 	return 0;

@@ -3281,13 +3408,25 @@ int ata_qc_issue(struct ata_queued_cmd *
 
 	ap->ops->qc_prep(qc);
 
-	qc->ap->active_tag = qc->tag;
 	qc->flags |= ATA_QCFLAG_ACTIVE;
 
-	if (qc->flags & ATA_QCFLAG_NCQ)
-		ap->flags |= ATA_FLAG_NCQ_QUEUED;
+	assert(ap->preempted_tag == ATA_TAG_POISON);
 
-	ap->queue_depth++;
+	if (!(qc->flags & ATA_QCFLAG_PREEMPT)) {
+		assert(ap->active_tag == ATA_TAG_POISON);
+
+		if (qc->flags & ATA_QCFLAG_NCQ)
+			ap->sactive |= 1 << qc->tag;
+		else {
+			assert(!ap->sactive);
+			ap->active_tag = qc->tag;
+		}
+	} else {
+		ap->preempted_tag = ap->active_tag;
+		ap->active_tag = qc->tag;
+	}
+
+	ap->flags |= ATA_FLAG_INFLIGHT;
 
 	rc = ap->ops->qc_issue(qc);
 	if (rc != ATA_QC_ISSUE_OK)

@@ -3655,7 +3794,10 @@ inline unsigned int ata_host_intr (struc
 		ap->ops->irq_clear(ap);
 
 		/* complete taskfile transaction */
-		ata_qc_complete(qc, status);
+		if (!(status & (ATA_ERR | ATA_DRQ)))
+			ata_qc_complete(qc);
+		else
+			ata_qc_error(qc);
 		break;
 
 	default:

@@ -3723,6 +3865,236 @@ irqreturn_t ata_interrupt (int irq, void
 	return IRQ_RETVAL(handled);
 }
 
+/*
+ * NCQ helpers
+ */
+
+/**
+ *     ata_ncq_complete - NCQ driver helper.  Complete requests normally.
+ *     @ap: port in question
+ *
+ *     Complete in-flight commands.  One device per port is assumed.
+ *     This functions is meant to be called from specific driver's
+ *     interrupt routine to complete requests normally.  On
+ *     invocation, if non-NCQ command was in-flight, it's completed
+ *     normally.  If NCQ commands were in-flight, Sactive register is
+ *     read and completed commands are processed.
+ *
+ *     LOCKING:
+ *     spin_lock_irqsave(host_set lock)
+ */
+int ata_ncq_complete(struct ata_port *ap)
+{
+	int nr_done = 0;
+	unsigned long done_mask = 0;
+	unsigned tag;
+
+	/*
+	 * ap->active_tag test should come before ap->sactive test to
+	 * complete EH requests.
+	 */
+	if (ap->active_tag != ATA_TAG_POISON)
+		done_mask = 1 << ap->active_tag;
+	else if (ap->sactive) {
+		unsigned long new_sactive = scr_read(ap, SCR_ACTIVE);
+		done_mask = new_sactive ^ ap->sactive;
+
+		if (unlikely(done_mask & new_sactive)) {
+			printk(KERN_ERR "ata%u: illegal sactive transition (%08lx->%08lx)\n",
+			       ap->id, ap->sactive, new_sactive);
+			done_mask &= ~new_sactive;
+		}
+	}
+
+	ata_for_each_tag(tag, done_mask) {
+		struct ata_queued_cmd *qc = ata_qc_from_tag(ap, tag);
+		assert(qc);
+		ata_qc_complete(qc);
+		nr_done++;
+	}
+
+	return nr_done;
+}
+
+/**
+ *     ata_ncq_error - NCQ driver helper.  Abort commands and invoke EH.
+ *     @ap: port in question.
+ *
+ *     Abort in-flight commands and invoke EH.  This function is
+ *     meant to be called from specific driver's interrupt routine to
+ *     indicate error.
+ *
+ *     LOCKING:
+ *     spin_lock_irqsave(host_set lock)
+ */
+
+int ata_ncq_error(struct ata_port *ap)
+{
+	int nr_aborted = 0;
+	unsigned long sactive = 0;
+	unsigned tag;
+
+	printk(KERN_WARNING "ata%u: aborting commands due to error.  "
+	       "active_tag %d, sactive %08lx\n",
+	       ap->id,
+	       ap->active_tag != ATA_TAG_POISON ? ap->active_tag : -1,
+	       ap->sactive);
+
+	/*
+	 * ap->active_tag test should come before ap->sactive test to
+	 * complete EH requests.
+	 */
+	if (ap->active_tag != ATA_TAG_POISON)
+		sactive = 1 << ap->active_tag;
+	else if (ap->sactive) {
+		/* Complete successful requests before aborting. */
+		ata_ncq_complete(ap);
+		if (!ap->sactive)
+			printk(KERN_WARNING "ata%u: device reports successful "
+			       "completion of all NCQ commands after error\n",
+			       ap->id);
+		sactive = ap->sactive;
+	}
+
+	ata_for_each_tag(tag, sactive) {
+		struct ata_queued_cmd *qc = ata_qc_from_tag(ap, tag);
+		assert(qc);
+		ata_qc_error(qc);
+		nr_aborted++;
+	}
+
+	return nr_aborted;
+}
+
+static inline int ata_read_log_10h(struct ata_port *ap, unsigned *tagp,
+				   u8 *drv_statp, u8 *drv_errp)
+{
+	char *buffer;
+	int rc;
+
+	buffer = kmalloc(512, GFP_KERNEL);
+	if (buffer == NULL) {
+		printk(KERN_ERR "ata%u: unable to allocate memory for error\n",
+		       ap->id);
+		return -ENOMEM;
+	}
+
+	rc = ata_read_log_page(ap, 0, READ_LOG_SATA_NCQ_PAGE, buffer, 1);
+	if (rc < 0) {
+		printk(KERN_ERR "ata%u: failed to read log page 10h (%d)\n",
+		       ap->id, rc);
+		goto out;
+	}
+
+	if (buffer[0] & 0x80) {
+		printk(KERN_WARNING "ata%u: NQ bit set on log page 10h\n",
+		       ap->id);
+		rc = -EIO;
+		goto out;
+	}
+
+	*tagp = buffer[0] & 0x1f;
+	*drv_statp = buffer[2] | ATA_ERR;
+	*drv_errp = buffer[3];
+	rc = 0;
+ out:
+	kfree(buffer);
+	return rc;
+}
+
+/**
+ *     ata_ncq_recover - NCQ driver helper.  Recover from error.
+ *     @ap: port in question
+ *     @did_reset: specific driver performed reset.  Log page 10h might
+ *                 be invalid.
+ *
+ *     This function is to be called from eng_timeout routine of
+ *     specific drivers.  Before calling this function, specific
+ *     drivers are required to
+ *
+ *     - Clear all in-flight requests.  Drivers only have to clear
+ *       low-level state (like stopping DMA engine and clearing
+ *       interrupts).  All generic command cancelling are dealt by
+ *       libata-core layer.
+ *
+ *     - Make the controller ready for new commands.
+ *
+ *     LOCKING:
+ *     Inherited from SCSI layer (in EH context, can sleep)
+ */
+void ata_ncq_recover(struct ata_port *ap, int did_reset)
+{
+	unsigned ncq_abort_tag = ATA_TAG_POISON;
+	u8 stat = 0, err = 0;
+	unsigned long sactive;
+	unsigned tag;
+
+	DPRINTK("ENTER\n");
+
+	stat = ata_chk_status(ap);
+	err = ata_chk_err(ap);
+
+	/*
+	 * if commands have timed out or DRQ/BSY is set, device needs
+	 * to be reset.
+	 */
+	if (!(ap->flags & ATA_FLAG_ERROR) || stat & (ATA_BUSY | ATA_DRQ)) {
+		printk(KERN_WARNING "ata%u: stat=%x, issuing COMRESET\n", ap->id, stat);
+		ap->ops->phy_reset(ap);
+		did_reset = 1;
+	}
+
+	if (ap->sactive) {
+		if (ap->flags & ATA_FLAG_ERROR) {
+			u8 tstat, terr;
+			/*
+			 * Regardless of did_reset, we read log page
+			 * 10h.  The drive might need it to restart
+			 * operation.  If did_reset, we ignore the
+			 * result.
+			 */
+			if (ata_read_log_10h(ap, &tag, &tstat, &terr) == 0) {
+				printk(KERN_INFO "ata%u: log_ext_10h, tag=%d "
+				       "stat=%02x err=%02x%s\n",
+				       ap->id, tag, tstat, terr,
+				       did_reset ? " (ignored due to reset)" : "");
+				if (!did_reset) {
+					ncq_abort_tag = tag;
+					stat = tstat;
+					err = terr;
+				}
+			} else {
+				printk(KERN_WARNING "ata%u: resetting...\n",
+				       ap->id);
+				ap->ops->phy_reset(ap);
+			}
+		}
+		sactive = ap->sactive;
+	} else
+		sactive = 1 << ap->active_tag;
+
+	ata_for_each_tag(tag, sactive) {
+		struct ata_queued_cmd *qc = ata_qc_from_tag(ap, tag);
+		assert(qc);
+		if (ncq_abort_tag != ATA_TAG_POISON && tag != ncq_abort_tag) {
+			ata_eh_qc_retry(qc);
+			continue;
+		}
+
+		if (qc->scsicmd) {
+			if (qc->dev->class == ATA_DEV_ATA ||
+			    !(qc->flags & ATA_QCFLAG_ERROR))
+				ata_to_sense_error(qc, stat | ATA_ERR, err);
+			else
+				atapi_request_sense(ap, qc->dev, qc->scsicmd);
+		}
+
+		ata_eh_qc_complete(qc);
+	}
+
+	DPRINTK("EXIT\n");
+}
+
 /**
  *	atapi_packet_task - Write CDB bytes to hardware
  *	@_data: Port to which ATAPI device is attached.