1 files changed, 84 insertions, 92 deletions

diff --git a/usr.sbin/bhyve/pci_ahci.c b/usr.sbin/bhyve/pci_ahci.c
index e8cb2da..87a1cea 100644
--- a/usr.sbin/bhyve/pci_ahci.c
+++ b/usr.sbin/bhyve/pci_ahci.c
@@ -124,7 +124,7 @@ struct ahci_ioreq {
 	uint32_t len;
 	uint32_t done;
 	int slot;
-	int prdtl;
+	int more;
 };
 
 struct ahci_port {
@@ -520,26 +520,77 @@ atapi_string(uint8_t *dest, const char *src, int len)
 	}
 }
 
+/*
+ * Build up the iovec based on the PRDT, 'done' and 'len'.
+ */
+static void
+ahci_build_iov(struct ahci_port *p, struct ahci_ioreq *aior,
+    struct ahci_prdt_entry *prdt, uint16_t prdtl)
+{
+	struct blockif_req *breq = &aior->io_req;
+	int i, j, skip, todo, left, extra;
+	uint32_t dbcsz;
+
+	/* Copy part of PRDT between 'done' and 'len' bytes into the iov. */
+	skip = aior->done;
+	left = aior->len - aior->done;
+	todo = 0;
+	for (i = 0, j = 0; i < prdtl && j < BLOCKIF_IOV_MAX && left > 0;
+	    i++, prdt++) {
+		dbcsz = (prdt->dbc & DBCMASK) + 1;
+		/* Skip already done part of the PRDT */
+		if (dbcsz <= skip) {
+			skip -= dbcsz;
+			continue;
+		}
+		dbcsz -= skip;
+		if (dbcsz > left)
+			dbcsz = left;
+		breq->br_iov[j].iov_base = paddr_guest2host(ahci_ctx(p->pr_sc),
+		    prdt->dba + skip, dbcsz);
+		breq->br_iov[j].iov_len = dbcsz;
+		todo += dbcsz;
+		left -= dbcsz;
+		skip = 0;
+		j++;
+	}
+
+	/* If we got limited by IOV length, round I/O down to sector size. */
+	if (j == BLOCKIF_IOV_MAX) {
+		extra = todo % blockif_sectsz(p->bctx);
+		todo -= extra;
+		assert(todo > 0);
+		while (extra > 0) {
+			if (breq->br_iov[j - 1].iov_len > extra) {
+				breq->br_iov[j - 1].iov_len -= extra;
+				break;
+			}
+			extra -= breq->br_iov[j - 1].iov_len;
+			j--;
+		}
+	}
+
+	breq->br_iovcnt = j;
+	aior->done += todo;
+	aior->more = (aior->done < aior->len && i < prdtl);
+}
+
 static void
