# HG changeset patch
# Parent 0657c5769588bfcb68667cd7b566f3929121c0ab
Add XPT_SATA_FIO_IO support to the ahci sim.

diff -r 0657c5769588 sys/dev/ahci/ahci.c
--- a/sys/dev/ahci/ahci.c
+++ b/sys/dev/ahci/ahci.c
@@ -1316,8 +1316,10 @@ ahci_check_collision(device_t dev, union
 	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)) {
+	if (((ccb->ccb_h.func_code == XPT_ATA_IO) &&
+	    (ccb->ataio.cmd.flags & CAM_ATAIO_FPDMA)) ||
+	    ((ccb->ccb_h.func_code == XPT_SATA_FIS_IO) &&
+	    (ccb->satafisio.flags & CAM_ATAIO_FPDMA))) {
 		/* Tagged command while we have no supported tag free. */
 		if (((~ch->oslots) & (0xffffffff >> (32 -
 		    ch->curr[t].tags))) == 0)
@@ -1354,6 +1356,12 @@ ahci_check_collision(device_t dev, union
 		if (ch->numrslots != 0)
 			return (1);
 	}
+	if ((ccb->ccb_h.func_code == XPT_SATA_FIS_IO) &&
+	    (ccb->satafisio.flags & (CAM_ATAIO_CONTROL | CAM_ATAIO_NEEDRESULT))) {
+		/* Atomic command while anything active. */
+		if (ch->numrslots != 0)
+			return (1);
+	}
        /* We have some atomic command running. */
        if (ch->aslots != 0)
                return (1);
@@ -1373,6 +1381,9 @@ ahci_begin_transaction(device_t dev, uni
 	if ((ccb->ccb_h.func_code == XPT_ATA_IO) &&
 	    (ccb->ataio.cmd.flags & CAM_ATAIO_FPDMA))
 		tags = ch->curr[ccb->ccb_h.target_id].tags;
+	if ((ccb->ccb_h.func_code == XPT_SATA_FIS_IO) &&
+	    (ccb->satafisio.flags & CAM_ATAIO_FPDMA))
+		tags = ch->curr[ccb->ccb_h.target_id].tags;
 	tag = ch->lastslot;
 	while (1) {
 		if (tag >= tags)
@@ -1398,9 +1409,18 @@ ahci_begin_transaction(device_t dev, uni
 		ch->numtslotspd[ccb->ccb_h.target_id]++;
 		ch->taggedtarget = ccb->ccb_h.target_id;
 	}
+	if ((ccb->ccb_h.func_code == XPT_SATA_FIS_IO) &&
+	    (ccb->satafisio.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) &&
 	    (ccb->ataio.cmd.flags & (CAM_ATAIO_CONTROL | CAM_ATAIO_NEEDRESULT)))
 		ch->aslots |= (1 << slot->slot);
+	if ((ccb->ccb_h.func_code == XPT_SATA_FIS_IO) &&
+	    (ccb->satafisio.flags & (CAM_ATAIO_CONTROL | CAM_ATAIO_NEEDRESULT)))
+		ch->aslots |= (1 << slot->slot);
 	slot->dma.nsegs = 0;
 	if ((ccb->ccb_h.flags & CAM_DIR_MASK) != CAM_DIR_NONE) {
 		slot->state = AHCI_SLOT_LOADING;
@@ -1476,9 +1496,14 @@ ahci_execute_transaction(struct ahci_slo
 		    (port << 12));
 	clp->prd_length = htole16(slot->dma.nsegs);
 	/* Special handling for Soft Reset command. */
-	if ((ccb->ccb_h.func_code == XPT_ATA_IO) &&
-	    (ccb->ataio.cmd.flags & CAM_ATAIO_CONTROL)) {
-		if (ccb->ataio.cmd.control & ATA_A_RESET) {
+	if (((ccb->ccb_h.func_code == XPT_ATA_IO) &&
+	    (ccb->ataio.cmd.flags & CAM_ATAIO_CONTROL)) ||
+	    ((ccb->ccb_h.func_code == XPT_SATA_FIS_IO) &&
+	    (ccb->satafisio.flags & CAM_ATAIO_CONTROL))) {
+		if ((ccb->ccb_h.func_code == XPT_ATA_IO &&
+		    (ccb->ataio.cmd.control & ATA_A_RESET)) ||
+		    ((ccb->ccb_h.func_code == XPT_SATA_FIS_IO) &&
+		    (ccb->satafisio.fis_cmd[2] & ATA_A_RESET))) {
 			softreset = 1;
 			/* Kick controller into sane state */
 			ahci_stop(dev);
@@ -1505,6 +1530,10 @@ ahci_execute_transaction(struct ahci_slo
 	    (ccb->ataio.cmd.flags & CAM_ATAIO_FPDMA)) {
 		ATA_OUTL(ch->r_mem, AHCI_P_SACT, 1 << slot->slot);
 	}
+	if ((ccb->ccb_h.func_code == XPT_SATA_FIS_IO) &&
+	    (ccb->satafisio.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 |
@@ -1515,8 +1544,10 @@ ahci_execute_transaction(struct ahci_slo
 	ch->rslots |= (1 << slot->slot);
 	ATA_OUTL(ch->r_mem, AHCI_P_CI, (1 << slot->slot));
 	/* Device reset commands doesn't interrupt. Poll them. */
-	if (ccb->ccb_h.func_code == XPT_ATA_IO &&
-	    (ccb->ataio.cmd.command == ATA_DEVICE_RESET || softreset)) {
+	if ((ccb->ccb_h.func_code == XPT_ATA_IO &&
+	    (ccb->ataio.cmd.command == ATA_DEVICE_RESET || softreset)) ||
+	    ((ccb->ccb_h.func_code == XPT_SATA_FIS_IO) &&
+	    (ccb->satafisio.fis_cmd[2] == ATA_DEVICE_RESET || softreset))) {
 		int count, timeout = ccb->ccb_h.timeout * 100;
 		enum ahci_err_type et = AHCI_ERR_NONE;
 
@@ -1773,6 +1804,55 @@ ahci_end_transaction(struct ahci_slot *s
 			ccb->ataio.resid =
 			    ccb->ataio.dxfer_len - le32toh(clp->bytecount);
 		}
+	} else if (ccb->ccb_h.func_code == XPT_SATA_FIS_IO) {
+		uint8_t *rep = ccb->satafisio.fis_rep;
+
+		if ((et == AHCI_ERR_TFE) ||
+		    (ccb->satafisio.flags & CAM_ATAIO_NEEDRESULT)) {
+			u_int8_t *fis = ch->dma.rfis + 0x40;
+
+			bus_dmamap_sync(ch->dma.rfis_tag, ch->dma.rfis_map,
+			    BUS_DMASYNC_POSTREAD);
+			if (ch->fbs_enabled) {
+				fis += ccb->ccb_h.target_id * 256;
+				rep[0] = fis[0];
+				rep[1] = fis[1];
+				rep[2] = fis[2]; /* Status */
+				rep[3] = fis[3]; /* Error */
+			} else {
+				uint16_t tfd = ATA_INL(ch->r_mem, AHCI_P_TFD);
+
+				rep[0] = fis[0];
+				rep[1] = fis[1];
+				rep[2] = tfd; /* Status */
+				rep[3] = tfd >> 8; /* Error */
+			}
+			/*
+			 * Copy the rest of the fis response into the buffer.
+			 */
+			bcopy(fis + 4, rep + 4, 16 - 4);
+
+			/*
+			 * Some weird controllers do not return signature in
+			 * FIS receive area. Read it from PxSIG register.
+			 */
+			if ((ch->quirks & AHCI_Q_ALTSIG) &&
+			    (ccb->ataio.cmd.flags & CAM_ATAIO_CONTROL) &&
+			    (ccb->ataio.cmd.control & ATA_A_RESET) == 0) {
+				sig = ATA_INL(ch->r_mem,  AHCI_P_SIG);
+				rep[6] = sig >> 24;
+				rep[5] = sig >> 16;
+				rep[4] = sig >> 8;
+				rep[12] = sig;
+			}
+		} else
+			bzero(rep, 16);
+		if ((ccb->satafisio.flags & CAM_ATAIO_FPDMA) == 0 &&
+		    (ccb->ccb_h.flags & CAM_DIR_MASK) != CAM_DIR_NONE &&
+		    (ch->quirks & AHCI_Q_NOCOUNT) == 0) {
+			ccb->satafisio.resid =
+			    ccb->ataio.dxfer_len - le32toh(clp->bytecount);
+		}
 	} else {
 		if ((ccb->ccb_h.flags & CAM_DIR_MASK) != CAM_DIR_NONE &&
 		    (ch->quirks & AHCI_Q_NOCOUNT) == 0) {
@@ -1870,6 +1950,14 @@ ahci_end_transaction(struct ahci_slot *s
 		ahci_begin_transaction(dev, ccb);
 		return;
 	}
+	if ((ccb->ccb_h.func_code == XPT_SATA_FIS_IO) &&
+	    (ccb->satafisio.flags & CAM_ATAIO_CONTROL) &&
+	    (ccb->satafisio.fis_cmd[15] & ATA_A_RESET) &&
+	    et == AHCI_ERR_NONE) {
+		ccb->satafisio.fis_cmd[15] &= ~ATA_A_RESET;
+		ahci_begin_transaction(dev, ccb);
+		return;
+	}
 	/* If it was our READ LOG command - process it. */
 	if (ccb->ccb_h.recovery_type == RECOVERY_READ_LOG) {
 		ahci_process_read_log(dev, ccb);
@@ -1951,7 +2039,8 @@ completeall:
 		return;
 	}
 	ccb->ccb_h = ch->hold[i]->ccb_h;	/* Reuse old header. */
-	if (ccb->ccb_h.func_code == XPT_ATA_IO) {
+	if (ccb->ccb_h.func_code == XPT_ATA_IO ||
+	    ccb->ccb_h.func_code == XPT_SATA_FIS_IO) {	/* XXX not sure */
 		/* READ LOG */
 		ccb->ccb_h.recovery_type = RECOVERY_READ_LOG;
 		ccb->ccb_h.func_code = XPT_ATA_IO;
@@ -2012,6 +2101,7 @@ ahci_process_read_log(device_t dev, unio
 		for (i = 0; i < ch->numslots; i++) {
 			if (!ch->hold[i])
 				continue;
+			/* XXX XPT_SATA_FIS_IO needs recovery too */
 			if (ch->hold[i]->ccb_h.func_code != XPT_ATA_IO)
 				continue;
 			if ((data[0] & 0x1F) == i) {
@@ -2044,6 +2134,7 @@ ahci_process_read_log(device_t dev, unio
 		for (i = 0; i < ch->numslots; i++) {
 			if (!ch->hold[i])
 				continue;
+			/* XXX XPT_SATA_FIS_IO needs recovery too */
 			if (ch->hold[i]->ccb_h.func_code != XPT_ATA_IO)
 				continue;
 			ahci_done(ch, ch->hold[i]);
@@ -2329,46 +2420,50 @@ ahci_setup_fis(device_t dev, struct ahci
 	u_int8_t *fis = &ctp->cfis[0];
 
 	bzero(fis, 20);
-	fis[0] = 0x27;  		/* host to device */
-	fis[1] = (ccb->ccb_h.target_id & 0x0f);
-	if (ccb->ccb_h.func_code == XPT_SCSI_IO) {
-		fis[1] |= 0x80;
-		fis[2] = ATA_PACKET_CMD;
-		if ((ccb->ccb_h.flags & CAM_DIR_MASK) != CAM_DIR_NONE &&
-		    ch->curr[ccb->ccb_h.target_id].mode >= ATA_DMA)
-			fis[3] = ATA_F_DMA;
-		else {
-			fis[5] = ccb->csio.dxfer_len;
-		        fis[6] = ccb->csio.dxfer_len >> 8;
+	if (ccb->ccb_h.func_code == XPT_SATA_FIS_IO) {
+	}
+	else {
+		fis[0] = 0x27;  		/* host to device */
+		fis[1] = (ccb->ccb_h.target_id & 0x0f);
+		if (ccb->ccb_h.func_code == XPT_SCSI_IO) {
+			fis[1] |= 0x80;
+			fis[2] = ATA_PACKET_CMD;
+			if ((ccb->ccb_h.flags & CAM_DIR_MASK) != CAM_DIR_NONE &&
+			    ch->curr[ccb->ccb_h.target_id].mode >= ATA_DMA)
+				fis[3] = ATA_F_DMA;
+			else {
+				fis[5] = ccb->csio.dxfer_len;
+				fis[6] = ccb->csio.dxfer_len >> 8;
+			}
+			fis[7] = ATA_D_LBA;
+			fis[15] = ATA_A_4BIT;
+			bcopy((ccb->ccb_h.flags & CAM_CDB_POINTER) ?
+			    ccb->csio.cdb_io.cdb_ptr : ccb->csio.cdb_io.cdb_bytes,
+			    ctp->acmd, ccb->csio.cdb_len);
+			bzero(ctp->acmd + ccb->csio.cdb_len, 32 - ccb->csio.cdb_len);
+		} else if ((ccb->ataio.cmd.flags & CAM_ATAIO_CONTROL) == 0) {
+			fis[1] |= 0x80;
+			fis[2] = ccb->ataio.cmd.command;
+			fis[3] = ccb->ataio.cmd.features;
+			fis[4] = ccb->ataio.cmd.lba_low;
+			fis[5] = ccb->ataio.cmd.lba_mid;
+			fis[6] = ccb->ataio.cmd.lba_high;
+			fis[7] = ccb->ataio.cmd.device;
+			fis[8] = ccb->ataio.cmd.lba_low_exp;
+			fis[9] = ccb->ataio.cmd.lba_mid_exp;
+			fis[10] = ccb->ataio.cmd.lba_high_exp;
+			fis[11] = ccb->ataio.cmd.features_exp;
+			if (ccb->ataio.cmd.flags & CAM_ATAIO_FPDMA) {
+				fis[12] = tag << 3;
+				fis[13] = 0;
+			} else {
+				fis[12] = ccb->ataio.cmd.sector_count;
+				fis[13] = ccb->ataio.cmd.sector_count_exp;
+			}
+			fis[15] = ATA_A_4BIT;
+		} else {
+			fis[15] = ccb->ataio.cmd.control;
 		}
-		fis[7] = ATA_D_LBA;
-		fis[15] = ATA_A_4BIT;
-		bcopy((ccb->ccb_h.flags & CAM_CDB_POINTER) ?
-		    ccb->csio.cdb_io.cdb_ptr : ccb->csio.cdb_io.cdb_bytes,
-		    ctp->acmd, ccb->csio.cdb_len);
-		bzero(ctp->acmd + ccb->csio.cdb_len, 32 - ccb->csio.cdb_len);
-	} else if ((ccb->ataio.cmd.flags & CAM_ATAIO_CONTROL) == 0) {
-		fis[1] |= 0x80;
-		fis[2] = ccb->ataio.cmd.command;
-		fis[3] = ccb->ataio.cmd.features;
-		fis[4] = ccb->ataio.cmd.lba_low;
-		fis[5] = ccb->ataio.cmd.lba_mid;
-		fis[6] = ccb->ataio.cmd.lba_high;
-		fis[7] = ccb->ataio.cmd.device;
-		fis[8] = ccb->ataio.cmd.lba_low_exp;
-		fis[9] = ccb->ataio.cmd.lba_mid_exp;
-		fis[10] = ccb->ataio.cmd.lba_high_exp;
-		fis[11] = ccb->ataio.cmd.features_exp;
-		if (ccb->ataio.cmd.flags & CAM_ATAIO_FPDMA) {
-			fis[12] = tag << 3;
-			fis[13] = 0;
-		} else {
-			fis[12] = ccb->ataio.cmd.sector_count;
-			fis[13] = ccb->ataio.cmd.sector_count_exp;
-		}
-		fis[15] = ATA_A_4BIT;
-	} else {
-		fis[15] = ccb->ataio.cmd.control;
 	}
 	return (20);
 }
@@ -2490,6 +2585,7 @@ ahciaction(struct cam_sim *sim, union cc
 	switch (ccb->ccb_h.func_code) {
 	/* Common cases first */
 	case XPT_ATA_IO:	/* Execute the requested I/O operation */
+	case XPT_SATA_FIS_IO:
 	case XPT_SCSI_IO:
 		if (ahci_check_ids(dev, ccb))
 			return;