Moved all hard disk commands to disk.cpp for testing

src/raspberrypi/controllers/sasidev_ctrl.cpp

@@ -714,7 +714,7 @@ void SASIDEV::CmdTestUnitReady()
 	LOGTRACE("%s TEST UNIT READY Command ", __PRETTY_FUNCTION__);
 
 	// Command processing on drive
-	bool status = ctrl.device->TestUnitReady(ctrl.cmd);
+	bool status = ((Disk *)ctrl.device)->TestUnitReady(ctrl.cmd);
 	if (!status) {
 		// Failure (Error)
 		Error();
@@ -965,7 +965,6 @@ void SASIDEV::DaynaPortWrite()
 	DataOut();
 }
 
-
 //---------------------------------------------------------------------------
 //
 //	WRITE(6)

src/raspberrypi/controllers/sasidev_ctrl.h

@@ -183,6 +183,8 @@ public:
 	void MsgIn();							// Message in phase
 	void DataOut();						// Data out phase
 
+	void DaynaPortWrite();					// DaynaPort specific 'write' operation
+
 	virtual void Error(ERROR_CODES::sense_key sense_key = ERROR_CODES::sense_key::NO_SENSE,
 			ERROR_CODES::asc = ERROR_CODES::asc::NO_ADDITIONAL_SENSE_INFORMATION);	// Common error handling
 
@@ -207,7 +209,6 @@ protected:
 	void CmdAssign();						// ASSIGN command
 	void CmdSpecify();						// SPECIFY command
 	void CmdInvalid();						// Unsupported command
-	void DaynaPortWrite();					// DaynaPort specific 'write' operation
 	// データ転送
 	virtual void Send();						// Send data
 

src/raspberrypi/controllers/scsidev_ctrl.cpp

@@ -42,34 +42,34 @@ SCSIDEV::SCSIDEV() : SASIDEV()
 	scsi.msc = 0;
 	memset(scsi.msb, 0x00, sizeof(scsi.msb));
 
-	SetUpControllerCommand(eCmdTestUnitReady, "CmdTestUnitReady", &SCSIDEV::CmdTestUnitReady);
+	SetUpDeviceCommand(eCmdTestUnitReady, "CmdTestUnitReady", &Disk::TestUnitReady);
-	SetUpControllerCommand(eCmdRezero, "CmdRezero", &SCSIDEV::CmdRezero);
+	SetUpDeviceCommand(eCmdRezero, "CmdRezero", &Disk::Rezero);
-	SetUpControllerCommand(eCmdRequestSense, "CmdRequestSense", &SCSIDEV::CmdRequestSense);
+	SetUpDeviceCommand(eCmdRequestSense, "CmdRequestSense", &Disk::RequestSense);
-	SetUpControllerCommand(eCmdFormat, "CmdFormat", &SCSIDEV::CmdFormat);
+	SetUpDeviceCommand(eCmdFormat, "CmdFormat", &Disk::Format);
-	SetUpControllerCommand(eCmdReassign, "CmdReassign", &SCSIDEV::CmdReassign);
+	SetUpDeviceCommand(eCmdReassign, "CmdReassign", &Disk::ReassignBlocks);
-	SetUpControllerCommand(eCmdRead6, "CmdRead6", &SCSIDEV::CmdRead6);
+	SetUpDeviceCommand(eCmdRead6, "CmdRead6", &Disk::Read6);
-	SetUpControllerCommand(eCmdWrite6, "CmdWrite6", &SCSIDEV::CmdWrite6);
+	SetUpDeviceCommand(eCmdWrite6, "CmdWrite6", &Disk::Write6);
 	SetUpDeviceCommand(eCmdSeek6, "CmdSeek6", &Disk::Seek6);
-	SetUpControllerCommand(eCmdInquiry, "CmdInquiry", &SCSIDEV::CmdInquiry);
+	SetUpDeviceCommand(eCmdInquiry, "CmdInquiry", &Disk::Inquiry);
-	SetUpControllerCommand(eCmdModeSelect, "CmdModeSelect", &SCSIDEV::CmdModeSelect);
+	SetUpDeviceCommand(eCmdModeSelect, "CmdModeSelect", &Disk::ModeSelect);
 	SetUpDeviceCommand(eCmdReserve6, "CmdReserve6", &Disk::Reserve6);
 	SetUpDeviceCommand(eCmdRelease6, "CmdRelease6", &Disk::Release6);
