Add FIS-based switching support. If controller supports FBS, it allows

several devices beyond Port Multiplier to work simultaneously, substantially
increasing performance.

2 files changed, 212 insertions, 53 deletions

diff --git a/sys/dev/ahci/ahci.c b/sys/dev/ahci/ahci.c
index ecff8a0..c54a5b1 100644
--- a/sys/dev/ahci/ahci.c
+++ b/sys/dev/ahci/ahci.c
@@ -78,7 +78,7 @@ static void ahci_dmafini(device_t dev);
 static void ahci_slotsalloc(device_t dev);
 static void ahci_slotsfree(device_t dev);
 static void ahci_reset(device_t dev);
-static void ahci_start(device_t dev);
+static void ahci_start(device_t dev, int fbs);
 static void ahci_stop(device_t dev);
 static void ahci_clo(device_t dev);
 static void ahci_start_fr(device_t dev);
@@ -86,6 +86,7 @@ static void ahci_stop_fr(device_t dev);
 
 static int ahci_sata_connect(struct ahci_channel *ch);
 static int ahci_sata_phy_reset(device_t dev);
+static int ahci_wait_ready(device_t dev, int t);
 
 static void ahci_issue_read_log(device_t dev);
 static void ahci_process_read_log(device_t dev, union ccb *ccb);
