From 48bfd6f34364abffb21849d128f4f4fd6dd65134 Mon Sep 17 00:00:00 2001
From: Michael McMaster <michael@codesrc.com>
Date: Wed, 16 Apr 2014 20:43:43 +1000
Subject: [PATCH] Fix completely broken MODE SELECT.

Setting the blocksize via a MODE SELECT works now.
---
 software/SCSI2SD/src/mode.c | 40 +++++++++++++++++++++++++++++--------
 software/SCSI2SD/src/scsi.c | 18 ++++++++---------
 2 files changed, 41 insertions(+), 17 deletions(-)

diff --git a/software/SCSI2SD/src/mode.c b/software/SCSI2SD/src/mode.c
index 385fdf8..21ddadd 100755
--- a/software/SCSI2SD/src/mode.c
+++ b/software/SCSI2SD/src/mode.c
@@ -329,18 +329,39 @@ static void doModeSelect(void)
 		// scsiDev.dataLen bytes are in scsiDev.data
 
 		int idx;
-		if (scsiDev.cdb[0] == 0x15)
+		int blockDescLen;
+		if (scsiDev.cdb[0] == 0x55)
 		{
-			int blockDescLen =
+			blockDescLen =
 				(((uint16_t)scsiDev.data[6]) << 8) |scsiDev.data[7];
-			idx = 8 + blockDescLen;
+			idx = 8;
 		}
 		else
 		{
-			int blockDescLen = scsiDev.data[3];
-			idx = 4 + blockDescLen;
+			blockDescLen = scsiDev.data[3];
+			idx = 4;
 		}
-		if (idx > scsiDev.dataLen) goto bad;
+		
+		// The unwritten rule.  Blocksizes are normally set using the
+		// block descriptor value, not by changing page 0x03.
+		if (blockDescLen >= 8)
+		{
+			uint32_t bytesPerSector =
+				(((uint32_t)scsiDev.data[idx+5]) << 16) |
+				(((uint32_t)scsiDev.data[idx+6]) << 8) |
+				scsiDev.data[idx+7];
+			if ((bytesPerSector < MIN_SECTOR_SIZE) ||
+				(bytesPerSector > MAX_SECTOR_SIZE))
+			{
+				goto bad;
+			}
+			else if (bytesPerSector != config->bytesPerSector)
+			{
+				config->bytesPerSector = bytesPerSector;
+				configSave();
+			}
+		}
+		idx += blockDescLen;
 
 		while (idx < scsiDev.dataLen)
 		{
@@ -373,9 +394,12 @@ static void doModeSelect(void)
 				}
 			}
 			break;
-			default:
-				goto bad;
+			//default:
+			
+				// Easiest to just ignore for now. We'll get here when changing
+				// the SCSI block size via the descriptor header.
 			}
+			idx += 2 + pageLen;
 		}
 	}
 
diff --git a/software/SCSI2SD/src/scsi.c b/software/SCSI2SD/src/scsi.c
index 581c17a..3751833 100755
--- a/software/SCSI2SD/src/scsi.c
+++ b/software/SCSI2SD/src/scsi.c
@@ -178,14 +178,7 @@ static void process_DataIn()
 	if ((scsiDev.dataPtr >= scsiDev.dataLen) &&
 		(transfer.currentBlock == transfer.blocks))
 	{
-		if (scsiDev.postDataOutHook != NULL)
-		{
-			scsiDev.postDataOutHook();
-		}
-		else
-		{
-			enter_Status(GOOD);
-		}
+		enter_Status(GOOD);
 	}
 }
 
@@ -219,7 +212,14 @@ static void process_DataOut()
 	if ((scsiDev.dataPtr >= scsiDev.dataLen) &&
 		(transfer.currentBlock == transfer.blocks))
 	{
-		enter_Status(GOOD);
+		if (scsiDev.postDataOutHook != NULL)
+		{
+			scsiDev.postDataOutHook();
+		}
+		else
+		{
+			enter_Status(GOOD);
+		}
 	}
 }
 
-- 
2.38.5