-	SetUpControllerCommand(eCmdModeSense, "CmdModeSense", &SCSIDEV::CmdModeSense);
+	SetUpDeviceCommand(eCmdModeSense, "CmdModeSense", &Disk::ModeSense);
-	SetUpControllerCommand(eCmdStartStop, "CmdStartStop", &SCSIDEV::CmdStartStop);
+	SetUpDeviceCommand(eCmdStartStop, "CmdStartStop", &Disk::StartStop);
-	SetUpControllerCommand(eCmdSendDiag, "CmdSendDiag", &SCSIDEV::CmdSendDiag);
+	SetUpDeviceCommand(eCmdSendDiag, "CmdSendDiag", &Disk::SendDiagnostic);
-	SetUpControllerCommand(eCmdRemoval, "CmdRemoval", &SCSIDEV::CmdRemoval);
+	SetUpDeviceCommand(eCmdRemoval, "CmdRemoval", &Disk::PreventAllowRemoval);
 	SetUpDeviceCommand(eCmdReadCapacity10, "CmdReadCapacity10", &Disk::ReadCapacity10);
 	SetUpDeviceCommand(eCmdRead10, "CmdRead10", &Disk::Read10);
 	SetUpDeviceCommand(eCmdWrite10, "CmdWrite10", &Disk::Write10);
 	SetUpDeviceCommand(eCmdVerify10, "CmdVerify10", &Disk::Write10);
 	SetUpDeviceCommand(eCmdSeek10, "CmdSeek10", &Disk::Seek10);
 	SetUpDeviceCommand(eCmdVerify, "CmdVerify", &Disk::Verify);
-	SetUpControllerCommand(eCmdSynchronizeCache, "CmdSynchronizeCache", &SCSIDEV::CmdSynchronizeCache);
+	SetUpDeviceCommand(eCmdSynchronizeCache, "CmdSynchronizeCache", &Disk::SynchronizeCache);
-	SetUpControllerCommand(eCmdReadDefectData10, "CmdReadDefectData10", &SCSIDEV::CmdReadDefectData10);
+	SetUpDeviceCommand(eCmdReadDefectData10, "CmdReadDefectData10", &Disk::ReadDefectData10);
-	SetUpControllerCommand(eCmdModeSelect10, "CmdModeSelect10", &SCSIDEV::CmdModeSelect10);
+	SetUpDeviceCommand(eCmdModeSelect10, "CmdModeSelect10", &Disk::ModeSelect10);
 	SetUpDeviceCommand(eCmdReserve10, "CmdReserve10", &Disk::Reserve10);
 	SetUpDeviceCommand(eCmdRelease10, "CmdRelease10", &Disk::Release10);
-	SetUpControllerCommand(eCmdModeSense10, "CmdModeSense10", &SCSIDEV::CmdModeSense10);
+	SetUpDeviceCommand(eCmdModeSense10, "CmdModeSense10", &Disk::ModeSense10);
 	SetUpDeviceCommand(eCmdRead16, "CmdRead16", &Disk::Read16);
 	SetUpDeviceCommand(eCmdWrite16, "CmdWrite16", &Disk::Write16);
 	SetUpDeviceCommand(eCmdVerify16, "CmdVerify16", &Disk::Write16);
@@ -432,197 +432,6 @@ void SCSIDEV::Error(ERROR_CODES::sense_key sense_key, ERROR_CODES::asc asc)
 //
 //---------------------------------------------------------------------------
 
