From 231c50d9b5c3856296078912774ff7968cb16b75 Mon Sep 17 00:00:00 2001
From: NIIBE Yutaka <gniibe@fsij.org>
Date: Fri, 9 Dec 2011 15:05:34 +0900
Subject: [PATCH] cleanup

---
 src/usb_msc.c              | 129 ++++++++++++++++++++-----------------
 src/usb_msc.h              |   7 ++
 src/virtual_block_device.c | 126 ++++++++++++++++--------------------
 3 files changed, 134 insertions(+), 128 deletions(-)

diff --git a/src/usb_msc.c b/src/usb_msc.c
index cd8cac7..4665484 100644
--- a/src/usb_msc.c
+++ b/src/usb_msc.c
@@ -193,17 +193,22 @@ static void set_scsi_sense_data(uint8_t sense_key, uint8_t asc) {
 
 static uint8_t buf[512];
 
-static uint8_t media_available;
-static uint8_t media_changed;
+static uint8_t contingent_allegiance;
+static uint8_t keep_contingent_allegiance;
+
+uint8_t media_available;
 
 void msc_media_insert_change (int available)
 {
+  contingent_allegiance = 1;
   media_available = available;
-  media_changed = 1;
   if (available)
     set_scsi_sense_data (0x06, 0x28); /* UNIT_ATTENTION */
   else
-    set_scsi_sense_data (0x02, 0x3a); /* NOT_READY */
+    {
+      set_scsi_sense_data (0x02, 0x3a); /* NOT_READY */
+      keep_contingent_allegiance = 1;
+    }
 }
 
 
@@ -222,22 +227,6 @@ static struct CBW CBW;
 
 static struct CSW CSW;
 
-int msc_recv_cbw (void)
-{
-  msg_t msg;
-
-  chSysLock();
-  msc_state = MSC_IDLE;
-  the_thread = chThdSelf ();
-  usb_start_receive ((uint8_t *)&CBW, sizeof CBW);
-  chSchGoSleepTimeoutS (THD_STATE_SUSPENDED, MS2ST (1000));
-  msg = chThdSelf ()->p_u.rdymsg;
-  chSysUnlock ();
-  if (msg == RDY_OK)
-    return 0;
-  else
-    return -1;
-}
 
 static int msc_recv_data (void)
 {
@@ -291,36 +280,42 @@ static void msc_send_result (const uint8_t *p, size_t n)
 }
 
 
-void msc_handle_err (int err)
-{
-  if (err == 0)
-    {
-      msc_state = MSC_ERROR;
-      chSysLock ();
-      usb_stall_receive ();
-      chSysUnlock ();
-    }
-  else
-    {
-      msc_state = MSC_ERROR;
-      chSysLock ();
-      usb_stall_transmit ();
-      usb_stall_receive ();
-      chSysUnlock ();
-    }
-}
-
-int msc_handle_cbw (void)
+void msc_handle_command (void)
 {
   size_t n;
   uint32_t nblocks, secsize;
-  uint8_t lun;
   uint32_t lba;
+  int r;
+  msg_t msg;
+
+  chSysLock();
+  msc_state = MSC_IDLE;
+  the_thread = chThdSelf ();
+  usb_start_receive ((uint8_t *)&CBW, sizeof CBW);
+  chSchGoSleepTimeoutS (THD_STATE_SUSPENDED, MS2ST (1000));
+  msg = chThdSelf ()->p_u.rdymsg;
+  chSysUnlock ();
+
+  if (msg != RDY_OK)
+    {
+      /* Error occured, ignore the request and go into error state */
+      msc_state = MSC_ERROR;
+      chSysLock ();
+      usb_stall_receive ();
+      chSysUnlock ();
+      return;
+    }
 
   n = ep7_out.rxcnt;
 
   if ((n != sizeof (struct CBW)) || (CBW.dCBWSignature != MSC_CBW_SIGNATURE))
-    return 0;
+    {
+      msc_state = MSC_ERROR;
+      chSysLock ();
+      usb_stall_receive ();
+      chSysUnlock ();
+      return;
+    }
 
   CSW.dCSWTag = CBW.dCBWTag;
   switch (CBW.CBWCB[0]) {
@@ -331,12 +326,13 @@ int msc_handle_cbw (void)
     else
       msc_send_result ((uint8_t *)&scsi_sense_data_fixed,
 		       sizeof scsi_sense_data_fixed);
-    if (media_changed && media_available)
+    /* After the error is reported, clear it, if it's .  */
+    if (!keep_contingent_allegiance)
       {
-	media_changed = 0;
+	contingent_allegiance = 0;
 	set_scsi_sense_data (0x00, 0x00);
       }
-    return 1;
+    return;
   case SCSI_INQUIRY:
     if (CBW.CBWCB[1] & 0x01) /* EVPD */
       /* assume page 00 */
@@ -345,7 +341,7 @@ int msc_handle_cbw (void)
     else
       msc_send_result ((uint8_t *)&scsi_inquiry_data,
 		       sizeof scsi_inquiry_data);
-    return 1;
+    return;
   case SCSI_READ_FORMAT_CAPACITIES:
     buf[8]  = scsi_read_format_capacities (&nblocks, &secsize);
     buf[0]  = buf[1] = buf[2] = 0;
@@ -358,14 +354,14 @@ int msc_handle_cbw (void)
     buf[10] = (uint8_t)(secsize >> 8);
     buf[11] = (uint8_t)(secsize >> 0);
     msc_send_result (buf, 12);
-    return 1;
+    return;
   case SCSI_TEST_UNIT_READY:
-    if (media_available == 0 || media_changed)
+    if (contingent_allegiance)
       {
 	CSW.bCSWStatus = MSC_CSW_STATUS_FAILED;
 	CSW.dCSWDataResidue = 0;
 	msc_send_result (NULL, 0);
-	return 1;
+	return;
       }
     /* fall through */
   case SCSI_SYNCHRONIZE_CACHE:
@@ -375,12 +371,12 @@ int msc_handle_cbw (void)
     CSW.bCSWStatus = MSC_CSW_STATUS_PASSED;
     CSW.dCSWDataResidue = CBW.dCBWDataTransferLength;
     msc_send_result (NULL, 0);
-    return 1;
+    return;
   case SCSI_MODE_SENSE6:
     buf[0] = 0x03;
     buf[1] = buf[2] = buf[3] = 0;
     msc_send_result (buf, 4);
-    return 1;
+    return;
   case SCSI_READ_CAPACITY10:
     scsi_read_format_capacities (&nblocks, &secsize);
     buf[0]  = (uint8_t)((nblocks - 1) >> 24);
@@ -392,7 +388,7 @@ int msc_handle_cbw (void)
     buf[6] = (uint8_t)(secsize >> 8);
     buf[7] = (uint8_t)(secsize >> 0);
     msc_send_result (buf, 8);
-    return 1;
+    return;
   case SCSI_READ10:
   case SCSI_WRITE10:
     break;
@@ -402,12 +398,19 @@ int msc_handle_cbw (void)
 	CSW.bCSWStatus = MSC_CSW_STATUS_FAILED;
 	CSW.dCSWDataResidue = 0;
 	msc_send_result (NULL, 0);
-	return 1;
+	return;
+      }
+    else
+      {
+	msc_state = MSC_ERROR;
+	chSysLock ();
+	usb_stall_transmit ();
+	usb_stall_receive ();
+	chSysUnlock ();
+	return;
       }
-    return -1;
   }
 
-  lun = CBW.bCBWLUN;
   lba = (CBW.CBWCB[2] << 24) | (CBW.CBWCB[3] << 16)
       | (CBW.CBWCB[4] <<  8) | CBW.CBWCB[5];
 
@@ -429,7 +432,7 @@ int msc_handle_cbw (void)
 		  break;
 		}
 
