diff --git a/src/raspberrypi/controllers/sasidev_ctrl.cpp b/src/raspberrypi/controllers/sasidev_ctrl.cpp
index d9590a53..c089b1f9 100644
--- a/src/raspberrypi/controllers/sasidev_ctrl.cpp
+++ b/src/raspberrypi/controllers/sasidev_ctrl.cpp
@@ -139,7 +139,7 @@ void SASIDEV::SetUnit(int no, Disk *dev)
 //	Check to see if this has a valid logical unit
 //
 //---------------------------------------------------------------------------
-BOOL SASIDEV::HasUnit()
+bool SASIDEV::HasUnit()
 {
 	for (int i = 0; i < UnitMax; i++) {
 		if (ctrl.unit[i]) {
@@ -402,6 +402,13 @@ void SASIDEV::Execute()
 		ctrl.status = 0;
 	}
 
+	ctrl.device = NULL;
+
+	// REQUEST SENSE requires a special LUN handling
+	if (ctrl.cmd[0] != eCmdRequestSense) {
+		ctrl.device = ctrl.unit[GetLun()];
+	}
+
 	// Process by command
 	// TODO This code does not belong here. Each device type needs such a dispatcher, which the controller has to call.
 	switch ((SASIDEV::scsi_command)ctrl.cmd[0]) {
@@ -706,10 +713,8 @@ void SASIDEV::CmdTestUnitReady()
 {
 	LOGTRACE("%s TEST UNIT READY Command ", __PRETTY_FUNCTION__);
 
-	DWORD lun = GetLun();
-
 	// Command processing on drive
-	BOOL status = ctrl.unit[lun]->TestUnitReady(ctrl.cmd);
+	bool status = ctrl.device->TestUnitReady(ctrl.cmd);
 	if (!status) {
 		// Failure (Error)
 		Error();
@@ -729,10 +734,8 @@ void SASIDEV::CmdRezero()
 {
 	LOGTRACE( "%s REZERO UNIT Command ", __PRETTY_FUNCTION__);
 
-	DWORD lun = GetLun();
-
 	// Command processing on drive
-	BOOL status = ctrl.unit[lun]->Rezero(ctrl.cmd);
+	bool status = ctrl.device->Rezero(ctrl.cmd);
 	if (!status) {
 		// Failure (Error)
 		Error();
@@ -785,10 +788,8 @@ void SASIDEV::CmdFormat()
 {
 	LOGTRACE( "%s FORMAT UNIT Command ", __PRETTY_FUNCTION__);
 
-	DWORD lun = GetLun();
-
 	// Command processing on drive
-	BOOL status = ctrl.unit[lun]->Format(ctrl.cmd);
+	bool status = ctrl.device->Format(ctrl.cmd);
 	if (!status) {
 		// Failure (Error)
 		Error();
@@ -808,10 +809,8 @@ void SASIDEV::CmdReassign()
 {
 	LOGTRACE("%s REASSIGN BLOCKS Command ", __PRETTY_FUNCTION__);
 
-	DWORD lun = GetLun();
-
 	// Command processing on drive
-	BOOL status = ctrl.unit[lun]->Reassign(ctrl.cmd);
+	bool status = ctrl.device->Reassign(ctrl.cmd);
 	if (!status) {
 		// Failure (Error)
 		Error();
@@ -865,8 +864,6 @@ void SASIDEV::CmdReleaseUnit()
 //---------------------------------------------------------------------------
 void SASIDEV::CmdRead6()
 {
-	DWORD lun = GetLun();
-
 	// Get record number and block number
 	DWORD record = ctrl.cmd[1] & 0x1f;
 	record <<= 8;
@@ -878,15 +875,16 @@ void SASIDEV::CmdRead6()
 		ctrl.blocks = 0x100;
 	}
 
+	// TODO Move Daynaport specific test
 	// TODO This class must not know about SCDP
-	if(ctrl.unit[lun]->IsDaynaPort()){
+	if(ctrl.device->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 = ctrl.unit[lun]->GetBlockCount();
+		DWORD capacity = ctrl.device->GetBlockCount();
 		if (record > capacity || record + ctrl.blocks > capacity) {
 			ostringstream s;
 			s << "ID " << GetSCSIID() << ": Media capacity of " << capacity << " blocks exceeded: "
@@ -900,11 +898,11 @@ void SASIDEV::CmdRead6()
 	LOGTRACE("%s READ(6) command record=%d blocks=%d", __PRETTY_FUNCTION__, (unsigned int)record, (int)ctrl.blocks);
 
 	// Command processing on drive
-	ctrl.length = ctrl.unit[lun]->Read(ctrl.cmd, ctrl.buffer, record);
+	ctrl.length = ctrl.device->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 && !ctrl.unit[lun]->IsDaynaPort()) {
+	if (ctrl.length <= 0 && !ctrl.device->IsDaynaPort()) {
 		// Failure (Error)
 		Error();
 		return;
@@ -925,10 +923,8 @@ void SASIDEV::CmdRead6()
 //---------------------------------------------------------------------------
 void SASIDEV::DaynaPortWrite()
 {
-	DWORD lun = GetLun();
-
 	// Error if not a DaynaPort device
-	if (!ctrl.unit[lun]->IsDaynaPort()) {
+	if (!ctrl.device->IsDaynaPort()) {
 		LOGERROR("Received DaynaPortWrite for a non-DaynaPort device");
 		Error();
 		return;
@@ -977,10 +973,8 @@ void SASIDEV::DaynaPortWrite()
 //---------------------------------------------------------------------------
 void SASIDEV::CmdWrite6()
 {
-	DWORD lun = GetLun();
-
 	// Special receive function for the DaynaPort
-	if (ctrl.unit[lun]->IsDaynaPort()){
+	if (ctrl.device->IsDaynaPort()){
 		DaynaPortWrite();
 		return;
 	}
@@ -997,7 +991,7 @@ void SASIDEV::CmdWrite6()
 	}
 
 	// Check capacity
-	DWORD capacity = ctrl.unit[lun]->GetBlockCount();
+	DWORD capacity = ctrl.device->GetBlockCount();
 	if (record > capacity || record + ctrl.blocks > capacity) {
 		ostringstream s;
 		s << "ID " << GetSCSIID() << ": Media capacity of " << capacity << " blocks exceeded: "
@@ -1010,7 +1004,7 @@ void SASIDEV::CmdWrite6()
 	LOGTRACE("%s WRITE(6) command record=%d blocks=%d", __PRETTY_FUNCTION__, (WORD)record, (WORD)ctrl.blocks);
 
 	// Command processing on drive
-	ctrl.length = ctrl.unit[lun]->WriteCheck(record);
+	ctrl.length = ctrl.device->WriteCheck(record);
 	if (ctrl.length <= 0) {
 		// Failure (Error)
 		Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::WRITE_PROTECTED);
@@ -1033,10 +1027,8 @@ void SASIDEV::CmdSeek6()
 {
 	LOGTRACE("%s SEEK(6) Command ", __PRETTY_FUNCTION__);
 
-	DWORD lun = GetLun();
-
 	// Command processing on drive
-	BOOL status = ctrl.unit[lun]->Seek(ctrl.cmd);
+	bool status = ctrl.device->Seek(ctrl.cmd);
 	if (!status) {
 		// Failure (Error)
 		Error();
@@ -1056,10 +1048,8 @@ void SASIDEV::CmdAssign()
 {
 	LOGTRACE("%s ASSIGN Command ", __PRETTY_FUNCTION__);
 
-        DWORD lun = GetLun();
-
 	// Command processing on drive
-	BOOL status = ctrl.unit[lun]->Assign(ctrl.cmd);
+	bool status = ctrl.device->Assign(ctrl.cmd);
 	if (!status) {
 		// Failure (Error)
 		Error();
@@ -1082,10 +1072,8 @@ void SASIDEV::CmdSpecify()
 {
 	LOGTRACE("%s SPECIFY Command ", __PRETTY_FUNCTION__);
 
-	DWORD lun = GetLun();
-
 	// Command processing on drive
-	BOOL status = ctrl.unit[lun]->Assign(ctrl.cmd);
+	bool status = ctrl.device->Assign(ctrl.cmd);
 	if (!status) {
 		// Failure (Error)
 		Error();
@@ -1125,7 +1113,6 @@ void SASIDEV::CmdInvalid()
 void SASIDEV::Send()
 {
 	int len;
-	BOOL result;
 
 	ASSERT(!ctrl.bus->GetREQ());
 	ASSERT(ctrl.bus->GetIO());
@@ -1153,7 +1140,7 @@ void SASIDEV::Send()
 
 	// Remove block and initialize the result
 	ctrl.blocks--;
-	result = TRUE;
+	BOOL result = TRUE;
 
 	// Process after data collection (read/data-in only)
 	if (ctrl.phase == BUS::datain) {
@@ -1217,7 +1204,6 @@ void SASIDEV::Send()
 void SASIDEV::Receive()
 {
 	int len;
-	BOOL result;
 
 	// REQ is low
 	ASSERT(!ctrl.bus->GetREQ());
@@ -1248,7 +1234,7 @@ void SASIDEV::Receive()
 
 	// Remove the control block and initialize the result
 	ctrl.blocks--;
-	result = TRUE;
+	BOOL result = TRUE;
 
 	// Process the data out phase
 	if (ctrl.phase == BUS::dataout) {
@@ -1354,11 +1340,12 @@ BOOL SASIDEV::XferOut(BOOL cont)
 	if (!ctrl.unit[lun]) {
 		return FALSE;
 	}
+	Disk *device = ctrl.unit[lun];
 
 	switch ((SASIDEV::scsi_command) ctrl.cmd[0]) {
 		case SASIDEV::eCmdModeSelect:
 		case SASIDEV::eCmdModeSelect10:
-			if (!ctrl.unit[lun]->ModeSelect(
+			if (!device->ModeSelect(
 				ctrl.cmd, ctrl.buffer, ctrl.offset)) {
 				// MODE SELECT failed
 				return FALSE;
@@ -1372,8 +1359,8 @@ BOOL SASIDEV::XferOut(BOOL cont)
 		case SASIDEV::eCmdVerify16:
 			// If we're a host bridge, use the host bridge's SendMessage10 function
 			// TODO This class must not know about SCSIBR
-			if (ctrl.unit[lun]->IsBridge()) {
-				if (!((SCSIBR*)ctrl.unit[lun])->SendMessage10(ctrl.cmd, ctrl.buffer)) {
+			if (device->IsBridge()) {
+				if (!((SCSIBR*)device)->SendMessage10(ctrl.cmd, ctrl.buffer)) {
 					// write failed
 					return FALSE;
 				}
@@ -1385,9 +1372,9 @@ BOOL SASIDEV::XferOut(BOOL cont)
 
 			// Special case Write function for DaynaPort
 			// TODO This class must not know about SCSIDP
-			if (ctrl.unit[lun]->IsDaynaPort()) {
+			if (device->IsDaynaPort()) {
 				LOGTRACE("%s Doing special case write for DaynaPort", __PRETTY_FUNCTION__);
-				if (!(SCSIDaynaPort*)ctrl.unit[lun]->Write(ctrl.cmd, ctrl.buffer, ctrl.length)) {
+				if (!(SCSIDaynaPort*)device->Write(ctrl.cmd, ctrl.buffer, ctrl.length)) {
 					// write failed
 					return FALSE;
 				}
@@ -1400,7 +1387,7 @@ BOOL SASIDEV::XferOut(BOOL cont)
 			}
 
 			LOGTRACE("%s eCmdVerify Calling Write... cmd: %02X next: %d", __PRETTY_FUNCTION__, (WORD)ctrl.cmd[0], (int)ctrl.next);
-			if (!ctrl.unit[lun]->Write(ctrl.cmd, ctrl.buffer, ctrl.next - 1)) {
+			if (!device->Write(ctrl.cmd, ctrl.buffer, ctrl.next - 1)) {
 				// Write failed
 				return FALSE;
 			}
@@ -1412,7 +1399,7 @@ BOOL SASIDEV::XferOut(BOOL cont)
 			}
 
 			// Check the next block
-			ctrl.length = ctrl.unit[lun]->WriteCheck(ctrl.next - 1);
+			ctrl.length = device->WriteCheck(ctrl.next - 1);
 			if (ctrl.length <= 0) {
 				// Cannot write
 				return FALSE;
@@ -1454,6 +1441,7 @@ void SASIDEV::FlushUnit()
 	if (!ctrl.unit[lun]) {
 		return;
 	}
+	Disk *device = ctrl.unit[lun];
 
 	// WRITE system only
 	switch ((SASIDEV::scsi_command)ctrl.cmd[0]) {
@@ -1480,7 +1468,7 @@ void SASIDEV::FlushUnit()
             LOGWARN("   Reserved: %02X\n",(WORD)ctrl.cmd[5]);
             LOGWARN("   Ctrl Len: %08X\n",(WORD)ctrl.length);
 
-			if (!ctrl.unit[lun]->ModeSelect(
+			if (!device->ModeSelect(
 				ctrl.cmd, ctrl.buffer, ctrl.offset)) {
 				// MODE SELECT failed
 				LOGWARN("Error occured while processing Mode Select command %02X\n", (unsigned char)ctrl.cmd[0]);
@@ -1554,7 +1542,7 @@ void SASIDEV::GetPhaseStr(char *str)
 
 //---------------------------------------------------------------------------
 //
-//	Validate LUN
+//	Validate and get LUN
 //
 //---------------------------------------------------------------------------
 DWORD SASIDEV::GetLun()
diff --git a/src/raspberrypi/controllers/sasidev_ctrl.h b/src/raspberrypi/controllers/sasidev_ctrl.h
index c78d1b0e..f969fed0 100644
--- a/src/raspberrypi/controllers/sasidev_ctrl.h
+++ b/src/raspberrypi/controllers/sasidev_ctrl.h
@@ -136,13 +136,15 @@ public:
 		// Transfer
 		BYTE *buffer;					// Transfer data buffer
 		int bufsize;					// Transfer data buffer size
-		DWORD blocks;					// Number of transfer block
+		uint32_t blocks;				// Number of transfer block
 		DWORD next;						// Next record
 		DWORD offset;					// Transfer offset
 		DWORD length;					// Transfer remaining length
 
 		// Logical unit
 		Disk *unit[UnitMax];
+
+		Disk *device;
 	} ctrl_t;
 
 public:
@@ -158,7 +160,7 @@ public:
 	void Connect(int id, BUS *sbus);				// Controller connection
 	Disk* GetUnit(int no);							// Get logical unit
 	void SetUnit(int no, Disk *dev);				// Logical unit setting
-	BOOL HasUnit();						// Has a valid logical unit
+	bool HasUnit();						// Has a valid logical unit
 
 	// Other
 	BUS::phase_t GetPhase() {return ctrl.phase;}			// Get the phase
@@ -175,6 +177,9 @@ public:
 	virtual BOOL IsSCSI() const {return FALSE;}			// SCSI check
 	Disk* GetBusyUnit();								// Get the busy unit
 
+public:
+	void DataIn();							// Data in phase
+
 protected:
 	// Phase processing
 	virtual void BusFree();					// Bus free phase
@@ -183,7 +188,6 @@ protected:
 	virtual void Execute();					// Execution phase
 	void Status();							// Status phase
 	void MsgIn();							// Message in phase
-	void DataIn();							// Data in phase
 	void DataOut();						// Data out phase
 	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
diff --git a/src/raspberrypi/controllers/scsidev_ctrl.cpp b/src/raspberrypi/controllers/scsidev_ctrl.cpp
index cf6362f3..f22c59df 100644
--- a/src/raspberrypi/controllers/scsidev_ctrl.cpp
+++ b/src/raspberrypi/controllers/scsidev_ctrl.cpp
@@ -42,64 +42,73 @@ SCSIDEV::SCSIDEV() : SASIDEV()
 	scsi.msc = 0;
 	memset(scsi.msb, 0x00, sizeof(scsi.msb));
 
-	SetUpCommand(eCmdTestUnitReady, "CmdTestUnitReady", &SCSIDEV::CmdTestUnitReady);
-	SetUpCommand(eCmdRezero, "CmdRezero", &SCSIDEV::CmdRezero);
-	SetUpCommand(eCmdRequestSense, "CmdRequestSense", &SCSIDEV::CmdRequestSense);
-	SetUpCommand(eCmdFormat, "CmdFormat", &SCSIDEV::CmdFormat);
-	SetUpCommand(eCmdReassign, "CmdReassign", &SCSIDEV::CmdReassign);
-	SetUpCommand(eCmdRead6, "CmdRead6", &SCSIDEV::CmdRead6);
-	SetUpCommand(eCmdWrite6, "CmdWrite6", &SCSIDEV::CmdWrite6);
-	SetUpCommand(eCmdSeek6, "CmdSeek6", &SCSIDEV::CmdSeek6);
-	SetUpCommand(eCmdInquiry, "CmdInquiry", &SCSIDEV::CmdInquiry);
-	SetUpCommand(eCmdModeSelect, "CmdModeSelect", &SCSIDEV::CmdModeSelect);
-	SetUpCommand(eCmdReserve6, "CmdReserve6", &SCSIDEV::CmdReserve6);
-	SetUpCommand(eCmdRelease6, "CmdRelease6", &SCSIDEV::CmdRelease6);
-	SetUpCommand(eCmdModeSense, "CmdModeSense", &SCSIDEV::CmdModeSense);
-	SetUpCommand(eCmdStartStop, "CmdStartStop", &SCSIDEV::CmdStartStop);
-	SetUpCommand(eCmdSendDiag, "CmdSendDiag", &SCSIDEV::CmdSendDiag);
-	SetUpCommand(eCmdRemoval, "CmdRemoval", &SCSIDEV::CmdRemoval);
-	SetUpCommand(eCmdReadCapacity10, "CmdReadCapacity10", &SCSIDEV::CmdReadCapacity10);
-	SetUpCommand(eCmdRead10, "CmdRead10", &SCSIDEV::CmdRead10);
-	SetUpCommand(eCmdWrite10, "CmdWrite10", &SCSIDEV::CmdWrite10);
-	SetUpCommand(eCmdVerify10, "CmdVerify10", &SCSIDEV::CmdWrite10);
-	SetUpCommand(eCmdSeek10, "CmdSeek10", &SCSIDEV::CmdSeek10);
-	SetUpCommand(eCmdVerify, "CmdVerify", &SCSIDEV::CmdVerify);
-	SetUpCommand(eCmdSynchronizeCache, "CmdSynchronizeCache", &SCSIDEV::CmdSynchronizeCache);
-	SetUpCommand(eCmdReadDefectData10, "CmdReadDefectData10", &SCSIDEV::CmdReadDefectData10);
-	SetUpCommand(eCmdModeSelect10, "CmdModeSelect10", &SCSIDEV::CmdModeSelect10);
-	SetUpCommand(eCmdReserve10, "CmdReserve10", &SCSIDEV::CmdReserve10);
-	SetUpCommand(eCmdRelease10, "CmdRelease10", &SCSIDEV::CmdRelease10);
-	SetUpCommand(eCmdModeSense10, "CmdModeSense10", &SCSIDEV::CmdModeSense10);
-	SetUpCommand(eCmdRead16, "CmdRead16", &SCSIDEV::CmdRead16);
-	SetUpCommand(eCmdWrite16, "CmdWrite16", &SCSIDEV::CmdWrite16);
-	SetUpCommand(eCmdVerify16, "CmdVerify16", &SCSIDEV::CmdWrite16);
-	SetUpCommand(eCmdReadCapacity16, "CmdReadCapacity16", &SCSIDEV::CmdReadCapacity16);
-	SetUpCommand(eCmdReportLuns, "CmdReportLuns", &SCSIDEV::CmdReportLuns);
+	SetUpControllerCommand(eCmdTestUnitReady, "CmdTestUnitReady", &SCSIDEV::CmdTestUnitReady);
+	SetUpControllerCommand(eCmdRezero, "CmdRezero", &SCSIDEV::CmdRezero);
+	SetUpControllerCommand(eCmdRequestSense, "CmdRequestSense", &SCSIDEV::CmdRequestSense);
+	SetUpControllerCommand(eCmdFormat, "CmdFormat", &SCSIDEV::CmdFormat);
+	SetUpControllerCommand(eCmdReassign, "CmdReassign", &SCSIDEV::CmdReassign);
+	SetUpControllerCommand(eCmdRead6, "CmdRead6", &SCSIDEV::CmdRead6);
+	SetUpControllerCommand(eCmdWrite6, "CmdWrite6", &SCSIDEV::CmdWrite6);
+	SetUpControllerCommand(eCmdSeek6, "CmdSeek6", &SCSIDEV::CmdSeek6);
+	SetUpControllerCommand(eCmdInquiry, "CmdInquiry", &SCSIDEV::CmdInquiry);
+	SetUpControllerCommand(eCmdModeSelect, "CmdModeSelect", &SCSIDEV::CmdModeSelect);
+	SetUpControllerCommand(eCmdReserve6, "CmdReserve6", &SCSIDEV::CmdReserve6);
+	SetUpControllerCommand(eCmdRelease6, "CmdRelease6", &SCSIDEV::CmdRelease6);
+	SetUpControllerCommand(eCmdModeSense, "CmdModeSense", &SCSIDEV::CmdModeSense);
+	SetUpControllerCommand(eCmdStartStop, "CmdStartStop", &SCSIDEV::CmdStartStop);
+	SetUpControllerCommand(eCmdSendDiag, "CmdSendDiag", &SCSIDEV::CmdSendDiag);
+	SetUpControllerCommand(eCmdRemoval, "CmdRemoval", &SCSIDEV::CmdRemoval);
+	SetUpDeviceCommand(eCmdReadCapacity10, "CmdReadCapacity10", &Disk::ReadCapacity10);
+	SetUpControllerCommand(eCmdRead10, "CmdRead10", &SCSIDEV::CmdRead10);
+	SetUpControllerCommand(eCmdWrite10, "CmdWrite10", &SCSIDEV::CmdWrite10);
+	SetUpControllerCommand(eCmdVerify10, "CmdVerify10", &SCSIDEV::CmdWrite10);
+	SetUpControllerCommand(eCmdSeek10, "CmdSeek10", &SCSIDEV::CmdSeek10);
+	SetUpControllerCommand(eCmdVerify, "CmdVerify", &SCSIDEV::CmdVerify);
+	SetUpControllerCommand(eCmdSynchronizeCache, "CmdSynchronizeCache", &SCSIDEV::CmdSynchronizeCache);
+	SetUpControllerCommand(eCmdReadDefectData10, "CmdReadDefectData10", &SCSIDEV::CmdReadDefectData10);
+	SetUpControllerCommand(eCmdModeSelect10, "CmdModeSelect10", &SCSIDEV::CmdModeSelect10);
+	SetUpControllerCommand(eCmdReserve10, "CmdReserve10", &SCSIDEV::CmdReserve10);
+	SetUpControllerCommand(eCmdRelease10, "CmdRelease10", &SCSIDEV::CmdRelease10);
+	SetUpControllerCommand(eCmdModeSense10, "CmdModeSense10", &SCSIDEV::CmdModeSense10);
+	SetUpControllerCommand(eCmdRead16, "CmdRead16", &SCSIDEV::CmdRead16);
+	SetUpControllerCommand(eCmdWrite16, "CmdWrite16", &SCSIDEV::CmdWrite16);
+	SetUpControllerCommand(eCmdVerify16, "CmdVerify16", &SCSIDEV::CmdWrite16);
+	SetUpDeviceCommand(eCmdReadCapacity16, "CmdReadCapacity16", &Disk::ReadCapacity16);
+	SetUpControllerCommand(eCmdReportLuns, "CmdReportLuns", &SCSIDEV::CmdReportLuns);
 
 	// MMC specific. TODO Move to separate class
-	SetUpCommand(eCmdReadToc, "CmdReadToc", &SCSIDEV::CmdReadToc);
-	SetUpCommand(eCmdPlayAudio10, "CmdPlayAudio10", &SCSIDEV::CmdPlayAudio10);
-	SetUpCommand(eCmdPlayAudioMSF, "CmdPlayAudioMSF", &SCSIDEV::CmdPlayAudioMSF);
-	SetUpCommand(eCmdPlayAudioTrack, "CmdPlayAudioTrack", &SCSIDEV::CmdPlayAudioTrack);
-	SetUpCommand(eCmdGetEventStatusNotification, "CmdGetEventStatusNotification", &SCSIDEV::CmdGetEventStatusNotification);
+	SetUpControllerCommand(eCmdReadToc, "CmdReadToc", &SCSIDEV::CmdReadToc);
+	SetUpControllerCommand(eCmdPlayAudio10, "CmdPlayAudio10", &SCSIDEV::CmdPlayAudio10);
+	SetUpControllerCommand(eCmdPlayAudioMSF, "CmdPlayAudioMSF", &SCSIDEV::CmdPlayAudioMSF);
+	SetUpControllerCommand(eCmdPlayAudioTrack, "CmdPlayAudioTrack", &SCSIDEV::CmdPlayAudioTrack);
+	SetUpControllerCommand(eCmdGetEventStatusNotification, "CmdGetEventStatusNotification", &SCSIDEV::CmdGetEventStatusNotification);
 
 	// DaynaPort specific. TODO Move to separate class
-	SetUpCommand(eCmdRetrieveStats, "CmdRetrieveStats", &SCSIDEV::CmdRetrieveStats);
-	SetUpCommand(eCmdSetIfaceMode, "CmdSetIfaceMode", &SCSIDEV::CmdSetIfaceMode);
-	SetUpCommand(eCmdSetMcastAddr, "CmdSetMcastAddr", &SCSIDEV::CmdSetMcastAddr);
-	SetUpCommand(eCmdEnableInterface, "CmdEnableInterface", &SCSIDEV::CmdEnableInterface);
+	SetUpControllerCommand(eCmdRetrieveStats, "CmdRetrieveStats", &SCSIDEV::CmdRetrieveStats);
+	SetUpControllerCommand(eCmdSetIfaceMode, "CmdSetIfaceMode", &SCSIDEV::CmdSetIfaceMode);
+	SetUpControllerCommand(eCmdSetMcastAddr, "CmdSetMcastAddr", &SCSIDEV::CmdSetMcastAddr);
+	SetUpControllerCommand(eCmdEnableInterface, "CmdEnableInterface", &SCSIDEV::CmdEnableInterface);
 }
 
 SCSIDEV::~SCSIDEV()
 {
-	for (auto const& command : scsi_commands) {
+	for (auto const& command : controller_commands) {
+		free(command.second);
+	}
+
+	for (auto const& command : device_commands) {
 		free(command.second);
 	}
 }
 
-void SCSIDEV::SetUpCommand(scsi_command opcode, const char* name, void (SCSIDEV::*execute)(void))
+void SCSIDEV::SetUpControllerCommand(scsi_command opcode, const char* name, void (SCSIDEV::*execute)(void))
 {
-	scsi_commands[opcode] = new command_t(name, execute);
+	controller_commands[opcode] = new controller_command_t(name, execute);
+}
+
+void SCSIDEV::SetUpDeviceCommand(scsi_command opcode, const char* name, void (Disk::*execute)(SCSIDEV *, SASIDEV::ctrl_t *))
+{
+	device_commands[opcode] = new device_command_t(name, execute);
 }
 
 //---------------------------------------------------------------------------
@@ -296,13 +305,30 @@ void SCSIDEV::Execute()
 	ctrl.blocks = 1;
 	ctrl.execstart = SysTimer::GetTimerLow();
 
+	ctrl.device = NULL;
+
+	// INQUIRY requires a special LUN handling
+	if (ctrl.cmd[0] != eCmdInquiry) {
+		ctrl.device = ctrl.unit[GetLun()];
+	}
+
+	if (device_commands.count(static_cast<scsi_command>(ctrl.cmd[0]))) {
+		device_command_t *command = device_commands[static_cast<scsi_command>(ctrl.cmd[0])];
+
+		LOGDEBUG("++++ CMD ++++ %s ID %d received %s ($%02X)", __PRETTY_FUNCTION__, GetSCSIID(), command->name, (unsigned int)ctrl.cmd[0]);
+
+		(ctrl.device->*command->execute)(this, &ctrl);
+
+		return;
+	}
+
 	// If the command is valid it must be contained in the command map
-	if (!scsi_commands.count(static_cast<scsi_command>(ctrl.cmd[0]))) {
+	if (!controller_commands.count(static_cast<scsi_command>(ctrl.cmd[0]))) {
 		CmdInvalid();
 		return;
 	}
 
-	command_t* command = scsi_commands[static_cast<scsi_command>(ctrl.cmd[0])];
+	controller_command_t* command = controller_commands[static_cast<scsi_command>(ctrl.cmd[0])];
 
 	LOGDEBUG("++++ CMD ++++ %s ID %d received %s ($%02X)", __PRETTY_FUNCTION__, GetSCSIID(), command->name, (unsigned int)ctrl.cmd[0]);
 
@@ -463,10 +489,8 @@ void SCSIDEV::CmdModeSelect()
 {
 	LOGTRACE( "%s MODE SELECT Command", __PRETTY_FUNCTION__);
 
-	DWORD lun = GetLun();
-
 	// Command processing on drive
-	ctrl.length = ctrl.unit[lun]->SelectCheck(ctrl.cmd);
+	ctrl.length = ctrl.device->SelectCheck(ctrl.cmd);
 	if (ctrl.length <= 0) {
 		// Failure (Error)
 		Error();
@@ -558,10 +582,8 @@ void SCSIDEV::CmdModeSense()
 {
 	LOGTRACE( "%s MODE SENSE Command ", __PRETTY_FUNCTION__);
 
-	DWORD lun = GetLun();
-
 	// Command processing on drive
-	ctrl.length = ctrl.unit[lun]->ModeSense(ctrl.cmd, ctrl.buffer);
+	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]);
@@ -584,10 +606,8 @@ void SCSIDEV::CmdStartStop()
 {
 	LOGTRACE( "%s START STOP UNIT Command ", __PRETTY_FUNCTION__);
 
-	DWORD lun = GetLun();
-
 	// Command processing on drive
-	bool status = ctrl.unit[lun]->StartStop(ctrl.cmd);
+	bool status = ctrl.device->StartStop(ctrl.cmd);
 	if (!status) {
 		// Failure (Error)
 		Error();
@@ -607,10 +627,8 @@ void SCSIDEV::CmdSendDiag()
 {
 	LOGTRACE( "%s SEND DIAGNOSTIC Command ", __PRETTY_FUNCTION__);
 
-	DWORD lun = GetLun();
-
 	// Command processing on drive
-	bool status = ctrl.unit[lun]->SendDiag(ctrl.cmd);
+	bool status = ctrl.device->SendDiag(ctrl.cmd);
 	if (!status) {
 		// Failure (Error)
 		Error();
@@ -630,10 +648,8 @@ void SCSIDEV::CmdRemoval()
 {
 	LOGTRACE( "%s PREVENT/ALLOW MEDIUM REMOVAL Command ", __PRETTY_FUNCTION__);
 
-	DWORD lun = GetLun();
-
 	// Command processing on drive
-	bool status = ctrl.unit[lun]->Removal(ctrl.cmd);
+	bool status = ctrl.device->Removal(ctrl.cmd);
 	if (!status) {
 		// Failure (Error)
 		Error();
@@ -644,51 +660,6 @@ void SCSIDEV::CmdRemoval()
 	Status();
 }
 
-//---------------------------------------------------------------------------
-//
-//	READ CAPACITY
-//
-//---------------------------------------------------------------------------
-void SCSIDEV::CmdReadCapacity10()
-{
-	LOGTRACE( "%s READ CAPACITY(10) Command ", __PRETTY_FUNCTION__);
-
-	DWORD lun = GetLun();
-
-	// Command processing on drive
-	int length = ctrl.unit[lun]->ReadCapacity10(ctrl.cmd, ctrl.buffer);
-	if (length <= 0) {
-		Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::MEDIUM_NOT_PRESENT);
-		return;
-	}
-
-	// Length setting
-	ctrl.length = length;
-
-	// Data-in Phase
-	DataIn();
-}
-
-void SCSIDEV::CmdReadCapacity16()
-{
-	LOGTRACE( "%s READ CAPACITY(16) Command ", __PRETTY_FUNCTION__);
-
-	DWORD lun = GetLun();
-
-	// Command processing on drive
-	int length = ctrl.unit[lun]->ReadCapacity16(ctrl.cmd, ctrl.buffer);
-	if (length <= 0) {
-		Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::MEDIUM_NOT_PRESENT);
-		return;
-	}
-
-	// Length setting
-	ctrl.length = length;
-
-	// Data-in Phase
-	DataIn();
-}
-
 //---------------------------------------------------------------------------
 //
 //	READ(10)
@@ -696,47 +667,23 @@ void SCSIDEV::CmdReadCapacity16()
 //---------------------------------------------------------------------------
 void SCSIDEV::CmdRead10()
 {
-	DWORD lun = GetLun();
-
+	// TODO Move bridge specific test
 	// Receive message if host bridge
-	if (ctrl.unit[lun]->IsBridge()) {
+	if (ctrl.device->IsBridge()) {
 		CmdGetMessage10();
 		return;
 	}
 
 	// Get record number and block number
-	DWORD record = ctrl.cmd[2];
-	record <<= 8;
-	record |= ctrl.cmd[3];
-	record <<= 8;
-	record |= ctrl.cmd[4];
-	record <<= 8;
-	record |= ctrl.cmd[5];
-	ctrl.blocks = ctrl.cmd[7];
-	ctrl.blocks <<= 8;
-	ctrl.blocks |= ctrl.cmd[8];
-
-	// Check capacity
-	DWORD capacity = ctrl.unit[lun]->GetBlockCount();
-	if (record > capacity || record + ctrl.blocks > capacity) {
-		ostringstream s;
-		s << "ID " << GetSCSIID() << ": Media capacity of " << capacity << " blocks exceeded: "
-				<< "Trying to read block " << record << ", block count " << ctrl.blocks;
-		LOGWARN("%s", s.str().c_str());
-		Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::LBA_OUT_OF_RANGE);
+	uint64_t record;
+	if (!GetStartAndCount(record, ctrl.blocks, false)) {
 		return;
 	}
 
 	LOGTRACE("%s READ(10) command record=%d blocks=%d", __PRETTY_FUNCTION__, (unsigned int)record, (int)ctrl.blocks);
 
-	// Do not process 0 blocks
-	if (ctrl.blocks == 0) {
-		Status();
-		return;
-	}
-
 	// Command processing on drive
-	ctrl.length = ctrl.unit[lun]->Read(ctrl.cmd, ctrl.buffer, record);
+	ctrl.length = ctrl.device->Read(ctrl.cmd, ctrl.buffer, record);
 	if (ctrl.length <= 0) {
 		// Failure (Error)
 		Error();
@@ -757,58 +704,23 @@ void SCSIDEV::CmdRead10()
 //---------------------------------------------------------------------------
 void SCSIDEV::CmdRead16()
 {
-	DWORD lun = GetLun();
-
+	// TODO Move bridge specific test
 	// Receive message if host bridge
-	if (ctrl.unit[lun]->IsBridge()) {
+	if (ctrl.device->IsBridge()) {
 		Error();
 		return;
 	}
 
-	// Report an error as long as big drives are not supported
-	if (ctrl.cmd[2] || ctrl.cmd[3] || ctrl.cmd[4] || ctrl.cmd[5]) {
-		LOGWARN("Can't execute READ(16) with 64 bit sector number");
-		Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::LBA_OUT_OF_RANGE);
-		return;
-	}
-
 	// Get record number and block number
-	DWORD record = ctrl.cmd[6];
-	record <<= 8;
-	record |= ctrl.cmd[7];
-	record <<= 8;
-	record |= ctrl.cmd[8];
-	record <<= 8;
-	record |= ctrl.cmd[9];
-	ctrl.blocks = ctrl.cmd[10];
-	ctrl.blocks <<= 8;
-	ctrl.blocks |= ctrl.cmd[11];
-	ctrl.blocks <<= 8;
-	ctrl.blocks |= ctrl.cmd[12];
-	ctrl.blocks <<= 8;
-	ctrl.blocks |= ctrl.cmd[13];
-
-	// Check capacity
-	DWORD capacity = ctrl.unit[lun]->GetBlockCount();
-	if (record > capacity || record + ctrl.blocks > capacity) {
-		ostringstream s;
-		s << "ID " << GetSCSIID() << ": Media capacity of " << capacity << " blocks exceeded: "
-				<< "Trying to read block " << record << ", block count " << ctrl.blocks;
-		LOGWARN("%s", s.str().c_str());
-		Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::LBA_OUT_OF_RANGE);
+	uint64_t record;
+	if (!GetStartAndCount(record, ctrl.blocks, true)) {
 		return;
 	}
 
 	LOGTRACE("%s READ(16) command record=%d blocks=%d", __PRETTY_FUNCTION__, (unsigned int)record, (int)ctrl.blocks);
 
-	// Do not process 0 blocks
-	if (ctrl.blocks == 0) {
-		Status();
-		return;
-	}
-
 	// Command processing on drive
-	ctrl.length = ctrl.unit[lun]->Read(ctrl.cmd, ctrl.buffer, record);
+	ctrl.length = ctrl.device->Read(ctrl.cmd, ctrl.buffer, record);
 	if (ctrl.length <= 0) {
 		// Failure (Error)
 		Error();
@@ -829,47 +741,23 @@ void SCSIDEV::CmdRead16()
 //---------------------------------------------------------------------------
 void SCSIDEV::CmdWrite10()
 {
-	DWORD lun = GetLun();
-
+	// TODO Move bridge specific test
 	// Receive message with host bridge
-	if (ctrl.unit[lun]->IsBridge()) {
+	if (ctrl.device->IsBridge()) {
 		CmdSendMessage10();
 		return;
 	}
 
 	// Get record number and block number
-	DWORD record = ctrl.cmd[2];
-	record <<= 8;
-	record |= ctrl.cmd[3];
-	record <<= 8;
-	record |= ctrl.cmd[4];
-	record <<= 8;
-	record |= ctrl.cmd[5];
-	ctrl.blocks = ctrl.cmd[7];
-	ctrl.blocks <<= 8;
-	ctrl.blocks |= ctrl.cmd[8];
-
-	// Check capacity
-	DWORD capacity = ctrl.unit[lun]->GetBlockCount();
-	if (record > capacity || record + ctrl.blocks > capacity) {
-		ostringstream s;
-		s << "ID " << GetSCSIID() << ": Media capacity of " << capacity << " blocks exceeded: "
-				<< "Trying to read block " << record << ", block count " << ctrl.blocks;
-		LOGWARN("%s", s.str().c_str());
-		Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::LBA_OUT_OF_RANGE);
+	uint64_t record;
+	if (!GetStartAndCount(record, ctrl.blocks, false)) {
 		return;
 	}
 
 	LOGTRACE("%s WRITE(10) command record=%d blocks=%d",__PRETTY_FUNCTION__, (unsigned int)record, (unsigned int)ctrl.blocks);
 
-	// Do not process 0 blocks
-	if (ctrl.blocks == 0) {
-		Status();
-		return;
-	}
-
 	// Command processing on drive
-	ctrl.length = ctrl.unit[lun]->WriteCheck(record);
+	ctrl.length = ctrl.device->WriteCheck(record);
 	if (ctrl.length <= 0) {
 		// Failure (Error)
 		Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::WRITE_PROTECTED);
@@ -890,58 +778,23 @@ void SCSIDEV::CmdWrite10()
 //---------------------------------------------------------------------------
 void SCSIDEV::CmdWrite16()
 {
-	DWORD lun = GetLun();
-
+	// TODO Move bridge specific test
 	// Receive message if host bridge
-	if (ctrl.unit[lun]->IsBridge()) {
+	if (ctrl.device->IsBridge()) {
 		Error();
 		return;
 	}
 
-	// Report an error as long as big drives are not supported
-	if (ctrl.cmd[2] || ctrl.cmd[3] || ctrl.cmd[4] || ctrl.cmd[5]) {
-		LOGWARN("Can't execute WRITE(16) with 64 bit sector number");
-		Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::LBA_OUT_OF_RANGE);
-		return;
-	}
-
 	// Get record number and block number
-	DWORD record = ctrl.cmd[6];
-	record <<= 8;
-	record |= ctrl.cmd[7];
-	record <<= 8;
-	record |= ctrl.cmd[8];
-	record <<= 8;
-	record |= ctrl.cmd[9];
-	ctrl.blocks = ctrl.cmd[10];
-	ctrl.blocks <<= 8;
-	ctrl.blocks |= ctrl.cmd[11];
-	ctrl.blocks <<= 8;
-	ctrl.blocks |= ctrl.cmd[12];
-	ctrl.blocks <<= 8;
-	ctrl.blocks |= ctrl.cmd[13];
-
-	// Check capacity
-	DWORD capacity = ctrl.unit[lun]->GetBlockCount();
-	if (record > capacity || record + ctrl.blocks > capacity) {
-		ostringstream s;
-		s << "ID " << GetSCSIID() << ": Media capacity of " << capacity << " blocks exceeded: "
-				<< "Trying to read block " << record << ", block count " << ctrl.blocks;
-		LOGWARN("%s", s.str().c_str());
-		Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::LBA_OUT_OF_RANGE);
+	uint64_t record;
+	if (!GetStartAndCount(record, ctrl.blocks, true)) {
 		return;
 	}
 
 	LOGTRACE("%s WRITE(16) command record=%d blocks=%d",__PRETTY_FUNCTION__, (unsigned int)record, (unsigned int)ctrl.blocks);
 
-	// Do not process 0 blocks
-	if (ctrl.blocks == 0) {
-		Status();
-		return;
-	}
-
 	// Command processing on drive
-	ctrl.length = ctrl.unit[lun]->WriteCheck(record);
+	ctrl.length = ctrl.device->WriteCheck(record);
 	if (ctrl.length <= 0) {
 		// Failure (Error)
 		Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::WRITE_PROTECTED);
@@ -964,10 +817,8 @@ void SCSIDEV::CmdSeek10()
 {
 	LOGTRACE( "%s SEEK(10) Command ", __PRETTY_FUNCTION__);
 
-	DWORD lun = GetLun();
-
 	// Command processing on drive
-	BOOL status = ctrl.unit[lun]->Seek(ctrl.cmd);
+	bool status = ctrl.device->Seek(ctrl.cmd);
 	if (!status) {
 		// Failure (Error)
 		Error();
@@ -985,34 +836,16 @@ void SCSIDEV::CmdSeek10()
 //---------------------------------------------------------------------------
 void SCSIDEV::CmdVerify()
 {
-	BOOL status;
-
-	DWORD lun = GetLun();
-
 	// Get record number and block number
-	DWORD record = ctrl.cmd[2];
-	record <<= 8;
-	record |= ctrl.cmd[3];
-	record <<= 8;
-	record |= ctrl.cmd[4];
-	record <<= 8;
-	record |= ctrl.cmd[5];
-	ctrl.blocks = ctrl.cmd[7];
-	ctrl.blocks <<= 8;
-	ctrl.blocks |= ctrl.cmd[8];
+	uint64_t record;
+	GetStartAndCount(record, ctrl.blocks, false);
 
 	LOGTRACE("%s VERIFY command record=%08X blocks=%d",__PRETTY_FUNCTION__, (unsigned int)record, (int)ctrl.blocks);
 
-	// Do not process 0 blocks
-	if (ctrl.blocks == 0) {
-		Status();
-		return;
-	}
-
 	// if BytChk=0
 	if ((ctrl.cmd[1] & 0x02) == 0) {
 		// Command processing on drive
-		status = ctrl.unit[lun]->Seek(ctrl.cmd);
+		bool status = ctrl.device->Seek(ctrl.cmd);
 		if (!status) {
 			// Failure (Error)
 			Error();
@@ -1025,7 +858,7 @@ void SCSIDEV::CmdVerify()
 	}
 
 	// Test loading
-	ctrl.length = ctrl.unit[lun]->Read(ctrl.cmd, ctrl.buffer, record);
+	ctrl.length = ctrl.device->Read(ctrl.cmd, ctrl.buffer, record);
 	if (ctrl.length <= 0) {
 		// Failure (Error)
 		Error();
@@ -1046,9 +879,7 @@ void SCSIDEV::CmdVerify()
 //---------------------------------------------------------------------------
 void SCSIDEV::CmdReportLuns()
 {
-	DWORD lun = GetLun();
-
-	int length = ctrl.unit[lun]->ReportLuns(ctrl.cmd, ctrl.buffer);
+	int length = ctrl.device->ReportLuns(ctrl.cmd, ctrl.buffer);
 	if (length <= 0) {
 		// Failure (Error)
 		Error();
@@ -1068,8 +899,6 @@ void SCSIDEV::CmdReportLuns()
 //---------------------------------------------------------------------------
 void SCSIDEV::CmdSynchronizeCache()
 {
-	GetLun();
-
 	// Make it do something (not implemented)...
 
 	// status phase
@@ -1085,10 +914,8 @@ void SCSIDEV::CmdReadDefectData10()
 {
 	LOGTRACE( "%s READ DEFECT DATA(10) Command ", __PRETTY_FUNCTION__);
 
-	DWORD lun = GetLun();
-
 	// Command processing on drive
-	ctrl.length = ctrl.unit[lun]->ReadDefectData10(ctrl.cmd, ctrl.buffer);
+	ctrl.length = ctrl.device->ReadDefectData10(ctrl.cmd, ctrl.buffer);
 	ASSERT(ctrl.length >= 0);
 
 	if (ctrl.length <= 4) {
@@ -1107,10 +934,8 @@ void SCSIDEV::CmdReadDefectData10()
 //---------------------------------------------------------------------------
 void SCSIDEV::CmdReadToc()
 {
-	DWORD lun = GetLun();
-
 	// Command processing on drive
-	ctrl.length = ctrl.unit[lun]->ReadToc(ctrl.cmd, ctrl.buffer);
+	ctrl.length = ctrl.device->ReadToc(ctrl.cmd, ctrl.buffer);
 	if (ctrl.length <= 0) {
 		// Failure (Error)
 		Error();
@@ -1128,10 +953,8 @@ void SCSIDEV::CmdReadToc()
 //---------------------------------------------------------------------------
 void SCSIDEV::CmdPlayAudio10()
 {
-	DWORD lun = GetLun();
-
 	// Command processing on drive
-	bool status = ctrl.unit[lun]->PlayAudio(ctrl.cmd);
+	bool status = ctrl.device->PlayAudio(ctrl.cmd);
 	if (!status) {
 		// Failure (Error)
 		Error();
@@ -1149,10 +972,8 @@ void SCSIDEV::CmdPlayAudio10()
 //---------------------------------------------------------------------------
 void SCSIDEV::CmdPlayAudioMSF()
 {
-	DWORD lun = GetLun();
-
 	// Command processing on drive
-	bool status = ctrl.unit[lun]->PlayAudioMSF(ctrl.cmd);
+	bool status = ctrl.device->PlayAudioMSF(ctrl.cmd);
 	if (!status) {
 		// Failure (Error)
 		Error();
@@ -1170,10 +991,8 @@ void SCSIDEV::CmdPlayAudioMSF()
 //---------------------------------------------------------------------------
 void SCSIDEV::CmdPlayAudioTrack()
 {
-	DWORD lun = GetLun();
-
 	// Command processing on drive
-	bool status = ctrl.unit[lun]->PlayAudioTrack(ctrl.cmd);
+	bool status = ctrl.device->PlayAudioTrack(ctrl.cmd);
 	if (!status) {
 		// Failure (Error)
 		Error();
@@ -1191,8 +1010,6 @@ void SCSIDEV::CmdPlayAudioTrack()
 //---------------------------------------------------------------------------
 void SCSIDEV::CmdGetEventStatusNotification()
 {
-	GetLun();
-
 	// This naive (but legal) implementation avoids constant warnings in the logs
 	Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::INVALID_FIELD_IN_CDB);
 }
@@ -1206,10 +1023,8 @@ void SCSIDEV::CmdModeSelect10()
 {
 	LOGTRACE( "%s MODE SELECT10 Command ", __PRETTY_FUNCTION__);
 
-	DWORD lun = GetLun();
-
 	// Command processing on drive
-	ctrl.length = ctrl.unit[lun]->SelectCheck10(ctrl.cmd);
+	ctrl.length = ctrl.device->SelectCheck10(ctrl.cmd);
 	if (ctrl.length <= 0) {
 		// Failure (Error)
 		Error();
@@ -1229,10 +1044,8 @@ void SCSIDEV::CmdModeSense10()
 {
 	LOGTRACE( "%s MODE SENSE(10) Command ", __PRETTY_FUNCTION__);
 
-	DWORD lun = GetLun();
-
 	// Command processing on drive
-	ctrl.length = ctrl.unit[lun]->ModeSense10(ctrl.cmd, ctrl.buffer);
+	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]);
@@ -1253,12 +1066,9 @@ void SCSIDEV::CmdModeSense10()
 //---------------------------------------------------------------------------
 void SCSIDEV::CmdGetMessage10()
 {
-	SCSIBR *bridge;
-
-	DWORD lun = GetLun();
-
+	// TODO Move bridge specific test
 	// Error if not a host bridge
-	if (!ctrl.unit[lun]->IsBridge()) {
+	if (!ctrl.device->IsBridge()) {
 		LOGWARN("Received a GetMessage10 command for a non-bridge unit");
 		Error();
 		return;
@@ -1272,7 +1082,7 @@ void SCSIDEV::CmdGetMessage10()
 	}
 
 	// Process with drive
-	bridge = (SCSIBR*)ctrl.unit[lun];
+	SCSIBR *bridge = (SCSIBR*)ctrl.device;
 	ctrl.length = bridge->GetMessage10(ctrl.cmd, ctrl.buffer);
 
 	if (ctrl.length <= 0) {
@@ -1298,10 +1108,9 @@ void SCSIDEV::CmdGetMessage10()
 //---------------------------------------------------------------------------
 void SCSIDEV::CmdSendMessage10()
 {
-	DWORD lun = GetLun();
-
+	// TODO Move bridge specific test
 	// Error if not a host bridge
-	if (!ctrl.unit[lun]->IsBridge()) {
+	if (!ctrl.device->IsBridge()) {
 		LOGERROR("Received CmdSendMessage10 for a non-bridge device");
 		Error();
 		return;
@@ -1342,17 +1151,16 @@ void SCSIDEV::CmdSendMessage10()
 //---------------------------------------------------------------------------
 void SCSIDEV::CmdRetrieveStats()
 {
-	DWORD lun = GetLun();
-
+	// TODO Move Daynaport specific test
 	// Error if not a DaynaPort SCSI Link
-	if (!ctrl.unit[lun]->IsDaynaPort()) {
-		LOGWARN("Received a CmdRetrieveStats command for a non-daynaport unit %s", ctrl.unit[lun]->GetType().c_str());
+	if (!ctrl.device->IsDaynaPort()) {
+		LOGWARN("Received a CmdRetrieveStats command for a non-daynaport unit %s", ctrl.device->GetType().c_str());
 		Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::INVALID_COMMAND_OPERATION_CODE);
 		return;
 	}
 
 	// Process with drive
-	SCSIDaynaPort *dayna_port = (SCSIDaynaPort*)ctrl.unit[lun];
+	SCSIDaynaPort *dayna_port = (SCSIDaynaPort*)ctrl.device;
 	ctrl.length = dayna_port->RetrieveStats(ctrl.cmd, ctrl.buffer);
 
 	if (ctrl.length <= 0) {
@@ -1378,16 +1186,15 @@ void SCSIDEV::CmdSetIfaceMode()
 {
 	LOGTRACE("%s",__PRETTY_FUNCTION__);
 
-	DWORD lun = GetLun();
-
+	// TODO Move DaynaPort specific test
 	// Error if not a DaynaPort SCSI Link
-	if (!ctrl.unit[lun]->IsDaynaPort()) {
-		LOGWARN("%s Received a CmdSetIfaceMode command for a non-daynaport unit %s", __PRETTY_FUNCTION__, ctrl.unit[lun]->GetType().c_str());
+	if (!ctrl.device->IsDaynaPort()) {
+		LOGWARN("%s Received a CmdSetIfaceMode command for a non-daynaport unit %s", __PRETTY_FUNCTION__, ctrl.device->GetType().c_str());
 		Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::INVALID_COMMAND_OPERATION_CODE);
 		return;
 	}
 
-	SCSIDaynaPort *dayna_port = (SCSIDaynaPort*)ctrl.unit[lun];
+	SCSIDaynaPort *dayna_port = (SCSIDaynaPort*)ctrl.device;
 
 	// Check whether this command is telling us to "Set Interface Mode" or "Set MAC Address"
 
@@ -1417,9 +1224,8 @@ void SCSIDEV::CmdSetMcastAddr()
 {
 	LOGTRACE("%s Set Multicast Address Command ", __PRETTY_FUNCTION__);
 
-	DWORD lun = GetLun();
-
-	if (!ctrl.unit[lun]->IsDaynaPort()) {
+	// TODO Move DaynaPort specific test
+	if (!ctrl.device->IsDaynaPort()) {
 		LOGWARN("Received a SetMcastAddress command for a non-daynaport unit");
 		Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::INVALID_COMMAND_OPERATION_CODE);
 		return;
@@ -1449,19 +1255,18 @@ void SCSIDEV::CmdEnableInterface()
 {
 	LOGTRACE("%s",__PRETTY_FUNCTION__);
 
-	DWORD lun = GetLun();
-
+	// TODO Move DaynaPort specific test
 	// Error if not a DaynaPort SCSI Link
-	if (!ctrl.unit[lun]->IsDaynaPort()) {
-		LOGWARN("%s Received a CmdEnableInterface command for a non-daynaport unit %s", __PRETTY_FUNCTION__, ctrl.unit[lun]->GetType().c_str());
+	if (!ctrl.device->IsDaynaPort()) {
+		LOGWARN("%s Received a CmdEnableInterface command for a non-daynaport unit %s", __PRETTY_FUNCTION__, ctrl.device->GetType().c_str());
 		Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::INVALID_COMMAND_OPERATION_CODE);
 		return;
 	}
 
-	SCSIDaynaPort *dayna_port = (SCSIDaynaPort*)ctrl.unit[lun];
+	SCSIDaynaPort *dayna_port = (SCSIDaynaPort*)ctrl.device;
 
 	// Command processing on drive
-	BOOL status = dayna_port->EnableInterface(ctrl.cmd);
+	bool status = dayna_port->EnableInterface(ctrl.cmd);
 	if (!status) {
 		// Failure (Error)
 		Error();
@@ -1487,7 +1292,6 @@ void SCSIDEV::CmdEnableInterface()
 void SCSIDEV::Send()
 {
 	int len;
-	BOOL result;
 
 	ASSERT(!ctrl.bus->GetREQ());
 	ASSERT(ctrl.bus->GetIO());
@@ -1525,7 +1329,7 @@ void SCSIDEV::Send()
 
 	// Block subtraction, result initialization
 	ctrl.blocks--;
-	result = TRUE;
+	BOOL result = TRUE;
 
 	// Processing after data collection (read/data-in only)
 	if (ctrl.phase == BUS::datain) {
@@ -1821,3 +1625,64 @@ BOOL SCSIDEV::XferMsg(DWORD msg)
 
 	return TRUE;
 }
+
+//---------------------------------------------------------------------------
+//
+//	Get start sector and sector count for a READ/WRITE(10/16) operation
+//
+//---------------------------------------------------------------------------
+bool SCSIDEV::GetStartAndCount(uint64_t& start, uint32_t& count, bool rw64)
+{
+	start = ctrl.cmd[2];
+	start <<= 8;
+	start |= ctrl.cmd[3];
+	start <<= 8;
+	start |= ctrl.cmd[4];
+	start <<= 8;
+	start |= ctrl.cmd[5];
+	if (rw64) {
+		start <<= 8;
+		start |= ctrl.cmd[6];
+		start <<= 8;
+		start |= ctrl.cmd[7];
+		start <<= 8;
+		start |= ctrl.cmd[8];
+		start <<= 8;
+		start |= ctrl.cmd[9];
+	}
+
+	if (rw64) {
+		count = ctrl.cmd[10];
+		count <<= 8;
+		count |= ctrl.cmd[11];
+		count <<= 8;
+		count |= ctrl.cmd[12];
+		count <<= 8;
+		count |= ctrl.cmd[13];
+	}
+	else {
+		count = ctrl.cmd[7];
+		count <<= 8;
+		count |= ctrl.cmd[8];
+	}
+
+	// Check capacity
+	uint64_t capacity = ctrl.device->GetBlockCount();
+	if (start > capacity || start + count > capacity) {
+		ostringstream s;
+		s << "Media capacity of " << capacity << " blocks exceeded: "
+				<< "Trying to read block " << start << ", block count " << ctrl.blocks;
+		LOGWARN("%s", s.str().c_str());
+		Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::LBA_OUT_OF_RANGE);
+		return false;
+	}
+
+	// Do not process 0 blocks
+	if (!count) {
+		LOGTRACE("NOT processing 0 blocks");
+		Status();
+		return false;
+	}
+
+	return true;
+}
diff --git a/src/raspberrypi/controllers/scsidev_ctrl.h b/src/raspberrypi/controllers/scsidev_ctrl.h
index b2179425..0ad93eae 100644
--- a/src/raspberrypi/controllers/scsidev_ctrl.h
+++ b/src/raspberrypi/controllers/scsidev_ctrl.h
@@ -40,15 +40,21 @@ public:
 	} scsi_t;
 
 	// SCSI command name and pointer to implementation
-	typedef struct _command_t {
+	typedef struct _controller_command_t {
 		const char* name;
 		void (SCSIDEV::*execute)(void);
 
-		_command_t(const char* _name, void (SCSIDEV::*_execute)(void)) : name(_name), execute(_execute) { };
-	} command_t;
+		_controller_command_t(const char* _name, void (SCSIDEV::*_execute)(void)) : name(_name), execute(_execute) { };
+	} controller_command_t;
+	std::map<scsi_command, controller_command_t*> controller_commands;
 
-	// Mapping of SCSI opcodes to command implementations
-	std::map<scsi_command, command_t*> scsi_commands;
+	typedef struct _device_command_t {
+		const char* name;
+		void (Disk::*execute)(SCSIDEV *, SASIDEV::ctrl_t *);
+
+		_device_command_t(const char* _name, void (Disk::*_execute)(SCSIDEV *, SASIDEV::ctrl_t *)) : name(_name), execute(_execute) { };
+	} device_command_t;
+	std::map<scsi_command, device_command_t*> device_commands;
 
 public:
 	// Basic Functions
@@ -66,16 +72,18 @@ public:
 	BOOL IsSASI() const {return FALSE;}				// SASI Check
 	BOOL IsSCSI() const {return TRUE;}				// SCSI check
 
+	void Error(ERROR_CODES::sense_key sense_key = ERROR_CODES::sense_key::NO_SENSE,
+			ERROR_CODES::asc asc = ERROR_CODES::asc::NO_ADDITIONAL_SENSE_INFORMATION);	// Common erorr handling
+
 private:
-	void SetUpCommand(scsi_command, const char*, void (SCSIDEV::*)(void));
+	void SetUpControllerCommand(scsi_command, const char*, void (SCSIDEV::*)(void));
+	void SetUpDeviceCommand(scsi_command, const char*, void (Disk::*)(SCSIDEV *, SASIDEV::ctrl_t *));
 
 	// Phase
 	void BusFree();						// Bus free phase
 	void Selection();						// Selection phase
 	void Execute();						// Execution phase
 	void MsgOut();							// Message out phase
-	void Error(ERROR_CODES::sense_key sense_key = ERROR_CODES::sense_key::NO_SENSE,
-			ERROR_CODES::asc asc = ERROR_CODES::asc::NO_ADDITIONAL_SENSE_INFORMATION);	// Common erorr handling
 
 	// commands
 	void CmdInquiry();						// INQUIRY command
@@ -88,7 +96,6 @@ private:
 	void CmdStartStop();						// START STOP UNIT command
 	void CmdSendDiag();						// SEND DIAGNOSTIC command
 	void CmdRemoval();						// PREVENT/ALLOW MEDIUM REMOVAL command
-	void CmdReadCapacity10();					// READ CAPACITY(10) command
 	void CmdRead10();						// READ(10) command
 	void CmdWrite10();						// WRITE(10) command
 	void CmdSeek10();						// SEEK(10) command
@@ -102,7 +109,6 @@ private:
 	void CmdGetEventStatusNotification();
 	void CmdModeSelect10();					// MODE SELECT(10) command
 	void CmdModeSense10();						// MODE SENSE(10) command
-	void CmdReadCapacity16();					// READ CAPACITY(16) command
 	void CmdRead16();						// READ(16) command
 	void CmdWrite16();						// WRITE(16) command
 	void CmdReportLuns();					// REPORT LUNS command
@@ -117,6 +123,8 @@ private:
 	void Receive();						// Receive data
 	BOOL XferMsg(DWORD msg);					// Data transfer message
 
+	bool GetStartAndCount(uint64_t&, uint32_t&, bool);
+
 	scsi_t scsi;								// Internal data
 };
 
diff --git a/src/raspberrypi/devices/block_device.h b/src/raspberrypi/devices/block_device.h
index d6dd4813..2bc15561 100644
--- a/src/raspberrypi/devices/block_device.h
+++ b/src/raspberrypi/devices/block_device.h
@@ -11,6 +11,8 @@
 
 #pragma once
 
+#include "controllers/sasidev_ctrl.h"
+#include "controllers/scsidev_ctrl.h"
 #include "primary_device.h"
 
 class BlockDevice : public PrimaryDevice
@@ -29,8 +31,8 @@ public:
 	virtual int Read(const DWORD *cdb, BYTE *buf, DWORD block) = 0;
 	// WRITE(6), WRITE(10)
 	virtual bool Write(const DWORD *cdb, const BYTE *buf, DWORD block) = 0;
-	virtual int ReadCapacity10(const DWORD *cdb, BYTE *buf) = 0;
-	virtual int ReadCapacity16(const DWORD *cdb, BYTE *buf) = 0;
+	virtual void ReadCapacity10(SCSIDEV *, SASIDEV::ctrl_t *) = 0;
+	virtual void ReadCapacity16(SCSIDEV *, SASIDEV::ctrl_t *) = 0;
 	// TODO Uncomment as soon as there is a clean separation between controllers and devices
 	//virtual int Read16(const DWORD *cdb, BYTE *buf, DWORD block) = 0;
 	//virtual int Write16(const DWORD *cdb, BYTE *buf, DWORD block) = 0;
diff --git a/src/raspberrypi/devices/disk.cpp b/src/raspberrypi/devices/disk.cpp
index 817cd11d..785672fa 100644
--- a/src/raspberrypi/devices/disk.cpp
+++ b/src/raspberrypi/devices/disk.cpp
@@ -1563,6 +1563,12 @@ int Disk::Read(const DWORD *cdb, BYTE *buf, DWORD block)
 		return 0;
 	}
 
+	// Error if the total number of blocks is exceeded
+	if (block >= disk.blocks) {
+		SetStatusCode(STATUS_INVALIDLBA);
+		return 0;
+	}
+
 	// leave it to the cache
 	if (!disk.dcache->Read(buf, block)) {
 		SetStatusCode(STATUS_READFAULT);
@@ -1592,6 +1598,12 @@ int Disk::WriteCheck(DWORD block)
 		return 0;
 	}
 
+	// Error if the total number of blocks is exceeded
+	if (block >= disk.blocks) {
+		LOGDEBUG("WriteCheck failed (capacity exceeded)");
+		return 0;
+	}
+
 	// Error if write protected
 	if (IsProtected()) {
 		LOGDEBUG("WriteCheck failed (protected)");
@@ -1626,6 +1638,12 @@ bool Disk::Write(const DWORD *cdb, const BYTE *buf, DWORD block)
 		return false;
 	}
 
+	// Error if the total number of blocks is exceeded
+	if (block >= disk.blocks) {
+		SetStatusCode(STATUS_INVALIDLBA);
+		return false;
+	}
+
 	// Error if write protected
 	if (IsProtected()) {
 		SetStatusCode(STATUS_WRITEPROTECT);
@@ -1758,8 +1776,12 @@ bool Disk::Removal(const DWORD *cdb)
 //	READ CAPACITY
 //
 //---------------------------------------------------------------------------
-int Disk::ReadCapacity10(const DWORD* /*cdb*/, BYTE *buf)
+void Disk::ReadCapacity10(SCSIDEV *controller, SASIDEV::ctrl_t *ctrl)
 {
+	LOGTRACE( "%s READ CAPACITY(10) Command ", __PRETTY_FUNCTION__);
+
+	BYTE *buf = ctrl->buffer;
+
 	ASSERT(buf);
 
 	// Buffer clear
@@ -1767,13 +1789,16 @@ int Disk::ReadCapacity10(const DWORD* /*cdb*/, BYTE *buf)
 
 	// Status check
 	if (!CheckReady()) {
-		return 0;
+		controller->Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::MEDIUM_NOT_PRESENT);
+		return;
 	}
 
 	if (disk.blocks <= 0) {
+		controller->Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::MEDIUM_NOT_PRESENT);
+
 		LOGWARN("%s Capacity not available, medium may not be present", __PRETTY_FUNCTION__);
 
-		return -1;
+		return;
 	}
 
 	// Create end of logical block address (disk.blocks-1)
@@ -1790,12 +1815,19 @@ int Disk::ReadCapacity10(const DWORD* /*cdb*/, BYTE *buf)
 	buf[6] = (BYTE)(length >> 8);
 	buf[7] = (BYTE)length;
 
-	// return the size
-	return 8;
+	// the size
+	ctrl->length = 8;
+
+	// Data-in Phase
+	controller->DataIn();
 }
 
-int Disk::ReadCapacity16(const DWORD* /*cdb*/, BYTE *buf)
+void Disk::ReadCapacity16(SCSIDEV *controller, SASIDEV::ctrl_t *ctrl)
 {
+	LOGTRACE( "%s READ CAPACITY(16) Command ", __PRETTY_FUNCTION__);
+
+	BYTE *buf = ctrl->buffer;
+
 	ASSERT(buf);
 
 	// Buffer clear
@@ -1803,13 +1835,16 @@ int Disk::ReadCapacity16(const DWORD* /*cdb*/, BYTE *buf)
 
 	// Status check
 	if (!CheckReady()) {
-		return 0;
+		controller->Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::MEDIUM_NOT_PRESENT);
+		return;
 	}
 
 	if (disk.blocks <= 0) {
+		controller->Error(ERROR_CODES::sense_key::ILLEGAL_REQUEST, ERROR_CODES::asc::MEDIUM_NOT_PRESENT);
+
 		LOGWARN("%s Capacity not available, medium may not be present", __PRETTY_FUNCTION__);
 
-		return -1;
+		return;
 	}
 
 	// Create end of logical block address (disk.blocks-1)
@@ -1830,8 +1865,11 @@ int Disk::ReadCapacity16(const DWORD* /*cdb*/, BYTE *buf)
 	// Logical blocks per physical block: not reported (1 or more)
 	buf[13] = 0;
 
-	// return the size
-	return 14;
+	// the size
+	ctrl->length = 14;
+
+	// Data-in Phase
+	controller->DataIn();
 }
 
 //---------------------------------------------------------------------------
diff --git a/src/raspberrypi/devices/disk.h b/src/raspberrypi/devices/disk.h
index 3cfd9987..af81e73c 100644
--- a/src/raspberrypi/devices/disk.h
+++ b/src/raspberrypi/devices/disk.h
@@ -20,6 +20,8 @@
 #include "xm6.h"
 #include "log.h"
 #include "scsi.h"
+#include "controllers/sasidev_ctrl.h"
+#include "controllers/scsidev_ctrl.h"
 #include "block_device.h"
 #include "file_support.h"
 #include "filepath.h"
@@ -170,8 +172,8 @@ public:
 	bool StartStop(const DWORD *cdb);				// START STOP UNIT command
 	bool SendDiag(const DWORD *cdb);				// SEND DIAGNOSTIC command
 	bool Removal(const DWORD *cdb);				// PREVENT/ALLOW MEDIUM REMOVAL command
-	int ReadCapacity10(const DWORD *cdb, BYTE *buf) override;			// READ CAPACITY(10) command
-	int ReadCapacity16(const DWORD *cdb, BYTE *buf) override;			// READ CAPACITY(16) command
+	void ReadCapacity10(SCSIDEV *, SASIDEV::ctrl_t *) override;			// READ CAPACITY(10) command
+	void ReadCapacity16(SCSIDEV *, SASIDEV::ctrl_t *) override;			// READ CAPACITY(16) command
 	int ReportLuns(const DWORD *cdb, BYTE *buf);				// REPORT LUNS command
 	int GetSectorSize() const;
 	void SetSectorSize(int);