@@ -108,6 +109,7 @@ static struct {
 #define AHCI_Q_4CH	32
 #define AHCI_Q_EDGEIS	64
 #define AHCI_Q_SATA2	128
+#define AHCI_Q_NOBSYRES	256
 } ahci_ids[] = {
 	{0x43801002, 0x00, "ATI IXP600",	0},
 	{0x43901002, 0x00, "ATI IXP700",	0},
@@ -162,8 +164,8 @@ static struct {
 	{0x612111ab, 0x00, "Marvell 88SX6121",	AHCI_Q_NOFORCE|AHCI_Q_2CH|AHCI_Q_EDGEIS},
 	{0x614111ab, 0x00, "Marvell 88SX6141",	AHCI_Q_NOFORCE|AHCI_Q_4CH|AHCI_Q_EDGEIS},
 	{0x614511ab, 0x00, "Marvell 88SX6145",	AHCI_Q_NOFORCE|AHCI_Q_4CH|AHCI_Q_EDGEIS},
-	{0x91231b4b, 0x11, "Marvell 88SE912x",	0},
-	{0x91231b4b, 0x00, "Marvell 88SE912x",	AHCI_Q_EDGEIS|AHCI_Q_SATA2},
+	{0x91231b4b, 0x11, "Marvell 88SE912x",	AHCI_Q_NOBSYRES},
+	{0x91231b4b, 0x00, "Marvell 88SE912x",	AHCI_Q_EDGEIS|AHCI_Q_SATA2|AHCI_Q_NOBSYRES},
 	{0x044c10de, 0x00, "NVIDIA MCP65",	0},
 	{0x044d10de, 0x00, "NVIDIA MCP65",	0},
 	{0x044e10de, 0x00, "NVIDIA MCP65",	0},
@@ -379,14 +381,16 @@ ahci_attach(device_t dev)
 	/* Announce HW capabilities. */
 	speed = (ctlr->caps & AHCI_CAP_ISS) >> AHCI_CAP_ISS_SHIFT;
 	device_printf(dev,
-		    "AHCI v%x.%02x with %d %sGbps ports, Port Multiplier %s\n",
+		    "AHCI v%x.%02x with %d %sGbps ports, Port Multiplier %s%s\n",
 		    ((version >> 20) & 0xf0) + ((version >> 16) & 0x0f),
 		    ((version >> 4) & 0xf0) + (version & 0x0f),
 		    (ctlr->caps & AHCI_CAP_NPMASK) + 1,
 		    ((speed == 1) ? "1.5":((speed == 2) ? "3":
 		    ((speed == 3) ? "6":"?"))),
 		    (ctlr->caps & AHCI_CAP_SPM) ?
-		    "supported" : "not supported");
+		    "supported" : "not supported",
+		    (ctlr->caps & AHCI_CAP_FBSS) ?
+		    " with FBS" : "");
 	if (bootverbose) {
 		device_printf(dev, "Caps:%s%s%s%s%s%s%s%s %sGbps",
 		    (ctlr->caps & AHCI_CAP_64BIT) ? " 64bit":"",
@@ -419,7 +423,7 @@ ahci_attach(device_t dev)
 		    (ctlr->caps2 & AHCI_CAP2_BOH) ? " BOH":"");
 	}
 	if (bootverbose && (ctlr->caps & AHCI_CAP_EMS)) {
-		device_printf(dev, "EM Caps: %s%s%s%s%s%s%s%s\n",
+		device_printf(dev, "EM Caps:%s%s%s%s%s%s%s%s\n",
 		    (ctlr->capsem & AHCI_EM_PM) ? " PM":"",
 		    (ctlr->capsem & AHCI_EM_ALHD) ? " ALHD":"",
 		    (ctlr->capsem & AHCI_EM_XMT) ? " XMT":"",
@@ -810,6 +814,7 @@ ahci_ch_attach(device_t dev)
 	struct ahci_channel *ch = device_get_softc(dev);
 	struct cam_devq *devq;
 	int rid, error, i, sata_rev = 0;
+	u_int32_t version;
 
 	ch->dev = dev;
 	ch->unit = (intptr_t)device_get_ivars(dev);
@@ -861,6 +866,18 @@ ahci_ch_attach(device_t dev)
 		error = ENXIO;
 		goto err1;
 	}
+	ch->chcaps = ATA_INL(ch->r_mem, AHCI_P_CMD);
+	version = ATA_INL(ctlr->r_mem, AHCI_VS);
+	if (version < 0x00010020 && (ctlr->caps & AHCI_CAP_FBSS))
+		ch->chcaps |= AHCI_P_CMD_FBSCP;
+	if (bootverbose) {
+		device_printf(dev, "Caps:%s%s%s%s%s\n",
+		    (ch->chcaps & AHCI_P_CMD_HPCP) ? " HPCP":"",
+		    (ch->chcaps & AHCI_P_CMD_MPSP) ? " MPSP":"",
+		    (ch->chcaps & AHCI_P_CMD_CPD) ? " CPD":"",
+		    (ch->chcaps & AHCI_P_CMD_ESP) ? " ESP":"",
+		    (ch->chcaps & AHCI_P_CMD_FBSCP) ? " FBSCP":"");
+	}
 	/* Create the device queue for our SIM. */
 	devq = cam_simq_alloc(ch->numslots);
 	if (devq == NULL) {
@@ -977,7 +994,7 @@ ahci_ch_resume(device_t dev)
 	     ((ch->pm_level == 2 || ch->pm_level == 3) ? AHCI_P_CMD_ALPE : 0) |
 	     ((ch->pm_level > 2) ? AHCI_P_CMD_ASP : 0 )));
 	ahci_start_fr(dev);
-	ahci_start(dev);
+	ahci_start(dev, 1);
 	return (0);
 }
 
@@ -1007,6 +1024,7 @@ ahci_dmainit(device_t dev)
 {
 	struct ahci_channel *ch = device_get_softc(dev);
 	struct ahci_dc_cb_args dcba;
+	size_t rfsize;
 
 	if (ch->caps & AHCI_CAP_64BIT)
 		ch->dma.max_address = BUS_SPACE_MAXADDR;
@@ -1028,16 +1046,20 @@ ahci_dmainit(device_t dev)
 	}
 	ch->dma.work_bus = dcba.maddr;
 	/* FIS receive area. */
-	if (bus_dma_tag_create(bus_get_dma_tag(dev), 4096, 0,
+	if (ch->chcaps & AHCI_P_CMD_FBSCP)
+	    rfsize = 4096;
+	else
+	    rfsize = 256;
+	if (bus_dma_tag_create(bus_get_dma_tag(dev), rfsize, 0,
 	    ch->dma.max_address, BUS_SPACE_MAXADDR,
-	    NULL, NULL, 4096, 1, 4096,
+	    NULL, NULL, rfsize, 1, rfsize,
 	    0, NULL, NULL, &ch->dma.rfis_tag))
 		goto error;
 	if (bus_dmamem_alloc(ch->dma.rfis_tag, (void **)&ch->dma.rfis, 0,
 	    &ch->dma.rfis_map))
 		goto error;
 	if (bus_dmamap_load(ch->dma.rfis_tag, ch->dma.rfis_map, ch->dma.rfis,
-	    4096, ahci_dmasetupc_cb, &dcba, 0) || dcba.error) {
+	    rfsize, ahci_dmasetupc_cb, &dcba, 0) || dcba.error) {
 		bus_dmamem_free(ch->dma.rfis_tag, ch->dma.rfis, ch->dma.rfis_map);
 		goto error;
 	}
@@ -1225,7 +1247,7 @@ ahci_ch_intr(void *data)
 	struct ahci_channel *ch = device_get_softc(dev);
 	uint32_t istatus, sstatus, cstatus, serr = 0, sntf = 0, ok, err;
 	enum ahci_err_type et;
-	int i, ccs, ncq_err = 0;
+	int i, ccs, port;
 
 	/* Read and clear interrupt statuses. */
 	istatus = ATA_INL(ch->r_mem, AHCI_P_IS);
@@ -1238,7 +1260,17 @@ ahci_ch_intr(void *data)
 	if (istatus & AHCI_P_IX_SDB) {
 		if (ch->caps & AHCI_CAP_SSNTF)
 			sntf = ATA_INL(ch->r_mem, AHCI_P_SNTF);
-		else {
+		else if (ch->fbs_enabled) {
+			u_int8_t *fis = ch->dma.rfis + 0x58;
+
+			for (i = 0; i < 16; i++) {
+				if (fis[1] & 0x80) {
+					fis[1] &= 0x7f;
+	    				sntf |= 1 << i;
+	    			}
+	    			fis += 256;
+	    		}
+		} else {
 			u_int8_t *fis = ch->dma.rfis + 0x58;
 
 			if (fis[1] & 0x80)
@@ -1257,18 +1289,35 @@ ahci_ch_intr(void *data)
 	/* Process command errors */
 	if (istatus & (AHCI_P_IX_OF | AHCI_P_IX_IF |
 	    AHCI_P_IX_HBD | AHCI_P_IX_HBF | AHCI_P_IX_TFE)) {
-//device_printf(dev, "%s ERROR is %08x cs %08x ss %08x rs %08x tfd %02x serr %08x\n",
-//    __func__, istatus, cstatus, sstatus, ch->rslots, ATA_INL(ch->r_mem, AHCI_P_TFD),
-//    serr);
 		ccs = (ATA_INL(ch->r_mem, AHCI_P_CMD) & AHCI_P_CMD_CCS_MASK)
 		    >> AHCI_P_CMD_CCS_SHIFT;
+//device_printf(dev, "%s ERROR is %08x cs %08x ss %08x rs %08x tfd %02x serr %08x fbs %08x ccs %d\n",
+//    __func__, istatus, cstatus, sstatus, ch->rslots, ATA_INL(ch->r_mem, AHCI_P_TFD),
+//    serr, ATA_INL(ch->r_mem, AHCI_P_FBS), ccs);
+		port = -1;
+		if (ch->fbs_enabled) {
+			uint32_t fbs = ATA_INL(ch->r_mem, AHCI_P_FBS);
+			if (fbs & AHCI_P_FBS_SDE) {
+				port = (fbs & AHCI_P_FBS_DWE)
+				    >> AHCI_P_FBS_DWE_SHIFT;
+			} else {
+				for (i = 0; i < 16; i++) {
+					if (ch->numrslotspd[i] == 0)
+						continue;
+					if (port == -1)
+						port = i;
+					else if (port != i) {
+						port = -2;
+						break;
+					}
+				}
+			}
+		}
 		err = ch->rslots & (cstatus | sstatus);
-		/* Kick controller into sane state */
-		ahci_stop(dev);
-		ahci_start(dev);
 	} else {
 		ccs = 0;
 		err = 0;
+		port = -1;
 	}
 	/* Complete all successfull commands. */
 	ok = ch->rslots & ~(cstatus | sstatus);
@@ -1292,9 +1341,14 @@ ahci_ch_intr(void *data)
 			/* XXX: reqests in loading state. */
 			if (((err >> i) & 1) == 0)
 				continue;
+			if (port >= 0 &&
+			    ch->slot[i].ccb->ccb_h.target_id != port)
+				continue;
 			if (istatus & AHCI_P_IX_TFE) {
+			    if (port != -2) {
 				/* Task File Error */
-				if (ch->numtslots == 0) {
+				if (ch->numtslotspd[
+				    ch->slot[i].ccb->ccb_h.target_id] == 0) {
 					/* Untagged operation. */
 					if (i == ccs)
 						et = AHCI_ERR_TFE;
@@ -1303,10 +1357,13 @@ ahci_ch_intr(void *data)
 				} else {
 					/* Tagged operation. */
 					et = AHCI_ERR_NCQ;
-					ncq_err = 1;
 				}
+			    } else {
+				et = AHCI_ERR_TFE;
+				ch->fatalerr = 1;
+			    }
 			} else if (istatus & AHCI_P_IX_IF) {
-				if (ch->numtslots == 0 && i != ccs)
+				if (ch->numtslots == 0 && i != ccs && port != -2)
 					et = AHCI_ERR_INNOCENT;
 				else
 					et = AHCI_ERR_SATA;
@@ -1314,8 +1371,12 @@ ahci_ch_intr(void *data)
 				et = AHCI_ERR_INVALID;
 			ahci_end_transaction(&ch->slot[i], et);
 		}
-		if (ncq_err)
-			ahci_issue_read_log(dev);
+		/*
+		 * We can't reinit port if there are some other
+		 * commands active, use resume to complete them.
+		 */
+		if (ch->rslots != 0) 
+			ATA_OUTL(ch->r_mem, AHCI_P_FBS, AHCI_P_FBS_EN | AHCI_P_FBS_DEC);
 	}
 	/* Process NOTIFY events */
 	if (sntf)
@@ -1327,24 +1388,39 @@ static int
 ahci_check_collision(device_t dev, union ccb *ccb)
 {
 	struct ahci_channel *ch = device_get_softc(dev);
+	int t = ccb->ccb_h.target_id;
 
 	if ((ccb->ccb_h.func_code == XPT_ATA_IO) &&
 	    (ccb->ataio.cmd.flags & CAM_ATAIO_FPDMA)) {
-		/* Tagged command while untagged are active. */
-		if (ch->numrslots != 0 && ch->numtslots == 0)
-			return (1);
-		/* Tagged command while tagged to other target is active. */
-		if (ch->numtslots != 0 &&
-		    ch->taggedtarget != ccb->ccb_h.target_id)
-			return (1);
 		/* Tagged command while we have no supported tag free. */
 		if (((~ch->oslots) & (0xffffffff >> (32 -
-		    ch->curr[ccb->ccb_h.target_id].tags))) == 0)
+		    ch->curr[t].tags))) == 0)
 			return (1);
+		/* If we have FBS */
+		if (ch->fbs_enabled) {
+			/* Tagged command while untagged are active. */
+			if (ch->numrslotspd[t] != 0 && ch->numtslotspd[t] == 0)
+				return (1);
+		} else {
+			/* Tagged command while untagged are active. */
+			if (ch->numrslots != 0 && ch->numtslots == 0)
+				return (1);
+			/* Tagged command while tagged to other target is active. */
+			if (ch->numtslots != 0 &&
+			    ch->taggedtarget != ccb->ccb_h.target_id)
+				return (1);
+		}
 	} else {
-		/* Untagged command while tagged are active. */
-		if (ch->numrslots != 0 && ch->numtslots != 0)
-			return (1);
+		/* If we have FBS */
+		if (ch->fbs_enabled) {
+			/* Untagged command while tagged are active. */
+			if (ch->numrslotspd[t] != 0 && ch->numtslotspd[t] != 0)
+				return (1);
+		} else {
+			/* Untagged command while tagged are active. */
+			if (ch->numrslots != 0 && ch->numtslots != 0)
+				return (1);
+		}
 	}
 	if ((ccb->ccb_h.func_code == XPT_ATA_IO) &&
 	    (ccb->ataio.cmd.flags & (CAM_ATAIO_CONTROL | CAM_ATAIO_NEEDRESULT))) {
@@ -1389,9 +1465,11 @@ ahci_begin_transaction(device_t dev, union ccb *ccb)
 	/* Update channel stats. */
 	ch->oslots |= (1 << slot->slot);
 	ch->numrslots++;
+	ch->numrslotspd[ccb->ccb_h.target_id]++;
 	if ((ccb->ccb_h.func_code == XPT_ATA_IO) &&
 	    (ccb->ataio.cmd.flags & CAM_ATAIO_FPDMA)) {
 		ch->numtslots++;
+		ch->numtslotspd[ccb->ccb_h.target_id]++;
 		ch->taggedtarget = ccb->ccb_h.target_id;
 	}
 	if ((ccb->ccb_h.func_code == XPT_ATA_IO) &&
@@ -1459,7 +1537,9 @@ ahci_execute_transaction(struct ahci_slot *slot)
 	struct ahci_cmd_list *clp;
 	union ccb *ccb = slot->ccb;
 	int port = ccb->ccb_h.target_id & 0x0f;
-	int fis_size;
+	int fis_size, i;
+	uint8_t *fis = ch->dma.rfis + 0x40;
+	uint8_t val;
 
 	/* Get a piece of the workspace for this request */
 	ctp = (struct ahci_cmd_tab *)
@@ -1481,13 +1561,18 @@ ahci_execute_transaction(struct ahci_slot *slot)
 		     (port << 12);
 	/* Special handling for Soft Reset command. */
 	if ((ccb->ccb_h.func_code == XPT_ATA_IO) &&
-	    (ccb->ataio.cmd.flags & CAM_ATAIO_CONTROL) &&
-	    (ccb->ataio.cmd.control & ATA_A_RESET)) {
-		/* Kick controller into sane state */
-		ahci_stop(dev);
-		ahci_clo(dev);
-		ahci_start(dev);
-		clp->cmd_flags |= AHCI_CMD_RESET | AHCI_CMD_CLR_BUSY;
+	    (ccb->ataio.cmd.flags & CAM_ATAIO_CONTROL)) {
+		if (ccb->ataio.cmd.control & ATA_A_RESET) {
+			/* Kick controller into sane state */
+			ahci_stop(dev);
+			ahci_clo(dev);
+			ahci_start(dev, 0);
+			clp->cmd_flags |= AHCI_CMD_RESET | AHCI_CMD_CLR_BUSY;
+		} else {
+			/* Prepare FIS receive area for check. */
+			for (i = 0; i < 20; i++)
+				fis[i] = 0xff;
+		}
 	}
 	clp->bytecount = 0;
 	clp->cmd_table_phys = htole64(ch->dma.work_bus + AHCI_CT_OFFSET +
@@ -1501,6 +1586,11 @@ ahci_execute_transaction(struct ahci_slot *slot)
 	    (ccb->ataio.cmd.flags & CAM_ATAIO_FPDMA)) {
 		ATA_OUTL(ch->r_mem, AHCI_P_SACT, 1 << slot->slot);
 	}
+	/* If FBS is enabled, set PMP port. */
+	if (ch->fbs_enabled) {
+		ATA_OUTL(ch->r_mem, AHCI_P_FBS, AHCI_P_FBS_EN |
+		    (port << AHCI_P_FBS_DEV_SHIFT));
+	}
 	/* Issue command to the controller. */
 	slot->state = AHCI_SLOT_RUNNING;
 	ch->rslots |= (1 << slot->slot);
@@ -1543,12 +1633,30 @@ ahci_execute_transaction(struct ahci_slot *slot)
 			    ATA_INL(ch->r_mem, AHCI_P_SERR));
 			et = AHCI_ERR_TIMEOUT;
 		}
-		if (et != AHCI_ERR_NONE) {
-			/* Kick controller into sane state */
-			ahci_stop(ch->dev);
-			ahci_start(ch->dev);
+		/* Marvell controllers do not wait for readyness. */
+		if ((ch->quirks & AHCI_Q_NOBSYRES) &&
+		    (ccb->ccb_h.func_code == XPT_ATA_IO) &&
+		    (ccb->ataio.cmd.flags & CAM_ATAIO_CONTROL) &&
+		    (ccb->ataio.cmd.control & ATA_A_RESET) == 0) {
+			while ((val = fis[2]) & (ATA_S_BUSY | ATA_S_DRQ)) {
+				DELAY(1000);
+				if (count++ >= timeout) {
+					device_printf(dev, "device is not "
+					    "ready after soft-reset: "
+					    "tfd = %08x\n", val);
+	    				et = AHCI_ERR_TIMEOUT;
+	    				break;
+				}
+			} 
 		}
 		ahci_end_transaction(slot, et);
+		/* Kick controller into sane state and enable FBS. */
+		if ((ccb->ccb_h.func_code == XPT_ATA_IO) &&
+		    (ccb->ataio.cmd.flags & CAM_ATAIO_CONTROL) &&
+		    (ccb->ataio.cmd.control & ATA_A_RESET) == 0) {
+			ahci_stop(ch->dev);
+			ahci_start(ch->dev, 1);
+		}
 		return;
 	}
 	/* Start command execution timeout */
@@ -1577,7 +1685,8 @@ ahci_timeout(struct ahci_slot *slot)
 		sstatus = ATA_INL(ch->r_mem, AHCI_P_SACT);
 		ccs = (ATA_INL(ch->r_mem, AHCI_P_CMD) & AHCI_P_CMD_CCS_MASK)
 		    >> AHCI_P_CMD_CCS_SHIFT;
-		if ((sstatus & (1 << slot->slot)) != 0 || ccs == slot->slot)
+		if ((sstatus & (1 << slot->slot)) != 0 || ccs == slot->slot ||
+		    ch->fbs_enabled)
 			slot->state = AHCI_SLOT_EXECUTING;
 
 		callout_reset(&slot->timeout,
@@ -1635,12 +1744,19 @@ ahci_end_transaction(struct ahci_slot *slot, enum ahci_err_type et)
 		if ((et == AHCI_ERR_TFE) ||
 		    (ccb->ataio.cmd.flags & CAM_ATAIO_NEEDRESULT)) {
 			u_int8_t *fis = ch->dma.rfis + 0x40;
-			uint16_t tfd = ATA_INL(ch->r_mem, AHCI_P_TFD);
 
 			bus_dmamap_sync(ch->dma.rfis_tag, ch->dma.rfis_map,
 			    BUS_DMASYNC_POSTREAD);
-			res->status = tfd;
-			res->error = tfd >> 8;
+			if (ch->fbs_enabled) {
+				fis += ccb->ccb_h.target_id * 256;
+				res->status = fis[2];
+				res->error = fis[3];
+			} else {
+				uint16_t tfd = ATA_INL(ch->r_mem, AHCI_P_TFD);
+
+				res->status = tfd;
+				res->error = tfd >> 8;
+			}
 			res->lba_low = fis[4];
 			res->lba_mid = fis[5];
 			res->lba_high = fis[6];
@@ -1659,6 +1775,8 @@ ahci_end_transaction(struct ahci_slot *slot, enum ahci_err_type et)
 		    BUS_DMASYNC_POSTREAD : BUS_DMASYNC_POSTWRITE);
 		bus_dmamap_unload(ch->dma.data_tag, slot->dma.data_map);
 	}
+	if (et != AHCI_ERR_NONE)
+		ch->eslots |= (1 << slot->slot);
 	/* In case of error, freeze device for proper recovery. */
 	if ((et != AHCI_ERR_NONE) && (!ch->readlog) &&
 	    !(ccb->ccb_h.status & CAM_DEV_QFRZN)) {
@@ -1722,9 +1840,11 @@ ahci_end_transaction(struct ahci_slot *slot, enum ahci_err_type et)
 	slot->ccb = NULL;
 	/* Update channel stats. */
 	ch->numrslots--;
+	ch->numrslotspd[ccb->ccb_h.target_id]--;
 	if ((ccb->ccb_h.func_code == XPT_ATA_IO) &&
 	    (ccb->ataio.cmd.flags & CAM_ATAIO_FPDMA)) {
 		ch->numtslots--;
+		ch->numtslotspd[ccb->ccb_h.target_id]--;
 	}
 	/* If it was first request of reset sequence and there is no error,
 	 * proceed to second request. */
@@ -1742,6 +1862,7 @@ ahci_end_transaction(struct ahci_slot *slot, enum ahci_err_type et)
 	/* If it was NCQ command error, put result on hold. */
 	} else if (et == AHCI_ERR_NCQ) {
 		ch->hold[slot->slot] = ccb;
+		ch->numhslots++;
 	} else
 		xpt_done(ccb);
 	/* Unfreeze frozen command. */
@@ -1756,6 +1877,15 @@ ahci_end_transaction(struct ahci_slot *slot, enum ahci_err_type et)
 		/* if there was fatal error - reset port. */
 		if (ch->fatalerr) {
 			ahci_reset(dev);
+		} else {
+			/* if we have slots in error, we can reinit port. */
+			if (ch->eslots != 0) {
+				ahci_stop(dev);
+				ahci_start(dev, 1);
+			}
+			/* if there commands on hold, we can do READ LOG. */
+			if (!ch->readlog && ch->numhslots)
+				ahci_issue_read_log(dev);
 		}
 	}
 	/* Start PM timer. */
@@ -1843,6 +1973,7 @@ ahci_process_read_log(device_t dev, union ccb *ccb)
 			}
 			xpt_done(ch->hold[i]);
 			ch->hold[i] = NULL;