-//---------------------------------------------------------------------------
-//
-//	INQUIRY
-//
-//---------------------------------------------------------------------------
-void SCSIDEV::CmdInquiry()
-{
-	LOGTRACE("%s INQUIRY Command", __PRETTY_FUNCTION__);
-
-	// Find a valid unit
-	// TODO The code below is probably wrong. It results in the same INQUIRY data being
-	// used for all LUNs, even though each LUN has its individual set of INQUIRY data.
-	PrimaryDevice *device = NULL;
-	for (int valid_lun = 0; valid_lun < UnitMax; valid_lun++) {
-		if (ctrl.unit[valid_lun]) {
-			device = ctrl.unit[valid_lun];
-			break;
-		}
-	}
-
-	// Processed on the disk side (it is originally processed by the controller)
-	if (device) {
-		ctrl.length = device->Inquiry(ctrl.cmd, ctrl.buffer);
-	} else {
-		ctrl.length = 0;
-	}
-
-	if (ctrl.length <= 0) {
-		// failure (error)
-		Error();
-		return;
-	}
-
-	// Add synchronous transfer support information
-	if (scsi.syncenable) {
-		ctrl.buffer[7] |= (1 << 4);
-	}
-
-	// Report if the device does not support the requested LUN
-	DWORD lun = (ctrl.cmd[1] >> 5) & 0x07;
-	if (!ctrl.unit[lun]) {
-		ctrl.buffer[0] |= 0x7f;
-	}
-
-	// Data-in Phase
-	DataIn();
-}
-
-//---------------------------------------------------------------------------
-//
-//	MODE SELECT
-//
-//---------------------------------------------------------------------------
-void SCSIDEV::CmdModeSelect()
-{
-	LOGTRACE( "%s MODE SELECT Command", __PRETTY_FUNCTION__);
-
-	// Command processing on drive
-	ctrl.length = ctrl.device->SelectCheck(ctrl.cmd);
-	if (ctrl.length <= 0) {
-		// Failure (Error)
-		Error();
-		return;
-	}
-
-	// Data out phase
-	DataOut();
-}
-
-//---------------------------------------------------------------------------
-//
-//	MODE SENSE
-//
-//---------------------------------------------------------------------------
-void SCSIDEV::CmdModeSense()
-{
-	LOGTRACE( "%s MODE SENSE Command ", __PRETTY_FUNCTION__);
-
-	// Command processing on drive
-	ctrl.length = ctrl.device->ModeSense(ctrl.cmd, ctrl.buffer);
-	ASSERT(ctrl.length >= 0);
-	if (ctrl.length == 0) {
-		LOGWARN("%s Not supported MODE SENSE page $%02X",__PRETTY_FUNCTION__, (unsigned int)ctrl.cmd[2]);
-
-		// Failure (Error)
-		Error();
-		return;
-	}
-
-	// Data-in Phase
-	DataIn();
-}
-
-//---------------------------------------------------------------------------
-//
-//	START STOP UNIT
-//
-//---------------------------------------------------------------------------
-void SCSIDEV::CmdStartStop()
-{
-	LOGTRACE( "%s START STOP UNIT Command ", __PRETTY_FUNCTION__);
-
-	// Command processing on drive
-	bool status = ctrl.device->StartStop(ctrl.cmd);
-	if (!status) {
-		// Failure (Error)
-		Error();
-		return;
-	}
-
-	// status phase
-	Status();
-}
-
-//---------------------------------------------------------------------------
-//
-//	SEND DIAGNOSTIC
-//
-//---------------------------------------------------------------------------
-void SCSIDEV::CmdSendDiag()
-{
-	LOGTRACE( "%s SEND DIAGNOSTIC Command ", __PRETTY_FUNCTION__);
-
-	// Command processing on drive
-	bool status = ctrl.device->SendDiag(ctrl.cmd);
-	if (!status) {
-		// Failure (Error)
-		Error();
-		return;
-	}
-
-	// status phase
-	Status();
-}
-
-//---------------------------------------------------------------------------
-//
-//	PREVENT/ALLOW MEDIUM REMOVAL
-//
-//---------------------------------------------------------------------------
-void SCSIDEV::CmdRemoval()
-{
-	LOGTRACE( "%s PREVENT/ALLOW MEDIUM REMOVAL Command ", __PRETTY_FUNCTION__);
-
-	// Command processing on drive
-	bool status = ctrl.device->Removal(ctrl.cmd);
-	if (!status) {
-		// Failure (Error)
-		Error();
-		return;
-	}
-
-	// status phase
-	Status();
-}
-
-//---------------------------------------------------------------------------
-//
-//	SYNCHRONIZE CACHE
-//
-//---------------------------------------------------------------------------
-void SCSIDEV::CmdSynchronizeCache()
-{
-	// Nothing to do
-
-	// status phase
-	Status();
-}
-
-//---------------------------------------------------------------------------
-//
-//	READ DEFECT DATA(10)
-//
-//---------------------------------------------------------------------------
-void SCSIDEV::CmdReadDefectData10()
-{
-	LOGTRACE( "%s READ DEFECT DATA(10) Command ", __PRETTY_FUNCTION__);
-
-	// Command processing on drive
-	ctrl.length = ctrl.device->ReadDefectData10(ctrl.cmd, ctrl.buffer);
-	ASSERT(ctrl.length >= 0);
-
-	if (ctrl.length <= 4) {
-		Error();
-		return;
-	}
-
-	// Data-in Phase
-	DataIn();
-}
-
 //---------------------------------------------------------------------------
 //
 //	READ TOC
@@ -710,51 +519,6 @@ void SCSIDEV::CmdGetEventStatusNotification()
 	Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::INVALID_FIELD_IN_CDB);
 }
 
-//---------------------------------------------------------------------------
-//
-//	MODE SELECT10
-//
-//---------------------------------------------------------------------------
-void SCSIDEV::CmdModeSelect10()
-{
-	LOGTRACE( "%s MODE SELECT10 Command ", __PRETTY_FUNCTION__);
-
-	// Command processing on drive
-	ctrl.length = ctrl.device->SelectCheck10(ctrl.cmd);
-	if (ctrl.length <= 0) {
-		// Failure (Error)
-		Error();
-		return;
-	}
-
-	// Data out phase
-	DataOut();
-}
-
-//---------------------------------------------------------------------------
-//
-//	MODE SENSE(10)
-//
-//---------------------------------------------------------------------------
-void SCSIDEV::CmdModeSense10()
-{
-	LOGTRACE( "%s MODE SENSE(10) Command ", __PRETTY_FUNCTION__);
-
-	// Command processing on drive
-	ctrl.length = ctrl.device->ModeSense10(ctrl.cmd, ctrl.buffer);
-	ASSERT(ctrl.length >= 0);
-	if (ctrl.length == 0) {
-		LOGWARN("%s Not supported MODE SENSE(10) page $%02X", __PRETTY_FUNCTION__, (WORD)ctrl.cmd[2]);
-
-		// Failure (Error)
-		Error();
-		return;
-	}
-
-	// Data-in Phase
-	DataIn();
-}
-
 //---------------------------------------------------------------------------
 //
 //	GET MESSAGE(10)

