ider: implement READ CAPACITY

3 files changed, 63 insertions, 12 deletions

diff --git a/amtider.c b/amtider.c
index b62a4d5..1757cdd 100644
--- a/amtider.c
+++ b/amtider.c
@@ -26,8 +26,11 @@
 #include <fcntl.h>
 #include <termios.h>
 #include <signal.h>
+#include <sys/types.h>
+#include <sys/stat.h>
 #include <sys/ioctl.h>
 #include <sys/signalfd.h>
+#include <sys/mman.h>
 
 #include "redir.h"
 
@@ -188,7 +191,8 @@ int main(int argc, char *argv[])
 {
     struct redir r;
     char *h;
-    int c;
+    int c, fd;
+    struct stat st;
 
     memset(&r, 0, sizeof(r));
     r.verbose = 1;
@@ -204,7 +208,7 @@ int main(int argc, char *argv[])
 	snprintf(r.pass, sizeof(r.pass), "%s", h);
 
     for (;;) {
-	if (-1 == (c = getopt(argc, argv, "hvqu:p:LC:")))
+	if (-1 == (c = getopt(argc, argv, "f:hvqu:p:LC:")))
 	    break;
 	switch (c) {
 	case 'v':
@@ -241,15 +245,42 @@ int main(int argc, char *argv[])
 	}
     }
 
-    if (optind < argc)
+    if (optind < argc) {
 	snprintf(r.host, sizeof(r.host), "%s", argv[optind]);
-    if (optind+1 < argc)
-	snprintf(r.port, sizeof(r.port), "%s", argv[optind+1]);
+	optind++;
+    }
+    if (optind < argc) {
+	snprintf(r.port, sizeof(r.port), "%s", argv[optind]);
+	optind++;
+    }
     if (0 == strlen(r.host)) {
 	usage(stderr);
 	exit(1);
     }
 
+    if (0 == strlen(r.filename)) {
+	usage(stderr);
+	exit(1);
+    }
+
+    if (stat(r.filename, &st) < 0) {
+	perror("stat");
+	exit(1);
+    }
+    r.mmap_size = st.st_size;
+    fd = open(r.filename, O_RDONLY);
+    if (fd < 0) {
+	perror("open");
+	exit(1);
+    }
+    r.mmap_buf = mmap(NULL, r.mmap_size, PROT_READ,
+		      MAP_PRIVATE, fd, 0);
+    close(fd);
+    if (r.mmap_buf == MAP_FAILED) {
+	perror("mmap");
+	exit(1);
+    }
+
     if (0 == strlen(r.pass)) {
 	tty_save();
 	tty_noecho();
@@ -269,6 +300,7 @@ int main(int argc, char *argv[])
 
     redir_start(&r);
     redir_loop(&r);
+    munmap(r.mmap_buf, r.mmap_size);
 
     exit(0);
 }
diff --git a/ider.c b/ider.c
index f1f5f65..5b5bddd 100644
--- a/ider.c
+++ b/ider.c
@@ -21,12 +21,14 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <stdbool.h>
 #include <sys/types.h>
 #include <scsi/scsi.h>
 #include "redir.h"
 
 static int ider_data_to_host(struct redir *r, unsigned int seqno,
-			     unsigned char *data, unsigned int data_len)
+			     unsigned char *data, unsigned int data_len,
+			     bool completed)
 {
     unsigned char device = 0xb0;
     unsigned char *request;
@@ -35,6 +37,7 @@ static int ider_data_to_host(struct redir *r, unsigned int seqno,
 	IDER_INTERRUPT_MASK;
     struct ider_data_to_host_message msg = {
 	.type = IDER_DATA_TO_HOST,
+	.attributes = completed ? 2 : 0,
 	.input.mask = mask | IDER_BYTE_CNT_LSB_MASK | IDER_BYTE_CNT_MSB_MASK,
 	.input.sector_count = IDER_INTERRUPT_IO,
 	.input.byte_count_lsb = (data_len & 0xff),
@@ -66,6 +69,7 @@ static int ider_packet_sense(struct redir *r, unsigned int seqno,
 	IDER_DRIVE_SELECT_MASK | IDER_STATUS_MASK;
     struct ider_command_response_message msg = {
 	.type = IDER_COMMAND_END_RESPONSE,
+	.attributes = 2,
 	.output.mask = mask,
 	.output.sector_count = IDER_INTERRUPT_IO | IDER_INTERRUPT_CD,
 	.output.drive_select = device,
@@ -88,8 +92,9 @@ int ider_handle_command(struct redir *r, unsigned int seqno,
 {
     unsigned char device = 0xb0;
     unsigned char resp[512];
+    uint32_t lba, sector_size;
 
-    if (!r->filename)
+    if (!r->mmap_size)
 	/* NOT READY, MEDIUM NOT PRESENT */
 	return ider_packet_sense(r, seqno, device, 0x02, 0x3a, 0x0);
 
@@ -99,11 +104,23 @@ int ider_handle_command(struct redir *r, unsigned int seqno,
     case MODE_SENSE:
 	if (cdb[2] != 0x3f || cdb[3] != 0x00)
 	    return ider_packet_sense(r, seqno, device, 0x05, 0x24, 0x00);
-	resp[0] = 0;
-	resp[1] = 0x05;
-	resp[2] = 0x80;
-	resp[3] = 0;
-	return ider_data_to_host(r, seqno, resp, 4);
+	resp[0] = 0;    /* Mode data length */
+	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);
+    case READ_CAPACITY:
+	lba = (r->mmap_size >> 11) - 1;
+	sector_size = (unsigned int)1 << 11;
+	resp[0] = (lba >> 24) & 0xff;
+	resp[1] = (lba >> 16) & 0xff;
+	resp[2] = (lba >>  8) & 0xff;
+	resp[3] = lba & 0xff;
+	resp[4] = (sector_size >> 24) & 0xff;
+	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);
     default:
 	break;
     }
diff --git a/redir.h b/redir.h
index 9f0eca4..1974c6d 100644
--- a/redir.h
+++ b/redir.h
@@ -33,6 +33,8 @@ struct redir {
 
     /* ide-redirection */
     unsigned char     filename[256];
+    void              *mmap_buf;
+    ssize_t           mmap_size;
     unsigned int      tx_bufsize;
     unsigned int      rx_bufsize;
     unsigned int      enable_options;