+			ch->numhslots--;
 		}
 	} else {
 		if ((ccb->ccb_h.status & CAM_STATUS_MASK) != CAM_REQ_CMP)
@@ -1855,6 +1986,7 @@ ahci_process_read_log(device_t dev, union ccb *ccb)
 				continue;
 			xpt_done(ch->hold[i]);
 			ch->hold[i] = NULL;
+			ch->numhslots--;
 		}
 	}
 	free(ccb->ataio.data_ptr, M_AHCI);
@@ -1863,7 +1995,7 @@ ahci_process_read_log(device_t dev, union ccb *ccb)
 }
 
 static void
-ahci_start(device_t dev)
+ahci_start(device_t dev, int fbs)
 {
 	struct ahci_channel *ch = device_get_softc(dev);
 	u_int32_t cmd;
@@ -1872,6 +2004,12 @@ ahci_start(device_t dev)
 	ATA_OUTL(ch->r_mem, AHCI_P_SERR, 0xFFFFFFFF);
 	/* Clear any interrupts pending on this channel */
 	ATA_OUTL(ch->r_mem, AHCI_P_IS, 0xFFFFFFFF);
+	/* Configure FIS-based switching if supported. */
+	if (ch->chcaps & AHCI_P_CMD_FBSCP) {
+		ch->fbs_enabled = (fbs && ch->pm_present) ? 1 : 0;
+		ATA_OUTL(ch->r_mem, AHCI_P_FBS,
+		    ch->fbs_enabled ? AHCI_P_FBS_EN : 0);
+	}
 	/* Start operations on this channel */
 	cmd = ATA_INL(ch->r_mem, AHCI_P_CMD);
 	ATA_OUTL(ch->r_mem, AHCI_P_CMD, cmd | AHCI_P_CMD_ST |
@@ -1897,6 +2035,7 @@ ahci_stop(device_t dev)
 			break;
 		}
 	} while (ATA_INL(ch->r_mem, AHCI_P_CMD) & AHCI_P_CMD_CR);