src/raspberrypi/controllers/scsidev_ctrl.h

@@ -66,8 +66,6 @@ public:
 	// 外部API
 	BUS::phase_t Process();					// Run
 
-	void SyncTransfer(BOOL enable) { scsi.syncenable = enable; }	// Synchronouse transfer enable setting
-
 	// Other
 	BOOL IsSASI() const {return FALSE;}				// SASI Check
 	BOOL IsSCSI() const {return TRUE;}				// SCSI check
@@ -89,14 +87,6 @@ private:
 	void MsgOut();							// Message out phase
 
 	// commands
-	void CmdInquiry();						// INQUIRY command
-	void CmdModeSelect();						// MODE SELECT command
-	void CmdModeSense();						// MODE SENSE command
-	void CmdStartStop();						// START STOP UNIT command
-	void CmdSendDiag();						// SEND DIAGNOSTIC command
-	void CmdRemoval();						// PREVENT/ALLOW MEDIUM REMOVAL command
-	void CmdSynchronizeCache();					// SYNCHRONIZE CACHE  command
-	void CmdReadDefectData10();					// READ DEFECT DATA(10)  command
 	void CmdReadToc();						// READ TOC command
 	void CmdPlayAudio10();						// PLAY AUDIO(10) command
 	void CmdPlayAudioMSF();					// PLAY AUDIO MSF command

src/raspberrypi/devices/block_device.h

@@ -23,10 +23,10 @@ public:
 	virtual ~BlockDevice() {};
 
 	// Mandatory commands
-	virtual bool TestUnitReady(const DWORD *cdb) override = 0;
+	virtual void TestUnitReady(SASIDEV *) override = 0;
-	virtual int Inquiry(const DWORD *cdb, BYTE *buf) override = 0;
+	virtual void Inquiry(SASIDEV *) override = 0;
 	virtual void ReportLuns(SASIDEV *) override = 0;
-	virtual bool Format(const DWORD *cdb) = 0;
+	virtual void Format(SASIDEV *) = 0;
 	virtual void ReadCapacity10(SASIDEV *) = 0;
 	virtual void ReadCapacity16(SASIDEV *) = 0;
 	virtual void Read10(SASIDEV *) = 0;
@@ -38,10 +38,10 @@ public:
 	// TODO uncomment
 	//virtual void Verify10(SASIDEV *) = 0;
 	//virtual void Verify16(SASIDEV *) = 0;
-	virtual int RequestSense(const DWORD *cdb, BYTE *buf) override = 0;
+	virtual void RequestSense(SASIDEV *) override = 0;
-	virtual int ModeSense(const DWORD *cdb, BYTE *buf) override = 0;
+	//virtual void ModeSense(SASIDEV *) override = 0;
-	virtual int ModeSense10(const DWORD *cdb, BYTE *buf) override = 0;
+	//virtual void ModeSense10(SASIDEV *) override = 0;
-	virtual bool ModeSelect(const DWORD *cdb, const BYTE *buf, int length) override = 0;
+	//virtual void ModeSelect(SASIDEV *) override = 0;
 
 	// TODO Add the other optional commands currently implemented
 };

src/raspberrypi/devices/disk.cpp

@@ -26,6 +26,7 @@
 #include "cfilesystem.h"
 #include "controllers/sasidev_ctrl.h"
 #include "controllers/scsidev_ctrl.h"
+#include "exceptions.h"
 #include "disk.h"
 #include <sstream>
 
@@ -352,6 +353,162 @@ BOOL DiskTrack::Read(BYTE *buf, int sec) const
 	return TRUE;
 }
 