-	      if ((p = msc_scsi_read (lun, lba)))
+	      if ((r = msc_scsi_read (lba, &p)) == 0)
 		{
 		  msc_send_data (p, 512);
 		  if (++CBW.CBWCB[5] == 0)
@@ -443,6 +446,11 @@ int msc_handle_cbw (void)
 	      else
 		{
 		  CSW.bCSWStatus = MSC_CSW_STATUS_FAILED;
+		  contingent_allegiance = 1;
+		  if (r == SCSI_ERROR_NOT_READY)
+		    set_scsi_sense_data (SCSI_ERROR_NOT_READY, 0x3a);
+		  else
+		    set_scsi_sense_data (r, 0x00);
 		  break;
 		}
 	    }
@@ -466,7 +474,7 @@ int msc_handle_cbw (void)
 		}
 
 	      msc_recv_data ();
-	      if (msc_scsi_write (lun, lba, buf, 512))
+	      if ((r = msc_scsi_write (lba, buf, 512)) == 0)
 		{
 		  if (++CBW.CBWCB[5] == 0)
 		    if (++CBW.CBWCB[4] == 0)
@@ -479,6 +487,11 @@ int msc_handle_cbw (void)
 	      else
 		{
 		  CSW.bCSWStatus = MSC_CSW_STATUS_FAILED;
+		  contingent_allegiance = 1;
+		  if (r == SCSI_ERROR_NOT_READY)
+		    set_scsi_sense_data (SCSI_ERROR_NOT_READY, 0x3a);
+		  else
+		    set_scsi_sense_data (r, 0x00);
 		  break;
 		}
 	    }
