socsvn commit: r256485 - soc2013/zcore/head/usr.sbin/bhyve

[zcore at FreeBSD.org]

Author: zcore
Date: Sun Aug 25 10:16:43 2013
New Revision: 256485
URL: http://svnweb.FreeBSD.org/socsvn/?view=rev&rev=256485

Log:
  support ATA_ATA_IDENTIFY

Modified:
  soc2013/zcore/head/usr.sbin/bhyve/pci_ahci.c

Modified: soc2013/zcore/head/usr.sbin/bhyve/pci_ahci.c
==============================================================================
--- soc2013/zcore/head/usr.sbin/bhyve/pci_ahci.c	Sun Aug 25 10:15:54 2013	(r256484)
+++ soc2013/zcore/head/usr.sbin/bhyve/pci_ahci.c	Sun Aug 25 10:16:43 2013	(r256485)
@@ -35,6 +35,7 @@
 #include <sys/uio.h>
 #include <sys/ioctl.h>
 #include <sys/disk.h>
+#include <sys/ata.h>
 
 #include <errno.h>
 #include <fcntl.h>
@@ -168,6 +169,7 @@
 			sc->is |= (1 << i);
 	}
 
+	dprintf("%x %x\n", sc->port[0].is, sc->port[0].ie);
 	DPRINTF(("%s %x\n", __func__, sc->is));
 	if (sc->is && (sc->ghc & AHCI_GHC_IE))
 		pci_generate_msi(sc->asc_pi, 0);
@@ -192,13 +194,20 @@
 		len = 8;
 		irq = AHCI_P_IX_SDB;
 		break;
+	case FIS_TYPE_PIOSETUP:
+		offset = 0x20;
+		len = 20;
+		irq = 0;
+		break;
 	default:
 		WPRINTF(("unsupported fis type %d\n", ft));
 		break;
 	}
 	memcpy(p->rfis + offset, fis, len);
-	p->is |= irq;
-	ahci_generate_intr(p->pr_sc);
+	if (irq) {
+		p->is |= irq;
+		ahci_generate_intr(p->pr_sc);
+	}
 }
 
 static void ahci_write_fis_d2h(struct ahci_port *p)
@@ -274,8 +283,6 @@
 static void
 ahci_port_reset(struct ahci_port *pr)
 {
-	pr->is = 0;
-	pr->ie = 0;
 	pr->ssts = 0;
 	pr->sctl = 0;
 	pr->serr = 0;
@@ -305,8 +312,101 @@
 
 	sc->ghc = AHCI_GHC_AE;
 	sc->is = 0;
-	for (i = 0; i < sc->ports; i++)
+	for (i = 0; i < sc->ports; i++) {
+		sc->port[i].ie = 0;
+		sc->port[i].is = 0;
 		ahci_port_reset(&sc->port[i]);
+	}
+}
+
+static void
+ata_string(uint8_t *dest, const char *src, int len)
+{
+	int i;
+
+	for (i = 0; i < len; i++) {
+		if (*src)
+			dest[i ^ 1] = *src++;
+		else
+			dest[i ^ 1] = ' ';
+	}
+}
+
+static void
+handle_cmd(struct ahci_port *p, int slot, uint8_t *cfis,
+		struct ahci_cmd_hdr *hdr)
+{
+	struct pci_ahci_softc *sc = p->pr_sc;
+	struct ahci_prdt_entry *prdt = (struct ahci_prdt_entry *)(cfis + 0x80);
+
+	switch(cfis[2]) {
+	case ATA_ATA_IDENTIFY:
+	{
+		uint16_t buf[256];
+		uint64_t sectors;
+		int i, len;
+		void *from;
+
+		sectors = blockif_size(p->bctx) / blockif_sectsz(p->bctx);
+		memset(buf, 0, sizeof(buf));
+		buf[0] = 0x0040;
+		/* TODO emulate different serial? */
+		ata_string((uint8_t *)(buf+10), "123456", 20);
+		ata_string((uint8_t *)(buf+23), "001", 8);
+		ata_string((uint8_t *)(buf+27), "BHYVE SATA DISK", 40);
+		buf[47] = (0x8000 | 16);
+		buf[48] = 0x1;
+		buf[49] = (1 << 8 | 1 << 9 | 1 << 11);
+		buf[50] = (1 << 14);
+		buf[53] = (1 << 1 | 1 << 2);
+		buf[59] = (0x100 | 16);
+		buf[60] = sectors;
+		buf[61] = (sectors >> 16);
+		buf[63] = 0x7;
+		buf[64] = 0x3;
+		buf[65] = 100;
+		buf[66] = 100;
+		buf[67] = 100;
+		buf[68] = 100;
+		buf[75] = 31;
+		buf[76] = (1 << 8 | 1 << 2);
+		buf[80] = 0x1f0;
+		buf[81] = 0x28;
+		buf[82] = (1 | 1 << 5 | 1 << 14);
+		buf[83] = (1 << 10 | 1 << 12 | 1 << 13 | 1 << 14);
+		buf[84] = (1 << 14);
+		buf[85] = (1 | 1 << 14);
+		buf[86] = (1 << 10 | 1 << 12 | 1 << 13);
+		buf[87] = (1 << 14);
+		buf[88] = (0x7f | 1 << 14);
+		buf[93] = (1 | 1 <<14);
+		buf[100] = sectors;
+		buf[101] = (sectors >> 16);
+		buf[102] = (sectors >> 32);
+		buf[103] = (sectors >> 48);
+		if (hdr->prdtl == 0) {
+			WPRINTF(("wrong prdtl\n"));
+			return;
+		}
+		len = sizeof(buf);
+		from = buf;
+		for (i = 0; i < hdr->prdtl && len; i++) {
+			uint8_t *p = paddr_guest2host(ahci_ctx(sc),
+					prdt->dba, prdt->dbc + 1);
+			memcpy(p, from, prdt->dbc + 1);
+			len -= (prdt->dbc + 1);
+			from += (prdt->dbc + 1);
+			prdt++;
+		}
+		hdr->prdbc = sizeof(buf) - len;
+		p->tfd = ATA_S_DSC | ATA_S_READY;
+		p->is |= AHCI_P_IX_DP;
+		ahci_generate_intr(p->pr_sc);
+		break;
+	}
+	default:
+		break;
+	}
 }
 
 static void
@@ -322,8 +422,9 @@
 	hdr = p->cmd_lst + slot * AHCI_CL_SIZE;
 	cfl = (hdr->flags & 0x1f) * 4;
 	dprintf("cfis length:%d prdt entries: %d\n", cfl, hdr->prdtl);
-	cfis = paddr_guest2host(ahci_ctx(sc), hdr->ctba, cfl);
-	prdt = paddr_guest2host(ahci_ctx(sc), hdr->ctba + 0x80, hdr->prdtl * 16);
+	cfis = paddr_guest2host(ahci_ctx(sc), hdr->ctba,
+			0x80 + hdr->prdtl * sizeof(struct ahci_prdt_entry));
+	prdt = (struct ahci_prdt_entry *)(cfis + 0x80);
 
 	/* dump cfis, will be removed latter */
 	for (i = 0; i < cfl; i++) {
@@ -346,6 +447,7 @@
 
 	if (cfis[1] & 0x80) {
 		dprintf("command cfis\n");
+		handle_cmd(p, slot, cfis, hdr);
 	} else {
 		dprintf("control cfis\n");
 		if (cfis[15] & (1 << 2))