+void Disk::TestUnitReady(SASIDEV *controller)
+{
+	LOGTRACE("%s TEST UNIT READY Command ", __PRETTY_FUNCTION__);
+
+	SASIDEV::ctrl_t *ctrl = controller->GetWorkAddr();
+
+	// Command processing on drive
+	bool status = TestUnitReady(ctrl->cmd);
+	if (!status) {
+		// Failure (Error)
+		controller->Error();
+		return;
+	}
+
+	// status phase
+	controller->Status();
+}
+
+void Disk::Rezero(SASIDEV *controller)
+{
+	LOGTRACE( "%s REZERO Command ", __PRETTY_FUNCTION__);
+
+	SASIDEV::ctrl_t *ctrl = controller->GetWorkAddr();
+
+	bool status = Rezero(ctrl->cmd);
+	if (!status) {
+		// Failure (Error)
+		controller->Error();
+		return;
+	}
+
+	// status phase
+	controller->Status();
+}
+
+void Disk::RequestSense(SASIDEV *controller)
+{
+	LOGTRACE( "%s REQUEST SENSE Command ", __PRETTY_FUNCTION__);
+
+	SASIDEV::ctrl_t *ctrl = controller->GetWorkAddr();
+
+    DWORD lun;
+    try {
+     	lun = GetLun();
+    }
+    catch(const lun_exception& e) {
+        // Note: According to the SCSI specs the LUN handling for REQUEST SENSE is special.
+        // Non-existing LUNs do *not* result in CHECK CONDITION.
+        // Only the Sense Key and ASC are set in order to signal the non-existing LUN.
+
+        // LUN 0 can be assumed to be present (required to call RequestSense() below)
+        lun = 0;
+
+        controller->Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::INVALID_LUN);
+    }
+
+    ctrl->length = ctrl->unit[lun]->RequestSense(ctrl->cmd, ctrl->buffer);
+	ASSERT(ctrl->length > 0);
+
+    LOGTRACE("%s Sense Key $%02X, ASC $%02X",__PRETTY_FUNCTION__, ctrl->buffer[2], ctrl->buffer[12]);
+
+	// Read phase
+    controller->DataIn();
+}
+
+void Disk::Format(SASIDEV *controller)
+{
+	LOGTRACE( "%s FORMAT UNIT Command ", __PRETTY_FUNCTION__);
+
+	SASIDEV::ctrl_t *ctrl = controller->GetWorkAddr();
+
+	// Command processing on drive
+	bool status = Format(ctrl->cmd);
+	if (!status) {
+		// Failure (Error)
+		controller->Error();
+		return;
+	}
+
+	// status phase
+	controller->Status();
+}
+
+void Disk::ReassignBlocks(SASIDEV *controller)
+{
+	LOGTRACE("%s REASSIGN BLOCKS Command ", __PRETTY_FUNCTION__);
+
+	SASIDEV::ctrl_t *ctrl = controller->GetWorkAddr();
+
+	// Command processing on drive
+	bool status = Reassign(ctrl->cmd);
+	if (!status) {
+		// Failure (Error)
+		controller->Error();
+		return;
+	}
+
+	// status phase
+	controller->Status();
+}
+
+void Disk::Read6(SASIDEV *controller)
+{
+	SASIDEV::ctrl_t *ctrl = controller->GetWorkAddr();
+
+	// Get record number and block number
+	DWORD record = ctrl->cmd[1] & 0x1f;
+	record <<= 8;
+	record |= ctrl->cmd[2];
+	record <<= 8;
+	record |= ctrl->cmd[3];
+	ctrl->blocks = ctrl->cmd[4];
+	if (ctrl->blocks == 0) {
+		ctrl->blocks = 0x100;
+	}
+
+	// TODO Move Daynaport specific test
+	// TODO This class must not know about SCDP
+	if(IsDaynaPort()){
+		// The DaynaPort only wants one block.
+		// ctrl.cmd[4] and ctrl.cmd[5] are used to specify the maximum buffer size for the DaynaPort
+		ctrl->blocks=1;
+	}
+	else {
+		// Check capacity
+		DWORD capacity = GetBlockCount();
+		if (record > capacity || record + ctrl->blocks > capacity) {
+			ostringstream s;
+			s << "Media capacity of " << capacity << " blocks exceeded: "
+					<< "Trying to read block " << record << ", block count " << ctrl->blocks;
+			LOGWARN("%s", s.str().c_str());
+			controller->Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::LBA_OUT_OF_RANGE);
+			return;
+		}
+	}
+
+	LOGTRACE("%s READ(6) command record=%d blocks=%d", __PRETTY_FUNCTION__, (unsigned int)record, (int)ctrl->blocks);
+
+	// Command processing on drive
+	ctrl->length = Read(ctrl->cmd, ctrl->buffer, record);
+	LOGTRACE("%s ctrl.length is %d", __PRETTY_FUNCTION__, (int)ctrl->length);
+
+	// The DaynaPort will respond a status of 0x02 when a read of size 1 occurs.
+	if (ctrl->length <= 0 && !IsDaynaPort()) {
+		// Failure (Error)
+		controller->Error();
+		return;
+	}
+
+	// Set next block
+	ctrl->next = record + 1;
+
+	// Read phase
+	controller->DataIn();
+}
+
 void Disk::Read10(SASIDEV *controller)
 {
 	// TODO Move to subclass
@@ -454,6 +611,55 @@ BOOL DiskTrack::Write(const BYTE *buf, int sec)
 	return TRUE;
 }
 