-ahci_handle_dma(struct ahci_port *p, int slot, uint8_t *cfis, uint32_t done,
-    int seek)
+ahci_handle_rw(struct ahci_port *p, int slot, uint8_t *cfis, uint32_t done)
 {
 	struct ahci_ioreq *aior;
 	struct blockif_req *breq;
-	struct pci_ahci_softc *sc;
 	struct ahci_prdt_entry *prdt;
 	struct ahci_cmd_hdr *hdr;
 	uint64_t lba;
 	uint32_t len;
-	int i, err, iovcnt, ncq, readop;
+	int err, ncq, readop;
 
-	sc = p->pr_sc;
 	prdt = (struct ahci_prdt_entry *)(cfis + 0x80);
 	hdr = (struct ahci_cmd_hdr *)(p->cmd_lst + slot * AHCI_CL_SIZE);
 	ncq = 0;
 	readop = 1;
 
-	prdt += seek;
 	if (cfis[2] == ATA_WRITE || cfis[2] == ATA_WRITE48 ||
 	    cfis[2] == ATA_WRITE_MUL || cfis[2] == ATA_WRITE_MUL48 ||
 	    cfis[2] == ATA_WRITE_DMA || cfis[2] == ATA_WRITE_DMA48 ||
@@ -580,49 +631,25 @@ ahci_handle_dma(struct ahci_port *p, int slot, uint8_t *cfis, uint32_t done,
 	lba *= blockif_sectsz(p->bctx);
 	len *= blockif_sectsz(p->bctx);
 
-	/*
-	 * Pull request off free list
-	 */
+	/* Pull request off free list */
 	aior = STAILQ_FIRST(&p->iofhd);
 	assert(aior != NULL);
 	STAILQ_REMOVE_HEAD(&p->iofhd, io_flist);
+
 	aior->cfis = cfis;
 	aior->slot = slot;
 	aior->len = len;
 	aior->done = done;
 	breq = &aior->io_req;
 	breq->br_offset = lba + done;
-	iovcnt = hdr->prdtl - seek;
-	if (iovcnt > BLOCKIF_IOV_MAX) {
-		aior->prdtl = iovcnt - BLOCKIF_IOV_MAX;
-		iovcnt = BLOCKIF_IOV_MAX;
-	} else
-		aior->prdtl = 0;
-	breq->br_iovcnt = iovcnt;
+	ahci_build_iov(p, aior, prdt, hdr->prdtl);
 
-	/*
-	 * Mark this command in-flight.
-	 */
+	/* Mark this command in-flight. */
 	p->pending |= 1 << slot;
 
-	/*
-	 * Stuff request onto busy list
-	 */
+	/* Stuff request onto busy list. */
 	TAILQ_INSERT_HEAD(&p->iobhd, aior, io_blist);
 
-	/*
-	 * Build up the iovec based on the prdt
-	 */
-	for (i = 0; i < iovcnt; i++) {
-		uint32_t dbcsz;
-
-		dbcsz = (prdt->dbc & DBCMASK) + 1;
-		breq->br_iov[i].iov_base = paddr_guest2host(ahci_ctx(sc),
-		    prdt->dba, dbcsz);
-		breq->br_iov[i].iov_len = dbcsz;
-		aior->done += dbcsz;
-		prdt++;
-	}
 	if (readop)
 		err = blockif_read(p->bctx, breq);
 	else
@@ -650,7 +677,7 @@ ahci_handle_flush(struct ahci_port *p, int slot, uint8_t *cfis)
 	aior->slot = slot;
 	aior->len = 0;
 	aior->done = 0;
-	aior->prdtl = 0;
+	aior->more = 0;
 	breq = &aior->io_req;
 
 	/*
@@ -745,7 +772,7 @@ next:
 	aior->slot = slot;
 	aior->len = len;
 	aior->done = done;
-	aior->prdtl = 0;
+	aior->more = (len != done);
 
 	breq = &aior->io_req;
 	breq->br_offset = elba * blockif_sectsz(p->bctx);
@@ -1242,8 +1269,7 @@ atapi_report_luns(struct ahci_port *p, int slot, uint8_t *cfis)
 }
 
 static void
-atapi_read(struct ahci_port *p, int slot, uint8_t *cfis,
-		uint32_t done, int seek)
+atapi_read(struct ahci_port *p, int slot, uint8_t *cfis, uint32_t done)
 {
 	struct ahci_ioreq *aior;
 	struct ahci_cmd_hdr *hdr;
@@ -1253,14 +1279,13 @@ atapi_read(struct ahci_port *p, int slot, uint8_t *cfis,
 	uint8_t *acmd;
 	uint64_t lba;
 	uint32_t len;
-	int i, err, iovcnt;
+	int err;
 
 	sc = p->pr_sc;
 	acmd = cfis + 0x40;
 	hdr = (struct ahci_cmd_hdr *)(p->cmd_lst + slot * AHCI_CL_SIZE);
 	prdt = (struct ahci_prdt_entry *)(cfis + 0x80);
 
-	prdt += seek;
 	lba = be32dec(acmd + 2);
 	if (acmd[0] == READ_10)
 		len = be16dec(acmd + 7);
@@ -1285,37 +1310,14 @@ atapi_read(struct ahci_port *p, int slot, uint8_t *cfis,
 	aior->done = done;
 	breq = &aior->io_req;
 	breq->br_offset = lba + done;
-	iovcnt = hdr->prdtl - seek;
-	if (iovcnt > BLOCKIF_IOV_MAX) {
-		aior->prdtl = iovcnt - BLOCKIF_IOV_MAX;
-		iovcnt = BLOCKIF_IOV_MAX;
-	} else
-		aior->prdtl = 0;
-	breq->br_iovcnt = iovcnt;
+	ahci_build_iov(p, aior, prdt, hdr->prdtl);
 
-	/*
-	 * Mark this command in-flight.
-	 */
+	/* Mark this command in-flight. */
 	p->pending |= 1 << slot;
 
-	/*
-	 * Stuff request onto busy list
-	 */
+	/* Stuff request onto busy list. */
 	TAILQ_INSERT_HEAD(&p->iobhd, aior, io_blist);
 
-	/*
-	 * Build up the iovec based on the prdt
-	 */
-	for (i = 0; i < iovcnt; i++) {
-		uint32_t dbcsz;
-
-		dbcsz = (prdt->dbc & DBCMASK) + 1;
-		breq->br_iov[i].iov_base = paddr_guest2host(ahci_ctx(sc),
-		    prdt->dba, dbcsz);
-		breq->br_iov[i].iov_len = dbcsz;
-		aior->done += dbcsz;
-		prdt++;
-	}
 	err = blockif_read(p->bctx, breq);
 	assert(err == 0);
 }
@@ -1515,7 +1517,7 @@ handle_packet_cmd(struct ahci_port *p, int slot, uint8_t *cfis)
 		break;
 	case READ_10:
 	case READ_12:
-		atapi_read(p, slot, cfis, 0, 0);
+		atapi_read(p, slot, cfis, 0);
 		break;
 	case REQUEST_SENSE:
 		atapi_request_sense(p, slot, cfis);
@@ -1614,7 +1616,7 @@ ahci_handle_cmd(struct ahci_port *p, int slot, uint8_t *cfis)
 	case ATA_WRITE_DMA48:
 	case ATA_READ_FPDMA_QUEUED:
 	case ATA_WRITE_FPDMA_QUEUED:
-		ahci_handle_dma(p, slot, cfis, 0, 0);
+		ahci_handle_rw(p, slot, cfis, 0);
 		break;
 	case ATA_FLUSHCACHE:
 	case ATA_FLUSHCACHE48:
@@ -1755,7 +1757,7 @@ ata_ioreq_cb(struct blockif_req *br, int err)
 	struct pci_ahci_softc *sc;
 	uint32_t tfd;
 	uint8_t *cfis;
-	int pending, slot, ncq, dsm;
+	int slot, ncq, dsm;
 
 	DPRINTF("%s %d\n", __func__, err);
 
@@ -1764,7 +1766,6 @@ ata_ioreq_cb(struct blockif_req *br, int err)
 	p = aior->io_pr;
 	cfis = aior->cfis;
 	slot = aior->slot;
-	pending = aior->prdtl;
 	sc = p->pr_sc;
 	hdr = (struct ahci_cmd_hdr *)(p->cmd_lst + slot * AHCI_CL_SIZE);
 
@@ -1792,25 +1793,18 @@ ata_ioreq_cb(struct blockif_req *br, int err)
 	if (!err)
 		hdr->prdbc = aior->done;
 
-	if (dsm) {
-		if (aior->done != aior->len && !err) {
+	if (!err && aior->more) {
+		if (dsm)
 			ahci_handle_dsm_trim(p, slot, cfis, aior->done);
-			goto out;
-		}
-	} else {
-		if (pending && !err) {
-			ahci_handle_dma(p, slot, cfis, aior->done,
-			    hdr->prdtl - pending);
-			goto out;
-		}
+		else 
+			ahci_handle_rw(p, slot, cfis, aior->done);
+		goto out;
 	}
 
-	if (!err && aior->done == aior->len) {
+	if (!err)
 		tfd = ATA_S_READY | ATA_S_DSC;
-	} else {
+	else
 		tfd = (ATA_E_ABORT << 8) | ATA_S_READY | ATA_S_ERROR;
-	}
-
 	if (ncq)
 		ahci_write_fis_sdb(p, slot, cfis, tfd);
 	else
@@ -1836,7 +1830,7 @@ atapi_ioreq_cb(struct blockif_req *br, int err)
 	struct pci_ahci_softc *sc;
 	uint8_t *cfis;
 	uint32_t tfd;
-	int pending, slot;
+	int slot;
 
 	DPRINTF("%s %d\n", __func__, err);
 
@@ -1844,7 +1838,6 @@ atapi_ioreq_cb(struct blockif_req *br, int err)
 	p = aior->io_pr;
 	cfis = aior->cfis;
 	slot = aior->slot;
-	pending = aior->prdtl;
 	sc = p->pr_sc;
 	hdr = (struct ahci_cmd_hdr *)(p->cmd_lst + aior->slot * AHCI_CL_SIZE);
 
@@ -1863,19 +1856,18 @@ atapi_ioreq_cb(struct blockif_req *br, int err)
 	if (!err)
 		hdr->prdbc = aior->done;
 
-	if (pending && !err) {
-		atapi_read(p, slot, cfis, aior->done, hdr->prdtl - pending);
+	if (!err && aior->more) {
+		atapi_read(p, slot, cfis, aior->done);
 		goto out;
 	}
 
-	if (!err && aior->done == aior->len) {
+	if (!err) {
 		tfd = ATA_S_READY | ATA_S_DSC;
 	} else {
 		p->sense_key = ATA_SENSE_ILLEGAL_REQUEST;
 		p->asc = 0x21;
 		tfd = (p->sense_key << 12) | ATA_S_READY | ATA_S_ERROR;
 	}
-
 	cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
 	ahci_write_fis_d2h(p, slot, cfis, tfd);