dos2unix - CRLF to LF

lib/SCSI2SD/src/firmware/config.h

@@ -1,30 +1,30 @@
-//	Copyright (C) 2013 Michael McMaster <michael@codesrc.com>
-//
-//	This file is part of SCSI2SD.
-//
-//	SCSI2SD is free software: you can redistribute it and/or modify
-//	it under the terms of the GNU General Public License as published by
-//	the Free Software Foundation, either version 3 of the License, or
-//	(at your option) any later version.
-//
-//	SCSI2SD is distributed in the hope that it will be useful,
-//	but WITHOUT ANY WARRANTY; without even the implied warranty of
-//	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-//	GNU General Public License for more details.
-//
-//	You should have received a copy of the GNU General Public License
-//	along with SCSI2SD.  If not, see <http://www.gnu.org/licenses/>.
-#ifndef S2S_Config_H
-#define S2S_Config_H
-
-#include "scsi2sd.h"
-
-void s2s_configInit(S2S_BoardCfg* config);
-void s2s_debugInit(void);
-void s2s_configPoll(void);
-void s2s_configSave(int scsiId, uint16_t byesPerSector);
-
-const S2S_TargetCfg* s2s_getConfigByIndex(int index);
-const S2S_TargetCfg* s2s_getConfigById(int scsiId);
-
-#endif
+//	Copyright (C) 2013 Michael McMaster <michael@codesrc.com>
+//
+//	This file is part of SCSI2SD.
+//
+//	SCSI2SD is free software: you can redistribute it and/or modify
+//	it under the terms of the GNU General Public License as published by
+//	the Free Software Foundation, either version 3 of the License, or
+//	(at your option) any later version.
+//
+//	SCSI2SD is distributed in the hope that it will be useful,
+//	but WITHOUT ANY WARRANTY; without even the implied warranty of
+//	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//	GNU General Public License for more details.
+//
+//	You should have received a copy of the GNU General Public License
+//	along with SCSI2SD.  If not, see <http://www.gnu.org/licenses/>.
+#ifndef S2S_Config_H
+#define S2S_Config_H
+
+#include "scsi2sd.h"
+
+void s2s_configInit(S2S_BoardCfg* config);
+void s2s_debugInit(void);
+void s2s_configPoll(void);
+void s2s_configSave(int scsiId, uint16_t byesPerSector);
+
+const S2S_TargetCfg* s2s_getConfigByIndex(int index);
+const S2S_TargetCfg* s2s_getConfigById(int scsiId);
+
+#endif

lib/SCSI2SD/src/firmware/diagnostic.c

@@ -1,247 +1,247 @@
-//	Copyright (C) 2013 Michael McMaster <michael@codesrc.com>
-//
-//	This file is part of SCSI2SD.
-//
-//	SCSI2SD is free software: you can redistribute it and/or modify
-//	it under the terms of the GNU General Public License as published by
-//	the Free Software Foundation, either version 3 of the License, or
-//	(at your option) any later version.
-//
-//	SCSI2SD is distributed in the hope that it will be useful,
-//	but WITHOUT ANY WARRANTY; without even the implied warranty of
-//	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-//	GNU General Public License for more details.
-//
-//	You should have received a copy of the GNU General Public License
-//	along with SCSI2SD.  If not, see <http://www.gnu.org/licenses/>.
-
-#include "scsi.h"
-#include "diagnostic.h"
-
-#include <string.h>
-
-static const uint8_t SupportedDiagnosticPages[] =
-{
-0x00, // Page Code
-0x00, // Reserved
-0x02, // Page length
-0x00, // Support "Supported diagnostic page"
-0x40  // Support "Translate address page"
-};
-
-void scsiSendDiagnostic()
-{
-	// SEND DIAGNOSTIC
-	// Pretend to do self-test. Actual data is returned via the
-	// RECEIVE DIAGNOSTIC RESULTS command.
-	int selfTest = scsiDev.cdb[1] & 0x04;
-	uint32_t paramLength =
-		(((uint32_t) scsiDev.cdb[3]) << 8) +
-		scsiDev.cdb[4];
-
-	if (!selfTest)
-	{
-		// Initiator sends us page data.
-		scsiDev.dataLen = paramLength;
-		scsiDev.phase = DATA_OUT;
-
-		if (scsiDev.dataLen > sizeof (scsiDev.data))
-		{
-			// Nowhere to store this data!
-			// Shouldn't happen - our buffer should be many magnitudes larger
-			// than the required size for diagnostic parameters.
-			scsiDev.target->sense.code = ILLEGAL_REQUEST;
-			scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
-			scsiDev.status = CHECK_CONDITION;
-			scsiDev.phase = STATUS;
-		}
-	}
-	else
-	{
-		// Default command result will be a status of GOOD anyway.
-	}
-}
-
-void scsiReceiveDiagnostic()
-{
-	// RECEIVE DIAGNOSTIC RESULTS
-	// We assume scsiDev.data contains the contents of a previous
-	// SEND DIAGNOSTICS command.  We only care about the page-code part
-	// of the parameter list.
-	uint8_t pageCode = scsiDev.data[0];
-
-	int allocLength =
-		(((uint16_t) scsiDev.cdb[3]) << 8) +
-		scsiDev.cdb[4];
-
-
-	if (pageCode == 0x00)
-	{
-		memcpy(
-			scsiDev.data,
-			SupportedDiagnosticPages,
-			sizeof(SupportedDiagnosticPages));
-		scsiDev.dataLen = sizeof(SupportedDiagnosticPages);
-		scsiDev.phase = DATA_IN;
-	}
-	else if (pageCode == 0x40)
-	{
-		// Translate between logical block address, physical sector address, or
-		// physical bytes.
-		uint8_t suppliedFmt = scsiDev.data[4] & 0x7;
-		uint8_t translateFmt = scsiDev.data[5] & 0x7;
-
-		// Convert each supplied address back to a simple
-		// 64bit linear address, then convert back again.
-		uint64_t fromByteAddr =
-			scsiByteAddress(
-				scsiDev.target->liveCfg.bytesPerSector,
-				scsiDev.target->cfg->headsPerCylinder,
-				scsiDev.target->cfg->sectorsPerTrack,
-				suppliedFmt,
-				&scsiDev.data[6]);
-
-		scsiSaveByteAddress(
-			scsiDev.target->liveCfg.bytesPerSector,
-			scsiDev.target->cfg->headsPerCylinder,
-			scsiDev.target->cfg->sectorsPerTrack,
-			translateFmt,
-			fromByteAddr,
-			&scsiDev.data[6]);
-
-		// Fill out the rest of the response.
-		// (Clear out any optional bits).
-		scsiDev.data[4] = suppliedFmt;
-		scsiDev.data[5] = translateFmt;
-
-		scsiDev.dataLen = 14;
-		scsiDev.phase = DATA_IN;
-	}
-	else
-	{
-		// error.
-		scsiDev.status = CHECK_CONDITION;
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
-		scsiDev.phase = STATUS;
-	}
-
-	if (scsiDev.phase == DATA_IN && scsiDev.dataLen > allocLength)
-	{
-		// simply truncate the response.
-		scsiDev.dataLen = allocLength;
-	}
-
-	{
-		// Set the first byte to indicate LUN presence.
-		if (scsiDev.lun) // We only support lun 0
-		{
-			scsiDev.data[0] = 0x7F;
-		}
-	}
-}
-
-void scsiReadBuffer()
-{
-	// READ BUFFER
-	// Used for testing the speed of the SCSI interface.
-	uint8_t mode = scsiDev.data[1] & 7;
-
-	int allocLength =
-		(((uint32_t) scsiDev.cdb[6]) << 16) +
-		(((uint32_t) scsiDev.cdb[7]) << 8) +
-		scsiDev.cdb[8];
-
-	if (mode == 0)
-	{
-		uint32_t maxSize = sizeof(scsiDev.data) - 4;
-		// 4 byte header
-		scsiDev.data[0] = 0;
-		scsiDev.data[1] = (maxSize >> 16) & 0xff;
-		scsiDev.data[2] = (maxSize >> 8) & 0xff;
-		scsiDev.data[3] = maxSize & 0xff;
-
-		scsiDev.dataLen =
-			(allocLength > sizeof(scsiDev.data)) ? sizeof(scsiDev.data) : allocLength;
-		scsiDev.phase = DATA_IN;
-	}
-	else if (mode == 0x2 && (scsiDev.cdb[2] == 0))
-	{
-		// TODO support BUFFER OFFSET fields in CDB
-		scsiDev.dataLen =
-			(allocLength > sizeof(scsiDev.data)) ? sizeof(scsiDev.data) : allocLength;
-		scsiDev.phase = DATA_IN;
-	}
-	else if (mode == 0x3)
-	{
-		uint32_t maxSize = sizeof(scsiDev.data) - 4;
-		// 4 byte header
-		scsiDev.data[0] = 0;
-		scsiDev.data[1] = (maxSize >> 16) & 0xff;
-		scsiDev.data[2] = (maxSize >> 8) & 0xff;
-		scsiDev.data[3] = maxSize & 0xff;
-
-		scsiDev.dataLen =
-			(allocLength > 4) ? 4: allocLength;
-		scsiDev.phase = DATA_IN;
-	}
-	else
-	{
-		// error.
-		scsiDev.status = CHECK_CONDITION;
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
-		scsiDev.phase = STATUS;
-	}
-}
-
-// Callback after the DATA OUT phase is complete.
-static void doWriteBuffer(void)
-{
-	if (scsiDev.status == GOOD) // skip if we've already encountered an error
-	{
-		// scsiDev.dataLen bytes are in scsiDev.data
-		// Don't shift it down 4 bytes ... this space is taken by
-		// the read buffer header anyway
-		scsiDev.phase = STATUS;
-	}
-}
-
-void scsiWriteBuffer()
-{
-	// WRITE BUFFER
-	// Used for testing the speed of the SCSI interface.
-	uint8_t mode = scsiDev.data[1] & 7;
-
-	int allocLength =
-		(((uint32_t) scsiDev.cdb[6]) << 16) +
-		(((uint32_t) scsiDev.cdb[7]) << 8) +
-		scsiDev.cdb[8];
-
-	if ((mode == 0 || mode == 2) && allocLength <= sizeof(scsiDev.data))
-	{
-		scsiDev.dataLen = allocLength;
-		scsiDev.phase = DATA_OUT;
-		scsiDev.postDataOutHook = doWriteBuffer;
-	}
-	else
-	{
-		// error.
-		scsiDev.status = CHECK_CONDITION;
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
-		scsiDev.phase = STATUS;
-	}
-}
-
-// XEBEC specific command. See
-// http://www.bitsavers.org/pdf/westernDigital/WD100x/79-000004_WD1002-SHD_OEM_Manual_Aug1984.pdf
-// Section 4.3.14
-void scsiWriteSectorBuffer()
-{
-	scsiDev.dataLen = scsiDev.target->liveCfg.bytesPerSector;
-	scsiDev.phase = DATA_OUT;
-	scsiDev.postDataOutHook = doWriteBuffer;
-}
-
-
+//	Copyright (C) 2013 Michael McMaster <michael@codesrc.com>
+//
+//	This file is part of SCSI2SD.
+//
+//	SCSI2SD is free software: you can redistribute it and/or modify
+//	it under the terms of the GNU General Public License as published by
+//	the Free Software Foundation, either version 3 of the License, or
+//	(at your option) any later version.
+//
+//	SCSI2SD is distributed in the hope that it will be useful,
+//	but WITHOUT ANY WARRANTY; without even the implied warranty of
+//	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//	GNU General Public License for more details.
+//
+//	You should have received a copy of the GNU General Public License
+//	along with SCSI2SD.  If not, see <http://www.gnu.org/licenses/>.
+
+#include "scsi.h"
+#include "diagnostic.h"
+
+#include <string.h>
+
+static const uint8_t SupportedDiagnosticPages[] =
+{
+0x00, // Page Code
+0x00, // Reserved
+0x02, // Page length
+0x00, // Support "Supported diagnostic page"
+0x40  // Support "Translate address page"
+};
+
+void scsiSendDiagnostic()
+{
+	// SEND DIAGNOSTIC
+	// Pretend to do self-test. Actual data is returned via the
+	// RECEIVE DIAGNOSTIC RESULTS command.
+	int selfTest = scsiDev.cdb[1] & 0x04;
+	uint32_t paramLength =
+		(((uint32_t) scsiDev.cdb[3]) << 8) +
+		scsiDev.cdb[4];
+
+	if (!selfTest)
+	{
+		// Initiator sends us page data.
+		scsiDev.dataLen = paramLength;
+		scsiDev.phase = DATA_OUT;
+
+		if (scsiDev.dataLen > sizeof (scsiDev.data))
+		{
+			// Nowhere to store this data!
+			// Shouldn't happen - our buffer should be many magnitudes larger
+			// than the required size for diagnostic parameters.
+			scsiDev.target->sense.code = ILLEGAL_REQUEST;
+			scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+			scsiDev.status = CHECK_CONDITION;
+			scsiDev.phase = STATUS;
+		}
+	}
+	else
+	{
+		// Default command result will be a status of GOOD anyway.
+	}
+}
+
+void scsiReceiveDiagnostic()
+{
+	// RECEIVE DIAGNOSTIC RESULTS
+	// We assume scsiDev.data contains the contents of a previous
+	// SEND DIAGNOSTICS command.  We only care about the page-code part
+	// of the parameter list.
+	uint8_t pageCode = scsiDev.data[0];
+
+	int allocLength =
+		(((uint16_t) scsiDev.cdb[3]) << 8) +
+		scsiDev.cdb[4];
+
+
+	if (pageCode == 0x00)
+	{
+		memcpy(
+			scsiDev.data,
+			SupportedDiagnosticPages,
+			sizeof(SupportedDiagnosticPages));
+		scsiDev.dataLen = sizeof(SupportedDiagnosticPages);
+		scsiDev.phase = DATA_IN;
+	}
+	else if (pageCode == 0x40)
+	{
+		// Translate between logical block address, physical sector address, or
+		// physical bytes.
+		uint8_t suppliedFmt = scsiDev.data[4] & 0x7;
+		uint8_t translateFmt = scsiDev.data[5] & 0x7;
+
+		// Convert each supplied address back to a simple
+		// 64bit linear address, then convert back again.
+		uint64_t fromByteAddr =
+			scsiByteAddress(
+				scsiDev.target->liveCfg.bytesPerSector,
+				scsiDev.target->cfg->headsPerCylinder,
+				scsiDev.target->cfg->sectorsPerTrack,
+				suppliedFmt,
+				&scsiDev.data[6]);
+
+		scsiSaveByteAddress(
+			scsiDev.target->liveCfg.bytesPerSector,
+			scsiDev.target->cfg->headsPerCylinder,
+			scsiDev.target->cfg->sectorsPerTrack,
+			translateFmt,
+			fromByteAddr,
+			&scsiDev.data[6]);
+
+		// Fill out the rest of the response.
+		// (Clear out any optional bits).
+		scsiDev.data[4] = suppliedFmt;
+		scsiDev.data[5] = translateFmt;
+
+		scsiDev.dataLen = 14;
+		scsiDev.phase = DATA_IN;
+	}
+	else
+	{
+		// error.
+		scsiDev.status = CHECK_CONDITION;
+		scsiDev.target->sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+		scsiDev.phase = STATUS;
+	}
+
+	if (scsiDev.phase == DATA_IN && scsiDev.dataLen > allocLength)
+	{
+		// simply truncate the response.
+		scsiDev.dataLen = allocLength;
+	}
+
+	{
+		// Set the first byte to indicate LUN presence.
+		if (scsiDev.lun) // We only support lun 0
+		{
+			scsiDev.data[0] = 0x7F;
+		}
+	}
+}
+
+void scsiReadBuffer()
+{
+	// READ BUFFER
+	// Used for testing the speed of the SCSI interface.
+	uint8_t mode = scsiDev.data[1] & 7;
+
+	int allocLength =
+		(((uint32_t) scsiDev.cdb[6]) << 16) +
+		(((uint32_t) scsiDev.cdb[7]) << 8) +
+		scsiDev.cdb[8];
+
+	if (mode == 0)
+	{
+		uint32_t maxSize = sizeof(scsiDev.data) - 4;
+		// 4 byte header
+		scsiDev.data[0] = 0;
+		scsiDev.data[1] = (maxSize >> 16) & 0xff;
+		scsiDev.data[2] = (maxSize >> 8) & 0xff;
+		scsiDev.data[3] = maxSize & 0xff;
+
+		scsiDev.dataLen =
+			(allocLength > sizeof(scsiDev.data)) ? sizeof(scsiDev.data) : allocLength;
+		scsiDev.phase = DATA_IN;
+	}
+	else if (mode == 0x2 && (scsiDev.cdb[2] == 0))
+	{
+		// TODO support BUFFER OFFSET fields in CDB
+		scsiDev.dataLen =
+			(allocLength > sizeof(scsiDev.data)) ? sizeof(scsiDev.data) : allocLength;
+		scsiDev.phase = DATA_IN;
+	}
+	else if (mode == 0x3)
+	{
+		uint32_t maxSize = sizeof(scsiDev.data) - 4;
+		// 4 byte header
+		scsiDev.data[0] = 0;
+		scsiDev.data[1] = (maxSize >> 16) & 0xff;
+		scsiDev.data[2] = (maxSize >> 8) & 0xff;
+		scsiDev.data[3] = maxSize & 0xff;
+
+		scsiDev.dataLen =
+			(allocLength > 4) ? 4: allocLength;
+		scsiDev.phase = DATA_IN;
+	}
+	else
+	{
+		// error.
+		scsiDev.status = CHECK_CONDITION;
+		scsiDev.target->sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+		scsiDev.phase = STATUS;
+	}
+}
+
+// Callback after the DATA OUT phase is complete.
+static void doWriteBuffer(void)
+{
+	if (scsiDev.status == GOOD) // skip if we've already encountered an error
+	{
+		// scsiDev.dataLen bytes are in scsiDev.data
+		// Don't shift it down 4 bytes ... this space is taken by
+		// the read buffer header anyway
+		scsiDev.phase = STATUS;
+	}
+}
+
+void scsiWriteBuffer()
+{
+	// WRITE BUFFER
+	// Used for testing the speed of the SCSI interface.
+	uint8_t mode = scsiDev.data[1] & 7;
+
+	int allocLength =
+		(((uint32_t) scsiDev.cdb[6]) << 16) +
+		(((uint32_t) scsiDev.cdb[7]) << 8) +
+		scsiDev.cdb[8];
+
+	if ((mode == 0 || mode == 2) && allocLength <= sizeof(scsiDev.data))
+	{
+		scsiDev.dataLen = allocLength;
+		scsiDev.phase = DATA_OUT;
+		scsiDev.postDataOutHook = doWriteBuffer;
+	}
+	else
+	{
+		// error.
+		scsiDev.status = CHECK_CONDITION;
+		scsiDev.target->sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+		scsiDev.phase = STATUS;
+	}
+}
+
+// XEBEC specific command. See
+// http://www.bitsavers.org/pdf/westernDigital/WD100x/79-000004_WD1002-SHD_OEM_Manual_Aug1984.pdf
+// Section 4.3.14
+void scsiWriteSectorBuffer()
+{
+	scsiDev.dataLen = scsiDev.target->liveCfg.bytesPerSector;
+	scsiDev.phase = DATA_OUT;
+	scsiDev.postDataOutHook = doWriteBuffer;
+}
+
+

lib/SCSI2SD/src/firmware/geometry.c