+void Disk::Write6(SASIDEV *controller)
+{
+	SASIDEV::ctrl_t *ctrl = controller->GetWorkAddr();
+
+	// Special receive function for the DaynaPort
+	if (IsDaynaPort()){
+		controller->DaynaPortWrite();
+		return;
+	}
+
+	// Get record number and block number
+	DWORD record = ctrl->cmd[1] & 0x1f;
+	record <<= 8;
+	record |= ctrl->cmd[2];
+	record <<= 8;
+	record |= ctrl->cmd[3];
+	ctrl->blocks = ctrl->cmd[4];
+	if (ctrl->blocks == 0) {
+		ctrl->blocks = 0x100;
+	}
+
+	// Check capacity
+	DWORD capacity = GetBlockCount();
+	if (record > capacity || record + ctrl->blocks > capacity) {
+		ostringstream s;
+		s << "Media capacity of " << capacity << " blocks exceeded: "
+				<< "Trying to write block " << record << ", block count " << ctrl->blocks;
+		LOGWARN("%s", s.str().c_str());
+		controller->Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::LBA_OUT_OF_RANGE);
+		return;
+	}
+
+	LOGTRACE("%s WRITE(6) command record=%d blocks=%d", __PRETTY_FUNCTION__, (WORD)record, (WORD)ctrl->blocks);
+
+	// Command processing on drive
+	ctrl->length = WriteCheck(record);
+	if (ctrl->length <= 0) {
+		// Failure (Error)
+		controller->Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::WRITE_PROTECTED);
+		return;
+	}
+
+	// Set next block
+	ctrl->next = record + 1;
+
+	// Write phase
+	controller->DataOut();
+}
+
 void Disk::Write10(SASIDEV *controller)
 {
 	SASIDEV::ctrl_t *ctrl = controller->GetWorkAddr();
@@ -549,6 +755,205 @@ void Disk::Verify(SASIDEV *controller)
 	controller->DataOut();
 }
 