+	ch->eslots = 0;
 }
 
 static void
@@ -2010,7 +2149,9 @@ ahci_reset(device_t dev)
 			continue;
 		xpt_done(ch->hold[i]);
 		ch->hold[i] = NULL;
+		ch->numhslots--;
 	}
+	ch->eslots = 0;
 	ch->fatalerr = 0;
 	/* Tell the XPT about the event */
 	xpt_async(AC_BUS_RESET, ch->path, NULL);
@@ -2031,7 +2172,7 @@ ahci_reset(device_t dev)
 	/* Wait for clearing busy status. */
 	if (ahci_wait_ready(dev, 15000))
 		ahci_clo(dev);
-	ahci_start(dev);
+	ahci_start(dev, 1);
 	ch->devices = 1;
 	/* Enable wanted port interrupts */
 	ATA_OUTL(ch->r_mem, AHCI_P_IE,
diff --git a/sys/dev/ahci/ahci.h b/sys/dev/ahci/ahci.h
index d136d82..5b524ab 100644
--- a/sys/dev/ahci/ahci.h
+++ b/sys/dev/ahci/ahci.h
@@ -247,8 +247,11 @@
 #define         AHCI_P_CMD_CPS      0x00010000
 #define         AHCI_P_CMD_PMA      0x00020000
 #define         AHCI_P_CMD_HPCP     0x00040000
-#define         AHCI_P_CMD_ISP      0x00080000
+#define         AHCI_P_CMD_MPSP     0x00080000
 #define         AHCI_P_CMD_CPD      0x00100000
+#define         AHCI_P_CMD_ESP      0x00200000
+#define         AHCI_P_CMD_FBSCP    0x00400000
+#define         AHCI_P_CMD_APSTE    0x00800000
 #define         AHCI_P_CMD_ATAPI    0x01000000
 #define         AHCI_P_CMD_DLAE     0x02000000
 #define         AHCI_P_CMD_ALPE     0x04000000
@@ -268,6 +271,15 @@
 #define AHCI_P_CI                   0x38
 #define AHCI_P_SNTF                 0x3C
 #define AHCI_P_FBS                  0x40
+#define 	AHCI_P_FBS_EN       0x00000001
+#define 	AHCI_P_FBS_DEC      0x00000002
+#define 	AHCI_P_FBS_SDE      0x00000004
+#define 	AHCI_P_FBS_DEV      0x00000f00
+#define 	AHCI_P_FBS_DEV_SHIFT 8
+#define 	AHCI_P_FBS_ADO      0x0000f000
+#define 	AHCI_P_FBS_ADO_SHIFT 12
+#define 	AHCI_P_FBS_DWE      0x000f0000
+#define 	AHCI_P_FBS_DWE_SHIFT 16
 
 /* Just to be sure, if building as module. */
 #if MAXPHYS < 512 * 1024
@@ -373,6 +385,7 @@ struct ahci_channel {
 	struct cam_path		*path;
 	uint32_t		caps;		/* Controller capabilities */
 	uint32_t		caps2;		/* Controller capabilities */
+	uint32_t		chcaps;		/* Channel capabilities */
 	int			quirks;
 	int			numslots;	/* Number of present slots */
 	int			pm_level;	/* power management level */
@@ -382,11 +395,16 @@ struct ahci_channel {
 	struct mtx		mtx;		/* state lock */
 	int			devices;        /* What is present */
 	int			pm_present;	/* PM presence reported */
+	int			fbs_enabled;	/* FIS-based switching enabled */
 	uint32_t		oslots;		/* Occupied slots */
 	uint32_t		rslots;		/* Running slots */
 	uint32_t		aslots;		/* Slots with atomic commands  */
+	uint32_t		eslots;		/* Slots in error */
 	int			numrslots;	/* Number of running slots */
+	int			numrslotspd[16];/* Number of running slots per dev */
 	int			numtslots;	/* Number of tagged slots */
+	int			numtslotspd[16];/* Number of tagged slots per dev */
+	int			numhslots;	/* Number of holden slots */
 	int			readlog;	/* Our READ LOG active */
 	int			fatalerr;	/* Fatal error happend */
 	int			lastslot;	/* Last used slot */