ider: Add MODE_SENSE_10

1 files changed, 145 insertions, 6 deletions

diff --git a/ider.c b/ider.c
index 2e4429e..6b1b470 100644
--- a/ider.c
+++ b/ider.c
@@ -27,10 +27,9 @@
 #include "redir.h"
 
 static int ider_data_to_host(struct redir *r, unsigned int seqno,
-			     unsigned char *data, unsigned int data_len,
-			     bool completed)
+			     unsigned char device,  unsigned char *data,
+			     unsigned int data_len, bool completed)
 {
-    unsigned char device = 0xb0;
     unsigned char *request;
     int ret;
     unsigned char mask = IDER_STATUS_MASK | IDER_SECTOR_COUNT_MASK |
@@ -87,11 +86,94 @@ static int ider_packet_sense(struct redir *r, unsigned int seqno,
     return redir_write(r, (const char *)&msg, sizeof(msg));
 }    
 
+unsigned char ider_mode_page_01_floppy[] = {
+    0x00, 0x12, 0x24, 0x80, 0x00, 0x00, 0x00, 0x00,
+    0x01, 0x0A, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00,
+    0x02, 0x00, 0x00, 0x00
+};
+unsigned char ider_mode_page_01_ls120[] = {
+    0x00, 0x12, 0x31, 0x80, 0x00, 0x00, 0x00, 0x00,
+    0x01, 0x0A, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00,
+    0x02, 0x00, 0x00, 0x00
+};
+unsigned char ider_mode_page_01_cdrom[] = {
+    0x00, 0x0E, 0x01, 0x80, 0x00, 0x00, 0x00, 0x00,
+    0x01, 0x06, 0x00, 0xFF, 0x00, 0x00, 0x00, 0x00
+};
+unsigned char ider_mode_page_05_floppy[] = {
+    0x00, 0x26, 0x24, 0x80, 0x00, 0x00, 0x00, 0x00,
+    0x05, 0x1E, 0x04, 0xB0, 0x02, 0x12, 0x02, 0x00,
+    0x00, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x28, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x02, 0xD0, 0x00, 0x00
+};
+unsigned char ider_mode_page_05_ls120[] = {
+    0x00, 0x26, 0x31, 0x80, 0x00, 0x00, 0x00, 0x00,
+    0x05, 0x1E, 0x10, 0xA9, 0x08, 0x20, 0x02, 0x00,
+    0x03, 0xC3, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x28, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x02, 0xD0, 0x00, 0x00
+};
+unsigned char ider_mode_page_3f_ls120[] =  {
+    0x00, 0x5c, 0x24, 0x80, 0x00, 0x00, 0x00, 0x00,
+    0x01, 0x0a, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00,
+    0x02, 0x00, 0x00, 0x00, 0x03, 0x16, 0x00, 0xa0,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa0, 0x00,
+    0x00, 0x00, 0x05, 0x1E, 0x10, 0xA9, 0x08, 0x20,
+    0x02, 0x00, 0x03, 0xC3, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x28, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0xD0,
+    0x00, 0x00, 0x08, 0x0a, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0b, 0x06,
+    0x00, 0x00, 0x00, 0x11, 0x24, 0x31
+};
+unsigned char ider_mode_page_3f_floppy[] = {
+    0x00, 0x5c, 0x24, 0x80, 0x00, 0x00, 0x00, 0x00,
+    0x01, 0x0a, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00,
+    0x02, 0x00, 0x00, 0x00, 0x03, 0x16, 0x00, 0xa0,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa0, 0x00,
+    0x00, 0x00, 0x05, 0x1e, 0x04, 0xb0, 0x02, 0x12,
+    0x02, 0x00, 0x00, 0x50, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x28, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0xd0,
+    0x00, 0x00, 0x08, 0x0a, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0b, 0x06,
+    0x00, 0x00, 0x00, 0x11, 0x24, 0x31
+};
+unsigned char ider_mode_page_3f_cdrom[] = {
+    0x00, 0x28, 0x01, 0x80, 0x00, 0x00, 0x00, 0x00,
+    0x01, 0x06, 0x00, 0xff, 0x00, 0x00, 0x00, 0x00,
+    0x2a, 0x18, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00
+};
+unsigned char ider_mode_page_1a_cdrom[] = {
+    0x00, 0x12, 0x01, 0x80, 0x00, 0x00, 0x00, 0x00,
+    0x1A, 0x0A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00
+};
+unsigned char ider_mode_page_1d_cdrom[] = {
+    0x00, 0x12, 0x01, 0x80, 0x00, 0x00, 0x00, 0x00,
+    0x1D, 0x0A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00
+};
+unsigned char ider_mode_page_2a_cdrom[] = {
+    0x00, 0x20, 0x01, 0x80, 0x00, 0x00, 0x00, 0x00,
+    0x2a, 0x18, 0x00, 0x00, 0x00, 0x00, 0x20, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00
+};
+
 int ider_handle_command(struct redir *r, unsigned int seqno,
 			unsigned char device, unsigned char *cdb)
 {
     unsigned char resp[512];
-    uint32_t lba, sector_size;
+    unsigned char *mode_sense = NULL;
+    uint32_t lba, sector_size, mode_len;
 
     if (!r->mmap_size)
 	/* NOT READY, MEDIUM NOT PRESENT */
@@ -107,8 +189,65 @@ int ider_handle_command(struct redir *r, unsigned int seqno,
 	resp[1] = 0x05; /* Medium type: CD-ROM data only */
 	resp[2] = 0x80; /* device-specific parameters: Write Protect */
 	resp[3] = 0;    /* Block-descriptor length */
-	return ider_data_to_host(r, seqno, resp, 4, true);
+	return ider_data_to_host(r, seqno, device, resp, 4, true);
+    case MODE_SENSE_10:
+	mode_len = ((unsigned int)cdb[7] << 8) | (unsigned int)(cdb[8]);
+	if (device == 0xa0) {
+	    lba = 0;
+	} else {
+	    lba = (r->mmap_size >> 11);
+	}
+	switch (cdb[2] & 0x3f) {
+	case 0x01:
+	    if (device == 0xa0) {
+		if (lba < 0xb40)
+		    mode_sense = ider_mode_page_01_floppy;
+		else
+		    mode_sense = ider_mode_page_01_ls120;
+	    } else
+		mode_sense = ider_mode_page_01_cdrom;
+	    break;
+	case 0x05:
+	    if (device == 0xa0) {
+		if (lba < 0xb40)
+		    mode_sense = ider_mode_page_05_floppy;
+		else
+		    mode_sense = ider_mode_page_05_ls120;
+	    }
+	    break;
+	case 0x3f:
+	    if (device == 0xa0) {
+		if (lba < 0xb40)
+		    mode_sense = ider_mode_page_3f_floppy;
+		else
+		    mode_sense = ider_mode_page_3f_ls120;
+	    } else
+		mode_sense = ider_mode_page_3f_cdrom;
+	    break;
+	case 0x1a:
+	    if (device == 0xb0)
+		mode_sense = ider_mode_page_1a_cdrom;
+	    break;
+	case 0x1d:
+	    if (device == 0xb0)
+		mode_sense = ider_mode_page_1d_cdrom;
+	    break;
+	case 0x2a:
+	    if (device == 0xb0)
+		mode_sense = ider_mode_page_2a_cdrom;
+	    break;
+	}
+	if (!mode_sense)
+	    return ider_packet_sense(r, seqno, device, 0x05, 0x20, 0x00);
+	if (mode_len > sizeof(mode_sense))
+	    mode_len = sizeof(mode_sense);
+	return ider_data_to_host(r, seqno, device,
+				 mode_sense, mode_len, true);
     case READ_CAPACITY:
+	if (device == 0xa0) {
+	    /* NOT READY, MEDIUM NOT PRESENT */
+	    return ider_packet_sense(r, seqno, device, 0x02, 0x3a, 0x0);
+	}
 	lba = (r->mmap_size >> 11) - 1;
 	sector_size = (unsigned int)1 << 11;
 	resp[0] = (lba >> 24) & 0xff;
@@ -119,7 +258,7 @@ int ider_handle_command(struct redir *r, unsigned int seqno,
 	resp[5] = (sector_size >> 16) & 0xff;
 	resp[6] = (sector_size >>  8) & 0xff;
 	resp[7] = sector_size & 0xff;
-	return ider_data_to_host(r, seqno, resp, 8, true);
+	return ider_data_to_host(r, seqno, device, resp, 8, true);
     default:
 	break;
     }