@@ -1,228 +1,228 @@
-//	Copyright (C) 2013 Michael McMaster <michael@codesrc.com>
-//
-//	This file is part of SCSI2SD.
-//
-//	SCSI2SD is free software: you can redistribute it and/or modify
-//	it under the terms of the GNU General Public License as published by
-//	the Free Software Foundation, either version 3 of the License, or
-//	(at your option) any later version.
-//
-//	SCSI2SD is distributed in the hope that it will be useful,
-//	but WITHOUT ANY WARRANTY; without even the implied warranty of
-//	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-//	GNU General Public License for more details.
-//
-//	You should have received a copy of the GNU General Public License
-//	along with SCSI2SD.  If not, see <http://www.gnu.org/licenses/>.
-
-#include "geometry.h"
-#include "scsi.h"
-#include "sd.h"
-#include "config.h"
-
-#include <string.h>
-
-uint32_t getScsiCapacity(
-	uint32_t sdSectorStart,
-	uint16_t bytesPerSector,
-	uint32_t scsiSectors)
-{
-	uint32_t capacity =
-		(sdDev.capacity - sdSectorStart - S2S_CFG_SIZE) /
-			SDSectorsPerSCSISector(bytesPerSector);
-
-
-	if (sdDev.capacity == 0)
-	{
-		capacity = 0;
-	}
-	else if (sdSectorStart >= (sdDev.capacity - S2S_CFG_SIZE))
-	{
-		capacity = 0;
-	}
-	else if (scsiSectors && (capacity > scsiSectors))
-	{
-		capacity = scsiSectors;
-	}
-	return capacity;
-}
-
-
-uint32_t SCSISector2SD(
-	uint32_t sdSectorStart,
-	uint16_t bytesPerSector,
-	uint32_t scsiSector)
-{
-	return scsiSector * SDSectorsPerSCSISector(bytesPerSector) + sdSectorStart;
-}
-
-// Standard mapping according to ECMA-107 and ISO/IEC 9293:1994
-// Sector always starts at 1. There is no 0 sector.
-uint64_t CHS2LBA(
-	uint32_t c,
-	uint8_t h,
-	uint32_t s,
-	uint16_t headsPerCylinder,
-	uint16_t sectorsPerTrack)
-{
-	return (
-		(((uint64_t)c) * headsPerCylinder + h) *
-			(uint64_t) sectorsPerTrack
-		) + (s - 1);
-}
-
-
-void LBA2CHS(
-	uint32_t lba,
-	uint32_t* c,
-	uint8_t* h,
-	uint32_t* s,
-	uint16_t headsPerCylinder,
-	uint16_t sectorsPerTrack)
-{
-	*c = lba / (((uint32_t) sectorsPerTrack) * headsPerCylinder);
-	*h = (lba / sectorsPerTrack) % headsPerCylinder;
-	*s = (lba % sectorsPerTrack) + 1;
-}
-
-uint64_t scsiByteAddress(
-	uint16_t bytesPerSector,
-	uint16_t headsPerCylinder,
-	uint16_t sectorsPerTrack,
-	int format,
-	const uint8_t* addr)
-{
-	uint64_t result;
-	switch (format)
-	{
-	case ADDRESS_BLOCK:
-	{
-		uint32_t lba =
-			(((uint32_t) addr[0]) << 24) +
-			(((uint32_t) addr[1]) << 16) +
-			(((uint32_t) addr[2]) << 8) +
-			addr[3];
-
-		result = (uint64_t) bytesPerSector * lba;
-	} break;
-
-	case ADDRESS_PHYSICAL_BYTE:
-	{
-		uint32_t cyl =
-			(((uint32_t) addr[0]) << 16) +
-			(((uint32_t) addr[1]) << 8) +
-			addr[2];
-
-		uint8_t head = addr[3];
-
-		uint32_t bytes =
-			(((uint32_t) addr[4]) << 24) +
-			(((uint32_t) addr[5]) << 16) +
-			(((uint32_t) addr[6]) << 8) +
-			addr[7];
-
-		result = CHS2LBA(cyl, head, 1, headsPerCylinder, sectorsPerTrack) *
-			(uint64_t) bytesPerSector + bytes;
-	} break;
-
-	case ADDRESS_PHYSICAL_SECTOR:
-	{
-		uint32_t cyl =
-			(((uint32_t) addr[0]) << 16) +
-			(((uint32_t) addr[1]) << 8) +
-			addr[2];
-
-		uint8_t head = scsiDev.data[3];
-
-		uint32_t sector =
-			(((uint32_t) addr[4]) << 24) +
-			(((uint32_t) addr[5]) << 16) +
-			(((uint32_t) addr[6]) << 8) +
-			addr[7];
-
-		result = CHS2LBA(cyl, head, sector, headsPerCylinder, sectorsPerTrack) * (uint64_t) bytesPerSector;
-	} break;
-
-	default:
-		result = (uint64_t) -1;
-	}
-
-	return result;
-}
-
-
-void scsiSaveByteAddress(
-	uint16_t bytesPerSector,
-	uint16_t headsPerCylinder,
-	uint16_t sectorsPerTrack,
-	int format,
-	uint64_t byteAddr,
-	uint8_t* buf)
-{
-	uint32_t lba = byteAddr / bytesPerSector;
-	uint32_t byteOffset = byteAddr % bytesPerSector;
-
-	switch (format)
-	{
-	case ADDRESS_BLOCK:
-	{
-		buf[0] = lba >> 24;
-		buf[1] = lba >> 16;
-		buf[2] = lba >> 8;
-		buf[3] = lba;
-
-		buf[4] = 0;
-		buf[5] = 0;
-		buf[6] = 0;
-		buf[7] = 0;
-	} break;
-
-	case ADDRESS_PHYSICAL_BYTE:
-	{
-		uint32_t cyl;
-		uint8_t head;
-		uint32_t sector;
-		uint32_t bytes;
-
-		LBA2CHS(lba, &cyl, &head, &sector, headsPerCylinder, sectorsPerTrack);
-
-		bytes = sector * bytesPerSector + byteOffset;
-
-		buf[0] = cyl >> 16;
-		buf[1] = cyl >> 8;
-		buf[2] = cyl;
-
-		buf[3] = head;
-
-		buf[4] = bytes >> 24;
-		buf[5] = bytes >> 16;
-		buf[6] = bytes >> 8;
-		buf[7] = bytes;
-	} break;
-
-	case ADDRESS_PHYSICAL_SECTOR:
-	{
-		uint32_t cyl;
-		uint8_t head;
-		uint32_t sector;
-
-		LBA2CHS(lba, &cyl, &head, &sector, headsPerCylinder, sectorsPerTrack);
-
-		buf[0] = cyl >> 16;
-		buf[1] = cyl >> 8;
-		buf[2] = cyl;
-
-		buf[3] = head;
-
-		buf[4] = sector >> 24;
-		buf[5] = sector >> 16;
-		buf[6] = sector >> 8;
-		buf[7] = sector;
-	} break;
-
-	default:
-		memset(buf, 0, 8);
-	}
-
-}
-
+//	Copyright (C) 2013 Michael McMaster <michael@codesrc.com>
+//
+//	This file is part of SCSI2SD.
+//
+//	SCSI2SD is free software: you can redistribute it and/or modify
+//	it under the terms of the GNU General Public License as published by
+//	the Free Software Foundation, either version 3 of the License, or
+//	(at your option) any later version.
+//
+//	SCSI2SD is distributed in the hope that it will be useful,
+//	but WITHOUT ANY WARRANTY; without even the implied warranty of
+//	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//	GNU General Public License for more details.
+//
+//	You should have received a copy of the GNU General Public License
+//	along with SCSI2SD.  If not, see <http://www.gnu.org/licenses/>.
+
+#include "geometry.h"
+#include "scsi.h"
+#include "sd.h"
+#include "config.h"
+
+#include <string.h>
+
+uint32_t getScsiCapacity(
+	uint32_t sdSectorStart,
+	uint16_t bytesPerSector,
+	uint32_t scsiSectors)
+{
+	uint32_t capacity =
+		(sdDev.capacity - sdSectorStart - S2S_CFG_SIZE) /
+			SDSectorsPerSCSISector(bytesPerSector);
+
+
+	if (sdDev.capacity == 0)
+	{
+		capacity = 0;
+	}
+	else if (sdSectorStart >= (sdDev.capacity - S2S_CFG_SIZE))
+	{
+		capacity = 0;
+	}
+	else if (scsiSectors && (capacity > scsiSectors))
+	{
+		capacity = scsiSectors;
+	}
+	return capacity;
+}
+
+
+uint32_t SCSISector2SD(
+	uint32_t sdSectorStart,
+	uint16_t bytesPerSector,
+	uint32_t scsiSector)
+{
+	return scsiSector * SDSectorsPerSCSISector(bytesPerSector) + sdSectorStart;
+}
+
+// Standard mapping according to ECMA-107 and ISO/IEC 9293:1994
+// Sector always starts at 1. There is no 0 sector.
+uint64_t CHS2LBA(
+	uint32_t c,
+	uint8_t h,
+	uint32_t s,
+	uint16_t headsPerCylinder,
+	uint16_t sectorsPerTrack)
+{
+	return (
+		(((uint64_t)c) * headsPerCylinder + h) *
+			(uint64_t) sectorsPerTrack
+		) + (s - 1);
+}
+
+
+void LBA2CHS(
+	uint32_t lba,
+	uint32_t* c,
+	uint8_t* h,
+	uint32_t* s,
+	uint16_t headsPerCylinder,
+	uint16_t sectorsPerTrack)
+{
+	*c = lba / (((uint32_t) sectorsPerTrack) * headsPerCylinder);
+	*h = (lba / sectorsPerTrack) % headsPerCylinder;
+	*s = (lba % sectorsPerTrack) + 1;
+}
+
+uint64_t scsiByteAddress(
+	uint16_t bytesPerSector,
+	uint16_t headsPerCylinder,
+	uint16_t sectorsPerTrack,
+	int format,
+	const uint8_t* addr)
+{
+	uint64_t result;
+	switch (format)
+	{
+	case ADDRESS_BLOCK:
+	{
+		uint32_t lba =
+			(((uint32_t) addr[0]) << 24) +
+			(((uint32_t) addr[1]) << 16) +
+			(((uint32_t) addr[2]) << 8) +
+			addr[3];
+
+		result = (uint64_t) bytesPerSector * lba;
+	} break;
+
+	case ADDRESS_PHYSICAL_BYTE:
+	{
+		uint32_t cyl =
+			(((uint32_t) addr[0]) << 16) +
+			(((uint32_t) addr[1]) << 8) +
+			addr[2];
+
+		uint8_t head = addr[3];
+
+		uint32_t bytes =
+			(((uint32_t) addr[4]) << 24) +
+			(((uint32_t) addr[5]) << 16) +
+			(((uint32_t) addr[6]) << 8) +
+			addr[7];
+
+		result = CHS2LBA(cyl, head, 1, headsPerCylinder, sectorsPerTrack) *
+			(uint64_t) bytesPerSector + bytes;
+	} break;
+
+	case ADDRESS_PHYSICAL_SECTOR:
+	{
+		uint32_t cyl =
+			(((uint32_t) addr[0]) << 16) +
+			(((uint32_t) addr[1]) << 8) +
+			addr[2];
+
+		uint8_t head = scsiDev.data[3];
+
+		uint32_t sector =
+			(((uint32_t) addr[4]) << 24) +
+			(((uint32_t) addr[5]) << 16) +
+			(((uint32_t) addr[6]) << 8) +
+			addr[7];
+
+		result = CHS2LBA(cyl, head, sector, headsPerCylinder, sectorsPerTrack) * (uint64_t) bytesPerSector;
+	} break;
+
+	default:
+		result = (uint64_t) -1;
+	}
+
+	return result;
+}
+
+
+void scsiSaveByteAddress(
+	uint16_t bytesPerSector,
+	uint16_t headsPerCylinder,
+	uint16_t sectorsPerTrack,
+	int format,
+	uint64_t byteAddr,
+	uint8_t* buf)
+{
+	uint32_t lba = byteAddr / bytesPerSector;
+	uint32_t byteOffset = byteAddr % bytesPerSector;
+
+	switch (format)
+	{
+	case ADDRESS_BLOCK:
+	{
+		buf[0] = lba >> 24;
+		buf[1] = lba >> 16;
+		buf[2] = lba >> 8;
+		buf[3] = lba;
+
+		buf[4] = 0;
+		buf[5] = 0;
+		buf[6] = 0;
+		buf[7] = 0;
+	} break;
+
+	case ADDRESS_PHYSICAL_BYTE:
+	{
+		uint32_t cyl;
+		uint8_t head;
+		uint32_t sector;
+		uint32_t bytes;
+
+		LBA2CHS(lba, &cyl, &head, &sector, headsPerCylinder, sectorsPerTrack);
+
+		bytes = sector * bytesPerSector + byteOffset;
+
+		buf[0] = cyl >> 16;
+		buf[1] = cyl >> 8;
+		buf[2] = cyl;
+
+		buf[3] = head;
+
+		buf[4] = bytes >> 24;
+		buf[5] = bytes >> 16;
+		buf[6] = bytes >> 8;
+		buf[7] = bytes;
+	} break;
+
+	case ADDRESS_PHYSICAL_SECTOR:
+	{
+		uint32_t cyl;
+		uint8_t head;
+		uint32_t sector;
+
+		LBA2CHS(lba, &cyl, &head, &sector, headsPerCylinder, sectorsPerTrack);
+
+		buf[0] = cyl >> 16;
+		buf[1] = cyl >> 8;
+		buf[2] = cyl;
+
+		buf[3] = head;
+
+		buf[4] = sector >> 24;
+		buf[5] = sector >> 16;
+		buf[6] = sector >> 8;
+		buf[7] = sector;
+	} break;
+
+	default:
+		memset(buf, 0, 8);
+	}
+
+}
+

lib/SCSI2SD/src/firmware/inquiry.c

@@ -1,272 +1,272 @@
-//	Copyright (C) 2013 Michael McMaster <michael@codesrc.com>
-//	Copyright (C) 2019 Landon Rodgers  <g.landon.rodgers@gmail.com>
-//
-//	This file is part of SCSI2SD.
-//
-//	SCSI2SD is free software: you can redistribute it and/or modify
-//	it under the terms of the GNU General Public License as published by
-//	the Free Software Foundation, either version 3 of the License, or
-//	(at your option) any later version.
-//
-//	SCSI2SD is distributed in the hope that it will be useful,
-//	but WITHOUT ANY WARRANTY; without even the implied warranty of
-//	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-//	GNU General Public License for more details.
-//
-//	You should have received a copy of the GNU General Public License
-//	along with SCSI2SD.  If not, see <http://www.gnu.org/licenses/>.
-
-#include "scsi.h"
-#include "config.h"
-#include "inquiry.h"
-
-#include <string.h>
-
-static uint8_t StandardResponse[] =
-{
-0x00, // "Direct-access device". AKA standard hard disk
-0x00, // device type modifier
-0x02, // Complies with ANSI SCSI-2.
-0x01, // Response format is compatible with the old CCS format.
-0x1f, // standard length.
-0, 0, // Reserved
-0x18 // Enable sync and linked commands
-};
-// Vendor set by config 'c','o','d','e','s','r','c',' ',
-// prodId set by config'S','C','S','I','2','S','D',' ',' ',' ',' ',' ',' ',' ',' ',' ',
-// Revision set by config'2','.','0','a'
-
-
-static const uint8_t SupportedVitalPages[] =
-{
-0x00, // "Direct-access device". AKA standard hard disk
-0x00, // Page Code
-0x00, // Reserved
-0x04, // Page length
-0x00, // Support "Supported vital product data pages"
-0x80, // Support "Unit serial number page"
-0x81, // Support "Implemented operating definition page"
-0x82 // Support "ASCII Implemented operating definition page"
-};
-
-static const uint8_t UnitSerialNumber[] =
-{
-0x00, // "Direct-access device". AKA standard hard disk
-0x80, // Page Code
-0x00, // Reserved
-0x10, // Page length
-'c','o','d','e','s','r','c','-','1','2','3','4','5','6','7','8'
-};
-
-static const uint8_t ImpOperatingDefinition[] =
-{
-0x00, // "Direct-access device". AKA standard hard disk
-0x81, // Page Code
-0x00, // Reserved
-0x03, // Page length
-0x03, // Current: SCSI-2 operating definition
-0x03, // Default: SCSI-2 operating definition
-0x03 // Supported (list): SCSI-2 operating definition.
-};
-
-static const uint8_t AscImpOperatingDefinition[] =
-{
-0x00, // "Direct-access device". AKA standard hard disk
-0x82, // Page Code
-0x00, // Reserved
-0x07, // Page length
-0x06, // Ascii length
-'S','C','S','I','-','2'
-};
-
-void s2s_scsiInquiry()
-{
-	uint8_t evpd = scsiDev.cdb[1] & 1; // enable vital product data.
-	uint8_t pageCode = scsiDev.cdb[2];
-	uint32_t allocationLength = scsiDev.cdb[4];
-
-	// SASI standard, X3T9.3_185_RevE  states that 0 == 256 bytes
-	// BUT SCSI 2 standard says 0 == 0.
-	if (scsiDev.compatMode <= COMPAT_SCSI1) // excludes COMPAT_SCSI2_DISABLED
-	{
-		if (allocationLength == 0) allocationLength = 256;
-	}
-
-	if (!evpd)
-	{
-		if (pageCode)
-		{
-			// error.
-			scsiDev.status = CHECK_CONDITION;
-			scsiDev.target->sense.code = ILLEGAL_REQUEST;
-			scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
-			scsiDev.phase = STATUS;
-		}
-		else
-		{
-			const S2S_TargetCfg* config = scsiDev.target->cfg;
-			scsiDev.dataLen =
-				s2s_getStandardInquiry(
-					config,
-					scsiDev.data,
-					sizeof(scsiDev.data));
-			scsiDev.phase = DATA_IN;
-		}
-	}
-	else if (pageCode == 0x00)
-	{
-		memcpy(scsiDev.data, SupportedVitalPages, sizeof(SupportedVitalPages));
-		scsiDev.dataLen = sizeof(SupportedVitalPages);
-		scsiDev.phase = DATA_IN;
-	}
-	else if (pageCode == 0x80)
-	{
-		memcpy(scsiDev.data, UnitSerialNumber, sizeof(UnitSerialNumber));
-		scsiDev.dataLen = sizeof(UnitSerialNumber);
-        const S2S_TargetCfg* config = scsiDev.target->cfg;
-        memcpy(&scsiDev.data[4], config->serial, sizeof(config->serial));
-		scsiDev.phase = DATA_IN;
-	}
-	else if (pageCode == 0x81)
-	{
-		memcpy(
-			scsiDev.data,
-			ImpOperatingDefinition,
-			sizeof(ImpOperatingDefinition));
-		scsiDev.dataLen = sizeof(ImpOperatingDefinition);
-		scsiDev.phase = DATA_IN;
-	}
-	else if (pageCode == 0x82)
-	{
-		memcpy(
-			scsiDev.data,
-			AscImpOperatingDefinition,
-			sizeof(AscImpOperatingDefinition));
-		scsiDev.dataLen = sizeof(AscImpOperatingDefinition);
-		scsiDev.phase = DATA_IN;
-	}
-	else
-	{
-		// error.
-		scsiDev.status = CHECK_CONDITION;
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
-		scsiDev.phase = STATUS;
-	}
-
-
-	if (scsiDev.phase == DATA_IN)
-	{
-		// VAX workaround
-		if (allocationLength == 255 &&
-			(scsiDev.target->cfg->quirks & S2S_CFG_QUIRKS_VMS))
-		{
-			allocationLength = 254;
-		}
-
-		// "real" hard drives send back exactly allocationLenth bytes, padded
-		// with zeroes. This only seems to happen for Inquiry responses, and not
-		// other commands that also supply an allocation length such as Mode Sense or
-		// Request Sense.
-		// (See below for exception to this rule when 0 allocation length)
-		if (scsiDev.dataLen < allocationLength)
-		{
-			memset(
-				&scsiDev.data[scsiDev.dataLen],
-				0,
-				allocationLength - scsiDev.dataLen);
-		}
-		// Spec 8.2.5 requires us to simply truncate the response if it's
-		// too big.
-		scsiDev.dataLen = allocationLength;
-
-		// Set the device type as needed.
-		scsiDev.data[0] = getDeviceTypeQualifier();
-
-		switch (scsiDev.target->cfg->deviceType)
-		{
-		case S2S_CFG_OPTICAL:
-			scsiDev.data[1] |= 0x80; // Removable bit.
-			break;
-
-		case S2S_CFG_SEQUENTIAL:
-			scsiDev.data[1] |= 0x80; // Removable bit.
-			break;
-
-		case S2S_CFG_MO:
-			scsiDev.data[1] |= 0x80; // Removable bit.
-			break;
-
-		case S2S_CFG_FLOPPY_14MB:
-		case S2S_CFG_REMOVEABLE:
-			scsiDev.data[1] |= 0x80; // Removable bit.
-			break;
-		default:
-			// Accept defaults for a fixed disk.
-			break;
-		}
-	}
-
-	// Set the first byte to indicate LUN presence.
-	if (scsiDev.lun) // We only support lun 0
-	{
-		scsiDev.data[0] = 0x7F;
-	}
-}
-
-uint32_t s2s_getStandardInquiry(
-	const S2S_TargetCfg* cfg, uint8_t* out, uint32_t maxlen
-	)
-{
-	uint32_t buflen = sizeof(StandardResponse);
-	if (buflen > maxlen) buflen = maxlen;
-
-	memcpy(out, StandardResponse, buflen);
-	out[1] = cfg->deviceTypeModifier;
-
-	if (!(scsiDev.boardCfg.flags & S2S_CFG_ENABLE_SCSI2))
-	{
-		out[2] = 1; // Report only SCSI 1 compliance version
-	}
-
-	if (scsiDev.compatMode >= COMPAT_SCSI2)
-	{
-		out[3] = 2; // SCSI 2 response format.
-	}
-	memcpy(&out[8], cfg->vendor, sizeof(cfg->vendor));
-	memcpy(&out[16], cfg->prodId, sizeof(cfg->prodId));
-	memcpy(&out[32], cfg->revision, sizeof(cfg->revision));
-	return sizeof(StandardResponse) +
-		sizeof(cfg->vendor) +
-		sizeof(cfg->prodId) +
-		sizeof(cfg->revision);
-}
-
-uint8_t getDeviceTypeQualifier()
-{
-	// Set the device type as needed.
-	switch (scsiDev.target->cfg->deviceType)
-	{
-	case S2S_CFG_OPTICAL:
-		return 0x05;
-		break;
-
-	case S2S_CFG_SEQUENTIAL:
-		return 0x01;
-		break;
-
-	case S2S_CFG_MO:
-		return 0x07;
-		break;
-
-	case S2S_CFG_FLOPPY_14MB:
-	case S2S_CFG_REMOVEABLE:
-		return 0;
-		break;
-
-	default:
-		// Accept defaults for a fixed disk.
-		return 0;
-	}
-}
-
+//	Copyright (C) 2013 Michael McMaster <michael@codesrc.com>
+//	Copyright (C) 2019 Landon Rodgers  <g.landon.rodgers@gmail.com>
+//
+//	This file is part of SCSI2SD.
+//
+//	SCSI2SD is free software: you can redistribute it and/or modify
+//	it under the terms of the GNU General Public License as published by
+//	the Free Software Foundation, either version 3 of the License, or
+//	(at your option) any later version.
+//
+//	SCSI2SD is distributed in the hope that it will be useful,
+//	but WITHOUT ANY WARRANTY; without even the implied warranty of
+//	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//	GNU General Public License for more details.
+//
+//	You should have received a copy of the GNU General Public License
+//	along with SCSI2SD.  If not, see <http://www.gnu.org/licenses/>.
+
+#include "scsi.h"
+#include "config.h"
+#include "inquiry.h"
+
+#include <string.h>
+
+static uint8_t StandardResponse[] =
+{
+0x00, // "Direct-access device". AKA standard hard disk
+0x00, // device type modifier
+0x02, // Complies with ANSI SCSI-2.
+0x01, // Response format is compatible with the old CCS format.
+0x1f, // standard length.
+0, 0, // Reserved
+0x18 // Enable sync and linked commands
+};
+// Vendor set by config 'c','o','d','e','s','r','c',' ',
+// prodId set by config'S','C','S','I','2','S','D',' ',' ',' ',' ',' ',' ',' ',' ',' ',
+// Revision set by config'2','.','0','a'
+
+
+static const uint8_t SupportedVitalPages[] =
+{
+0x00, // "Direct-access device". AKA standard hard disk
+0x00, // Page Code
+0x00, // Reserved
+0x04, // Page length
+0x00, // Support "Supported vital product data pages"
+0x80, // Support "Unit serial number page"
+0x81, // Support "Implemented operating definition page"
+0x82 // Support "ASCII Implemented operating definition page"
+};
+
+static const uint8_t UnitSerialNumber[] =
+{
+0x00, // "Direct-access device". AKA standard hard disk
+0x80, // Page Code
+0x00, // Reserved
+0x10, // Page length
+'c','o','d','e','s','r','c','-','1','2','3','4','5','6','7','8'
+};
+
+static const uint8_t ImpOperatingDefinition[] =
+{
+0x00, // "Direct-access device". AKA standard hard disk
+0x81, // Page Code
+0x00, // Reserved
+0x03, // Page length
+0x03, // Current: SCSI-2 operating definition
+0x03, // Default: SCSI-2 operating definition
+0x03 // Supported (list): SCSI-2 operating definition.
+};
+
+static const uint8_t AscImpOperatingDefinition[] =
+{
+0x00, // "Direct-access device". AKA standard hard disk
+0x82, // Page Code
+0x00, // Reserved
+0x07, // Page length
+0x06, // Ascii length
+'S','C','S','I','-','2'
+};
+
+void s2s_scsiInquiry()
+{
+	uint8_t evpd = scsiDev.cdb[1] & 1; // enable vital product data.
+	uint8_t pageCode = scsiDev.cdb[2];
+	uint32_t allocationLength = scsiDev.cdb[4];
+
+	// SASI standard, X3T9.3_185_RevE  states that 0 == 256 bytes
+	// BUT SCSI 2 standard says 0 == 0.
+	if (scsiDev.compatMode <= COMPAT_SCSI1) // excludes COMPAT_SCSI2_DISABLED
+	{
+		if (allocationLength == 0) allocationLength = 256;
+	}
+
+	if (!evpd)
+	{
+		if (pageCode)
+		{
+			// error.
+			scsiDev.status = CHECK_CONDITION;
+			scsiDev.target->sense.code = ILLEGAL_REQUEST;
+			scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+			scsiDev.phase = STATUS;
+		}
+		else
+		{
+			const S2S_TargetCfg* config = scsiDev.target->cfg;
+			scsiDev.dataLen =
+				s2s_getStandardInquiry(
+					config,
+					scsiDev.data,
+					sizeof(scsiDev.data));
+			scsiDev.phase = DATA_IN;
+		}
+	}
+	else if (pageCode == 0x00)
+	{
+		memcpy(scsiDev.data, SupportedVitalPages, sizeof(SupportedVitalPages));
+		scsiDev.dataLen = sizeof(SupportedVitalPages);
+		scsiDev.phase = DATA_IN;
+	}
+	else if (pageCode == 0x80)
+	{
+		memcpy(scsiDev.data, UnitSerialNumber, sizeof(UnitSerialNumber));
+		scsiDev.dataLen = sizeof(UnitSerialNumber);
+        const S2S_TargetCfg* config = scsiDev.target->cfg;
+        memcpy(&scsiDev.data[4], config->serial, sizeof(config->serial));
+		scsiDev.phase = DATA_IN;
+	}
+	else if (pageCode == 0x81)
+	{
+		memcpy(
+			scsiDev.data,
+			ImpOperatingDefinition,
+			sizeof(ImpOperatingDefinition));
+		scsiDev.dataLen = sizeof(ImpOperatingDefinition);
+		scsiDev.phase = DATA_IN;
+	}
+	else if (pageCode == 0x82)
+	{
+		memcpy(
+			scsiDev.data,
+			AscImpOperatingDefinition,
+			sizeof(AscImpOperatingDefinition));
+		scsiDev.dataLen = sizeof(AscImpOperatingDefinition);
+		scsiDev.phase = DATA_IN;
+	}
+	else
+	{
+		// error.
+		scsiDev.status = CHECK_CONDITION;
+		scsiDev.target->sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+		scsiDev.phase = STATUS;
+	}
+
+
+	if (scsiDev.phase == DATA_IN)
+	{
+		// VAX workaround
+		if (allocationLength == 255 &&
+			(scsiDev.target->cfg->quirks & S2S_CFG_QUIRKS_VMS))
+		{
+			allocationLength = 254;
+		}
+
+		// "real" hard drives send back exactly allocationLenth bytes, padded
+		// with zeroes. This only seems to happen for Inquiry responses, and not
+		// other commands that also supply an allocation length such as Mode Sense or
+		// Request Sense.
+		// (See below for exception to this rule when 0 allocation length)
+		if (scsiDev.dataLen < allocationLength)
+		{
+			memset(
+				&scsiDev.data[scsiDev.dataLen],
+				0,
+				allocationLength - scsiDev.dataLen);
+		}
+		// Spec 8.2.5 requires us to simply truncate the response if it's
+		// too big.
+		scsiDev.dataLen = allocationLength;
+
+		// Set the device type as needed.
+		scsiDev.data[0] = getDeviceTypeQualifier();
+
+		switch (scsiDev.target->cfg->deviceType)
+		{
+		case S2S_CFG_OPTICAL:
+			scsiDev.data[1] |= 0x80; // Removable bit.
+			break;
+
+		case S2S_CFG_SEQUENTIAL:
+			scsiDev.data[1] |= 0x80; // Removable bit.
+			break;
+
+		case S2S_CFG_MO:
+			scsiDev.data[1] |= 0x80; // Removable bit.
+			break;
+
+		case S2S_CFG_FLOPPY_14MB:
+		case S2S_CFG_REMOVEABLE:
+			scsiDev.data[1] |= 0x80; // Removable bit.
+			break;
+		default:
+			// Accept defaults for a fixed disk.
+			break;
+		}
+	}
+
+	// Set the first byte to indicate LUN presence.
+	if (scsiDev.lun) // We only support lun 0
+	{
+		scsiDev.data[0] = 0x7F;
+	}
+}
+
+uint32_t s2s_getStandardInquiry(
+	const S2S_TargetCfg* cfg, uint8_t* out, uint32_t maxlen
+	)
+{
+	uint32_t buflen = sizeof(StandardResponse);
+	if (buflen > maxlen) buflen = maxlen;
+
+	memcpy(out, StandardResponse, buflen);
+	out[1] = cfg->deviceTypeModifier;
+
+	if (!(scsiDev.boardCfg.flags & S2S_CFG_ENABLE_SCSI2))
+	{
+		out[2] = 1; // Report only SCSI 1 compliance version
+	}
+
+	if (scsiDev.compatMode >= COMPAT_SCSI2)
+	{
+		out[3] = 2; // SCSI 2 response format.
+	}
+	memcpy(&out[8], cfg->vendor, sizeof(cfg->vendor));
+	memcpy(&out[16], cfg->prodId, sizeof(cfg->prodId));
+	memcpy(&out[32], cfg->revision, sizeof(cfg->revision));
+	return sizeof(StandardResponse) +
+		sizeof(cfg->vendor) +
+		sizeof(cfg->prodId) +
+		sizeof(cfg->revision);
+}
+
+uint8_t getDeviceTypeQualifier()
+{
+	// Set the device type as needed.
+	switch (scsiDev.target->cfg->deviceType)
+	{
+	case S2S_CFG_OPTICAL:
+		return 0x05;
+		break;
+
+	case S2S_CFG_SEQUENTIAL:
+		return 0x01;
+		break;
+
+	case S2S_CFG_MO:
+		return 0x07;
+		break;
+
+	case S2S_CFG_FLOPPY_14MB:
+	case S2S_CFG_REMOVEABLE:
+		return 0;
+		break;
+
+	default:
+		// Accept defaults for a fixed disk.
+		return 0;
+	}
+}
+

