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

[zcore at FreeBSD.org]

Author: zcore
Date: Sat Sep 14 16:00:00 2013
New Revision: 257331
URL: http://svnweb.FreeBSD.org/socsvn/?view=rev&rev=257331

Log:
  support ATAPI_INQUIRY

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	Sat Sep 14 15:59:26 2013	(r257330)
+++ soc2013/zcore/head/usr.sbin/bhyve/pci_ahci.c	Sat Sep 14 16:00:00 2013	(r257331)
@@ -336,6 +336,19 @@
 }
 
 static void
+atapi_string(uint8_t *dest, const char *src, int len)
+{
+	int i;
+
+	for (i = 0; i < len; i++) {
+		if (*src)
+			dest[i] = *src++;
+		else
+			dest[i] = ' ';
+	}
+}
+
+static void
 handle_dma(struct ahci_port *p, int slot, uint8_t *cfis)
 {
 	int i, err, readop = 1;
@@ -497,9 +510,9 @@
 		from = buf;
 		prdt = (struct ahci_prdt_entry *)(cfis + 0x80);
 		for (i = 0; i < hdr->prdtl && len; i++) {
-			uint8_t *p = paddr_guest2host(ahci_ctx(sc),
+			uint8_t *ptr = paddr_guest2host(ahci_ctx(sc),
 					prdt->dba, prdt->dbc + 1);
-			memcpy(p, from, prdt->dbc + 1);
+			memcpy(ptr, from, prdt->dbc + 1);
 			len -= (prdt->dbc + 1);
 			from += (prdt->dbc + 1);
 			prdt++;
@@ -572,6 +585,46 @@
 }
 
 static void
+atapi_inquiry(struct ahci_port *p, int slot, uint8_t *cfis)
+{
+	uint8_t buf[36];
+	uint8_t *acmd = cfis + 0x40;
+	int i, len;
+	void *from;
+	struct ahci_cmd_hdr *hdr;
+	struct ahci_prdt_entry *prdt;
+
+	buf[0] = 0x05;
+	buf[1] = 0x80;
+	buf[2] = 0x00;
+	buf[3] = 0x21;
+	buf[4] = 31;
+	buf[5] = 0;
+	buf[6] = 0;
+	buf[7] = 0;
+	atapi_string(buf + 8, "BHYVE", 8);
+	atapi_string(buf + 16, "BHYVE DVD-ROM", 16);
+	atapi_string(buf + 32, "001", 4);
+
+	len = sizeof(buf);
+	if (len > acmd[4])
+		len = acmd[4];
+	from = buf;
+	hdr = p->cmd_lst + slot * AHCI_CL_SIZE;
+	prdt = (struct ahci_prdt_entry *)(cfis + 0x80);
+	for (i = 0; i < hdr->prdtl && len; i++) {
+		uint8_t *ptr = paddr_guest2host(ahci_ctx(p->pr_sc),
+				prdt->dba, prdt->dbc + 1);
+		memcpy(ptr, from, prdt->dbc + 1);
+		len -= (prdt->dbc + 1);
+		from += (prdt->dbc + 1);
+		prdt++;
+	}
+	hdr->prdbc = sizeof(buf) - len;
+	ahci_write_fis_d2h(p, slot, cfis, ATA_S_READY | ATA_S_DSC);
+}
+
+static void
 handle_packet_cmd(struct ahci_port *p, int slot, uint8_t *cfis)
 {
 	uint8_t *acmd = cfis + 0x40;
@@ -588,6 +641,9 @@
 		cfis[4] = (cfis[4] & ~7) | ATA_I_CMD | ATA_I_IN;
 		ahci_write_fis_d2h(p, slot, cfis, ATA_S_READY | ATA_S_DSC);
 		break;
+	case ATAPI_INQUIRY:
+		atapi_inquiry(p, slot, cfis);
+		break;
 	default:
 		break;
 	}