@@ -486,6 +499,4 @@ int msc_handle_cbw (void)
 	  msc_send_result (NULL, 0);
 	}
     }
-
-  return 1;
 }
diff --git a/src/usb_msc.h b/src/usb_msc.h
index 3a117e0..567e4dc 100644
--- a/src/usb_msc.h
+++ b/src/usb_msc.h
@@ -43,3 +43,10 @@ struct CSW {
   uint32_t dCSWDataResidue;
   uint8_t bCSWStatus;
 } __attribute__((packed));
+
+#define SCSI_ERROR_NOT_READY 2
+#define SCSI_ERROR_ILLEAGAL_REQUEST 5
+#define SCSI_ERROR_UNIT_ATTENTION 6
+#define SCSI_ERROR_DATA_PROTECT 7
+
+extern uint8_t media_available;
diff --git a/src/virtual_block_device.c b/src/virtual_block_device.c
index 51cadca..27e0b8c 100644
--- a/src/virtual_block_device.c
+++ b/src/virtual_block_device.c
@@ -6,6 +6,8 @@
 
 extern Thread *main_thread;
 
+#define TOTAL_SECTOR 68
+
 /*
 
 blk=0: master boot record sector
@@ -17,7 +19,7 @@ blk=4: fat cluster #2
 blk=4+63: fat cluster #2+63
 */
 