lib/SCSI2SD/src/firmware/mode.c

@@ -1,746 +1,746 @@
-//	Copyright (C) 2013 Michael McMaster <michael@codesrc.com>
-//  Copyright (C) 2014 Doug Brown <doug@downtowndougbrown.com>
-//  Copyright (C) 2019 Landon Rodgers <g.landon.rodgers@gmail.com>
-//
-//	This file is part of SCSI2SD.
-//
-//	SCSI2SD is free software: you can redistribute it and/or modify
-//	it under the terms of the GNU General Public License as published by
-//	the Free Software Foundation, either version 3 of the License, or
-//	(at your option) any later version.
-//
-//	SCSI2SD is distributed in the hope that it will be useful,
-//	but WITHOUT ANY WARRANTY; without even the implied warranty of
-//	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-//	GNU General Public License for more details.
-//
-//	You should have received a copy of the GNU General Public License
-//	along with SCSI2SD.  If not, see <http://www.gnu.org/licenses/>.
-
-#include "scsi.h"
-#include "mode.h"
-#include "disk.h"
-#include "inquiry.h"
-
-#include <string.h>
-
-// "Vendor" defined page which was included by Seagate, and required for\r
-// Amiga 500 using DKB SpitFire controller.\r
-static const uint8_t OperatingPage[] =
-{
-0x00, // Page code
-0x02, // Page length
-
-// Bit 4 = unit attension (0 = on, 1 = off).
-// Bit 7 = usage bit, EEPROM life exceeded warning = 1.
-0x80, 
-
-// Bit 7 = reserved.
-// Bits 0:6: Device type qualifier, as per Inquiry data
-0x00
-};
-
-static const uint8_t ReadWriteErrorRecoveryPage[] =
-{
-0x01, // Page code
-0x0A, // Page length
-
-// VMS 5.5-2 is very particular regarding the mode page values.
-// The required values for a SCSI2/NoTCQ device are:
-// AWRE=0 ARRE=0 TB=1 RC=0 EER=? PER=1 DTE=1 DCR=?
-// See ftp://www.digiater.nl/openvms/decus/vms94b/net94b/scsi_params_dkdriver.txt
-// X-Newsgroups: comp.os.vms
-// Subject: Re: VMS 6.1 vs. Seagate Disk Drives
-// Message-Id: <32g87h$8q@nntpd.lkg.dec.com>
-// From: weber@evms.enet.dec.com (Ralph O. Weber -- OpenVMS AXP)
-// Date: 12 Aug 1994 16:32:49 GMT
-0x26,
-
-0x00, // Don't try recovery algorithm during reads
-0x00, // Correction span 0
-0x00, // Head offset count 0,
-0x00, // Data strobe offset count 0,
-0x00, // Reserved
-0x00, // Don't try recovery algorithm during writes
-0x00, // Reserved
-0x00, 0x00 // Recovery time limit 0 (use default)*/
-};
-
-static const uint8_t ReadWriteErrorRecoveryPage_SCSI1[] =
-{
-0x01, // Page code
-0x06, // Page length
-0x26,
-0x00, // Don't try recovery algorithm during reads
-0x00, // Correction span 0
-0x00, // Head offset count 0,
-0x00, // Data strobe offset count 0,
-0xFF // Reserved
-};
-
-static const uint8_t DisconnectReconnectPage[] =
-{
-0x02, // Page code
-0x0E, // Page length
-0, // Buffer full ratio
-0, // Buffer empty ratio
-0x00, 10, // Bus inactivity limit, 100us increments. Allow 1ms.
-0x00, 0x00, // Disconnect time limit
-0x00, 0x00, // Connect time limit
-0x00, 0x00, // Maximum burst size
-0x00 ,// DTDC. Not used.
-0x00, 0x00, 0x00 // Reserved
-};
-
-static const uint8_t DisconnectReconnectPage_SCSI1[] =
-{
-0x02, // Page code
-0x0A, // Page length
-0, // Buffer full ratio
-0, // Buffer empty ratio
-0x00, 10, // Bus inactivity limit, 100us increments. Allow 1ms.
-0x00, 0x00, // Disconnect time limit
-0x00, 0x00, // Connect time limit
-0x00, 0x00 // Maximum burst size
-};
-
-static const uint8_t FormatDevicePage[] =
-{
-0x03 | 0x80, // Page code | PS (persist) bit.
-0x16, // Page length
-0x00, 0x00, // Single zone
-0x00, 0x00, // No alternate sectors
-0x00, 0x00, // No alternate tracks
-0x00, 0x00, // No alternate tracks per lun
-0x00, 0x00, // Sectors per track, configurable
-0xFF, 0xFF, // Data bytes per physical sector. Configurable.
-0x00, 0x01, // Interleave
-0x00, 0x00, // Track skew factor
-0x00, 0x00, // Cylinder skew factor
-0xC0, // SSEC(set) HSEC(set) RMB SURF
-0x00, 0x00, 0x00 // Reserved
-};
-
-static const uint8_t RigidDiskDriveGeometry[] =
-{
-0x04, // Page code
-0x16, // Page length
-0xFF, 0xFF, 0xFF, // Number of cylinders
-0x00, // Number of heads (replaced by configured value)
-0xFF, 0xFF, 0xFF, // Starting cylinder-write precompensation
-0xFF, 0xFF, 0xFF, // Starting cylinder-reduced write current
-0x00, 0x1, // Drive step rate (units of 100ns)
-0x00, 0x00, 0x00, // Landing zone cylinder
-0x00, // RPL
-0x00, // Rotational offset
-0x00, // Reserved
-5400 >> 8, 5400 & 0xFF, // Medium rotation rate (RPM)
-0x00, 0x00 // Reserved
-};
-
-static const uint8_t FlexibleDiskDriveGeometry[] =
-{
-0x05, // Page code
-0x1E, // Page length
-0x01, 0xF4, // Transfer Rate (500kbits)
-0x01, // heads
-18, // sectors per track
-0x20,0x00, // bytes per sector
-0x00, 80, // Cylinders
-0x00, 0x80, // Write-precomp
-0x00, 0x80, // reduced current,
-0x00, 0x00, // Drive step rate
-0x00, // pulse width
-0x00, 0x00, // Head settle delay
-0x00, // motor on delay
-0x00,  // motor off delay
-0x00,
-0x00,
-0x00,
-0x00,
-0x00,
-0x00,
-0x00,
-0x00,
-0x00,
-0x00,
-0x00
-};
-
-static const uint8_t RigidDiskDriveGeometry_SCSI1[] =
-{
-0x04, // Page code
-0x12, // Page length
-0xFF, 0xFF, 0xFF, // Number of cylinders
-0x00, // Number of heads (replaced by configured value)
-0xFF, 0xFF, 0xFF, // Starting cylinder-write precompensation
-0xFF, 0xFF, 0xFF, // Starting cylinder-reduced write current
-0x00, 0x1, // Drive step rate (units of 100ns)
-0x00, 0x00, 0x00, // Landing zone cylinder
-0x00, // RPL
-0x00, // Rotational offset
-0x00 // Reserved
-};
-
-static const uint8_t CachingPage[] =
-{
-0x08, // Page Code
-0x0A, // Page length
-0x01, // Read cache disable
-0x00, // No useful rention policy.
-0x00, 0x00, // Pre-fetch always disabled
-0x00, 0x00, // Minimum pre-fetch
-0x00, 0x00, // Maximum pre-fetch
-0x00, 0x00, // Maximum pre-fetch ceiling
-};
-
-// Old CCS SCSI-1 cache page
-static const uint8_t CCSCachingPage[] =
-{
-0x38, // Page Code
-0x0E, // Page length
-0x00, // Read cache disable
-0x00, // Prefetch threshold
-0x00, 0x00, // Max threshold / multiplier
-0x00, 0x00, // Min threshold / multiplier
-0x00, 0x00, // Reserved
-0x00, 0x00,
-0x00, 0x00,
-0x00, 0x00,
-};
-
-static const uint8_t ControlModePage[] =
-{
-0x0A, // Page code
-0x06, // Page length
-0x00, // No logging
-0x01, // Disable tagged queuing
-0x00, // No async event notifications
-0x00, // Reserved
-0x00, 0x00 // AEN holdoff period.
-};
-
-static const uint8_t SequentialDeviceConfigPage[] =
-{
-0x10, // page code
-0x0E, // Page length
-0x00, // CAP, CAF, Active Format
-0x00, // Active partition
-0x00, // Write buffer full ratio
-0x00, // Read buffer empty ratio
-0x00,0x01, // Write delay time, in 100ms units
-0x00, // Default gap size
-0x10, // auto-generation of default eod (end of data)
-0x00,0x00,0x00, // buffer-size at early warning
-0x00, // No data compression
-0x00 // reserved
-};
-
-// Allow Apple 68k Drive Setup to format this drive.
-// Code
-static const uint8_t AppleVendorPage[] =
-{
-0x30, // Page code
-23, // Page length
-'A','P','P','L','E',' ','C','O','M','P','U','T','E','R',',',' ','I','N','C',' ',' ',' ',0x00
-};
-
-static void pageIn(int pc, int dataIdx, const uint8_t* pageData, int pageLen)
-{
-	memcpy(&scsiDev.data[dataIdx], pageData, pageLen);
-
-	if (pc == 0x01) // Mask out (un)changable values
-	{
-		memset(&scsiDev.data[dataIdx+2], 0, pageLen - 2);
-	}
-}
-
-static void doModeSense(
-	int sixByteCmd, int dbd, int pc, int pageCode, int allocLength)
-{
-	////////////// Mode Parameter Header
-	////////////////////////////////////
-
-	// Skip the Mode Data Length, we set that last.
-	int idx = 1;
-	if (!sixByteCmd) ++idx;
-
-	uint8_t mediumType = 0;
-	uint8_t deviceSpecificParam = 0;
-	uint8_t density = 0;
-	switch (scsiDev.target->cfg->deviceType)
-	{
-	case S2S_CFG_FIXED:
-	case S2S_CFG_REMOVEABLE:
-		mediumType = 0; // We should support various floppy types here!
-		// Contains cache bits (0) and a Write-Protect bit.
-		deviceSpecificParam =
-			(blockDev.state & DISK_WP) ? 0x80 : 0;
-		density = 0; // reserved for direct access
-		break;
-
-	case S2S_CFG_FLOPPY_14MB:
-		mediumType = 0x1E; // 90mm/3.5"
-		deviceSpecificParam =
-			(blockDev.state & DISK_WP) ? 0x80 : 0;
-		density = 0; // reserved for direct access
-		break;
-
-	case S2S_CFG_OPTICAL:
-		mediumType = 0x02; // 120mm CDROM, data only.
-		deviceSpecificParam = 0;
-		density = 0x01; // User data only, 2048bytes per sector.
-		break;
-
-	case S2S_CFG_SEQUENTIAL:
-		mediumType = 0; // reserved
-		deviceSpecificParam =
-			(blockDev.state & DISK_WP) ? 0x80 : 0;
-		density = 0x13; // DAT Data Storage, X3B5/88-185A 
-		break;
-
-	case S2S_CFG_MO:
-        mediumType = 0x03; // Optical reversible or erasable medium
-		deviceSpecificParam =
-			(blockDev.state & DISK_WP) ? 0x80 : 0;
-		density = 0x00; // Default
-		break;
-
-	};
-
-	scsiDev.data[idx++] = mediumType;
-	scsiDev.data[idx++] = deviceSpecificParam;
-
-	if (sixByteCmd)
-	{
-		if (dbd)
-		{
-			scsiDev.data[idx++] = 0; // No block descriptor
-		}
-		else
-		{
-			// One block descriptor of length 8 bytes.
-			scsiDev.data[idx++] = 8;
-		}
-	}
-	else
-	{
-		scsiDev.data[idx++] = 0; // Reserved
-		scsiDev.data[idx++] = 0; // Reserved
-		if (dbd)
-		{
-			scsiDev.data[idx++] = 0; // No block descriptor
-			scsiDev.data[idx++] = 0; // No block descriptor
-		}
-		else
-		{
-			// One block descriptor of length 8 bytes.
-			scsiDev.data[idx++] = 0;
-			scsiDev.data[idx++] = 8;
-		}
-	}
-
-	////////////// Block Descriptor
-	////////////////////////////////////
-	if (!dbd)
-	{
-		scsiDev.data[idx++] = density;
-		// Number of blocks
-		// Zero == all remaining blocks shall have the medium
-		// characteristics specified.
-		scsiDev.data[idx++] = 0;
-		scsiDev.data[idx++] = 0;
-		scsiDev.data[idx++] = 0;
-
-		scsiDev.data[idx++] = 0; // reserved
-
-		// Block length
-		uint32_t bytesPerSector = scsiDev.target->liveCfg.bytesPerSector;
-		scsiDev.data[idx++] = bytesPerSector >> 16;
-		scsiDev.data[idx++] = bytesPerSector >> 8;
-		scsiDev.data[idx++] = bytesPerSector & 0xFF;
-	}
-
-	int pageFound = 0;
-
-	if (pageCode == 0x01 || pageCode == 0x3F)
-	{
-		pageFound = 1;
-		if ((scsiDev.compatMode >= COMPAT_SCSI2))
-		{
-			pageIn(pc, idx, ReadWriteErrorRecoveryPage, sizeof(ReadWriteErrorRecoveryPage));
-			idx += sizeof(ReadWriteErrorRecoveryPage);
-		}
-		else
-		{
-			pageIn(pc, idx, ReadWriteErrorRecoveryPage_SCSI1, sizeof(ReadWriteErrorRecoveryPage_SCSI1));
-			idx += sizeof(ReadWriteErrorRecoveryPage_SCSI1);
-		}
-	}
-
-	if (pageCode == 0x02 || pageCode == 0x3F)
-	{
-		pageFound = 1;
-		if ((scsiDev.compatMode >= COMPAT_SCSI2))
-		{
-			pageIn(pc, idx, DisconnectReconnectPage, sizeof(DisconnectReconnectPage));
-			idx += sizeof(DisconnectReconnectPage);
-		}
-		else
-		{
-			pageIn(pc, idx, DisconnectReconnectPage_SCSI1, sizeof(DisconnectReconnectPage_SCSI1));
-			idx += sizeof(DisconnectReconnectPage_SCSI1);
-		}
-	}
-
-	if (pageCode == 0x03 || pageCode == 0x3F)
-	{
-		pageFound = 1;
-		pageIn(pc, idx, FormatDevicePage, sizeof(FormatDevicePage));
-		if (pc != 0x01)
-		{
-			uint16_t sectorsPerTrack = scsiDev.target->cfg->sectorsPerTrack;
-			scsiDev.data[idx+10] = sectorsPerTrack >> 8;
-			scsiDev.data[idx+11] = sectorsPerTrack & 0xFF;
-
-			// Fill out the configured bytes-per-sector
-			uint32_t bytesPerSector = scsiDev.target->liveCfg.bytesPerSector;
-			scsiDev.data[idx+12] = bytesPerSector >> 8;
-			scsiDev.data[idx+13] = bytesPerSector & 0xFF;
-		}
-		else
-		{
-			// Set a mask for the changeable values.
-			scsiDev.data[idx+12] = 0xFF;
-			scsiDev.data[idx+13] = 0xFF;
-		}
-
-		idx += sizeof(FormatDevicePage);
-	}
-
-	if (pageCode == 0x04 || pageCode == 0x3F)
-	{
-		pageFound = 1;
-		if ((scsiDev.compatMode >= COMPAT_SCSI2))
-		{
-			pageIn(pc, idx, RigidDiskDriveGeometry, sizeof(RigidDiskDriveGeometry));
-		}
-		else
-		{
-			pageIn(pc, idx, RigidDiskDriveGeometry_SCSI1, sizeof(RigidDiskDriveGeometry_SCSI1));
-		}
-
-		if (pc != 0x01)
-		{
-			// Need to fill out the number of cylinders.
-			uint32_t cyl;
-			uint8_t head;
-			uint32_t sector;
-			LBA2CHS(
-				getScsiCapacity(
-					scsiDev.target->cfg->sdSectorStart,
-					scsiDev.target->liveCfg.bytesPerSector,
-					scsiDev.target->cfg->scsiSectors),
-				&cyl,
-				&head,
-				&sector,
-				scsiDev.target->cfg->headsPerCylinder,
-				scsiDev.target->cfg->sectorsPerTrack);
-
-			scsiDev.data[idx+2] = cyl >> 16;
-			scsiDev.data[idx+3] = cyl >> 8;
-			scsiDev.data[idx+4] = cyl;
-
-			memcpy(&scsiDev.data[idx+6], &scsiDev.data[idx+2], 3);
-			memcpy(&scsiDev.data[idx+9], &scsiDev.data[idx+2], 3);
-
-			scsiDev.data[idx+5] = scsiDev.target->cfg->headsPerCylinder;
-		}
-
-		if ((scsiDev.compatMode >= COMPAT_SCSI2))
-		{
-			idx += sizeof(RigidDiskDriveGeometry);
-		}
-		else
-		{
-			idx += sizeof(RigidDiskDriveGeometry_SCSI1);
-		}
-	}
-
-	if ((pageCode == 0x05 || pageCode == 0x3F) &&
-		(scsiDev.target->cfg->deviceType == S2S_CFG_FLOPPY_14MB))
-	{
-		pageFound = 1;
-		pageIn(pc, idx, FlexibleDiskDriveGeometry, sizeof(FlexibleDiskDriveGeometry));
-		idx += sizeof(FlexibleDiskDriveGeometry);
-	}
-
-	// DON'T output the following pages for SCSI1 hosts. They get upset when
-	// we have more data to send than the allocation length provided.
-	// (ie. Try not to output any more pages below this comment)
-
-
-	if ((scsiDev.compatMode >= COMPAT_SCSI2) &&
-		(pageCode == 0x08 || pageCode == 0x3F))
-	{
-		pageFound = 1;
-		pageIn(pc, idx, CachingPage, sizeof(CachingPage));
-		idx += sizeof(CachingPage);
-	}
-
-	if ((scsiDev.compatMode >= COMPAT_SCSI2)
-		&& (pageCode == 0x0A || pageCode == 0x3F))
-	{
-		pageFound = 1;
-		pageIn(pc, idx, ControlModePage, sizeof(ControlModePage));
-		idx += sizeof(ControlModePage);
-	}
-
-	if ((scsiDev.target->cfg->deviceType == S2S_CFG_SEQUENTIAL) &&
-		(pageCode == 0x10 || pageCode == 0x3F))
-	{
-		pageFound = 1;
-		pageIn(
-			pc,
-			idx,
-			SequentialDeviceConfigPage,
-			sizeof(SequentialDeviceConfigPage));
-		idx += sizeof(SequentialDeviceConfigPage);
-	}
-
-	if ((
-			(scsiDev.target->cfg->quirks == S2S_CFG_QUIRKS_APPLE) ||
-			(idx + sizeof(AppleVendorPage) <= allocLength)
-		) &&
-		(pageCode == 0x30 || pageCode == 0x3F))
-	{
-		pageFound = 1;
-		pageIn(pc, idx, AppleVendorPage, sizeof(AppleVendorPage));
-		idx += sizeof(AppleVendorPage);
-	}
-
-	if (pageCode == 0x38) // Don't send unless requested
-	{
-		pageFound = 1;
-		pageIn(pc, idx, CCSCachingPage, sizeof(CCSCachingPage));
-		idx += sizeof(CCSCachingPage);
-	}
-
-	// SCSI 2 standard says page 0 is always last.
-	if (pageCode == 0x00 || pageCode == 0x3F)
-	{
-		pageFound = 1;
-		pageIn(pc, idx, OperatingPage, sizeof(OperatingPage));
-
-		// Note inverted logic for the flag.
-		scsiDev.data[idx+2] =
-			(scsiDev.boardCfg.flags & S2S_CFG_ENABLE_UNIT_ATTENTION) ? 0x80 : 0x90;
-
-		scsiDev.data[idx+3] = getDeviceTypeQualifier();
-
-		idx += sizeof(OperatingPage);
-	}
-
-	if (!pageFound)
-	{
-		// Unknown Page Code
-		pageFound = 0;
-		scsiDev.status = CHECK_CONDITION;
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
-		scsiDev.phase = STATUS;
-	}
-	else
-	{
-		// Go back and fill out the mode data length
-		if (sixByteCmd)
-		{
-			// Cannot currently exceed limits. yay
-			scsiDev.data[0] = idx - 1;
-		}
-		else
-		{
-			scsiDev.data[0] = ((idx - 2) >> 8);
-			scsiDev.data[1] = (idx - 2);
-		}
-
-		scsiDev.dataLen = idx > allocLength ? allocLength : idx;
-		scsiDev.phase = DATA_IN;
-	}
-}
-
-
-// Callback after the DATA OUT phase is complete.
-static void doModeSelect(void)
-{
-	if (scsiDev.status == GOOD) // skip if we've already encountered an error
-	{
-		// scsiDev.dataLen bytes are in scsiDev.data
-
-		int idx;
-		int blockDescLen;
-		if (scsiDev.cdb[0] == 0x55)
-		{
-			blockDescLen =
-				(((uint16_t)scsiDev.data[6]) << 8) |scsiDev.data[7];
-			idx = 8;
-		}
-		else
-		{
-			blockDescLen = scsiDev.data[3];
-			idx = 4;
-		}
-
-		// 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
-			{
-				scsiDev.target->liveCfg.bytesPerSector = bytesPerSector;
-				if (bytesPerSector != scsiDev.target->cfg->bytesPerSector)
-				{
-					s2s_configSave(scsiDev.target->targetId, bytesPerSector);
-				}
-			}
-		}
-		idx += blockDescLen;
-
-		while (idx < scsiDev.dataLen)
-		{
-			int pageLen = scsiDev.data[idx + 1];
-			if (idx + 2 + pageLen > scsiDev.dataLen) goto bad;
-
-			int pageCode = scsiDev.data[idx] & 0x3F;
-			switch (pageCode)
-			{
-			case 0x03: // Format Device Page
-			{
-				if (pageLen != 0x16) goto bad;
-
-				// Fill out the configured bytes-per-sector
-				uint16_t bytesPerSector =
-					(((uint16_t)scsiDev.data[idx+12]) << 8) |
-					scsiDev.data[idx+13];
-
-				// Sane values only, ok ?
-				if ((bytesPerSector < MIN_SECTOR_SIZE) ||
-					(bytesPerSector > MAX_SECTOR_SIZE))
-				{
-					goto bad;
-				}
-
-				scsiDev.target->liveCfg.bytesPerSector = bytesPerSector;
-				if (scsiDev.cdb[1] & 1) // SP Save Pages flag
-				{
-					s2s_configSave(scsiDev.target->targetId, bytesPerSector);
-				}
-			}
-			break;
-			//default:
-
-				// Easiest to just ignore for now. We'll get here when changing
-				// the SCSI block size via the descriptor header.
-			}
-			idx += 2 + pageLen;
-		}
-	}
-
-	goto out;
-bad:
-	scsiDev.status = CHECK_CONDITION;
-	scsiDev.target->sense.code = ILLEGAL_REQUEST;
-	scsiDev.target->sense.asc = INVALID_FIELD_IN_PARAMETER_LIST;
-
-out:
-	scsiDev.phase = STATUS;
-}
-
-int scsiModeCommand()
-{
-	int commandHandled = 1;
-
-	uint8_t command = scsiDev.cdb[0];
-
-	// We don't currently support the setting of any parameters.
-	// (ie. no MODE SELECT(6) or MODE SELECT(10) commands)
-
-	if (command == 0x1A)
-	{
-		// MODE SENSE(6)
-		int dbd = scsiDev.cdb[1] & 0x08; // Disable block descriptors
-		int pc = scsiDev.cdb[2] >> 6; // Page Control
-		int pageCode = scsiDev.cdb[2] & 0x3F;
-		int allocLength = scsiDev.cdb[4];
-
-		// SCSI1 standard: (CCS X3T9.2/86-52)
-		// "An Allocation Length of zero indicates that no MODE SENSE data shall
-		// be transferred. This condition shall not be considered as an error."
-		doModeSense(1, dbd, pc, pageCode, allocLength);
-	}
-	else if (command == 0x5A)
-	{
-		// MODE SENSE(10)
-		int dbd = scsiDev.cdb[1] & 0x08; // Disable block descriptors
-		int pc = scsiDev.cdb[2] >> 6; // Page Control
-		int pageCode = scsiDev.cdb[2] & 0x3F;
-		int allocLength =
-			(((uint16_t) scsiDev.cdb[7]) << 8) +
-			scsiDev.cdb[8];
-		doModeSense(0, dbd, pc, pageCode, allocLength);
-	}
-	else if (command == 0x15)
-	{
-		// MODE SELECT(6)
-		int len = scsiDev.cdb[4];
-		if (len == 0)
-		{
-			// If len == 0, then transfer no data. From the SCSI 2 standard:
-			//      A parameter list length of zero indicates that no data shall
-			//      be transferred. This condition shall not be considered as an
-			//		error.
-			scsiDev.phase = STATUS;
-		}
-		else
-		{
-			scsiDev.dataLen = len;
-			scsiDev.phase = DATA_OUT;
-			scsiDev.postDataOutHook = doModeSelect;
-		}
-	}
-	else if (command == 0x55)
-	{
-		// MODE SELECT(10)
-		int allocLength = (((uint16_t) scsiDev.cdb[7]) << 8) + scsiDev.cdb[8];
-		if (allocLength == 0)
-		{
-			// If len == 0, then transfer no data. From the SCSI 2 standard:
-			//      A parameter list length of zero indicates that no data shall
-			//      be transferred. This condition shall not be considered as an
-			//		error.
-			scsiDev.phase = STATUS;
-		}
-		else
-		{
-			scsiDev.dataLen = allocLength;
-			scsiDev.phase = DATA_OUT;
-			scsiDev.postDataOutHook = doModeSelect;
-		}
-	}
-	else
-	{
-		commandHandled = 0;
-	}
-
-	return commandHandled;
-}
-
+//	Copyright (C) 2013 Michael McMaster <michael@codesrc.com>
+//  Copyright (C) 2014 Doug Brown <doug@downtowndougbrown.com>
+//  Copyright (C) 2019 Landon Rodgers <g.landon.rodgers@gmail.com>
+//
+//	This file is part of SCSI2SD.
+//
+//	SCSI2SD is free software: you can redistribute it and/or modify
+//	it under the terms of the GNU General Public License as published by
+//	the Free Software Foundation, either version 3 of the License, or
+//	(at your option) any later version.
+//
+//	SCSI2SD is distributed in the hope that it will be useful,
+//	but WITHOUT ANY WARRANTY; without even the implied warranty of
+//	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//	GNU General Public License for more details.
+//
+//	You should have received a copy of the GNU General Public License
+//	along with SCSI2SD.  If not, see <http://www.gnu.org/licenses/>.
+
+#include "scsi.h"
+#include "mode.h"
+#include "disk.h"
+#include "inquiry.h"
+
+#include <string.h>
+
+// "Vendor" defined page which was included by Seagate, and required for\r
+// Amiga 500 using DKB SpitFire controller.\r
+static const uint8_t OperatingPage[] =
+{
+0x00, // Page code
+0x02, // Page length
+
+// Bit 4 = unit attension (0 = on, 1 = off).
+// Bit 7 = usage bit, EEPROM life exceeded warning = 1.
+0x80, 
+
+// Bit 7 = reserved.
+// Bits 0:6: Device type qualifier, as per Inquiry data
+0x00
+};
+
+static const uint8_t ReadWriteErrorRecoveryPage[] =
+{
+0x01, // Page code
+0x0A, // Page length
+
+// VMS 5.5-2 is very particular regarding the mode page values.
+// The required values for a SCSI2/NoTCQ device are:
+// AWRE=0 ARRE=0 TB=1 RC=0 EER=? PER=1 DTE=1 DCR=?
+// See ftp://www.digiater.nl/openvms/decus/vms94b/net94b/scsi_params_dkdriver.txt
+// X-Newsgroups: comp.os.vms
+// Subject: Re: VMS 6.1 vs. Seagate Disk Drives
+// Message-Id: <32g87h$8q@nntpd.lkg.dec.com>
+// From: weber@evms.enet.dec.com (Ralph O. Weber -- OpenVMS AXP)
+// Date: 12 Aug 1994 16:32:49 GMT
+0x26,
+
+0x00, // Don't try recovery algorithm during reads
+0x00, // Correction span 0
+0x00, // Head offset count 0,
+0x00, // Data strobe offset count 0,
+0x00, // Reserved
+0x00, // Don't try recovery algorithm during writes
+0x00, // Reserved
+0x00, 0x00 // Recovery time limit 0 (use default)*/
+};
+
+static const uint8_t ReadWriteErrorRecoveryPage_SCSI1[] =
+{
+0x01, // Page code
+0x06, // Page length
+0x26,
+0x00, // Don't try recovery algorithm during reads
+0x00, // Correction span 0
+0x00, // Head offset count 0,
+0x00, // Data strobe offset count 0,
+0xFF // Reserved
+};
+
+static const uint8_t DisconnectReconnectPage[] =
+{
+0x02, // Page code
+0x0E, // Page length
+0, // Buffer full ratio
+0, // Buffer empty ratio
+0x00, 10, // Bus inactivity limit, 100us increments. Allow 1ms.
+0x00, 0x00, // Disconnect time limit
+0x00, 0x00, // Connect time limit
+0x00, 0x00, // Maximum burst size
+0x00 ,// DTDC. Not used.
+0x00, 0x00, 0x00 // Reserved
+};
+
+static const uint8_t DisconnectReconnectPage_SCSI1[] =
+{
+0x02, // Page code
+0x0A, // Page length
+0, // Buffer full ratio
+0, // Buffer empty ratio
+0x00, 10, // Bus inactivity limit, 100us increments. Allow 1ms.
+0x00, 0x00, // Disconnect time limit
+0x00, 0x00, // Connect time limit
+0x00, 0x00 // Maximum burst size
+};
+
+static const uint8_t FormatDevicePage[] =
+{
+0x03 | 0x80, // Page code | PS (persist) bit.
+0x16, // Page length
+0x00, 0x00, // Single zone
+0x00, 0x00, // No alternate sectors
+0x00, 0x00, // No alternate tracks
+0x00, 0x00, // No alternate tracks per lun
+0x00, 0x00, // Sectors per track, configurable
+0xFF, 0xFF, // Data bytes per physical sector. Configurable.
+0x00, 0x01, // Interleave
+0x00, 0x00, // Track skew factor
+0x00, 0x00, // Cylinder skew factor
+0xC0, // SSEC(set) HSEC(set) RMB SURF
+0x00, 0x00, 0x00 // Reserved
+};
+
+static const uint8_t RigidDiskDriveGeometry[] =
+{
+0x04, // Page code
+0x16, // Page length
+0xFF, 0xFF, 0xFF, // Number of cylinders
+0x00, // Number of heads (replaced by configured value)
+0xFF, 0xFF, 0xFF, // Starting cylinder-write precompensation
+0xFF, 0xFF, 0xFF, // Starting cylinder-reduced write current
+0x00, 0x1, // Drive step rate (units of 100ns)
+0x00, 0x00, 0x00, // Landing zone cylinder
+0x00, // RPL
+0x00, // Rotational offset
+0x00, // Reserved
+5400 >> 8, 5400 & 0xFF, // Medium rotation rate (RPM)
+0x00, 0x00 // Reserved
+};
+
+static const uint8_t FlexibleDiskDriveGeometry[] =
+{
+0x05, // Page code
+0x1E, // Page length
+0x01, 0xF4, // Transfer Rate (500kbits)
+0x01, // heads
+18, // sectors per track
+0x20,0x00, // bytes per sector
+0x00, 80, // Cylinders
+0x00, 0x80, // Write-precomp
+0x00, 0x80, // reduced current,
+0x00, 0x00, // Drive step rate
+0x00, // pulse width
+0x00, 0x00, // Head settle delay
+0x00, // motor on delay
+0x00,  // motor off delay
+0x00,
+0x00,
+0x00,
+0x00,
+0x00,
+0x00,
+0x00,
+0x00,
+0x00,
+0x00,
+0x00
+};
+
+static const uint8_t RigidDiskDriveGeometry_SCSI1[] =
+{
+0x04, // Page code
+0x12, // Page length
+0xFF, 0xFF, 0xFF, // Number of cylinders
+0x00, // Number of heads (replaced by configured value)
+0xFF, 0xFF, 0xFF, // Starting cylinder-write precompensation
+0xFF, 0xFF, 0xFF, // Starting cylinder-reduced write current
+0x00, 0x1, // Drive step rate (units of 100ns)
+0x00, 0x00, 0x00, // Landing zone cylinder
+0x00, // RPL
+0x00, // Rotational offset
+0x00 // Reserved
+};
+
+static const uint8_t CachingPage[] =
+{
+0x08, // Page Code
+0x0A, // Page length
+0x01, // Read cache disable
+0x00, // No useful rention policy.
+0x00, 0x00, // Pre-fetch always disabled
+0x00, 0x00, // Minimum pre-fetch
+0x00, 0x00, // Maximum pre-fetch
+0x00, 0x00, // Maximum pre-fetch ceiling
+};
+
+// Old CCS SCSI-1 cache page
+static const uint8_t CCSCachingPage[] =
+{
+0x38, // Page Code
+0x0E, // Page length
+0x00, // Read cache disable
+0x00, // Prefetch threshold
+0x00, 0x00, // Max threshold / multiplier
+0x00, 0x00, // Min threshold / multiplier
+0x00, 0x00, // Reserved
+0x00, 0x00,
+0x00, 0x00,
+0x00, 0x00,
+};
+
+static const uint8_t ControlModePage[] =
+{
+0x0A, // Page code
+0x06, // Page length
+0x00, // No logging
+0x01, // Disable tagged queuing
+0x00, // No async event notifications
+0x00, // Reserved
+0x00, 0x00 // AEN holdoff period.
+};
+
+static const uint8_t SequentialDeviceConfigPage[] =
+{
+0x10, // page code
+0x0E, // Page length
+0x00, // CAP, CAF, Active Format
+0x00, // Active partition
+0x00, // Write buffer full ratio
+0x00, // Read buffer empty ratio
+0x00,0x01, // Write delay time, in 100ms units
+0x00, // Default gap size
+0x10, // auto-generation of default eod (end of data)
+0x00,0x00,0x00, // buffer-size at early warning
+0x00, // No data compression
+0x00 // reserved
+};
+
+// Allow Apple 68k Drive Setup to format this drive.
+// Code
+static const uint8_t AppleVendorPage[] =
+{
+0x30, // Page code
+23, // Page length
+'A','P','P','L','E',' ','C','O','M','P','U','T','E','R',',',' ','I','N','C',' ',' ',' ',0x00
+};
+
+static void pageIn(int pc, int dataIdx, const uint8_t* pageData, int pageLen)
+{
+	memcpy(&scsiDev.data[dataIdx], pageData, pageLen);
+
+	if (pc == 0x01) // Mask out (un)changable values
+	{
+		memset(&scsiDev.data[dataIdx+2], 0, pageLen - 2);
+	}
+}
+
+static void doModeSense(
+	int sixByteCmd, int dbd, int pc, int pageCode, int allocLength)
+{
+	////////////// Mode Parameter Header
+	////////////////////////////////////
+
+	// Skip the Mode Data Length, we set that last.
+	int idx = 1;
+	if (!sixByteCmd) ++idx;
+
+	uint8_t mediumType = 0;
+	uint8_t deviceSpecificParam = 0;
+	uint8_t density = 0;
+	switch (scsiDev.target->cfg->deviceType)
+	{
+	case S2S_CFG_FIXED:
+	case S2S_CFG_REMOVEABLE:
+		mediumType = 0; // We should support various floppy types here!
+		// Contains cache bits (0) and a Write-Protect bit.
+		deviceSpecificParam =
+			(blockDev.state & DISK_WP) ? 0x80 : 0;
+		density = 0; // reserved for direct access
+		break;
+
+	case S2S_CFG_FLOPPY_14MB:
+		mediumType = 0x1E; // 90mm/3.5"
+		deviceSpecificParam =
+			(blockDev.state & DISK_WP) ? 0x80 : 0;
+		density = 0; // reserved for direct access
+		break;
+
+	case S2S_CFG_OPTICAL:
+		mediumType = 0x02; // 120mm CDROM, data only.
+		deviceSpecificParam = 0;
+		density = 0x01; // User data only, 2048bytes per sector.
+		break;
+
+	case S2S_CFG_SEQUENTIAL:
+		mediumType = 0; // reserved
+		deviceSpecificParam =
+			(blockDev.state & DISK_WP) ? 0x80 : 0;
+		density = 0x13; // DAT Data Storage, X3B5/88-185A 
+		break;
+
+	case S2S_CFG_MO:
+        mediumType = 0x03; // Optical reversible or erasable medium
+		deviceSpecificParam =
+			(blockDev.state & DISK_WP) ? 0x80 : 0;
+		density = 0x00; // Default
+		break;
+
+	};
+
+	scsiDev.data[idx++] = mediumType;
+	scsiDev.data[idx++] = deviceSpecificParam;
+
+	if (sixByteCmd)
+	{
+		if (dbd)
+		{
+			scsiDev.data[idx++] = 0; // No block descriptor
+		}
+		else
+		{
+			// One block descriptor of length 8 bytes.
+			scsiDev.data[idx++] = 8;
+		}
+	}
+	else
+	{
+		scsiDev.data[idx++] = 0; // Reserved
+		scsiDev.data[idx++] = 0; // Reserved
+		if (dbd)
+		{
+			scsiDev.data[idx++] = 0; // No block descriptor
+			scsiDev.data[idx++] = 0; // No block descriptor
+		}
+		else
+		{
+			// One block descriptor of length 8 bytes.
+			scsiDev.data[idx++] = 0;
+			scsiDev.data[idx++] = 8;
+		}
+	}
+
+	////////////// Block Descriptor
+	////////////////////////////////////
+	if (!dbd)
+	{
+		scsiDev.data[idx++] = density;
+		// Number of blocks
+		// Zero == all remaining blocks shall have the medium
+		// characteristics specified.
+		scsiDev.data[idx++] = 0;
+		scsiDev.data[idx++] = 0;
+		scsiDev.data[idx++] = 0;
+
+		scsiDev.data[idx++] = 0; // reserved
+
+		// Block length
+		uint32_t bytesPerSector = scsiDev.target->liveCfg.bytesPerSector;
+		scsiDev.data[idx++] = bytesPerSector >> 16;
+		scsiDev.data[idx++] = bytesPerSector >> 8;
+		scsiDev.data[idx++] = bytesPerSector & 0xFF;
+	}
+
+	int pageFound = 0;
+
+	if (pageCode == 0x01 || pageCode == 0x3F)
+	{
+		pageFound = 1;
+		if ((scsiDev.compatMode >= COMPAT_SCSI2))
+		{
+			pageIn(pc, idx, ReadWriteErrorRecoveryPage, sizeof(ReadWriteErrorRecoveryPage));
+			idx += sizeof(ReadWriteErrorRecoveryPage);
+		}
+		else
+		{
+			pageIn(pc, idx, ReadWriteErrorRecoveryPage_SCSI1, sizeof(ReadWriteErrorRecoveryPage_SCSI1));
+			idx += sizeof(ReadWriteErrorRecoveryPage_SCSI1);
+		}
+	}
+
+	if (pageCode == 0x02 || pageCode == 0x3F)
+	{
+		pageFound = 1;
+		if ((scsiDev.compatMode >= COMPAT_SCSI2))
+		{
+			pageIn(pc, idx, DisconnectReconnectPage, sizeof(DisconnectReconnectPage));
+			idx += sizeof(DisconnectReconnectPage);
+		}
+		else
+		{
+			pageIn(pc, idx, DisconnectReconnectPage_SCSI1, sizeof(DisconnectReconnectPage_SCSI1));
+			idx += sizeof(DisconnectReconnectPage_SCSI1);
+		}
+	}
+
+	if (pageCode == 0x03 || pageCode == 0x3F)
+	{
+		pageFound = 1;
+		pageIn(pc, idx, FormatDevicePage, sizeof(FormatDevicePage));
+		if (pc != 0x01)
+		{
+			uint16_t sectorsPerTrack = scsiDev.target->cfg->sectorsPerTrack;
+			scsiDev.data[idx+10] = sectorsPerTrack >> 8;
+			scsiDev.data[idx+11] = sectorsPerTrack & 0xFF;
+
+			// Fill out the configured bytes-per-sector
+			uint32_t bytesPerSector = scsiDev.target->liveCfg.bytesPerSector;
+			scsiDev.data[idx+12] = bytesPerSector >> 8;
+			scsiDev.data[idx+13] = bytesPerSector & 0xFF;
+		}
+		else
+		{
+			// Set a mask for the changeable values.
+			scsiDev.data[idx+12] = 0xFF;
+			scsiDev.data[idx+13] = 0xFF;
+		}
+
+		idx += sizeof(FormatDevicePage);
+	}
+
+	if (pageCode == 0x04 || pageCode == 0x3F)
+	{
+		pageFound = 1;
+		if ((scsiDev.compatMode >= COMPAT_SCSI2))
+		{
+			pageIn(pc, idx, RigidDiskDriveGeometry, sizeof(RigidDiskDriveGeometry));
+		}
+		else
+		{
+			pageIn(pc, idx, RigidDiskDriveGeometry_SCSI1, sizeof(RigidDiskDriveGeometry_SCSI1));
+		}
+
+		if (pc != 0x01)
+		{
+			// Need to fill out the number of cylinders.
+			uint32_t cyl;
+			uint8_t head;
+			uint32_t sector;
+			LBA2CHS(
+				getScsiCapacity(
+					scsiDev.target->cfg->sdSectorStart,
+					scsiDev.target->liveCfg.bytesPerSector,
+					scsiDev.target->cfg->scsiSectors),
+				&cyl,
+				&head,
+				&sector,
+				scsiDev.target->cfg->headsPerCylinder,
+				scsiDev.target->cfg->sectorsPerTrack);
+
+			scsiDev.data[idx+2] = cyl >> 16;
+			scsiDev.data[idx+3] = cyl >> 8;
+			scsiDev.data[idx+4] = cyl;
+
+			memcpy(&scsiDev.data[idx+6], &scsiDev.data[idx+2], 3);
+			memcpy(&scsiDev.data[idx+9], &scsiDev.data[idx+2], 3);
+
+			scsiDev.data[idx+5] = scsiDev.target->cfg->headsPerCylinder;
+		}
+
+		if ((scsiDev.compatMode >= COMPAT_SCSI2))
+		{
+			idx += sizeof(RigidDiskDriveGeometry);
+		}
+		else
+		{
+			idx += sizeof(RigidDiskDriveGeometry_SCSI1);
+		}
+	}
+
+	if ((pageCode == 0x05 || pageCode == 0x3F) &&
+		(scsiDev.target->cfg->deviceType == S2S_CFG_FLOPPY_14MB))
+	{
+		pageFound = 1;
+		pageIn(pc, idx, FlexibleDiskDriveGeometry, sizeof(FlexibleDiskDriveGeometry));
+		idx += sizeof(FlexibleDiskDriveGeometry);
+	}
+
+	// DON'T output the following pages for SCSI1 hosts. They get upset when
+	// we have more data to send than the allocation length provided.
+	// (ie. Try not to output any more pages below this comment)
+
+
+	if ((scsiDev.compatMode >= COMPAT_SCSI2) &&
+		(pageCode == 0x08 || pageCode == 0x3F))
+	{
+		pageFound = 1;
+		pageIn(pc, idx, CachingPage, sizeof(CachingPage));
+		idx += sizeof(CachingPage);
+	}
+
+	if ((scsiDev.compatMode >= COMPAT_SCSI2)
+		&& (pageCode == 0x0A || pageCode == 0x3F))
+	{
+		pageFound = 1;
+		pageIn(pc, idx, ControlModePage, sizeof(ControlModePage));
+		idx += sizeof(ControlModePage);
+	}
+
+	if ((scsiDev.target->cfg->deviceType == S2S_CFG_SEQUENTIAL) &&
+		(pageCode == 0x10 || pageCode == 0x3F))
+	{
+		pageFound = 1;
+		pageIn(
+			pc,
+			idx,
+			SequentialDeviceConfigPage,
+			sizeof(SequentialDeviceConfigPage));
+		idx += sizeof(SequentialDeviceConfigPage);
+	}
+
+	if ((
+			(scsiDev.target->cfg->quirks == S2S_CFG_QUIRKS_APPLE) ||
+			(idx + sizeof(AppleVendorPage) <= allocLength)
+		) &&
+		(pageCode == 0x30 || pageCode == 0x3F))
+	{
+		pageFound = 1;
+		pageIn(pc, idx, AppleVendorPage, sizeof(AppleVendorPage));
+		idx += sizeof(AppleVendorPage);
+	}
+
+	if (pageCode == 0x38) // Don't send unless requested
+	{
+		pageFound = 1;
+		pageIn(pc, idx, CCSCachingPage, sizeof(CCSCachingPage));
+		idx += sizeof(CCSCachingPage);
+	}
+
+	// SCSI 2 standard says page 0 is always last.
+	if (pageCode == 0x00 || pageCode == 0x3F)
+	{
+		pageFound = 1;
+		pageIn(pc, idx, OperatingPage, sizeof(OperatingPage));
+
+		// Note inverted logic for the flag.
+		scsiDev.data[idx+2] =
+			(scsiDev.boardCfg.flags & S2S_CFG_ENABLE_UNIT_ATTENTION) ? 0x80 : 0x90;
+
+		scsiDev.data[idx+3] = getDeviceTypeQualifier();
+
+		idx += sizeof(OperatingPage);
+	}
+
+	if (!pageFound)
+	{
+		// Unknown Page Code
+		pageFound = 0;
+		scsiDev.status = CHECK_CONDITION;
+		scsiDev.target->sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+		scsiDev.phase = STATUS;
+	}
+	else
+	{
+		// Go back and fill out the mode data length
+		if (sixByteCmd)
+		{
+			// Cannot currently exceed limits. yay
+			scsiDev.data[0] = idx - 1;
+		}
+		else
+		{
+			scsiDev.data[0] = ((idx - 2) >> 8);
+			scsiDev.data[1] = (idx - 2);
+		}
+
+		scsiDev.dataLen = idx > allocLength ? allocLength : idx;
+		scsiDev.phase = DATA_IN;
+	}
+}
+
+
+// Callback after the DATA OUT phase is complete.
+static void doModeSelect(void)
+{
+	if (scsiDev.status == GOOD) // skip if we've already encountered an error
+	{
+		// scsiDev.dataLen bytes are in scsiDev.data
+
+		int idx;
+		int blockDescLen;
+		if (scsiDev.cdb[0] == 0x55)
+		{
+			blockDescLen =
+				(((uint16_t)scsiDev.data[6]) << 8) |scsiDev.data[7];
+			idx = 8;
+		}
+		else
+		{
+			blockDescLen = scsiDev.data[3];
+			idx = 4;
+		}
+
+		// 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
+			{
+				scsiDev.target->liveCfg.bytesPerSector = bytesPerSector;
+				if (bytesPerSector != scsiDev.target->cfg->bytesPerSector)
+				{
+					s2s_configSave(scsiDev.target->targetId, bytesPerSector);
+				}
+			}
+		}
+		idx += blockDescLen;
+
+		while (idx < scsiDev.dataLen)
+		{
+			int pageLen = scsiDev.data[idx + 1];
+			if (idx + 2 + pageLen > scsiDev.dataLen) goto bad;
+
+			int pageCode = scsiDev.data[idx] & 0x3F;
+			switch (pageCode)
+			{
+			case 0x03: // Format Device Page
+			{
+				if (pageLen != 0x16) goto bad;
+
+				// Fill out the configured bytes-per-sector
+				uint16_t bytesPerSector =
+					(((uint16_t)scsiDev.data[idx+12]) << 8) |
+					scsiDev.data[idx+13];
+
+				// Sane values only, ok ?
+				if ((bytesPerSector < MIN_SECTOR_SIZE) ||
+					(bytesPerSector > MAX_SECTOR_SIZE))
+				{
+					goto bad;
+				}
+
+				scsiDev.target->liveCfg.bytesPerSector = bytesPerSector;
+				if (scsiDev.cdb[1] & 1) // SP Save Pages flag
+				{
+					s2s_configSave(scsiDev.target->targetId, bytesPerSector);
+				}
+			}
+			break;
+			//default:
+
+				// Easiest to just ignore for now. We'll get here when changing
+				// the SCSI block size via the descriptor header.
+			}
+			idx += 2 + pageLen;
+		}
+	}
+
+	goto out;
+bad:
+	scsiDev.status = CHECK_CONDITION;
+	scsiDev.target->sense.code = ILLEGAL_REQUEST;
+	scsiDev.target->sense.asc = INVALID_FIELD_IN_PARAMETER_LIST;
+
+out:
+	scsiDev.phase = STATUS;
+}
+
+int scsiModeCommand()
+{
+	int commandHandled = 1;
+
+	uint8_t command = scsiDev.cdb[0];
+
+	// We don't currently support the setting of any parameters.
+	// (ie. no MODE SELECT(6) or MODE SELECT(10) commands)
+
+	if (command == 0x1A)
+	{
+		// MODE SENSE(6)
+		int dbd = scsiDev.cdb[1] & 0x08; // Disable block descriptors
+		int pc = scsiDev.cdb[2] >> 6; // Page Control
+		int pageCode = scsiDev.cdb[2] & 0x3F;
+		int allocLength = scsiDev.cdb[4];
+
+		// SCSI1 standard: (CCS X3T9.2/86-52)
+		// "An Allocation Length of zero indicates that no MODE SENSE data shall
+		// be transferred. This condition shall not be considered as an error."
+		doModeSense(1, dbd, pc, pageCode, allocLength);
+	}
+	else if (command == 0x5A)
+	{
+		// MODE SENSE(10)
+		int dbd = scsiDev.cdb[1] & 0x08; // Disable block descriptors
+		int pc = scsiDev.cdb[2] >> 6; // Page Control
+		int pageCode = scsiDev.cdb[2] & 0x3F;
+		int allocLength =
+			(((uint16_t) scsiDev.cdb[7]) << 8) +
+			scsiDev.cdb[8];
+		doModeSense(0, dbd, pc, pageCode, allocLength);
+	}
+	else if (command == 0x15)
+	{
+		// MODE SELECT(6)
+		int len = scsiDev.cdb[4];
+		if (len == 0)
+		{
+			// If len == 0, then transfer no data. From the SCSI 2 standard:
+			//      A parameter list length of zero indicates that no data shall
+			//      be transferred. This condition shall not be considered as an
+			//		error.
+			scsiDev.phase = STATUS;
+		}
+		else
+		{
+			scsiDev.dataLen = len;
+			scsiDev.phase = DATA_OUT;
+			scsiDev.postDataOutHook = doModeSelect;
+		}
+	}
+	else if (command == 0x55)
+	{
+		// MODE SELECT(10)
+		int allocLength = (((uint16_t) scsiDev.cdb[7]) << 8) + scsiDev.cdb[8];
+		if (allocLength == 0)
+		{
+			// If len == 0, then transfer no data. From the SCSI 2 standard:
+			//      A parameter list length of zero indicates that no data shall
+			//      be transferred. This condition shall not be considered as an
+			//		error.
+			scsiDev.phase = STATUS;
+		}
+		else
+		{
+			scsiDev.dataLen = allocLength;
+			scsiDev.phase = DATA_OUT;
+			scsiDev.postDataOutHook = doModeSelect;
+		}
+	}
+	else
+	{
+		commandHandled = 0;
+	}
+
+	return commandHandled;
+}
+