+void Disk::Inquiry(SASIDEV *controller)
+{
+	LOGTRACE("%s INQUIRY Command", __PRETTY_FUNCTION__);
+
+	SASIDEV::ctrl_t *ctrl = controller->GetWorkAddr();
+
+	// Find a valid unit
+	// TODO The code below is probably wrong. It results in the same INQUIRY data being
+	// used for all LUNs, even though each LUN has its individual set of INQUIRY data.
+	PrimaryDevice *device = NULL;
+	for (int valid_lun = 0; valid_lun < SASIDEV::UnitMax; valid_lun++) {
+		if (ctrl->unit[valid_lun]) {
+			device = ctrl->unit[valid_lun];
+			break;
+		}
+	}
+
+	// Processed on the disk side (it is originally processed by the controller)
+	if (device) {
+		ctrl->length = Inquiry(ctrl->cmd, ctrl->buffer);
+	} else {
+		ctrl->length = 0;
+	}
+
+	if (ctrl->length <= 0) {
+		// failure (error)
+		controller->Error();
+		return;
+	}
+
+	// Report if the device does not support the requested LUN
+	DWORD lun = (ctrl->cmd[1] >> 5) & 0x07;
+	if (!ctrl->unit[lun]) {
+		ctrl->buffer[0] |= 0x7f;
+	}
+
+	// Data-in Phase
+	controller->DataIn();
+}
+
+void Disk::ModeSelect(SASIDEV *controller)
+{
+	LOGTRACE( "%s MODE SELECT Command", __PRETTY_FUNCTION__);
+
+	SASIDEV::ctrl_t *ctrl = controller->GetWorkAddr();
+
+	// Command processing on drive
+	ctrl->length = SelectCheck(ctrl->cmd);
+	if (ctrl->length <= 0) {
+		// Failure (Error)
+		controller->Error();
+		return;
+	}
+
+	// Data out phase
+	controller->DataOut();
+}
+
+void Disk::ModeSelect10(SASIDEV *controller)
+{
+	LOGTRACE( "%s MODE SELECT10 Command ", __PRETTY_FUNCTION__);
+
+	SASIDEV::ctrl_t *ctrl = controller->GetWorkAddr();
+
+	// Command processing on drive
+	ctrl->length = SelectCheck10(ctrl->cmd);
+	if (ctrl->length <= 0) {
+		// Failure (Error)
+		controller->Error();
+		return;
+	}
+
+	// Data out phase
+	controller->DataOut();
+}
+
+void Disk::ModeSense(SASIDEV *controller)
+{
+	LOGTRACE( "%s MODE SENSE Command ", __PRETTY_FUNCTION__);
+
+	SASIDEV::ctrl_t *ctrl = controller->GetWorkAddr();
+
+	// Command processing on drive
+	ctrl->length = ModeSense(ctrl->cmd, ctrl->buffer);
+	ASSERT(ctrl->length >= 0);
+	if (ctrl->length == 0) {
+		LOGWARN("%s Not supported MODE SENSE page $%02X",__PRETTY_FUNCTION__, (unsigned int)ctrl->cmd[2]);
+
+		// Failure (Error)
+		controller->Error();
+		return;
+	}
+
+	// Data-in Phase
+	controller->DataIn();
+}
+
+void Disk::ModeSense10(SASIDEV *controller)
+{
+	LOGTRACE( "%s MODE SENSE(10) Command ", __PRETTY_FUNCTION__);
+
+	SASIDEV::ctrl_t *ctrl = controller->GetWorkAddr();
+
+	// Command processing on drive
+	ctrl->length = ModeSense10(ctrl->cmd, ctrl->buffer);
+	ASSERT(ctrl->length >= 0);
+	if (ctrl->length == 0) {
+		LOGWARN("%s Not supported MODE SENSE(10) page $%02X", __PRETTY_FUNCTION__, (WORD)ctrl->cmd[2]);
+
+		// Failure (Error)
+		controller->Error();
+		return;
+	}
+
+	// Data-in Phase
+	controller->DataIn();
+}
+
+void Disk::StartStop(SASIDEV *controller)
+{
+	LOGTRACE( "%s START STOP Command ", __PRETTY_FUNCTION__);
+
+	SASIDEV::ctrl_t *ctrl = controller->GetWorkAddr();
+
+	// Command processing on drive
+	bool status = StartStop(ctrl->cmd);
+	if (!status) {
+		// Failure (Error)
+		controller->Error();
+		return;
+	}
+
+	// status phase
+	controller->Status();
+}
+
+void Disk::SendDiagnostic(SASIDEV *controller)
+{
+	LOGTRACE( "%s SEND DIAGNOSTIC Command ", __PRETTY_FUNCTION__);
+
+	SASIDEV::ctrl_t *ctrl = controller->GetWorkAddr();
+
+	// Command processing on drive
+	bool status = SendDiag(ctrl->cmd);
+	if (!status) {
+		// Failure (Error)
+		controller->Error();
+		return;
+	}
+
+	// status phase
+	controller->Status();
+}
+
+void Disk::PreventAllowRemoval(SASIDEV *controller)
+{
+	LOGTRACE( "%s PREVENT/ALLOW MEDIUM REMOVAL Command ", __PRETTY_FUNCTION__);
+
+	SASIDEV::ctrl_t *ctrl = controller->GetWorkAddr();
+
+	// Command processing on drive
+	bool status = Removal(ctrl->cmd);
+	if (!status) {
+		// Failure (Error)
+		controller->Error();
+		return;
+	}
+
+	// status phase
+	controller->Status();
+}
+
+void Disk::SynchronizeCache(SASIDEV *controller)
+{
+	// Nothing to do
+
+	// status phase
+	controller->Status();
+}
+
+void Disk::ReadDefectData10(SASIDEV *controller)
+{
+	LOGTRACE( "%s READ DEFECT DATA(10) Command ", __PRETTY_FUNCTION__);
+
+	SASIDEV::ctrl_t *ctrl = controller->GetWorkAddr();
+
+	// Command processing on drive
+	ctrl->length = ReadDefectData10(ctrl->cmd, ctrl->buffer);
+	ASSERT(ctrl->length >= 0);
+
+	if (ctrl->length <= 4) {
+		controller->Error();
+		return;
+	}
+
+	// Data-in Phase
+	controller->DataIn();
+}
+
 //===========================================================================
 //
 //	Disk Cache
@@ -1963,6 +2368,7 @@ void Disk::ReadCapacity10(SASIDEV *controller)
 	LOGTRACE( "%s READ CAPACITY(10) Command ", __PRETTY_FUNCTION__);
 
 	SASIDEV::ctrl_t *ctrl = controller->GetWorkAddr();
+
 	BYTE *buf = ctrl->buffer;
 
 	ASSERT(buf);

src/raspberrypi/devices/disk.h

@@ -149,24 +149,33 @@ public:
 	bool Eject(bool) override;					// Eject
 	bool Flush();							// Flush the cache
 
-	// commands
+	// Commands
-	virtual bool TestUnitReady(const DWORD *cdb) override;	// TEST UNIT READY command
+	void TestUnitReady(SASIDEV *) override;
-	virtual int Inquiry(const DWORD *cdb, BYTE *buf) override;	// INQUIRY command
+	void Inquiry(SASIDEV *) override;
-	virtual int RequestSense(const DWORD *cdb, BYTE *buf) override;		// REQUEST SENSE command
+	void RequestSense(SASIDEV *) override;
 	int SelectCheck(const DWORD *cdb);				// SELECT check
 	int SelectCheck10(const DWORD *cdb);				// SELECT(10) check