-static const uint8_t d0_0_sector[512] = {
+static const uint8_t d0_0_sector[] = {
   0xeb, 0x3c,			       /* Jump instruction */
   0x90, /* NOP */
 
@@ -29,7 +31,7 @@ static const uint8_t d0_0_sector[512] = {
   0x01, 0x00,			/* reserved sector count: 1 */
   0x02, 			/* Number of FATs: 2 */
   0x10, 0x00,			/* Max. root directory entries: 16 (1 sector) */
-  0x44, 0x00,			/* total sectors: 68 */
+  TOTAL_SECTOR, 0x00,		/* total sectors: 68 */
   0xf8,				/* media descriptor: fixed disk */
   0x01, 0x00,			/* sectors per FAT: 1 */
   0x04, 0x00, 			/* sectors per track: 4 */
@@ -46,14 +48,24 @@ static const uint8_t d0_0_sector[512] = {
 
   0x46, 0x41, 0x54, 0x31, 0x32, 0x20, 0x20, 0x20, /* FAT12 */
 
-  0x0e, 0x1f,
-  0xbe, 0x5b, 0x7c, 0xac, 0x22, 0xc0, 0x74, 0x0b,
-  0x56, 0xb4, 0x0e, 0xbb, 0x07, 0x00, 0xcd, 0x10,
-  0x5e, 0xeb, 0xf0, 0x32, 0xe4, 0xcd, 0x16, 0xcd,
-  0x19,
-  0xeb, 0xfe,			/* loop: jmp loop */
+  0x0e,				/*    push cs */
+  0x1f,				/*    pop ds */
+  0xbe, 0x5b, 0x7c,		/*    mov si, offset message_txt */
+  0xac,				/* 1: lodsb */
+  0x22, 0xc0,			/*    and al, al */
+  0x74, 0x0b,			/*    jz 2f */
+  0x56,				/*    push si */
+  0xb4, 0x0e,			/*    mov ah, 0eh */
+  0xbb, 0x07, 0x00,		/*    mov bx, 0007h */
+  0xcd, 0x10,			/*    int 10h ; output char color=white */
+  0x5e,				/*    pop si */
+  0xeb, 0xf0,			/*    jmp 1b */
+  0x32, 0xe4,			/* 2: xor ah, ah */
+  0xcd, 0x16,			/*    int 16h; key input */
+  0xcd, 0x19,			/*    int 19h; load OS */
+  0xeb, 0xfe,			/* 3: jmp 3b */
 
-  /* "Thisis not a bootable disk... \r\n" */
+  /* "This is not a bootable disk... \r\n" */
   0x54, 0x68, 0x69, 0x73, 0x20,
   0x69, 0x73, 0x20, 0x6e, 0x6f, 0x74, 0x20, 0x61,
   0x20, 0x62, 0x6f, 0x6f, 0x74, 0x61, 0x62, 0x6c,
@@ -67,61 +79,16 @@ static const uint8_t d0_0_sector[512] = {
   0x6b, 0x65, 0x79, 0x20, 0x74, 0x6f, 0x20, 0x74,
   0x72, 0x79, 0x20, 0x61, 0x67, 0x61, 0x69, 0x6e,
   0x20, 0x2e, 0x2e, 0x2e, 0x20, 0x0d, 0x0a, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x55, 0xaa
 };
 
-static const uint8_t d0_fat0_sector[512] = {
+static const uint8_t d0_fat0_sector[] = {
   0xf8, 0xff, 0xff,	/* Media descriptor: fixed disk */ /* EOC */
   0xff, 0xff, 0xff,	/* cluster 2: used */ /* cluster 3: used */
   0xff, 0xff, 0xff,	/* cluster 4: used */ /* cluster 5: used */
   0xff, 0xff, 0xff,	/* cluster 6: used */ /* cluster 7: used */
   0xff, 0x0f, 0x00,	/* cluster 8: used */ /* cluster 9: free */
-  0x00,
 };
 
-static const uint8_t zero_sector[512] = {
-  0x00,
-}; 
-
 static uint8_t the_sector[512];
 
 struct folder {
@@ -157,14 +124,13 @@ static uint8_t *fill_file_entry (uint8_t *p, const uint8_t *filename,
   return p;
 }
 
-static const uint8_t *build_directory_sector (uint8_t *p, uint8_t index)
+static void build_directory_sector (uint8_t *p, uint8_t index)
 {
   uint16_t cluster_no = FOLDER_INDEX_TO_CLUSTER_NO (index);
   int i;
   uint8_t filename[11] = { 0x2e, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
 			   0x20, 0x20, 0x20 };
   uint8_t child;
-  uint8_t *p_orig = p;
 
   memset (p, 0, 512);
 
@@ -184,20 +150,33 @@ static const uint8_t *build_directory_sector (uint8_t *p, uint8_t index)
       }
     else
       break;
-
-  return p_orig;
 }
 
-const uint8_t *
-msc_scsi_read (uint8_t lun, uint32_t lba)
+int
+msc_scsi_read (uint32_t lba, const uint8_t **sector_p)
 {
+  if (!media_available)
+    return SCSI_ERROR_NOT_READY;
+
+  if (lba >= TOTAL_SECTOR)
+    return SCSI_ERROR_ILLEAGAL_REQUEST;
+
   switch (lba)
     {
     case 0:
-      return d0_0_sector;
+      *sector_p = the_sector;
+      memcpy (the_sector, d0_0_sector, sizeof d0_0_sector);
+      memset (the_sector + sizeof d0_0_sector, 0, 512 - sizeof d0_0_sector);
+      the_sector[510] = 0x55;
+      the_sector[511] = 0xaa;
+      return 0;
     case 1:
     case 2:
-      return d0_fat0_sector;
+      *sector_p = the_sector;
+      memcpy (the_sector, d0_fat0_sector, sizeof d0_fat0_sector);
+      memset (the_sector + sizeof d0_fat0_sector, 0,
+	      512 - sizeof d0_fat0_sector);
+      return 0;
     case 3:
     case 4:
     case 5:
@@ -206,9 +185,13 @@ msc_scsi_read (uint8_t lun, uint32_t lba)
     case 8:
     case 9:
     case 10:
-      return build_directory_sector (the_sector, LBA_TO_FOLDER_INDEX (lba));
+      *sector_p = the_sector;
+      build_directory_sector (the_sector, LBA_TO_FOLDER_INDEX (lba));
+      return 0;
     default:
-      return zero_sector;
+      *sector_p = the_sector;
+      memset (the_sector, 0, 512);
+      return 0;
     }
 }
 
@@ -258,14 +241,19 @@ static void parse_directory_sector (const uint8_t *p, uint8_t index)
 int
 msc_scsi_write (uint8_t lun, uint32_t lba, const uint8_t *buf, size_t size)
 {
-  if (lba <= 2)
-    return 1;				/* error */
+  if (!media_available)
+    return SCSI_ERROR_NOT_READY;
+
+  if (lba >= TOTAL_SECTOR)
+    return SCSI_ERROR_ILLEAGAL_REQUEST;
+
+  if (lba <= 2 || lba >= 11)
+    return SCSI_ERROR_DATA_PROTECT;
   else
     {
       uint8_t index = LBA_TO_FOLDER_INDEX (lba);
 
       parse_directory_sector (buf, index);
+      return 0;
     }
-
-  return 1;
 }