lib/SCSI2SD/src/firmware/scsi.c

@@ -1,1371 +1,1371 @@
-//	Copyright (C) 2014 Michael McMaster <michael@codesrc.com>
-//
-//	This file is part of SCSI2SD.
-//
-//	SCSI2SD is free software: you can redistribute it and/or modify
-//	it under the terms of the GNU General Public License as published by
-//	the Free Software Foundation, either version 3 of the License, or
-//	(at your option) any later version.
-//
-//	SCSI2SD is distributed in the hope that it will be useful,
-//	but WITHOUT ANY WARRANTY; without even the implied warranty of
-//	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-//	GNU General Public License for more details.
-//
-//	You should have received a copy of the GNU General Public License
-//	along with SCSI2SD.  If not, see <http://www.gnu.org/licenses/>.
-
-#include "scsi.h"
-#include "scsiPhy.h"
-#include "config.h"
-#include "diagnostic.h"
-#include "disk.h"
-#include "inquiry.h"
-#include "led.h"
-#include "mode.h"
-#include "scsi2sd_time.h"
-#include "bsp.h"
-#include "cdrom.h"
-//#include "debug.h"
-#include "tape.h"
-#include "mo.h"
-#include "vendor.h"
-
-#include <string.h>
-
-// Global SCSI device state.
-ScsiDevice scsiDev S2S_DMA_ALIGN;
-
-static void enter_SelectionPhase(void);
-static void process_SelectionPhase(void);
-static void enter_MessageIn(uint8_t message);
-static void enter_Status(uint8_t status);
-static void enter_DataIn(int len);
-static void process_DataIn(void);
-static void process_DataOut(void);
-static void process_Command(void);
-
-static void doReserveRelease(void);
-
-void enter_BusFree()
-{
-	// This delay probably isn't needed for most SCSI hosts, but it won't
-	// hurt either. It's possible some of the samplers needed this delay.
-	if (scsiDev.compatMode < COMPAT_SCSI2)
-	{
-		s2s_delay_us(2);
-	}
-
-#if 0
-	if (scsiDev.status != GOOD)// && isDebugEnabled())
-	{
-		// We want to capture debug information for failure cases.
-		s2s_delay_ms(80);
-	}
-#endif
-
-
-	scsiEnterBusFree();
-
-	// Wait for the initiator to cease driving signals
-	// Bus settle delay + bus clear delay = 1200ns
-    // Just waiting the clear delay is sufficient.
-	s2s_delay_ns(800);
-
-	s2s_ledOff();
-	scsiDev.phase = BUS_FREE;
-	scsiDev.selFlag = 0;
-}
-
-static void enter_MessageIn(uint8_t message)
-{
-	scsiDev.msgIn = message;
-	scsiDev.phase = MESSAGE_IN;
-}
-
-int process_MessageIn(int releaseBusFree)
-{
-	scsiEnterPhase(MESSAGE_IN);
-	scsiWriteByte(scsiDev.msgIn);
-
-	if (unlikely(scsiDev.atnFlag))
-	{
-		// If there was a parity error, we go
-		// back to MESSAGE_OUT first, get out parity error message, then come
-		// back here.
-		return 0;
-	}
-	else if ((scsiDev.msgIn == MSG_LINKED_COMMAND_COMPLETE) ||
-		(scsiDev.msgIn == MSG_LINKED_COMMAND_COMPLETE_WITH_FLAG))
-	{
-		// Go back to the command phase and start again.
-		scsiDev.phase = COMMAND;
-		scsiDev.dataPtr = 0;
-		scsiDev.savedDataPtr = 0;
-		scsiDev.dataLen = 0;
-		scsiDev.status = GOOD;
-		transfer.blocks = 0;
-		transfer.currentBlock = 0;
-		return 0;
-	}
-	else if (releaseBusFree) /*if (scsiDev.msgIn == MSG_COMMAND_COMPLETE)*/
-	{
-		enter_BusFree();
-		return 1;
-	}
-	else
-	{
-		return 1;
-	}
-}
-
-static void messageReject()
-{
-	scsiEnterPhase(MESSAGE_IN);
-	scsiWriteByte(MSG_REJECT);
-}
-
-static void enter_Status(uint8_t status)
-{
-	scsiDev.status = status;
-	scsiDev.phase = STATUS;
-
-	scsiDev.lastStatus = scsiDev.status;
-	scsiDev.lastSense = scsiDev.target->sense.code;
-	scsiDev.lastSenseASC = scsiDev.target->sense.asc;
-}
-
-void process_Status()
-{
-	scsiEnterPhase(STATUS);
-
-	uint8_t message;
-
-	uint8_t control = scsiDev.cdb[scsiDev.cdbLen - 1];
-
-	if (scsiDev.target->cfg->quirks == S2S_CFG_QUIRKS_OMTI)
-	{
-		// All commands have a control byte, except 0xC0
-		if (scsiDev.cdb[0] == 0xC0)
-		{
-			control = 0;
-		}
-
-		// OMTI non-standard LINK control
-		if (control & 0x01)
-		{
-			scsiDev.phase = COMMAND;
-			return;
-		}
-	}
-
-	if ((scsiDev.status == GOOD) && (control & 0x01) &&
-		scsiDev.target->cfg->quirks != S2S_CFG_QUIRKS_XEBEC)
-	{
-		// Linked command.
-		scsiDev.status = INTERMEDIATE;
-		if (control & 0x02)
-		{
-			message = MSG_LINKED_COMMAND_COMPLETE_WITH_FLAG;
-		}
-		else
-		{
-			message = MSG_LINKED_COMMAND_COMPLETE;
-		}
-	}
-	else
-	{
-		message = MSG_COMMAND_COMPLETE;
-	}
-
-	if (scsiDev.target->cfg->quirks == S2S_CFG_QUIRKS_XEBEC)
-	{
-		// More non-standardness. Expects 2 status bytes (really status + msg)
-		// 00 d 000 err 0
-		// d == disk number
-		// ERR = 1 if error.
-		if (scsiDev.status == GOOD)
-		{
-			scsiWriteByte(scsiDev.cdb[1] & 0x20);
-		}
-		else
-		{
-			scsiWriteByte((scsiDev.cdb[1] & 0x20) | 0x2);
-		}
-		s2s_delay_us(10); // Seems to need a delay before changing phase bits.
-	}
-	else if (scsiDev.target->cfg->quirks == S2S_CFG_QUIRKS_OMTI)
-	{
-		scsiDev.status |= (scsiDev.target->targetId & 0x03) << 5;
-		scsiWriteByte(scsiDev.status);
-	}
-	else
-	{
-		scsiWriteByte(scsiDev.status);
-	}
-
-	scsiDev.lastStatus = scsiDev.status;
-	scsiDev.lastSense = scsiDev.target->sense.code;
-	scsiDev.lastSenseASC = scsiDev.target->sense.asc;
-
-	// Command Complete occurs AFTER a valid status has been
-	// sent. then we go bus-free.
-	enter_MessageIn(message);
-}
-
-static void enter_DataIn(int len)
-{
-	scsiDev.dataLen = len;
-	scsiDev.phase = DATA_IN;
-}
-
-static void process_DataIn()
-{
-	uint32_t len;
-
-	if (scsiDev.dataLen > sizeof(scsiDev.data))
-	{
-		scsiDev.dataLen = sizeof(scsiDev.data);
-	}
-
-	len = scsiDev.dataLen - scsiDev.dataPtr;
-	if (len > 0)
-	{
-		scsiEnterPhase(DATA_IN);
-		scsiWrite(scsiDev.data + scsiDev.dataPtr, len);
-		scsiDev.dataPtr += len;
-	}
-
-	if ((scsiDev.dataPtr >= scsiDev.dataLen) &&
-		(transfer.currentBlock == transfer.blocks))
-	{
-		enter_Status(GOOD);
-	}
-}
-
-static void process_DataOut()
-{
-	uint32_t len;
-
-	if (scsiDev.dataLen > sizeof(scsiDev.data))
-	{
-		scsiDev.dataLen = sizeof(scsiDev.data);
-	}
-
-	len = scsiDev.dataLen - scsiDev.dataPtr;
-	if (len > 0)
-	{
-		scsiEnterPhase(DATA_OUT);
-
-		int parityError = 0;
-		scsiRead(scsiDev.data + scsiDev.dataPtr, len, &parityError);
-		scsiDev.dataPtr += len;
-
-		if (parityError &&
-			(scsiDev.boardCfg.flags & S2S_CFG_ENABLE_PARITY))
-		{
-			scsiDev.target->sense.code = ABORTED_COMMAND;
-			scsiDev.target->sense.asc = SCSI_PARITY_ERROR;
-			enter_Status(CHECK_CONDITION);
-		}
-	}
-
-	if ((scsiDev.dataPtr >= scsiDev.dataLen) &&
-		(transfer.currentBlock == transfer.blocks))
-	{
-		if (scsiDev.postDataOutHook != NULL)
-		{
-			scsiDev.postDataOutHook();
-		}
-		else
-		{
-			enter_Status(GOOD);
-		}
-	}
-}
-
-static const uint8_t CmdGroupBytes[] = {
-	6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
-	10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,
-	10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,
-	10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,
-	10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,
-	10,10,10,10,10,10,10,10,10,10,10,10,10,16,16,16,16,16,16,16,16,
-	16,16,16,16,16,16,16,16,16,16,16,16,16,16,16,16,16,16,16,16,16,
-	16,16,16,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,
-	12,12,12,12,12,12,12,12,12,12,12,12,12,12,10,10,10,10,10,10,10,
-	10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,
-	10,10,10,10,10,10,10,10,10,10,10,10,10,10
-};
-
-static void process_Command()
-{
-	uint8_t command;
-	uint8_t control;
-
-	scsiEnterPhase(COMMAND);
-
-	memset(scsiDev.cdb + 6, 0, sizeof(scsiDev.cdb) - 6);
-	int parityError = 0;
-	scsiRead(scsiDev.cdb, 6, &parityError);
-
-	// Handle Atari ST ICD extended commands
-	if (scsiDev.cdb[0] == 0x1F)
-	{
-		scsiDev.cdb[0] = scsiDev.cdb[1];
-		scsiDev.cdb[1] = scsiDev.cdb[2];
-		scsiDev.cdb[2] = scsiDev.cdb[3];
-		scsiDev.cdb[3] = scsiDev.cdb[4];
-		scsiDev.cdb[4] = scsiDev.cdb[5];
-		scsiDev.cdb[5] = scsiReadByte();
-	}
-
-	scsiDev.cdbLen = CmdGroupBytes[scsiDev.cdb[0]];
-	if (parityError &&
-		(scsiDev.boardCfg.flags & S2S_CFG_ENABLE_PARITY))
-	{
-		// Don't try and read more bytes, as we cannot be sure what group
-		// the command should be.
-	}
-	else if (scsiDev.cdbLen - 6 > 0)
-	{
-		scsiRead(scsiDev.cdb + 6, scsiDev.cdbLen - 6, &parityError);
-	}
-	command = scsiDev.cdb[0];
-
-	// Prefer LUN's set by IDENTIFY messages for newer hosts.
-	if (scsiDev.lun < 0)
-	{
-		if (command == 0xE0 || command == 0xE4) // XEBEC s1410
-		{
-			scsiDev.lun = 0;
-		}
-		else
-		{
-			scsiDev.lun = scsiDev.cdb[1] >> 5;
-		}
-	}
-
-
-	// For Philips P2000C with Xebec S1410 SASI/MFM adapter
-	// http://bitsavers.trailing-edge.com/pdf/xebec/104524C_S1410Man_Aug83.pdf
-	if ((scsiDev.lun > 0) && (scsiDev.boardCfg.flags & S2S_CFG_MAP_LUNS_TO_IDS))
-	{
-		int tgtIndex;
-		for (tgtIndex = 0; tgtIndex < S2S_MAX_TARGETS; ++tgtIndex)
-		{
-			if (scsiDev.targets[tgtIndex].targetId == scsiDev.lun)
-			{
-				scsiDev.target = &scsiDev.targets[tgtIndex];
-				scsiDev.lun = 0;
-				break;
-			}
-		}
-	}
-
-	control = scsiDev.cdb[scsiDev.cdbLen - 1];
-
-	scsiDev.cmdCount++;
-	const S2S_TargetCfg* cfg = scsiDev.target->cfg;
-
-	if (unlikely(scsiDev.resetFlag))
-	{
-		// Don't log bogus commands
-		scsiDev.cmdCount--;
-		memset(scsiDev.cdb, 0xff, sizeof(scsiDev.cdb));
-		return;
-	}
-	else if (parityError &&
-		(scsiDev.boardCfg.flags & S2S_CFG_ENABLE_PARITY))
-	{
-		scsiDev.target->sense.code = ABORTED_COMMAND;
-		scsiDev.target->sense.asc = SCSI_PARITY_ERROR;
-		enter_Status(CHECK_CONDITION);
-	}
-	else if ((control & 0x02) && ((control & 0x01) == 0) &&
-		// used for head step options on xebec.
-		likely(scsiDev.target->cfg->quirks != S2S_CFG_QUIRKS_XEBEC))
-	{
-		// FLAG set without LINK flag.
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
-		enter_Status(CHECK_CONDITION);
-	}
-	else if (command == 0x12)
-	{
-		s2s_scsiInquiry();
-	}
-	else if (command == 0x03)
-	{
-		// REQUEST SENSE
-		uint32_t allocLength = scsiDev.cdb[4];
-
-		if (scsiDev.target->cfg->quirks == S2S_CFG_QUIRKS_XEBEC)
-		{
-			// Completely non-standard
-			allocLength = 4;
-			if (scsiDev.target->sense.code == NO_SENSE)
-				scsiDev.data[0] = 0;
-			else if (scsiDev.target->sense.code == ILLEGAL_REQUEST)
-				scsiDev.data[0] = 0x20; // Illegal command
-			else if (scsiDev.target->sense.code == NOT_READY)
-				scsiDev.data[0] = 0x04; // Drive not ready
-			else
-				scsiDev.data[0] = 0x11;  // Uncorrectable data error
-
-			scsiDev.data[1] = (scsiDev.cdb[1] & 0x20) | ((transfer.lba >> 16) & 0x1F);
-			scsiDev.data[2] = transfer.lba >> 8;
-			scsiDev.data[3] = transfer.lba;
-		}
-		else if (cfg->quirks == S2S_CFG_QUIRKS_OMTI)
-		{
-			// The response is completely non-standard.
-			if (likely(allocLength > 12))
-			    allocLength = 12;
-			else if (unlikely(allocLength < 4))
-				allocLength = 4;
-			if (cfg->deviceType != S2S_CFG_SEQUENTIAL)
-			    allocLength = 4;
-			memset(scsiDev.data, 0, allocLength);
-			if (scsiDev.target->sense.code == NO_SENSE)
-			{
-				// Nothing to report.
-			}
-			else if (scsiDev.target->sense.code == UNIT_ATTENTION &&
-			    cfg->deviceType == S2S_CFG_SEQUENTIAL)
-			{
-				scsiDev.data[0] = 0x10; // Tape exception
-			}
-			else if (scsiDev.target->sense.code == ILLEGAL_REQUEST)
-			{
-				if (scsiDev.target->sense.asc == LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE)
-				{
-					if (cfg->deviceType == S2S_CFG_SEQUENTIAL)
-						scsiDev.data[0] = 0x10; // Tape exception
-					else
-						scsiDev.data[0] = 0x21; // Illegal Parameters
-				}
-				else if (scsiDev.target->sense.asc == INVALID_COMMAND_OPERATION_CODE)
-				{
-					scsiDev.data[0] = 0x20; // Invalid Command
-				}
-			}
-			else if (scsiDev.target->sense.code == NOT_READY)
-			{
-				scsiDev.data[0] = 0x04; // Drive not ready
-			}
-			else if (scsiDev.target->sense.code == BLANK_CHECK)
-			{
-				scsiDev.data[0] = 0x10; // Tape exception
-			}
-			else
-			{
-				scsiDev.data[0] = 0x11; // Uncorrectable data error
-			}
-			scsiDev.data[1] = (scsiDev.cdb[1] & 0x60) | ((transfer.lba >> 16) & 0x1F);
-			scsiDev.data[2] = transfer.lba >> 8;
-			scsiDev.data[3] = transfer.lba;
-			if (cfg->deviceType == S2S_CFG_SEQUENTIAL)
-			{
-				// For the tape drive there are 8 extra sense bytes.
-				if (scsiDev.target->sense.code == BLANK_CHECK)
-					scsiDev.data[11] = 0x88; // End of data recorded on the tape
-				else if (scsiDev.target->sense.code == UNIT_ATTENTION)
-					scsiDev.data[5] = 0x81; // Power On Reset occurred
-				else if (scsiDev.target->sense.code == ILLEGAL_REQUEST &&
-					 scsiDev.target->sense.asc == LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE)
-					scsiDev.data[4] = 0x81; // File Mark detected
-			}
-		}
-		else
-		{
-			// As specified by the SASI and SCSI1 standard.
-			// Newer initiators won't be specifying 0 anyway.
-			if (allocLength == 0) allocLength = 4;
-
-			memset(scsiDev.data, 0, 256); // Max possible alloc length
-			scsiDev.data[0] = 0xF0;
-			scsiDev.data[2] = scsiDev.target->sense.code & 0x0F;
-
-			scsiDev.data[3] = transfer.lba >> 24;
-			scsiDev.data[4] = transfer.lba >> 16;
-			scsiDev.data[5] = transfer.lba >> 8;
-			scsiDev.data[6] = transfer.lba;
-
-			// Additional bytes if there are errors to report
-			scsiDev.data[7] = 10; // additional length
-			scsiDev.data[12] = scsiDev.target->sense.asc >> 8;
-			scsiDev.data[13] = scsiDev.target->sense.asc;
-		}
-
-		// Silently truncate results. SCSI-2 spec 8.2.14.
-		enter_DataIn(allocLength);
-
-		// This is a good time to clear out old sense information.
-		scsiDev.target->sense.code = NO_SENSE;
-		scsiDev.target->sense.asc = NO_ADDITIONAL_SENSE_INFORMATION;
-	}
-	// Some old SCSI drivers do NOT properly support
-	// unitAttention. eg. the Mac Plus would trigger a SCSI reset
-	// on receiving the unit attention response on boot, thus
-	// triggering another unit attention condition.
-	else if (scsiDev.target->unitAttention &&
-		(scsiDev.boardCfg.flags & S2S_CFG_ENABLE_UNIT_ATTENTION))
-	{
-		scsiDev.target->sense.code = UNIT_ATTENTION;
-		scsiDev.target->sense.asc = scsiDev.target->unitAttention;
-
-		// If initiator doesn't do REQUEST SENSE for the next command, then
-		// data is lost.
-		scsiDev.target->unitAttention = 0;
-
-		enter_Status(CHECK_CONDITION);
-	}
-	else if (scsiDev.lun)
-	{
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = LOGICAL_UNIT_NOT_SUPPORTED;
-		enter_Status(CHECK_CONDITION);
-	}
-	else if (command == 0x17 || command == 0x16)
-	{
-		doReserveRelease();
-	}
-	else if ((scsiDev.target->reservedId >= 0) &&
-		(scsiDev.target->reservedId != scsiDev.initiatorId))
-	{
-		enter_Status(CONFLICT);
-	}
-	// Handle odd device types first that may override basic read and
-	// write commands. Will fall-through to generic disk handling.
-	else if (((cfg->deviceType == S2S_CFG_OPTICAL) && scsiCDRomCommand()) ||
-		((cfg->deviceType == S2S_CFG_SEQUENTIAL) && scsiTapeCommand()) ||
-		((cfg->deviceType == S2S_CFG_MO) && scsiMOCommand()))
-	{
-		// Already handled.
-	}
-	else if (scsiDiskCommand())
-	{
-		// Already handled.
-		// check for the performance-critical read/write
-		// commands ASAP.
-	}
-	else if (command == 0x1C)
-	{
-		scsiReceiveDiagnostic();
-	}
-	else if (command == 0x1D)
-	{
-		scsiSendDiagnostic();
-	}
-	else if (command == 0x3B)
-	{
-		scsiWriteBuffer();
-	}
-	else if (command == 0x0f &&
-		scsiDev.target->cfg->quirks == S2S_CFG_QUIRKS_XEBEC)
-	{
-		scsiWriteSectorBuffer();
-	}
-	else if (command == 0x3C)
-	{
-		scsiReadBuffer();
-	}
-	else if (!scsiModeCommand() && !scsiVendorCommand())
-	{
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = INVALID_COMMAND_OPERATION_CODE;
-		enter_Status(CHECK_CONDITION);
-	}
-
-	// Successful
-	if (scsiDev.phase == COMMAND) // No status set, and not in DATA_IN
-	{
-		enter_Status(GOOD);
-	}
-
-}
-
-static void doReserveRelease()
-{
-	int extentReservation = scsiDev.cdb[1] & 1;
-	int thirdPty = scsiDev.cdb[1] & 0x10;
-	int thirdPtyId = (scsiDev.cdb[1] >> 1) & 0x7;
-	uint8_t command = scsiDev.cdb[0];
-
-	int canRelease =
-		(!thirdPty && (scsiDev.initiatorId == scsiDev.target->reservedId)) ||
-			(thirdPty &&
-				(scsiDev.target->reserverId == scsiDev.initiatorId) &&
-				(scsiDev.target->reservedId == thirdPtyId)
-			);
-
-	if (extentReservation)
-	{
-		// Not supported.
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
-		enter_Status(CHECK_CONDITION);
-	}
-	else if (command == 0x17) // release
-	{
-		if ((scsiDev.target->reservedId < 0) || canRelease)
-		{
-			scsiDev.target->reservedId = -1;
-			scsiDev.target->reserverId = -1;
-		}
-		else
-		{
-			enter_Status(CONFLICT);
-		}
-	}
-	else // assume reserve.
-	{
-		if ((scsiDev.target->reservedId < 0) || canRelease)
-		{
-			scsiDev.target->reserverId = scsiDev.initiatorId;
-			if (thirdPty)
-			{
-				scsiDev.target->reservedId = thirdPtyId;
-			}
-			else
-			{
-				scsiDev.target->reservedId = scsiDev.initiatorId;
-			}
-		}
-		else
-		{
-			// Already reserved by someone else!
-			enter_Status(CONFLICT);
-		}
-	}
-}
-
-static uint32_t resetUntil = 0;
-
-static void scsiReset()
-{
-	scsiDev.rstCount++;
-	s2s_ledOff();
-
-	scsiPhyReset();
-
-	scsiDev.phase = BUS_FREE;
-	scsiDev.atnFlag = 0;
-	scsiDev.resetFlag = 0;
-	scsiDev.selFlag = 0;
-	scsiDev.lun = -1;
-	scsiDev.compatMode = COMPAT_UNKNOWN;
-
-	if (scsiDev.target)
-	{
-		if (scsiDev.target->unitAttention != POWER_ON_RESET)
-		{
-			scsiDev.target->unitAttention = SCSI_BUS_RESET;
-		}
-		scsiDev.target->reservedId = -1;
-		scsiDev.target->reserverId = -1;
-		scsiDev.target->sense.code = NO_SENSE;
-		scsiDev.target->sense.asc = NO_ADDITIONAL_SENSE_INFORMATION;
-	}
-	scsiDev.target = NULL;
-
-	for (int i = 0; i < S2S_MAX_TARGETS; ++i)
-	{
-		scsiDev.targets[i].syncOffset = 0;
-		scsiDev.targets[i].syncPeriod = 0;
-	}
-	scsiDev.minSyncPeriod = 0;
-
-	scsiDiskReset();
-
-	scsiDev.postDataOutHook = NULL;
-
-	scsiDev.sdUnderrunCount = 0;
-
-	// Sleep to allow the bus to settle down a bit.
-	// We must be ready again within the "Reset to selection time" of
-	// 250ms.
-	// There is no guarantee that the RST line will be negated by then.
-	// NOTE: We could be connected and powered by USB for configuration,
-	// in which case TERMPWR cannot be supplied, and reset will ALWAYS
-	// be true. Therefore, the sleep here must be slow to avoid slowing
-	// USB comms
-	resetUntil = s2s_getTime_ms() + 2; // At least 1ms.
-}
-
-static void enter_SelectionPhase()
-{
-	// Ignore stale versions of this flag, but ensure we know the
-	// current value if the flag is still set.
-	scsiDev.atnFlag = 0;
-	scsiDev.dataPtr = 0;
-	scsiDev.savedDataPtr = 0;
-	scsiDev.dataLen = 0;
-	scsiDev.status = GOOD;
-	scsiDev.phase = SELECTION;
-	scsiDev.lun = -1;
-	scsiDev.discPriv = 0;
-
-	scsiDev.initiatorId = -1;
-	scsiDev.target = NULL;
-
-	transfer.blocks = 0;
-	transfer.currentBlock = 0;
-
-	scsiDev.postDataOutHook = NULL;
-
-	scsiDev.needSyncNegotiationAck = 0;
-}
-
-static void process_SelectionPhase()
-{
-	// Selection delays.
-	// Many SCSI1 samplers that use a 5380 chip need a delay of at least 1ms.
-	// The Mac Plus boot-time (ie. rom code) selection abort time
-	// is < 1ms and must have no delay (standard suggests 250ms abort time)
-	// Most newer SCSI2 hosts don't care either way.
-	if (scsiDev.target->cfg->quirks == S2S_CFG_QUIRKS_XEBEC)
-	{
-		s2s_delay_ms(1); // Simply won't work if set to 0.
-	}
-	else if (scsiDev.boardCfg.selectionDelay == 255) // auto
-	{
-		if (scsiDev.compatMode < COMPAT_SCSI2)
-		{
-			s2s_delay_ms(1);
-		}
-	}
-	else if (scsiDev.boardCfg.selectionDelay != 0)
-	{
-		s2s_delay_ms(scsiDev.boardCfg.selectionDelay);
-	}
-
-	uint8_t selStatus = *SCSI_STS_SELECTED;
-	if ((selStatus == 0) && (scsiDev.boardCfg.flags & S2S_CFG_ENABLE_SEL_LATCH))
-	{
-		selStatus = scsiDev.selFlag;
-	}
-
-	int tgtIndex;
-	TargetState* target = NULL;
-	for (tgtIndex = 0; tgtIndex < S2S_MAX_TARGETS; ++tgtIndex)
-	{
-		if (scsiDev.targets[tgtIndex].targetId == (selStatus & 7))
-		{
-			target = &scsiDev.targets[tgtIndex];
-			break;
-		}
-	}
-	if ((target != NULL) && (selStatus & 0x40))
-	{
-		// We've been selected!
-		// Assert BSY - Selection success!
-		// must happen within 200us (Selection abort time) of seeing our
-		// ID + SEL.
-		// (Note: the initiator will be waiting the "Selection time-out delay"
-		// for our BSY response, which is actually a very generous 250ms)
-		*SCSI_CTRL_BSY = 1;
-		s2s_ledOn();
-
-		scsiDev.target = target;
-
-		// Do we enter MESSAGE OUT immediately ? SCSI 1 and 2 standards says
-		// move to MESSAGE OUT if ATN is true before we assert BSY.
-		// The initiator should assert ATN with SEL.
-		scsiDev.atnFlag = selStatus & 0x80;
-
-
-		// Unit attention breaks many older SCSI hosts. Disable it completely
-		// for SCSI-1 (and older) hosts, regardless of our configured setting.
-		// Enable the compatability mode also as many SASI and SCSI1
-		// controllers don't generate parity bits.
-		if (!scsiDev.atnFlag)
-		{
-			target->unitAttention = 0;
-			scsiDev.compatMode = COMPAT_SCSI1;
-		}
-		else if (!(scsiDev.boardCfg.flags & S2S_CFG_ENABLE_SCSI2))
-		{
-			scsiDev.compatMode = COMPAT_SCSI2_DISABLED;
-		}
-		else
-		{
-			scsiDev.compatMode = COMPAT_SCSI2;
-		}
-
-		scsiDev.selCount++;
-
-
-		// Save our initiator now that we're no longer in a time-critical
-		// section.
-		// SCSI1/SASI initiators may not set their own ID.
-		scsiDev.initiatorId = (selStatus >> 3) & 0x7;
-
-		// Wait until the end of the selection phase.
-		uint32_t selTimerBegin = s2s_getTime_ms();
-		while (likely(!scsiDev.resetFlag))
-		{
-			if (!scsiStatusSEL())
-			{
-				break;
-			}
-			else if (s2s_elapsedTime_ms(selTimerBegin) >= 10 &&
-				scsiDev.target->cfg->quirks == S2S_CFG_QUIRKS_XEBEC)
-			{
-				// XEBEC hosts may not bother releasing SEL at all until
-				// just before the command ends.
-				break;
-			}
-			else if (s2s_elapsedTime_ms(selTimerBegin) >= 250)
-			{
-				*SCSI_CTRL_BSY = 0;
-				scsiDev.resetFlag = 1;
-				break;
-			}
-		}
-
-		scsiDev.phase = COMMAND;
-	}
-	else if (!selStatus)
-	{
-		scsiDev.phase = BUS_BUSY;
-	}
-	scsiDev.selFlag = 0;
-}
-
-static void process_MessageOut()
-{
-	int wasNeedSyncNegotiationAck = scsiDev.needSyncNegotiationAck;
-	scsiDev.needSyncNegotiationAck = 0; // Successful on -most- messages.
-
-	scsiEnterPhase(MESSAGE_OUT);
-
-	scsiDev.atnFlag = 0;
-	scsiDev.msgOut = scsiReadByte();
-	scsiDev.msgCount++;
-
-	if (scsiParityError() &&
-		(scsiDev.boardCfg.flags & S2S_CFG_ENABLE_PARITY))
-	{
-		// Skip the remaining message bytes, and then start the MESSAGE_OUT
-		// phase again from the start. The initiator will re-send the
-		// same set of messages.
-		while (scsiStatusATN() && !scsiDev.resetFlag)
-		{
-			scsiReadByte();
-		}
-
-		// Go-back and try the message again.
-		scsiDev.atnFlag = 1;
-	}
-	else if (scsiDev.msgOut == 0x00)
-	{
-		// COMMAND COMPLETE. but why would the target be receiving this ? nfi.
-		enter_BusFree();
-	}
-	else if (scsiDev.msgOut == 0x06)
-	{
-		// ABORT
-		scsiDiskReset();
-		enter_BusFree();
-	}
-	else if (scsiDev.msgOut == 0x0C)
-	{
-		// BUS DEVICE RESET
-
-		scsiDiskReset();
-
-		scsiDev.target->unitAttention = SCSI_BUS_RESET;
-
-		// ANY initiator can reset the reservation state via this message.
-		scsiDev.target->reservedId = -1;
-		scsiDev.target->reserverId = -1;
-
-		// Cancel any sync negotiation
-		scsiDev.target->syncOffset = 0;
-		scsiDev.target->syncPeriod = 0;
-
-		enter_BusFree();
-	}
-	else if (scsiDev.msgOut == 0x05)
-	{
-		// Initiate Detected Error
-		// Ignore for now
-	}
-	else if (scsiDev.msgOut == 0x0F)
-	{
-		// INITIATE RECOVERY
-		// Ignore for now
-	}
-	else if (scsiDev.msgOut == 0x10)
-	{
-		// RELEASE RECOVERY
-		// Ignore for now
-		enter_BusFree();
-	}
-	else if (scsiDev.msgOut == MSG_REJECT)
-	{
-		// Message Reject
-		// Oh well.
-
-		if (wasNeedSyncNegotiationAck)
-		{
-			scsiDev.target->syncOffset = 0;
-			scsiDev.target->syncPeriod = 0;
-		}
-	}
-	else if (scsiDev.msgOut == 0x08)
-	{
-		// NOP
-	}
-	else if (scsiDev.msgOut == 0x09)
-	{
-		// Message Parity Error
-		// Go back and re-send the last message.
-		scsiDev.phase = MESSAGE_IN;
-
-		if (wasNeedSyncNegotiationAck)
-		{
-			scsiDev.target->syncOffset = 0;
-			scsiDev.target->syncPeriod = 0;
-		}
-	}
-	else if (scsiDev.msgOut & 0x80) // 0x80 -> 0xFF
-	{
-		// IDENTIFY
-		if ((scsiDev.msgOut & 0x18) || // Reserved bits set.
-			(scsiDev.msgOut & 0x20))  // We don't have any target routines!
-		{
-			messageReject();
-		}
-
-		scsiDev.lun = scsiDev.msgOut & 0x7;
-		scsiDev.discPriv = 
-			((scsiDev.msgOut & 0x40) && (scsiDev.initiatorId >= 0))
-				? 1 : 0;
-	}
-	else if (scsiDev.msgOut >= 0x20 && scsiDev.msgOut <= 0x2F)
-	{
-		// Two byte message. We don't support these. read and discard.
-		scsiReadByte();
-
-		if (scsiDev.msgOut == 0x23) {
-			// Ignore Wide Residue. We're only 8 bit anyway.
-		} else {
-			messageReject();
-		}
-	}
-	else if (scsiDev.msgOut == 0x01)
-	{
-		int i;
-
-		// Extended message.
-		int msgLen = scsiReadByte();
-		if (msgLen == 0) msgLen = 256;
-		uint8_t extmsg[256];
-		for (i = 0; i < msgLen && !scsiDev.resetFlag; ++i)
-		{
-			// Discard bytes.
-			extmsg[i] = scsiReadByte();
-		}
-
-		if (extmsg[0] == 3 && msgLen == 2) // Wide Data Request
-		{
-			// Negotiate down to 8bit
-			scsiEnterPhase(MESSAGE_IN);
-			static const uint8_t WDTR[] = {0x01, 0x02, 0x03, 0x00};
-			scsiWrite(WDTR, sizeof(WDTR));
-
-			// SDTR becomes invalidated.
-			scsiDev.target->syncOffset = 0;
-			scsiDev.target->syncPeriod = 0;
-		}
-		else if (extmsg[0] == 1 && msgLen == 3) // Synchronous data request
-		{
-			int oldPeriod = scsiDev.target->syncPeriod;
-			int oldOffset = scsiDev.target->syncOffset;
-
-			int transferPeriod = extmsg[1];
-			int offset = extmsg[2];
-
-			if ((
-					(transferPeriod > 0) &&
-					(transferPeriod < scsiDev.minSyncPeriod)) ||
-				(scsiDev.minSyncPeriod == 0))
-			{
-				scsiDev.minSyncPeriod = transferPeriod;
-			}
-
-			if ((transferPeriod > 80) || // 320ns, 3.125MB/s
-				// Amiga A590 (WD33C93 chip) only does 3.5MB/s sync
-				// After 80 we start to run out of bits in the fpga timing
-				// register.
-				(transferPeriod == 0) ||
-				(offset == 0) ||
-				((scsiDev.boardCfg.scsiSpeed != S2S_CFG_SPEED_NoLimit) &&
-					(scsiDev.boardCfg.scsiSpeed <= S2S_CFG_SPEED_ASYNC_50)))
-			{
-				scsiDev.target->syncOffset = 0;
-				scsiDev.target->syncPeriod = 0;
-			} else {
-				scsiDev.target->syncOffset = offset <= 15 ? offset : 15;
-				// FAST20 / 50ns / 20MHz is disabled for now due to
-				// data corruption while reading data. We can count the
-				// ACK's correctly, but can't save the data to a register
-				// before it changes. (ie. transferPeriod == 12)
-				if ((scsiDev.boardCfg.scsiSpeed == S2S_CFG_SPEED_TURBO) &&
-					(transferPeriod <= 16))
-				{
-					scsiDev.target->syncPeriod = 16; // 15.6MB/s
-				}
-				else if (scsiDev.boardCfg.scsiSpeed == S2S_CFG_SPEED_TURBO)
-				{
-					scsiDev.target->syncPeriod = transferPeriod;
-				}
-				else if (transferPeriod <= 25 &&
-					((scsiDev.boardCfg.scsiSpeed == S2S_CFG_SPEED_NoLimit) ||
-						(scsiDev.boardCfg.scsiSpeed >= S2S_CFG_SPEED_SYNC_10)))
-				{
-					scsiDev.target->syncPeriod = 25; // 100ns, 10MB/s
-
-				} else if (transferPeriod < 50 &&
-					((scsiDev.boardCfg.scsiSpeed == S2S_CFG_SPEED_NoLimit) ||
-						(scsiDev.boardCfg.scsiSpeed >= S2S_CFG_SPEED_SYNC_10)))
-				{
-					scsiDev.target->syncPeriod = transferPeriod;
-				} else if (transferPeriod >= 50)
-				{
-					scsiDev.target->syncPeriod = transferPeriod;
-				} else {
-					scsiDev.target->syncPeriod = 50;
-				}
-			}
-
-			if (transferPeriod != oldPeriod ||
-				scsiDev.target->syncPeriod != oldPeriod ||
-				offset != oldOffset ||
-				scsiDev.target->syncOffset != oldOffset ||
-				!wasNeedSyncNegotiationAck) // Don't get into infinite loops negotiating.
-			{
-				scsiEnterPhase(MESSAGE_IN);
-				uint8_t SDTR[] = {0x01, 0x03, 0x01, scsiDev.target->syncPeriod, scsiDev.target->syncOffset};
-				scsiWrite(SDTR, sizeof(SDTR));
-				scsiDev.needSyncNegotiationAck = 1; // Check if this message is rejected.
-				scsiDev.sdUnderrunCount = 0;  // reset counter, may work now.
-
-				// Set to the theoretical speed, then adjust if we measure lower
-				// actual speeds.
-				scsiDev.hostSpeedKBs = s2s_getScsiRateKBs();
-				scsiDev.hostSpeedMeasured = 0;
-			}
-		}
-		else
-		{
-			// Not supported
-			messageReject();
-		}
-	}
-	else
-	{
-		messageReject();
-	}
-
-	// Re-check the ATN flag in case it stays asserted.
-	scsiDev.atnFlag |= scsiStatusATN();
-
-	if (!scsiDev.atnFlag)
-	{
-		// Message wasn't rejected!
-		scsiDev.needSyncNegotiationAck = 0;
-	}
-}
-
-void scsiPoll(void)
-{
-	if (resetUntil != 0 && resetUntil > s2s_getTime_ms())
-	{
-		return;
-	}
-	resetUntil = 0;
-
-	if (unlikely(scsiDev.resetFlag))
-	{
-		scsiReset();
-		// Still in reset phase for a few ms.
-		// Do not try and process any commands.
-		return;
-	}
-
-	switch (scsiDev.phase)
-	{
-	case BUS_FREE:
-		if (scsiStatusBSY())
-		{
-			scsiDev.phase = BUS_BUSY;
-		}
-		// The Arbitration phase is optional for SCSI1/SASI hosts if there is only
-		// one initiator in the chain. Support this by moving
-		// straight to selection if SEL is asserted.
-		// ie. the initiator won't assert BSY and it's own ID before moving to selection.
-		else if (scsiDev.selFlag || *SCSI_STS_SELECTED)
-		{
-			enter_SelectionPhase();
-		}
-	break;
-
-	case BUS_BUSY:
-		// Someone is using the bus. Perhaps they are trying to
-		// select us.
-		if (scsiDev.selFlag || *SCSI_STS_SELECTED)
-		{
-			enter_SelectionPhase();
-		}
-		else if (!scsiStatusBSY())
-		{
-			scsiDev.phase = BUS_FREE;
-		}
-	break;
-
-	case ARBITRATION:
-		// TODO Support reselection.
-		break;
-
-	case SELECTION:
-		process_SelectionPhase();
-	break;
-
-	case RESELECTION:
-		// Not currently supported!
-	break;
-
-	case COMMAND:
-		// Do not check ATN here. SCSI 1 & 2 initiators must set ATN
-		// and SEL together upon entering the selection phase if they
-		// want to send a message (IDENTIFY) immediately.
-		if (scsiDev.atnFlag)
-		{
-			process_MessageOut();
-		}
-		else
-		{
-			process_Command();
-		}
-	break;
-
-	case DATA_IN:
-		scsiDev.atnFlag |= scsiStatusATN();
-		if (scsiDev.atnFlag)
-		{
-			process_MessageOut();
-		}
-		else
-		{
-			process_DataIn();
-		}
-	break;
-
-	case DATA_OUT:
-		scsiDev.atnFlag |= scsiStatusATN();
-		if (scsiDev.atnFlag)
-		{
-			process_MessageOut();
-		}
-		else
-		{
-			process_DataOut();
-		}
-	break;
-
-	case STATUS:
-		scsiDev.atnFlag |= scsiStatusATN();
-		if (scsiDev.atnFlag)
-		{
-			process_MessageOut();
-		}
-		else
-		{
-			process_Status();
-		}
-	break;
-
-	case MESSAGE_IN:
-		scsiDev.atnFlag |= scsiStatusATN();
-		if (scsiDev.atnFlag)
-		{
-			process_MessageOut();
-		}
-		else
-		{
-			process_MessageIn(1);
-		}
-
-	break;
-
-	case MESSAGE_OUT:
-		process_MessageOut();
-	break;
-	}
-}
-
-void scsiInit()
-{
-	static int firstInit = 1;
-
-	scsiDev.atnFlag = 0;
-	scsiDev.resetFlag = 1;
-	scsiDev.selFlag = 0;
-	scsiDev.phase = BUS_FREE;
-	scsiDev.target = NULL;
-	scsiDev.compatMode = COMPAT_UNKNOWN;
-	scsiDev.hostSpeedKBs = 0;
-	scsiDev.hostSpeedMeasured = 0;
-
-	int i;
-	for (i = 0; i < S2S_MAX_TARGETS; ++i)
-	{
-		const S2S_TargetCfg* cfg = s2s_getConfigByIndex(i);
-		if (cfg && (cfg->scsiId & S2S_CFG_TARGET_ENABLED))
-		{
-			scsiDev.targets[i].targetId = cfg->scsiId & S2S_CFG_TARGET_ID_BITS;
-			scsiDev.targets[i].cfg = cfg;
-
-			scsiDev.targets[i].liveCfg.bytesPerSector = cfg->bytesPerSector;
-		}
-		else
-		{
-			scsiDev.targets[i].targetId = 0xff;
-			scsiDev.targets[i].cfg = NULL;
-		}
-		scsiDev.targets[i].reservedId = -1;
-		scsiDev.targets[i].reserverId = -1;
-		if (firstInit)
-		{
-			scsiDev.targets[i].unitAttention = POWER_ON_RESET;
-		}
-		else
-		{
-			scsiDev.targets[i].unitAttention = PARAMETERS_CHANGED;
-		}
-		scsiDev.targets[i].sense.code = NO_SENSE;
-		scsiDev.targets[i].sense.asc = NO_ADDITIONAL_SENSE_INFORMATION;
-
-		scsiDev.targets[i].syncOffset = 0;
-		scsiDev.targets[i].syncPeriod = 0;
-
-		// Always "start" the device. Many systems (eg. Apple System 7)
-		// won't respond properly to
-		// LOGICAL_UNIT_NOT_READY_INITIALIZING_COMMAND_REQUIRED sense
-		// code
-		scsiDev.targets[i].started = 1;
-	}
-	firstInit = 0;
-}
-
-/* TODO REENABLE
-void scsiDisconnect()
-{
-	scsiEnterPhase(MESSAGE_IN);
-	scsiWriteByte(0x02); // save data pointer
-	scsiWriteByte(0x04); // disconnect msg.
-
-	// For now, the caller is responsible for tracking the disconnected
-	// state, and calling scsiReconnect.
-	// Ideally the client would exit their loop and we'd implement this
-	// as part of scsiPoll
-	int phase = scsiDev.phase;
-	enter_BusFree();
-	scsiDev.phase = phase;
-}
-*/
-
-/* TODO REENABLE
-int scsiReconnect()
-{
-	int reconnected = 0;
-
-	int sel = SCSI_ReadFilt(SCSI_Filt_SEL);
-	int bsy = SCSI_ReadFilt(SCSI_Filt_BSY);
-	if (!sel && !bsy)
-	{
-		s2s_delay_us(1);
-		sel = SCSI_ReadFilt(SCSI_Filt_SEL);
-		bsy = SCSI_ReadFilt(SCSI_Filt_BSY);
-	}
-
-	if (!sel && !bsy)
-	{
-		// Arbitrate.
-		s2s_ledOn();
-		uint8_t scsiIdMask = 1 << scsiDev.target->targetId;
-		SCSI_Out_Bits_Write(scsiIdMask);
-		SCSI_Out_Ctl_Write(1); // Write bits manually.
-		SCSI_SetPin(SCSI_Out_BSY);
-
-		s2s_delay_us(3); // arbitrate delay. 2.4us.
-
-		uint8_t dbx = scsiReadDBxPins();
-		sel = SCSI_ReadFilt(SCSI_Filt_SEL);
-		if (sel || ((dbx ^ scsiIdMask) > scsiIdMask))
-		{
-			// Lost arbitration.
-			SCSI_Out_Ctl_Write(0);
-			SCSI_ClearPin(SCSI_Out_BSY);
-			s2s_ledOff();
-		}
-		else
-		{
-			// Won arbitration
-			SCSI_SetPin(SCSI_Out_SEL);
-			s2s_delay_us(1); // Bus clear + Bus settle.
-
-			// Reselection phase
-			SCSI_CTL_PHASE_Write(__scsiphase_io);
-			SCSI_Out_Bits_Write(scsiIdMask | (1 << scsiDev.initiatorId));
-			scsiDeskewDelay(); // 2 deskew delays
-			scsiDeskewDelay(); // 2 deskew delays
-			SCSI_ClearPin(SCSI_Out_BSY);
-			s2s_delay_us(1);  // Bus Settle Delay
-
-			uint32_t waitStart_ms = getTime_ms();
-			bsy = SCSI_ReadFilt(SCSI_Filt_BSY);
-			// Wait for initiator.
-			while (
-				!bsy &&
-				!scsiDev.resetFlag &&
-				(elapsedTime_ms(waitStart_ms) < 250))
-			{
-				bsy = SCSI_ReadFilt(SCSI_Filt_BSY);
-			}
-
-			if (bsy)
-			{
-				SCSI_SetPin(SCSI_Out_BSY);
-				scsiDeskewDelay(); // 2 deskew delays
-				scsiDeskewDelay(); // 2 deskew delays
-				SCSI_ClearPin(SCSI_Out_SEL);
-
-				// Prepare for the initial IDENTIFY message.
-				SCSI_Out_Ctl_Write(0);
-				scsiEnterPhase(MESSAGE_IN);
-
-				// Send identify command
-				scsiWriteByte(0x80);
-
-				scsiEnterPhase(scsiDev.phase);
-				reconnected = 1;
-			}
-			else
-			{
-				// reselect timeout.
-				SCSI_Out_Ctl_Write(0);
-				SCSI_ClearPin(SCSI_Out_SEL);
-				SCSI_CTL_PHASE_Write(0);
-				s2s_ledOff();
-			}
-		}
-	}
-	return reconnected;
-}
-*/
-
+//	Copyright (C) 2014 Michael McMaster <michael@codesrc.com>
+//
+//	This file is part of SCSI2SD.
+//
+//	SCSI2SD is free software: you can redistribute it and/or modify
+//	it under the terms of the GNU General Public License as published by
+//	the Free Software Foundation, either version 3 of the License, or
+//	(at your option) any later version.
+//
+//	SCSI2SD is distributed in the hope that it will be useful,
+//	but WITHOUT ANY WARRANTY; without even the implied warranty of
+//	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//	GNU General Public License for more details.
+//
+//	You should have received a copy of the GNU General Public License
+//	along with SCSI2SD.  If not, see <http://www.gnu.org/licenses/>.
+
+#include "scsi.h"
+#include "scsiPhy.h"
+#include "config.h"
+#include "diagnostic.h"
+#include "disk.h"
+#include "inquiry.h"
+#include "led.h"
+#include "mode.h"
+#include "scsi2sd_time.h"
+#include "bsp.h"
+#include "cdrom.h"
+//#include "debug.h"
+#include "tape.h"
+#include "mo.h"
+#include "vendor.h"
+
+#include <string.h>
+
+// Global SCSI device state.
+ScsiDevice scsiDev S2S_DMA_ALIGN;
+
+static void enter_SelectionPhase(void);
+static void process_SelectionPhase(void);
+static void enter_MessageIn(uint8_t message);
+static void enter_Status(uint8_t status);
+static void enter_DataIn(int len);
+static void process_DataIn(void);
+static void process_DataOut(void);
+static void process_Command(void);
+
+static void doReserveRelease(void);
+
+void enter_BusFree()
+{
+	// This delay probably isn't needed for most SCSI hosts, but it won't
+	// hurt either. It's possible some of the samplers needed this delay.
+	if (scsiDev.compatMode < COMPAT_SCSI2)
+	{
+		s2s_delay_us(2);
+	}
+
+#if 0
+	if (scsiDev.status != GOOD)// && isDebugEnabled())
+	{
+		// We want to capture debug information for failure cases.
+		s2s_delay_ms(80);
+	}
+#endif
+
+
+	scsiEnterBusFree();
+
+	// Wait for the initiator to cease driving signals
+	// Bus settle delay + bus clear delay = 1200ns
+    // Just waiting the clear delay is sufficient.
+	s2s_delay_ns(800);
+
+	s2s_ledOff();
+	scsiDev.phase = BUS_FREE;
+	scsiDev.selFlag = 0;
+}
+
+static void enter_MessageIn(uint8_t message)
+{
+	scsiDev.msgIn = message;
+	scsiDev.phase = MESSAGE_IN;
+}
+
+int process_MessageIn(int releaseBusFree)
+{
+	scsiEnterPhase(MESSAGE_IN);
+	scsiWriteByte(scsiDev.msgIn);
+
+	if (unlikely(scsiDev.atnFlag))
+	{
+		// If there was a parity error, we go
+		// back to MESSAGE_OUT first, get out parity error message, then come
+		// back here.
+		return 0;
+	}
+	else if ((scsiDev.msgIn == MSG_LINKED_COMMAND_COMPLETE) ||
+		(scsiDev.msgIn == MSG_LINKED_COMMAND_COMPLETE_WITH_FLAG))
+	{
+		// Go back to the command phase and start again.
+		scsiDev.phase = COMMAND;
+		scsiDev.dataPtr = 0;
+		scsiDev.savedDataPtr = 0;
+		scsiDev.dataLen = 0;
+		scsiDev.status = GOOD;
+		transfer.blocks = 0;
+		transfer.currentBlock = 0;
+		return 0;
+	}
+	else if (releaseBusFree) /*if (scsiDev.msgIn == MSG_COMMAND_COMPLETE)*/
+	{
+		enter_BusFree();
+		return 1;
+	}
+	else
+	{
+		return 1;
+	}
+}
+
+static void messageReject()
+{
+	scsiEnterPhase(MESSAGE_IN);
+	scsiWriteByte(MSG_REJECT);
+}
+
+static void enter_Status(uint8_t status)
+{
+	scsiDev.status = status;
+	scsiDev.phase = STATUS;
+
+	scsiDev.lastStatus = scsiDev.status;
+	scsiDev.lastSense = scsiDev.target->sense.code;
+	scsiDev.lastSenseASC = scsiDev.target->sense.asc;
+}
+
+void process_Status()
+{
+	scsiEnterPhase(STATUS);
+
+	uint8_t message;
+
+	uint8_t control = scsiDev.cdb[scsiDev.cdbLen - 1];
+
+	if (scsiDev.target->cfg->quirks == S2S_CFG_QUIRKS_OMTI)
+	{
+		// All commands have a control byte, except 0xC0
+		if (scsiDev.cdb[0] == 0xC0)
+		{
+			control = 0;
+		}
+
+		// OMTI non-standard LINK control
+		if (control & 0x01)
+		{
+			scsiDev.phase = COMMAND;
+			return;
+		}
+	}
+
+	if ((scsiDev.status == GOOD) && (control & 0x01) &&
+		scsiDev.target->cfg->quirks != S2S_CFG_QUIRKS_XEBEC)
+	{
+		// Linked command.
+		scsiDev.status = INTERMEDIATE;
+		if (control & 0x02)
+		{
+			message = MSG_LINKED_COMMAND_COMPLETE_WITH_FLAG;
+		}
+		else
+		{
+			message = MSG_LINKED_COMMAND_COMPLETE;
+		}
+	}
+	else
+	{
+		message = MSG_COMMAND_COMPLETE;
+	}
+
+	if (scsiDev.target->cfg->quirks == S2S_CFG_QUIRKS_XEBEC)
+	{
+		// More non-standardness. Expects 2 status bytes (really status + msg)
+		// 00 d 000 err 0
+		// d == disk number
+		// ERR = 1 if error.
+		if (scsiDev.status == GOOD)
+		{
+			scsiWriteByte(scsiDev.cdb[1] & 0x20);
+		}
+		else
+		{
+			scsiWriteByte((scsiDev.cdb[1] & 0x20) | 0x2);
+		}
+		s2s_delay_us(10); // Seems to need a delay before changing phase bits.
+	}
+	else if (scsiDev.target->cfg->quirks == S2S_CFG_QUIRKS_OMTI)
+	{
+		scsiDev.status |= (scsiDev.target->targetId & 0x03) << 5;
+		scsiWriteByte(scsiDev.status);
+	}
+	else
+	{
+		scsiWriteByte(scsiDev.status);
+	}
+
+	scsiDev.lastStatus = scsiDev.status;
+	scsiDev.lastSense = scsiDev.target->sense.code;
+	scsiDev.lastSenseASC = scsiDev.target->sense.asc;
+
+	// Command Complete occurs AFTER a valid status has been
+	// sent. then we go bus-free.
+	enter_MessageIn(message);
+}
+
+static void enter_DataIn(int len)
+{
+	scsiDev.dataLen = len;
+	scsiDev.phase = DATA_IN;
+}
+
+static void process_DataIn()
+{
+	uint32_t len;
+
+	if (scsiDev.dataLen > sizeof(scsiDev.data))
+	{
+		scsiDev.dataLen = sizeof(scsiDev.data);
+	}
+
+	len = scsiDev.dataLen - scsiDev.dataPtr;
+	if (len > 0)
+	{
+		scsiEnterPhase(DATA_IN);
+		scsiWrite(scsiDev.data + scsiDev.dataPtr, len);
+		scsiDev.dataPtr += len;
+	}
+
+	if ((scsiDev.dataPtr >= scsiDev.dataLen) &&
+		(transfer.currentBlock == transfer.blocks))
+	{
+		enter_Status(GOOD);
+	}
+}
+
+static void process_DataOut()
+{
+	uint32_t len;
+
+	if (scsiDev.dataLen > sizeof(scsiDev.data))
+	{
+		scsiDev.dataLen = sizeof(scsiDev.data);
+	}
+
+	len = scsiDev.dataLen - scsiDev.dataPtr;
+	if (len > 0)
+	{
+		scsiEnterPhase(DATA_OUT);
+
+		int parityError = 0;
+		scsiRead(scsiDev.data + scsiDev.dataPtr, len, &parityError);
+		scsiDev.dataPtr += len;
+
+		if (parityError &&
+			(scsiDev.boardCfg.flags & S2S_CFG_ENABLE_PARITY))
+		{
+			scsiDev.target->sense.code = ABORTED_COMMAND;
+			scsiDev.target->sense.asc = SCSI_PARITY_ERROR;
+			enter_Status(CHECK_CONDITION);
+		}
+	}
+
+	if ((scsiDev.dataPtr >= scsiDev.dataLen) &&
+		(transfer.currentBlock == transfer.blocks))
+	{
+		if (scsiDev.postDataOutHook != NULL)
+		{
+			scsiDev.postDataOutHook();
+		}
+		else
+		{
+			enter_Status(GOOD);
+		}
+	}
+}
+
+static const uint8_t CmdGroupBytes[] = {
+	6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
+	10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,
+	10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,
+	10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,
+	10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,
+	10,10,10,10,10,10,10,10,10,10,10,10,10,16,16,16,16,16,16,16,16,
+	16,16,16,16,16,16,16,16,16,16,16,16,16,16,16,16,16,16,16,16,16,
+	16,16,16,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,
+	12,12,12,12,12,12,12,12,12,12,12,12,12,12,10,10,10,10,10,10,10,
+	10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,
+	10,10,10,10,10,10,10,10,10,10,10,10,10,10
+};
+
+static void process_Command()
+{
+	uint8_t command;
+	uint8_t control;
+
+	scsiEnterPhase(COMMAND);
+
+	memset(scsiDev.cdb + 6, 0, sizeof(scsiDev.cdb) - 6);
+	int parityError = 0;
+	scsiRead(scsiDev.cdb, 6, &parityError);
+
+	// Handle Atari ST ICD extended commands
+	if (scsiDev.cdb[0] == 0x1F)
+	{
+		scsiDev.cdb[0] = scsiDev.cdb[1];
+		scsiDev.cdb[1] = scsiDev.cdb[2];
+		scsiDev.cdb[2] = scsiDev.cdb[3];
+		scsiDev.cdb[3] = scsiDev.cdb[4];
+		scsiDev.cdb[4] = scsiDev.cdb[5];
+		scsiDev.cdb[5] = scsiReadByte();
+	}
+
+	scsiDev.cdbLen = CmdGroupBytes[scsiDev.cdb[0]];
+	if (parityError &&
+		(scsiDev.boardCfg.flags & S2S_CFG_ENABLE_PARITY))
+	{
+		// Don't try and read more bytes, as we cannot be sure what group
+		// the command should be.
+	}
+	else if (scsiDev.cdbLen - 6 > 0)
+	{
+		scsiRead(scsiDev.cdb + 6, scsiDev.cdbLen - 6, &parityError);
+	}
+	command = scsiDev.cdb[0];
+
+	// Prefer LUN's set by IDENTIFY messages for newer hosts.
+	if (scsiDev.lun < 0)
+	{
+		if (command == 0xE0 || command == 0xE4) // XEBEC s1410
+		{
+			scsiDev.lun = 0;
+		}
+		else
+		{
+			scsiDev.lun = scsiDev.cdb[1] >> 5;
+		}
+	}
+
+
+	// For Philips P2000C with Xebec S1410 SASI/MFM adapter
+	// http://bitsavers.trailing-edge.com/pdf/xebec/104524C_S1410Man_Aug83.pdf
+	if ((scsiDev.lun > 0) && (scsiDev.boardCfg.flags & S2S_CFG_MAP_LUNS_TO_IDS))
+	{
+		int tgtIndex;
+		for (tgtIndex = 0; tgtIndex < S2S_MAX_TARGETS; ++tgtIndex)
+		{
+			if (scsiDev.targets[tgtIndex].targetId == scsiDev.lun)
+			{
+				scsiDev.target = &scsiDev.targets[tgtIndex];
+				scsiDev.lun = 0;
+				break;
+			}
+		}
+	}
+
+	control = scsiDev.cdb[scsiDev.cdbLen - 1];
+
+	scsiDev.cmdCount++;
+	const S2S_TargetCfg* cfg = scsiDev.target->cfg;
+
+	if (unlikely(scsiDev.resetFlag))
+	{
+		// Don't log bogus commands
+		scsiDev.cmdCount--;
+		memset(scsiDev.cdb, 0xff, sizeof(scsiDev.cdb));
+		return;
+	}
+	else if (parityError &&
+		(scsiDev.boardCfg.flags & S2S_CFG_ENABLE_PARITY))
+	{
+		scsiDev.target->sense.code = ABORTED_COMMAND;
+		scsiDev.target->sense.asc = SCSI_PARITY_ERROR;
+		enter_Status(CHECK_CONDITION);
+	}
+	else if ((control & 0x02) && ((control & 0x01) == 0) &&
+		// used for head step options on xebec.
+		likely(scsiDev.target->cfg->quirks != S2S_CFG_QUIRKS_XEBEC))
+	{
+		// FLAG set without LINK flag.
+		scsiDev.target->sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+		enter_Status(CHECK_CONDITION);
+	}
+	else if (command == 0x12)
+	{
+		s2s_scsiInquiry();
+	}
+	else if (command == 0x03)
+	{
+		// REQUEST SENSE
+		uint32_t allocLength = scsiDev.cdb[4];
+
+		if (scsiDev.target->cfg->quirks == S2S_CFG_QUIRKS_XEBEC)
+		{
+			// Completely non-standard
+			allocLength = 4;
+			if (scsiDev.target->sense.code == NO_SENSE)
+				scsiDev.data[0] = 0;
+			else if (scsiDev.target->sense.code == ILLEGAL_REQUEST)
+				scsiDev.data[0] = 0x20; // Illegal command
+			else if (scsiDev.target->sense.code == NOT_READY)
+				scsiDev.data[0] = 0x04; // Drive not ready
+			else
+				scsiDev.data[0] = 0x11;  // Uncorrectable data error
+
+			scsiDev.data[1] = (scsiDev.cdb[1] & 0x20) | ((transfer.lba >> 16) & 0x1F);
+			scsiDev.data[2] = transfer.lba >> 8;
+			scsiDev.data[3] = transfer.lba;
+		}
+		else if (cfg->quirks == S2S_CFG_QUIRKS_OMTI)
+		{
+			// The response is completely non-standard.
+			if (likely(allocLength > 12))
+			    allocLength = 12;
+			else if (unlikely(allocLength < 4))
+				allocLength = 4;
+			if (cfg->deviceType != S2S_CFG_SEQUENTIAL)
+			    allocLength = 4;
+			memset(scsiDev.data, 0, allocLength);
+			if (scsiDev.target->sense.code == NO_SENSE)
+			{
+				// Nothing to report.
+			}
+			else if (scsiDev.target->sense.code == UNIT_ATTENTION &&
+			    cfg->deviceType == S2S_CFG_SEQUENTIAL)
+			{
+				scsiDev.data[0] = 0x10; // Tape exception
+			}
+			else if (scsiDev.target->sense.code == ILLEGAL_REQUEST)
+			{
+				if (scsiDev.target->sense.asc == LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE)
+				{
+					if (cfg->deviceType == S2S_CFG_SEQUENTIAL)
+						scsiDev.data[0] = 0x10; // Tape exception
+					else
+						scsiDev.data[0] = 0x21; // Illegal Parameters
+				}
+				else if (scsiDev.target->sense.asc == INVALID_COMMAND_OPERATION_CODE)
+				{
+					scsiDev.data[0] = 0x20; // Invalid Command
+				}
+			}
+			else if (scsiDev.target->sense.code == NOT_READY)
+			{
+				scsiDev.data[0] = 0x04; // Drive not ready
+			}
+			else if (scsiDev.target->sense.code == BLANK_CHECK)
+			{
+				scsiDev.data[0] = 0x10; // Tape exception
+			}
+			else
+			{
+				scsiDev.data[0] = 0x11; // Uncorrectable data error
+			}
+			scsiDev.data[1] = (scsiDev.cdb[1] & 0x60) | ((transfer.lba >> 16) & 0x1F);
+			scsiDev.data[2] = transfer.lba >> 8;
+			scsiDev.data[3] = transfer.lba;
+			if (cfg->deviceType == S2S_CFG_SEQUENTIAL)
+			{
+				// For the tape drive there are 8 extra sense bytes.
+				if (scsiDev.target->sense.code == BLANK_CHECK)
+					scsiDev.data[11] = 0x88; // End of data recorded on the tape
+				else if (scsiDev.target->sense.code == UNIT_ATTENTION)
+					scsiDev.data[5] = 0x81; // Power On Reset occurred
+				else if (scsiDev.target->sense.code == ILLEGAL_REQUEST &&
+					 scsiDev.target->sense.asc == LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE)
+					scsiDev.data[4] = 0x81; // File Mark detected
+			}
+		}
+		else
+		{
+			// As specified by the SASI and SCSI1 standard.
+			// Newer initiators won't be specifying 0 anyway.
+			if (allocLength == 0) allocLength = 4;
+
+			memset(scsiDev.data, 0, 256); // Max possible alloc length
+			scsiDev.data[0] = 0xF0;
+			scsiDev.data[2] = scsiDev.target->sense.code & 0x0F;
+
+			scsiDev.data[3] = transfer.lba >> 24;
+			scsiDev.data[4] = transfer.lba >> 16;
+			scsiDev.data[5] = transfer.lba >> 8;
+			scsiDev.data[6] = transfer.lba;
+
+			// Additional bytes if there are errors to report
+			scsiDev.data[7] = 10; // additional length
+			scsiDev.data[12] = scsiDev.target->sense.asc >> 8;
+			scsiDev.data[13] = scsiDev.target->sense.asc;
+		}
+
+		// Silently truncate results. SCSI-2 spec 8.2.14.
+		enter_DataIn(allocLength);
+
+		// This is a good time to clear out old sense information.
+		scsiDev.target->sense.code = NO_SENSE;
+		scsiDev.target->sense.asc = NO_ADDITIONAL_SENSE_INFORMATION;
+	}
+	// Some old SCSI drivers do NOT properly support
+	// unitAttention. eg. the Mac Plus would trigger a SCSI reset
+	// on receiving the unit attention response on boot, thus
+	// triggering another unit attention condition.
+	else if (scsiDev.target->unitAttention &&
+		(scsiDev.boardCfg.flags & S2S_CFG_ENABLE_UNIT_ATTENTION))
+	{
+		scsiDev.target->sense.code = UNIT_ATTENTION;
+		scsiDev.target->sense.asc = scsiDev.target->unitAttention;
+
+		// If initiator doesn't do REQUEST SENSE for the next command, then
+		// data is lost.
+		scsiDev.target->unitAttention = 0;
+
+		enter_Status(CHECK_CONDITION);
+	}
+	else if (scsiDev.lun)
+	{
+		scsiDev.target->sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->sense.asc = LOGICAL_UNIT_NOT_SUPPORTED;
+		enter_Status(CHECK_CONDITION);
+	}
+	else if (command == 0x17 || command == 0x16)
+	{
+		doReserveRelease();
+	}
+	else if ((scsiDev.target->reservedId >= 0) &&
+		(scsiDev.target->reservedId != scsiDev.initiatorId))
+	{
+		enter_Status(CONFLICT);
+	}
+	// Handle odd device types first that may override basic read and
+	// write commands. Will fall-through to generic disk handling.
+	else if (((cfg->deviceType == S2S_CFG_OPTICAL) && scsiCDRomCommand()) ||
+		((cfg->deviceType == S2S_CFG_SEQUENTIAL) && scsiTapeCommand()) ||
+		((cfg->deviceType == S2S_CFG_MO) && scsiMOCommand()))
+	{
+		// Already handled.
+	}
+	else if (scsiDiskCommand())
+	{
+		// Already handled.
+		// check for the performance-critical read/write
+		// commands ASAP.
+	}
+	else if (command == 0x1C)
+	{
+		scsiReceiveDiagnostic();
+	}
+	else if (command == 0x1D)
+	{
+		scsiSendDiagnostic();
+	}
+	else if (command == 0x3B)
+	{
+		scsiWriteBuffer();
+	}
+	else if (command == 0x0f &&
+		scsiDev.target->cfg->quirks == S2S_CFG_QUIRKS_XEBEC)
+	{
+		scsiWriteSectorBuffer();
+	}
+	else if (command == 0x3C)
+	{
+		scsiReadBuffer();
+	}
+	else if (!scsiModeCommand() && !scsiVendorCommand())
+	{
+		scsiDev.target->sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->sense.asc = INVALID_COMMAND_OPERATION_CODE;
+		enter_Status(CHECK_CONDITION);
+	}
+
+	// Successful
+	if (scsiDev.phase == COMMAND) // No status set, and not in DATA_IN
+	{
+		enter_Status(GOOD);
+	}
+
+}
+
+static void doReserveRelease()
+{
+	int extentReservation = scsiDev.cdb[1] & 1;
+	int thirdPty = scsiDev.cdb[1] & 0x10;
+	int thirdPtyId = (scsiDev.cdb[1] >> 1) & 0x7;
+	uint8_t command = scsiDev.cdb[0];
+
+	int canRelease =
+		(!thirdPty && (scsiDev.initiatorId == scsiDev.target->reservedId)) ||
+			(thirdPty &&
+				(scsiDev.target->reserverId == scsiDev.initiatorId) &&
+				(scsiDev.target->reservedId == thirdPtyId)
+			);
+
+	if (extentReservation)
+	{
+		// Not supported.
+		scsiDev.target->sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+		enter_Status(CHECK_CONDITION);
+	}
+	else if (command == 0x17) // release
+	{
+		if ((scsiDev.target->reservedId < 0) || canRelease)
+		{
+			scsiDev.target->reservedId = -1;
+			scsiDev.target->reserverId = -1;
+		}
+		else
+		{
+			enter_Status(CONFLICT);
+		}
+	}
+	else // assume reserve.
+	{
+		if ((scsiDev.target->reservedId < 0) || canRelease)
+		{
+			scsiDev.target->reserverId = scsiDev.initiatorId;
+			if (thirdPty)
+			{
+				scsiDev.target->reservedId = thirdPtyId;
+			}
+			else
+			{
+				scsiDev.target->reservedId = scsiDev.initiatorId;
+			}
+		}
+		else
+		{
+			// Already reserved by someone else!
+			enter_Status(CONFLICT);
+		}
+	}
+}
+
+static uint32_t resetUntil = 0;
+
+static void scsiReset()
+{
+	scsiDev.rstCount++;
+	s2s_ledOff();
+
+	scsiPhyReset();
+
+	scsiDev.phase = BUS_FREE;
+	scsiDev.atnFlag = 0;
+	scsiDev.resetFlag = 0;
+	scsiDev.selFlag = 0;
+	scsiDev.lun = -1;
+	scsiDev.compatMode = COMPAT_UNKNOWN;
+
+	if (scsiDev.target)
+	{
+		if (scsiDev.target->unitAttention != POWER_ON_RESET)
+		{
+			scsiDev.target->unitAttention = SCSI_BUS_RESET;
+		}
+		scsiDev.target->reservedId = -1;
+		scsiDev.target->reserverId = -1;
+		scsiDev.target->sense.code = NO_SENSE;
+		scsiDev.target->sense.asc = NO_ADDITIONAL_SENSE_INFORMATION;
+	}
+	scsiDev.target = NULL;
+
+	for (int i = 0; i < S2S_MAX_TARGETS; ++i)
+	{
+		scsiDev.targets[i].syncOffset = 0;
+		scsiDev.targets[i].syncPeriod = 0;
+	}
+	scsiDev.minSyncPeriod = 0;
+
+	scsiDiskReset();
+
+	scsiDev.postDataOutHook = NULL;
+
+	scsiDev.sdUnderrunCount = 0;
+
+	// Sleep to allow the bus to settle down a bit.
+	// We must be ready again within the "Reset to selection time" of
+	// 250ms.
+	// There is no guarantee that the RST line will be negated by then.
+	// NOTE: We could be connected and powered by USB for configuration,
+	// in which case TERMPWR cannot be supplied, and reset will ALWAYS
+	// be true. Therefore, the sleep here must be slow to avoid slowing
+	// USB comms
+	resetUntil = s2s_getTime_ms() + 2; // At least 1ms.
+}
+
+static void enter_SelectionPhase()
+{
+	// Ignore stale versions of this flag, but ensure we know the
+	// current value if the flag is still set.
+	scsiDev.atnFlag = 0;
+	scsiDev.dataPtr = 0;
+	scsiDev.savedDataPtr = 0;
+	scsiDev.dataLen = 0;
+	scsiDev.status = GOOD;
+	scsiDev.phase = SELECTION;
+	scsiDev.lun = -1;
+	scsiDev.discPriv = 0;
+
+	scsiDev.initiatorId = -1;
+	scsiDev.target = NULL;
+
+	transfer.blocks = 0;
+	transfer.currentBlock = 0;
+
+	scsiDev.postDataOutHook = NULL;
+
+	scsiDev.needSyncNegotiationAck = 0;
+}
+
+static void process_SelectionPhase()
+{
+	// Selection delays.
+	// Many SCSI1 samplers that use a 5380 chip need a delay of at least 1ms.
+	// The Mac Plus boot-time (ie. rom code) selection abort time
+	// is < 1ms and must have no delay (standard suggests 250ms abort time)
+	// Most newer SCSI2 hosts don't care either way.
+	if (scsiDev.target->cfg->quirks == S2S_CFG_QUIRKS_XEBEC)
+	{
+		s2s_delay_ms(1); // Simply won't work if set to 0.
+	}
+	else if (scsiDev.boardCfg.selectionDelay == 255) // auto
+	{
+		if (scsiDev.compatMode < COMPAT_SCSI2)
+		{
+			s2s_delay_ms(1);
+		}
+	}
+	else if (scsiDev.boardCfg.selectionDelay != 0)
+	{
+		s2s_delay_ms(scsiDev.boardCfg.selectionDelay);
+	}
+
+	uint8_t selStatus = *SCSI_STS_SELECTED;
+	if ((selStatus == 0) && (scsiDev.boardCfg.flags & S2S_CFG_ENABLE_SEL_LATCH))
+	{
+		selStatus = scsiDev.selFlag;
+	}
+
+	int tgtIndex;
+	TargetState* target = NULL;
+	for (tgtIndex = 0; tgtIndex < S2S_MAX_TARGETS; ++tgtIndex)
+	{
+		if (scsiDev.targets[tgtIndex].targetId == (selStatus & 7))
+		{
+			target = &scsiDev.targets[tgtIndex];
+			break;
+		}
+	}
+	if ((target != NULL) && (selStatus & 0x40))
+	{
+		// We've been selected!
+		// Assert BSY - Selection success!
+		// must happen within 200us (Selection abort time) of seeing our
+		// ID + SEL.
+		// (Note: the initiator will be waiting the "Selection time-out delay"
+		// for our BSY response, which is actually a very generous 250ms)
+		*SCSI_CTRL_BSY = 1;
+		s2s_ledOn();
+
+		scsiDev.target = target;
+
+		// Do we enter MESSAGE OUT immediately ? SCSI 1 and 2 standards says
+		// move to MESSAGE OUT if ATN is true before we assert BSY.
+		// The initiator should assert ATN with SEL.
+		scsiDev.atnFlag = selStatus & 0x80;
+
+
+		// Unit attention breaks many older SCSI hosts. Disable it completely
+		// for SCSI-1 (and older) hosts, regardless of our configured setting.
+		// Enable the compatability mode also as many SASI and SCSI1
+		// controllers don't generate parity bits.
+		if (!scsiDev.atnFlag)
+		{
+			target->unitAttention = 0;
+			scsiDev.compatMode = COMPAT_SCSI1;
+		}
+		else if (!(scsiDev.boardCfg.flags & S2S_CFG_ENABLE_SCSI2))
+		{
+			scsiDev.compatMode = COMPAT_SCSI2_DISABLED;
+		}
+		else
+		{
+			scsiDev.compatMode = COMPAT_SCSI2;
+		}
+
+		scsiDev.selCount++;
+
+
+		// Save our initiator now that we're no longer in a time-critical
+		// section.
+		// SCSI1/SASI initiators may not set their own ID.
+		scsiDev.initiatorId = (selStatus >> 3) & 0x7;
+
+		// Wait until the end of the selection phase.
+		uint32_t selTimerBegin = s2s_getTime_ms();
+		while (likely(!scsiDev.resetFlag))
+		{
+			if (!scsiStatusSEL())
+			{
+				break;
+			}
+			else if (s2s_elapsedTime_ms(selTimerBegin) >= 10 &&
+				scsiDev.target->cfg->quirks == S2S_CFG_QUIRKS_XEBEC)
+			{
+				// XEBEC hosts may not bother releasing SEL at all until
+				// just before the command ends.
+				break;
+			}
+			else if (s2s_elapsedTime_ms(selTimerBegin) >= 250)
+			{
+				*SCSI_CTRL_BSY = 0;
+				scsiDev.resetFlag = 1;
+				break;
+			}
+		}
+
+		scsiDev.phase = COMMAND;
+	}
+	else if (!selStatus)
+	{
+		scsiDev.phase = BUS_BUSY;
+	}
+	scsiDev.selFlag = 0;
+}
+
+static void process_MessageOut()
+{
+	int wasNeedSyncNegotiationAck = scsiDev.needSyncNegotiationAck;
+	scsiDev.needSyncNegotiationAck = 0; // Successful on -most- messages.
+
+	scsiEnterPhase(MESSAGE_OUT);
+
+	scsiDev.atnFlag = 0;
+	scsiDev.msgOut = scsiReadByte();
+	scsiDev.msgCount++;
+
+	if (scsiParityError() &&
+		(scsiDev.boardCfg.flags & S2S_CFG_ENABLE_PARITY))
+	{
+		// Skip the remaining message bytes, and then start the MESSAGE_OUT
+		// phase again from the start. The initiator will re-send the
+		// same set of messages.
+		while (scsiStatusATN() && !scsiDev.resetFlag)
+		{
+			scsiReadByte();
+		}
+
+		// Go-back and try the message again.
+		scsiDev.atnFlag = 1;
+	}
+	else if (scsiDev.msgOut == 0x00)
+	{
+		// COMMAND COMPLETE. but why would the target be receiving this ? nfi.
+		enter_BusFree();
+	}
+	else if (scsiDev.msgOut == 0x06)
+	{
+		// ABORT
+		scsiDiskReset();
+		enter_BusFree();
+	}
+	else if (scsiDev.msgOut == 0x0C)
+	{
+		// BUS DEVICE RESET
+
+		scsiDiskReset();
+
+		scsiDev.target->unitAttention = SCSI_BUS_RESET;
+
+		// ANY initiator can reset the reservation state via this message.
+		scsiDev.target->reservedId = -1;
+		scsiDev.target->reserverId = -1;
+
+		// Cancel any sync negotiation
+		scsiDev.target->syncOffset = 0;
+		scsiDev.target->syncPeriod = 0;
+
+		enter_BusFree();
+	}
+	else if (scsiDev.msgOut == 0x05)
+	{
+		// Initiate Detected Error
+		// Ignore for now
+	}
+	else if (scsiDev.msgOut == 0x0F)
+	{
+		// INITIATE RECOVERY
+		// Ignore for now
+	}
+	else if (scsiDev.msgOut == 0x10)
+	{
+		// RELEASE RECOVERY
+		// Ignore for now
+		enter_BusFree();
+	}
+	else if (scsiDev.msgOut == MSG_REJECT)
+	{
+		// Message Reject
+		// Oh well.
+
+		if (wasNeedSyncNegotiationAck)
+		{
+			scsiDev.target->syncOffset = 0;
+			scsiDev.target->syncPeriod = 0;
+		}
+	}
+	else if (scsiDev.msgOut == 0x08)
+	{
+		// NOP
+	}
+	else if (scsiDev.msgOut == 0x09)
+	{
+		// Message Parity Error
+		// Go back and re-send the last message.
+		scsiDev.phase = MESSAGE_IN;
+
+		if (wasNeedSyncNegotiationAck)
+		{
+			scsiDev.target->syncOffset = 0;
+			scsiDev.target->syncPeriod = 0;
+		}
+	}
+	else if (scsiDev.msgOut & 0x80) // 0x80 -> 0xFF
+	{
+		// IDENTIFY
+		if ((scsiDev.msgOut & 0x18) || // Reserved bits set.
+			(scsiDev.msgOut & 0x20))  // We don't have any target routines!
+		{
+			messageReject();
+		}
+
+		scsiDev.lun = scsiDev.msgOut & 0x7;
+		scsiDev.discPriv = 
+			((scsiDev.msgOut & 0x40) && (scsiDev.initiatorId >= 0))
+				? 1 : 0;
+	}
+	else if (scsiDev.msgOut >= 0x20 && scsiDev.msgOut <= 0x2F)
+	{
+		// Two byte message. We don't support these. read and discard.
+		scsiReadByte();
+
+		if (scsiDev.msgOut == 0x23) {
+			// Ignore Wide Residue. We're only 8 bit anyway.
+		} else {
+			messageReject();
+		}
+	}
+	else if (scsiDev.msgOut == 0x01)
+	{
+		int i;
+
+		// Extended message.
+		int msgLen = scsiReadByte();
+		if (msgLen == 0) msgLen = 256;
+		uint8_t extmsg[256];
+		for (i = 0; i < msgLen && !scsiDev.resetFlag; ++i)
+		{
+			// Discard bytes.
+			extmsg[i] = scsiReadByte();
+		}
+
+		if (extmsg[0] == 3 && msgLen == 2) // Wide Data Request
+		{
+			// Negotiate down to 8bit
+			scsiEnterPhase(MESSAGE_IN);
+			static const uint8_t WDTR[] = {0x01, 0x02, 0x03, 0x00};
+			scsiWrite(WDTR, sizeof(WDTR));
+
+			// SDTR becomes invalidated.
+			scsiDev.target->syncOffset = 0;
+			scsiDev.target->syncPeriod = 0;
+		}
+		else if (extmsg[0] == 1 && msgLen == 3) // Synchronous data request
+		{
+			int oldPeriod = scsiDev.target->syncPeriod;
+			int oldOffset = scsiDev.target->syncOffset;
+
+			int transferPeriod = extmsg[1];
+			int offset = extmsg[2];
+
+			if ((
+					(transferPeriod > 0) &&
+					(transferPeriod < scsiDev.minSyncPeriod)) ||
+				(scsiDev.minSyncPeriod == 0))
+			{
+				scsiDev.minSyncPeriod = transferPeriod;
+			}
+
+			if ((transferPeriod > 80) || // 320ns, 3.125MB/s
+				// Amiga A590 (WD33C93 chip) only does 3.5MB/s sync
+				// After 80 we start to run out of bits in the fpga timing
+				// register.
+				(transferPeriod == 0) ||
+				(offset == 0) ||
+				((scsiDev.boardCfg.scsiSpeed != S2S_CFG_SPEED_NoLimit) &&
+					(scsiDev.boardCfg.scsiSpeed <= S2S_CFG_SPEED_ASYNC_50)))
+			{
+				scsiDev.target->syncOffset = 0;
+				scsiDev.target->syncPeriod = 0;
+			} else {
+				scsiDev.target->syncOffset = offset <= 15 ? offset : 15;
+				// FAST20 / 50ns / 20MHz is disabled for now due to
+				// data corruption while reading data. We can count the
+				// ACK's correctly, but can't save the data to a register
+				// before it changes. (ie. transferPeriod == 12)
+				if ((scsiDev.boardCfg.scsiSpeed == S2S_CFG_SPEED_TURBO) &&
+					(transferPeriod <= 16))
+				{
+					scsiDev.target->syncPeriod = 16; // 15.6MB/s
+				}
+				else if (scsiDev.boardCfg.scsiSpeed == S2S_CFG_SPEED_TURBO)
+				{
+					scsiDev.target->syncPeriod = transferPeriod;
+				}
+				else if (transferPeriod <= 25 &&
+					((scsiDev.boardCfg.scsiSpeed == S2S_CFG_SPEED_NoLimit) ||
+						(scsiDev.boardCfg.scsiSpeed >= S2S_CFG_SPEED_SYNC_10)))
+				{
+					scsiDev.target->syncPeriod = 25; // 100ns, 10MB/s
+
+				} else if (transferPeriod < 50 &&
+					((scsiDev.boardCfg.scsiSpeed == S2S_CFG_SPEED_NoLimit) ||
+						(scsiDev.boardCfg.scsiSpeed >= S2S_CFG_SPEED_SYNC_10)))
+				{
+					scsiDev.target->syncPeriod = transferPeriod;
+				} else if (transferPeriod >= 50)
+				{
+					scsiDev.target->syncPeriod = transferPeriod;
+				} else {
+					scsiDev.target->syncPeriod = 50;
+				}
+			}
+
+			if (transferPeriod != oldPeriod ||
+				scsiDev.target->syncPeriod != oldPeriod ||
+				offset != oldOffset ||
+				scsiDev.target->syncOffset != oldOffset ||
+				!wasNeedSyncNegotiationAck) // Don't get into infinite loops negotiating.
+			{
+				scsiEnterPhase(MESSAGE_IN);
+				uint8_t SDTR[] = {0x01, 0x03, 0x01, scsiDev.target->syncPeriod, scsiDev.target->syncOffset};
+				scsiWrite(SDTR, sizeof(SDTR));
+				scsiDev.needSyncNegotiationAck = 1; // Check if this message is rejected.
+				scsiDev.sdUnderrunCount = 0;  // reset counter, may work now.
+
+				// Set to the theoretical speed, then adjust if we measure lower
+				// actual speeds.
+				scsiDev.hostSpeedKBs = s2s_getScsiRateKBs();
+				scsiDev.hostSpeedMeasured = 0;
+			}
+		}
+		else
+		{
+			// Not supported
+			messageReject();
+		}
+	}
+	else
+	{
+		messageReject();
+	}
+
+	// Re-check the ATN flag in case it stays asserted.
+	scsiDev.atnFlag |= scsiStatusATN();
+
+	if (!scsiDev.atnFlag)
+	{
+		// Message wasn't rejected!
+		scsiDev.needSyncNegotiationAck = 0;
+	}
+}
+
+void scsiPoll(void)
+{
+	if (resetUntil != 0 && resetUntil > s2s_getTime_ms())
+	{
+		return;
+	}
+	resetUntil = 0;
+
+	if (unlikely(scsiDev.resetFlag))
+	{
+		scsiReset();
+		// Still in reset phase for a few ms.
+		// Do not try and process any commands.
+		return;
+	}
+
+	switch (scsiDev.phase)
+	{
+	case BUS_FREE:
+		if (scsiStatusBSY())
+		{
+			scsiDev.phase = BUS_BUSY;
+		}
+		// The Arbitration phase is optional for SCSI1/SASI hosts if there is only
+		// one initiator in the chain. Support this by moving
+		// straight to selection if SEL is asserted.
+		// ie. the initiator won't assert BSY and it's own ID before moving to selection.
+		else if (scsiDev.selFlag || *SCSI_STS_SELECTED)
+		{
+			enter_SelectionPhase();
+		}
+	break;
+
+	case BUS_BUSY:
+		// Someone is using the bus. Perhaps they are trying to
+		// select us.
+		if (scsiDev.selFlag || *SCSI_STS_SELECTED)
+		{
+			enter_SelectionPhase();
+		}
+		else if (!scsiStatusBSY())
+		{
+			scsiDev.phase = BUS_FREE;
+		}
+	break;
+
+	case ARBITRATION:
+		// TODO Support reselection.
+		break;
+
+	case SELECTION:
+		process_SelectionPhase();
+	break;
+
+	case RESELECTION:
+		// Not currently supported!
+	break;
+
+	case COMMAND:
+		// Do not check ATN here. SCSI 1 & 2 initiators must set ATN
+		// and SEL together upon entering the selection phase if they
+		// want to send a message (IDENTIFY) immediately.
+		if (scsiDev.atnFlag)
+		{
+			process_MessageOut();
+		}
+		else
+		{
+			process_Command();
+		}
+	break;
+
+	case DATA_IN:
+		scsiDev.atnFlag |= scsiStatusATN();
+		if (scsiDev.atnFlag)
+		{
+			process_MessageOut();
+		}
+		else
+		{
+			process_DataIn();
+		}
+	break;
+
+	case DATA_OUT:
+		scsiDev.atnFlag |= scsiStatusATN();
+		if (scsiDev.atnFlag)
+		{
+			process_MessageOut();
+		}
+		else
+		{
+			process_DataOut();
+		}
+	break;
+
+	case STATUS:
+		scsiDev.atnFlag |= scsiStatusATN();
+		if (scsiDev.atnFlag)
+		{
+			process_MessageOut();
+		}
+		else
+		{
+			process_Status();
+		}
+	break;
+
+	case MESSAGE_IN:
+		scsiDev.atnFlag |= scsiStatusATN();
+		if (scsiDev.atnFlag)
+		{
+			process_MessageOut();
+		}
+		else
+		{
+			process_MessageIn(1);
+		}
+
+	break;
+
+	case MESSAGE_OUT:
+		process_MessageOut();
+	break;
+	}
+}
+
+void scsiInit()
+{
+	static int firstInit = 1;
+
+	scsiDev.atnFlag = 0;
+	scsiDev.resetFlag = 1;
+	scsiDev.selFlag = 0;
+	scsiDev.phase = BUS_FREE;
+	scsiDev.target = NULL;
+	scsiDev.compatMode = COMPAT_UNKNOWN;
+	scsiDev.hostSpeedKBs = 0;
+	scsiDev.hostSpeedMeasured = 0;
+
+	int i;
+	for (i = 0; i < S2S_MAX_TARGETS; ++i)
+	{
+		const S2S_TargetCfg* cfg = s2s_getConfigByIndex(i);
+		if (cfg && (cfg->scsiId & S2S_CFG_TARGET_ENABLED))
+		{
+			scsiDev.targets[i].targetId = cfg->scsiId & S2S_CFG_TARGET_ID_BITS;
+			scsiDev.targets[i].cfg = cfg;
+
+			scsiDev.targets[i].liveCfg.bytesPerSector = cfg->bytesPerSector;
+		}
+		else
+		{
+			scsiDev.targets[i].targetId = 0xff;
+			scsiDev.targets[i].cfg = NULL;
+		}
+		scsiDev.targets[i].reservedId = -1;
+		scsiDev.targets[i].reserverId = -1;
+		if (firstInit)
+		{
+			scsiDev.targets[i].unitAttention = POWER_ON_RESET;
+		}
+		else
+		{
+			scsiDev.targets[i].unitAttention = PARAMETERS_CHANGED;
+		}
+		scsiDev.targets[i].sense.code = NO_SENSE;
+		scsiDev.targets[i].sense.asc = NO_ADDITIONAL_SENSE_INFORMATION;
+
+		scsiDev.targets[i].syncOffset = 0;
+		scsiDev.targets[i].syncPeriod = 0;
+
+		// Always "start" the device. Many systems (eg. Apple System 7)
+		// won't respond properly to
+		// LOGICAL_UNIT_NOT_READY_INITIALIZING_COMMAND_REQUIRED sense
+		// code
+		scsiDev.targets[i].started = 1;
+	}
+	firstInit = 0;
+}
+
+/* TODO REENABLE
+void scsiDisconnect()
+{
+	scsiEnterPhase(MESSAGE_IN);
+	scsiWriteByte(0x02); // save data pointer
+	scsiWriteByte(0x04); // disconnect msg.
+
+	// For now, the caller is responsible for tracking the disconnected
+	// state, and calling scsiReconnect.
+	// Ideally the client would exit their loop and we'd implement this
+	// as part of scsiPoll
+	int phase = scsiDev.phase;
+	enter_BusFree();
+	scsiDev.phase = phase;
+}
+*/
+
+/* TODO REENABLE
+int scsiReconnect()
+{
+	int reconnected = 0;
+
+	int sel = SCSI_ReadFilt(SCSI_Filt_SEL);
+	int bsy = SCSI_ReadFilt(SCSI_Filt_BSY);
+	if (!sel && !bsy)
+	{
+		s2s_delay_us(1);
+		sel = SCSI_ReadFilt(SCSI_Filt_SEL);
+		bsy = SCSI_ReadFilt(SCSI_Filt_BSY);
+	}
+
+	if (!sel && !bsy)
+	{
+		// Arbitrate.
+		s2s_ledOn();
+		uint8_t scsiIdMask = 1 << scsiDev.target->targetId;
+		SCSI_Out_Bits_Write(scsiIdMask);
+		SCSI_Out_Ctl_Write(1); // Write bits manually.
+		SCSI_SetPin(SCSI_Out_BSY);
+
+		s2s_delay_us(3); // arbitrate delay. 2.4us.
+
+		uint8_t dbx = scsiReadDBxPins();
+		sel = SCSI_ReadFilt(SCSI_Filt_SEL);
+		if (sel || ((dbx ^ scsiIdMask) > scsiIdMask))
+		{
+			// Lost arbitration.
+			SCSI_Out_Ctl_Write(0);
+			SCSI_ClearPin(SCSI_Out_BSY);
+			s2s_ledOff();
+		}
+		else
+		{
+			// Won arbitration
+			SCSI_SetPin(SCSI_Out_SEL);
+			s2s_delay_us(1); // Bus clear + Bus settle.
+
+			// Reselection phase
+			SCSI_CTL_PHASE_Write(__scsiphase_io);
+			SCSI_Out_Bits_Write(scsiIdMask | (1 << scsiDev.initiatorId));
+			scsiDeskewDelay(); // 2 deskew delays
+			scsiDeskewDelay(); // 2 deskew delays
+			SCSI_ClearPin(SCSI_Out_BSY);
+			s2s_delay_us(1);  // Bus Settle Delay
+
+			uint32_t waitStart_ms = getTime_ms();
+			bsy = SCSI_ReadFilt(SCSI_Filt_BSY);
+			// Wait for initiator.
+			while (
+				!bsy &&
+				!scsiDev.resetFlag &&
+				(elapsedTime_ms(waitStart_ms) < 250))
+			{
+				bsy = SCSI_ReadFilt(SCSI_Filt_BSY);
+			}
+
+			if (bsy)
+			{
+				SCSI_SetPin(SCSI_Out_BSY);
+				scsiDeskewDelay(); // 2 deskew delays
+				scsiDeskewDelay(); // 2 deskew delays
+				SCSI_ClearPin(SCSI_Out_SEL);
+
+				// Prepare for the initial IDENTIFY message.
+				SCSI_Out_Ctl_Write(0);
+				scsiEnterPhase(MESSAGE_IN);
+
+				// Send identify command
+				scsiWriteByte(0x80);
+
+				scsiEnterPhase(scsiDev.phase);
+				reconnected = 1;
+			}
+			else
+			{
+				// reselect timeout.
+				SCSI_Out_Ctl_Write(0);
+				SCSI_ClearPin(SCSI_Out_SEL);
+				SCSI_CTL_PHASE_Write(0);
+				s2s_ledOff();
+			}
+		}
+	}
+	return reconnected;
+}
+*/
+