kexts: ahci: implement identification of ATAPI devices

kexts/drivers/block/ahci/ahci.h

@@ -26,9 +26,15 @@
 #define HBA_PxCMD_CR    0x8000
 
 #define HBA_PxIS_TFES   0x40000000
+#define HBA_PxIS_DPS    0x20u
 
 #define ATA_CMD_READ_DMA_EX 0x25u
 #define ATA_CMD_IDENTIFY_DEVICE 0xECu
+#define ATA_CMD_PACKET 0xA0u
+#define ATA_CMD_IDENTIFY_PACKET_DEVICE 0xA1u
+
+#define SCSI_CMD_READ 0xA8u
+#define SCSI_CMD_READ_CAPACITY 0x25u
 
 enum fis_type {
 	FIS_TYPE_REG_H2D = 0x27u,
@@ -193,10 +199,37 @@ struct hba_prdt_entry {
 	uint32_t i : 1;
 } __packed;
 
+struct scsi_command {
+	uint8_t cmd;
+
+	union {
+		struct {
+			uint8_t rsvd[15];
+		} read_capacity;
+
+		struct {
+			uint8_t rdprotect : 3;
+			uint8_t dpo : 1;
+			uint8_t fua : 1;
+			uint8_t rarc : 1;
+			uint8_t obsolete : 2;
+			uint32_t lba;
+			uint32_t count;
+		} read;
+	} __packed;
+} __packed;
+
+/* struct scsi_command /must/ be exactly 16 bytes long, so that it fits properly
+   within struct hba_cmd_table */
+_Static_assert(sizeof(struct scsi_command) == 16, "ahci: scsi_command must be 16 bytes");
+_Static_assert(sizeof(struct fis_dma_setup) == 28, "ahci: fis_dma_setup must be 28 bytes");
+_Static_assert(sizeof(struct fis_pio_setup) == 20, "ahci: fis_pio_set must be 20 bytes");
+_Static_assert(sizeof(struct fis_reg_d2h) == 20, "ahci: fis_reg_d2h must be 20 bytes");
+
 struct hba_cmd_table {
 	uint8_t cfis[64];
 
-	uint8_t acmd[16];
+	struct scsi_command acmd;
 
 	uint8_t rsv[48];
 
@@ -245,18 +278,8 @@ struct hba_config {
 	struct hba_port ports[32];
 } __packed;
 
-struct ata_identify_device_data {
-	uint16_t g_config;
-	uint8_t rsv0[2];
-	uint16_t s_config;
-	uint8_t rsv1[14];
-	uint8_t serial_number[20];
-	uint8_t rsv2[6];
-	uint32_t firmware;
-	char model[40];
-} __packed;
-
 struct identify_device_data {
+	union {
 		struct {
 			uint16_t reserved_1 : 1;
 			uint16_t retired_3 : 1;
@@ -267,6 +290,28 @@ struct identify_device_data {
 			uint16_t retired_1 : 7;
 			uint16_t device_type : 1;
 		} general_config;
+
+		struct {
+			/*
+			uint16_t is_atapi : 2;
+			uint16_t reserved_1: 1;
+			uint16_t command_set : 5;
+			uint16_t obsolete_1: 1;
+			uint16_t packet_drq_time : 2;
+			uint16_t reserved_2 : 2;
+			uint16_t identify_incomplete : 1;
+			uint16_t packet_size : 2;
+			*/
+			uint16_t packet_size : 2;
+			uint16_t identify_incomplete : 1;
+			uint16_t reserved_2 : 2;
+			uint16_t packet_drq_time : 2;
+			uint16_t obsolete_1: 1;
+			uint16_t command_set : 5;
+			uint16_t reserved_1: 1;
+			uint16_t is_atapi : 2;
+		} atapi_general_config;
+	};
 	uint16_t num_cylinders;
 	uint16_t specific_configuration;
 	uint16_t num_heads;
@@ -628,4 +673,9 @@ struct identify_device_data {
 	uint16_t check_sum : 8;
 } __packed;
 
+struct scsi_read_capacity_data {
+	uint32_t last_sector;
+	uint32_t block_size;
+} __packed;
+
 #endif

kexts/drivers/block/ahci/main.c

@@ -3,20 +3,36 @@
 #include <socks/device.h>
 #include <socks/kext.h>
 #include <socks/pci.h>
+#include <socks/util.h>
 #include <socks/machine/init.h>
 #include <socks/libc/stdio.h>
 #include <socks/libc/ctype.h>
 #include <arch/irq.h>
 #include "ahci.h"
 
+struct rfis_data {
+	struct fis_dma_setup dma_setup;
+	char pad0[4];
+	struct fis_pio_setup pio_setup;
+	char pad1[12];
+	struct fis_reg_d2h reg_d2h;
+} __packed;
+
 struct ahci_device {
 	int port_no;
 	int type;
 	volatile struct hba_port *port;
 
 	struct hba_cmd_header cmd_header[32];
-	char fis_data[256];
+	union {
+		char rfis_data[256];
+		struct rfis_data rfis;
 	};
+};
+
+_Static_assert(offsetof(struct rfis_data, dma_setup) == 0, "offsetof dma_setup in rfis_data must be 0x00");
+_Static_assert(offsetof(struct rfis_data, pio_setup) == 0x20, "offsetof dma_setup in rfis_data must be 0x20");
+_Static_assert(offsetof(struct rfis_data, reg_d2h) == 0x40, "offsetof dma_setup in rfis_data must be 0x40");
 
 /************************** STOLEN **********************/
 
@@ -76,6 +92,7 @@ static kern_status_t send_command(struct ahci_device *dev, unsigned int cmd, str
 	struct hba_cmd_header *cmdheader = &dev->cmd_header[slot];
 	cmdheader->cfl = sizeof(struct fis_reg_h2d) / sizeof(uint32_t);
 	cmdheader->w = 0;
+	cmdheader->a = 0;
 	cmdheader->prdtl = nvec;
 
 	struct hba_cmd_table *cmdtbl = create_cmd_table(vec, nvec);
@@ -114,6 +131,82 @@ static kern_status_t send_command(struct ahci_device *dev, unsigned int cmd, str
 			return KERN_IO_ERROR;
 		}
 	}
+	free_cmd_table(cmdtbl);
+
+	if (port->is & HBA_PxIS_TFES) {
+		printk("Read disk error 2");
+		return KERN_IO_ERROR;
+	}
+
+	return KERN_OK;
+}
+
+static kern_status_t send_atapi_command(struct ahci_device *dev, struct scsi_command *cmd, struct iovec *vec, size_t nvec)
+{
+	if (nvec == 0) {
+		return KERN_OK;
+	}
+
+	volatile struct hba_port *port = dev->port;
+	port->is = (uint32_t) -1;
+	int spin = 0;
+	int slot = find_cmdslot(port);
+	if (slot == -1) {
+		return KERN_BUSY;
+	}
+
+	struct hba_cmd_header *cmdheader = &dev->cmd_header[slot];
+	memset(cmdheader, 0x0, sizeof *cmdheader);
+	cmdheader->cfl = sizeof(struct fis_reg_h2d) / sizeof(uint32_t);
+	cmdheader->w = 0;
+	cmdheader->a = 1;
+	cmdheader->prdtl = nvec;
+
+	struct hba_cmd_table *cmdtbl = create_cmd_table(vec, nvec);
+	phys_addr_t cmdtbl_phys = vm_virt_to_phys(cmdtbl);
+	cmdheader->ctba = cmdtbl_phys & 0xFFFFFFFF;
+	cmdheader->ctbau = (cmdtbl_phys >> 32) & 0xFFFFFFFF;
+	memcpy(&cmdtbl->acmd, cmd, sizeof *cmd);
+
+	struct fis_reg_h2d *cmdfis = (struct fis_reg_h2d *)(&cmdtbl->cfis);
+	memset(cmdfis, 0x0, sizeof *cmdfis);
+
+	cmdfis->fis_type = FIS_TYPE_REG_H2D;
+	cmdfis->c = 1;
+	cmdfis->rsv0 = 0;
+	cmdfis->feature_l = 1;
+	cmdfis->command = ATA_CMD_PACKET;
+
+	while ((port->tfd & (ATA_DEV_BUSY | ATA_DEV_DRQ)) && spin < 1000000) {
+		spin++;
+	}
+
+	if (spin == 1000000) {
+		printk("Port is hung");
+		free_cmd_table(cmdtbl);
+		return KERN_DEVICE_STUCK;
+	}
+
+	port->ci = 1 << slot;
+
+	// Wait for completion
+	while (1)
+	{
+		if ((port->ci & (1 << slot)) == 0) {
+			break;
+		}
+
+		if (port->is & HBA_PxIS_TFES) {
+			printk("Read disk error 1");
+			free_cmd_table(cmdtbl);
+			return KERN_IO_ERROR;
+		}
+	}
+
+	volatile int _i = 0;
+	while ((port->is & 0x1) != 0x1) {
+		_i++;
+	}
 
 	free_cmd_table(cmdtbl);
 
@@ -125,72 +218,8 @@ static kern_status_t send_command(struct ahci_device *dev, unsigned int cmd, str
 	return KERN_OK;
 }
 
-static kern_status_t identify_device(struct ahci_device *dev, struct identify_device_data *out)
+static kern_status_t identify_ata_device(struct ahci_device *dev, struct identify_device_data *out)
 {
-	/*
-	port->is = (uint32_t) -1;		// Clear pending interrupt bits
-	int spin = 0; // Spin lock timeout counter
-	int slot = find_cmdslot(port);
-	if (slot == -1)
-		return false;
-
-	struct hba_cmd_header *cmdheader = vm_phys_to_virt(port->clb);
-	cmdheader += slot;
-	cmdheader->cfl = sizeof(struct fis_reg_h2d)/sizeof(uint32_t);	// Command FIS size
-	cmdheader->w = 0;		// Read from device
-	cmdheader->prdtl = 1;
-
-	struct hba_cmd_table *cmdtbl = vm_phys_to_virt(cmdheader->ctba);
-	memset(cmdtbl, 0, sizeof(struct hba_cmd_table) +
- 		(cmdheader->prdtl-1)*sizeof(struct hba_prdt_entry));
-
-	cmdtbl->prdt_entry[0].dba = (uint32_t)vm_virt_to_phys(out);
-	cmdtbl->prdt_entry[0].dbc = sizeof *out - 1;
-	cmdtbl->prdt_entry[0].i = 1;
-
-	// Setup command
-	struct fis_reg_h2d *cmdfis = (struct fis_reg_h2d*)(&cmdtbl->cfis);
-
-	cmdfis->fis_type = FIS_TYPE_REG_H2D;
-	cmdfis->c = 1;	// Command
-	cmdfis->command = ATA_CMD_IDENTIFY_DEVICE;
-
-	// The below loop waits until the port is no longer busy before issuing a new command
-	while ((port->tfd & (ATA_DEV_BUSY | ATA_DEV_DRQ)) && spin < 1000000)
-	{
-		spin++;
-	}
-	if (spin == 1000000)
-	{
-		printk("Port is hung");
-		return -1;
-	}
-
-	port->ci = 1<<slot;	// Issue command
-
-	// Wait for completion
-	while (1)
-	{
-		// In some longer duration reads, it may be helpful to spin on the DPS bit
-		// in the PxIS port field as well (1 << 5)
-		if ((port->ci & (1<<slot)) == 0)
-			break;
-		if (port->is & HBA_PxIS_TFES)	// Task file error
-		{
-			printk("Read disk error 1");
-			return -1;
-		}
-	}
-
-	// Check again
-	if (port->is & HBA_PxIS_TFES)
-	{
-		printk("Read disk error 2");
-		return -1;
-	}
-
-	return 0;
-	*/
 	struct iovec vec = { .io_buf = out, .io_len = sizeof *out };
 	kern_status_t status = send_command(dev, ATA_CMD_IDENTIFY_DEVICE, &vec, 1);
 	if (status != KERN_OK) {
@@ -215,6 +244,32 @@ static kern_status_t identify_device(struct ahci_device *dev, struct identify_de
 	return KERN_OK;
 }
 
+static kern_status_t identify_atapi_device(struct ahci_device *dev, struct identify_device_data *out)
+{
+	struct iovec vec = { .io_buf = out, .io_len = sizeof *out };
+	kern_status_t status = send_command(dev, ATA_CMD_IDENTIFY_PACKET_DEVICE, &vec, 1);
+	if (status != KERN_OK) {
+		return status;
+	}
+
+	out->model_number[39] = 0;
+	for (int i = 0; i < 40; i += 2) {
+		unsigned char c = out->model_number[i];
+		out->model_number[i] = out->model_number[i + 1];
+		out->model_number[i + 1] = c;
+	}
+
+	for (int i = sizeof out->model_number - 1; i >= 0; i--) {
+		if (isspace(out->model_number[i]) || out->model_number[i] == 0) {
+			out->model_number[i] = 0;
+		} else {
+			break;
+		}
+	}
+
+	return KERN_OK;
+}
+
 // Start command engine
 void start_cmd(volatile struct hba_port *port)
 {
@@ -252,43 +307,21 @@ void port_rebase(struct ahci_device *dev)
 {
 	volatile struct hba_port *port = dev->port;
 
-	stop_cmd(port);	// Stop command engine
+	stop_cmd(port);
 
 	struct hba_cmd_header *cmdheader = dev->cmd_header;
 
-	// Command list offset: 1K*portno
-	// Command list entry size = 32
-	// Command list entry maxim count = 32
-	// Command list maxim size = 32*32 = 1K per port
 	phys_addr_t cmdheader_phys = vm_virt_to_phys(cmdheader);
 	port->clb = cmdheader_phys & 0xFFFFFFFF;
 	port->clbu = (cmdheader_phys >> 32) & 0xFFFFFFFF;
 	memset(cmdheader, 0, sizeof *cmdheader);
 
-	// FIS offset: 32K+256*portno
+	phys_addr_t fis_phys = vm_virt_to_phys(dev->rfis_data);
-	// FIS entry size = 256 bytes per port
-	phys_addr_t fis_phys = vm_virt_to_phys(dev->fis_data);
 	port->fb = fis_phys & 0xFFFFFFFF;
 	port->fbu = (fis_phys >> 32) & 0xFFFFFFFF;
-	memset(dev->fis_data, 0, 256);
+	memset(dev->rfis_data, 0, 256);
 
-	/*
+	start_cmd(port);
-	// Command table offset: 40K + 8K*portno
-	// Command table size = 256*32 = 8K per port
-	for (int i=0; i<32; i++)
-	{
-		cmdheader[i].prdtl = 8;	// 8 prdt entries per command table
-					// 256 bytes per command table, 64+16+48+16*8
-		// Command table offset: 40K + 8K*portno + cmdheader_index*256
-		cmdheader[i].ctba = phys + offset;
-		cmdheader[i].ctbau = 0;
-		memset(virt + offset, 0, 256);
-
-		offset += 256;
-	}
-	*/
-
-	start_cmd(port);	// Start command engine
 }
 
 /*
@@ -452,7 +485,7 @@ void probe_ports(struct driver *driver, struct device *controller, volatile stru
 
 		if (dt == AHCI_DEV_SATA) {
 			struct identify_device_data identity_data = {0};
-			kern_status_t status = identify_device(ahci_dev, &identity_data);
+			kern_status_t status = identify_ata_device(ahci_dev, &identity_data);
 			if (status != KERN_OK) {
 				kfree(ahci_dev);
 				device_deref(bdev_base);
@@ -471,6 +504,31 @@ void probe_ports(struct driver *driver, struct device *controller, volatile stru
 			bdev->sector_size = 512;
 			bdev->capacity = identity_data.user_addressable_sectors;
 
+			snprintf(bdev_base->dev_model_name, sizeof bdev_base->dev_model_name, "%s", identity_data.model_number);
+		} else if (dt == AHCI_DEV_SATAPI) {
+			struct identify_device_data identity_data = {0};
+			kern_status_t status = identify_atapi_device(ahci_dev, &identity_data);
+			if (status != KERN_OK) {
+				kfree(ahci_dev);
+				device_deref(bdev_base);
+				continue;
+			}
+
+			struct scsi_command cmd = {0};
+			cmd.cmd = SCSI_CMD_READ_CAPACITY;
+
+			struct scsi_read_capacity_data cmd_result = {0};
+			struct iovec vec = { .io_buf = &cmd_result, .io_len = sizeof cmd_result };
+
+			status = send_atapi_command(ahci_dev, &cmd, &vec, 1);
+			if (status != KERN_OK) {
+				printk("ahci: failed to contact ATAPI device: %s", kern_status_string(status));
+				continue;
+			}
+
+			bdev->sector_size = big_to_host_u32(cmd_result.block_size);
+			bdev->capacity = big_to_host_u32(cmd_result.last_sector) + 1;
+
 			snprintf(bdev_base->dev_model_name, sizeof bdev_base->dev_model_name, "%s", identity_data.model_number);
 		}