-	virtual bool ModeSelect(const DWORD *cdb, const BYTE *buf, int length) override;// MODE SELECT command
+	void ModeSelect(SASIDEV *);
-	virtual int ModeSense(const DWORD *cdb, BYTE *buf) override;		// MODE SENSE command
+	void ModeSelect10(SASIDEV *);
-	virtual int ModeSense10(const DWORD *cdb, BYTE *buf) override;		// MODE SENSE(10) command
+	void ModeSense(SASIDEV *);
-	int ReadDefectData10(const DWORD *cdb, BYTE *buf);		// READ DEFECT DATA(10) command
+	void ModeSense10(SASIDEV *);
-	bool Rezero(const DWORD *cdb);					// REZERO command
+	void Rezero(SASIDEV *);
-	bool Format(const DWORD *cdb) override;					// FORMAT UNIT command
+	void Format(SASIDEV *) override;
-	bool Reassign(const DWORD *cdb);				// REASSIGN UNIT command
+	void Reassign(SASIDEV *);
+	void ReassignBlocks(SASIDEV *);
+	void StartStop(SASIDEV *);						// START STOP UNIT command
+	void SendDiagnostic(SASIDEV *);
+	void PreventAllowRemoval(SASIDEV *);						// PREVENT/ALLOW MEDIUM REMOVAL command
+	void SynchronizeCache(SASIDEV *);
+	void ReadDefectData10(SASIDEV *);
 	virtual int Read(const DWORD *cdb, BYTE *buf, DWORD block);			// READ command
+	void Read6(SASIDEV *);
 	void Read10(SASIDEV *);
 	void Read16(SASIDEV *);
+	virtual int Inquiry(const DWORD *cdb, BYTE *buf);	// INQUIRY command
 	virtual int WriteCheck(DWORD block);					// WRITE check
 	virtual bool Write(const DWORD *cdb, const BYTE *buf, DWORD block);			// WRITE command
+	void Write6(SASIDEV *);
 	void Write10(SASIDEV *);
 	void Write16(SASIDEV *);
 	void Seek(SASIDEV *);
@@ -200,6 +209,18 @@ public:
 
 	bool GetStartAndCount(SASIDEV *, uint64_t&, uint32_t&, bool);
 
+	virtual int ModeSense10(const DWORD *cdb, BYTE *buf);		// MODE SENSE(10) command
+	int ReadDefectData10(const DWORD *cdb, BYTE *buf);		// READ DEFECT DATA(10) command
+
+	// TODO Try to get rid of these methods, which are currently use by SASIDEV
+	virtual bool TestUnitReady(const DWORD *cdb);	// TEST UNIT READY command
+	bool Rezero(const DWORD *cdb);					// REZERO command
+	virtual int RequestSense(const DWORD *cdb, BYTE *buf);		// REQUEST SENSE command
+	virtual bool ModeSelect(const DWORD *cdb, const BYTE *buf, int length);// MODE SELECT command
+	virtual int ModeSense(const DWORD *cdb, BYTE *buf);		// MODE SENSE command
+	bool Format(const DWORD *cdb);					// FORMAT UNIT command
+	bool Reassign(const DWORD *cdb);				// REASSIGN UNIT command
+
 protected:
 	// Internal processing
 	virtual int AddError(bool change, BYTE *buf);			// Add error

src/raspberrypi/devices/primary_device.h

@@ -23,13 +23,13 @@ public:
 	virtual ~PrimaryDevice() {};
 
 	// Mandatory commands
-	virtual bool TestUnitReady(const DWORD *cdb) = 0;
+	virtual void TestUnitReady(SASIDEV *) = 0;
-	virtual int Inquiry(const DWORD *cdb, BYTE *buf) = 0;
+	virtual void Inquiry(SASIDEV *) = 0;
 	virtual void ReportLuns(SASIDEV *) = 0;
 
 	// Implemented optional commands
-	virtual int RequestSense(const DWORD *cdb, BYTE *buf) = 0;
+	virtual void RequestSense(SASIDEV *) = 0;
-	virtual int ModeSense(const DWORD *cdb, BYTE *buf) = 0;
+	virtual void ModeSense(SASIDEV *) = 0;
-	virtual int ModeSense10(const DWORD *cdb, BYTE *buf) = 0;
+	virtual void ModeSense10(SASIDEV *) = 0;
-	virtual bool ModeSelect(const DWORD *cdb, const BYTE *buf, int length) = 0;
+	virtual void ModeSelect(SASIDEV *) = 0;
 };