diff --git a/cpp/Makefile b/cpp/Makefile
index 6bf645d6..500b6cea 100644
--- a/cpp/Makefile
+++ b/cpp/Makefile
@@ -32,7 +32,7 @@ endif
 # Depending on the GCC version the compilation flags differ
 GCCVERSION10 := $(shell expr `$(CXX) -dumpversion` \>= 10)
 
-CXXFLAGS += -std=c++17 -iquote . -D_FILE_OFFSET_BITS=64 -D_LARGEFILE64_SOURCE -MD -MP -I../BPI-WiringPi2/wiringPi
+CXXFLAGS += -std=c++17 -iquote . -D_FILE_OFFSET_BITS=64 -D_LARGEFILE64_SOURCE -MD -MP
 
 ## EXTRA_FLAGS : Can be used to pass special purpose flags
 CXXFLAGS += $(EXTRA_FLAGS)
@@ -96,9 +96,7 @@ SRC_SHARED = \
 	protobuf_util.cpp \
 	protobuf_serializer.cpp
 
-SRC_RASCSI_CORE = \
-	bus.cpp
-SRC_RASCSI_CORE += $(shell find ./rascsi -name '*.cpp')
+SRC_RASCSI_CORE = $(shell find ./rascsi -name '*.cpp')
 SRC_RASCSI_CORE += $(shell find ./controllers -name '*.cpp')
 SRC_RASCSI_CORE += $(shell find ./devices -name '*.cpp')
 SRC_RASCSI_CORE += $(shell find ./hal -name '*.cpp')
@@ -107,7 +105,6 @@ SRC_RASCSI = rascsi.cpp
 
 SRC_SCSIMON = \
 	scsimon.cpp \
-	bus.cpp \
 	rascsi_version.cpp
 SRC_SCSIMON += $(shell find ./monitor -name '*.cpp')
 SRC_SCSIMON += $(shell find ./hal -name '*.cpp')
@@ -118,22 +115,20 @@ SRC_RASCTL = rasctl.cpp
 
 SRC_RASDUMP = \
 	rasdump.cpp \
-	bus.cpp \
-	rasdump_fileio.cpp \
 	rascsi_version.cpp
+SRC_RASDUMP += $(shell find ./rasdump -name '*.cpp')
 SRC_RASDUMP += $(shell find ./hal -name '*.cpp')
 
 SRC_RASCSI_TEST = $(shell find ./test -name '*.cpp')
 
 SRC_SCSILOOP= \
 	scsiloop.cpp \
-	bus.cpp \
 	rasutil.cpp \
 	rascsi_version.cpp
 SRC_SCSILOOP += $(shell find ./hal -name '*.cpp')
 
-vpath %.h ./ ./controllers ./devices ./monitor ./hal ./hal/boards ./hal/pi_defs ./rascsi ./rasctl
-vpath %.cpp ./ ./controllers ./devices ./monitor ./test ./hal ./hal/boards ./hal/pi_defs ./rascsi ./rasctl
+vpath %.h ./ ./controllers ./devices ./monitor ./hal ./hal/boards ./hal/pi_defs ./rascsi ./rasctl ./rasdump
+vpath %.cpp ./ ./controllers ./devices ./monitor ./test ./hal ./hal/boards ./hal/pi_defs ./rascsi ./rasctl ./rasdump 
 vpath %.o ./$(OBJDIR)
 vpath ./$(BINDIR)
 
@@ -197,23 +192,21 @@ docs: $(DOC_DIR)/rascsi_man_page.txt $(DOC_DIR)/rasctl_man_page.txt $(DOC_DIR)/s
 $(SRC_SHARED) $(SRC_RASCSI_CORE) $(SRC_RASCTL_CORE): $(OBJ_PROTOBUF)
 
 $(BINDIR)/$(RASCSI): $(SRC_PROTOBUF) $(OBJ_RASCSI_CORE) $(OBJ_RASCSI) $(OBJ_SHARED) $(OBJ_PROTOBUF) | $(BINDIR)
-	$(CXX) $(CXXFLAGS) -o $@ $(OBJ_RASCSI_CORE) $(OBJ_RASCSI) $(OBJ_SHARED) $(OBJ_PROTOBUF) $(LIB_BPI_WIRINGPI)  -lpthread -lpcap -lprotobuf -lstdc++fs
+	$(CXX) $(CXXFLAGS) -o $@ $(OBJ_RASCSI_CORE) $(OBJ_RASCSI) $(OBJ_SHARED) $(OBJ_PROTOBUF) -lpthread -lpcap -lprotobuf -lstdc++fs
 
 $(BINDIR)/$(RASCTL): $(SRC_PROTOBUF) $(OBJ_RASCTL_CORE) $(OBJ_RASCTL) $(OBJ_SHARED) $(OBJ_PROTOBUF) | $(BINDIR)
 	$(CXX) $(CXXFLAGS) -o $@ $(OBJ_RASCTL_CORE) $(OBJ_RASCTL) $(OBJ_SHARED) $(OBJ_PROTOBUF) -lpthread -lprotobuf
 
 $(BINDIR)/$(RASDUMP): $(OBJ_RASDUMP) | $(BINDIR)
-	$(CXX) $(CXXFLAGS) -o $@ $(OBJ_RASDUMP) $(LIB_BPI_WIRINGPI) -lpthread 
+	$(CXX) $(CXXFLAGS) -o $@ $(OBJ_RASDUMP) -lpthread 
 
 $(BINDIR)/$(SCSIMON): $(OBJ_SCSIMON) | $(BINDIR)
-	$(CXX) $(CXXFLAGS) -o $@ $(OBJ_SCSIMON) $(LIB_BPI_WIRINGPI) -lpthread
+	$(CXX) $(CXXFLAGS) -o $@ $(OBJ_SCSIMON) -lpthread
 
 $(BINDIR)/$(RASCSI_TEST): $(SRC_PROTOBUF) $(OBJ_RASCSI_CORE) $(OBJ_RASCTL_CORE) $(OBJ_RASCSI_TEST) $(OBJ_RASCTL_TEST) $(OBJ_SHARED) $(OBJ_PROTOBUF) | $(BINDIR)
-	$(CXX) $(CXXFLAGS) -o $@ $(OBJ_RASCSI_CORE) $(OBJ_RASCTL_CORE) $(OBJ_RASCSI_TEST) $(OBJ_SHARED) $(OBJ_PROTOBUF) $(LIB_BPI_WIRINGPI) -lpthread -lpcap -lprotobuf -lstdc++fs -lgmock -lgtest
+	$(CXX) $(CXXFLAGS) -o $@ $(OBJ_RASCSI_CORE) $(OBJ_RASCTL_CORE) $(OBJ_RASCSI_TEST) $(OBJ_SHARED) $(OBJ_PROTOBUF) -lpthread -lpcap -lprotobuf -lstdc++fs -lgmock -lgtest
 
 $(BINDIR)/$(SCSILOOP): $(SRC_PROTOBUF) $(OBJ_SCSILOOP) | $(BINDIR)
-	echo SCSILOOP SRC: $(SRC_SCSILOOP)
-	echo SCSILOOP OBJ:  $(OBJ_SCSILOOP)
 	$(CXX) $(CXXFLAGS) -o $@ $(OBJ_SCSILOOP) -lpthread
 
 # Phony rules for building individual utilities
diff --git a/cpp/controllers/abstract_controller.cpp b/cpp/controllers/abstract_controller.cpp
index be54f6bb..7354bce3 100644
--- a/cpp/controllers/abstract_controller.cpp
+++ b/cpp/controllers/abstract_controller.cpp
@@ -7,6 +7,7 @@
 //
 //---------------------------------------------------------------------------
 
+#include "log.h"
 #include "rascsi_exceptions.h"
 #include "devices/primary_device.h"
 #include "abstract_controller.h"
@@ -116,7 +117,7 @@ bool AbstractController::AddDevice(shared_ptr<PrimaryDevice> device)
 	}
 
 	luns[device->GetLun()] = device;
-	device->SetController(this);
+	device->SetController(shared_from_this());
 
 	return true;
 }
diff --git a/cpp/controllers/abstract_controller.h b/cpp/controllers/abstract_controller.h
index 2951a835..0b4a24f0 100644
--- a/cpp/controllers/abstract_controller.h
+++ b/cpp/controllers/abstract_controller.h
@@ -12,8 +12,9 @@
 #pragma once
 
 #include "scsi.h"
-#include "bus.h"
+#include "hal/bus.h"
 #include "phase_handler.h"
+#include "controller_manager.h"
 #include <unordered_set>
 #include <unordered_map>
 #include <vector>
@@ -23,7 +24,7 @@ using namespace std;
 
 class PrimaryDevice;
 
-class AbstractController : public PhaseHandler
+class AbstractController : public PhaseHandler, public enable_shared_from_this<AbstractController>
 {
 public:
 
@@ -34,7 +35,8 @@ public:
 		RESTART_PI
 	};
 
-	AbstractController(shared_ptr<BUS> bus, int target_id, int max_luns) : target_id(target_id), bus(bus), max_luns(max_luns) {}
+	AbstractController(shared_ptr<ControllerManager> controller_manager, int target_id, int max_luns)
+		: controller_manager(controller_manager), target_id(target_id), max_luns(max_luns) {}
 	~AbstractController() override = default;
 
 	virtual void Error(scsi_defs::sense_key, scsi_defs::asc = scsi_defs::asc::NO_ADDITIONAL_SENSE_INFORMATION,
@@ -82,7 +84,8 @@ public:
 
 protected:
 
-	inline shared_ptr<BUS> GetBus() const { return bus; }
+	shared_ptr<ControllerManager> GetControllerManager() const { return controller_manager.lock(); }
+	inline BUS& GetBus() const { return controller_manager.lock()->GetBus(); }
 
 	scsi_defs::scsi_command GetOpcode() const { return static_cast<scsi_defs::scsi_command>(ctrl.cmd[0]); }
 	int GetLun() const { return (ctrl.cmd[1] >> 5) & 0x07; }
@@ -116,13 +119,13 @@ private:
 
 	ctrl_t ctrl = {};
 
+	weak_ptr<ControllerManager> controller_manager;
+
 	// Logical units of this controller mapped to their LUN numbers
 	unordered_map<int, shared_ptr<PrimaryDevice>> luns;
 
 	int target_id;
 
-	shared_ptr<BUS> bus;
-
 	int max_luns;
 
 	bool is_byte_transfer = false;
diff --git a/cpp/controllers/controller_manager.cpp b/cpp/controllers/controller_manager.cpp
index 424e4ba6..a5cbba86 100644
--- a/cpp/controllers/controller_manager.cpp
+++ b/cpp/controllers/controller_manager.cpp
@@ -23,7 +23,7 @@ bool ControllerManager::AttachToScsiController(int id, shared_ptr<PrimaryDevice>
 
 	// If there is no LUN yet the first LUN must be LUN 0
 	if (device->GetLun() == 0) {
-		controller = make_shared<ScsiController>(bus, id);
+		controller = make_shared<ScsiController>(shared_from_this(), id);
 
 		if (controller->AddDevice(device)) {
 			controllers[id] = controller;
@@ -74,13 +74,6 @@ void ControllerManager::DeleteAllControllers()
 	controllers.clear();
 }
 
-void ControllerManager::ResetAllControllers() const
-{
-	for (const auto& [id, controller] : controllers) {
-		controller->Reset();
-	}
-}
-
 shared_ptr<PrimaryDevice> ControllerManager::GetDeviceByIdAndLun(int id, int lun) const
 {
 	if (const auto& controller = FindController(id); controller != nullptr) {
diff --git a/cpp/controllers/controller_manager.h b/cpp/controllers/controller_manager.h
index 36a28fa9..640eaade 100644
--- a/cpp/controllers/controller_manager.h
+++ b/cpp/controllers/controller_manager.h
@@ -21,26 +21,26 @@ class BUS;
 class AbstractController;
 class PrimaryDevice;
 
-class ControllerManager
+class ControllerManager : public enable_shared_from_this<ControllerManager>
 {
-	shared_ptr<BUS> bus;
+	BUS& bus;
 
 	unordered_map<int, shared_ptr<AbstractController>> controllers;
 
 public:
 
-	explicit ControllerManager(shared_ptr<BUS> bus) : bus(bus) {}
+	explicit ControllerManager(BUS& bus) : bus(bus) {}
 	~ControllerManager() = default;
 
 	// Maximum number of controller devices
 	static const int DEVICE_MAX = 8;
 
+	inline BUS& GetBus() const { return bus; }
 	bool AttachToScsiController(int, shared_ptr<PrimaryDevice>);
 	bool DeleteController(shared_ptr<AbstractController>);
 	shared_ptr<AbstractController> IdentifyController(int) const;
 	shared_ptr<AbstractController> FindController(int) const;
 	unordered_set<shared_ptr<PrimaryDevice>> GetAllDevices() const;
 	void DeleteAllControllers();
-	void ResetAllControllers() const;
 	shared_ptr<PrimaryDevice> GetDeviceByIdAndLun(int, int) const;
 };
diff --git a/cpp/controllers/scsi_controller.cpp b/cpp/controllers/scsi_controller.cpp
index 2b466ed5..e53fab05 100644
--- a/cpp/controllers/scsi_controller.cpp
+++ b/cpp/controllers/scsi_controller.cpp
@@ -28,7 +28,8 @@
 
 using namespace scsi_defs;
 
-ScsiController::ScsiController(shared_ptr<BUS> bus, int target_id) : AbstractController(bus, target_id, LUN_MAX)
+ScsiController::ScsiController(shared_ptr<ControllerManager> controller_manager, int target_id)
+	: AbstractController(controller_manager, target_id, LUN_MAX)
 {
 	// The initial buffer size will default to either the default buffer size OR
 	// the size of an Ethernet message, whichever is larger.
@@ -51,18 +52,14 @@ void ScsiController::Reset()
 
 BUS::phase_t ScsiController::Process(int id)
 {
-	// Get bus information
-	GetBus()->Acquire();
+	GetBus().Acquire();
 
-	// Check to see if the reset signal was asserted
-	if (GetBus()->GetRST()) {
+	if (GetBus().GetRST()) {
 		LOGWARN("RESET signal received!")
 
-		// Reset the controller
 		Reset();
 
-		// Reset the bus
-		GetBus()->Reset();
+		GetBus().Reset();
 
 		return GetPhase();
 	}
@@ -84,7 +81,7 @@ BUS::phase_t ScsiController::Process(int id)
 		LOGERROR("%s Unhandled SCSI error, resetting controller and bus and entering bus free phase", __PRETTY_FUNCTION__)
 
 		Reset();
-		GetBus()->Reset();
+		GetBus().Reset();
 
 		BusFree();
 	}
@@ -99,11 +96,11 @@ void ScsiController::BusFree()
 
 		SetPhase(BUS::phase_t::busfree);
 
-		GetBus()->SetREQ(false);
-		GetBus()->SetMSG(false);
-		GetBus()->SetCD(false);
-		GetBus()->SetIO(false);
-		GetBus()->SetBSY(false);
+		GetBus().SetREQ(false);
+		GetBus().SetMSG(false);
+		GetBus().SetCD(false);
+		GetBus().SetIO(false);
+		GetBus().SetBSY(false);
 
 		// Initialize status and message
 		SetStatus(status::GOOD);
@@ -116,6 +113,13 @@ void ScsiController::BusFree()
 
 		SetByteTransfer(false);
 
+		if (shutdown_mode != rascsi_shutdown_mode::NONE) {
+			// Prepare the shutdown by flushing all caches
+			for (const auto& device : GetControllerManager()->GetAllDevices()) {
+				device->FlushCache();
+			}
+		}
+
 		// When the bus is free RaSCSI or the Pi may be shut down.
 		// This code has to be executed in the bus free phase and thus has to be located in the controller.
 		switch(shutdown_mode) {
@@ -146,7 +150,7 @@ void ScsiController::BusFree()
 	}
 
 	// Move to selection phase
-	if (GetBus()->GetSEL() && !GetBus()->GetBSY()) {
+	if (GetBus().GetSEL() && !GetBus().GetBSY()) {
 		Selection();
 	}
 }
@@ -155,7 +159,7 @@ void ScsiController::Selection()
 {
 	if (!IsSelection()) {
 		// A different device controller was selected
-		if (int id = 1 << GetTargetId(); (static_cast<int>(GetBus()->GetDAT()) & id) == 0) {
+		if (int id = 1 << GetTargetId(); (static_cast<int>(GetBus().GetDAT()) & id) == 0) {
 			return;
 		}
 
@@ -169,14 +173,14 @@ void ScsiController::Selection()
 		SetPhase(BUS::phase_t::selection);
 
 		// Raise BSY and respond
-		GetBus()->SetBSY(true);
+		GetBus().SetBSY(true);
 		return;
 	}
 
 	// Selection completed
-	if (!GetBus()->GetSEL() && GetBus()->GetBSY()) {
+	if (!GetBus().GetSEL() && GetBus().GetBSY()) {
 		// Message out phase if ATN=1, otherwise command phase
-		if (GetBus()->GetATN()) {
+		if (GetBus().GetATN()) {
 			MsgOut();
 		} else {
 			Command();
@@ -191,13 +195,12 @@ void ScsiController::Command()
 
 		SetPhase(BUS::phase_t::command);
 
-		GetBus()->SetMSG(false);
-		GetBus()->SetCD(true);
-		GetBus()->SetIO(false);
+		GetBus().SetMSG(false);
+		GetBus().SetCD(true);
+		GetBus().SetIO(false);
 
-		const int actual_count = GetBus()->CommandHandShake(GetBuffer().data());
-		// TODO Try to move GetCommandByteCount() to BUS, so that the controller does not need to know GPIOBUS
-		const int command_byte_count = GPIOBUS::GetCommandByteCount(GetBuffer()[0]);
+		const int actual_count = GetBus().CommandHandShake(GetBuffer().data());
+		const int command_byte_count = BUS::GetCommandByteCount(GetBuffer()[0]);
 
 		// If not able to receive all, move to the status phase
 		if (actual_count != command_byte_count) {
@@ -225,7 +228,7 @@ void ScsiController::Command()
 
 void ScsiController::Execute()
 {
-	LOGDEBUG("++++ CMD ++++ %s Executing command $%02X", __PRETTY_FUNCTION__, static_cast<int>(GetOpcode()))
+	LOGDEBUG("++++ CMD ++++ Executing command $%02X", static_cast<int>(GetOpcode()))
 
 	// Initialization for data transfer
 	ResetOffset();
@@ -240,24 +243,25 @@ void ScsiController::Execute()
 	int lun = GetEffectiveLun();
 	if (!HasDeviceForLun(lun)) {
 		if (GetOpcode() != scsi_command::eCmdInquiry && GetOpcode() != scsi_command::eCmdRequestSense) {
-			LOGDEBUG("Invalid LUN %d for ID %d", lun, GetTargetId())
+			LOGTRACE("Invalid LUN %d for device ID %d", lun, GetTargetId())
 
 			Error(sense_key::ILLEGAL_REQUEST, asc::INVALID_LUN);
+
 			return;
 		}
-		// Use LUN 0 for INQUIRY and REQUEST SENSE because LUN0 is assumed to be always available.
-		// INQUIRY and REQUEST SENSE have a special LUN handling of their own, required by the SCSI standard.
-		else {
-			if (!HasDeviceForLun(0)) {
-				LOGERROR("No LUN 0 for device %d", GetTargetId())
 
-				GetBuffer().data()[0] = 0x7f;
+		assert(HasDeviceForLun(0));
 
-				return;
-			}
+		lun = 0;
+	}
 
-			lun = 0;
-		}
+	// SCSI-2 4.4.3 Incorrect logical unit handling
+	if (GetOpcode() == scsi_command::eCmdInquiry && !HasDeviceForLun(lun)) {
+		LOGTRACE("Reporting LUN %d for device ID %d as not supported", GetEffectiveLun(), GetTargetId())
+
+		GetBuffer().data()[0] = 0x7f;
+
+		return;
 	}
 
 	auto device = GetDeviceForLun(lun);
@@ -267,31 +271,16 @@ void ScsiController::Execute()
 		device->SetStatusCode(0);
 	}
 
-	if (!device->CheckReservation(initiator_id, GetOpcode(), GetCmd(4) & 0x01)) {
-		Error(sense_key::ABORTED_COMMAND, asc::NO_ADDITIONAL_SENSE_INFORMATION, status::RESERVATION_CONFLICT);
+	if (device->CheckReservation(initiator_id, GetOpcode(), GetCmd(4) & 0x01)) {
+		try {
+			device->Dispatch(GetOpcode());
+		}
+		catch(const scsi_exception& e) {
+			Error(e.get_sense_key(), e.get_asc());
+		}
 	}
 	else {
-		try {
-			if (!device->Dispatch(GetOpcode())) {
-				LOGTRACE("ID %d LUN %d received unsupported command: $%02X", GetTargetId(), lun, static_cast<int>(GetOpcode()))
-
-				throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_COMMAND_OPERATION_CODE);
-			}
-		}
-		catch(const scsi_exception& e) { //NOSONAR This exception is handled properly
-			Error(e.get_sense_key(), e.get_asc());
-
-			// Fall through
-		}
-	}
-
-	// SCSI-2 p.104 4.4.3 Incorrect logical unit handling
-	if (GetOpcode() == scsi_command::eCmdInquiry && !HasDeviceForLun(lun)) {
-		lun = GetEffectiveLun();
-
-		LOGTRACE("Reporting LUN %d for device ID %d as not supported", lun, device->GetId())
-
-		GetBuffer().data()[0] = 0x7f;
+		Error(sense_key::ABORTED_COMMAND, asc::NO_ADDITIONAL_SENSE_INFORMATION, status::RESERVATION_CONFLICT);
 	}
 }
 
@@ -310,9 +299,9 @@ void ScsiController::Status()
 		SetPhase(BUS::phase_t::status);
 
 		// Signal line operated by the target
-		GetBus()->SetMSG(false);
-		GetBus()->SetCD(true);
-		GetBus()->SetIO(true);
+		GetBus().SetMSG(false);
+		GetBus().SetCD(true);
+		GetBus().SetIO(true);
 
 		// Data transfer is 1 byte x 1 block
 		ResetOffset();
@@ -333,9 +322,9 @@ void ScsiController::MsgIn()
 
 		SetPhase(BUS::phase_t::msgin);
 
-		GetBus()->SetMSG(true);
-		GetBus()->SetCD(true);
-		GetBus()->SetIO(true);
+		GetBus().SetMSG(true);
+		GetBus().SetCD(true);
+		GetBus().SetIO(true);
 
 		ResetOffset();
 		return;
@@ -360,9 +349,9 @@ void ScsiController::MsgOut()
 
 		SetPhase(BUS::phase_t::msgout);
 
-		GetBus()->SetMSG(true);
-		GetBus()->SetCD(true);
-		GetBus()->SetIO(false);
+		GetBus().SetMSG(true);
+		GetBus().SetCD(true);
+		GetBus().SetIO(false);
 
 		// Data transfer is 1 byte x 1 block
 		ResetOffset();
@@ -393,9 +382,9 @@ void ScsiController::DataIn()
 
 		SetPhase(BUS::phase_t::datain);
 
-		GetBus()->SetMSG(false);
-		GetBus()->SetCD(false);
-		GetBus()->SetIO(true);
+		GetBus().SetMSG(false);
+		GetBus().SetCD(false);
+		GetBus().SetIO(true);
 
 		ResetOffset();
 
@@ -424,9 +413,9 @@ void ScsiController::DataOut()
 		SetPhase(BUS::phase_t::dataout);
 
 		// Signal line operated by the target
-		GetBus()->SetMSG(false);
-		GetBus()->SetCD(false);
-		GetBus()->SetIO(false);
+		GetBus().SetMSG(false);
+		GetBus().SetCD(false);
+		GetBus().SetIO(false);
 
 		ResetOffset();
 		return;
@@ -438,12 +427,12 @@ void ScsiController::DataOut()
 void ScsiController::Error(sense_key sense_key, asc asc, status status)
 {
 	// Get bus information
-	GetBus()->Acquire();
+	GetBus().Acquire();
 
 	// Reset check
-	if (GetBus()->GetRST()) {
+	if (GetBus().GetRST()) {
 		Reset();
-		GetBus()->Reset();
+		GetBus().Reset();
 
 		return;
 	}
@@ -487,16 +476,16 @@ void ScsiController::Error(sense_key sense_key, asc asc, status status)
 
 void ScsiController::Send()
 {
-	assert(!GetBus()->GetREQ());
-	assert(GetBus()->GetIO());
+	assert(!GetBus().GetREQ());
+	assert(GetBus().GetIO());
 
 	if (HasValidLength()) {
 		LOGTRACE("%s%s", __PRETTY_FUNCTION__, (" Sending handhake with offset " + to_string(GetOffset()) + ", length "
 				+ to_string(GetLength())).c_str())
 
-		// TODO The delay has to be taken from ctrl.unit[lun], but as there are currently no Daynaport drivers for
+		// The delay should be taken from the respective LUN, but as there are no Daynaport drivers for
 		// LUNs other than 0 this work-around works.
-		if (const int len = GetBus()->SendHandShake(GetBuffer().data() + GetOffset(), GetLength(),
+		if (const int len = GetBus().SendHandShake(GetBuffer().data() + GetOffset(), GetLength(),
 				HasDeviceForLun(0) ? GetDeviceForLun(0)->GetSendDelay() : 0);
 			len != static_cast<int>(GetLength())) {
 			// If you cannot send all, move to status phase
@@ -583,15 +572,15 @@ void ScsiController::Receive()
 	LOGTRACE("%s",__PRETTY_FUNCTION__)
 
 	// REQ is low
-	assert(!GetBus()->GetREQ());
-	assert(!GetBus()->GetIO());
+	assert(!GetBus().GetREQ());
+	assert(!GetBus().GetIO());
 
 	// Length != 0 if received
 	if (HasValidLength()) {
 		LOGTRACE("%s Length is %d bytes", __PRETTY_FUNCTION__, GetLength())
 
 		// If not able to receive all, move to status phase
-		if (int len = GetBus()->ReceiveHandShake(GetBuffer().data() + GetOffset(), GetLength());
+		if (int len = GetBus().ReceiveHandShake(GetBuffer().data() + GetOffset(), GetLength());
 			len != static_cast<int>(GetLength())) {
 			LOGERROR("%s Not able to receive %d bytes of data, only received %d",__PRETTY_FUNCTION__, GetLength(), len)
 			Error(sense_key::ABORTED_COMMAND);
@@ -688,14 +677,14 @@ bool ScsiController::XferMsg(int msg)
 
 void ScsiController::ReceiveBytes()
 {
-	assert(!GetBus()->GetREQ());
-	assert(!GetBus()->GetIO());
+	assert(!GetBus().GetREQ());
+	assert(!GetBus().GetIO());
 
 	if (HasValidLength()) {
 		LOGTRACE("%s Length is %d bytes", __PRETTY_FUNCTION__, GetLength())
 
 		// If not able to receive all, move to status phase
-		if (uint32_t len = GetBus()->ReceiveHandShake(GetBuffer().data() + GetOffset(), GetLength()); len != GetLength()) {
+		if (uint32_t len = GetBus().ReceiveHandShake(GetBuffer().data() + GetOffset(), GetLength()); len != GetLength()) {
 			LOGERROR("%s Not able to receive %d bytes of data, only received %d",
 					__PRETTY_FUNCTION__, GetLength(), len)
 			Error(sense_key::ABORTED_COMMAND);
@@ -850,7 +839,6 @@ bool ScsiController::XferIn(vector<uint8_t>& buf)
 			ResetOffset();
 			break;
 
-		// Other (impossible)
 		default:
 			assert(false);
 			return false;
@@ -929,14 +917,7 @@ bool ScsiController::XferOutBlockOriented(bool cont)
 				break;
 			}
 
-			// Check the next block
-			try {
-				SetLength(disk->WriteCheck(GetNext() - 1));
-			}
-			catch(const scsi_exception&) {
-				return false;
-			}
-
+			SetLength(disk->GetSectorSizeInBytes());
 			ResetOffset();
 			break;
 		}
@@ -1037,7 +1018,7 @@ void ScsiController::ParseMessage()
 void ScsiController::ProcessMessage()
 {
 	// Continue message out phase as long as ATN keeps asserting
-	if (GetBus()->GetATN()) {
+	if (GetBus().GetATN()) {
 		// Data transfer is 1 byte x 1 block
 		ResetOffset();
 		SetLength(1);
diff --git a/cpp/controllers/scsi_controller.h b/cpp/controllers/scsi_controller.h
index 8616b796..b1167540 100644
--- a/cpp/controllers/scsi_controller.h
+++ b/cpp/controllers/scsi_controller.h
@@ -14,6 +14,7 @@
 
 #pragma once
 
+#include "controller_manager.h"
 #include "abstract_controller.h"
 #include "scsi.h"
 #include <array>
@@ -53,7 +54,7 @@ public:
 	// Maximum number of logical units
 	static inline const int LUN_MAX = 32;
 
-	ScsiController(shared_ptr<BUS>, int);
+	explicit ScsiController(shared_ptr<ControllerManager>, int);
 	~ScsiController() override = default;
 
 	void Reset() override;
diff --git a/cpp/devices/ctapdriver.cpp b/cpp/devices/ctapdriver.cpp
index 236c4a48..02f24a27 100644
--- a/cpp/devices/ctapdriver.cpp
+++ b/cpp/devices/ctapdriver.cpp
@@ -129,19 +129,6 @@ bool CTapDriver::Init(const unordered_map<string, string>& const_params)
 	return false;
 #else
 	unordered_map<string, string> params = const_params;
-	if (params.count("interfaces")) {
-		LOGWARN("You are using the deprecated 'interfaces' parameter. "
-				"Provide the interface list and the IP address/netmask with the 'interface' and 'inet' parameters")
-
-		// TODO Remove the deprecated syntax in a future version
-		const string& ifaces = params["interfaces"];
-		size_t separatorPos = ifaces.find(':');
-		if (separatorPos != string::npos) {
-			params["interface"] = ifaces.substr(0, separatorPos);
-			params["inet"] = ifaces.substr(separatorPos + 1);
-		}
-	}
-
 	stringstream s(params["interface"]);
 	string interface;
 	while (getline(s, interface, ',')) {
diff --git a/cpp/devices/device.cpp b/cpp/devices/device.cpp
index 7a68e06a..8a329409 100644
--- a/cpp/devices/device.cpp
+++ b/cpp/devices/device.cpp
@@ -90,6 +90,11 @@ void Device::SetParams(const unordered_map<string, string>& set_params)
 {
 	params = default_params;
 
+	// Devices with image file support implicitly support the "file" parameter
+	if (SupportsFile()) {
+		params["file"] = "";
+	}
+
 	for (const auto& [key, value] : set_params) {
 		// It is assumed that there are default parameters for all supported parameters
 		if (params.find(key) != params.end()) {
diff --git a/cpp/devices/device.h b/cpp/devices/device.h
index 5a8fd383..cb403ef4 100644
--- a/cpp/devices/device.h
+++ b/cpp/devices/device.h
@@ -64,11 +64,10 @@ class Device //NOSONAR The number of fields and methods is justified, the comple
 	// The default parameters
 	unordered_map<string, string> default_params;
 
-	// Sense Key, ASC and ASCQ
+	// Sense Key and ASC
 	//	MSB		Reserved (0x00)
 	//			Sense Key
 	//			Additional Sense Code (ASC)
-	//	LSB		Additional Sense Code Qualifier(ASCQ)
 	int status_code = 0;
 
 protected:
diff --git a/cpp/devices/device_factory.cpp b/cpp/devices/device_factory.cpp
index 52d66697..5706035c 100644
--- a/cpp/devices/device_factory.cpp
+++ b/cpp/devices/device_factory.cpp
@@ -14,7 +14,6 @@
 #include "scsi_printer.h"
 #include "scsi_host_bridge.h"
 #include "scsi_daynaport.h"
-#include "rascsi_exceptions.h"
 #include "host_services.h"
 #include "rasutil.h"
 #include "device_factory.h"
@@ -22,6 +21,7 @@
 #include <sys/ioctl.h>
 #include <netinet/in.h>
 #include <net/if.h>
+#include <unistd.h>
 
 using namespace std;
 using namespace rascsi_interface;
@@ -77,8 +77,7 @@ PbDeviceType DeviceFactory::GetTypeForFile(const string& filename) const
 	return UNDEFINED;
 }
 
-shared_ptr<PrimaryDevice> DeviceFactory::CreateDevice(const ControllerManager& controller_manager, PbDeviceType type,
-		int lun, const string& filename)
+shared_ptr<PrimaryDevice> DeviceFactory::CreateDevice(PbDeviceType type, int lun, const string& filename) const
 {
 	// If no type was specified try to derive the device type from the filename
 	if (type == UNDEFINED) {
@@ -94,7 +93,7 @@ shared_ptr<PrimaryDevice> DeviceFactory::CreateDevice(const ControllerManager& c
 		if (const string ext = GetExtensionLowerCase(filename); ext == "hdn" || ext == "hdi" || ext == "nhd") {
 			device = make_shared<SCSIHD_NEC>(lun);
 		} else {
-			device = make_shared<SCSIHD>(lun, sector_sizes[SCHD], false,
+			device = make_shared<SCSIHD>(lun, sector_sizes.find(SCHD)->second, false,
 					ext == "hd1" ? scsi_level::SCSI_1_CCS : scsi_level::SCSI_2);
 
 			// Some Apple tools require a particular drive identification
@@ -107,17 +106,17 @@ shared_ptr<PrimaryDevice> DeviceFactory::CreateDevice(const ControllerManager& c
 	}
 
 	case SCRM:
-		device = make_shared<SCSIHD>(lun, sector_sizes[SCRM], true);
+		device = make_shared<SCSIHD>(lun, sector_sizes.find(SCRM)->second, true);
 		device->SetProduct("SCSI HD (REM.)");
 		break;
 
 	case SCMO:
-		device = make_shared<SCSIMO>(lun, sector_sizes[SCMO]);
+		device = make_shared<SCSIMO>(lun, sector_sizes.find(SCMO)->second);
 		device->SetProduct("SCSI MO");
 		break;
 
 	case SCCD:
-		device = make_shared<SCSICD>(lun, sector_sizes[SCCD]);
+		device = make_shared<SCSICD>(lun, sector_sizes.find(SCCD)->second);
 		device->SetProduct("SCSI CD-ROM");
 		break;
 
@@ -125,7 +124,7 @@ shared_ptr<PrimaryDevice> DeviceFactory::CreateDevice(const ControllerManager& c
 		device = make_shared<SCSIBR>(lun);
 		// Since this is an emulation for a specific driver the product name has to be set accordingly
 		device->SetProduct("RASCSI BRIDGE");
-		device->SetDefaultParams(default_params[SCBR]);
+		device->SetDefaultParams(default_params.find(SCBR)->second);
 		break;
 
 	case SCDP:
@@ -134,11 +133,11 @@ shared_ptr<PrimaryDevice> DeviceFactory::CreateDevice(const ControllerManager& c
 		device->SetVendor("Dayna");
 		device->SetProduct("SCSI/Link");
 		device->SetRevision("1.4a");
-		device->SetDefaultParams(default_params[SCDP]);
+		device->SetDefaultParams(default_params.find(SCDP)->second);
 		break;
 
 	case SCHS:
-		device = make_shared<HostServices>(lun, controller_manager);
+		device = make_shared<HostServices>(lun);
 		// Since this is an emulation for a specific device the full INQUIRY data have to be set accordingly
 		device->SetVendor("RaSCSI");
 		device->SetProduct("Host Services");
@@ -147,7 +146,7 @@ shared_ptr<PrimaryDevice> DeviceFactory::CreateDevice(const ControllerManager& c
 	case SCLP:
 		device = make_shared<SCSIPrinter>(lun);
 		device->SetProduct("SCSI PRINTER");
-		device->SetDefaultParams(default_params[SCLP]);
+		device->SetDefaultParams(default_params.find(SCLP)->second);
 		break;
 
 	default:
diff --git a/cpp/devices/device_factory.h b/cpp/devices/device_factory.h
index 43ac0bd3..08287952 100644
--- a/cpp/devices/device_factory.h
+++ b/cpp/devices/device_factory.h
@@ -5,7 +5,7 @@
 //
 // Copyright (C) 2021-2022 Uwe Seimet
 //
-// The DeviceFactory singleton creates devices based on their type and the image file extension
+// The DeviceFactory creates devices based on their type and the image file extension
 //
 //---------------------------------------------------------------------------
 
@@ -20,19 +20,18 @@
 using namespace std;
 using namespace rascsi_interface;
 
-class ControllerManager;
 class PrimaryDevice;
 
 class DeviceFactory
 {
-	const string DEFAULT_IP = "10.10.20.1/24"; //NOSONAR This hardcoded IP address is safe
+	const inline static string DEFAULT_IP = "10.10.20.1/24"; //NOSONAR This hardcoded IP address is safe
 
 public:
 
 	DeviceFactory();
 	~DeviceFactory() = default;
 
-	shared_ptr<PrimaryDevice> CreateDevice(const ControllerManager&, PbDeviceType, int, const string&);
+	shared_ptr<PrimaryDevice> CreateDevice(PbDeviceType, int, const string&) const;
 	PbDeviceType GetTypeForFile(const string&) const;
 	const unordered_set<uint32_t>& GetSectorSizes(PbDeviceType type) const;
 	const unordered_map<string, string>& GetDefaultParams(PbDeviceType type) const;
diff --git a/cpp/devices/disk.cpp b/cpp/devices/disk.cpp
index 0741ddfc..95285931 100644
--- a/cpp/devices/disk.cpp
+++ b/cpp/devices/disk.cpp
@@ -14,45 +14,14 @@
 //
 //---------------------------------------------------------------------------
 
+#include "log.h"
 #include "rascsi_exceptions.h"
-#include "dispatcher.h"
 #include "scsi_command_util.h"
 #include "disk.h"
 
 using namespace scsi_defs;
 using namespace scsi_command_util;
 
-const unordered_map<uint32_t, uint32_t> Disk::shift_counts = { { 512, 9 }, { 1024, 10 }, { 2048, 11 }, { 4096, 12 } };
-
-Disk::Disk(PbDeviceType type, int lun) : StorageDevice(type, lun)
-{
-	// REZERO implementation is identical with Seek
-	dispatcher.Add(scsi_command::eCmdRezero, "Rezero", &Disk::Seek);
-	dispatcher.Add(scsi_command::eCmdFormat, "FormatUnit", &Disk::FormatUnit);
-	// REASSIGN BLOCKS implementation is identical with Seek
-	dispatcher.Add(scsi_command::eCmdReassign, "ReassignBlocks", &Disk::Seek);
-	dispatcher.Add(scsi_command::eCmdRead6, "Read6", &Disk::Read6);
-	dispatcher.Add(scsi_command::eCmdWrite6, "Write6", &Disk::Write6);
-	dispatcher.Add(scsi_command::eCmdSeek6, "Seek6", &Disk::Seek6);
-	dispatcher.Add(scsi_command::eCmdStartStop, "StartStopUnit", &Disk::StartStopUnit);
-	dispatcher.Add(scsi_command::eCmdRemoval, "PreventAllowMediumRemoval", &Disk::PreventAllowMediumRemoval);
-	dispatcher.Add(scsi_command::eCmdReadCapacity10, "ReadCapacity10", &Disk::ReadCapacity10);
-	dispatcher.Add(scsi_command::eCmdRead10, "Read10", &Disk::Read10);
-	dispatcher.Add(scsi_command::eCmdWrite10, "Write10", &Disk::Write10);
-	dispatcher.Add(scsi_command::eCmdReadLong10, "ReadLong10", &Disk::ReadWriteLong10);
-	dispatcher.Add(scsi_command::eCmdWriteLong10, "WriteLong10", &Disk::ReadWriteLong10);
-	dispatcher.Add(scsi_command::eCmdWriteLong16, "WriteLong16", &Disk::ReadWriteLong16);
-	dispatcher.Add(scsi_command::eCmdSeek10, "Seek10", &Disk::Seek10);
-	dispatcher.Add(scsi_command::eCmdVerify10, "Verify10", &Disk::Verify10);
-	dispatcher.Add(scsi_command::eCmdSynchronizeCache10, "SynchronizeCache10", &Disk::SynchronizeCache);
-	dispatcher.Add(scsi_command::eCmdSynchronizeCache16, "SynchronizeCache16", &Disk::SynchronizeCache);
-	dispatcher.Add(scsi_command::eCmdReadDefectData10, "ReadDefectData10", &Disk::ReadDefectData10);
-	dispatcher.Add(scsi_command::eCmdRead16, "Read16", &Disk::Read16);
-	dispatcher.Add(scsi_command::eCmdWrite16, "Write16", &Disk::Write16);
-	dispatcher.Add(scsi_command::eCmdVerify16, "Verify16", &Disk::Verify16);
-	dispatcher.Add(scsi_command::eCmdReadCapacity16_ReadLong16, "ReadCapacity16/ReadLong16", &Disk::ReadCapacity16_ReadLong16);
-}
-
 Disk::~Disk()
 {
 	// Save disk cache, only if ready
@@ -61,7 +30,40 @@ Disk::~Disk()
 	}
 }
 
-bool Disk::Dispatch(scsi_command cmd)
+bool Disk::Init(const unordered_map<string, string>& params)
+{
+	StorageDevice::Init(params);
+
+	// REZERO implementation is identical with Seek
+	AddCommand(scsi_command::eCmdRezero, [this] { Seek(); });
+	AddCommand(scsi_command::eCmdFormatUnit, [this] { FormatUnit(); });
+	// REASSIGN BLOCKS implementation is identical with Seek
+	AddCommand(scsi_command::eCmdReassignBlocks, [this] { Seek(); });
+	AddCommand(scsi_command::eCmdRead6, [this] { Read6(); });
+	AddCommand(scsi_command::eCmdWrite6, [this] { Write6(); });
+	AddCommand(scsi_command::eCmdSeek6, [this] { Seek6(); });
+	AddCommand(scsi_command::eCmdStartStop, [this] { StartStopUnit(); });
+	AddCommand(scsi_command::eCmdPreventAllowMediumRemoval, [this]{ PreventAllowMediumRemoval(); });
+	AddCommand(scsi_command::eCmdReadCapacity10, [this] { ReadCapacity10(); });
+	AddCommand(scsi_command::eCmdRead10, [this] { Read10(); });
+	AddCommand(scsi_command::eCmdWrite10, [this] { Write10(); });
+	AddCommand(scsi_command::eCmdReadLong10, [this] { ReadWriteLong10(); });
+	AddCommand(scsi_command::eCmdWriteLong10, [this] { ReadWriteLong10(); });
+	AddCommand(scsi_command::eCmdWriteLong16, [this] { ReadWriteLong16(); });
+	AddCommand(scsi_command::eCmdSeek10, [this] { Seek10(); });
+	AddCommand(scsi_command::eCmdVerify10, [this] { Verify10(); });
+	AddCommand(scsi_command::eCmdSynchronizeCache10, [this] { SynchronizeCache(); });
+	AddCommand(scsi_command::eCmdSynchronizeCache16, [this] { SynchronizeCache(); });
+	AddCommand(scsi_command::eCmdReadDefectData10, [this] { ReadDefectData10(); });
+	AddCommand(scsi_command::eCmdRead16,[this] { Read16(); });
+	AddCommand(scsi_command::eCmdWrite16, [this] { Write16(); });
+	AddCommand(scsi_command::eCmdVerify16, [this] { Verify16(); });
+	AddCommand(scsi_command::eCmdReadCapacity16_ReadLong16, [this] { ReadCapacity16_ReadLong16(); });
+
+	return true;
+}
+
+void Disk::Dispatch(scsi_command cmd)
 {
 	// Media changes must be reported on the next access, i.e. not only for TEST UNIT READY
 	if (IsMediumChanged()) {
@@ -69,11 +71,11 @@ bool Disk::Dispatch(scsi_command cmd)
 
 		SetMediumChanged(false);
 
-		throw scsi_exception(sense_key::UNIT_ATTENTION, asc::NOT_READY_TO_READY_CHANGE);
+		GetController()->Error(sense_key::UNIT_ATTENTION, asc::NOT_READY_TO_READY_CHANGE);
+	}
+	else {
+		PrimaryDevice::Dispatch(cmd);
 	}
-
-	// The superclass handles the less specific commands
-	return dispatcher.Dispatch(this, cmd) ? true : super::Dispatch(cmd);
 }
 
 void Disk::SetUpCache(off_t image_offset, bool raw)
@@ -100,7 +102,7 @@ void Disk::FormatUnit()
 	CheckReady();
 
 	// FMTDATA=1 is not supported (but OK if there is no DEFECT LIST)
-	if ((controller->GetCmd(1) & 0x10) != 0 && controller->GetCmd(4) != 0) {
+	if ((GetController()->GetCmd(1) & 0x10) != 0 && GetController()->GetCmd(4) != 0) {
 		throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_FIELD_IN_CDB);
 	}
 
@@ -109,15 +111,15 @@ void Disk::FormatUnit()
 
 void Disk::Read(access_mode mode)
 {
-	uint64_t start;
-	uint32_t blocks;
-	if (CheckAndGetStartAndCount(start, blocks, mode)) {
-		controller->SetBlocks(blocks);
-		controller->SetLength(Read(controller->GetCmd(), controller->GetBuffer(), start));
-		LOGTRACE("%s ctrl.length is %d", __PRETTY_FUNCTION__, controller->GetLength())
+	const auto& [valid, start, blocks] = CheckAndGetStartAndCount(mode);
+	if (valid) {
+		GetController()->SetBlocks(blocks);
+		GetController()->SetLength(Read(GetController()->GetCmd(), GetController()->GetBuffer(), start));
+
+		LOGTRACE("%s ctrl.length is %d", __PRETTY_FUNCTION__, GetController()->GetLength())
 
 		// Set next block
-		controller->SetNext(start + 1);
+		GetController()->SetNext(start + 1);
 
 		EnterDataInPhase();
 	}
@@ -126,40 +128,43 @@ void Disk::Read(access_mode mode)
 	}
 }
 
-void Disk::ReadWriteLong10()
+void Disk::ReadWriteLong10() const
 {
-	// Transfer lengths other than 0 are not supported, which is compliant with the SCSI standard
-	if (GetInt16(controller->GetCmd(), 7) != 0) {
-		throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_FIELD_IN_CDB);
-	}
-
 	ValidateBlockAddress(RW10);
 
-	EnterStatusPhase();
-}
-
-void Disk::ReadWriteLong16()
-{
 	// Transfer lengths other than 0 are not supported, which is compliant with the SCSI standard
-	if (GetInt16(controller->GetCmd(), 12) != 0) {
+	if (GetInt16(GetController()->GetCmd(), 7) != 0) {
 		throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_FIELD_IN_CDB);
 	}
 
+	EnterStatusPhase();
+}
+
+void Disk::ReadWriteLong16() const
+{
 	ValidateBlockAddress(RW16);
 
+	// Transfer lengths other than 0 are not supported, which is compliant with the SCSI standard
+	if (GetInt16(GetController()->GetCmd(), 12) != 0) {
+		throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_FIELD_IN_CDB);
+	}
+
 	EnterStatusPhase();
 }
 
-void Disk::Write(access_mode mode)
+void Disk::Write(access_mode mode) const
 {
-	uint64_t start;
-	uint32_t blocks;
-	if (CheckAndGetStartAndCount(start, blocks, mode)) {
-		controller->SetBlocks(blocks);
-		controller->SetLength(WriteCheck(start));
+	if (IsProtected()) {
+		throw scsi_exception(sense_key::DATA_PROTECT, asc::WRITE_PROTECTED);
+	}
+
+	const auto& [valid, start, blocks] = CheckAndGetStartAndCount(mode);
+	if (valid) {
+		GetController()->SetBlocks(blocks);
+		GetController()->SetLength(GetSectorSizeInBytes());
 
 		// Set next block
-		controller->SetNext(start + 1);
+		GetController()->SetNext(start + 1);
 
 		EnterDataOutPhase();
 	}
@@ -170,21 +175,20 @@ void Disk::Write(access_mode mode)
 
 void Disk::Verify(access_mode mode)
 {
-	uint64_t start;
-	uint32_t blocks;
-	if (CheckAndGetStartAndCount(start, blocks, mode)) {
+	const auto& [valid, start, blocks] = CheckAndGetStartAndCount(mode);
+	if (valid) {
 		// if BytChk=0
-		if ((controller->GetCmd(1) & 0x02) == 0) {
+		if ((GetController()->GetCmd(1) & 0x02) == 0) {
 			Seek();
 			return;
 		}
 
 		// Test reading
-		controller->SetBlocks(blocks);
-		controller->SetLength(Read(controller->GetCmd(), controller->GetBuffer(), start));
+		GetController()->SetBlocks(blocks);
+		GetController()->SetLength(Read(GetController()->GetCmd(), GetController()->GetBuffer(), start));
 
 		// Set next block
-		controller->SetNext(start + 1);
+		GetController()->SetNext(start + 1);
 
 		EnterDataOutPhase();
 	}
@@ -195,8 +199,8 @@ void Disk::Verify(access_mode mode)
 
 void Disk::StartStopUnit()
 {
-	const bool start = controller->GetCmd(4) & 0x01;
-	const bool load = controller->GetCmd(4) & 0x02;
+	const bool start = GetController()->GetCmd(4) & 0x01;
+	const bool load = GetController()->GetCmd(4) & 0x02;
 
 	if (load) {
 		LOGTRACE(start ? "Loading medium" : "Ejecting medium")
@@ -232,7 +236,7 @@ void Disk::PreventAllowMediumRemoval()
 {
 	CheckReady();
 
-	const bool lock = controller->GetCmd(4) & 0x01;
+	const bool lock = GetController()->GetCmd(4) & 0x01;
 
 	LOGTRACE(lock ? "Locking medium" : "Unlocking medium")
 
@@ -248,20 +252,21 @@ void Disk::SynchronizeCache()
 	EnterStatusPhase();
 }
 
-void Disk::ReadDefectData10()
+void Disk::ReadDefectData10() const
 {
-	const size_t allocation_length = min(static_cast<size_t>(GetInt16(controller->GetCmd(), 7)), static_cast<size_t>(4));
+	const size_t allocation_length = min(static_cast<size_t>(GetInt16(GetController()->GetCmd(), 7)),
+			static_cast<size_t>(4));
 
 	// The defect list is empty
-	fill_n(controller->GetBuffer().begin(), allocation_length, 0);
-	controller->SetLength(static_cast<uint32_t>(allocation_length));
+	fill_n(GetController()->GetBuffer().begin(), allocation_length, 0);
+	GetController()->SetLength(static_cast<uint32_t>(allocation_length));
 
 	EnterDataInPhase();
 }
 
 bool Disk::Eject(bool force)
 {
-	const bool status = super::Eject(force);
+	const bool status = PrimaryDevice::Eject(force);
 	if (status) {
 		FlushCache();
 		cache.reset();
@@ -389,11 +394,8 @@ void Disk::SetUpModePages(map<int, vector<byte>>& pages, int page, bool changeab
 
 void Disk::AddErrorPage(map<int, vector<byte>>& pages, bool) const
 {
-	vector<byte> buf(12);
-
 	// Retry count is 0, limit time uses internal default value
-
-	pages[1] = buf;
+	pages[1] = vector<byte>(12);
 }
 
 void Disk::AddFormatPage(map<int, vector<byte>>& pages, bool changeable) const
@@ -491,16 +493,10 @@ void Disk::AddCachePage(map<int, vector<byte>>& pages, bool changeable) const
 
 int Disk::Read(const vector<int>&, vector<uint8_t>& buf, uint64_t block)
 {
-	LOGTRACE("%s", __PRETTY_FUNCTION__)
+	assert(block < GetBlockCount());
 
 	CheckReady();
 
-	// Error if the total number of blocks is exceeded
-	if (block >= GetBlockCount()) {
-		 throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_FIELD_IN_CDB);
-	}
-
-	// leave it to the cache
 	if (!cache->ReadSector(buf, static_cast<uint32_t>(block))) {
 		throw scsi_exception(sense_key::MEDIUM_ERROR, asc::READ_FAULT);
 	}
@@ -508,40 +504,12 @@ int Disk::Read(const vector<int>&, vector<uint8_t>& buf, uint64_t block)
 	return GetSectorSizeInBytes();
 }
 
-int Disk::WriteCheck(uint64_t block)
-{
-	CheckReady();
-
-	// Error if the total number of blocks is exceeded
-	if (block >= GetBlockCount()) {
-		throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_FIELD_IN_CDB);
-	}
-
-	// Error if write protected
-	if (IsProtected()) {
-		throw scsi_exception(sense_key::DATA_PROTECT, asc::WRITE_PROTECTED);
-	}
-
-	return GetSectorSizeInBytes();
-}
-
 void Disk::Write(const vector<int>&, const vector<uint8_t>& buf, uint64_t block)
 {
-	LOGTRACE("%s", __PRETTY_FUNCTION__)
+	assert(block < GetBlockCount());
 
 	CheckReady();
 
-	// Error if the total number of blocks is exceeded
-	if (block >= GetBlockCount()) {
-		throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::LBA_OUT_OF_RANGE);
-	}
-
-	// Error if write protected
-	if (IsProtected()) {
-		throw scsi_exception(sense_key::DATA_PROTECT, asc::WRITE_PROTECTED);
-	}
-
-	// Leave it to the cache
 	if (!cache->WriteSector(buf, static_cast<uint32_t>(block))) {
 		throw scsi_exception(sense_key::MEDIUM_ERROR, asc::WRITE_FAULT);
 	}
@@ -556,8 +524,8 @@ void Disk::Seek()
 
 void Disk::Seek6()
 {
-	uint64_t start;
-	if (uint32_t blocks; CheckAndGetStartAndCount(start, blocks, SEEK6)) {
+	const auto& [valid, start, blocks] = CheckAndGetStartAndCount(SEEK6);
+	if (valid) {
 		CheckReady();
 	}
 
@@ -566,8 +534,8 @@ void Disk::Seek6()
 
 void Disk::Seek10()
 {
-	uint64_t start;
-	if (uint32_t blocks; CheckAndGetStartAndCount(start, blocks, SEEK10)) {
+	const auto& [valid, start, blocks] = CheckAndGetStartAndCount(SEEK10);
+	if (valid) {
 		CheckReady();
 	}
 
@@ -582,7 +550,7 @@ void Disk::ReadCapacity10()
 		throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::MEDIUM_NOT_PRESENT);
 	}
 
-	vector<uint8_t>& buf = controller->GetBuffer();
+	vector<uint8_t>& buf = GetController()->GetBuffer();
 
 	// Create end of logical block address (blocks-1)
 	uint64_t capacity = GetBlockCount() - 1;
@@ -596,8 +564,7 @@ void Disk::ReadCapacity10()
 	// Create block length (1 << size)
 	SetInt32(buf, 4, 1 << size_shift_count);
 
-	// the size
-	controller->SetLength(8);
+	GetController()->SetLength(8);
 
 	EnterDataInPhase();
 }
@@ -610,7 +577,7 @@ void Disk::ReadCapacity16()
 		throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::MEDIUM_NOT_PRESENT);
 	}
 
-	vector<uint8_t>& buf = controller->GetBuffer();
+	vector<uint8_t>& buf = GetController()->GetBuffer();
 
 	// Create end of logical block address (blocks-1)
 	SetInt64(buf, 0, GetBlockCount() - 1);
@@ -623,8 +590,7 @@ void Disk::ReadCapacity16()
 	// Logical blocks per physical block: not reported (1 or more)
 	buf[13] = 0;
 
-	// the size
-	controller->SetLength(14);
+	GetController()->SetLength(14);
 
 	EnterDataInPhase();
 }
@@ -632,7 +598,7 @@ void Disk::ReadCapacity16()
 void Disk::ReadCapacity16_ReadLong16()
 {
 	// The service action determines the actual command
-	switch (controller->GetCmd(1) & 0x1f) {
+	switch (GetController()->GetCmd(1) & 0x1f) {
 	case 0x10:
 		ReadCapacity16();
 		break;
@@ -647,15 +613,9 @@ void Disk::ReadCapacity16_ReadLong16()
 	}
 }
 
-//---------------------------------------------------------------------------
-//
-//	Check/Get start sector and sector count for a READ/WRITE or READ/WRITE LONG operation
-//
-//---------------------------------------------------------------------------
-
 void Disk::ValidateBlockAddress(access_mode mode) const
 {
-	const uint64_t block = mode == RW16 ? GetInt64(controller->GetCmd(), 2) : GetInt32(controller->GetCmd(), 2);
+	const uint64_t block = mode == RW16 ? GetInt64(GetController()->GetCmd(), 2) : GetInt32(GetController()->GetCmd(), 2);
 
 	if (block > GetBlockCount()) {
 		LOGTRACE("%s", ("Capacity of " + to_string(GetBlockCount()) + " block(s) exceeded: Trying to access block "
@@ -664,24 +624,27 @@ void Disk::ValidateBlockAddress(access_mode mode) const
 	}
 }
 
-bool Disk::CheckAndGetStartAndCount(uint64_t& start, uint32_t& count, access_mode mode) const
+tuple<bool, uint64_t, uint32_t> Disk::CheckAndGetStartAndCount(access_mode mode) const
 {
-	if (mode == RW6 || mode == SEEK6) {
-		start = GetInt24(controller->GetCmd(), 1);
+	uint64_t start;
+	uint32_t count;
 
-		count = controller->GetCmd(4);
+	if (mode == RW6 || mode == SEEK6) {
+		start = GetInt24(GetController()->GetCmd(), 1);
+
+		count = GetController()->GetCmd(4);
 		if (!count) {
 			count= 0x100;
 		}
 	}
 	else {
-		start = mode == RW16 ? GetInt64(controller->GetCmd(), 2) : GetInt32(controller->GetCmd(), 2);
+		start = mode == RW16 ? GetInt64(GetController()->GetCmd(), 2) : GetInt32(GetController()->GetCmd(), 2);
 
 		if (mode == RW16) {
-			count = GetInt32(controller->GetCmd(), 10);
+			count = GetInt32(GetController()->GetCmd(), 10);
 		}
 		else if (mode != SEEK6 && mode != SEEK10) {
-			count = GetInt16(controller->GetCmd(), 7);
+			count = GetInt16(GetController()->GetCmd(), 7);
 		}
 		else {
 			count = 0;
@@ -700,10 +663,10 @@ bool Disk::CheckAndGetStartAndCount(uint64_t& start, uint32_t& count, access_mod
 
 	// Do not process 0 blocks
 	if (!count && (mode != SEEK6 && mode != SEEK10)) {
-		return false;
+		return tuple(false, start, count);
 	}
 
-	return true;
+	return tuple(true, start, count);
 }
 
 uint32_t Disk::CalculateShiftCount(uint32_t size_in_bytes)
diff --git a/cpp/devices/disk.h b/cpp/devices/disk.h
index 2513cb70..c6d10b00 100644
--- a/cpp/devices/disk.h
+++ b/cpp/devices/disk.h
@@ -24,6 +24,7 @@
 #include <string>
 #include <unordered_set>
 #include <unordered_map>
+#include <tuple>
 
 using namespace std;
 
@@ -31,8 +32,6 @@ class Disk : public StorageDevice, private ScsiBlockCommands
 {
 	enum access_mode { RW6, RW10, RW16, SEEK6, SEEK10 };
 
-	Dispatcher<Disk> dispatcher;
-
 	unique_ptr<DiskCache> cache;
 
 	// The supported configurable sector sizes, empty if not configurable
@@ -44,15 +43,15 @@ class Disk : public StorageDevice, private ScsiBlockCommands
 
 public:
 
-	Disk(PbDeviceType, int);
+	Disk(PbDeviceType type, int lun) : StorageDevice(type, lun) {}
 	~Disk() override;
 
-	bool Dispatch(scsi_command) override;
+	bool Init(const unordered_map<string, string>&) override;
+
+	void Dispatch(scsi_command) override;
 
 	bool Eject(bool) override;
 
-	// Command helpers
-	virtual int WriteCheck(uint64_t);
 	virtual void Write(const vector<int>&, const vector<uint8_t>&, uint64_t);
 
 	virtual int Read(const vector<int>&, vector<uint8_t>& , uint64_t);
@@ -64,13 +63,11 @@ public:
 
 private:
 
-	using super = StorageDevice;
-
 	// Commands covered by the SCSI specifications (see https://www.t10.org/drafts.htm)
 	void StartStopUnit();
 	void PreventAllowMediumRemoval();
 	void SynchronizeCache();
-	void ReadDefectData10();
+	void ReadDefectData10() const;
 	virtual void Read6() { Read(RW6); }
 	void Read10() override { Read(RW10); }
 	void Read16() override { Read(RW16); }
@@ -86,19 +83,20 @@ private:
 	void FormatUnit() override;
 	void Seek6();
 	void Read(access_mode);
-	void Write(access_mode);
+	void Write(access_mode) const;
 	void Verify(access_mode);
-	void ReadWriteLong10();
-	void ReadWriteLong16();
+	void ReadWriteLong10() const;
+	void ReadWriteLong16() const;
 	void ReadCapacity16_ReadLong16();
 
 	void ValidateBlockAddress(access_mode) const;
-	bool CheckAndGetStartAndCount(uint64_t&, uint32_t&, access_mode) const;
+	tuple<bool, uint64_t, uint32_t> CheckAndGetStartAndCount(access_mode) const;
 
 	int ModeSense6(const vector<int>&, vector<uint8_t>&) const override;
 	int ModeSense10(const vector<int>&, vector<uint8_t>&) const override;
 
-	static const unordered_map<uint32_t, uint32_t> shift_counts;
+	static inline const unordered_map<uint32_t, uint32_t> shift_counts =
+		{ { 512, 9 }, { 1024, 10 }, { 2048, 11 }, { 4096, 12 } };
 
 protected:
 
diff --git a/cpp/devices/dispatcher.h b/cpp/devices/dispatcher.h
deleted file mode 100644
index 8290937b..00000000
--- a/cpp/devices/dispatcher.h
+++ /dev/null
@@ -1,55 +0,0 @@
-//---------------------------------------------------------------------------
-//
-// SCSI Target Emulator RaSCSI Reloaded
-// for Raspberry Pi
-//
-// Copyright (C) 2022 Uwe Seimet
-//
-// A template for dispatching SCSI commands
-//
-//---------------------------------------------------------------------------
-
-#pragma once
-
-#include "log.h"
-#include <unordered_map>
-
-using namespace std;
-using namespace scsi_defs;
-
-template<class T>
-class Dispatcher
-{
-public:
-
-	Dispatcher() = default;
-	~Dispatcher() = default;
-
-	using operation = void (T::*)();
-	using command_t = struct _command_t {
-		const char *name;
-		operation execute;
-
-		_command_t(const char *_name, operation _execute) : name(_name), execute(_execute) { };
-	};
-	unordered_map<scsi_command, unique_ptr<command_t>> commands;
-
-	void Add(scsi_command opcode, const char *name, operation execute)
-	{
-		commands[opcode] = make_unique<command_t>(name, execute);
-	}
-
-	bool Dispatch(T *instance, scsi_command cmd) const
-	{
-		if (const auto& it = commands.find(cmd); it != commands.end()) {
-			LOGDEBUG("%s Executing %s ($%02X)", __PRETTY_FUNCTION__, it->second->name, (int)cmd)
-
-			(instance->*it->second->execute)();
-
-			return true;
-		}
-
-		// Unknown command
-		return false;
-	}
-};
diff --git a/cpp/devices/host_services.cpp b/cpp/devices/host_services.cpp
index ee780746..88bc6558 100644
--- a/cpp/devices/host_services.cpp
+++ b/cpp/devices/host_services.cpp
@@ -20,30 +20,25 @@
 //   c) start && load (LOAD): Reboot the Raspberry Pi
 //
 
-#include "controllers/controller_manager.h"
 #include "controllers/scsi_controller.h"
 #include "rascsi_exceptions.h"
 #include "scsi_command_util.h"
-#include "dispatcher.h"
 #include "host_services.h"
 #include <algorithm>
 
 using namespace scsi_defs;
 using namespace scsi_command_util;
 
-HostServices::HostServices(int lun, const ControllerManager& manager)
-	: ModePageDevice(SCHS, lun), controller_manager(manager)
+bool HostServices::Init(const unordered_map<string, string>& params)
 {
-	dispatcher.Add(scsi_command::eCmdTestUnitReady, "TestUnitReady", &HostServices::TestUnitReady);
-	dispatcher.Add(scsi_command::eCmdStartStop, "StartStopUnit", &HostServices::StartStopUnit);
+	ModePageDevice::Init(params);
+
+	AddCommand(scsi_command::eCmdTestUnitReady, [this] { TestUnitReady(); });
+	AddCommand(scsi_command::eCmdStartStop, [this] { StartStopUnit(); });
 
 	SetReady(true);
-}
 
-bool HostServices::Dispatch(scsi_command cmd)
-{
-	// The superclass class handles the less specific commands
-	return dispatcher.Dispatch(this, cmd) ? true : super::Dispatch(cmd);
+	return true;
 }
 
 void HostServices::TestUnitReady()
@@ -52,31 +47,26 @@ void HostServices::TestUnitReady()
 	EnterStatusPhase();
 }
 
-vector<byte> HostServices::InquiryInternal() const
+vector<uint8_t> HostServices::InquiryInternal() const
 {
 	return HandleInquiry(device_type::PROCESSOR, scsi_level::SPC_3, false);
 }
 
-void HostServices::StartStopUnit()
+void HostServices::StartStopUnit() const
 {
-	const bool start = controller->GetCmd(4) & 0x01;
-	const bool load = controller->GetCmd(4) & 0x02;
+	const bool start = GetController()->GetCmd(4) & 0x01;
+	const bool load = GetController()->GetCmd(4) & 0x02;
 
 	if (!start) {
-		// Flush any caches
-		for (const auto& device : controller_manager.GetAllDevices()) {
-			device->FlushCache();
-		}
-
 		if (load) {
-			controller->ScheduleShutdown(AbstractController::rascsi_shutdown_mode::STOP_PI);
+			GetController()->ScheduleShutdown(AbstractController::rascsi_shutdown_mode::STOP_PI);
 		}
 		else {
-			controller->ScheduleShutdown(AbstractController::rascsi_shutdown_mode::STOP_RASCSI);
+			GetController()->ScheduleShutdown(AbstractController::rascsi_shutdown_mode::STOP_RASCSI);
 		}
 	}
 	else if (load) {
-		controller->ScheduleShutdown(AbstractController::rascsi_shutdown_mode::RESTART_PI);
+		GetController()->ScheduleShutdown(AbstractController::rascsi_shutdown_mode::RESTART_PI);
 	}
 	else {
 		throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_FIELD_IN_CDB);
@@ -96,7 +86,7 @@ int HostServices::ModeSense6(const vector<int>& cdb, vector<uint8_t>& buf) const
 	fill_n(buf.begin(), length, 0);
 
 	// 4 bytes basic information
-	int size = AddModePages(cdb, buf, 4, length, 255);
+	const int size = AddModePages(cdb, buf, 4, length, 255);
 
 	buf[0] = (uint8_t)size;
 
@@ -114,7 +104,7 @@ int HostServices::ModeSense10(const vector<int>& cdb, vector<uint8_t>& buf) cons
 	fill_n(buf.begin(), length, 0);
 
 	// 8 bytes basic information
-	int size = AddModePages(cdb, buf, 8, length, 65535);
+	const int size = AddModePages(cdb, buf, 8, length, 65535);
 
 	SetInt16(buf, 0, size);
 
@@ -130,7 +120,7 @@ void HostServices::SetUpModePages(map<int, vector<byte>>& pages, int page, bool
 
 void HostServices::AddRealtimeClockPage(map<int, vector<byte>>& pages, bool changeable) const
 {
-	vector<byte> buf(10);
+	pages[32] = vector<byte>(10);
 
 	if (!changeable) {
 		time_t t = time(nullptr);
@@ -148,8 +138,6 @@ void HostServices::AddRealtimeClockPage(map<int, vector<byte>>& pages, bool chan
 		// Ignore leap second for simplicity
 		datetime.second = (uint8_t)(localtime.tm_sec < 60 ? localtime.tm_sec : 59);
 
-		memcpy(&buf[2], &datetime, sizeof(datetime));
+		memcpy(&pages[32][2], &datetime, sizeof(datetime));
 	}
-
-	pages[32] = buf;
 }
diff --git a/cpp/devices/host_services.h b/cpp/devices/host_services.h
index deaad8f8..5beb7b47 100644
--- a/cpp/devices/host_services.h
+++ b/cpp/devices/host_services.h
@@ -15,19 +15,17 @@
 #include <vector>
 #include <map>
 
-class ControllerManager;
-
 class HostServices: public ModePageDevice
 {
 
 public:
 
-	HostServices(int, const ControllerManager&);
+	explicit HostServices(int lun) : ModePageDevice(SCHS, lun) {}
 	~HostServices() override = default;
 
-	bool Dispatch(scsi_command) override;
+	bool Init(const unordered_map<string, string>&) override;
 
-	vector<byte> InquiryInternal() const override;
+	vector<uint8_t> InquiryInternal() const override;
 	void TestUnitReady() override;
 
 protected:
@@ -49,13 +47,7 @@ private:
 	    uint8_t second; // 0-59
 	};
 
-	using super = ModePageDevice;
-
-	Dispatcher<HostServices> dispatcher;
-
-	const ControllerManager& controller_manager;
-
-	void StartStopUnit();
+	void StartStopUnit() const;
 	int ModeSense6(const vector<int>&, vector<uint8_t>&) const override;
 	int ModeSense10(const vector<int>&, vector<uint8_t>&) const override;
 
diff --git a/cpp/devices/interfaces/scsi_primary_commands.h b/cpp/devices/interfaces/scsi_primary_commands.h
index 110de77a..1a9ca6dd 100644
--- a/cpp/devices/interfaces/scsi_primary_commands.h
+++ b/cpp/devices/interfaces/scsi_primary_commands.h
@@ -24,6 +24,9 @@ public:
 	virtual void Inquiry() = 0;
 	virtual void ReportLuns() = 0;
 
-	// Implemented for all RaSCSI device types
+	// Optional commands implemented by all RaSCSI device types
 	virtual void RequestSense() = 0;
+	virtual void ReleaseUnit() = 0;
+	virtual void ReserveUnit() = 0;
+	virtual void SendDiagnostic() = 0;
 };
diff --git a/cpp/devices/mode_page_device.cpp b/cpp/devices/mode_page_device.cpp
index c3b8e82c..ff069fb9 100644
--- a/cpp/devices/mode_page_device.cpp
+++ b/cpp/devices/mode_page_device.cpp
@@ -12,7 +12,6 @@
 #include "log.h"
 #include "rascsi_exceptions.h"
 #include "scsi_command_util.h"
-#include "dispatcher.h"
 #include "mode_page_device.h"
 #include <cstddef>
 
@@ -20,23 +19,21 @@ using namespace std;
 using namespace scsi_defs;
 using namespace scsi_command_util;
 
-ModePageDevice::ModePageDevice(PbDeviceType type, int lun) : PrimaryDevice(type, lun)
+bool ModePageDevice::Init(const unordered_map<string, string>& params)
 {
-	dispatcher.Add(scsi_command::eCmdModeSense6, "ModeSense6", &ModePageDevice::ModeSense6);
-	dispatcher.Add(scsi_command::eCmdModeSense10, "ModeSense10", &ModePageDevice::ModeSense10);
-	dispatcher.Add(scsi_command::eCmdModeSelect6, "ModeSelect6", &ModePageDevice::ModeSelect6);
-	dispatcher.Add(scsi_command::eCmdModeSelect10, "ModeSelect10", &ModePageDevice::ModeSelect10);
-}
+	PrimaryDevice::Init(params);
 
-bool ModePageDevice::Dispatch(scsi_command cmd)
-{
-	// The superclass class handles the less specific commands
-	return dispatcher.Dispatch(this, cmd) ? true : super::Dispatch(cmd);
+	AddCommand(scsi_command::eCmdModeSense6, [this] { ModeSense6(); });
+	AddCommand(scsi_command::eCmdModeSense10, [this] { ModeSense10(); });
+	AddCommand(scsi_command::eCmdModeSelect6, [this] { ModeSelect6(); });
+	AddCommand(scsi_command::eCmdModeSelect10, [this] { ModeSelect10(); });
+
+	return true;
 }
 
 int ModePageDevice::AddModePages(const vector<int>& cdb, vector<uint8_t>& buf, int offset, int length, int max_size) const
 {
-	int max_length = length - offset;
+	const int max_length = length - offset;
 	if (max_length < 0) {
 		return length;
 	}
@@ -61,7 +58,7 @@ int ModePageDevice::AddModePages(const vector<int>& cdb, vector<uint8_t>& buf, i
 	vector<byte> result;
 
 	vector<byte> page0;
-	for (auto const& [index, data] : pages) {
+	for (const auto& [index, data] : pages) {
 		// The specification mandates that page 0 must be returned after all others
 		if (index) {
 			const size_t off = result.size();
@@ -80,7 +77,7 @@ int ModePageDevice::AddModePages(const vector<int>& cdb, vector<uint8_t>& buf, i
 
 	// Page 0 must be last
 	if (!page0.empty()) {
-		size_t off = result.size();
+		const size_t off = result.size();
 
 		// Page data
 		result.insert(result.end(), page0.begin(), page0.end());
@@ -92,53 +89,52 @@ int ModePageDevice::AddModePages(const vector<int>& cdb, vector<uint8_t>& buf, i
 		throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_FIELD_IN_CDB);
 	}
 
-	auto size = static_cast<int>(min(static_cast<size_t>(max_length), result.size()));
+	const auto size = static_cast<int>(min(static_cast<size_t>(max_length), result.size()));
 	memcpy(&buf.data()[offset], result.data(), size);
 
 	// Do not return more than the requested number of bytes
 	return size + offset < length ? size + offset : length;
 }
 
-void ModePageDevice::ModeSense6()
+void ModePageDevice::ModeSense6() const
 {
-	controller->SetLength(ModeSense6(controller->GetCmd(), controller->GetBuffer()));
+	GetController()->SetLength(ModeSense6(GetController()->GetCmd(), GetController()->GetBuffer()));
 
 	EnterDataInPhase();
 }
 
-void ModePageDevice::ModeSense10()
+void ModePageDevice::ModeSense10() const
 {
-	controller->SetLength(ModeSense10(controller->GetCmd(), controller->GetBuffer()));
+	GetController()->SetLength(ModeSense10(GetController()->GetCmd(), GetController()->GetBuffer()));
 
 	EnterDataInPhase();
 }
 
 void ModePageDevice::ModeSelect(scsi_command, const vector<int>&, const vector<uint8_t>&, int) const
 {
+	// There is no default implementation of MDOE SELECT
 	throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_COMMAND_OPERATION_CODE);
 }
 
-void ModePageDevice::ModeSelect6()
+void ModePageDevice::ModeSelect6() const
 {
-	controller->SetLength(SaveParametersCheck(controller->GetCmd(4)));
-
-	EnterDataOutPhase();
+	SaveParametersCheck(GetController()->GetCmd(4));
 }
 
-void ModePageDevice::ModeSelect10()
+void ModePageDevice::ModeSelect10() const
 {
-	const size_t length = min(controller->GetBuffer().size(), static_cast<size_t>(GetInt16(controller->GetCmd(), 7)));
+	const auto length = min(GetController()->GetBuffer().size(), static_cast<size_t>(GetInt16(GetController()->GetCmd(), 7)));
 
-	controller->SetLength(SaveParametersCheck(static_cast<uint32_t>(length)));
-
-	EnterDataOutPhase();
+	SaveParametersCheck(static_cast<uint32_t>(length));
 }
 
-int ModePageDevice::SaveParametersCheck(int length) const
+void ModePageDevice::SaveParametersCheck(int length) const
 {
-	if (!SupportsSaveParameters() && (controller->GetCmd(1) & 0x01)) {
+	if (!SupportsSaveParameters() && (GetController()->GetCmd(1) & 0x01)) {
 		throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_FIELD_IN_CDB);
 	}
 
-	return length;
+	GetController()->SetLength(length);
+
+	EnterDataOutPhase();
 }
diff --git a/cpp/devices/mode_page_device.h b/cpp/devices/mode_page_device.h
index 5e258899..ac8cd07e 100644
--- a/cpp/devices/mode_page_device.h
+++ b/cpp/devices/mode_page_device.h
@@ -14,14 +14,14 @@
 #include <vector>
 #include <map>
 
-class ModePageDevice: public PrimaryDevice
+// TODO Maybe this should better be a mixin class because not all storage devicess have mode pages
+class ModePageDevice : public PrimaryDevice
 {
 public:
 
-	ModePageDevice(PbDeviceType, int);
-	~ModePageDevice() override = default;
+	using PrimaryDevice::PrimaryDevice;
 
-	bool Dispatch(scsi_command) override;
+	bool Init(const unordered_map<string, string>&) override;
 
 	virtual void ModeSelect(scsi_defs::scsi_command, const vector<int>&, const vector<uint8_t>&, int) const;
 
@@ -37,19 +37,15 @@ protected:
 
 private:
 
-	using super = PrimaryDevice;
-
-	Dispatcher<ModePageDevice> dispatcher;
-
 	bool supports_save_parameters = false;
 
 	virtual int ModeSense6(const vector<int>&, vector<uint8_t>&) const = 0;
 	virtual int ModeSense10(const vector<int>&, vector<uint8_t>&) const = 0;
 
-	void ModeSense6();
-	void ModeSense10();
-	void ModeSelect6();
-	void ModeSelect10();
+	void ModeSense6() const;
+	void ModeSense10() const;
+	void ModeSelect6() const;
+	void ModeSelect10() const;
 
-	int SaveParametersCheck(int) const;
+	void SaveParametersCheck(int) const;
 };
diff --git a/cpp/devices/primary_device.cpp b/cpp/devices/primary_device.cpp
index 50f52043..94545636 100644
--- a/cpp/devices/primary_device.cpp
+++ b/cpp/devices/primary_device.cpp
@@ -10,30 +10,47 @@
 #include "log.h"
 #include "rascsi_exceptions.h"
 #include "scsi_command_util.h"
-#include "dispatcher.h"
 #include "primary_device.h"
 
 using namespace std;
 using namespace scsi_defs;
 using namespace scsi_command_util;
 
-PrimaryDevice::PrimaryDevice(PbDeviceType type, int lun) : Device(type, lun)
+bool PrimaryDevice::Init(const unordered_map<string, string>& params)
 {
 	// Mandatory SCSI primary commands
-	dispatcher.Add(scsi_command::eCmdTestUnitReady, "TestUnitReady", &PrimaryDevice::TestUnitReady);
-	dispatcher.Add(scsi_command::eCmdInquiry, "Inquiry", &PrimaryDevice::Inquiry);
-	dispatcher.Add(scsi_command::eCmdReportLuns, "ReportLuns", &PrimaryDevice::ReportLuns);
+	AddCommand(scsi_command::eCmdTestUnitReady, [this] { TestUnitReady(); });
+	AddCommand(scsi_command::eCmdInquiry, [this] { Inquiry(); });
+	AddCommand(scsi_command::eCmdReportLuns, [this] { ReportLuns(); });
 
 	// Optional commands supported by all RaSCSI devices
-	dispatcher.Add(scsi_command::eCmdRequestSense, "RequestSense", &PrimaryDevice::RequestSense);
-	dispatcher.Add(scsi_command::eCmdReserve6, "ReserveUnit", &PrimaryDevice::ReserveUnit);
-	dispatcher.Add(scsi_command::eCmdRelease6, "ReleaseUnit", &PrimaryDevice::ReleaseUnit);
-	dispatcher.Add(scsi_command::eCmdSendDiag, "SendDiagnostic", &PrimaryDevice::SendDiagnostic);
+	AddCommand(scsi_command::eCmdRequestSense, [this] { RequestSense(); });
+	AddCommand(scsi_command::eCmdReserve6, [this] { ReserveUnit(); });
+	AddCommand(scsi_command::eCmdRelease6, [this] { ReleaseUnit(); });
+	AddCommand(scsi_command::eCmdSendDiagnostic, [this] { SendDiagnostic(); });
+
+	SetParams(params);
+
+	return true;
 }
 
-bool PrimaryDevice::Dispatch(scsi_command cmd)
+void PrimaryDevice::AddCommand(scsi_command opcode, const operation& execute)
 {
-	return dispatcher.Dispatch(this, cmd);
+	commands[opcode] = execute;
+}
+
+void PrimaryDevice::Dispatch(scsi_command cmd)
+{
+	if (const auto& it = commands.find(cmd); it != commands.end()) {
+		LOGDEBUG("Executing %s ($%02X)", command_names.find(cmd)->second, static_cast<int>(cmd))
+
+		it->second();
+	}
+	else {
+		LOGTRACE("ID %d LUN %d received unsupported command: $%02X", GetId(), GetLun(), static_cast<int>(cmd))
+
+		throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_COMMAND_OPERATION_CODE);
+	}
 }
 
 void PrimaryDevice::Reset()
@@ -45,14 +62,14 @@ void PrimaryDevice::Reset()
 
 int PrimaryDevice::GetId() const
 {
-	if (controller == nullptr) {
+	if (GetController() == nullptr) {
 		LOGERROR("Device is missing its controller")
 	}
 
-	return controller != nullptr ? controller->GetTargetId() : -1;
+	return GetController() != nullptr ? GetController()->GetTargetId() : -1;
 }
 
-void PrimaryDevice::SetController(AbstractController *c)
+void PrimaryDevice::SetController(shared_ptr<AbstractController> c)
 {
 	controller = c;
 }
@@ -67,23 +84,23 @@ void PrimaryDevice::TestUnitReady()
 void PrimaryDevice::Inquiry()
 {
 	// EVPD and page code check
-	if ((controller->GetCmd(1) & 0x01) || controller->GetCmd(2)) {
+	if ((GetController()->GetCmd(1) & 0x01) || GetController()->GetCmd(2)) {
 		throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_FIELD_IN_CDB);
 	}
 
-	vector<byte> buf = InquiryInternal();
+	vector<uint8_t> buf = InquiryInternal();
 
-	const size_t allocation_length = min(buf.size(), static_cast<size_t>(GetInt16(controller->GetCmd(), 3)));
+	const size_t allocation_length = min(buf.size(), static_cast<size_t>(GetInt16(GetController()->GetCmd(), 3)));
 
-	memcpy(controller->GetBuffer().data(), buf.data(), allocation_length);
-	controller->SetLength(static_cast<uint32_t>(allocation_length));
+	memcpy(GetController()->GetBuffer().data(), buf.data(), allocation_length);
+	GetController()->SetLength(static_cast<uint32_t>(allocation_length));
 
 	// Report if the device does not support the requested LUN
-	if (int lun = controller->GetEffectiveLun(); !controller->HasDeviceForLun(lun)) {
+	if (int lun = GetController()->GetEffectiveLun(); !GetController()->HasDeviceForLun(lun)) {
 		LOGTRACE("Reporting LUN %d for device ID %d as not supported", lun, GetId())
 
 		// Signal that the requested LUN does not exist
-		controller->GetBuffer().data()[0] = 0x7f;
+		GetController()->GetBuffer().data()[0] = 0x7f;
 	}
 
 	EnterDataInPhase();
@@ -92,18 +109,18 @@ void PrimaryDevice::Inquiry()
 void PrimaryDevice::ReportLuns()
 {
 	// Only SELECT REPORT mode 0 is supported
-	if (controller->GetCmd(2)) {
+	if (GetController()->GetCmd(2)) {
 		throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_FIELD_IN_CDB);
 	}
 
-	const uint32_t allocation_length = GetInt32(controller->GetCmd(), 6);
+	const uint32_t allocation_length = GetInt32(GetController()->GetCmd(), 6);
 
-	vector<uint8_t>& buf = controller->GetBuffer();
+	vector<uint8_t>& buf = GetController()->GetBuffer();
 	fill_n(buf.begin(), min(buf.size(), static_cast<size_t>(allocation_length)), 0);
 
 	uint32_t size = 0;
-	for (int lun = 0; lun < controller->GetMaxLuns(); lun++) {
-		if (controller->HasDeviceForLun(lun)) {
+	for (int lun = 0; lun < GetController()->GetMaxLuns(); lun++) {
+		if (GetController()->HasDeviceForLun(lun)) {
 			size += 8;
 			buf[size + 7] = (uint8_t)lun;
 		}
@@ -113,35 +130,35 @@ void PrimaryDevice::ReportLuns()
 
 	size += 8;
 
-	controller->SetLength(min(allocation_length, size));
+	GetController()->SetLength(min(allocation_length, size));
 
 	EnterDataInPhase();
 }
 
 void PrimaryDevice::RequestSense()
 {
-	int lun = controller->GetEffectiveLun();
+	int lun = GetController()->GetEffectiveLun();
 
     // Note: According to the SCSI specs the LUN handling for REQUEST SENSE 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.
-	if (!controller->HasDeviceForLun(lun)) {
+	if (!GetController()->HasDeviceForLun(lun)) {
         // LUN 0 can be assumed to be present (required to call RequestSense() below)
-		assert(controller->HasDeviceForLun(0));
+		assert(GetController()->HasDeviceForLun(0));
 
 		lun = 0;
 
 		// Do not raise an exception here because the rest of the code must be executed
-		controller->Error(sense_key::ILLEGAL_REQUEST, asc::INVALID_LUN);
+		GetController()->Error(sense_key::ILLEGAL_REQUEST, asc::INVALID_LUN);
 
-		controller->SetStatus(status::GOOD);
+		GetController()->SetStatus(status::GOOD);
 	}
 
-    vector<byte> buf = controller->GetDeviceForLun(lun)->HandleRequestSense();
+    vector<byte> buf = GetController()->GetDeviceForLun(lun)->HandleRequestSense();
 
-	const size_t allocation_length = min(buf.size(), static_cast<size_t>(controller->GetCmd(4)));
+	const size_t allocation_length = min(buf.size(), static_cast<size_t>(GetController()->GetCmd(4)));
 
-    memcpy(controller->GetBuffer().data(), buf.data(), allocation_length);
-    controller->SetLength(static_cast<uint32_t>(allocation_length));
+    memcpy(GetController()->GetBuffer().data(), buf.data(), allocation_length);
+    GetController()->SetLength(static_cast<uint32_t>(allocation_length));
 
     EnterDataInPhase();
 }
@@ -149,12 +166,12 @@ void PrimaryDevice::RequestSense()
 void PrimaryDevice::SendDiagnostic()
 {
 	// Do not support PF bit
-	if (controller->GetCmd(1) & 0x10) {
+	if (GetController()->GetCmd(1) & 0x10) {
 		throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_FIELD_IN_CDB);
 	}
 
 	// Do not support parameter list
-	if ((controller->GetCmd(3) != 0) || (controller->GetCmd(4) != 0)) {
+	if ((GetController()->GetCmd(3) != 0) || (GetController()->GetCmd(4) != 0)) {
 		throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_FIELD_IN_CDB);
 	}
 
@@ -187,9 +204,9 @@ void PrimaryDevice::CheckReady()
 	LOGTRACE("%s Device is ready", __PRETTY_FUNCTION__)
 }
 
-vector<byte> PrimaryDevice::HandleInquiry(device_type type, scsi_level level, bool is_removable) const
+vector<uint8_t> PrimaryDevice::HandleInquiry(device_type type, scsi_level level, bool is_removable) const
 {
-	vector<byte> buf(0x1F + 5);
+	vector<uint8_t> buf(0x1F + 5);
 
 	// Basic data
 	// buf[0] ... SCSI device type
@@ -197,11 +214,12 @@ vector<byte> PrimaryDevice::HandleInquiry(device_type type, scsi_level level, bo
 	// buf[2] ... SCSI compliance level of command system
 	// buf[3] ... SCSI compliance level of Inquiry response
 	// buf[4] ... Inquiry additional data
-	buf[0] = (byte)type;
-	buf[1] = (byte)(is_removable ? 0x80 : 0x00);
-	buf[2] = (byte)level;
-	buf[3] = (byte)(level >= scsi_level::SCSI_2 ? scsi_level::SCSI_2 : scsi_level::SCSI_1_CCS);
-	buf[4] = (byte)0x1F;
+	buf[0] = static_cast<uint8_t>(type);
+	buf[1] = is_removable ? 0x80 : 0x00;
+	buf[2] = static_cast<uint8_t>(level);
+	buf[3] = level >= scsi_level::SCSI_2 ?
+			static_cast<uint8_t>(scsi_level::SCSI_2) : static_cast<uint8_t>(scsi_level::SCSI_1_CCS);
+	buf[4] = 0x1F;
 
 	// Padded vendor, product, revision
 	memcpy(&buf.data()[8], GetPaddedName().c_str(), 28);
@@ -228,7 +246,7 @@ vector<byte> PrimaryDevice::HandleRequestSense() const
 	buf[12] = (byte)(GetStatusCode() >> 8);
 	buf[13] = (byte)GetStatusCode();
 
-	LOGTRACE("%s Status $%02X, Sense Key $%02X, ASC $%02X",__PRETTY_FUNCTION__, static_cast<int>(controller->GetStatus()),
+	LOGTRACE("%s Status $%02X, Sense Key $%02X, ASC $%02X",__PRETTY_FUNCTION__, static_cast<int>(GetController()->GetStatus()),
 			static_cast<int>(buf[2]), static_cast<int>(buf[12]))
 
 	return buf;
@@ -243,7 +261,7 @@ bool PrimaryDevice::WriteByteSequence(vector<uint8_t>&, uint32_t)
 
 void PrimaryDevice::ReserveUnit()
 {
-	reserving_initiator = controller->GetInitiatorId();
+	reserving_initiator = GetController()->GetInitiatorId();
 
 	if (reserving_initiator != -1) {
 		LOGTRACE("Reserved device ID %d, LUN %d for initiator ID %d", GetId(), GetLun(), reserving_initiator)
@@ -280,7 +298,7 @@ bool PrimaryDevice::CheckReservation(int initiator_id, scsi_command cmd, bool pr
 		return true;
 	}
 	// PREVENT ALLOW MEDIUM REMOVAL is permitted if the prevent bit is 0
-	if (cmd == scsi_command::eCmdRemoval && !prevent_removal) {
+	if (cmd == scsi_command::eCmdPreventAllowMediumRemoval && !prevent_removal) {
 		return true;
 	}
 
diff --git a/cpp/devices/primary_device.h b/cpp/devices/primary_device.h
index 053efa1c..d57791f9 100644
--- a/cpp/devices/primary_device.h
+++ b/cpp/devices/primary_device.h
@@ -11,24 +11,34 @@
 
 #pragma once
 
+#include "scsi.h"
 #include "interfaces/scsi_primary_commands.h"
 #include "controllers/abstract_controller.h"
 #include "device.h"
-#include "dispatcher.h"
 #include <string>
+#include <unordered_map>
+#include <functional>
+
+using namespace std;
+using namespace scsi_defs;
 
 class PrimaryDevice: private ScsiPrimaryCommands, public Device
 {
+	using operation = function<void()>;
+
 public:
 
-	PrimaryDevice(PbDeviceType, int);
+	PrimaryDevice(PbDeviceType type, int lun) : Device(type, lun) {}
 	~PrimaryDevice() override = default;
 
-	virtual bool Dispatch(scsi_command);
+	virtual bool Init(const unordered_map<string, string>&);
+
+	virtual void Dispatch(scsi_command);
 
 	int GetId() const override;
 
-	void SetController(AbstractController *);
+	void SetController(shared_ptr<AbstractController>);
+
 	virtual bool WriteByteSequence(vector<uint8_t>&, uint32_t);
 
 	int GetSendDelay() const { return send_delay; }
@@ -36,8 +46,6 @@ public:
 	bool CheckReservation(int, scsi_command, bool) const;
 	void DiscardReservation();
 
-	// Override for device specific initializations
-	virtual bool Init(const unordered_map<string, string>&) { return false; };
 	void Reset() override;
 
 	virtual void FlushCache() {
@@ -46,22 +54,23 @@ public:
 
 protected:
 
-	vector<byte> HandleInquiry(scsi_defs::device_type, scsi_level, bool) const;
-	virtual vector<byte> InquiryInternal() const = 0;
+	void AddCommand(scsi_command, const operation&);
+
+	vector<uint8_t> HandleInquiry(scsi_defs::device_type, scsi_level, bool) const;
+	virtual vector<uint8_t> InquiryInternal() const = 0;
 	void CheckReady();
 
 	void SetSendDelay(int s) { send_delay = s; }
 
-	virtual void SendDiagnostic();
-	virtual void ReserveUnit();
-	virtual void ReleaseUnit();
+	void SendDiagnostic() override;
+	void ReserveUnit() override;
+	void ReleaseUnit() override;
 
-	void EnterStatusPhase() { controller->Status(); }
-	void EnterDataInPhase() { controller->DataIn(); }
-	void EnterDataOutPhase() { controller->DataOut(); }
+	void EnterStatusPhase() const { controller.lock()->Status(); }
+	void EnterDataInPhase() const { controller.lock()->DataIn(); }
+	void EnterDataOutPhase() const { controller.lock()->DataOut(); }
 
-	// TODO Try to replace this raw pointer, maybe by a weak_ptr
-	AbstractController *controller = nullptr;
+	inline shared_ptr<AbstractController> GetController() const { return controller.lock(); }
 
 private:
 
@@ -74,7 +83,9 @@ private:
 
 	vector<byte> HandleRequestSense() const;
 
-	Dispatcher<PrimaryDevice> dispatcher;
+	weak_ptr<AbstractController> controller;
+
+	unordered_map<scsi_command, operation> commands;
 
 	int send_delay = BUS::SEND_NO_DELAY;
 
diff --git a/cpp/devices/scsi_command_util.cpp b/cpp/devices/scsi_command_util.cpp
index edfbebee..9737f7ea 100644
--- a/cpp/devices/scsi_command_util.cpp
+++ b/cpp/devices/scsi_command_util.cpp
@@ -21,8 +21,9 @@ void scsi_command_util::ModeSelect(scsi_command cmd, const vector<int>& cdb, con
 
 	// PF
 	if (!(cdb[1] & 0x10)) {
-		// Vendor-specific parameters (SCSI-1) are not supported
-		throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_FIELD_IN_PARAMETER_LIST);
+		// Vendor-specific parameters (SCSI-1) are not supported.
+		// Do not report an error in order to support Apple's HD SC Setup.
+		return;
 	}
 
 	// Skip block descriptors
@@ -62,7 +63,7 @@ void scsi_command_util::ModeSelect(scsi_command cmd, const vector<int>& cdb, con
 		}
 
 		// Advance to the next page
-		int size = buf[offset + 1] + 2;
+		const int size = buf[offset + 1] + 2;
 
 		length -= size;
 		offset += size;
@@ -77,9 +78,7 @@ void scsi_command_util::EnrichFormatPage(map<int, vector<byte>>& pages, bool cha
 {
 	if (changeable) {
 		// The sector size is simulated to be changeable, see the MODE SELECT implementation for details
-		vector<byte>& format_page = pages[3];
-		format_page[12] = (byte)(sector_size >> 8);
-		format_page[13] = (byte)sector_size;
+		SetInt16(pages[3], 12, sector_size);
 	}
 }
 
@@ -88,15 +87,13 @@ void scsi_command_util::AddAppleVendorModePage(map<int, vector<byte>>& pages, bo
 	// Page code 48 (30h) - Apple Vendor Mode Page
 	// Needed for SCCD for stock Apple driver support
 	// Needed for SCHD for stock Apple HD SC Setup
-	vector<byte> buf(30);
+	pages[48] = vector<byte>(30);
 
 	// No changeable area
 	if (!changeable) {
 		const char APPLE_DATA[] = "APPLE COMPUTER, INC   ";
-		memcpy(&buf.data()[2], APPLE_DATA, sizeof(APPLE_DATA));
+		memcpy(&pages[48].data()[2], APPLE_DATA, sizeof(APPLE_DATA));
 	}
-
-	pages[48] = buf;
 }
 
 int scsi_command_util::GetInt16(const vector<uint8_t>& buf, int offset)
diff --git a/cpp/devices/scsi_daynaport.cpp b/cpp/devices/scsi_daynaport.cpp
index 590603d0..08821a2e 100644
--- a/cpp/devices/scsi_daynaport.cpp
+++ b/cpp/devices/scsi_daynaport.cpp
@@ -24,40 +24,31 @@
 //  Note: This requires a DaynaPort SCSI Link driver.
 //---------------------------------------------------------------------------
 
+#include "log.h"
 #include "rascsi_exceptions.h"
 #include "scsi_command_util.h"
-#include "dispatcher.h"
 #include "scsi_daynaport.h"
 
 using namespace scsi_defs;
 using namespace scsi_command_util;
 
-SCSIDaynaPort::SCSIDaynaPort(int lun) : PrimaryDevice(SCDP, lun)
+bool SCSIDaynaPort::Init(const unordered_map<string, string>& params)
 {
-	dispatcher.Add(scsi_command::eCmdTestUnitReady, "TestUnitReady", &SCSIDaynaPort::TestUnitReady);
-	dispatcher.Add(scsi_command::eCmdRead6, "Read6", &SCSIDaynaPort::Read6);
-	dispatcher.Add(scsi_command::eCmdWrite6, "Write6", &SCSIDaynaPort::Write6);
-	dispatcher.Add(scsi_command::eCmdRetrieveStats, "RetrieveStats", &SCSIDaynaPort::RetrieveStatistics);
-	dispatcher.Add(scsi_command::eCmdSetIfaceMode, "SetIfaceMode", &SCSIDaynaPort::SetInterfaceMode);
-	dispatcher.Add(scsi_command::eCmdSetMcastAddr, "SetMcastAddr", &SCSIDaynaPort::SetMcastAddr);
-	dispatcher.Add(scsi_command::eCmdEnableInterface, "EnableInterface", &SCSIDaynaPort::EnableInterface);
+	PrimaryDevice::Init(params);
+
+	AddCommand(scsi_command::eCmdTestUnitReady, [this] { TestUnitReady(); });
+	AddCommand(scsi_command::eCmdRead6, [this] { Read6(); });
+	AddCommand(scsi_command::eCmdWrite6, [this] { Write6(); });
+	AddCommand(scsi_command::eCmdRetrieveStats, [this] { RetrieveStatistics(); });
+	AddCommand(scsi_command::eCmdSetIfaceMode, [this] { SetInterfaceMode(); });
+	AddCommand(scsi_command::eCmdSetMcastAddr, [this] { SetMcastAddr(); });
+	AddCommand(scsi_command::eCmdEnableInterface, [this] { EnableInterface(); });
 
 	// The Daynaport needs to have a delay after the size/flags field of the read response.
 	// In the MacOS driver, it looks like the driver is doing two "READ" system calls.
 	SetSendDelay(DAYNAPORT_READ_HEADER_SZ);
 
 	SupportsParams(true);
-}
-
-bool SCSIDaynaPort::Dispatch(scsi_command cmd)
-{
-	// The superclass class handles the less specific commands
-	return dispatcher.Dispatch(this, cmd) ? true : super::Dispatch(cmd);
-}
-
-bool SCSIDaynaPort::Init(const unordered_map<string, string>& params)
-{
-	SetParams(params);
 
 	m_bTapEnable = m_tap.Init(GetParams());
 	if(!m_bTapEnable){
@@ -78,14 +69,14 @@ bool SCSIDaynaPort::Init(const unordered_map<string, string>& params)
 	return true;
 }
 
-vector<byte> SCSIDaynaPort::InquiryInternal() const
+vector<uint8_t> SCSIDaynaPort::InquiryInternal() const
 {
-	vector<byte> buf = HandleInquiry(device_type::PROCESSOR, scsi_level::SCSI_2, false);
+	vector<uint8_t> buf = HandleInquiry(device_type::PROCESSOR, scsi_level::SCSI_2, false);
 
 	// The Daynaport driver for the Mac expects 37 bytes: Increase additional length and
 	// add a vendor-specific byte in order to satisfy this driver.
-	buf[4] = (byte)(to_integer<int>(buf[4]) + 1);
-	buf.push_back((byte)0);
+	buf[4]++;
+	buf.push_back(0);
 
 	return buf;
 }
@@ -274,7 +265,7 @@ bool SCSIDaynaPort::WriteBytes(const vector<int>& cdb, vector<uint8_t>& buf, uin
 		LOGWARN("%s Unknown data format %02X", __PRETTY_FUNCTION__, data_format)
 	}
 
-	controller->SetBlocks(0);
+	GetController()->SetBlocks(0);
 
 	return true;
 }
@@ -311,65 +302,65 @@ void SCSIDaynaPort::TestUnitReady()
 void SCSIDaynaPort::Read6()
 {
 	// Get record number and block number
-    const uint32_t record = GetInt24(controller->GetCmd(), 1) & 0x1fffff;
-	controller->SetBlocks(1);
+    const uint32_t record = GetInt24(GetController()->GetCmd(), 1) & 0x1fffff;
+    GetController()->SetBlocks(1);
 
 	// If any commands have a bogus control value, they were probably not
 	// generated by the DaynaPort driver so ignore them
-	if (controller->GetCmd(5) != 0xc0 && controller->GetCmd(5) != 0x80) {
+	if (GetController()->GetCmd(5) != 0xc0 && GetController()->GetCmd(5) != 0x80) {
 		LOGTRACE("%s Control value %d, (%04X), returning invalid CDB", __PRETTY_FUNCTION__,
-				controller->GetCmd(5), controller->GetCmd(5))
+				GetController()->GetCmd(5), GetController()->GetCmd(5))
 		throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_FIELD_IN_CDB);
 	}
 
-	LOGTRACE("%s READ(6) command record=%d blocks=%d", __PRETTY_FUNCTION__, record, controller->GetBlocks())
+	LOGTRACE("%s READ(6) command record=%d blocks=%d", __PRETTY_FUNCTION__, record, GetController()->GetBlocks())
 
-	controller->SetLength(Read(controller->GetCmd(), controller->GetBuffer(), record));
-	LOGTRACE("%s ctrl.length is %d", __PRETTY_FUNCTION__, controller->GetLength())
+	GetController()->SetLength(Read(GetController()->GetCmd(), GetController()->GetBuffer(), record));
+	LOGTRACE("%s ctrl.length is %d", __PRETTY_FUNCTION__, GetController()->GetLength())
 
 	// Set next block
-	controller->SetNext(record + 1);
+	GetController()->SetNext(record + 1);
 
 	EnterDataInPhase();
 }
 
-void SCSIDaynaPort::Write6()
+void SCSIDaynaPort::Write6() const
 {
 	// Ensure a sufficient buffer size (because it is not transfer for each block)
-	controller->AllocateBuffer(DAYNAPORT_BUFFER_SIZE);
+	GetController()->AllocateBuffer(DAYNAPORT_BUFFER_SIZE);
 
-	const int data_format = controller->GetCmd(5);
+	const int data_format = GetController()->GetCmd(5);
 
 	if (data_format == 0x00) {
-		controller->SetLength(GetInt16(controller->GetCmd(), 3));
+		GetController()->SetLength(GetInt16(GetController()->GetCmd(), 3));
 	}
 	else if (data_format == 0x80) {
-		controller->SetLength(GetInt16(controller->GetCmd(), 3) + 8);
+		GetController()->SetLength(GetInt16(GetController()->GetCmd(), 3) + 8);
 	}
 	else {
 		LOGWARN("%s Unknown data format $%02X", __PRETTY_FUNCTION__, data_format)
 	}
-	LOGTRACE("%s length: $%04X (%d) format: $%02X", __PRETTY_FUNCTION__, controller->GetLength(),
-			controller->GetLength(), data_format)
+	LOGTRACE("%s length: $%04X (%d) format: $%02X", __PRETTY_FUNCTION__, GetController()->GetLength(),
+			GetController()->GetLength(), data_format)
 
-	if (controller->GetLength() <= 0) {
+	if (GetController()->GetLength() <= 0) {
 		throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_FIELD_IN_CDB);
 	}
 
 	// Set next block
-	controller->SetBlocks(1);
-	controller->SetNext(1);
+	GetController()->SetBlocks(1);
+	GetController()->SetNext(1);
 
 	EnterDataOutPhase();
 }
 
-void SCSIDaynaPort::RetrieveStatistics()
+void SCSIDaynaPort::RetrieveStatistics() const
 {
-	controller->SetLength(RetrieveStats(controller->GetCmd(), controller->GetBuffer()));
+	GetController()->SetLength(RetrieveStats(GetController()->GetCmd(), GetController()->GetBuffer()));
 
 	// Set next block
-	controller->SetBlocks(1);
-	controller->SetNext(1);
+	GetController()->SetBlocks(1);
+	GetController()->SetNext(1);
 
 	EnterDataInPhase();
 }
@@ -400,41 +391,41 @@ void SCSIDaynaPort::RetrieveStatistics()
 //             value.
 //
 //---------------------------------------------------------------------------
-void SCSIDaynaPort::SetInterfaceMode()
+void SCSIDaynaPort::SetInterfaceMode() const
 {
 	// Check whether this command is telling us to "Set Interface Mode" or "Set MAC Address"
 
-	controller->SetLength(RetrieveStats(controller->GetCmd(), controller->GetBuffer()));
-	switch(controller->GetCmd(5)){
+	GetController()->SetLength(RetrieveStats(GetController()->GetCmd(), GetController()->GetBuffer()));
+	switch(GetController()->GetCmd(5)){
 		case CMD_SCSILINK_SETMODE:
 			// TODO Not implemented, do nothing
 			EnterStatusPhase();
 			break;
 
 		case CMD_SCSILINK_SETMAC:
-			controller->SetLength(6);
+			GetController()->SetLength(6);
 			EnterDataOutPhase();
 			break;
 
 		case CMD_SCSILINK_STATS:
 		case CMD_SCSILINK_ENABLE:
 		case CMD_SCSILINK_SET:
-			LOGWARN("%s Unsupported SetInterface command received: %02X", __PRETTY_FUNCTION__, controller->GetCmd(5))
+			LOGWARN("%s Unsupported SetInterface command received: %02X", __PRETTY_FUNCTION__, GetController()->GetCmd(5))
 			throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_COMMAND_OPERATION_CODE);
 			break;
 
 		default:
-			LOGWARN("%s Unknown SetInterface command received: %02X", __PRETTY_FUNCTION__, controller->GetCmd(5))
+			LOGWARN("%s Unknown SetInterface command received: %02X", __PRETTY_FUNCTION__, GetController()->GetCmd(5))
 			throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_COMMAND_OPERATION_CODE);
 			break;
 	}
 }
 
-void SCSIDaynaPort::SetMcastAddr()
+void SCSIDaynaPort::SetMcastAddr() const
 {
-	controller->SetLength(controller->GetCmd(4));
-	if (controller->GetLength() == 0) {
-		LOGWARN("%s Not supported SetMcastAddr Command %02X", __PRETTY_FUNCTION__, controller->GetCmd(2))
+	GetController()->SetLength(GetController()->GetCmd(4));
+	if (GetController()->GetLength() == 0) {
+		LOGWARN("%s Not supported SetMcastAddr Command %02X", __PRETTY_FUNCTION__, GetController()->GetCmd(2))
 
 		throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_FIELD_IN_CDB);
 	}
@@ -456,7 +447,7 @@ void SCSIDaynaPort::SetMcastAddr()
 //---------------------------------------------------------------------------
 void SCSIDaynaPort::EnableInterface()
 {
-	if (controller->GetCmd(5) & 0x80) {
+	if (GetController()->GetCmd(5) & 0x80) {
 		if (!m_tap.Enable()) {
 			LOGWARN("Unable to enable the DaynaPort Interface")
 
diff --git a/cpp/devices/scsi_daynaport.h b/cpp/devices/scsi_daynaport.h
index cc3ecbe6..f5c66b8a 100644
--- a/cpp/devices/scsi_daynaport.h
+++ b/cpp/devices/scsi_daynaport.h
@@ -45,13 +45,13 @@ class SCSIDaynaPort : public PrimaryDevice, public ByteWriter
 {
 public:
 
-	explicit SCSIDaynaPort(int);
+	explicit SCSIDaynaPort(int lun) : PrimaryDevice(SCDP, lun) {}
 	~SCSIDaynaPort() override = default;
 
 	bool Init(const unordered_map<string, string>&) override;
 
 	// Commands
-	vector<byte> InquiryInternal() const override;
+	vector<uint8_t> InquiryInternal() const override;
 	int Read(const vector<int>&, vector<uint8_t>&, uint64_t);
 	bool WriteBytes(const vector<int>&, vector<uint8_t>&, uint32_t) override;
 
@@ -59,14 +59,12 @@ public:
 
 	void TestUnitReady() override;
 	void Read6();
-	void Write6();
-	void RetrieveStatistics();
-	void SetInterfaceMode();
-	void SetMcastAddr();
+	void Write6() const;
+	void RetrieveStatistics() const;
+	void SetInterfaceMode() const;
+	void SetMcastAddr() const;
 	void EnableInterface();
 
-	bool Dispatch(scsi_command) override;
-
 	static const int DAYNAPORT_BUFFER_SIZE = 0x1000000;
 
 	static const int CMD_SCSILINK_STATS        = 0x09;
@@ -88,10 +86,6 @@ public:
 
 private:
 
-	using super = PrimaryDevice;
-
-	Dispatcher<SCSIDaynaPort> dispatcher;
-
 	enum class read_data_flags_t : uint32_t {
 		e_no_more_data = 0x00000000,
 		e_more_data_available = 0x00000001,
diff --git a/cpp/devices/scsi_host_bridge.cpp b/cpp/devices/scsi_host_bridge.cpp
index 0b61ecd6..7be8c333 100644
--- a/cpp/devices/scsi_host_bridge.cpp
+++ b/cpp/devices/scsi_host_bridge.cpp
@@ -16,9 +16,9 @@
 //        work with the Sharp X68000 operating system.
 //---------------------------------------------------------------------------
 
+#include "log.h"
 #include "rascsi_exceptions.h"
 #include "scsi_command_util.h"
-#include "dispatcher.h"
 #include "scsi_host_bridge.h"
 #include <arpa/inet.h>
 #include <array>
@@ -27,21 +27,18 @@ using namespace std;
 using namespace scsi_defs;
 using namespace scsi_command_util;
 
-SCSIBR::SCSIBR(int lun) : PrimaryDevice(SCBR, lun)
+bool SCSIBR::Init(const unordered_map<string, string>& params)
 {
+	PrimaryDevice::Init(params);
+
 	// Create host file system
 	fs.Reset();
 
-	dispatcher.Add(scsi_command::eCmdTestUnitReady, "TestUnitReady", &SCSIBR::TestUnitReady);
-	dispatcher.Add(scsi_command::eCmdRead6, "GetMessage10", &SCSIBR::GetMessage10);
-	dispatcher.Add(scsi_command::eCmdWrite6, "SendMessage10", &SCSIBR::SendMessage10);
+	AddCommand(scsi_command::eCmdTestUnitReady, [this] { TestUnitReady(); });
+	AddCommand(scsi_command::eCmdGetMessage10, [this] { GetMessage10(); });
+	AddCommand(scsi_command::eCmdSendMessage10, [this] { SendMessage10(); });
 
 	SupportsParams(true);
-}
-
-bool SCSIBR::Init(const unordered_map<string, string>& params)
-{
-	SetParams(params);
 
 #ifdef __linux__
 	// TAP Driver Generation
@@ -71,31 +68,25 @@ bool SCSIBR::Init(const unordered_map<string, string>& params)
 #endif
 }
 
-bool SCSIBR::Dispatch(scsi_command cmd)
+vector<uint8_t> SCSIBR::InquiryInternal() const
 {
-	// The superclass class handles the less specific commands
-	return dispatcher.Dispatch(this, cmd) ? true : super::Dispatch(cmd);
-}
-
-vector<byte> SCSIBR::InquiryInternal() const
-{
-	vector<byte> buf = HandleInquiry(device_type::COMMUNICATIONS, scsi_level::SCSI_2, false);
+	vector<uint8_t> buf = HandleInquiry(device_type::COMMUNICATIONS, scsi_level::SCSI_2, false);
 
 	// The bridge returns more additional bytes than the other devices
 	buf.resize(0x1F + 8 + 5);
 
-	buf[4] = byte{0x1F + 8};
+	buf[4] = 0x1F + 8;
 
 	// Optional function valid flag
-	buf[36] = byte{'0'};
+	buf[36] = '0';
 
 	// TAP Enable
 	if (m_bTapEnable) {
-		buf[37] = byte{'1'};
+		buf[37] = '1';
 	}
 
 	// CFileSys Enable
-	buf[38] = byte{'1'};
+	buf[38] = '1';
 
 	return buf;
 }
@@ -255,16 +246,16 @@ bool SCSIBR::WriteBytes(const vector<int>& cdb, vector<uint8_t>& buf, uint32_t)
 void SCSIBR::GetMessage10()
 {
 	// Ensure a sufficient buffer size (because it is not a transfer for each block)
-	controller->AllocateBuffer(0x1000000);
+	GetController()->AllocateBuffer(0x1000000);
 
-	controller->SetLength(GetMessage10(controller->GetCmd(), controller->GetBuffer()));
-	if (controller->GetLength() <= 0) {
+	GetController()->SetLength(GetMessage10(GetController()->GetCmd(), GetController()->GetBuffer()));
+	if (GetController()->GetLength() <= 0) {
 		throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_FIELD_IN_CDB);
 	}
 
 	// Set next block
-	controller->SetBlocks(1);
-	controller->SetNext(1);
+	GetController()->SetBlocks(1);
+	GetController()->SetNext(1);
 
 	EnterDataInPhase();
 }
@@ -276,19 +267,19 @@ void SCSIBR::GetMessage10()
 //  This Send Message command is used by the X68000 host driver
 //
 //---------------------------------------------------------------------------
-void SCSIBR::SendMessage10()
+void SCSIBR::SendMessage10() const
 {
-	controller->SetLength(GetInt24(controller->GetCmd(), 6));
-	if (controller->GetLength() <= 0) {
+	GetController()->SetLength(GetInt24(GetController()->GetCmd(), 6));
+	if (GetController()->GetLength() <= 0) {
 		throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_FIELD_IN_CDB);
 	}
 
 	// Ensure a sufficient buffer size (because it is not a transfer for each block)
-	controller->AllocateBuffer(0x1000000);
+	GetController()->AllocateBuffer(0x1000000);
 
 	// Set next block
-	controller->SetBlocks(1);
-	controller->SetNext(1);
+	GetController()->SetBlocks(1);
+	GetController()->SetNext(1);
 
 	EnterDataOutPhase();
 }
diff --git a/cpp/devices/scsi_host_bridge.h b/cpp/devices/scsi_host_bridge.h
index 0eabd531..6472a35c 100644
--- a/cpp/devices/scsi_host_bridge.h
+++ b/cpp/devices/scsi_host_bridge.h
@@ -32,26 +32,21 @@ class SCSIBR : public PrimaryDevice, public ByteWriter
 
 public:
 
-	explicit SCSIBR(int);
+	explicit SCSIBR(int lun) : PrimaryDevice(SCBR, lun) {}
 	~SCSIBR() override = default;
 
 	bool Init(const unordered_map<string, string>&) override;
-	bool Dispatch(scsi_command) override;
 
 	// Commands
-	vector<byte> InquiryInternal() const override;
+	vector<uint8_t> InquiryInternal() const override;
 	int GetMessage10(const vector<int>&, vector<uint8_t>&);
 	bool WriteBytes(const vector<int>&, vector<uint8_t>&, uint32_t) override;
 	void TestUnitReady() override;
 	void GetMessage10();
-	void SendMessage10();
+	void SendMessage10() const;
 
 private:
 
-	using super = PrimaryDevice;
-
-	Dispatcher<SCSIBR> dispatcher;
-
 	int GetMacAddr(vector<uint8_t>&) const;		// Get MAC address
 	void SetMacAddr(const vector<uint8_t>&);		// Set MAC address
 	void ReceivePacket();						// Receive a packet
diff --git a/cpp/devices/scsi_printer.cpp b/cpp/devices/scsi_printer.cpp
index 1e46e62a..986ca0ff 100644
--- a/cpp/devices/scsi_printer.cpp
+++ b/cpp/devices/scsi_printer.cpp
@@ -29,11 +29,11 @@
 // With STOP PRINT printing can be cancelled before SYNCHRONIZE BUFFER was sent.
 //
 
-#include <sys/stat.h>
+#include "log.h"
 #include "rascsi_exceptions.h"
 #include "scsi_command_util.h"
-#include "dispatcher.h"
 #include "scsi_printer.h"
+#include <sys/stat.h>
 #include <filesystem>
 
 using namespace std;
@@ -41,18 +41,25 @@ using namespace filesystem;
 using namespace scsi_defs;
 using namespace scsi_command_util;
 
-SCSIPrinter::SCSIPrinter(int lun) : PrimaryDevice(SCLP, lun)
+bool SCSIPrinter::Init(const unordered_map<string, string>& params)
 {
-	dispatcher.Add(scsi_command::eCmdTestUnitReady, "TestUnitReady", &SCSIPrinter::TestUnitReady);
-	dispatcher.Add(scsi_command::eCmdPrint, "Print", &SCSIPrinter::Print);
-	dispatcher.Add(scsi_command::eCmdSynchronizeBuffer, "SynchronizeBuffer", &SCSIPrinter::SynchronizeBuffer);
+	PrimaryDevice::Init(params);
+
+	AddCommand(scsi_command::eCmdTestUnitReady, [this] { TestUnitReady(); });
+	AddCommand(scsi_command::eCmdPrint, [this] { Print(); });
+	AddCommand(scsi_command::eCmdSynchronizeBuffer, [this] { SynchronizeBuffer(); });
 	// STOP PRINT is identical with TEST UNIT READY, it just returns the status
-	dispatcher.Add(scsi_command::eCmdStopPrint, "StopPrint", &SCSIPrinter::TestUnitReady);
+	AddCommand(scsi_command::eCmdStopPrint, [this] { TestUnitReady(); });
 
 	// Required also in this class in order to fulfill the ScsiPrinterCommands interface contract
-	dispatcher.Add(scsi_command::eCmdReserve6, "ReserveUnit", &SCSIPrinter::ReserveUnit);
-	dispatcher.Add(scsi_command::eCmdRelease6, "ReleaseUnit", &SCSIPrinter::ReleaseUnit);
-	dispatcher.Add(scsi_command::eCmdSendDiag, "SendDiagnostic", &SCSIPrinter::SendDiagnostic);
+	AddCommand(scsi_command::eCmdReserve6, [this] { ReserveUnit(); });
+	AddCommand(scsi_command::eCmdRelease6, [this] { ReleaseUnit(); });
+	AddCommand(scsi_command::eCmdSendDiagnostic, [this] { SendDiagnostic(); });
+
+	if (GetParam("cmd").find("%f") == string::npos) {
+		LOGERROR("Missing filename specifier %%f")
+		return false;
+	}
 
 	error_code error;
 	file_template = temp_directory_path(error); //NOSONAR Publicly writable directory is fine here
@@ -60,57 +67,36 @@ SCSIPrinter::SCSIPrinter(int lun) : PrimaryDevice(SCLP, lun)
 
 	SupportsParams(true);
 	SetReady(true);
-}
-
-SCSIPrinter::~SCSIPrinter()
-{
-	Cleanup();
-}
-
-bool SCSIPrinter::Init(const unordered_map<string, string>& params)
-{
-	SetParams(params);
-
-	if (GetParam("cmd").find("%f") == string::npos) {
-		LOGERROR("Missing filename specifier %s", "%f")
-		return false;
-	}
 
 	return true;
 }
 
-bool SCSIPrinter::Dispatch(scsi_command cmd)
-{
-	// The superclass class handles the less specific commands
-	return dispatcher.Dispatch(this, cmd) ? true : super::Dispatch(cmd);
-}
-
 void SCSIPrinter::TestUnitReady()
 {
 	// The printer is always ready
 	EnterStatusPhase();
 }
 
-vector<byte> SCSIPrinter::InquiryInternal() const
+vector<uint8_t> SCSIPrinter::InquiryInternal() const
 {
 	return HandleInquiry(device_type::PRINTER, scsi_level::SCSI_2, false);
 }
 
 void SCSIPrinter::Print()
 {
-	const uint32_t length = GetInt24(controller->GetCmd(), 2);
+	const uint32_t length = GetInt24(GetController()->GetCmd(), 2);
 
 	LOGTRACE("Receiving %d byte(s) to be printed", length)
 
-	if (length > controller->GetBuffer().size()) {
-		LOGERROR("%s", ("Transfer buffer overflow: Buffer size is " + to_string(controller->GetBuffer().size()) +
+	if (length > GetController()->GetBuffer().size()) {
+		LOGERROR("%s", ("Transfer buffer overflow: Buffer size is " + to_string(GetController()->GetBuffer().size()) +
 				" bytes, " + to_string(length) + " bytes expected").c_str())
 
 		throw scsi_exception(sense_key::ILLEGAL_REQUEST, asc::INVALID_FIELD_IN_CDB);
 	}
 
-	controller->SetLength(length);
-	controller->SetByteTransfer(true);
+	GetController()->SetLength(length);
+	GetController()->SetByteTransfer(true);
 
 	EnterDataOutPhase();
 }
diff --git a/cpp/devices/scsi_printer.h b/cpp/devices/scsi_printer.h
index 6c6afd82..ca9995e7 100644
--- a/cpp/devices/scsi_printer.h
+++ b/cpp/devices/scsi_printer.h
@@ -18,7 +18,7 @@
 
 using namespace std;
 
-class SCSIPrinter : public PrimaryDevice, ScsiPrinterCommands //NOSONAR Custom destructor cannot be removed
+class SCSIPrinter : public PrimaryDevice, private ScsiPrinterCommands
 {
 	static const int NOT_RESERVED = -2;
 
@@ -26,15 +26,17 @@ class SCSIPrinter : public PrimaryDevice, ScsiPrinterCommands //NOSONAR Custom d
 
 public:
 
-	explicit SCSIPrinter(int);
-	~SCSIPrinter() override;
-
-	bool Dispatch(scsi_command) override;
+	explicit SCSIPrinter(int lun) : PrimaryDevice(SCLP, lun) {}
+	~SCSIPrinter() override = default;
 
 	bool Init(const unordered_map<string, string>&) override;
-	void Cleanup();
 
-	vector<byte> InquiryInternal() const override;
+	vector<uint8_t> InquiryInternal() const override;
+
+	bool WriteByteSequence(vector<uint8_t>&, uint32_t) override;
+
+private:
+
 	void TestUnitReady() override;
 	void ReserveUnit() override { PrimaryDevice::ReserveUnit(); }
 	void ReleaseUnit() override { PrimaryDevice::ReleaseUnit(); }
@@ -42,13 +44,7 @@ public:
 	void Print() override;
 	void SynchronizeBuffer();
 
-	bool WriteByteSequence(vector<uint8_t>&, uint32_t) override;
-
-private:
-
-	using super = PrimaryDevice;
-
-	Dispatcher<SCSIPrinter> dispatcher;
+	void Cleanup();
 
 	string file_template;
 
diff --git a/cpp/devices/scsicd.cpp b/cpp/devices/scsicd.cpp
index 8403a94c..cabe321d 100644
--- a/cpp/devices/scsicd.cpp
+++ b/cpp/devices/scsicd.cpp
@@ -16,7 +16,6 @@
 
 #include "rascsi_exceptions.h"
 #include "scsi_command_util.h"
-#include "dispatcher.h"
 #include "scsicd.h"
 #include <array>
 #include <fstream>
@@ -27,18 +26,19 @@ using namespace scsi_command_util;
 SCSICD::SCSICD(int lun, const unordered_set<uint32_t>& sector_sizes) : Disk(SCCD, lun)
 {
 	SetSectorSizes(sector_sizes);
+}
 
-	dispatcher.Add(scsi_command::eCmdReadToc, "ReadToc", &SCSICD::ReadToc);
+bool SCSICD::Init(const unordered_map<string, string>& params)
+{
+	Disk::Init(params);
+
+	AddCommand(scsi_command::eCmdReadToc, [this] { ReadToc(); });
 
 	SetReadOnly(true);
 	SetRemovable(true);
 	SetLockable(true);
-}
 
-bool SCSICD::Dispatch(scsi_command cmd)
-{
-	// The superclass class handles the less specific commands
-	return dispatcher.Dispatch(this, cmd) ? true : super::Dispatch(cmd);
+	return true;
 }
 
 void SCSICD::Open()
@@ -72,7 +72,7 @@ void SCSICD::Open()
 		}
 	}
 
-	super::ValidateFile();
+	Disk::ValidateFile();
 
 	SetUpCache(0, rawfile);
 
@@ -162,19 +162,19 @@ void SCSICD::CreateDataTrack()
 
 void SCSICD::ReadToc()
 {
-	controller->SetLength(ReadTocInternal(controller->GetCmd(), controller->GetBuffer()));
+	GetController()->SetLength(ReadTocInternal(GetController()->GetCmd(), GetController()->GetBuffer()));
 
 	EnterDataInPhase();
 }
 
-vector<byte> SCSICD::InquiryInternal() const
+vector<uint8_t> SCSICD::InquiryInternal() const
 {
 	return HandleInquiry(device_type::CD_ROM, scsi_level::SCSI_2, true);
 }
 
 void SCSICD::SetUpModePages(map<int, vector<byte>>& pages, int page, bool changeable) const
 {
-	super::SetUpModePages(pages, page, changeable);
+	Disk::SetUpModePages(pages, page, changeable);
 
 	// Page code 13
 	if (page == 0x0d || page == 0x3f) {
@@ -251,7 +251,7 @@ int SCSICD::Read(const vector<int>& cdb, vector<uint8_t>& buf, uint64_t block)
 
 	// Base class
 	assert(dataindex >= 0);
-	return super::Read(cdb, buf, block);
+	return Disk::Read(cdb, buf, block);
 }
 
 int SCSICD::ReadTocInternal(const vector<int>& cdb, vector<uint8_t>& buf)
diff --git a/cpp/devices/scsicd.h b/cpp/devices/scsicd.h
index 6f3fa7bc..ab29e690 100644
--- a/cpp/devices/scsicd.h
+++ b/cpp/devices/scsicd.h
@@ -25,12 +25,11 @@ public:
 	SCSICD(int, const unordered_set<uint32_t>&);
 	~SCSICD() override = default;
 
-	bool Dispatch(scsi_command) override;
+	bool Init(const unordered_map<string, string>&) override;
 
 	void Open() override;
 
-	// Commands
-	vector<byte> InquiryInternal() const override;
+	vector<uint8_t> InquiryInternal() const override;
 	int Read(const vector<int>&, vector<uint8_t>&, uint64_t) override;
 
 protected:
@@ -40,10 +39,6 @@ protected:
 
 private:
 
-	using super = Disk;
-
-	Dispatcher<SCSICD> dispatcher;
-
 	int ReadTocInternal(const vector<int>&, vector<uint8_t>&);
 
 	void AddCDROMPage(map<int, vector<byte>>&, bool) const;
diff --git a/cpp/devices/scsihd.cpp b/cpp/devices/scsihd.cpp
index 3cc42b24..c76dfee6 100644
--- a/cpp/devices/scsihd.cpp
+++ b/cpp/devices/scsihd.cpp
@@ -56,7 +56,7 @@ string SCSIHD::GetProductData() const
 
 void SCSIHD::FinalizeSetup(off_t image_offset)
 {
-	super::ValidateFile();
+	Disk::ValidateFile();
 
 	// For non-removable media drives set the default product name based on the drive capacity
 	if (!IsRemovable()) {
@@ -79,7 +79,7 @@ void SCSIHD::Open()
 	FinalizeSetup(0);
 }
 
-vector<byte> SCSIHD::InquiryInternal() const
+vector<uint8_t> SCSIHD::InquiryInternal() const
 {
 	return HandleInquiry(device_type::DIRECT_ACCESS, scsi_level, IsRemovable());
 }
diff --git a/cpp/devices/scsihd.h b/cpp/devices/scsihd.h
index 2253346c..c2ccd6e7 100644
--- a/cpp/devices/scsihd.h
+++ b/cpp/devices/scsihd.h
@@ -35,7 +35,7 @@ public:
 	void Open() override;
 
 	// Commands
-	vector<byte> InquiryInternal() const override;
+	vector<uint8_t> InquiryInternal() const override;
 	void ModeSelect(scsi_defs::scsi_command, const vector<int>&, const vector<uint8_t>&, int) const override;
 
 	void AddFormatPage(map<int, vector<byte>>&, bool) const override;
@@ -45,7 +45,5 @@ private:
 
 	string GetProductData() const;
 
-	using super = Disk;
-
 	scsi_defs::scsi_level scsi_level;
 };
diff --git a/cpp/devices/scsihd_nec.cpp b/cpp/devices/scsihd_nec.cpp
index 6c8bdd17..66fadb17 100644
--- a/cpp/devices/scsihd_nec.cpp
+++ b/cpp/devices/scsihd_nec.cpp
@@ -112,7 +112,7 @@ pair<int, int> SCSIHD_NEC::SetParameters(const array<char, 512>& data, int size)
 	return make_pair(image_size, sector_size);
 }
 
-vector<byte> SCSIHD_NEC::InquiryInternal() const
+vector<uint8_t> SCSIHD_NEC::InquiryInternal() const
 {
 	return HandleInquiry(device_type::DIRECT_ACCESS, scsi_level::SCSI_1_CCS, false);
 }
diff --git a/cpp/devices/scsihd_nec.h b/cpp/devices/scsihd_nec.h
index 1c29b7a6..423ecdcb 100644
--- a/cpp/devices/scsihd_nec.h
+++ b/cpp/devices/scsihd_nec.h
@@ -40,7 +40,7 @@ public:
 
 protected:
 
-	vector<byte> InquiryInternal() const override;
+	vector<uint8_t> InquiryInternal() const override;
 
 	void AddFormatPage(map<int, vector<byte>>&, bool) const override;
 	void AddDrivePage(map<int, vector<byte>>&, bool) const override;
diff --git a/cpp/devices/scsimo.cpp b/cpp/devices/scsimo.cpp
index 448cf6e2..c660d631 100644
--- a/cpp/devices/scsimo.cpp
+++ b/cpp/devices/scsimo.cpp
@@ -49,7 +49,7 @@ void SCSIMO::Open()
 		SetBlockCount(size >> GetSectorSizeShiftCount());
 	}
 
-	super::ValidateFile();
+	Disk::ValidateFile();
 
 	SetUpCache(0);
 
@@ -59,7 +59,7 @@ void SCSIMO::Open()
 	}
 }
 
-vector<byte> SCSIMO::InquiryInternal() const
+vector<uint8_t> SCSIMO::InquiryInternal() const
 {
 	return HandleInquiry(device_type::OPTICAL_MEMORY, scsi_level::SCSI_2, true);
 }
diff --git a/cpp/devices/scsimo.h b/cpp/devices/scsimo.h
index c763823d..d6b24546 100644
--- a/cpp/devices/scsimo.h
+++ b/cpp/devices/scsimo.h
@@ -29,7 +29,7 @@ public:
 
 	void Open() override;
 
-	vector<byte> InquiryInternal() const override;
+	vector<uint8_t> InquiryInternal() const override;
 	void ModeSelect(scsi_defs::scsi_command, const vector<int>&, const vector<uint8_t>&, int) const override;
 
 protected:
@@ -40,8 +40,6 @@ protected:
 
 private:
 
-	using super = Disk;
-
 	void AddOptionPage(map<int, vector<byte>>&, bool) const;
 
 	bool SetGeometryForCapacity(uint64_t);
diff --git a/cpp/devices/storage_device.cpp b/cpp/devices/storage_device.cpp
index db733187..0714bd9c 100644
--- a/cpp/devices/storage_device.cpp
+++ b/cpp/devices/storage_device.cpp
@@ -9,6 +9,7 @@
 
 #include "rascsi_exceptions.h"
 #include "storage_device.h"
+#include <sys/stat.h>
 #include <unistd.h>
 #include <filesystem>
 
@@ -49,13 +50,6 @@ void StorageDevice::ValidateFile()
 	SetReady(true);
 }
 
-void StorageDevice::MediumChanged()
-{
-	assert(IsRemovable());
-
-	medium_changed = true;
-}
-
 void StorageDevice::ReserveFile(const string& file, int id, int lun) const
 {
 	assert(!file.empty());
@@ -71,7 +65,7 @@ void StorageDevice::UnreserveFile()
 	filename = "";
 }
 
-pair<int, int> StorageDevice::GetIdsForReservedFile(const string& file)
+id_set StorageDevice::GetIdsForReservedFile(const string& file)
 {
 	if (const auto& it = reserved_files.find(file); it != reserved_files.end()) {
 		return make_pair(it->second.first, it->second.second);
diff --git a/cpp/devices/storage_device.h b/cpp/devices/storage_device.h
index 81b0d571..83f40fbd 100644
--- a/cpp/devices/storage_device.h
+++ b/cpp/devices/storage_device.h
@@ -31,8 +31,6 @@ public:
 	string GetFilename() const { return filename; }
 	void SetFilename(string_view f) { filename = f; }
 
-	void MediumChanged();
-
 	uint64_t GetBlockCount() const { return blocks; }
 
 	void ReserveFile(const string&, int, int) const;
@@ -42,16 +40,17 @@ public:
 	static bool FileExists(const string&);
 	bool IsReadOnlyFile() const;
 
+	void SetMediumChanged(bool b) { medium_changed = b; }
+
 	static unordered_map<string, id_set> GetReservedFiles() { return reserved_files; }
 	static void SetReservedFiles(const unordered_map<string, id_set>& r) { reserved_files = r; }
-	static pair<int, int> GetIdsForReservedFile(const string&);
+	static id_set GetIdsForReservedFile(const string&);
 
 protected:
 
 	void ValidateFile();
 
 	bool IsMediumChanged() const { return medium_changed; }
-	void SetMediumChanged(bool b) { medium_changed = b; }
 
 	void SetBlockCount(uint64_t b) { blocks = b; }
 
diff --git a/cpp/bus.cpp b/cpp/hal/bus.cpp
similarity index 80%
rename from cpp/bus.cpp
rename to cpp/hal/bus.cpp
index 206e1701..e791940f 100644
--- a/cpp/bus.cpp
+++ b/cpp/hal/bus.cpp
@@ -12,6 +12,25 @@
 
 using namespace std;
 
+//---------------------------------------------------------------------------
+//
+// Get the number of bytes for a command
+// TODO Add the byte count to the enum value/command name mapping and remove the comparisons
+//
+//---------------------------------------------------------------------------
+int BUS::GetCommandByteCount(uint8_t opcode)
+{
+    if (opcode == 0x88 || opcode == 0x8A || opcode == 0x8F || opcode == 0x91 || opcode == 0x9E || opcode == 0x9F) {
+        return 16;
+    } else if (opcode == 0xA0) {
+        return 12;
+    } else if (opcode >= 0x20 && opcode <= 0x7D) {
+        return 10;
+    } else {
+        return 6;
+    }
+}
+
 //---------------------------------------------------------------------------
 //
 //	Phase Acquisition
diff --git a/cpp/bus.h b/cpp/hal/bus.h
similarity index 98%
rename from cpp/bus.h
rename to cpp/hal/bus.h
index e01ac933..641e7cfb 100644
--- a/cpp/bus.h
+++ b/cpp/hal/bus.h
@@ -48,6 +48,7 @@ public:
 	// Basic Functions
 	virtual bool Init(mode_e mode, board_type::rascsi_board_type_e rascsi_type ) = 0;
 
+    static int GetCommandByteCount(uint8_t);
 	virtual void Reset() = 0;
 	virtual void Cleanup() = 0;
 	phase_t GetPhase();
diff --git a/cpp/hal/gpiobus.cpp b/cpp/hal/gpiobus.cpp
index 81ae0af4..c73a7266 100644
--- a/cpp/hal/gpiobus.cpp
+++ b/cpp/hal/gpiobus.cpp
@@ -596,7 +596,7 @@ int GPIOBUS::CommandHandShake(uint8_t *buf)
         }
     }
 
-    int command_byte_count = GetCommandByteCount(*buf);
+    const int command_byte_count = GetCommandByteCount(*buf);
 
     // Increment buffer pointer
     buf++;
@@ -1091,26 +1091,6 @@ BUS::phase_t GPIOBUS::GetPhaseRaw(uint32_t raw_data)
     //     return GetPhase(mci);
 }
 
-//---------------------------------------------------------------------------
-//
-// Get the number of bytes for a command
-//
-//---------------------------------------------------------------------------
-int GPIOBUS::GetCommandByteCount(uint8_t opcode)
-{
-    GPIO_FUNCTION_TRACE
-
-    if (opcode == 0x88 || opcode == 0x8A || opcode == 0x8F || opcode == 0x91 || opcode == 0x9E || opcode == 0x9F) {
-        return 16;
-    } else if (opcode == 0xA0) {
-        return 12;
-    } else if (opcode >= 0x20 && opcode <= 0x7D) {
-        return 10;
-    } else {
-        return 6;
-    }
-}
-
 const string GPIOBUS::GetConnectDesc()
 {
     if (board == nullptr) {
diff --git a/cpp/hal/gpiobus.h b/cpp/hal/gpiobus.h
index e9409a19..a5b1c712 100644
--- a/cpp/hal/gpiobus.h
+++ b/cpp/hal/gpiobus.h
@@ -351,7 +351,6 @@ class GPIOBUS : public BUS
     // Get the phase based on raw data
     static BUS::phase_t GetPhaseRaw(uint32_t raw_data);
 
-    static int GetCommandByteCount(uint8_t opcode);
 
     const string GetConnectDesc();
 
diff --git a/cpp/hal/sbc_version.cpp b/cpp/hal/sbc_version.cpp
index 5e2c5aee..2821ac05 100644
--- a/cpp/hal/sbc_version.cpp
+++ b/cpp/hal/sbc_version.cpp
@@ -35,7 +35,7 @@ const std::string SBC_Version::m_str_unknown_sbc       = "Unknown SBC";
 //     "Raspberry Pi 4 Model B" will match with both of the following:
 //         - Raspberry Pi 4 Model B Rev 1.4
 //         - Raspberry Pi 4 Model B Rev 1.3
-const std::map<std::string, SBC_Version::sbc_version_type> SBC_Version::m_proc_device_tree_mapping = {
+const std::map<std::string, SBC_Version::sbc_version_type, std::less<>> SBC_Version::m_proc_device_tree_mapping = {
     {"Raspberry Pi 1 Model ", sbc_version_type::sbc_raspberry_pi_1},
     {"Raspberry Pi 2 Model ", sbc_version_type::sbc_raspberry_pi_2_3},
     {"Raspberry Pi 3 Model ", sbc_version_type::sbc_raspberry_pi_2_3},
@@ -212,4 +212,4 @@ uint32_t SBC_Version::GetPeripheralAddress(void)
 {
     return 0;
 }
-#endif
\ No newline at end of file
+#endif
diff --git a/cpp/hal/sbc_version.h b/cpp/hal/sbc_version.h
index 4f63ae04..520f0c85 100644
--- a/cpp/hal/sbc_version.h
+++ b/cpp/hal/sbc_version.h
@@ -62,7 +62,7 @@ class SBC_Version
     static const std::string m_str_bananapi_m4;
     static const std::string m_str_unknown_sbc;
 
-    static const std::map<std::string, sbc_version_type> m_proc_device_tree_mapping;
+    static const std::map<std::string, sbc_version_type, std::less<>> m_proc_device_tree_mapping;
 
     static const std::string m_device_tree_model_path;
 
diff --git a/cpp/monitor/sm_core.cpp b/cpp/monitor/sm_core.cpp
new file mode 100644
index 00000000..6969acde
--- /dev/null
+++ b/cpp/monitor/sm_core.cpp
@@ -0,0 +1,413 @@
+//---------------------------------------------------------------------------
+//
+//	SCSI Target Emulator RaSCSI Reloaded
+//	for Raspberry Pi
+//
+//	Powered by XM6 TypeG Technology.
+//	Copyright (C) 2016-2020 GIMONS
+//
+//---------------------------------------------------------------------------
+
+#include "log.h"
+#include "hal/gpiobus.h"
+#include "hal/gpiobus_factory.h"
+#include "rascsi_version.h"
+#include <sys/time.h>
+#include <climits>
+#include <csignal>
+#include <sstream>
+#include <iostream>
+#include <getopt.h>
+#include <sched.h>
+#include "monitor/sm_reports.h"
+#include "monitor/sm_core.h"
+#include "monitor/data_sample.h"
+
+using namespace std;
+
+static const int _MAX_FNAME = 256;
+
+//---------------------------------------------------------------------------
+//
+//	Variable declarations
+//
+//---------------------------------------------------------------------------
+static volatile bool running; // Running flag
+shared_ptr<GPIOBUS> bus;			      // GPIO Bus
+
+uint32_t buff_size = 1000000;
+data_capture *data_buffer;
+uint32_t data_idx = 0;
+
+double ns_per_loop;
+
+bool print_help = false;
+bool import_data = false;
+
+// We don't really need to support 256 character file names - this causes
+// all kinds of compiler warnings when the log filename can be up to 256
+// characters. _MAX_FNAME/2 is just an arbitrary value.
+char file_base_name[_MAX_FNAME / 2] = "log";
+char vcd_file_name[_MAX_FNAME];
+char json_file_name[_MAX_FNAME];
+char html_file_name[_MAX_FNAME];
+char input_file_name[_MAX_FNAME];
+
+//---------------------------------------------------------------------------
+//
+//	Signal Processing
+//
+//---------------------------------------------------------------------------
+void KillHandler(int)
+{
+    // Stop instruction
+    running = false;
+}
+
+void ScsiMon::parse_arguments(const vector<char *>& args)
+{
+    int opt;
+
+    while ((opt = getopt(args.size(), args.data(), "-Hhb:i:")) != -1)
+    {
+        switch (opt)
+        {
+        // The three options below are kind of a compound option with two letters
+        case 'h':
+        case 'H':
+            print_help = true;
+            break;
+        case 'b':
+            buff_size = atoi(optarg);
+            break;
+        case 'i':
+            strncpy(input_file_name, optarg, sizeof(input_file_name)-1);
+            import_data = true;
+            break;
+        case 1:
+            strncpy(file_base_name, optarg, sizeof(file_base_name) - 5);
+            break;
+        default:
+            cout << "default: " << optarg << endl;
+            break;
+        }
+    }
+
+    /* Process any remaining command line arguments (not options). */
+    if (optind < static_cast<int>(args.size())) {
+        while (optind < static_cast<int>(args.size()))
+            strncpy(file_base_name, args[optind++], sizeof(file_base_name)-1);
+    }
+
+    strcpy(vcd_file_name, file_base_name);
+    strcat(vcd_file_name, ".vcd");
+    strcpy(json_file_name, file_base_name);
+    strcat(json_file_name, ".json");
+    strcpy(html_file_name, file_base_name);
+    strcat(html_file_name, ".html");
+}
+//---------------------------------------------------------------------------
+//
+//	Copyright text
+//
+//---------------------------------------------------------------------------
+void ScsiMon::print_copyright_text()
+{
+    LOGINFO("SCSI Monitor Capture Tool - part of RaSCSI(*^..^*) ")
+    LOGINFO("version %s (%s, %s)",
+            rascsi_get_version_string().c_str(),
+            __DATE__,
+            __TIME__)
+    LOGINFO("Powered by XM6 TypeG Technology ")
+    LOGINFO("Copyright (C) 2016-2020 GIMONS")
+    LOGINFO("Copyright (C) 2020-2022 Contributors to the RaSCSI project")
+    LOGINFO(" ")
+}
+
+//---------------------------------------------------------------------------
+//
+//	Help text
+//
+//---------------------------------------------------------------------------
+void ScsiMon::print_help_text(const vector<char *>& args)
+{
+    LOGINFO("%s -i [input file json] -b [buffer size] [output file]", args[0])
+    LOGINFO("       -i [input file json] - scsimon will parse the json file instead of capturing new data")
+    LOGINFO("                              If -i option is not specified, scsimon will read the gpio pins")
+    LOGINFO("       -b [buffer size]     - Override the default buffer size of %d.", buff_size)
+    LOGINFO("       [output file]        - Base name of the output files. The file extension (ex: .json)")
+    LOGINFO("                              will be appended to this file name")
+}
+
+//---------------------------------------------------------------------------
+//
+//	Banner Output
+//
+//---------------------------------------------------------------------------
+void ScsiMon::Banner()
+{
+    if (import_data) {
+        LOGINFO("Reading input file: %s", input_file_name)
+    }
+    else {
+        LOGINFO("Reading live data from the GPIO pins")
+        LOGINFO("    Connection type : %s", bus->GetConnectDesc().c_str())
+    }
+    LOGINFO("    Data buffer size: %u", buff_size)
+    LOGINFO(" ")
+    LOGINFO("Generating output files:")
+    LOGINFO("   %s - Value Change Dump file that can be opened with GTKWave", vcd_file_name)
+    LOGINFO("   %s - JSON file with raw data", json_file_name)
+    LOGINFO("   %s - HTML file with summary of commands", html_file_name)
+}
+
+//---------------------------------------------------------------------------
+//
+//	Initialization
+//
+//---------------------------------------------------------------------------
+bool ScsiMon::Init()
+{
+    // Interrupt handler settings
+    if (signal(SIGINT, KillHandler) == SIG_ERR) {
+        return false;
+    }
+    if (signal(SIGHUP, KillHandler) == SIG_ERR) {
+        return false;
+    }
+    if (signal(SIGTERM, KillHandler) == SIG_ERR) {
+        return false;
+    }
+
+    // GPIO Initialization
+    bus = GPIOBUS_Factory::Create();
+    if (!bus->Init())
+    {
+        LOGERROR("Unable to intiailize the GPIO bus. Exiting....")
+        return false;
+    }
+
+    // Bus Reset
+    bus->Reset();
+
+    // Other
+    running = false;
+
+    return true;
+}
+
+void ScsiMon::Cleanup()
+{
+    if (!import_data) {
+        LOGINFO("Stopping data collection....")
+    }
+    LOGINFO(" ")
+    LOGINFO("Generating %s...", vcd_file_name)
+    scsimon_generate_value_change_dump(vcd_file_name, data_buffer, data_idx);
+    LOGINFO("Generating %s...", json_file_name)
+    scsimon_generate_json(json_file_name, data_buffer, data_idx);
+    LOGINFO("Generating %s...", html_file_name)
+    scsimon_generate_html(html_file_name, data_buffer, data_idx);
+
+    // Cleanup the Bus
+    bus->Cleanup();
+}
+
+void ScsiMon::Reset()
+{
+    // Reset the bus
+    bus->Reset();
+}
+
+//---------------------------------------------------------------------------
+//
+//	Pin the thread to a specific CPU (Only applies to Linux)
+//
+//---------------------------------------------------------------------------
+#ifdef __linux__
+void ScsiMon::FixCpu(int cpu)
+{
+    // Get the number of CPUs
+    cpu_set_t cpuset;
+    CPU_ZERO(&cpuset);
+    sched_getaffinity(0, sizeof(cpu_set_t), &cpuset);
+    int cpus = CPU_COUNT(&cpuset);
+
+    // Set the thread affinity
+    if (cpu < cpus)
+    {
+        CPU_ZERO(&cpuset);
+        CPU_SET(cpu, &cpuset);
+        sched_setaffinity(0, sizeof(cpu_set_t), &cpuset);
+    }
+}
+#endif
+
+#ifdef DEBUG
+static uint32_t high_bits = 0x0;
+static uint32_t low_bits = 0xFFFFFFFF;
+#endif
+
+//---------------------------------------------------------------------------
+//
+//	Main processing
+//
+//---------------------------------------------------------------------------
+int ScsiMon::run(const vector<char *>& args)
+{
+
+#ifdef DEBUG
+    spdlog::set_level(spdlog::level::trace);
+#else
+    spdlog::set_level(spdlog::level::info);
+#endif
+    spdlog::set_pattern("%^[%l]%$ %v");
+
+    print_copyright_text();
+    parse_arguments(args);
+
+#ifdef DEBUG
+    uint32_t prev_high = high_bits;
+    uint32_t prev_low = low_bits;
+#endif
+    ostringstream s;
+    uint32_t prev_sample = 0xFFFFFFFF;
+    uint32_t this_sample = 0;
+    timeval start_time;
+    timeval stop_time;
+    uint64_t loop_count = 0;
+    timeval time_diff;
+    uint64_t elapsed_us;
+
+    if (print_help)
+    {
+        print_help_text(args);
+        exit(0);
+    }
+
+    // Output the Banner
+    Banner();
+
+    data_buffer = (data_capture *)calloc(buff_size, sizeof(data_capture_t));
+
+    if (import_data)
+    {
+        data_idx = scsimon_read_json(input_file_name, data_buffer, buff_size);
+        if (data_idx > 0)
+        {
+            LOGDEBUG("Read %d samples from %s", data_idx, input_file_name)
+            Cleanup();
+        }
+        exit(0);
+    }
+
+    LOGINFO(" ")
+    LOGINFO("Now collecting data.... Press CTRL-C to stop.")
+    LOGINFO(" ")
+
+    // Initialize
+    int ret = 0;
+    if (!Init()) {
+        ret = EPERM;
+        goto init_exit;
+    }
+
+    // Reset
+    Reset();
+
+#ifdef __linux__
+    // Set the affinity to a specific processor core
+    FixCpu(3);
+
+    // Scheduling policy setting (highest priority)
+    struct sched_param schparam;
+    schparam.sched_priority = sched_get_priority_max(SCHED_FIFO);
+    sched_setscheduler(0, SCHED_FIFO, &schparam);
+#endif
+
+    // Start execution
+    running = true;
+    bus->SetACT(false);
+
+    (void)gettimeofday(&start_time, nullptr);
+
+    // LOGDEBUG("ALL_SCSI_PINS %08X\n", ALL_SCSI_PINS)
+
+    // Main Loop
+    while (running)
+    {
+        // Work initialization
+        this_sample = (bus->Acquire());// & ALL_SCSI_PINS);
+        loop_count++;
+        if (loop_count > LLONG_MAX - 1)
+        {
+            LOGINFO("Maximum amount of time has elapsed. SCSIMON is terminating.")
+            running = false;
+        }
+        if (data_idx >= (buff_size - 2))
+        {
+            LOGINFO("Internal data buffer is full. SCSIMON is terminating.")
+            running = false;
+        }
+
+        if (this_sample != prev_sample)
+        {
+
+#ifdef DEBUG
+            // This is intended to be a debug check to see if every pin is set
+            // high and low at some point.
+            high_bits |= this_sample;
+            low_bits &= this_sample;
+            if ((high_bits != prev_high) || (low_bits != prev_low))
+            {
+                LOGDEBUG("   %08X    %08X\n", high_bits, low_bits)
+            }
+            prev_high = high_bits;
+            prev_low = low_bits;
+            if ((data_idx % 1000) == 0)
+            {
+                s.str("");
+                s << "Collected " << data_idx << " samples...";
+                LOGDEBUG("%s", s.str().c_str())
+            }
+#endif
+            data_buffer[data_idx].data = this_sample;
+            data_buffer[data_idx].timestamp = loop_count;
+            data_idx++;
+            prev_sample = this_sample;
+        }
+
+        continue;
+    }
+
+    // Collect one last sample, otherwise it looks like the end of the data was cut off
+    if (data_idx < buff_size)
+    {
+        data_buffer[data_idx].data = this_sample;
+        data_buffer[data_idx].timestamp = loop_count;
+        data_idx++;
+    }
+
+    (void)gettimeofday(&stop_time, nullptr);
+
+    timersub(&stop_time, &start_time, &time_diff);
+
+    elapsed_us = ((time_diff.tv_sec * 1000000) + time_diff.tv_usec);
+    s.str("");
+    s << "Elapsed time: " << elapsed_us << " microseconds (" << elapsed_us / 1000000 << " seconds)";
+    LOGINFO("%s", s.str().c_str())
+    s.str("");
+    s << "Collected " << data_idx << " changes";
+    LOGINFO("%s", s.str().c_str())
+
+    // Note: ns_per_loop is a global variable that is used by Cleanup() to printout the timestamps.
+    ns_per_loop = (elapsed_us * 1000) / (double)loop_count;
+    s.str("");
+    s << "Read the SCSI bus " << loop_count << " times with an average of " << ns_per_loop << " ns for each read";
+    LOGINFO("%s", s.str().c_str())
+
+    Cleanup();
+
+init_exit:
+    exit(ret);
+}
diff --git a/cpp/monitor/sm_core.h b/cpp/monitor/sm_core.h
new file mode 100644
index 00000000..a39354b6
--- /dev/null
+++ b/cpp/monitor/sm_core.h
@@ -0,0 +1,35 @@
+//---------------------------------------------------------------------------
+//
+// SCSI Target Emulator RaSCSI Reloaded
+// for Raspberry Pi
+//
+// Copyright (C) 2022 Uwe Seimet
+//
+//---------------------------------------------------------------------------
+
+#pragma once
+
+#include <vector>
+
+using namespace std;
+
+class ScsiMon
+{
+public:
+
+	ScsiMon() = default;
+	~ScsiMon() = default;
+
+	int run(const vector<char *>&);
+
+private:
+
+	void parse_arguments(const vector<char *>&);
+	void print_copyright_text();
+	void print_help_text(const vector<char *>&);
+	void Banner();
+	bool Init();
+	void Cleanup();
+	void Reset();
+	void FixCpu(int);
+};
diff --git a/cpp/protobuf_util.cpp b/cpp/protobuf_util.cpp
index 1aa534d1..aa8024bb 100644
--- a/cpp/protobuf_util.cpp
+++ b/cpp/protobuf_util.cpp
@@ -27,13 +27,9 @@ void protobuf_util::ParseParameters(PbDeviceDefinition& device, const string& pa
 		return;
 	}
 
-	// Old style parameters, for backwards compatibility only.
-	// Only one of these parameters will be used by rascsi, depending on the device type.
+	// Old style parameter (filename), for backwards compatibility only
 	if (params.find(KEY_VALUE_SEPARATOR) == string::npos) {
 		SetParam(device, "file", params);
-		if (params != "bridge" && params != "daynaport" && params != "printer" && params != "services") {
-			SetParam(device, "interfaces", params);
-		}
 
 		return;
 	}
diff --git a/cpp/rascsi.cpp b/cpp/rascsi.cpp
index 3c9e8b51..3871afe7 100644
--- a/cpp/rascsi.cpp
+++ b/cpp/rascsi.cpp
@@ -1,722 +1,19 @@
 //---------------------------------------------------------------------------
 //
-//	SCSI Target Emulator RaSCSI Reloaded
-//	for Raspberry Pi
+// SCSI Target Emulator RaSCSI Reloaded
+// for Raspberry Pi
 //
-//	Powered by XM6 TypeG Technology.
-//	Copyright (C) 2016-2020 GIMONS
-//	Copyright (C) 2020-2022 Contributors to the RaSCSI project
-//	[ RaSCSI main ]
+// Copyright (C) 2022 Uwe Seimet
 //
 //---------------------------------------------------------------------------
 
-#include "config.h"
-#include "log.h"
-#include "controllers/controller_manager.h"
-#include "controllers/scsi_controller.h"
-#include "devices/device_factory.h"
-#include "devices/disk.h"
-#include "hal/gpiobus.h"
-#include "hal/gpiobus_factory.h"
-#include "hal/sbc_version.h"
-#include "hal/systimer.h"
-#include "list_devices.h"
-#include "rascsi_version.h"
-#include "rascsi_exceptions.h"
-#include "protobuf_serializer.h"
-#include "protobuf_util.h"
-#include "rascsi_interface.pb.h"
-#include "rascsi/rascsi_executor.h"
-#include "rascsi/rascsi_response.h"
-#include "rascsi/rascsi_image.h"
-#include "rascsi/rascsi_service.h"
-#include "rasutil.h"
-#include "spdlog/sinks/stdout_color_sinks.h"
-#include <netinet/in.h>
-#include <csignal>
-#include <string>
-#include <sstream>
-#include <iostream>
-#include <fstream>
-#include <list>
-#include <map>
-#include <deque>
+#include "rascsi/rascsi_core.h"
 
 using namespace std;
-using namespace spdlog;
-using namespace rascsi_interface;
-using namespace ras_util;
-using namespace protobuf_util;
 
-//---------------------------------------------------------------------------
-//
-//  Constant declarations
-//
-//---------------------------------------------------------------------------
-static const int DEFAULT_PORT = 6868;
-static const char COMPONENT_SEPARATOR = ':';
-
-//---------------------------------------------------------------------------
-//
-//	Variable declarations
-//
-//---------------------------------------------------------------------------
-static volatile bool active;		// Processing flag
-RascsiService service;
-shared_ptr<GPIOBUS> bus;
-string current_log_level = "info";	// Some versions of spdlog do not support get_log_level()
-string access_token;
-DeviceFactory device_factory;
-shared_ptr<ControllerManager> controller_manager;
-RascsiImage rascsi_image;
-shared_ptr<RascsiResponse> rascsi_response;
-shared_ptr<RascsiExecutor> executor;
-const ProtobufSerializer serializer;
-
-using optarg_value_type = std::pair<int,string>;
-using optarg_queue_type = std::deque<optarg_value_type>;
-
-void Banner(int argc, char* argv[])
+int main(int argc, char *argv[])
 {
-	cout << Banner("Reloaded");
-	cout << "Connect type: " << bus->GetConnectDesc() << '\n' << flush;
+	const vector<char *> args(argv, argv + argc);
 
-	if ((argc > 1 && strcmp(argv[1], "-h") == 0) || (argc > 1 && strcmp(argv[1], "--help") == 0)){
-		cout << "\nUsage: " << argv[0] << " [-idn[:m] FILE] ...\n\n";
-		cout << " n is SCSI device ID (0-7).\n";
-		cout << " m is the optional logical unit (LUN) (0-31).\n";
-		cout << " FILE is a disk image file, \"daynaport\", \"bridge\", \"printer\" or \"services\".\n\n";
-		cout << " Image type is detected based on file extension if no explicit type is specified.\n";
-		cout << "  hd1 : SCSI-1 HD image (Non-removable generic SCSI-1 HD image)\n";
-		cout << "  hds : SCSI HD image (Non-removable generic SCSI HD image)\n";
-		cout << "  hdr : SCSI HD image (Removable generic HD image)\n";
-		cout << "  hda : SCSI HD image (Apple compatible image)\n";
-		cout << "  hdn : SCSI HD image (NEC compatible image)\n";
-		cout << "  hdi : SCSI HD image (Anex86 HD image)\n";
-		cout << "  nhd : SCSI HD image (T98Next HD image)\n";
-		cout << "  mos : SCSI MO image (MO image)\n";
-		cout << "  iso : SCSI CD image (ISO 9660 image)\n" << flush;
-
-		exit(EXIT_SUCCESS);
-	}
-}
-
-bool InitBus()
-{
-#ifdef USE_SEL_EVENT_ENABLE
-	SBC_Version::Init();
-#endif
-
-	// GPIOBUS creation
-	bus = GPIOBUS_Factory::Create();
-
-	controller_manager = make_shared<ControllerManager>(bus);
-	rascsi_response = make_shared<RascsiResponse>(device_factory, *controller_manager, ScsiController::LUN_MAX);
-	executor  = make_shared<RascsiExecutor>(*rascsi_response, rascsi_image, device_factory, *controller_manager);
-
-	// GPIO Initialization
-	if (!bus->Init()) {
-		return false;
-	}
-
-	bus->Reset();
-
-	return true;
-}
-
-void Cleanup()
-{
-	executor->DetachAll();
-
-	service.Cleanup();
-
-	bus->Cleanup();
-}
-
-void Reset()
-{
-	controller_manager->ResetAllControllers();
-
-	bus->Reset();
-}
-
-bool ReadAccessToken(const char *filename)
-{
-	struct stat st;
-	if (stat(filename, &st) || !S_ISREG(st.st_mode)) {
-		cerr << "Can't access token file '" << optarg << "'" << endl;
-		return false;
-	}
-
-	if (st.st_uid || st.st_gid || (st.st_mode & (S_IROTH | S_IWOTH | S_IRGRP | S_IWGRP))) {
-		cerr << "Access token file '" << optarg << "' must be owned by root and readable by root only" << endl;
-		return false;
-	}
-
-	ifstream token_file(filename);
-	if (token_file.fail()) {
-		cerr << "Can't open access token file '" << optarg << "'" << endl;
-		return false;
-	}
-
-	getline(token_file, access_token);
-	if (token_file.fail()) {
-		cerr << "Can't read access token file '" << optarg << "'" << endl;
-		return false;
-	}
-
-	if (access_token.empty()) {
-		cerr << "Access token file '" << optarg << "' must not be empty" << endl;
-		return false;
-	}
-
-	return true;
-}
-
-void LogDevices(string_view devices)
-{
-	stringstream ss(devices.data());
-	string line;
-
-	while (getline(ss, line, '\n')) {
-		LOGINFO("%s", line.c_str())
-	}
-}
-
-void TerminationHandler(int signum)
-{
-	Cleanup();
-
-	exit(signum);
-}
-
-bool ProcessId(const string& id_spec, int& id, int& unit)
-{
-	if (const size_t separator_pos = id_spec.find(COMPONENT_SEPARATOR); separator_pos == string::npos) {
-		if (!GetAsInt(id_spec, id) || id < 0 || id >= 8) {
-			cerr << optarg << ": Invalid device ID (0-7)" << endl;
-			return false;
-		}
-
-		unit = 0;
-	}
-	else if (!GetAsInt(id_spec.substr(0, separator_pos), id) || id < 0 || id > 7 ||
-			!GetAsInt(id_spec.substr(separator_pos + 1), unit) || unit < 0 || unit >= ScsiController::LUN_MAX) {
-		cerr << optarg << ": Invalid unit (0-" << (ScsiController::LUN_MAX - 1) << ")" << endl;
-		return false;
-	}
-
-	return true;
-}
-
-bool ParseArgument(int argc, char* argv[], int& port, optarg_queue_type& post_process)
-{
-	int block_size = 0;
-	string name;
-
-	opterr = 1;
-	int opt;
-	while ((opt = getopt(argc, argv, "-Iib:d:n:p:r:t:z:D:F:L:P:R:C:v")) != -1) {
-		switch (opt) {
-			// The following options can not be processed until AFTER
-			// the 'bus' object is created and configured
-			case 'i':
-			case 'I':
-			case 'd':
-			case 'D':
-			case 'R':
-			case 'n':
-			case 'r':
-			case 't':
-			case 'F': 
-			case 'z':
-			{
-				string optarg_str = (optarg == nullptr) ? "" : string(optarg);
-				post_process.push_back(optarg_value_type(opt,optarg_str));
-				continue;
-			}
-			case 'b': {
-				if (!GetAsInt(optarg, block_size)) {
-					cerr << "Invalid block size " << optarg << endl;
-					return false;
-				}
-				continue;
-			}
-
-			case 'L':
-				current_log_level = optarg;
-				continue;
-
-			case 'p':
-				if (!GetAsInt(optarg, port) || port <= 0 || port > 65535) {
-					cerr << "Invalid port " << optarg << ", port must be between 1 and 65535" << endl;
-					return false;
-				}
-				continue;
-
-			case 'P':
-				if (!ReadAccessToken(optarg)) {
-					return false;
-				}
-				continue;
-			
-			case 'v':
-				cout << rascsi_get_version_string() << endl;
-				exit(0);
-
-			case 1:
-			{
-				// Encountered filename
-				string optarg_str = (optarg == nullptr) ? "" : string(optarg);
-				post_process.push_back(optarg_value_type(opt,optarg_str));
-				continue;
-			}
-			default:
-				return false;
-		}
-
-		if (optopt) {
-			return false;
-		}
-	}
-
-	return true;
-}
-
-
-static bool CreateInitialDevices(optarg_queue_type& optarg_queue){
-	PbCommand command;
-	int id = -1;
-	int unit = -1;
-	PbDeviceType type = UNDEFINED;
-	int block_size = 0;
-	string name;
-	string log_level;
-
-	const char *locale = setlocale(LC_MESSAGES, "");
-	if (locale == nullptr || !strcmp(locale, "C")) {
-		locale = "en";
-	}
-
-
-	opterr = 1;
-	for(auto current_arg : optarg_queue){
-		switch (current_arg.first) {
-			// The two options below are kind of a compound option with two letters
-			case 'i':
-			case 'I':
-				id = -1;
-				unit = -1;
-				continue;
-
-			case 'd':
-			case 'D': {
-				if (!ProcessId(current_arg.second, id, unit)) {
-					return false;
-				}
-				continue;
-			}
-
-			case 'z':
-				locale = current_arg.second.c_str();
-				continue;
-
-			case 'F': {
-				if (const string result = rascsi_image.SetDefaultFolder(current_arg.second); !result.empty()) {
-					cerr << result << endl;
-					return false;
-				}
-				continue;
-			}
-
-			case 'R':
-				int depth;
-				if (!GetAsInt(current_arg.second, depth) || depth < 0) {
-					cerr << "Invalid image file scan depth " << current_arg.second << endl;
-					return false;
-				}
-				rascsi_image.SetDepth(depth);
-				continue;
-
-			case 'n':
-				name = current_arg.second;
-				continue;
-
-			case 'r': {
-					string error = executor->SetReservedIds(current_arg.second);
-					if (!error.empty()) {
-						cerr << error << endl;
-						return false;
-					}
-				}
-				continue;
-
-			case 't': {
-					string t = current_arg.second;
-					transform(t.begin(), t.end(), t.begin(), ::toupper);
-					if (!PbDeviceType_Parse(t, &type)) {
-						cerr << "Illegal device type '" << current_arg.second << "'" << endl;
-						return false;
-					}
-				}
-				continue;
-
-			case 1:
-				// Encountered filename
-				break;
-		}
-
-		// Set up the device data
-		PbDeviceDefinition *device = command.add_devices();
-		device->set_id(id);
-		device->set_unit(unit);
-		device->set_type(type);
-		device->set_block_size(block_size);
-
-		ParseParameters(*device, current_arg.second);
-
-		if (size_t separator_pos = name.find(COMPONENT_SEPARATOR); separator_pos != string::npos) {
-			device->set_vendor(name.substr(0, separator_pos));
-			name = name.substr(separator_pos + 1);
-			separator_pos = name.find(COMPONENT_SEPARATOR);
-			if (separator_pos != string::npos) {
-				device->set_product(name.substr(0, separator_pos));
-				device->set_revision(name.substr(separator_pos + 1));
-			}
-			else {
-				device->set_product(name);
-			}
-		}
-		else {
-			device->set_vendor(name);
-		}
-
-		id = -1;
-		type = UNDEFINED;
-		block_size = 0;
-		name = "";
-	}
-
-	// Attach all specified devices
-	command.set_operation(ATTACH);
-
-	if (CommandContext context(locale); !executor->ProcessCmd(context, command)) {
-		return false;
-	}
-
-	// Display and log the device list
-	PbServerInfo server_info;
-	rascsi_response->GetDevices(server_info, rascsi_image.GetDefaultFolder());
-	const list<PbDevice>& devices = { server_info.devices_info().devices().begin(), server_info.devices_info().devices().end() };
-	const string device_list = ListDevices(devices);
-	LogDevices(device_list);
-	cout << device_list << flush;
-
-	return true;
-}
-
-static bool ExecuteCommand(const CommandContext& context, PbCommand& command)
-{
-	if (!access_token.empty() && access_token != GetParam(command, "token")) {
-		return context.ReturnLocalizedError(LocalizationKey::ERROR_AUTHENTICATION, UNAUTHORIZED);
-	}
-
-	if (!PbOperation_IsValid(command.operation())) {
-		LOGERROR("Received unknown command with operation opcode %d", command.operation())
-
-		return context.ReturnLocalizedError(LocalizationKey::ERROR_OPERATION, UNKNOWN_OPERATION);
-	}
-
-	LOGTRACE("Received %s command", PbOperation_Name(command.operation()).c_str())
-
-	PbResult result;
-
-	switch(command.operation()) {
-		case LOG_LEVEL: {
-			const string log_level = GetParam(command, "level");
-			if (const bool status = executor->SetLogLevel(log_level); !status) {
-				context.ReturnLocalizedError(LocalizationKey::ERROR_LOG_LEVEL, log_level);
-			}
-			else {
-				current_log_level = log_level;
-
-				context.ReturnStatus();
-			}
-			break;
-		}
-
-		case DEFAULT_FOLDER: {
-			if (const string status = rascsi_image.SetDefaultFolder(GetParam(command, "folder")); !status.empty()) {
-				context.ReturnStatus(false, status);
-			}
-			else {
-				context.ReturnStatus();
-			}
-			break;
-		}
-
-		case DEVICES_INFO: {
-			rascsi_response->GetDevicesInfo(result, command, rascsi_image.GetDefaultFolder());
-			serializer.SerializeMessage(context.GetFd(), result);
-			break;
-		}
-
-		case DEVICE_TYPES_INFO: {
-			result.set_allocated_device_types_info(rascsi_response->GetDeviceTypesInfo(result).release());
-			serializer.SerializeMessage(context.GetFd(), result);
-			break;
-		}
-
-		case SERVER_INFO: {
-			result.set_allocated_server_info(rascsi_response->GetServerInfo(
-					result, executor->GetReservedIds(), current_log_level, rascsi_image.GetDefaultFolder(),
-					GetParam(command, "folder_pattern"), GetParam(command, "file_pattern"),
-					rascsi_image.GetDepth()).release());
-			serializer.SerializeMessage(context.GetFd(), result);
-			break;
-		}
-
-		case VERSION_INFO: {
-			result.set_allocated_version_info(rascsi_response->GetVersionInfo(result).release());
-			serializer.SerializeMessage(context.GetFd(), result);
-			break;
-		}
-
-		case LOG_LEVEL_INFO: {
-			result.set_allocated_log_level_info(rascsi_response->GetLogLevelInfo(result, current_log_level).release());
-			serializer.SerializeMessage(context.GetFd(), result);
-			break;
-		}
-
-		case DEFAULT_IMAGE_FILES_INFO: {
-			result.set_allocated_image_files_info(rascsi_response->GetAvailableImages(result,
-					rascsi_image.GetDefaultFolder(), GetParam(command, "folder_pattern"),
-					GetParam(command, "file_pattern"), rascsi_image.GetDepth()).release());
-			serializer.SerializeMessage(context.GetFd(), result);
-			break;
-		}
-
-		case IMAGE_FILE_INFO: {
-			if (string filename = GetParam(command, "file"); filename.empty()) {
-				context.ReturnLocalizedError( LocalizationKey::ERROR_MISSING_FILENAME);
-			}
-			else {
-				auto image_file = make_unique<PbImageFile>();
-				const bool status = rascsi_response->GetImageFile(*image_file.get(), rascsi_image.GetDefaultFolder(), filename);
-				if (status) {
-					result.set_status(true);
-					result.set_allocated_image_file_info(image_file.get());
-					serializer.SerializeMessage(context.GetFd(), result);
-				}
-				else {
-					context.ReturnLocalizedError(LocalizationKey::ERROR_IMAGE_FILE_INFO);
-				}
-			}
-			break;
-		}
-
-		case NETWORK_INTERFACES_INFO: {
-			result.set_allocated_network_interfaces_info(rascsi_response->GetNetworkInterfacesInfo(result).release());
-			serializer.SerializeMessage(context.GetFd(), result);
-			break;
-		}
-
-		case MAPPING_INFO: {
-			result.set_allocated_mapping_info(rascsi_response->GetMappingInfo(result).release());
-			serializer.SerializeMessage(context.GetFd(), result);
-			break;
-		}
-
-		case OPERATION_INFO: {
-			result.set_allocated_operation_info(rascsi_response->GetOperationInfo(result,
-					rascsi_image.GetDepth()).release());
-			serializer.SerializeMessage(context.GetFd(), result);
-			break;
-		}
-
-		case RESERVED_IDS_INFO: {
-			result.set_allocated_reserved_ids_info(rascsi_response->GetReservedIds(result,
-					executor->GetReservedIds()).release());
-			serializer.SerializeMessage(context.GetFd(), result);
-			break;
-		}
-
-		case SHUT_DOWN: {
-			if (executor->ShutDown(context, GetParam(command, "mode"))) {
-				TerminationHandler(0);
-			}
-			break;
-		}
-
-		default: {
-			// Wait until we become idle
-			const timespec ts = { .tv_sec = 0, .tv_nsec = 500'000'000};
-			while (active) {
-				nanosleep(&ts, nullptr);
-			}
-
-			executor->ProcessCmd(context, command);
-			break;
-		}
-	}
-
-	return true;
-}
-
-int main(int argc, char* argv[])
-{
-	optarg_queue_type optarg_queue;
-	GOOGLE_PROTOBUF_VERIFY_VERSION;
-
-	// added setvbuf to override stdout buffering, so logs are written immediately and not when the process exits.
-	setvbuf(stdout, nullptr, _IONBF, 0);
-
-	// Output the Banner
-	Banner(argc, argv);
-
-	int port = DEFAULT_PORT;
-	if (!ParseArgument(argc, argv, port, optarg_queue)) {
-		return -1;
-	}
-
-	// Note that current_log_level may have been modified by ParseArgument()
-	executor->SetLogLevel(current_log_level);
-
-	// Create a thread-safe stdout logger to process the log messages
-	const auto logger = stdout_color_mt("rascsi stdout logger");
-
-	if (!InitBus()) {
-		return EPERM;
-	}
-
-	if (!service.Init(&ExecuteCommand, port)) {
-		return EPERM;
-	}
-
-	// We need to wait to create the devices until after the bus/controller/etc
-	// objects have been created.
-	if (!CreateInitialDevices(optarg_queue)) {
-		Cleanup();
-		return -1;
-	}
-
-	// Signal handler to detach all devices on a KILL or TERM signal
-	struct sigaction termination_handler;
-	termination_handler.sa_handler = TerminationHandler;
-	sigemptyset(&termination_handler.sa_mask);
-	termination_handler.sa_flags = 0;
-	sigaction(SIGINT, &termination_handler, nullptr);
-	sigaction(SIGTERM, &termination_handler, nullptr);
-
-	// Reset
-	Reset();
-
-    // Set the affinity to a specific processor core
-	FixCpu(3);
-
-	sched_param schparam;
-#ifdef USE_SEL_EVENT_ENABLE
-	// Scheduling policy setting (highest priority)
-	schparam.sched_priority = sched_get_priority_max(SCHED_FIFO);
-	sched_setscheduler(0, SCHED_FIFO, &schparam);
-#else
-	cout << "Note: No RaSCSI hardware support, only client interface calls are supported" << endl;
-#endif
-
-	// Start execution
-	service.SetRunning(true);
-
-	// Main Loop
-	while (service.IsRunning()) {
-#ifdef USE_SEL_EVENT_ENABLE
-		// SEL signal polling
-		if (!bus->PollSelectEvent()) {
-			// Stop on interrupt
-			if (errno == EINTR) {
-				break;
-			}
-			continue;
-		}
-
-		// Get the bus
-		bus->Acquire();
-#else
-		bus->Acquire();
-		if (!bus->GetSEL()) {
-			const timespec ts = { .tv_sec = 0, .tv_nsec = 0};
-			nanosleep(&ts, nullptr);
-			continue;
-		}
-#endif
-
-        // Wait until BSY is released as there is a possibility for the
-        // initiator to assert it while setting the ID (for up to 3 seconds)
-		if (bus->GetBSY()) {
-			const uint32_t now = SysTimer::GetTimerLow();
-			while ((SysTimer::GetTimerLow() - now) < 3'000'000) {
-				bus->Acquire();
-				if (!bus->GetBSY()) {
-					break;
-				}
-			}
-		}
-
-		// Stop because the bus is busy or another device responded
-		if (bus->GetBSY() || !bus->GetSEL()) {
-			continue;
-		}
-
-		int initiator_id = -1;
-
-		// The initiator and target ID
-		const uint8_t id_data = bus->GetDAT();
-
-		BUS::phase_t phase = BUS::phase_t::busfree;
-
-		// Identify the responsible controller
-		shared_ptr<AbstractController> controller = controller_manager->IdentifyController(id_data);
-		if (controller != nullptr) {
-			initiator_id = controller->ExtractInitiatorId(id_data);
-
-			if (controller->Process(initiator_id) == BUS::phase_t::selection) {
-				phase = BUS::phase_t::selection;
-			}
-		}
-
-		// Return to bus monitoring if the selection phase has not started
-		if (phase != BUS::phase_t::selection) {
-			continue;
-		}
-
-		// Start target device
-		active = true;
-
-#if !defined(USE_SEL_EVENT_ENABLE) && defined(__linux__)
-		// Scheduling policy setting (highest priority)
-		schparam.sched_priority = sched_get_priority_max(SCHED_FIFO);
-		sched_setscheduler(0, SCHED_FIFO, &schparam);
-#endif
-
-		// Loop until the bus is free
-		while (service.IsRunning()) {
-			// Target drive
-			phase = controller->Process(initiator_id);
-
-			// End when the bus is free
-			if (phase == BUS::phase_t::busfree) {
-				break;
-			}
-		}
-
-#if !defined(USE_SEL_EVENT_ENABLE) && defined(__linux__)
-		// Set the scheduling priority back to normal
-		schparam.sched_priority = 0;
-		sched_setscheduler(0, SCHED_OTHER, &schparam);
-#endif
-
-		// End the target travel
-		active = false;
-	}
-
-	return 0;
+	return Rascsi().run(args);
 }
diff --git a/cpp/rascsi/rascsi_core.cpp b/cpp/rascsi/rascsi_core.cpp
new file mode 100644
index 00000000..622b0988
--- /dev/null
+++ b/cpp/rascsi/rascsi_core.cpp
@@ -0,0 +1,680 @@
+//---------------------------------------------------------------------------
+//
+//	SCSI Target Emulator RaSCSI Reloaded
+//	for Raspberry Pi
+//
+//	Powered by XM6 TypeG Technology.
+//	Copyright (C) 2016-2020 GIMONS
+//	Copyright (C) 2020-2022 Contributors to the RaSCSI project
+//	[ RaSCSI main ]
+//
+//---------------------------------------------------------------------------
+
+#include "config.h"
+#include "log.h"
+#include "controllers/controller_manager.h"
+#include "controllers/scsi_controller.h"
+#include "devices/device_factory.h"
+#include "devices/storage_device.h"
+#include "hal/board_type.h"
+#include "hal/gpiobus.h"
+#include "hal/gpiobus_factory.h"
+#include "hal/sbc_version.h"
+#include "hal/systimer.h"
+#include "list_devices.h"
+#include "rascsi_version.h"
+#include "protobuf_serializer.h"
+#include "protobuf_util.h"
+#include "rascsi/rascsi_executor.h"
+#include "rascsi/rascsi_core.h"
+#include "rasutil.h"
+#include "spdlog/sinks/stdout_color_sinks.h"
+#include <netinet/in.h>
+#include <csignal>
+#include <string>
+#include <sstream>
+#include <iostream>
+#include <fstream>
+#include <list>
+
+using namespace std;
+using namespace spdlog;
+using namespace rascsi_interface;
+using namespace ras_util;
+using namespace protobuf_util;
+
+void Rascsi::Banner(const vector<char *>& args) const
+{
+	cout << ras_util::Banner("Reloaded");
+
+	if ((args.size() > 1 && strcmp(args[1], "-h") == 0) || (args.size() > 1 && strcmp(args[1], "--help") == 0)){
+		cout << "\nUsage: " << args[0] << " [-idn[:m] FILE] ...\n\n";
+		cout << " n is SCSI device ID (0-7).\n";
+		cout << " m is the optional logical unit (LUN) (0-31).\n";
+		cout << " FILE is a disk image file, \"daynaport\", \"bridge\", \"printer\" or \"services\".\n\n";
+		cout << " Image type is detected based on file extension if no explicit type is specified.\n";
+		cout << "  hd1 : SCSI-1 HD image (Non-removable generic SCSI-1 HD image)\n";
+		cout << "  hds : SCSI HD image (Non-removable generic SCSI HD image)\n";
+		cout << "  hdr : SCSI HD image (Removable generic HD image)\n";
+		cout << "  hda : SCSI HD image (Apple compatible image)\n";
+		cout << "  hdn : SCSI HD image (NEC compatible image)\n";
+		cout << "  hdi : SCSI HD image (Anex86 HD image)\n";
+		cout << "  nhd : SCSI HD image (T98Next HD image)\n";
+		cout << "  mos : SCSI MO image (MO image)\n";
+		cout << "  iso : SCSI CD image (ISO 9660 image)\n" << flush;
+
+		exit(EXIT_SUCCESS);
+	}
+}
+
+bool Rascsi::InitBus() const
+{
+#ifdef USE_SEL_EVENT_ENABLE
+	SBC_Version::Init();
+#endif
+
+	// GPIOBUS creation
+	bus = GPIOBUS_Factory::Create();
+
+	// GPIO Initialization
+	if (!bus->Init(BUS::mode_e::TARGET, board_type::rascsi_board_type_e::BOARD_TYPE_FULLSPEC)) {
+		return false;
+	}
+
+	bus->Reset();
+
+	auto b = bus;
+	controller_manager = make_shared<ControllerManager>(*b);
+	auto c = controller_manager;
+	executor = make_shared<RascsiExecutor>(rascsi_image, *c);
+
+	return true;
+}
+
+void Rascsi::Cleanup()
+{
+	executor->DetachAll();
+
+	service.Cleanup();
+
+	bus->Cleanup();
+}
+
+bool Rascsi::ReadAccessToken(const char *filename) const
+{
+	struct stat st;
+	if (stat(filename, &st) || !S_ISREG(st.st_mode)) {
+		cerr << "Can't access token file '" << optarg << "'" << endl;
+		return false;
+	}
+
+	if (st.st_uid || st.st_gid || (st.st_mode & (S_IROTH | S_IWOTH | S_IRGRP | S_IWGRP))) {
+		cerr << "Access token file '" << optarg << "' must be owned by root and readable by root only" << endl;
+		return false;
+	}
+
+	ifstream token_file(filename);
+	if (token_file.fail()) {
+		cerr << "Can't open access token file '" << optarg << "'" << endl;
+		return false;
+	}
+
+	getline(token_file, access_token);
+	if (token_file.fail()) {
+		cerr << "Can't read access token file '" << optarg << "'" << endl;
+		return false;
+	}
+
+	if (access_token.empty()) {
+		cerr << "Access token file '" << optarg << "' must not be empty" << endl;
+		return false;
+	}
+
+	return true;
+}
+
+void Rascsi::LogDevices(string_view devices) const
+{
+	stringstream ss(devices.data());
+	string line;
+
+	while (getline(ss, line, '\n')) {
+		LOGINFO("%s", line.c_str())
+	}
+}
+
+void Rascsi::TerminationHandler(int)
+{
+	Cleanup();
+}
+
+bool Rascsi::ProcessId(const string& id_spec, int& id, int& unit) const
+{
+	if (const size_t separator_pos = id_spec.find(COMPONENT_SEPARATOR); separator_pos == string::npos) {
+		if (!GetAsInt(id_spec, id) || id < 0 || id >= 8) {
+			cerr << optarg << ": Invalid device ID (0-7)" << endl;
+			return false;
+		}
+
+		unit = 0;
+	}
+	else if (!GetAsInt(id_spec.substr(0, separator_pos), id) || id < 0 || id > 7 ||
+			!GetAsInt(id_spec.substr(separator_pos + 1), unit) || unit < 0 || unit >= ScsiController::LUN_MAX) {
+		cerr << optarg << ": Invalid unit (0-" << (ScsiController::LUN_MAX - 1) << ")" << endl;
+		return false;
+	}
+
+	return true;
+}
+
+bool Rascsi::ParseArguments(const vector<char *>& args, int& port, optarg_queue_type& post_process) const
+{
+	int block_size = 0;
+	string name;
+
+	opterr = 1;
+	int opt;
+	while ((opt = getopt(static_cast<int>(args.size()), args.data(), "-Iib:d:n:p:r:t:z:D:F:L:P:R:C:v")) != -1) {
+		switch (opt) {
+			// The following options can not be processed until AFTER
+			// the 'bus' object is created and configured
+			case 'i':
+			case 'I':
+			case 'd':
+			case 'D':
+			case 'R':
+			case 'n':
+			case 'r':
+			case 't':
+			case 'F':
+			case 'z':
+			{
+				const string optarg_str = optarg == nullptr ? "" : optarg;
+				post_process.emplace_back(opt, optarg_str);
+				continue;
+			}
+
+			case 'b': {
+				if (!GetAsInt(optarg, block_size)) {
+					cerr << "Invalid block size " << optarg << endl;
+					return false;
+				}
+				continue;
+			}
+
+			case 'L':
+				current_log_level = optarg;
+				continue;
+
+			case 'p':
+				if (!GetAsInt(optarg, port) || port <= 0 || port > 65535) {
+					cerr << "Invalid port " << optarg << ", port must be between 1 and 65535" << endl;
+					return false;
+				}
+				continue;
+
+			case 'P':
+				if (!ReadAccessToken(optarg)) {
+					return false;
+				}
+				continue;
+
+			case 'v':
+				cout << rascsi_get_version_string() << endl;
+				exit(0);
+
+			case 1:
+			{
+				// Encountered filename
+				const string optarg_str = (optarg == nullptr) ? "" : string(optarg);
+				post_process.emplace_back(opt, optarg_str);
+				continue;
+			}
+
+			default:
+				return false;
+		}
+
+		if (optopt) {
+			return false;
+		}
+	}
+
+	return true;
+}
+
+bool Rascsi::CreateInitialDevices(const optarg_queue_type& optarg_queue) const
+{
+	PbCommand command;
+	int id = -1;
+	int unit = -1;
+	PbDeviceType type = UNDEFINED;
+	int block_size = 0;
+	string name;
+	string log_level;
+
+	const char *locale = setlocale(LC_MESSAGES, "");
+	if (locale == nullptr || !strcmp(locale, "C")) {
+		locale = "en";
+	}
+
+
+	opterr = 1;
+	for (const auto& [option, value] : optarg_queue) {
+		switch (option) {
+			// The two options below are kind of a compound option with two letters
+			case 'i':
+			case 'I':
+				id = -1;
+				unit = -1;
+				continue;
+
+			case 'd':
+			case 'D': {
+				if (!ProcessId(value, id, unit)) {
+					return false;
+				}
+				continue;
+			}
+
+			case 'z':
+				locale = value.c_str();
+				continue;
+
+			case 'F': {
+				if (const string result = rascsi_image.SetDefaultFolder(value); !result.empty()) {
+					cerr << result << endl;
+					return false;
+				}
+				continue;
+			}
+
+			case 'R':
+				int depth;
+				if (!GetAsInt(value, depth) || depth < 0) {
+					cerr << "Invalid image file scan depth " << value << endl;
+					return false;
+				}
+				rascsi_image.SetDepth(depth);
+				continue;
+
+			case 'n':
+				name = value;
+				continue;
+
+			case 'r': {
+					string error = executor->SetReservedIds(value);
+					if (!error.empty()) {
+						cerr << error << endl;
+						return false;
+					}
+				}
+				continue;
+
+			case 't': {
+					string t = value;
+					transform(t.begin(), t.end(), t.begin(), ::toupper);
+					if (!PbDeviceType_Parse(t, &type)) {
+						cerr << "Illegal device type '" << value << "'" << endl;
+						return false;
+					}
+				}
+				continue;
+
+			case 1:
+				// Encountered filename
+				break;
+
+			default:
+				return false;
+		}
+
+		// Set up the device data
+		PbDeviceDefinition *device = command.add_devices();
+		device->set_id(id);
+		device->set_unit(unit);
+		device->set_type(type);
+		device->set_block_size(block_size);
+
+		ParseParameters(*device, value);
+
+		if (size_t separator_pos = name.find(COMPONENT_SEPARATOR); separator_pos != string::npos) {
+			device->set_vendor(name.substr(0, separator_pos));
+			name = name.substr(separator_pos + 1);
+			separator_pos = name.find(COMPONENT_SEPARATOR);
+			if (separator_pos != string::npos) {
+				device->set_product(name.substr(0, separator_pos));
+				device->set_revision(name.substr(separator_pos + 1));
+			}
+			else {
+				device->set_product(name);
+			}
+		}
+		else {
+			device->set_vendor(name);
+		}
+
+		id = -1;
+		type = UNDEFINED;
+		block_size = 0;
+		name = "";
+	}
+
+	// Attach all specified devices
+	command.set_operation(ATTACH);
+
+	if (CommandContext context(locale); !executor->ProcessCmd(context, command)) {
+		return false;
+	}
+
+	// Display and log the device list
+	PbServerInfo server_info;
+	rascsi_response.GetDevices(controller_manager->GetAllDevices(), server_info, rascsi_image.GetDefaultFolder());
+	const list<PbDevice>& devices = { server_info.devices_info().devices().begin(), server_info.devices_info().devices().end() };
+	const string device_list = ListDevices(devices);
+	LogDevices(device_list);
+	cout << device_list << flush;
+
+	return true;
+}
+
+bool Rascsi::ExecuteCommand(const CommandContext& context, const PbCommand& command)
+{
+	if (!access_token.empty() && access_token != GetParam(command, "token")) {
+		return context.ReturnLocalizedError(LocalizationKey::ERROR_AUTHENTICATION, UNAUTHORIZED);
+	}
+
+	if (!PbOperation_IsValid(command.operation())) {
+		LOGERROR("Received unknown command with operation opcode %d", command.operation())
+
+		return context.ReturnLocalizedError(LocalizationKey::ERROR_OPERATION, UNKNOWN_OPERATION);
+	}
+
+	LOGTRACE("Received %s command", PbOperation_Name(command.operation()).c_str())
+
+	PbResult result;
+	ProtobufSerializer serializer;
+
+	switch(command.operation()) {
+		case LOG_LEVEL: {
+			const string log_level = GetParam(command, "level");
+			if (const bool status = executor->SetLogLevel(log_level); !status) {
+				context.ReturnLocalizedError(LocalizationKey::ERROR_LOG_LEVEL, log_level);
+			}
+			else {
+				current_log_level = log_level;
+
+				context.ReturnStatus();
+			}
+			break;
+		}
+
+		case DEFAULT_FOLDER: {
+			if (const string status = rascsi_image.SetDefaultFolder(GetParam(command, "folder")); !status.empty()) {
+				context.ReturnStatus(false, status);
+			}
+			else {
+				context.ReturnStatus();
+			}
+			break;
+		}
+
+		case DEVICES_INFO: {
+			rascsi_response.GetDevicesInfo(controller_manager->GetAllDevices(), result, command,
+					rascsi_image.GetDefaultFolder());
+			serializer.SerializeMessage(context.GetFd(), result);
+			break;
+		}
+
+		case DEVICE_TYPES_INFO: {
+			result.set_allocated_device_types_info(rascsi_response.GetDeviceTypesInfo(result).release());
+			serializer.SerializeMessage(context.GetFd(), result);
+			break;
+		}
+
+		case SERVER_INFO: {
+			result.set_allocated_server_info(rascsi_response.GetServerInfo(controller_manager->GetAllDevices(),
+					result, executor->GetReservedIds(), current_log_level, rascsi_image.GetDefaultFolder(),
+					GetParam(command, "folder_pattern"), GetParam(command, "file_pattern"),
+					rascsi_image.GetDepth()).release());
+			serializer.SerializeMessage(context.GetFd(), result);
+			break;
+		}
+
+		case VERSION_INFO: {
+			result.set_allocated_version_info(rascsi_response.GetVersionInfo(result).release());
+			serializer.SerializeMessage(context.GetFd(), result);
+			break;
+		}
+
+		case LOG_LEVEL_INFO: {
+			result.set_allocated_log_level_info(rascsi_response.GetLogLevelInfo(result, current_log_level).release());
+			serializer.SerializeMessage(context.GetFd(), result);
+			break;
+		}
+
+		case DEFAULT_IMAGE_FILES_INFO: {
+			result.set_allocated_image_files_info(rascsi_response.GetAvailableImages(result,
+					rascsi_image.GetDefaultFolder(), GetParam(command, "folder_pattern"),
+					GetParam(command, "file_pattern"), rascsi_image.GetDepth()).release());
+			serializer.SerializeMessage(context.GetFd(), result);
+			break;
+		}
+
+		case IMAGE_FILE_INFO: {
+			if (string filename = GetParam(command, "file"); filename.empty()) {
+				context.ReturnLocalizedError( LocalizationKey::ERROR_MISSING_FILENAME);
+			}
+			else {
+				auto image_file = make_unique<PbImageFile>();
+				const bool status = rascsi_response.GetImageFile(*image_file.get(), rascsi_image.GetDefaultFolder(), filename);
+				if (status) {
+					result.set_status(true);
+					result.set_allocated_image_file_info(image_file.get());
+					serializer.SerializeMessage(context.GetFd(), result);
+				}
+				else {
+					context.ReturnLocalizedError(LocalizationKey::ERROR_IMAGE_FILE_INFO);
+				}
+			}
+			break;
+		}
+
+		case NETWORK_INTERFACES_INFO: {
+			result.set_allocated_network_interfaces_info(rascsi_response.GetNetworkInterfacesInfo(result).release());
+			serializer.SerializeMessage(context.GetFd(), result);
+			break;
+		}
+
+		case MAPPING_INFO: {
+			result.set_allocated_mapping_info(rascsi_response.GetMappingInfo(result).release());
+			serializer.SerializeMessage(context.GetFd(), result);
+			break;
+		}
+
+		case OPERATION_INFO: {
+			result.set_allocated_operation_info(rascsi_response.GetOperationInfo(result,
+					rascsi_image.GetDepth()).release());
+			serializer.SerializeMessage(context.GetFd(), result);
+			break;
+		}
+
+		case RESERVED_IDS_INFO: {
+			result.set_allocated_reserved_ids_info(rascsi_response.GetReservedIds(result,
+					executor->GetReservedIds()).release());
+			serializer.SerializeMessage(context.GetFd(), result);
+			break;
+		}
+
+		case SHUT_DOWN: {
+			if (executor->ShutDown(context, GetParam(command, "mode"))) {
+				TerminationHandler(0);
+			}
+			break;
+		}
+
+		default: {
+			// Wait until we become idle
+			const timespec ts = { .tv_sec = 0, .tv_nsec = 500'000'000};
+			while (active) {
+				nanosleep(&ts, nullptr);
+			}
+
+			executor->ProcessCmd(context, command);
+			break;
+		}
+	}
+
+	return true;
+}
+
+int Rascsi::run(const vector<char *>& args) const
+{
+	GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+	Banner(args);
+
+	int port = DEFAULT_PORT;
+	optarg_queue_type optarg_queue;
+	if (!ParseArguments(args, port, optarg_queue)) {
+		return EXIT_FAILURE;
+	}
+
+	// Note that current_log_level may have been modified by ParseArguments()
+	executor->SetLogLevel(current_log_level);
+
+	// Create a thread-safe stdout logger to process the log messages
+	const auto logger = stdout_color_mt("rascsi stdout logger");
+
+	if (!InitBus()) {
+		return EXIT_FAILURE;
+	}
+
+	// We need to wait to create the devices until after the bus/controller/etc
+	// objects have been created.
+	if (!CreateInitialDevices(optarg_queue)) {
+		Cleanup();
+		return EXIT_FAILURE;
+	}
+
+	if (!service.Init(&ExecuteCommand, port)) {
+		return EXIT_FAILURE;
+	}
+
+	// Signal handler to detach all devices on a KILL or TERM signal
+	struct sigaction termination_handler;
+	termination_handler.sa_handler = TerminationHandler;
+	sigemptyset(&termination_handler.sa_mask);
+	termination_handler.sa_flags = 0;
+	sigaction(SIGINT, &termination_handler, nullptr);
+	sigaction(SIGTERM, &termination_handler, nullptr);
+
+    // Set the affinity to a specific processor core
+	FixCpu(3);
+
+	sched_param schparam;
+#ifdef USE_SEL_EVENT_ENABLE
+	// Scheduling policy setting (highest priority)
+	schparam.sched_priority = sched_get_priority_max(SCHED_FIFO);
+	sched_setscheduler(0, SCHED_FIFO, &schparam);
+#else
+	cout << "Note: No RaSCSI hardware support, only client interface calls are supported" << endl;
+#endif
+
+	// Start execution
+	service.SetRunning(true);
+
+	// Main Loop
+	while (service.IsRunning()) {
+#ifdef USE_SEL_EVENT_ENABLE
+		// SEL signal polling
+		if (!bus->PollSelectEvent()) {
+			// Stop on interrupt
+			if (errno == EINTR) {
+				break;
+			}
+			continue;
+		}
+
+		// Get the bus
+		bus->Acquire();
+#else
+		bus->Acquire();
+		if (!bus->GetSEL()) {
+			const timespec ts = { .tv_sec = 0, .tv_nsec = 0};
+			nanosleep(&ts, nullptr);
+			continue;
+		}
+#endif
+
+        // Wait until BSY is released as there is a possibility for the
+        // initiator to assert it while setting the ID (for up to 3 seconds)
+		if (bus->GetBSY()) {
+			const uint32_t now = SysTimer::GetTimerLow();
+			while ((SysTimer::GetTimerLow() - now) < 3'000'000) {
+				bus->Acquire();
+				if (!bus->GetBSY()) {
+					break;
+				}
+			}
+		}
+
+		// Stop because the bus is busy or another device responded
+		if (bus->GetBSY() || !bus->GetSEL()) {
+			continue;
+		}
+
+		int initiator_id = -1;
+
+		// The initiator and target ID
+		const uint8_t id_data = bus->GetDAT();
+
+		BUS::phase_t phase = BUS::phase_t::busfree;
+
+		// Identify the responsible controller
+		auto controller = controller_manager->IdentifyController(id_data);
+		if (controller != nullptr) {
+			initiator_id = controller->ExtractInitiatorId(id_data);
+
+			if (controller->Process(initiator_id) == BUS::phase_t::selection) {
+				phase = BUS::phase_t::selection;
+			}
+		}
+
+		// Return to bus monitoring if the selection phase has not started
+		if (phase != BUS::phase_t::selection) {
+			continue;
+		}
+
+		// Start target device
+		active = true;
+
+#if !defined(USE_SEL_EVENT_ENABLE) && defined(__linux__)
+		// Scheduling policy setting (highest priority)
+		schparam.sched_priority = sched_get_priority_max(SCHED_FIFO);
+		sched_setscheduler(0, SCHED_FIFO, &schparam);
+#endif
+
+		// Loop until the bus is free
+		while (service.IsRunning()) {
+			// Target drive
+			phase = controller->Process(initiator_id);
+
+			// End when the bus is free
+			if (phase == BUS::phase_t::busfree) {
+				break;
+			}
+		}
+
+#if !defined(USE_SEL_EVENT_ENABLE) && defined(__linux__)
+		// Set the scheduling priority back to normal
+		schparam.sched_priority = 0;
+		sched_setscheduler(0, SCHED_OTHER, &schparam);
+#endif
+
+		// End the target travel
+		active = false;
+	}
+
+	return EXIT_SUCCESS;
+}
diff --git a/cpp/rascsi/rascsi_core.h b/cpp/rascsi/rascsi_core.h
new file mode 100644
index 00000000..ef13b912
--- /dev/null
+++ b/cpp/rascsi/rascsi_core.h
@@ -0,0 +1,77 @@
+//---------------------------------------------------------------------------
+//
+// SCSI Target Emulator RaSCSI Reloaded
+// for Raspberry Pi
+//
+// Copyright (C) 2022 Uwe Seimet
+//
+//---------------------------------------------------------------------------
+
+#pragma once
+
+#include "rascsi/command_context.h"
+#include "rascsi/rascsi_service.h"
+#include "rascsi/rascsi_image.h"
+#include "rascsi/rascsi_response.h"
+#include "rascsi_interface.pb.h"
+#include <vector>
+#include <string>
+
+using namespace std;
+
+// TODO Only BUS should be known
+class GPIOBUS;
+class ControllerManager;
+class RascsiExecutor;
+
+class Rascsi
+{
+	using optarg_queue_type = vector<pair<int, string>>;
+
+	static const int DEFAULT_PORT = 6868;
+	static const char COMPONENT_SEPARATOR = ':';
+
+public:
+
+	Rascsi() = default;
+	~Rascsi() = default;
+
+	int run(const vector<char *>&) const;
+
+private:
+
+	void Banner(const vector<char *>&) const;
+	bool InitBus() const;
+	static void Cleanup();
+	bool ReadAccessToken(const char *) const;
+	void LogDevices(string_view) const;
+	static void TerminationHandler(int);
+	bool ProcessId(const string&, int&, int&) const;
+	bool ParseArguments(const vector<char *>&, int&, optarg_queue_type&) const;
+	bool CreateInitialDevices(const optarg_queue_type&) const;
+
+	// TODO Should not be static and should be moved to RascsiService
+	static bool ExecuteCommand(const CommandContext&, const PbCommand&);
+
+	// TODO These fields should not be static
+
+	static inline RascsiService service;
+
+	static inline RascsiImage rascsi_image;
+
+	const static inline RascsiResponse rascsi_response;
+
+	static inline shared_ptr<GPIOBUS> bus;
+
+	static inline shared_ptr<ControllerManager> controller_manager;
+
+	static inline shared_ptr<RascsiExecutor> executor;
+
+	// Processing flag
+	static inline volatile bool active;
+
+	// Some versions of spdlog do not support get_log_level(), so we have to remember the level
+	static inline string current_log_level = "info";
+
+	static inline string access_token;
+};
diff --git a/cpp/rascsi/rascsi_executor.cpp b/cpp/rascsi/rascsi_executor.cpp
index 04f83874..dd8f8ba2 100644
--- a/cpp/rascsi/rascsi_executor.cpp
+++ b/cpp/rascsi/rascsi_executor.cpp
@@ -14,7 +14,6 @@
 #include "devices/primary_device.h"
 #include "devices/disk.h"
 #include "rascsi_service.h"
-#include "rascsi_response.h"
 #include "rascsi_image.h"
 #include "rascsi_exceptions.h"
 #include "localizer.h"
@@ -157,7 +156,8 @@ bool RascsiExecutor::ProcessCmd(const CommandContext& context, const PbCommand&
 		// A new command with an empty device list is required here in order to return data for all devices
 		PbCommand cmd;
 		PbResult result;
-		rascsi_response.GetDevicesInfo(result, cmd, rascsi_image.GetDefaultFolder());
+		rascsi_response.GetDevicesInfo(controller_manager.GetAllDevices(), result, cmd,
+				rascsi_image.GetDefaultFolder());
 		serializer.SerializeMessage(context.GetFd(), result);
 		return true;
 	}
@@ -318,7 +318,7 @@ bool RascsiExecutor::Attach(const CommandContext& context, const PbDeviceDefinit
 		params.erase("file");
 	}
 
-	if (device->SupportsParams() && !device->Init(params)) {
+	if (!device->Init(params)) {
 		return context.ReturnLocalizedError(LocalizationKey::ERROR_INITIALIZATION, PbDeviceType_Name(device->GetType()),
 				to_string(id), to_string(lun));
 	}
@@ -389,7 +389,7 @@ bool RascsiExecutor::Insert(const CommandContext& context, const PbDeviceDefinit
 
 	storage_device->SetProtected(pb_device.protected_());
 	storage_device->ReserveFile(full_path, storage_device->GetId(), storage_device->GetLun());
-	storage_device->MediumChanged();
+	storage_device->SetMediumChanged(true);
 
 	return true;
 }
@@ -522,7 +522,7 @@ string RascsiExecutor::SetReservedIds(string_view ids)
     if (!reserved_ids.empty()) {
     	string s;
     	bool isFirst = true;
-    	for (auto const& reserved_id : reserved) {
+    	for (const auto& reserved_id : reserved) {
     		if (!isFirst) {
     			s += ", ";
     		}
@@ -668,7 +668,7 @@ bool RascsiExecutor::VerifyExistingIdAndLun(const CommandContext& context, int i
 shared_ptr<PrimaryDevice> RascsiExecutor::CreateDevice(const CommandContext& context, const PbDeviceType type,
 		int lun, const string& filename) const
 {
-	auto device = device_factory.CreateDevice(controller_manager, type, lun, filename);
+	auto device = device_factory.CreateDevice(type, lun, filename);
 	if (device == nullptr) {
 		if (type == UNDEFINED) {
 			context.ReturnLocalizedError(LocalizationKey::ERROR_MISSING_DEVICE_TYPE, filename);
diff --git a/cpp/rascsi/rascsi_executor.h b/cpp/rascsi/rascsi_executor.h
index 1e6d45a7..942f7f7b 100644
--- a/cpp/rascsi/rascsi_executor.h
+++ b/cpp/rascsi/rascsi_executor.h
@@ -9,9 +9,9 @@
 
 #pragma once
 
+#include "rascsi/rascsi_response.h"
 #include "protobuf_serializer.h"
 
-class RascsiResponse;
 class RascsiImage;
 class DeviceFactory;
 class ControllerManager;
@@ -20,24 +20,10 @@ class CommandContext;
 
 class RascsiExecutor
 {
-	RascsiResponse& rascsi_response;
-
-	RascsiImage& rascsi_image;
-
-	DeviceFactory& device_factory;
-
-	ControllerManager& controller_manager;
-
-	ProtobufSerializer serializer;
-
-	unordered_set<int> reserved_ids;
-
 public:
 
-	RascsiExecutor(RascsiResponse& rascsi_response, RascsiImage& rascsi_image, DeviceFactory& device_factory,
-			ControllerManager& controller_manager)
-		: rascsi_response(rascsi_response), rascsi_image(rascsi_image), device_factory(device_factory),
-		  controller_manager(controller_manager) {}
+	RascsiExecutor(RascsiImage& rascsi_image, ControllerManager& controller_manager)
+		: rascsi_image(rascsi_image), controller_manager(controller_manager) {}
 	~RascsiExecutor() = default;
 
 	unordered_set<int> GetReservedIds() const { return reserved_ids; }
@@ -67,4 +53,17 @@ public:
 	static bool ValidateIdAndLun(const CommandContext&, int, int);
 	static bool SetProductData(const CommandContext&, const PbDeviceDefinition&, PrimaryDevice&);
 
+private:
+
+	const RascsiResponse rascsi_response;
+
+	RascsiImage& rascsi_image;
+
+	ControllerManager& controller_manager;
+
+	const DeviceFactory device_factory;
+
+	const ProtobufSerializer serializer;
+
+	unordered_set<int> reserved_ids;
 };
diff --git a/cpp/rascsi/rascsi_response.cpp b/cpp/rascsi/rascsi_response.cpp
index 58042cbe..6444994f 100644
--- a/cpp/rascsi/rascsi_response.cpp
+++ b/cpp/rascsi/rascsi_response.cpp
@@ -7,7 +7,7 @@
 //
 //---------------------------------------------------------------------------
 
-#include "controllers/controller_manager.h"
+#include "log.h"
 #include "devices/disk.h"
 #include "devices/device_factory.h"
 #include "protobuf_util.h"
@@ -25,7 +25,8 @@ unique_ptr<PbDeviceProperties> RascsiResponse::GetDeviceProperties(const Device&
 {
 	auto properties = make_unique<PbDeviceProperties>();
 
-	properties->set_luns(max_luns);
+	// Currently there is only a SCSI controller, i.e. there can always be 32 LUNs
+	properties->set_luns(32);
 	properties->set_read_only(device.IsReadOnly());
 	properties->set_protectable(device.IsProtectable());
 	properties->set_stoppable(device.IsStoppable());
@@ -52,7 +53,7 @@ void RascsiResponse::GetDeviceTypeProperties(PbDeviceTypesInfo& device_types_inf
 {
 	auto type_properties = device_types_info.add_properties();
 	type_properties->set_type(type);
-	const auto device = device_factory.CreateDevice(controller_manager, type, 0, "");
+	const auto device = device_factory.CreateDevice(type, 0, "");
 	type_properties->set_allocated_properties(GetDeviceProperties(*device).release());
 } //NOSONAR The allocated memory is managed by protobuf
 
@@ -214,20 +215,20 @@ unique_ptr<PbReservedIdsInfo> RascsiResponse::GetReservedIds(PbResult& result, c
 	return reserved_ids_info;
 }
 
-void RascsiResponse::GetDevices(PbServerInfo& server_info, const string& default_folder) const
+void RascsiResponse::GetDevices(const unordered_set<shared_ptr<PrimaryDevice>>& devices, PbServerInfo& server_info,
+		const string& default_folder) const
 {
-	for (const auto& device : controller_manager.GetAllDevices()) {
+	for (const auto& device : devices) {
 		PbDevice *pb_device = server_info.mutable_devices_info()->add_devices();
 		GetDevice(*device, *pb_device, default_folder);
 	}
 }
 
-void RascsiResponse::GetDevicesInfo(PbResult& result, const PbCommand& command, const string& default_folder) const
+void RascsiResponse::GetDevicesInfo(const unordered_set<shared_ptr<PrimaryDevice>>& devices, PbResult& result,
+		const PbCommand& command, const string& default_folder) const
 {
 	set<id_set> id_sets;
 
-	const auto& devices = controller_manager.GetAllDevices();
-
 	// If no device list was provided in the command get information on all devices
 	if (!command.devices_size()) {
 		for (const auto& device : devices) {
@@ -236,7 +237,7 @@ void RascsiResponse::GetDevicesInfo(PbResult& result, const PbCommand& command,
 	}
 	// Otherwise get information on the devices provided in the command
 	else {
-		id_sets = MatchDevices(result, command);
+		id_sets = MatchDevices(devices, result, command);
 		if (id_sets.empty()) {
 			return;
 		}
@@ -268,9 +269,9 @@ unique_ptr<PbDeviceTypesInfo> RascsiResponse::GetDeviceTypesInfo(PbResult& resul
 	return device_types_info;
 }
 
-unique_ptr<PbServerInfo> RascsiResponse::GetServerInfo(PbResult& result, const unordered_set<int>& reserved_ids,
-		const string& current_log_level, const string& default_folder, const string& folder_pattern,
-		const string& file_pattern, int scan_depth) const
+unique_ptr<PbServerInfo> RascsiResponse::GetServerInfo(const unordered_set<shared_ptr<PrimaryDevice>>& devices,
+		PbResult& result, const unordered_set<int>& reserved_ids, const string& current_log_level,
+		const string& default_folder, const string& folder_pattern, const string& file_pattern, int scan_depth) const
 {
 	auto server_info = make_unique<PbServerInfo>();
 
@@ -280,7 +281,7 @@ unique_ptr<PbServerInfo> RascsiResponse::GetServerInfo(PbResult& result, const u
 	GetAvailableImages(result, *server_info, default_folder, folder_pattern, file_pattern, scan_depth);
 	server_info->set_allocated_network_interfaces_info(GetNetworkInterfacesInfo(result).release());
 	server_info->set_allocated_mapping_info(GetMappingInfo(result).release()); //NOSONAR The allocated memory is managed by protobuf
-	GetDevices(*server_info, default_folder); //NOSONAR The allocated memory is managed by protobuf
+	GetDevices(devices, *server_info, default_folder); //NOSONAR The allocated memory is managed by protobuf
 	server_info->set_allocated_reserved_ids_info(GetReservedIds(result, reserved_ids).release());
 	server_info->set_allocated_operation_info(GetOperationInfo(result, scan_depth).release()); //NOSONAR The allocated memory is managed by protobuf
 
@@ -493,13 +494,14 @@ unique_ptr<PbOperationParameter> RascsiResponse::AddOperationParameter(PbOperati
 	return parameter;
 }
 
-set<id_set> RascsiResponse::MatchDevices(PbResult& result, const PbCommand& command) const
+set<id_set> RascsiResponse::MatchDevices(const unordered_set<shared_ptr<PrimaryDevice>>& devices, PbResult& result,
+		const PbCommand& command) const
 {
 	set<id_set> id_sets;
 
 	for (const auto& device : command.devices()) {
 		bool has_device = false;
-		for (const auto& d : controller_manager.GetAllDevices()) {
+		for (const auto& d : devices) {
 			if (d->GetId() == device.id() && d->GetLun() == device.unit()) {
 				id_sets.insert(make_pair(device.id(), device.unit()));
 				has_device = true;
diff --git a/cpp/rascsi/rascsi_response.h b/cpp/rascsi/rascsi_response.h
index f6635030..d9a188ff 100644
--- a/cpp/rascsi/rascsi_response.h
+++ b/cpp/rascsi/rascsi_response.h
@@ -9,6 +9,8 @@
 
 #pragma once
 
+#include "devices/device_factory.h"
+#include "devices/primary_device.h"
 #include "rascsi_interface.pb.h"
 #include <dirent.h>
 #include <array>
@@ -17,27 +19,24 @@
 using namespace std;
 using namespace rascsi_interface;
 
-class DeviceFactory;
-class ControllerManager;
-class Device;
-
 class RascsiResponse
 {
+	using id_set = pair<int, int>;
+
 public:
 
-	RascsiResponse(DeviceFactory& device_factory, const ControllerManager& controller_manager, int max_luns)
-		: device_factory(device_factory), controller_manager(controller_manager), max_luns(max_luns) {}
+	RascsiResponse() = default;
 	~RascsiResponse() = default;
 
 	bool GetImageFile(PbImageFile&, const string&, const string&) const;
 	unique_ptr<PbImageFilesInfo> GetAvailableImages(PbResult&, const string&, const string&, const string&, int) const;
 	unique_ptr<PbReservedIdsInfo> GetReservedIds(PbResult&, const unordered_set<int>&) const;
-	void GetDevices(PbServerInfo&, const string&) const;
-	void GetDevicesInfo(PbResult&, const PbCommand&, const string&) const;
+	void GetDevices(const unordered_set<shared_ptr<PrimaryDevice>>&, PbServerInfo&, const string&) const;
+	void GetDevicesInfo(const unordered_set<shared_ptr<PrimaryDevice>>&, PbResult&, const PbCommand&, const string&) const;
 	unique_ptr<PbDeviceTypesInfo> GetDeviceTypesInfo(PbResult&) const;
 	unique_ptr<PbVersionInfo> GetVersionInfo(PbResult&) const;
-	unique_ptr<PbServerInfo> GetServerInfo(PbResult&, const unordered_set<int>&, const string&, const string&, const string&,
-			const string&, int) const;
+	unique_ptr<PbServerInfo> GetServerInfo(const unordered_set<shared_ptr<PrimaryDevice>>&, PbResult&, const unordered_set<int>&,
+			const string&, const string&, const string&, const string&, int) const;
 	unique_ptr<PbNetworkInterfacesInfo> GetNetworkInterfacesInfo(PbResult&) const;
 	unique_ptr<PbMappingInfo> GetMappingInfo(PbResult&) const;
 	unique_ptr<PbLogLevelInfo> GetLogLevelInfo(PbResult&, const string&) const;
@@ -45,13 +44,9 @@ public:
 
 private:
 
-	DeviceFactory& device_factory;
+	DeviceFactory device_factory;
 
-	const ControllerManager& controller_manager;
-
-	int max_luns;
-
-	const array<string, 6> log_levels = { "trace", "debug", "info", "warn", "err", "off" };
+	const inline static array<string, 6> log_levels = { "trace", "debug", "info", "warn", "err", "off" };
 
 	unique_ptr<PbDeviceProperties> GetDeviceProperties(const Device&) const;
 	void GetDevice(const Device&, PbDevice&, const string&) const;
@@ -62,7 +57,7 @@ private:
 	unique_ptr<PbOperationMetaData> CreateOperation(PbOperationInfo&, const PbOperation&, const string&) const;
 	unique_ptr<PbOperationParameter> AddOperationParameter(PbOperationMetaData&, const string&, const string&,
 			const string& = "", bool = false) const;
-	set<id_set> MatchDevices(PbResult&, const PbCommand&) const;
+	set<id_set> MatchDevices(const unordered_set<shared_ptr<PrimaryDevice>>&, PbResult&, const PbCommand&) const;
 
 	static string GetNextImageFile(const dirent *, const string&);
 };
diff --git a/cpp/rascsi/rascsi_service.cpp b/cpp/rascsi/rascsi_service.cpp
index 511b71c3..68a94323 100644
--- a/cpp/rascsi/rascsi_service.cpp
+++ b/cpp/rascsi/rascsi_service.cpp
@@ -18,9 +18,12 @@
 #include <csignal>
 
 using namespace rascsi_interface;
+using namespace ras_util;
 
 void RascsiService::Cleanup() const
 {
+	running = false;
+
 	if (service_socket != -1) {
 		close(service_socket);
 	}
@@ -76,7 +79,7 @@ void RascsiService::Execute() const
 #endif
 
 	// Set the affinity to a specific processor core
-	ras_util::FixCpu(2);
+	FixCpu(2);
 
 	// Wait for the execution to start
 	const timespec ts = { .tv_sec = 0, .tv_nsec = 1000};
diff --git a/cpp/rasctl.cpp b/cpp/rasctl.cpp
index 18ccba91..4cae2663 100644
--- a/cpp/rasctl.cpp
+++ b/cpp/rasctl.cpp
@@ -1,327 +1,19 @@
 //---------------------------------------------------------------------------
 //
-//	SCSI Target Emulator RaSCSI Reloaded
-//	for Raspberry Pi
+// SCSI Target Emulator RaSCSI Reloaded
+// for Raspberry Pi
 //
-//	Powered by XM6 TypeG Technology.
-//	Copyright (C) 2016-2020 GIMONS
-//	Copyright (C) 2020-2021 Contributors to the RaSCSI project
-//	[ Send Control Command ]
+// Copyright (C) 2022 Uwe Seimet
 //
 //---------------------------------------------------------------------------
 
-#include "rascsi_version.h"
-#include "protobuf_util.h"
-#include "rasutil.h"
-#include "rascsi_exceptions.h"
-#include "rascsi_interface.pb.h"
-#include "rasctl/rasctl_parser.h"
-#include "rasctl/rasctl_commands.h"
-#include <unistd.h>
-#include <clocale>
-#include <iostream>
-
-// Separator for the INQUIRY name components and for compound parameters
-static const char COMPONENT_SEPARATOR = ':';
+#include "rasctl/rasctl_core.h"
 
 using namespace std;
-using namespace rascsi_interface;
-using namespace ras_util;
-using namespace protobuf_util;
 
-void Banner(int argc, char* argv[])
+int main(int argc, char *argv[])
 {
-	if (argc < 2) {
-		cout << Banner("Controller");
+	const vector<char *> args(argv, argv + argc);
 
-		cout << "\nUsage: " << argv[0] << " -i ID [-u UNIT] [-c CMD] [-C FILE] [-t TYPE] [-b BLOCK_SIZE] [-n NAME] [-f FILE|PARAM] ";
-		cout << "[-F IMAGE_FOLDER] [-L LOG_LEVEL] [-h HOST] [-p PORT] [-r RESERVED_IDS] ";
-		cout << "[-C FILENAME:FILESIZE] [-d FILENAME] [-w FILENAME] [-R CURRENT_NAME:NEW_NAME] ";
-		cout <<	"[-x CURRENT_NAME:NEW_NAME] [-z LOCALE] ";
-		cout << "[-e] [-E FILENAME] [-D] [-I] [-l] [-m] [o] [-O] [-P] [-s] [-v] [-V] [-y] [-X]\n";
-		cout << " where  ID := {0-7}\n";
-		cout << "        UNIT := {0-31}, default is 0\n";
-		cout << "        CMD := {attach|detach|insert|eject|protect|unprotect|show}\n";
-		cout << "        TYPE := {schd|scrm|sccd|scmo|scbr|scdp} or convenience type {hd|rm|mo|cd|bridge|daynaport}\n";
-		cout << "        BLOCK_SIZE := {512|1024|2048|4096) bytes per hard disk drive block\n";
-		cout << "        NAME := name of device to attach (VENDOR:PRODUCT:REVISION)\n";
-		cout << "        FILE|PARAM := image file path or device-specific parameter\n";
-		cout << "        IMAGE_FOLDER := default location for image files, default is '~/images'\n";
-		cout << "        HOST := rascsi host to connect to, default is 'localhost'\n";
-		cout << "        PORT := rascsi port to connect to, default is 6868\n";
-		cout << "        RESERVED_IDS := comma-separated list of IDs to reserve\n";
-		cout << "        LOG_LEVEL := log level {trace|debug|info|warn|err|off}, default is 'info'\n";
-		cout << " If CMD is 'attach' or 'insert' the FILE parameter is required.\n";
-		cout << "Usage: " << argv[0] << " -l\n";
-		cout << "       Print device list.\n" << flush;
-
-		exit(EXIT_SUCCESS);
-	}
-}
-
-int main(int argc, char* argv[])
-{
-	GOOGLE_PROTOBUF_VERIFY_VERSION;
-
-	Banner(argc, argv);
-
-	RasctlParser parser;
-	PbCommand command;
-	PbDeviceDefinition* device = command.add_devices();
-	device->set_id(-1);
-	const char *hostname = "localhost";
-	int port = 6868;
-	string param;
-	string log_level;
-	string default_folder;
-	string reserved_ids;
-	string image_params;
-	string filename;
-	string token;
-	bool list = false;
-
-	const char *locale = setlocale(LC_MESSAGES, "");
-	if (locale == nullptr || !strcmp(locale, "C")) {
-		locale = "en";
-	}
-
-	opterr = 1;
-	int opt;
-	while ((opt = getopt(argc, argv, "e::lmos::vDINOTVXa:b:c:d:f:h:i:n:p:r:t:u:x:z:C:E:F:L:P::R:")) != -1) {
-		switch (opt) {
-			case 'i': {
-				int id;
-				if (!GetAsInt(optarg, id)) {
-					cerr << "Error: Invalid device ID " << optarg << endl;
-					exit(EXIT_FAILURE);
-				}
-				device->set_id(id);
-				break;
-			}
-
-			case 'u': {
-				int unit;
-				if (!GetAsInt(optarg, unit)) {
-					cerr << "Error: Invalid unit " << optarg << endl;
-					exit(EXIT_FAILURE);
-				}
-				device->set_unit(unit);
-				break;
-			}
-
-			case 'C':
-				command.set_operation(CREATE_IMAGE);
-				image_params = optarg;
-				break;
-
-			case 'b':
-				int block_size;
-				if (!GetAsInt(optarg, block_size)) {
-					cerr << "Error: Invalid block size " << optarg << endl;
-					exit(EXIT_FAILURE);
-				}
-				device->set_block_size(block_size);
-				break;
-
-			case 'c':
-				command.set_operation(parser.ParseOperation(optarg));
-				if (command.operation() == NO_OPERATION) {
-					cerr << "Error: Unknown operation '" << optarg << "'" << endl;
-					exit(EXIT_FAILURE);
-				}
-				break;
-
-			case 'D':
-				command.set_operation(DETACH_ALL);
-				break;
-
-			case 'd':
-				command.set_operation(DELETE_IMAGE);
-				image_params = optarg;
-				break;
-
-			case 'E':
-				command.set_operation(IMAGE_FILE_INFO);
-				filename = optarg;
-				break;
-
-			case 'e':
-				command.set_operation(DEFAULT_IMAGE_FILES_INFO);
-                if (optarg) {
-                	SetPatternParams(command, optarg);
-                }
-                break;
-
-			case 'F':
-				command.set_operation(DEFAULT_FOLDER);
-				default_folder = optarg;
-				break;
-
-			case 'f':
-				param = optarg;
-				break;
-
-			case 'h':
-				hostname = optarg;
-				break;
-
-			case 'I':
-				command.set_operation(RESERVED_IDS_INFO);
-				break;
-
-			case 'L':
-				command.set_operation(LOG_LEVEL);
-				log_level = optarg;
-				break;
-
-			case 'l':
-				list = true;
-				break;
-
-			case 'm':
-				command.set_operation(MAPPING_INFO);
-				break;
-
-			case 'N':
-				command.set_operation(NETWORK_INTERFACES_INFO);
-				break;
-
-			case 'O':
-				command.set_operation(LOG_LEVEL_INFO);
-				break;
-
-			case 'o':
-				command.set_operation(OPERATION_INFO);
-				break;
-
-			case 't':
-				device->set_type(parser.ParseType(optarg));
-				if (device->type() == UNDEFINED) {
-					cerr << "Error: Unknown device type '" << optarg << "'" << endl;
-					exit(EXIT_FAILURE);
-				}
-				break;
-
-			case 'r':
-				command.set_operation(RESERVE_IDS);
-				reserved_ids = optarg;
-				break;
-
-			case 'R':
-				command.set_operation(RENAME_IMAGE);
-				image_params = optarg;
-				break;
-
-			case 'n': {
-					string vendor;
-					string product;
-					string revision;
-
-					string s = optarg;
-					if (size_t separator_pos = s.find(COMPONENT_SEPARATOR); separator_pos != string::npos) {
-						vendor = s.substr(0, separator_pos);
-						s = s.substr(separator_pos + 1);
-						separator_pos = s.find(COMPONENT_SEPARATOR);
-						if (separator_pos != string::npos) {
-							product = s.substr(0, separator_pos);
-							revision = s.substr(separator_pos + 1);
-						}
-						else {
-							product = s;
-						}
-					}
-					else {
-						vendor = s;
-					}
-
-					device->set_vendor(vendor);
-					device->set_product(product);
-					device->set_revision(revision);
-				}
-				break;
-
-			case 'p':
-				if (!GetAsInt(optarg, port) || port <= 0 || port > 65535) {
-					cerr << "Error: Invalid port " << optarg << ", port must be between 1 and 65535" << endl;
-					exit(EXIT_FAILURE);
-				}
-				break;
-
-			case 's':
-				command.set_operation(SERVER_INFO);
-                if (optarg) {
-                	SetPatternParams(command, optarg);
-                }
-                break;
-
-			case 'v':
-				cout << "rasctl version: " << rascsi_get_version_string() << endl;
-				exit(EXIT_SUCCESS);
-				break;
-
-			case 'P':
-				token = optarg ? optarg : getpass("Password: ");
-				break;
-
-			case 'V':
-				command.set_operation(VERSION_INFO);
-				break;
-
-			case 'x':
-				command.set_operation(COPY_IMAGE);
-				image_params = optarg;
-				break;
-
-			case 'T':
-				command.set_operation(DEVICE_TYPES_INFO);
-				break;
-
-			case 'X':
-				command.set_operation(SHUT_DOWN);
-				SetParam(command, "mode", "rascsi");
-				break;
-
-			case 'z':
-				locale = optarg;
-				break;
-
-			default:
-				break;
-		}
-	}
-
-	// For macos only 'if (optind != argc)' appears to work, but then non-argument options do not reject arguments
-	if (optopt) {
-		exit(EXIT_FAILURE);
-	}
-
-	SetParam(command, "token", token);
-	SetParam(command, "locale", locale);
-
-	RasctlCommands rasctl_commands(command, hostname, port);
-
-	bool status;
-	try {
-		// Listing devices is a special case (rasctl backwards compatibility)
-		if (list) {
-			command.clear_devices();
-			command.set_operation(DEVICES_INFO);
-
-			status = rasctl_commands.CommandDevicesInfo();
-		}
-		else {
-			ParseParameters(*device, param);
-
-			status = rasctl_commands.Execute(log_level, default_folder, reserved_ids, image_params, filename);
-		}
-	}
-    catch(const io_exception& e) {
-    	cerr << "Error: " << e.what() << endl;
-
-    	status = false;
-
-    	// Fall through
-	}
-
-    return status ? EXIT_SUCCESS : EXIT_FAILURE;
+	return RasCtl().run(args);
 }
diff --git a/cpp/rasctl/rasctl_core.cpp b/cpp/rasctl/rasctl_core.cpp
new file mode 100644
index 00000000..5db2e449
--- /dev/null
+++ b/cpp/rasctl/rasctl_core.cpp
@@ -0,0 +1,326 @@
+//---------------------------------------------------------------------------
+//
+//	SCSI Target Emulator RaSCSI Reloaded
+//	for Raspberry Pi
+//
+//	Powered by XM6 TypeG Technology.
+//	Copyright (C) 2016-2020 GIMONS
+//	Copyright (C) 2020-2021 Contributors to the RaSCSI project
+//	[ Send Control Command ]
+//
+//---------------------------------------------------------------------------
+
+#include "rascsi_version.h"
+#include "protobuf_util.h"
+#include "rasutil.h"
+#include "rascsi_exceptions.h"
+#include "rascsi_interface.pb.h"
+#include "rasctl/rasctl_parser.h"
+#include "rasctl/rasctl_commands.h"
+#include "rasctl/rasctl_core.h"
+#include <unistd.h>
+#include <clocale>
+#include <iostream>
+
+using namespace std;
+using namespace rascsi_interface;
+using namespace ras_util;
+using namespace protobuf_util;
+
+void RasCtl::Banner(const vector<char *>& args) const
+{
+	if (args.size() < 2) {
+		cout << ras_util::Banner("Controller");
+
+		cout << "\nUsage: " << args[0] << " -i ID [-u UNIT] [-c CMD] [-C FILE] [-t TYPE] [-b BLOCK_SIZE] [-n NAME] [-f FILE|PARAM] ";
+		cout << "[-F IMAGE_FOLDER] [-L LOG_LEVEL] [-h HOST] [-p PORT] [-r RESERVED_IDS] ";
+		cout << "[-C FILENAME:FILESIZE] [-d FILENAME] [-w FILENAME] [-R CURRENT_NAME:NEW_NAME] ";
+		cout <<	"[-x CURRENT_NAME:NEW_NAME] [-z LOCALE] ";
+		cout << "[-e] [-E FILENAME] [-D] [-I] [-l] [-m] [o] [-O] [-P] [-s] [-v] [-V] [-y] [-X]\n";
+		cout << " where  ID := {0-7}\n";
+		cout << "        UNIT := {0-31}, default is 0\n";
+		cout << "        CMD := {attach|detach|insert|eject|protect|unprotect|show}\n";
+		cout << "        TYPE := {schd|scrm|sccd|scmo|scbr|scdp} or convenience type {hd|rm|mo|cd|bridge|daynaport}\n";
+		cout << "        BLOCK_SIZE := {512|1024|2048|4096) bytes per hard disk drive block\n";
+		cout << "        NAME := name of device to attach (VENDOR:PRODUCT:REVISION)\n";
+		cout << "        FILE|PARAM := image file path or device-specific parameter\n";
+		cout << "        IMAGE_FOLDER := default location for image files, default is '~/images'\n";
+		cout << "        HOST := rascsi host to connect to, default is 'localhost'\n";
+		cout << "        PORT := rascsi port to connect to, default is 6868\n";
+		cout << "        RESERVED_IDS := comma-separated list of IDs to reserve\n";
+		cout << "        LOG_LEVEL := log level {trace|debug|info|warn|err|off}, default is 'info'\n";
+		cout << " If CMD is 'attach' or 'insert' the FILE parameter is required.\n";
+		cout << "Usage: " << args[0] << " -l\n";
+		cout << "       Print device list.\n" << flush;
+
+		exit(EXIT_SUCCESS);
+	}
+}
+
+int RasCtl::run(const vector<char *>& args) const
+{
+	GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+	Banner(args);
+
+	RasctlParser parser;
+	PbCommand command;
+	PbDeviceDefinition* device = command.add_devices();
+	device->set_id(-1);
+	const char *hostname = "localhost";
+	int port = 6868;
+	string param;
+	string log_level;
+	string default_folder;
+	string reserved_ids;
+	string image_params;
+	string filename;
+	string token;
+	bool list = false;
+
+	const char *locale = setlocale(LC_MESSAGES, "");
+	if (locale == nullptr || !strcmp(locale, "C")) {
+		locale = "en";
+	}
+
+	opterr = 1;
+	int opt;
+	while ((opt = getopt(static_cast<int>(args.size()), args.data(),
+			"e::lmos::vDINOTVXa:b:c:d:f:h:i:n:p:r:t:u:x:z:C:E:F:L:P::R:")) != -1) {
+		switch (opt) {
+			case 'i': {
+				int id;
+				if (!GetAsInt(optarg, id)) {
+					cerr << "Error: Invalid device ID " << optarg << endl;
+					exit(EXIT_FAILURE);
+				}
+				device->set_id(id);
+				break;
+			}
+
+			case 'u': {
+				int unit;
+				if (!GetAsInt(optarg, unit)) {
+					cerr << "Error: Invalid unit " << optarg << endl;
+					exit(EXIT_FAILURE);
+				}
+				device->set_unit(unit);
+				break;
+			}
+
+			case 'C':
+				command.set_operation(CREATE_IMAGE);
+				image_params = optarg;
+				break;
+
+			case 'b':
+				int block_size;
+				if (!GetAsInt(optarg, block_size)) {
+					cerr << "Error: Invalid block size " << optarg << endl;
+					exit(EXIT_FAILURE);
+				}
+				device->set_block_size(block_size);
+				break;
+
+			case 'c':
+				command.set_operation(parser.ParseOperation(optarg));
+				if (command.operation() == NO_OPERATION) {
+					cerr << "Error: Unknown operation '" << optarg << "'" << endl;
+					exit(EXIT_FAILURE);
+				}
+				break;
+
+			case 'D':
+				command.set_operation(DETACH_ALL);
+				break;
+
+			case 'd':
+				command.set_operation(DELETE_IMAGE);
+				image_params = optarg;
+				break;
+
+			case 'E':
+				command.set_operation(IMAGE_FILE_INFO);
+				filename = optarg;
+				break;
+
+			case 'e':
+				command.set_operation(DEFAULT_IMAGE_FILES_INFO);
+                if (optarg) {
+                	SetPatternParams(command, optarg);
+                }
+                break;
+
+			case 'F':
+				command.set_operation(DEFAULT_FOLDER);
+				default_folder = optarg;
+				break;
+
+			case 'f':
+				param = optarg;
+				break;
+
+			case 'h':
+				hostname = optarg;
+				break;
+
+			case 'I':
+				command.set_operation(RESERVED_IDS_INFO);
+				break;
+
+			case 'L':
+				command.set_operation(LOG_LEVEL);
+				log_level = optarg;
+				break;
+
+			case 'l':
+				list = true;
+				break;
+
+			case 'm':
+				command.set_operation(MAPPING_INFO);
+				break;
+
+			case 'N':
+				command.set_operation(NETWORK_INTERFACES_INFO);
+				break;
+
+			case 'O':
+				command.set_operation(LOG_LEVEL_INFO);
+				break;
+
+			case 'o':
+				command.set_operation(OPERATION_INFO);
+				break;
+
+			case 't':
+				device->set_type(parser.ParseType(optarg));
+				if (device->type() == UNDEFINED) {
+					cerr << "Error: Unknown device type '" << optarg << "'" << endl;
+					exit(EXIT_FAILURE);
+				}
+				break;
+
+			case 'r':
+				command.set_operation(RESERVE_IDS);
+				reserved_ids = optarg;
+				break;
+
+			case 'R':
+				command.set_operation(RENAME_IMAGE);
+				image_params = optarg;
+				break;
+
+			case 'n': {
+					string vendor;
+					string product;
+					string revision;
+
+					string s = optarg;
+					if (size_t separator_pos = s.find(COMPONENT_SEPARATOR); separator_pos != string::npos) {
+						vendor = s.substr(0, separator_pos);
+						s = s.substr(separator_pos + 1);
+						separator_pos = s.find(COMPONENT_SEPARATOR);
+						if (separator_pos != string::npos) {
+							product = s.substr(0, separator_pos);
+							revision = s.substr(separator_pos + 1);
+						}
+						else {
+							product = s;
+						}
+					}
+					else {
+						vendor = s;
+					}
+
+					device->set_vendor(vendor);
+					device->set_product(product);
+					device->set_revision(revision);
+				}
+				break;
+
+			case 'p':
+				if (!GetAsInt(optarg, port) || port <= 0 || port > 65535) {
+					cerr << "Error: Invalid port " << optarg << ", port must be between 1 and 65535" << endl;
+					exit(EXIT_FAILURE);
+				}
+				break;
+
+			case 's':
+				command.set_operation(SERVER_INFO);
+                if (optarg) {
+                	SetPatternParams(command, optarg);
+                }
+                break;
+
+			case 'v':
+				cout << "rasctl version: " << rascsi_get_version_string() << endl;
+				exit(EXIT_SUCCESS);
+				break;
+
+			case 'P':
+				token = optarg ? optarg : getpass("Password: ");
+				break;
+
+			case 'V':
+				command.set_operation(VERSION_INFO);
+				break;
+
+			case 'x':
+				command.set_operation(COPY_IMAGE);
+				image_params = optarg;
+				break;
+
+			case 'T':
+				command.set_operation(DEVICE_TYPES_INFO);
+				break;
+
+			case 'X':
+				command.set_operation(SHUT_DOWN);
+				SetParam(command, "mode", "rascsi");
+				break;
+
+			case 'z':
+				locale = optarg;
+				break;
+
+			default:
+				break;
+		}
+	}
+
+	// For macos only 'optind != argc' appears to work, but then non-argument options do not reject arguments
+	if (optopt) {
+		exit(EXIT_FAILURE);
+	}
+
+	SetParam(command, "token", token);
+	SetParam(command, "locale", locale);
+
+	RasctlCommands rasctl_commands(command, hostname, port);
+
+	bool status;
+	try {
+		// Listing devices is a special case (rasctl backwards compatibility)
+		if (list) {
+			command.clear_devices();
+			command.set_operation(DEVICES_INFO);
+
+			status = rasctl_commands.CommandDevicesInfo();
+		}
+		else {
+			ParseParameters(*device, param);
+
+			status = rasctl_commands.Execute(log_level, default_folder, reserved_ids, image_params, filename);
+		}
+	}
+    catch(const io_exception& e) {
+    	cerr << "Error: " << e.what() << endl;
+
+    	status = false;
+
+    	// Fall through
+	}
+
+    return status ? EXIT_SUCCESS : EXIT_FAILURE;
+}
diff --git a/cpp/rasctl/rasctl_core.h b/cpp/rasctl/rasctl_core.h
new file mode 100644
index 00000000..2251e6f1
--- /dev/null
+++ b/cpp/rasctl/rasctl_core.h
@@ -0,0 +1,31 @@
+//---------------------------------------------------------------------------
+//
+// SCSI Target Emulator RaSCSI Reloaded
+// for Raspberry Pi
+//
+// Copyright (C) 2022 Uwe Seimet
+//
+//---------------------------------------------------------------------------
+
+#pragma once
+
+#include <vector>
+
+using namespace std;
+
+class RasCtl
+{
+	// Separator for the INQUIRY name components and for compound parameters
+	static const char COMPONENT_SEPARATOR = ':';
+
+public:
+
+	RasCtl() = default;
+	~RasCtl() = default;
+
+	int run(const vector<char *>&) const;
+
+private:
+
+	void Banner(const vector<char *>&) const;
+};
diff --git a/cpp/rasdump.cpp b/cpp/rasdump.cpp
index 45e6079e..2523a5c1 100644
--- a/cpp/rasdump.cpp
+++ b/cpp/rasdump.cpp
@@ -1,997 +1,19 @@
 //---------------------------------------------------------------------------
 //
-//	SCSI Target Emulator RaSCSI Reloaded
-//	for Raspberry Pi
+// SCSI Target Emulator RaSCSI Reloaded
+// for Raspberry Pi
 //
-//	Powered by XM6 TypeG Technology.
-//	Copyright (C) 2016-2020 GIMONS
-//	[ HDD dump utility (initiator mode) ]
+// Copyright (C) 2022 Uwe Seimet
 //
 //---------------------------------------------------------------------------
 
-#include <cerrno>
-#include <csignal>
-#include <unistd.h>
-#include "rasdump_fileio.h"
-#include "hal/gpiobus.h"
-#include "hal/gpiobus_factory.h"
-#include "hal/systimer.h"
-#include "rascsi_version.h"
-#include <cstring>
-#include <iostream>
-#include <array>
+#include "rasdump/rasdump_core.h"
 
 using namespace std;
 
-//---------------------------------------------------------------------------
-//
-//	Constant Declaration
-//
-//---------------------------------------------------------------------------
-static const int BUFSIZE = 1024 * 64;			// Buffer size of about 64KB
-
-//---------------------------------------------------------------------------
-//
-//	Variable Declaration
-//
-//---------------------------------------------------------------------------
-unique_ptr<GPIOBUS> bus;      // GPIO Bus						// Bus
-int targetid;						// Target ID
-int boardid;						// Board ID (own ID)
-string hdsfile;						// HDS file
-bool restore;						// Restore flag
-uint8_t buffer[BUFSIZE];					// Work Buffer
-int result;						// Result Code
-
-//---------------------------------------------------------------------------
-//
-//	Cleanup() Function declaration
-//
-//---------------------------------------------------------------------------
-void Cleanup();
-
-//---------------------------------------------------------------------------
-//
-//	Signal processing
-//
-//---------------------------------------------------------------------------
-void KillHandler(int)
+int main(int argc, char *argv[])
 {
-	// Stop running
-	Cleanup();
-	exit(0);
-}
-
-//---------------------------------------------------------------------------
-//
-//	Banner Output
-//
-//---------------------------------------------------------------------------
-bool Banner(int argc, char* argv[])
-{
-	printf("RaSCSI hard disk dump utility ");
-	printf("version %s (%s, %s)\n",
-		rascsi_get_version_string().c_str(),
-		__DATE__,
-		__TIME__);
-
-	if (argc < 2 || strcmp(argv[1], "-h") == 0) {
-		printf("Usage: %s -i ID [-b BID] -f FILE [-r]\n", argv[0]);
-		printf(" ID is target device SCSI ID {0|1|2|3|4|5|6|7}.\n");
-		printf(" BID is rascsi board SCSI ID {0|1|2|3|4|5|6|7}. Default is 7.\n");
-		printf(" FILE is HDS file path.\n");
-		printf(" -r is restore operation.\n");
-		return false;
-	}
-
-	return true;
-}
-
-//---------------------------------------------------------------------------
-//
-//	Initialization
-//
-//---------------------------------------------------------------------------
-bool Init()
-{
-	// Interrupt handler setting
-	if (signal(SIGINT, KillHandler) == SIG_ERR) {
-		return false;
-	}
-	if (signal(SIGHUP, KillHandler) == SIG_ERR) {
-		return false;
-	}
-	if (signal(SIGTERM, KillHandler) == SIG_ERR) {
-		return false;
-	}
-
-	bus = GPIOBUS_Factory::Create();
-
-	// GPIO Initialization
-	if (!bus->Init(BUS::mode_e::INITIATOR)) {
-		return false;
-	}
-
-	// Work Intitialization
-	targetid = -1;
-	boardid = 7;
-	restore = false;
-
-	return true;
-}
-
-//---------------------------------------------------------------------------
-//
-//	Cleanup
-//
-//---------------------------------------------------------------------------
-void Cleanup()
-{
-	// Cleanup the bus
-	bus->Cleanup();
-}
-
-//---------------------------------------------------------------------------
-//
-//	Reset
-//
-//---------------------------------------------------------------------------
-void Reset()
-{
-	// Reset the bus signal line
-	bus->Reset();
-}
-
-//---------------------------------------------------------------------------
-//
-//	Argument processing
-//
-//---------------------------------------------------------------------------
-bool ParseArgument(int argc, char* argv[])
-{
-	int opt;
-	const char *file = nullptr;
-
-	// Argument Parsing
-	opterr = 0;
-	while ((opt = getopt(argc, argv, "i:b:f:r")) != -1) {
-		switch (opt) {
-			case 'i':
-				targetid = optarg[0] - '0';
-				break;
-
-			case 'b':
-				boardid = optarg[0] - '0';
-				break;
-
-			case 'f':
-				file = optarg;
-				break;
-
-			case 'r':
-				restore = true;
-				break;
-
-			default:
-				break;
-		}
-	}
-
-	// TARGET ID check
-	if (targetid < 0 || targetid > 7) {
-		fprintf(stderr,
-			"Error : Invalid target id range\n");
-		return false;
-	}
-
-	// BOARD ID check
-	if (boardid < 0 || boardid > 7) {
-		fprintf(stderr,
-			"Error : Invalid board id range\n");
-		return false;
-	}
-
-	// Target and Board ID duplication check
-	if (targetid == boardid) {
-		fprintf(stderr,
-			"Error : Invalid target or board id\n");
-		return false;
-	}
-
-	// File Check
-	if (!file) {
-		fprintf(stderr,
-			"Error : Invalid file path\n");
-		return false;
-	}
-
-	hdsfile = file;
-
-	return true;
-}
-
-//---------------------------------------------------------------------------
-//
-//	Wait Phase
-//
-//---------------------------------------------------------------------------
-bool WaitPhase(BUS::phase_t phase)
-{
-	// Timeout (3000ms)
-	const uint32_t now = SysTimer::GetTimerLow();
-	while ((SysTimer::GetTimerLow() - now) < 3 * 1000 * 1000) {
-		bus->Acquire();
-		if (bus->GetREQ() && bus->GetPhase() == phase) {
-			return true;
-		}
-	}
-
-	return false;
-}
-
-//---------------------------------------------------------------------------
-//
-//	Bus Free Phase
-//
-//---------------------------------------------------------------------------
-void BusFree()
-{
-	// Bus Reset
-	bus->Reset();
-}
-
-//---------------------------------------------------------------------------
-//
-//	Selection Phase
-//
-//---------------------------------------------------------------------------
-bool Selection(int id)
-{
-	// ID setting and SEL assert
-	uint8_t data = 1 << boardid;
-	data |= (1 << id);
-	bus->SetDAT(data);
-	bus->SetSEL(true);
-
-	// wait for busy
-	int count = 10000;
-	do {
-		// Wait 20 microseconds
-		const timespec ts = { .tv_sec = 0, .tv_nsec = 20 * 1000};
-		nanosleep(&ts, nullptr);
-		bus->Acquire();
-		if (bus->GetBSY()) {
-			break;
-		}
-	} while (count--);
-
-	// SEL negate
-	bus->SetSEL(false);
-
-	// Success if the target is busy
-	return bus->GetBSY();
-}
-
-//---------------------------------------------------------------------------
-//
-//	Command Phase
-//
-//---------------------------------------------------------------------------
-bool Command(uint8_t *buf, int length)
-{
-	// Waiting for Phase
-	if (!WaitPhase(BUS::phase_t::command)) {
-		return false;
-	}
-
-	// Send Command
-	const int count = bus->SendHandShake(buf, length, BUS::SEND_NO_DELAY);
-
-	// Success if the transmission result is the same as the number
-	// of requests
-	if (count == length) {
-		return true;
-	}
-
-	// Return error
-	return false;
-}
-
-//---------------------------------------------------------------------------
-//
-//	Data in phase
-//
-//---------------------------------------------------------------------------
-int DataIn(uint8_t *buf, int length)
-{
-	// Wait for phase
-	if (!WaitPhase(BUS::phase_t::datain)) {
-		return -1;
-	}
-
-	// Data reception
-	return bus->ReceiveHandShake(buf, length);
-}
-
-//---------------------------------------------------------------------------
-//
-//	Data out phase
-//
-//---------------------------------------------------------------------------
-int DataOut(uint8_t *buf, int length)
-{
-	// Wait for phase
-	if (!WaitPhase(BUS::phase_t::dataout)) {
-		return -1;
-	}
-
-	// Data transmission
-	return bus->SendHandShake(buf, length, BUS::SEND_NO_DELAY);
-}
-
-//---------------------------------------------------------------------------
-//
-//	Status Phase
-//
-//---------------------------------------------------------------------------
-int Status()
-{
-	uint8_t buf[256];
-
-	// Wait for phase
-	if (!WaitPhase(BUS::phase_t::status)) {
-		return -2;
-	}
-
-	// Data reception
-	if (bus->ReceiveHandShake(buf, 1) == 1) {
-		return (int)buf[0];
-	}
-
-	// Return error
-	return -1;
-}
-
-//---------------------------------------------------------------------------
-//
-//	Message in phase
-//
-//---------------------------------------------------------------------------
-int MessageIn()
-{
-	uint8_t buf[256];
-
-	// Wait for phase
-	if (!WaitPhase(BUS::phase_t::msgin)) {
-		return -2;
-	}
-
-	// Data reception
-	if (bus->ReceiveHandShake(buf, 1) == 1) {
-		return (int)buf[0];
-	}
-
-	// Return error
-	return -1;
-}
-
-//---------------------------------------------------------------------------
-//
-//	TEST UNIT READY
-//
-//---------------------------------------------------------------------------
-int TestUnitReady(int id)
-{
-	array<uint8_t, 256> cmd = {};
-
-	// Result code initialization
-	result = 0;
-
-	// SELECTION
-	if (!Selection(id)) {
-		result = -1;
-		goto exit;
-	}
-
-	// COMMAND
-	cmd[0] = 0x00;
-	if (!Command(cmd.data(), 6)) {
-		result = -2;
-		goto exit;
-	}
-
-	// STATUS
-	if (Status() < 0) {
-		result = -4;
-		goto exit;
-	}
-
-	// MESSAGE IN
-	if (MessageIn() < 0) {
-		result = -5;
-		goto exit;
-	}
-
-exit:
-	// Bus free
-	BusFree();
-
-	return result;
-}
-
-//---------------------------------------------------------------------------
-//
-//	REQUEST SENSE
-//
-//---------------------------------------------------------------------------
-int RequestSense(int id, uint8_t *buf)
-{
-	array<uint8_t, 256> cmd = {};
-
-	// Result code initialization
-	result = 0;
-	int count = 0;
-
-	// SELECTION
-	if (!Selection(id)) {
-		result = -1;
-		goto exit;
-	}
-
-	// COMMAND
-	cmd[0] = 0x03;
-	cmd[4] = 0xff;
-	if (!Command(cmd.data(), 6)) {
-		result = -2;
-		goto exit;
-	}
-
-	// DATAIN
-	memset(buf, 0x00, 256);
-	count = DataIn(buf, 256);
-	if (count <= 0) {
-		result = -3;
-		goto exit;
-	}
-
-	// STATUS
-	if (Status() < 0) {
-		result = -4;
-		goto exit;
-	}
-
-	// MESSAGE IN
-	if (MessageIn() < 0) {
-		result = -5;
-		goto exit;
-	}
-
-exit:
-	// Bus Free
-	BusFree();
-
-	// Returns the number of transfers if successful
-	if (result == 0) {
-		return count;
-	}
-
-	return result;
-}
-
-//---------------------------------------------------------------------------
-//
-//	MODE SENSE
-//
-//---------------------------------------------------------------------------
-int ModeSense(int id, uint8_t *buf)
-{
-	array<uint8_t, 256> cmd = {};
-
-	// Result code initialization
-	result = 0;
-	int count = 0;
-
-	// SELECTION
-	if (!Selection(id)) {
-		result = -1;
-		goto exit;
-	}
-
-	// COMMAND
-	cmd[0] = 0x1a;
-	cmd[2] = 0x3f;
-	cmd[4] = 0xff;
-	if (!Command(cmd.data(), 6)) {
-		result = -2;
-		goto exit;
-	}
-
-	// DATAIN
-	memset(buf, 0x00, 256);
-	count = DataIn(buf, 256);
-	if (count <= 0) {
-		result = -3;
-		goto exit;
-	}
-
-	// STATUS
-	if (Status() < 0) {
-		result = -4;
-		goto exit;
-	}
-
-	// MESSAGE IN
-	if (MessageIn() < 0) {
-		result = -5;
-		goto exit;
-	}
-
-exit:
-	// Bus free
-	BusFree();
-
-	// Returns the number of transfers if successful
-	if (result == 0) {
-		return count;
-	}
-
-	return result;
-}
-
-//---------------------------------------------------------------------------
-//
-//	INQUIRY
-//
-//---------------------------------------------------------------------------
-int Inquiry(int id, uint8_t *buf)
-{
-	array<uint8_t, 256> cmd = {};
-
-	// Result code initialization
-	result = 0;
-	int count = 0;
-
-	// SELECTION
-	if (!Selection(id)) {
-		result = -1;
-		goto exit;
-	}
-
-	// COMMAND
-	cmd[0] = 0x12;
-	cmd[4] = 0xff;
-	if (!Command(cmd.data(), 6)) {
-		result = -2;
-		goto exit;
-	}
-
-	// DATAIN
-	memset(buf, 0x00, 256);
-	count = DataIn(buf, 256);
-	if (count <= 0) {
-		result = -3;
-		goto exit;
-	}
-
-	// STATUS
-	if (Status() < 0) {
-		result = -4;
-		goto exit;
-	}
-
-	// MESSAGE IN
-	if (MessageIn() < 0) {
-		result = -5;
-		goto exit;
-	}
-
-exit:
-	// Bus free
-	BusFree();
-
-	// Returns the number of transfers if successful
-	if (result == 0) {
-		return count;
-	}
-
-	return result;
-}
-
-//---------------------------------------------------------------------------
-//
-//	READ CAPACITY
-//
-//---------------------------------------------------------------------------
-int ReadCapacity(int id, uint8_t *buf)
-{
-	array<uint8_t, 256> cmd = {};
-
-	// Result code initialization
-	result = 0;
-	int count = 0;
-
-	// SELECTION
-	if (!Selection(id)) {
-		result = -1;
-		goto exit;
-	}
-
-	// COMMAND
-	cmd[0] = 0x25;
-	if (!Command(cmd.data(), 10)) {
-		result = -2;
-		goto exit;
-	}
-
-	// DATAIN
-	memset(buf, 0x00, 8);
-	count = DataIn(buf, 8);
-	if (count <= 0) {
-		result = -3;
-		goto exit;
-	}
-
-	// STATUS
-	if (Status() < 0) {
-		result = -4;
-		goto exit;
-	}
-
-	// MESSAGE IN
-	if (MessageIn() < 0) {
-		result = -5;
-		goto exit;
-	}
-
-exit:
-	// Bus free
-	BusFree();
-
-	// Returns the number of transfers if successful
-	if (result == 0) {
-		return count;
-	}
-
-	return result;
-}
-
-//---------------------------------------------------------------------------
-//
-//	READ10
-//
-//---------------------------------------------------------------------------
-int Read10(int id, uint32_t bstart, uint32_t blength, uint32_t length, uint8_t *buf)
-{
-	array<uint8_t, 256> cmd = {};
-
-	// Result code initialization
-	result = 0;
-	int count = 0;
-
-	// SELECTION
-	if (!Selection(id)) {
-		result = -1;
-		goto exit;
-	}
-
-	// COMMAND
-	cmd[0] = 0x28;
-	cmd[2] = (uint8_t)(bstart >> 24);
-	cmd[3] = (uint8_t)(bstart >> 16);
-	cmd[4] = (uint8_t)(bstart >> 8);
-	cmd[5] = (uint8_t)bstart;
-	cmd[7] = (uint8_t)(blength >> 8);
-	cmd[8] = (uint8_t)blength;
-	if (!Command(cmd.data(), 10)) {
-		result = -2;
-		goto exit;
-	}
-
-	// DATAIN
-	count = DataIn(buf, length);
-	if (count <= 0) {
-		result = -3;
-		goto exit;
-	}
-
-	// STATUS
-	if (Status() < 0) {
-		result = -4;
-		goto exit;
-	}
-
-	// MESSAGE IN
-	if (MessageIn() < 0) {
-		result = -5;
-		goto exit;
-	}
-
-exit:
-	// Bus free
-	BusFree();
-
-	// Returns the number of transfers if successful
-	if (result == 0) {
-		return count;
-	}
-
-	return result;
-}
-
-//---------------------------------------------------------------------------
-//
-//	WRITE10
-//
-//---------------------------------------------------------------------------
-int Write10(int id, uint32_t bstart, uint32_t blength, uint32_t length, uint8_t *buf)
-{
-	array<uint8_t, 256> cmd = {};
-
-	// Result code initialization
-	result = 0;
-	int count = 0;
-
-	// SELECTION
-	if (!Selection(id)) {
-		result = -1;
-		goto exit;
-	}
-
-	// COMMAND
-	cmd[0] = 0x2a;
-	cmd[2] = (uint8_t)(bstart >> 24);
-	cmd[3] = (uint8_t)(bstart >> 16);
-	cmd[4] = (uint8_t)(bstart >> 8);
-	cmd[5] = (uint8_t)bstart;
-	cmd[7] = (uint8_t)(blength >> 8);
-	cmd[8] = (uint8_t)blength;
-	if (!Command(cmd.data(), 10)) {
-		result = -2;
-		goto exit;
-	}
-
-	// DATAOUT
-	count = DataOut(buf, length);
-	if (count <= 0) {
-		result = -3;
-		goto exit;
-	}
-
-	// STATUS
-	if (Status() < 0) {
-		result = -4;
-		goto exit;
-	}
-
-	// MESSAGE IN
-	if (MessageIn() < 0) {
-		result = -5;
-		goto exit;
-	}
-
-exit:
-	// Bus free
-	BusFree();
-
-	// Returns the number of transfers if successful
-	if (result == 0) {
-		return count;
-	}
-
-	return result;
-}
-
-//---------------------------------------------------------------------------
-//
-//	Main process
-//
-//---------------------------------------------------------------------------
-int main(int argc, char* argv[])
-{
-	int i;
-	char str[32];
-	uint32_t bsiz;
-	uint32_t bnum;
-	uint32_t duni;
-	uint32_t dsiz;
-	uint32_t dnum;
-	Fileio fio;
-	Fileio::OpenMode omode;
-	off_t size;
-
-	// Banner output
-	if (!Banner(argc, argv)) {
-		exit(0);
-	}
-
-	// Initialization
-	if (!Init()) {
-		fprintf(stderr, "Error : Initializing. Are you root?\n");
-
-		// Probably not root
-		exit(EPERM);
-	}
-
-	// Prase Argument
-	if (!ParseArgument(argc, argv)) {
-		// Cleanup
-		Cleanup();
-
-		// Exit with invalid argument error
-		exit(EINVAL);
-	}
-
-#ifndef USE_SEL_EVENT_ENABLE
-	cerr << "Error: No RaSCSI hardware support" << endl;
-	exit(EXIT_FAILURE);
-#endif
-
-	// Reset the SCSI bus
-	Reset();
-
-	// File Open
-	if (restore) {
-		omode = Fileio::OpenMode::ReadOnly;
-	} else {
-		omode = Fileio::OpenMode::WriteOnly;
-	}
-	if (!fio.Open(hdsfile.c_str(), omode)) {
-		fprintf(stderr, "Error : Can't open hds file\n");
-
-		// Cleanup
-		Cleanup();
-		exit(EPERM);
-	}
-
-	// Bus free
-	BusFree();
-
-	// Assert reset signal
-	bus->SetRST(true);
-	// Wait 1 ms
-	const timespec ts = { .tv_sec = 0, .tv_nsec = 1000 * 1000};
-	nanosleep(&ts, nullptr);
-	bus->SetRST(false);
-
-	// Start dump
-	printf("TARGET ID               : %d\n", targetid);
-	printf("BOARD ID                : %d\n", boardid);
-
-	// TEST UNIT READY
-	int count = TestUnitReady(targetid);
-	if (count < 0) {
-		fprintf(stderr, "TEST UNIT READY ERROR %d\n", count);
-		goto cleanup_exit;
-	}
-
-	// REQUEST SENSE(for CHECK CONDITION)
-	count = RequestSense(targetid, buffer);
-	if (count < 0) {
-		fprintf(stderr, "REQUEST SENSE ERROR %d\n", count);
-		goto cleanup_exit;
-	}
-
-	// INQUIRY
-	count = Inquiry(targetid, buffer);
-	if (count < 0) {
-		fprintf(stderr, "INQUIRY ERROR %d\n", count);
-		goto cleanup_exit;
-	}
-
-	// Display INQUIRY information
-	memset(str, 0x00, sizeof(str));
-	memcpy(str, &buffer[8], 8);
-	printf("Vendor                  : %s\n", str);
-	memset(str, 0x00, sizeof(str));
-	memcpy(str, &buffer[16], 16);
-	printf("Product                 : %s\n", str);
-	memset(str, 0x00, sizeof(str));
-	memcpy(str, &buffer[32], 4);
-	printf("Revison                 : %s\n", str);
-
-	// Get drive capacity
-	count = ReadCapacity(targetid, buffer);
-	if (count < 0) {
-		fprintf(stderr, "READ CAPACITY ERROR %d\n", count);
-		goto cleanup_exit;
-	}
-
-	// Display block size and number of blocks
-	bsiz =
-		(buffer[4] << 24) | (buffer[5] << 16) |
-		(buffer[6] << 8) | buffer[7];
-	bnum =
-		(buffer[0] << 24) | (buffer[1] << 16) |
-		(buffer[2] << 8) | buffer[3];
-	bnum++;
-	printf("Number of blocks        : %d Blocks\n", (int)bnum);
-	printf("Block length            : %d Bytes\n", (int)bsiz);
-	printf("Unit Capacity           : %d MBytes %d Bytes\n",
-		(int)(bsiz * bnum / 1024 / 1024),
-		(int)(bsiz * bnum));
-
-	// Get the restore file size
-	if (restore) {
-		size = fio.GetFileSize();
-		printf("Restore file size       : %d bytes", (int)size);
-		if (size > (off_t)(bsiz * bnum)) {
-			printf("(WARNING : File size is larger than disk size)");
-		} else if (size < (off_t)(bsiz * bnum)) {
-			printf("(ERROR   : File size is smaller than disk size)\n");
-			goto cleanup_exit;
-		}
-		printf("\n");
-	}
-
-	// Dump by buffer size
-	duni = BUFSIZE;
-	duni /= bsiz;
-	dsiz = BUFSIZE;
-	dnum = bnum * bsiz;
-	dnum /= BUFSIZE;
-
-	if (restore) {
-		printf("Restore progress        : ");
-	} else {
-		printf("Dump progress           : ");
-	}
-
-	for (i = 0; i < (int)dnum; i++) {
-		if (i > 0) {
-			printf("\033[21D");
-			printf("\033[0K");
-		}
-		printf("%3d%%(%7d/%7d)",
-			(int)((i + 1) * 100 / dnum),
-			(int)(i * duni),
-			(int)bnum);
-		fflush(stdout);
-
-		if (restore) {
-			if (fio.Read(buffer, dsiz) && Write10(targetid, i * duni, duni, dsiz, buffer) >= 0) {
-				continue;
-			}
-		} else {
-			if (Read10(targetid, i * duni, duni, dsiz, buffer) >= 0 && fio.Write(buffer, dsiz)) {
-				continue;
-			}
-		}
-
-		printf("\n");
-		printf("Error occured and aborted... %d\n", result);
-		goto cleanup_exit;
-	}
-
-	if (dnum > 0) {
-		printf("\033[21D");
-		printf("\033[0K");
-	}
-
-	// Rounding on capacity
-	dnum = bnum % duni;
-	dsiz = dnum * bsiz;
-	if (dnum > 0) {
-		if (restore) {
-			if (fio.Read(buffer, dsiz)) {
-				Write10(targetid, i * duni, dnum, dsiz, buffer);
-			}
-		} else {
-			if (Read10(targetid, i * duni, dnum, dsiz, buffer) >= 0) {
-				fio.Write(buffer, dsiz);
-			}
-		}
-	}
-
-	// Completion Message
-	printf("%3d%%(%7d/%7d)\n", 100, (int)bnum, (int)bnum);
-
-cleanup_exit:
-	// File close
-	fio.Close();
-
-	// Cleanup
-	Cleanup();
-
-	// end
-	exit(0);
+	const vector<char *> args(argv, argv + argc);
+
+	return RasDump().run(args);
 }
diff --git a/cpp/rasdump/rasdump_core.cpp b/cpp/rasdump/rasdump_core.cpp
new file mode 100644
index 00000000..150e7396
--- /dev/null
+++ b/cpp/rasdump/rasdump_core.cpp
@@ -0,0 +1,998 @@
+//---------------------------------------------------------------------------
+//
+//	SCSI Target Emulator RaSCSI Reloaded
+//	for Raspberry Pi
+//
+//	Powered by XM6 TypeG Technology.
+//	Copyright (C) 2016-2020 GIMONS
+//	[ HDD dump utility (initiator mode) ]
+//
+//---------------------------------------------------------------------------
+
+#include <cerrno>
+#include <csignal>
+#include <unistd.h>
+#include "rasdump/rasdump_fileio.h"
+#include "rasdump/rasdump_core.h"
+#include "hal/gpiobus.h"
+#include "hal/gpiobus_factory.h"
+#include "hal/systimer.h"
+#include "rascsi_version.h"
+#include <cstring>
+#include <iostream>
+#include <array>
+
+using namespace std;
+
+//---------------------------------------------------------------------------
+//
+//	Constant Declaration
+//
+//---------------------------------------------------------------------------
+static const int BUFSIZE = 1024 * 64;			// Buffer size of about 64KB
+
+//---------------------------------------------------------------------------
+//
+//	Variable Declaration
+//
+//---------------------------------------------------------------------------
+unique_ptr<GPIOBUS> bus;		// GPIO Bus
+int targetid;					// Target ID
+int boardid;					// Board ID (own ID)
+string hdsfile;					// HDS file
+bool restore;					// Restore flag
+array<uint8_t, BUFSIZE> buffer;	// Work Buffer
+int result;						// Result Code
+
+//---------------------------------------------------------------------------
+//
+//	Cleanup() Function declaration
+//
+//---------------------------------------------------------------------------
+void Cleanup();
+
+//---------------------------------------------------------------------------
+//
+//	Signal processing
+//
+//---------------------------------------------------------------------------
+void KillHandler(int)
+{
+	// Stop running
+	Cleanup();
+	exit(0);
+}
+
+//---------------------------------------------------------------------------
+//
+//	Banner Output
+//
+//---------------------------------------------------------------------------
+bool RasDump::Banner(const vector<char *>& args)
+{
+	printf("RaSCSI hard disk dump utility ");
+	printf("version %s (%s, %s)\n",
+		rascsi_get_version_string().c_str(),
+		__DATE__,
+		__TIME__);
+
+	if (args.size() < 2 || strcmp(args[1], "-h") == 0) {
+		printf("Usage: %s -i ID [-b BID] -f FILE [-r]\n", args[0]);
+		printf(" ID is target device SCSI ID {0|1|2|3|4|5|6|7}.\n");
+		printf(" BID is rascsi board SCSI ID {0|1|2|3|4|5|6|7}. Default is 7.\n");
+		printf(" FILE is HDS file path.\n");
+		printf(" -r is restore operation.\n");
+		return false;
+	}
+
+	return true;
+}
+
+//---------------------------------------------------------------------------
+//
+//	Initialization
+//
+//---------------------------------------------------------------------------
+bool RasDump::Init()
+{
+	// Interrupt handler setting
+	if (signal(SIGINT, KillHandler) == SIG_ERR) {
+		return false;
+	}
+	if (signal(SIGHUP, KillHandler) == SIG_ERR) {
+		return false;
+	}
+	if (signal(SIGTERM, KillHandler) == SIG_ERR) {
+		return false;
+	}
+
+	bus = GPIOBUS_Factory::Create();
+
+	// GPIO Initialization
+	if (!bus->Init(BUS::mode_e::INITIATOR)) {
+		return false;
+	}
+
+	// Work Intitialization
+	targetid = -1;
+	boardid = 7;
+	restore = false;
+
+	return true;
+}
+
+//---------------------------------------------------------------------------
+//
+//	Cleanup
+//
+//---------------------------------------------------------------------------
+void Cleanup()
+{
+	// Cleanup the bus
+	bus->Cleanup();
+}
+
+//---------------------------------------------------------------------------
+//
+//	Reset
+//
+//---------------------------------------------------------------------------
+void RasDump::Reset()
+{
+	// Reset the bus signal line
+	bus->Reset();
+}
+
+//---------------------------------------------------------------------------
+//
+//	Argument processing
+//
+//---------------------------------------------------------------------------
+bool RasDump::ParseArguments(const vector<char *>& args)
+{
+	int opt;
+	const char *file = nullptr;
+
+	// Argument Parsing
+	opterr = 0;
+	while ((opt = getopt(args.size(), args.data(), "i:b:f:r")) != -1) {
+		switch (opt) {
+			case 'i':
+				targetid = optarg[0] - '0';
+				break;
+
+			case 'b':
+				boardid = optarg[0] - '0';
+				break;
+
+			case 'f':
+				file = optarg;
+				break;
+
+			case 'r':
+				restore = true;
+				break;
+
+			default:
+				break;
+		}
+	}
+
+	// TARGET ID check
+	if (targetid < 0 || targetid > 7) {
+		fprintf(stderr,
+			"Error : Invalid target id range\n");
+		return false;
+	}
+
+	// BOARD ID check
+	if (boardid < 0 || boardid > 7) {
+		fprintf(stderr,
+			"Error : Invalid board id range\n");
+		return false;
+	}
+
+	// Target and Board ID duplication check
+	if (targetid == boardid) {
+		fprintf(stderr,
+			"Error : Invalid target or board id\n");
+		return false;
+	}
+
+	// File Check
+	if (!file) {
+		fprintf(stderr,
+			"Error : Invalid file path\n");
+		return false;
+	}
+
+	hdsfile = file;
+
+	return true;
+}
+
+//---------------------------------------------------------------------------
+//
+//	Wait Phase
+//
+//---------------------------------------------------------------------------
+bool RasDump::WaitPhase(BUS::phase_t phase)
+{
+	// Timeout (3000ms)
+	const uint32_t now = SysTimer::GetTimerLow();
+	while ((SysTimer::GetTimerLow() - now) < 3 * 1000 * 1000) {
+		bus->Acquire();
+		if (bus->GetREQ() && bus->GetPhase() == phase) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+//---------------------------------------------------------------------------
+//
+//	Bus Free Phase
+//
+//---------------------------------------------------------------------------
+void RasDump::BusFree()
+{
+	// Bus Reset
+	bus->Reset();
+}
+
+//---------------------------------------------------------------------------
+//
+//	Selection Phase
+//
+//---------------------------------------------------------------------------
+bool RasDump::Selection(int id)
+{
+	// ID setting and SEL assert
+	uint8_t data = 1 << boardid;
+	data |= (1 << id);
+	bus->SetDAT(data);
+	bus->SetSEL(true);
+
+	// wait for busy
+	int count = 10000;
+	do {
+		// Wait 20 microseconds
+		const timespec ts = { .tv_sec = 0, .tv_nsec = 20 * 1000};
+		nanosleep(&ts, nullptr);
+		bus->Acquire();
+		if (bus->GetBSY()) {
+			break;
+		}
+	} while (count--);
+
+	// SEL negate
+	bus->SetSEL(false);
+
+	// Success if the target is busy
+	return bus->GetBSY();
+}
+
+//---------------------------------------------------------------------------
+//
+//	Command Phase
+//
+//---------------------------------------------------------------------------
+bool RasDump::Command(uint8_t *buf, int length)
+{
+	// Waiting for Phase
+	if (!WaitPhase(BUS::phase_t::command)) {
+		return false;
+	}
+
+	// Send Command
+	const int count = bus->SendHandShake(buf, length, BUS::SEND_NO_DELAY);
+
+	// Success if the transmission result is the same as the number
+	// of requests
+	if (count == length) {
+		return true;
+	}
+
+	// Return error
+	return false;
+}
+
+//---------------------------------------------------------------------------
+//
+//	Data in phase
+//
+//---------------------------------------------------------------------------
+int RasDump::DataIn(uint8_t *buf, int length)
+{
+	// Wait for phase
+	if (!WaitPhase(BUS::phase_t::datain)) {
+		return -1;
+	}
+
+	// Data reception
+	return bus->ReceiveHandShake(buf, length);
+}
+
+//---------------------------------------------------------------------------
+//
+//	Data out phase
+//
+//---------------------------------------------------------------------------
+int RasDump::DataOut(uint8_t *buf, int length)
+{
+	// Wait for phase
+	if (!WaitPhase(BUS::phase_t::dataout)) {
+		return -1;
+	}
+
+	// Data transmission
+	return bus->SendHandShake(buf, length, BUS::SEND_NO_DELAY);
+}
+
+//---------------------------------------------------------------------------
+//
+//	Status Phase
+//
+//---------------------------------------------------------------------------
+int RasDump::Status()
+{
+	uint8_t buf[256];
+
+	// Wait for phase
+	if (!WaitPhase(BUS::phase_t::status)) {
+		return -2;
+	}
+
+	// Data reception
+	if (bus->ReceiveHandShake(buf, 1) == 1) {
+		return (int)buf[0];
+	}
+
+	// Return error
+	return -1;
+}
+
+//---------------------------------------------------------------------------
+//
+//	Message in phase
+//
+//---------------------------------------------------------------------------
+int RasDump::MessageIn()
+{
+	uint8_t buf[256];
+
+	// Wait for phase
+	if (!WaitPhase(BUS::phase_t::msgin)) {
+		return -2;
+	}
+
+	// Data reception
+	if (bus->ReceiveHandShake(buf, 1) == 1) {
+		return (int)buf[0];
+	}
+
+	// Return error
+	return -1;
+}
+
+//---------------------------------------------------------------------------
+//
+//	TEST UNIT READY
+//
+//---------------------------------------------------------------------------
+int RasDump::TestUnitReady(int id)
+{
+	array<uint8_t, 256> cmd = {};
+
+	// Result code initialization
+	result = 0;
+
+	// SELECTION
+	if (!Selection(id)) {
+		result = -1;
+		goto exit;
+	}
+
+	// COMMAND
+	cmd[0] = 0x00;
+	if (!Command(cmd.data(), 6)) {
+		result = -2;
+		goto exit;
+	}
+
+	// STATUS
+	if (Status() < 0) {
+		result = -4;
+		goto exit;
+	}
+
+	// MESSAGE IN
+	if (MessageIn() < 0) {
+		result = -5;
+		goto exit;
+	}
+
+exit:
+	// Bus free
+	BusFree();
+
+	return result;
+}
+
+//---------------------------------------------------------------------------
+//
+//	REQUEST SENSE
+//
+//---------------------------------------------------------------------------
+int RasDump::RequestSense(int id, uint8_t *buf)
+{
+	array<uint8_t, 256> cmd = {};
+
+	// Result code initialization
+	result = 0;
+	int count = 0;
+
+	// SELECTION
+	if (!Selection(id)) {
+		result = -1;
+		goto exit;
+	}
+
+	// COMMAND
+	cmd[0] = 0x03;
+	cmd[4] = 0xff;
+	if (!Command(cmd.data(), 6)) {
+		result = -2;
+		goto exit;
+	}
+
+	// DATAIN
+	memset(buf, 0x00, 256);
+	count = DataIn(buf, 256);
+	if (count <= 0) {
+		result = -3;
+		goto exit;
+	}
+
+	// STATUS
+	if (Status() < 0) {
+		result = -4;
+		goto exit;
+	}
+
+	// MESSAGE IN
+	if (MessageIn() < 0) {
+		result = -5;
+		goto exit;
+	}
+
+exit:
+	// Bus Free
+	BusFree();
+
+	// Returns the number of transfers if successful
+	if (result == 0) {
+		return count;
+	}
+
+	return result;
+}
+
+//---------------------------------------------------------------------------
+//
+//	MODE SENSE
+//
+//---------------------------------------------------------------------------
+int RasDump::ModeSense(int id, uint8_t *buf)
+{
+	array<uint8_t, 256> cmd = {};
+
+	// Result code initialization
+	result = 0;
+	int count = 0;
+
+	// SELECTION
+	if (!Selection(id)) {
+		result = -1;
+		goto exit;
+	}
+
+	// COMMAND
+	cmd[0] = 0x1a;
+	cmd[2] = 0x3f;
+	cmd[4] = 0xff;
+	if (!Command(cmd.data(), 6)) {
+		result = -2;
+		goto exit;
+	}
+
+	// DATAIN
+	memset(buf, 0x00, 256);
+	count = DataIn(buf, 256);
+	if (count <= 0) {
+		result = -3;
+		goto exit;
+	}
+
+	// STATUS
+	if (Status() < 0) {
+		result = -4;
+		goto exit;
+	}
+
+	// MESSAGE IN
+	if (MessageIn() < 0) {
+		result = -5;
+		goto exit;
+	}
+
+exit:
+	// Bus free
+	BusFree();
+
+	// Returns the number of transfers if successful
+	if (result == 0) {
+		return count;
+	}
+
+	return result;
+}
+
+//---------------------------------------------------------------------------
+//
+//	INQUIRY
+//
+//---------------------------------------------------------------------------
+int RasDump::Inquiry(int id, uint8_t *buf)
+{
+	array<uint8_t, 256> cmd = {};
+
+	// Result code initialization
+	result = 0;
+	int count = 0;
+
+	// SELECTION
+	if (!Selection(id)) {
+		result = -1;
+		goto exit;
+	}
+
+	// COMMAND
+	cmd[0] = 0x12;
+	cmd[4] = 0xff;
+	if (!Command(cmd.data(), 6)) {
+		result = -2;
+		goto exit;
+	}
+
+	// DATAIN
+	memset(buf, 0x00, 256);
+	count = DataIn(buf, 256);
+	if (count <= 0) {
+		result = -3;
+		goto exit;
+	}
+
+	// STATUS
+	if (Status() < 0) {
+		result = -4;
+		goto exit;
+	}
+
+	// MESSAGE IN
+	if (MessageIn() < 0) {
+		result = -5;
+		goto exit;
+	}
+
+exit:
+	// Bus free
+	BusFree();
+
+	// Returns the number of transfers if successful
+	if (result == 0) {
+		return count;
+	}
+
+	return result;
+}
+
+//---------------------------------------------------------------------------
+//
+//	READ CAPACITY
+//
+//---------------------------------------------------------------------------
+int RasDump::ReadCapacity(int id, uint8_t *buf)
+{
+	array<uint8_t, 256> cmd = {};
+
+	// Result code initialization
+	result = 0;
+	int count = 0;
+
+	// SELECTION
+	if (!Selection(id)) {
+		result = -1;
+		goto exit;
+	}
+
+	// COMMAND
+	cmd[0] = 0x25;
+	if (!Command(cmd.data(), 10)) {
+		result = -2;
+		goto exit;
+	}
+
+	// DATAIN
+	memset(buf, 0x00, 8);
+	count = DataIn(buf, 8);
+	if (count <= 0) {
+		result = -3;
+		goto exit;
+	}
+
+	// STATUS
+	if (Status() < 0) {
+		result = -4;
+		goto exit;
+	}
+
+	// MESSAGE IN
+	if (MessageIn() < 0) {
+		result = -5;
+		goto exit;
+	}
+
+exit:
+	// Bus free
+	BusFree();
+
+	// Returns the number of transfers if successful
+	if (result == 0) {
+		return count;
+	}
+
+	return result;
+}
+
+//---------------------------------------------------------------------------
+//
+//	READ10
+//
+//---------------------------------------------------------------------------
+int RasDump::Read10(int id, uint32_t bstart, uint32_t blength, uint32_t length, uint8_t *buf)
+{
+	array<uint8_t, 256> cmd = {};
+
+	// Result code initialization
+	result = 0;
+	int count = 0;
+
+	// SELECTION
+	if (!Selection(id)) {
+		result = -1;
+		goto exit;
+	}
+
+	// COMMAND
+	cmd[0] = 0x28;
+	cmd[2] = (uint8_t)(bstart >> 24);
+	cmd[3] = (uint8_t)(bstart >> 16);
+	cmd[4] = (uint8_t)(bstart >> 8);
+	cmd[5] = (uint8_t)bstart;
+	cmd[7] = (uint8_t)(blength >> 8);
+	cmd[8] = (uint8_t)blength;
+	if (!Command(cmd.data(), 10)) {
+		result = -2;
+		goto exit;
+	}
+
+	// DATAIN
+	count = DataIn(buf, length);
+	if (count <= 0) {
+		result = -3;
+		goto exit;
+	}
+
+	// STATUS
+	if (Status() < 0) {
+		result = -4;
+		goto exit;
+	}
+
+	// MESSAGE IN
+	if (MessageIn() < 0) {
+		result = -5;
+		goto exit;
+	}
+
+exit:
+	// Bus free
+	BusFree();
+
+	// Returns the number of transfers if successful
+	if (result == 0) {
+		return count;
+	}
+
+	return result;
+}
+
+//---------------------------------------------------------------------------
+//
+//	WRITE10
+//
+//---------------------------------------------------------------------------
+int RasDump::Write10(int id, uint32_t bstart, uint32_t blength, uint32_t length, uint8_t *buf)
+{
+	array<uint8_t, 256> cmd = {};
+
+	// Result code initialization
+	result = 0;
+	int count = 0;
+
+	// SELECTION
+	if (!Selection(id)) {
+		result = -1;
+		goto exit;
+	}
+
+	// COMMAND
+	cmd[0] = 0x2a;
+	cmd[2] = (uint8_t)(bstart >> 24);
+	cmd[3] = (uint8_t)(bstart >> 16);
+	cmd[4] = (uint8_t)(bstart >> 8);
+	cmd[5] = (uint8_t)bstart;
+	cmd[7] = (uint8_t)(blength >> 8);
+	cmd[8] = (uint8_t)blength;
+	if (!Command(cmd.data(), 10)) {
+		result = -2;
+		goto exit;
+	}
+
+	// DATAOUT
+	count = DataOut(buf, length);
+	if (count <= 0) {
+		result = -3;
+		goto exit;
+	}
+
+	// STATUS
+	if (Status() < 0) {
+		result = -4;
+		goto exit;
+	}
+
+	// MESSAGE IN
+	if (MessageIn() < 0) {
+		result = -5;
+		goto exit;
+	}
+
+exit:
+	// Bus free
+	BusFree();
+
+	// Returns the number of transfers if successful
+	if (result == 0) {
+		return count;
+	}
+
+	return result;
+}
+
+//---------------------------------------------------------------------------
+//
+//	Main process
+//
+//---------------------------------------------------------------------------
+int RasDump::run(const vector<char *>& args)
+{
+	int i;
+	char str[32];
+	uint32_t bsiz;
+	uint32_t bnum;
+	uint32_t duni;
+	uint32_t dsiz;
+	uint32_t dnum;
+	Fileio fio;
+	Fileio::OpenMode omode;
+	off_t size;
+
+	// Banner output
+	if (!Banner(args)) {
+		exit(0);
+	}
+
+	// Initialization
+	if (!Init()) {
+		fprintf(stderr, "Error : Initializing. Are you root?\n");
+
+		// Probably not root
+		exit(EPERM);
+	}
+
+	// Prase Argument
+	if (!ParseArguments(args)) {
+		// Cleanup
+		Cleanup();
+
+		// Exit with invalid argument error
+		exit(EINVAL);
+	}
+
+#ifndef USE_SEL_EVENT_ENABLE
+	cerr << "Error: No RaSCSI hardware support" << endl;
+	exit(EXIT_FAILURE);
+#endif
+
+	// Reset the SCSI bus
+	Reset();
+
+	// File Open
+	if (restore) {
+		omode = Fileio::OpenMode::ReadOnly;
+	} else {
+		omode = Fileio::OpenMode::WriteOnly;
+	}
+	if (!fio.Open(hdsfile.c_str(), omode)) {
+		fprintf(stderr, "Error : Can't open hds file\n");
+
+		// Cleanup
+		Cleanup();
+		exit(EPERM);
+	}
+
+	// Bus free
+	BusFree();
+
+	// Assert reset signal
+	bus->SetRST(true);
+	// Wait 1 ms
+	const timespec ts = { .tv_sec = 0, .tv_nsec = 1000 * 1000};
+	nanosleep(&ts, nullptr);
+	bus->SetRST(false);
+
+	// Start dump
+	printf("TARGET ID               : %d\n", targetid);
+	printf("BOARD ID                : %d\n", boardid);
+
+	// TEST UNIT READY
+	int count = TestUnitReady(targetid);
+	if (count < 0) {
+		fprintf(stderr, "TEST UNIT READY ERROR %d\n", count);
+		goto cleanup_exit;
+	}
+
+	// REQUEST SENSE(for CHECK CONDITION)
+	count = RequestSense(targetid, buffer.data());
+	if (count < 0) {
+		fprintf(stderr, "REQUEST SENSE ERROR %d\n", count);
+		goto cleanup_exit;
+	}
+
+	// INQUIRY
+	count = Inquiry(targetid, buffer.data());
+	if (count < 0) {
+		fprintf(stderr, "INQUIRY ERROR %d\n", count);
+		goto cleanup_exit;
+	}
+
+	// Display INQUIRY information
+	memset(str, 0x00, sizeof(str));
+	memcpy(str, &buffer[8], 8);
+	printf("Vendor                  : %s\n", str);
+	memset(str, 0x00, sizeof(str));
+	memcpy(str, &buffer[16], 16);
+	printf("Product                 : %s\n", str);
+	memset(str, 0x00, sizeof(str));
+	memcpy(str, &buffer[32], 4);
+	printf("Revison                 : %s\n", str);
+
+	// Get drive capacity
+	count = ReadCapacity(targetid, buffer.data());
+	if (count < 0) {
+		fprintf(stderr, "READ CAPACITY ERROR %d\n", count);
+		goto cleanup_exit;
+	}
+
+	// Display block size and number of blocks
+	bsiz =
+		(buffer[4] << 24) | (buffer[5] << 16) |
+		(buffer[6] << 8) | buffer[7];
+	bnum =
+		(buffer[0] << 24) | (buffer[1] << 16) |
+		(buffer[2] << 8) | buffer[3];
+	bnum++;
+	printf("Number of blocks        : %d Blocks\n", (int)bnum);
+	printf("Block length            : %d Bytes\n", (int)bsiz);
+	printf("Unit Capacity           : %d MBytes %d Bytes\n",
+		(int)(bsiz * bnum / 1024 / 1024),
+		(int)(bsiz * bnum));
+
+	// Get the restore file size
+	if (restore) {
+		size = fio.GetFileSize();
+		printf("Restore file size       : %d bytes", (int)size);
+		if (size > (off_t)(bsiz * bnum)) {
+			printf("(WARNING : File size is larger than disk size)");
+		} else if (size < (off_t)(bsiz * bnum)) {
+			printf("(ERROR   : File size is smaller than disk size)\n");
+			goto cleanup_exit;
+		}
+		printf("\n");
+	}
+
+	// Dump by buffer size
+	duni = BUFSIZE;
+	duni /= bsiz;
+	dsiz = BUFSIZE;
+	dnum = bnum * bsiz;
+	dnum /= BUFSIZE;
+
+	if (restore) {
+		printf("Restore progress        : ");
+	} else {
+		printf("Dump progress           : ");
+	}
+
+	for (i = 0; i < (int)dnum; i++) {
+		if (i > 0) {
+			printf("\033[21D");
+			printf("\033[0K");
+		}
+		printf("%3d%%(%7d/%7d)",
+			(int)((i + 1) * 100 / dnum),
+			(int)(i * duni),
+			(int)bnum);
+		fflush(stdout);
+
+		if (restore) {
+			if (fio.Read(buffer.data(), dsiz) && Write10(targetid, i * duni, duni, dsiz, buffer.data()) >= 0) {
+				continue;
+			}
+		} else {
+			if (Read10(targetid, i * duni, duni, dsiz, buffer.data()) >= 0 && fio.Write(buffer.data(), dsiz)) {
+				continue;
+			}
+		}
+
+		printf("\n");
+		printf("Error occured and aborted... %d\n", result);
+		goto cleanup_exit;
+	}
+
+	if (dnum > 0) {
+		printf("\033[21D");
+		printf("\033[0K");
+	}
+
+	// Rounding on capacity
+	dnum = bnum % duni;
+	dsiz = dnum * bsiz;
+	if (dnum > 0) {
+		if (restore) {
+			if (fio.Read(buffer.data(), dsiz)) {
+				Write10(targetid, i * duni, dnum, dsiz, buffer.data());
+			}
+		} else {
+			if (Read10(targetid, i * duni, dnum, dsiz, buffer.data()) >= 0) {
+				fio.Write(buffer.data(), dsiz);
+			}
+		}
+	}
+
+	// Completion Message
+	printf("%3d%%(%7d/%7d)\n", 100, (int)bnum, (int)bnum);
+
+cleanup_exit:
+	// File close
+	fio.Close();
+
+	// Cleanup
+	Cleanup();
+
+	// end
+	exit(0);
+}
diff --git a/cpp/rasdump/rasdump_core.h b/cpp/rasdump/rasdump_core.h
new file mode 100644
index 00000000..21bf806e
--- /dev/null
+++ b/cpp/rasdump/rasdump_core.h
@@ -0,0 +1,47 @@
+//---------------------------------------------------------------------------
+//
+// SCSI Target Emulator RaSCSI Reloaded
+// for Raspberry Pi
+//
+// Copyright (C) 2022 Uwe Seimet
+//
+//---------------------------------------------------------------------------
+
+#pragma once
+
+#include "hal/bus.h"
+#include <vector>
+
+using namespace std;
+
+class RasDump
+{
+public:
+
+	RasDump() = default;
+	~RasDump() = default;
+
+	int run(const vector<char *>&);
+
+private:
+
+	bool Banner(const vector<char *>&);
+	bool Init();
+	void Reset();
+	bool ParseArguments(const vector<char *>&);
+	bool WaitPhase(BUS::phase_t);
+	void BusFree();
+	bool Selection(int);
+	bool Command(uint8_t *, int);
+	int DataIn(uint8_t *, int);
+	int DataOut(uint8_t *, int);
+	int Status();
+	int MessageIn();
+	int TestUnitReady(int);
+	int RequestSense(int, uint8_t *);
+	int ModeSense(int, uint8_t *);
+	int Inquiry(int, uint8_t *);
+	int ReadCapacity(int, uint8_t *);
+	int Read10(int, uint32_t, uint32_t, uint32_t, uint8_t *);
+	int Write10(int, uint32_t, uint32_t, uint32_t, uint8_t *);
+};
diff --git a/cpp/rasdump_fileio.cpp b/cpp/rasdump/rasdump_fileio.cpp
similarity index 98%
rename from cpp/rasdump_fileio.cpp
rename to cpp/rasdump/rasdump_fileio.cpp
index 84e38ec1..4002dc8a 100644
--- a/cpp/rasdump_fileio.cpp
+++ b/cpp/rasdump/rasdump_fileio.cpp
@@ -8,7 +8,7 @@
 //
 //---------------------------------------------------------------------------
 
-#include "rasdump_fileio.h"
+#include "rasdump/rasdump_fileio.h"
 #include <fcntl.h>
 #include <unistd.h>
 #include <cassert>
diff --git a/cpp/rasdump_fileio.h b/cpp/rasdump/rasdump_fileio.h
similarity index 100%
rename from cpp/rasdump_fileio.h
rename to cpp/rasdump/rasdump_fileio.h
diff --git a/cpp/scsi.h b/cpp/scsi.h
index 749b9956..37d29f8c 100644
--- a/cpp/scsi.h
+++ b/cpp/scsi.h
@@ -9,6 +9,10 @@
 
 #pragma once
 
+#include <unordered_map>
+
+using namespace std;
+
 namespace scsi_defs {
 	enum class scsi_level : int {
 		SCSI_1_CCS = 1,
@@ -30,17 +34,20 @@ namespace scsi_defs {
 		COMMUNICATIONS = 9
 	};
 
-	// TODO Use a mapping of enum to structure with command byte count and enum name
 	enum class scsi_command : int {
 		eCmdTestUnitReady = 0x00,
 		eCmdRezero =  0x01,
 		eCmdRequestSense = 0x03,
-		eCmdFormat = 0x04,
-		eCmdReassign = 0x07,
+		eCmdFormatUnit = 0x04,
+		eCmdReassignBlocks = 0x07,
 		eCmdRead6 = 0x08,
+		// Bridge specific command
+		eCmdGetMessage10 = 0x08,
 		// DaynaPort specific command
 		eCmdRetrieveStats = 0x09,
 		eCmdWrite6 = 0x0A,
+		// Bridge specific ommand
+		eCmdSendMessage10 = 0x0A,
 		eCmdPrint = 0x0A,
 		eCmdSeek6 = 0x0B,
 		// DaynaPort specific command
@@ -57,10 +64,8 @@ namespace scsi_defs {
 		eCmdModeSense6 = 0x1A,
 		eCmdStartStop = 0x1B,
 		eCmdStopPrint = 0x1B,
-		eCmdSendDiag = 0x1D,
-		eCmdRemoval = 0x1E,
-		// ICD specific command, evaluated by the controller
-		eCmdIcd = 0x1F,
+		eCmdSendDiagnostic = 0x1D,
+		eCmdPreventAllowMediumRemoval = 0x1E,
 		eCmdReadCapacity10 = 0x25,
 		eCmdRead10 = 0x28,
 		eCmdWrite10 = 0x2A,
@@ -114,4 +119,49 @@ namespace scsi_defs {
 		MEDIUM_NOT_PRESENT = 0x3a,
 		LOAD_OR_EJECT_FAILED = 0x53
 	};
+
+	static const unordered_map<scsi_command, const char *> command_names = {
+			{ scsi_command::eCmdTestUnitReady, "TestUnitReady" },
+			{ scsi_command::eCmdRezero, "Rezero" },
+			{ scsi_command::eCmdRequestSense, "RequestSense" },
+			{ scsi_command::eCmdFormatUnit, "FormatUnit" },
+			{ scsi_command::eCmdReassignBlocks, "ReassignBlocks" },
+			{ scsi_command::eCmdRead6, "Read6/GetMessage10" },
+			{ scsi_command::eCmdRetrieveStats, "RetrieveStats" },
+			{ scsi_command::eCmdWrite6, "Write6/Print/SendMessage10" },
+			{ scsi_command::eCmdSeek6, "Seek6" },
+			{ scsi_command::eCmdSetIfaceMode, "SetIfaceMode" },
+			{ scsi_command::eCmdSetMcastAddr, "SetMcastAddr" },
+			{ scsi_command::eCmdEnableInterface, "EnableInterface" },
+			{ scsi_command::eCmdSynchronizeBuffer, "SynchronizeBuffer" },
+			{ scsi_command::eCmdInquiry, "Inquiry" },
+			{ scsi_command::eCmdModeSelect6, "ModeSelect6" },
+			{ scsi_command::eCmdReserve6, "Reserve6" },
+			{ scsi_command::eCmdRelease6, "Release6" },
+			{ scsi_command::eCmdModeSense6, "ModeSense6" },
+			{ scsi_command::eCmdStartStop, "StartStop" },
+			{ scsi_command::eCmdStopPrint, "StopPrint" },
+			{ scsi_command::eCmdSendDiagnostic, "SendDiagnostic" },
+			{ scsi_command::eCmdPreventAllowMediumRemoval, "PreventAllowMediumRemoval" },
+			{ scsi_command::eCmdReadCapacity10, "ReadCapacity10" },
+			{ scsi_command::eCmdRead10, "Read10" },
+			{ scsi_command::eCmdWrite10, "Write10" },
+			{ scsi_command::eCmdSeek10, "Seek10" },
+			{ scsi_command::eCmdVerify10, "Verify10" },
+			{ scsi_command::eCmdSynchronizeCache10, "SynchronizeCache10" },
+			{ scsi_command::eCmdReadDefectData10, "ReadDefectData10" },
+			{ scsi_command::eCmdReadLong10, "ReadLong10" },
+			{ scsi_command::eCmdWriteLong10, "WriteLong10" },
+			{ scsi_command::eCmdReadToc, "ReadToc" },
+			{ scsi_command::eCmdGetEventStatusNotification, "GetEventStatusNotification" },
+			{ scsi_command::eCmdModeSelect10, "ModeSelect10" },
+			{ scsi_command::eCmdModeSense10, "ModeSense10" },
+			{ scsi_command::eCmdRead16, "Read16" },
+			{ scsi_command::eCmdWrite16, "Write16" },
+			{ scsi_command::eCmdVerify16, "Verify16" },
+			{ scsi_command::eCmdSynchronizeCache16, "SynchronizeCache16" },
+			{ scsi_command::eCmdReadCapacity16_ReadLong16, "ReadCapacity16/ReadLong16" },
+			{ scsi_command::eCmdWriteLong16, "WriteLong16" },
+			{ scsi_command::eCmdReportLuns, "ReportLuns" }
+	};
 };
diff --git a/cpp/scsimon.cpp b/cpp/scsimon.cpp
index cdf6ac65..e6c6287d 100644
--- a/cpp/scsimon.cpp
+++ b/cpp/scsimon.cpp
@@ -1,412 +1,19 @@
 //---------------------------------------------------------------------------
 //
-//	SCSI Target Emulator RaSCSI Reloaded
-//	for Raspberry Pi
+// SCSI Target Emulator RaSCSI Reloaded
+// for Raspberry Pi
 //
-//	Powered by XM6 TypeG Technology.
-//	Copyright (C) 2016-2020 GIMONS
+// Copyright (C) 2022 Uwe Seimet
 //
 //---------------------------------------------------------------------------
 
-#include "log.h"
-#include "hal/gpiobus.h"
-#include "hal/gpiobus_factory.h"
-#include "rascsi_version.h"
-#include <sys/time.h>
-#include <climits>
-#include <csignal>
-#include <sstream>
-#include <iostream>
-#include <getopt.h>
-#include <sched.h>
-#include "monitor/sm_reports.h"
-#include "monitor/data_sample.h"
+#include "monitor/sm_core.h"
 
 using namespace std;
 
-static const int _MAX_FNAME = 256;
-
-//---------------------------------------------------------------------------
-//
-//	Variable declarations
-//
-//---------------------------------------------------------------------------
-static volatile bool running; // Running flag
-shared_ptr<GPIOBUS> bus;			      // GPIO Bus
-
-uint32_t buff_size = 1000000;
-data_capture *data_buffer;
-uint32_t data_idx = 0;
-
-double ns_per_loop;
-
-bool print_help = false;
-bool import_data = false;
-
-// We don't really need to support 256 character file names - this causes
-// all kinds of compiler warnings when the log filename can be up to 256
-// characters. _MAX_FNAME/2 is just an arbitrary value.
-char file_base_name[_MAX_FNAME / 2] = "log";
-char vcd_file_name[_MAX_FNAME];
-char json_file_name[_MAX_FNAME];
-char html_file_name[_MAX_FNAME];
-char input_file_name[_MAX_FNAME];
-
-//---------------------------------------------------------------------------
-//
-//	Signal Processing
-//
-//---------------------------------------------------------------------------
-void KillHandler(int)
-{
-    // Stop instruction
-    running = false;
-}
-
-void parse_arguments(int argc, char *argv[])
-{
-    int opt;
-
-    while ((opt = getopt(argc, argv, "-Hhb:i:")) != -1)
-    {
-        switch (opt)
-        {
-        // The three options below are kind of a compound option with two letters
-        case 'h':
-        case 'H':
-            print_help = true;
-            break;
-        case 'b':
-            buff_size = atoi(optarg);
-            break;
-        case 'i':
-            strncpy(input_file_name, optarg, sizeof(input_file_name)-1);
-            import_data = true;
-            break;
-        case 1:
-            strncpy(file_base_name, optarg, sizeof(file_base_name) - 5);
-            break;
-        default:
-            cout << "default: " << optarg << endl;
-            break;
-        }
-    }
-
-    /* Process any remaining command line arguments (not options). */
-    if (optind < argc) {
-        while (optind < argc)
-            strncpy(file_base_name, argv[optind++], sizeof(file_base_name)-1);
-    }
-
-    strcpy(vcd_file_name, file_base_name);
-    strcat(vcd_file_name, ".vcd");
-    strcpy(json_file_name, file_base_name);
-    strcat(json_file_name, ".json");
-    strcpy(html_file_name, file_base_name);
-    strcat(html_file_name, ".html");
-}
-//---------------------------------------------------------------------------
-//
-//	Copyright text
-//
-//---------------------------------------------------------------------------
-void print_copyright_text(int, char *[])
-{
-    LOGINFO("SCSI Monitor Capture Tool - part of RaSCSI(*^..^*) ")
-    LOGINFO("version %s (%s, %s)",
-            rascsi_get_version_string().c_str(),
-            __DATE__,
-            __TIME__)
-    LOGINFO("Powered by XM6 TypeG Technology ")
-    LOGINFO("Copyright (C) 2016-2020 GIMONS")
-    LOGINFO("Copyright (C) 2020-2022 Contributors to the RaSCSI project")
-    LOGINFO(" ")
-}
-
-//---------------------------------------------------------------------------
-//
-//	Help text
-//
-//---------------------------------------------------------------------------
-void print_help_text(int, char *argv[])
-{
-    LOGINFO("%s -i [input file json] -b [buffer size] [output file]", argv[0])
-    LOGINFO("       -i [input file json] - scsimon will parse the json file instead of capturing new data")
-    LOGINFO("                              If -i option is not specified, scsimon will read the gpio pins")
-    LOGINFO("       -b [buffer size]     - Override the default buffer size of %d.", buff_size)
-    LOGINFO("       [output file]        - Base name of the output files. The file extension (ex: .json)")
-    LOGINFO("                              will be appended to this file name")
-}
-
-//---------------------------------------------------------------------------
-//
-//	Banner Output
-//
-//---------------------------------------------------------------------------
-void Banner(int, char *[])
-{
-    if (import_data) {
-        LOGINFO("Reading input file: %s", input_file_name)
-    }
-    else {
-        LOGINFO("Reading live data from the GPIO pins")
-        LOGINFO("    Connection type : %s", bus->GetConnectDesc().c_str())
-    }
-    LOGINFO("    Data buffer size: %u", buff_size)
-    LOGINFO(" ")
-    LOGINFO("Generating output files:")
-    LOGINFO("   %s - Value Change Dump file that can be opened with GTKWave", vcd_file_name)
-    LOGINFO("   %s - JSON file with raw data", json_file_name)
-    LOGINFO("   %s - HTML file with summary of commands", html_file_name)
-}
-
-//---------------------------------------------------------------------------
-//
-//	Initialization
-//
-//---------------------------------------------------------------------------
-bool Init()
-{
-    // Interrupt handler settings
-    if (signal(SIGINT, KillHandler) == SIG_ERR) {
-        return false;
-    }
-    if (signal(SIGHUP, KillHandler) == SIG_ERR) {
-        return false;
-    }
-    if (signal(SIGTERM, KillHandler) == SIG_ERR) {
-        return false;
-    }
-
-    // GPIO Initialization
-    bus = GPIOBUS_Factory::Create();
-    if (!bus->Init())
-    {
-        LOGERROR("Unable to intiailize the GPIO bus. Exiting....")
-        return false;
-    }
-
-    // Bus Reset
-    bus->Reset();
-
-    // Other
-    running = false;
-
-    return true;
-}
-
-void Cleanup()
-{
-    if (!import_data) {
-        LOGINFO("Stopping data collection....")
-    }
-    LOGINFO(" ")
-    LOGINFO("Generating %s...", vcd_file_name)
-    scsimon_generate_value_change_dump(vcd_file_name, data_buffer, data_idx);
-    LOGINFO("Generating %s...", json_file_name)
-    scsimon_generate_json(json_file_name, data_buffer, data_idx);
-    LOGINFO("Generating %s...", html_file_name)
-    scsimon_generate_html(html_file_name, data_buffer, data_idx);
-
-    // Cleanup the Bus
-    bus->Cleanup();
-}
-
-void Reset()
-{
-    // Reset the bus
-    bus->Reset();
-}
-
-//---------------------------------------------------------------------------
-//
-//	Pin the thread to a specific CPU (Only applies to Linux)
-//
-//---------------------------------------------------------------------------
-#ifdef __linux__
-void FixCpu(int cpu)
-{
-    // Get the number of CPUs
-    cpu_set_t cpuset;
-    CPU_ZERO(&cpuset);
-    sched_getaffinity(0, sizeof(cpu_set_t), &cpuset);
-    int cpus = CPU_COUNT(&cpuset);
-
-    // Set the thread affinity
-    if (cpu < cpus)
-    {
-        CPU_ZERO(&cpuset);
-        CPU_SET(cpu, &cpuset);
-        sched_setaffinity(0, sizeof(cpu_set_t), &cpuset);
-    }
-}
-#endif
-
-#ifdef DEBUG
-static uint32_t high_bits = 0x0;
-static uint32_t low_bits = 0xFFFFFFFF;
-#endif
-
-//---------------------------------------------------------------------------
-//
-//	Main processing
-//
-//---------------------------------------------------------------------------
 int main(int argc, char *argv[])
 {
+	const vector<char *> args(argv, argv + argc);
 
-#ifdef DEBUG
-    spdlog::set_level(spdlog::level::trace);
-#else
-    spdlog::set_level(spdlog::level::info);
-#endif
-    spdlog::set_pattern("%^[%l]%$ %v");
-
-    print_copyright_text(argc, argv);
-    parse_arguments(argc, argv);
-
-#ifdef DEBUG
-    uint32_t prev_high = high_bits;
-    uint32_t prev_low = low_bits;
-#endif
-    ostringstream s;
-    uint32_t prev_sample = 0xFFFFFFFF;
-    uint32_t this_sample = 0;
-    timeval start_time;
-    timeval stop_time;
-    uint64_t loop_count = 0;
-    timeval time_diff;
-    uint64_t elapsed_us;
-
-    if (print_help)
-    {
-        print_help_text(argc, argv);
-        exit(0);
-    }
-
-    // Output the Banner
-    Banner(argc, argv);
-
-    data_buffer = (data_capture *)calloc(buff_size, sizeof(data_capture_t));
-
-    if (import_data)
-    {
-        data_idx = scsimon_read_json(input_file_name, data_buffer, buff_size);
-        if (data_idx > 0)
-        {
-            LOGDEBUG("Read %d samples from %s", data_idx, input_file_name)
-            Cleanup();
-        }
-        exit(0);
-    }
-
-    LOGINFO(" ")
-    LOGINFO("Now collecting data.... Press CTRL-C to stop.")
-    LOGINFO(" ")
-
-    // Initialize
-    int ret = 0;
-    if (!Init()) {
-        ret = EPERM;
-        goto init_exit;
-    }
-
-    // Reset
-    Reset();
-
-#ifdef __linux__
-    // Set the affinity to a specific processor core
-    FixCpu(3);
-
-    // Scheduling policy setting (highest priority)
-    struct sched_param schparam;
-    schparam.sched_priority = sched_get_priority_max(SCHED_FIFO);
-    sched_setscheduler(0, SCHED_FIFO, &schparam);
-#endif
-
-    // Start execution
-    running = true;
-    bus->SetACT(false);
-
-    (void)gettimeofday(&start_time, nullptr);
-
-    // LOGDEBUG("ALL_SCSI_PINS %08X\n", ALL_SCSI_PINS)
-
-    // Main Loop
-    while (running)
-    {
-        // Work initialization
-        this_sample = (bus->Acquire());// & ALL_SCSI_PINS);
-        loop_count++;
-        if (loop_count > LLONG_MAX - 1)
-        {
-            LOGINFO("Maximum amount of time has elapsed. SCSIMON is terminating.")
-            running = false;
-        }
-        if (data_idx >= (buff_size - 2))
-        {
-            LOGINFO("Internal data buffer is full. SCSIMON is terminating.")
-            running = false;
-        }
-
-        if (this_sample != prev_sample)
-        {
-
-#ifdef DEBUG
-            // This is intended to be a debug check to see if every pin is set
-            // high and low at some point.
-            high_bits |= this_sample;
-            low_bits &= this_sample;
-            if ((high_bits != prev_high) || (low_bits != prev_low))
-            {
-                LOGDEBUG("   %08X    %08X\n", high_bits, low_bits)
-            }
-            prev_high = high_bits;
-            prev_low = low_bits;
-            if ((data_idx % 1000) == 0)
-            {
-                s.str("");
-                s << "Collected " << data_idx << " samples...";
-                LOGDEBUG("%s", s.str().c_str())
-            }
-#endif
-            data_buffer[data_idx].data = this_sample;
-            data_buffer[data_idx].timestamp = loop_count;
-            data_idx++;
-            prev_sample = this_sample;
-        }
-
-        continue;
-    }
-
-    // Collect one last sample, otherwise it looks like the end of the data was cut off
-    if (data_idx < buff_size)
-    {
-        data_buffer[data_idx].data = this_sample;
-        data_buffer[data_idx].timestamp = loop_count;
-        data_idx++;
-    }
-
-    (void)gettimeofday(&stop_time, nullptr);
-
-    timersub(&stop_time, &start_time, &time_diff);
-
-    elapsed_us = ((time_diff.tv_sec * 1000000) + time_diff.tv_usec);
-    s.str("");
-    s << "Elapsed time: " << elapsed_us << " microseconds (" << elapsed_us / 1000000 << " seconds)";
-    LOGINFO("%s", s.str().c_str())
-    s.str("");
-    s << "Collected " << data_idx << " changes";
-    LOGINFO("%s", s.str().c_str())
-
-    // Note: ns_per_loop is a global variable that is used by Cleanup() to printout the timestamps.
-    ns_per_loop = (elapsed_us * 1000) / (double)loop_count;
-    s.str("");
-    s << "Read the SCSI bus " << loop_count << " times with an average of " << ns_per_loop << " ns for each read";
-    LOGINFO("%s", s.str().c_str())
-
-    Cleanup();
-
-init_exit:
-    exit(ret);
+	return ScsiMon().run(args);
 }
diff --git a/cpp/test/abstract_controller_test.cpp b/cpp/test/abstract_controller_test.cpp
index e7a67bae..e8a19995 100644
--- a/cpp/test/abstract_controller_test.cpp
+++ b/cpp/test/abstract_controller_test.cpp
@@ -7,7 +7,8 @@
 //
 //---------------------------------------------------------------------------
 
-#include "bus.h"
+#include "mocks.h"
+#include "rascsi_exceptions.h"
 #include "controllers/abstract_controller.h"
 #include "mocks.h"
 #include "rascsi_exceptions.h"
@@ -16,7 +17,9 @@ using namespace scsi_defs;
 
 TEST(AbstractControllerTest, AllocateCmd)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockAbstractController controller(controller_manager, 0);
 
 	EXPECT_EQ(16, controller.GetCmd().size());
 	controller.AllocateCmd(1234);
@@ -25,7 +28,9 @@ TEST(AbstractControllerTest, AllocateCmd)
 
 TEST(AbstractControllerTest, AllocateBuffer)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockAbstractController controller(controller_manager, 0);
 
 	controller.AllocateBuffer(1);
 	EXPECT_LE(1, controller.GetBuffer().size());
@@ -35,22 +40,48 @@ TEST(AbstractControllerTest, AllocateBuffer)
 
 TEST(AbstractControllerTest, Reset)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto device = make_shared<MockPrimaryDevice>(0);
 
-    controller.AddDevice(device);
+	controller->AddDevice(device);
 
-	controller.SetPhase(BUS::phase_t::status);
-	EXPECT_EQ(BUS::phase_t::status, controller.GetPhase());
-	controller.Reset();
-	EXPECT_TRUE(controller.IsBusFree());
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
-	EXPECT_EQ(0, controller.GetLength());
+	controller->SetPhase(BUS::phase_t::status);
+	EXPECT_EQ(BUS::phase_t::status, controller->GetPhase());
+	controller->Reset();
+	EXPECT_TRUE(controller->IsBusFree());
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
+	EXPECT_EQ(0, controller->GetLength());
+}
+
+TEST(AbstractControllerTest, Next)
+{
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockAbstractController controller(controller_manager, 0);
+
+	controller.SetNext(0x1234);
+	EXPECT_EQ(0x1234, controller.GetNext());
+	controller.IncrementNext();
+	EXPECT_EQ(0x1235, controller.GetNext());
+}
+
+TEST(AbstractControllerTest, Message)
+{
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockAbstractController controller(controller_manager, 0);
+
+	controller.SetMessage(0x12);
+	EXPECT_EQ(0x12, controller.GetMessage());
 }
 
 TEST(AbstractControllerTest, ByteTransfer)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockAbstractController controller(controller_manager, 0);
 
 	controller.SetByteTransfer(false);
 	EXPECT_FALSE(controller.IsByteTransfer());
@@ -58,16 +89,32 @@ TEST(AbstractControllerTest, ByteTransfer)
 	EXPECT_TRUE(controller.IsByteTransfer());
 }
 
+TEST(AbstractControllerTest, BytesToTransfer)
+{
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockAbstractController controller(controller_manager, 0);
+
+	controller.SetBytesToTransfer(0x1234);
+	EXPECT_EQ(0x1234, controller.GetBytesToTransfer());
+	controller.SetByteTransfer(false);
+	EXPECT_EQ(0, controller.GetBytesToTransfer());
+}
+
 TEST(AbstractControllerTest, GetMaxLuns)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockAbstractController controller(controller_manager, 0);
 
 	EXPECT_EQ(32, controller.GetMaxLuns());
 }
 
 TEST(AbstractControllerTest, Status)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockAbstractController controller(controller_manager, 0);
 
 	controller.SetStatus(status::RESERVATION_CONFLICT);
 	EXPECT_EQ(status::RESERVATION_CONFLICT, controller.GetStatus());
@@ -75,38 +122,40 @@ TEST(AbstractControllerTest, Status)
 
 TEST(AbstractControllerTest, ProcessPhase)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockAbstractController controller(controller_manager, 0);
 
 	controller.SetPhase(BUS::phase_t::selection);
-	EXPECT_CALL(controller, Selection());
+	EXPECT_CALL(controller, Selection);
 	controller.ProcessPhase();
 
 	controller.SetPhase(BUS::phase_t::busfree);
-	EXPECT_CALL(controller, BusFree());
+	EXPECT_CALL(controller, BusFree);
 	controller.ProcessPhase();
 
 	controller.SetPhase(BUS::phase_t::datain);
-	EXPECT_CALL(controller, DataIn());
+	EXPECT_CALL(controller, DataIn);
 	controller.ProcessPhase();
 
 	controller.SetPhase(BUS::phase_t::dataout);
-	EXPECT_CALL(controller, DataOut());
+	EXPECT_CALL(controller, DataOut);
 	controller.ProcessPhase();
 
 	controller.SetPhase(BUS::phase_t::command);
-	EXPECT_CALL(controller, Command());
+	EXPECT_CALL(controller, Command);
 	controller.ProcessPhase();
 
 	controller.SetPhase(BUS::phase_t::status);
-	EXPECT_CALL(controller, Status());
+	EXPECT_CALL(controller, Status);
 	controller.ProcessPhase();
 
 	controller.SetPhase(BUS::phase_t::msgin);
-	EXPECT_CALL(controller, MsgIn());
+	EXPECT_CALL(controller, MsgIn);
 	controller.ProcessPhase();
 
 	controller.SetPhase(BUS::phase_t::msgout);
-	EXPECT_CALL(controller, MsgOut());
+	EXPECT_CALL(controller, MsgOut);
 	controller.ProcessPhase();
 
 	controller.SetPhase(BUS::phase_t::reselection);
@@ -125,25 +174,27 @@ TEST(AbstractControllerTest, DeviceLunLifeCycle)
     const int ID  = 1;
     const int LUN = 4;
 
-	MockAbstractController controller(make_shared<MockBus>(), ID);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<NiceMock<MockAbstractController>>(controller_manager, ID);
 
 	auto device1 = make_shared<MockPrimaryDevice>(LUN);
 	auto device2 = make_shared<MockPrimaryDevice>(32);
 	auto device3 = make_shared<MockPrimaryDevice>(-1);
 
-	EXPECT_EQ(0, controller.GetLunCount());
-	EXPECT_EQ(ID, controller.GetTargetId());
-	EXPECT_TRUE(controller.AddDevice(device1));
-	EXPECT_FALSE(controller.AddDevice(device2));
-	EXPECT_FALSE(controller.AddDevice(device3));
-	EXPECT_TRUE(controller.GetLunCount() > 0);
-	EXPECT_TRUE(controller.HasDeviceForLun(LUN));
-	EXPECT_FALSE(controller.HasDeviceForLun(0));
-	EXPECT_NE(nullptr, controller.GetDeviceForLun(LUN));
-	EXPECT_EQ(nullptr, controller.GetDeviceForLun(0));
-	EXPECT_TRUE(controller.RemoveDevice(device1));
-	EXPECT_EQ(0, controller.GetLunCount());
-	EXPECT_FALSE(controller.RemoveDevice(device1));
+	EXPECT_EQ(0, controller->GetLunCount());
+	EXPECT_EQ(ID, controller->GetTargetId());
+	EXPECT_TRUE(controller->AddDevice(device1));
+	EXPECT_FALSE(controller->AddDevice(device2));
+	EXPECT_FALSE(controller->AddDevice(device3));
+	EXPECT_TRUE(controller->GetLunCount() > 0);
+	EXPECT_TRUE(controller->HasDeviceForLun(LUN));
+	EXPECT_FALSE(controller->HasDeviceForLun(0));
+	EXPECT_NE(nullptr, controller->GetDeviceForLun(LUN));
+	EXPECT_EQ(nullptr, controller->GetDeviceForLun(0));
+	EXPECT_TRUE(controller->RemoveDevice(device1));
+	EXPECT_EQ(0, controller->GetLunCount());
+	EXPECT_FALSE(controller->RemoveDevice(device1));
 }
 
 TEST(AbstractControllerTest, ExtractInitiatorId)
@@ -152,7 +203,9 @@ TEST(AbstractControllerTest, ExtractInitiatorId)
     const int INITIATOR_ID         = 7;
     const int UNKNOWN_INITIATOR_ID = -1;
 
-	MockAbstractController controller(make_shared<MockBus>(), ID);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockAbstractController controller(controller_manager, ID);
 
     EXPECT_EQ(INITIATOR_ID, controller.ExtractInitiatorId((1 << INITIATOR_ID) | (1 << ID)));
     EXPECT_EQ(UNKNOWN_INITIATOR_ID, controller.ExtractInitiatorId(1 << ID));
@@ -160,9 +213,11 @@ TEST(AbstractControllerTest, ExtractInitiatorId)
 
 TEST(AbstractControllerTest, GetOpcode)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockAbstractController controller(controller_manager, 0);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller.GetCmd();
 
 	cmd[0] = static_cast<int>(scsi_command::eCmdInquiry);
 	EXPECT_EQ(scsi_command::eCmdInquiry, controller.GetOpcode());
@@ -172,9 +227,11 @@ TEST(AbstractControllerTest, GetLun)
 {
     const int LUN = 3;
 
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockAbstractController controller(controller_manager, 0);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller.GetCmd();
 
     cmd[1] = LUN << 5;
     EXPECT_EQ(LUN, controller.GetLun());
@@ -182,7 +239,9 @@ TEST(AbstractControllerTest, GetLun)
 
 TEST(AbstractControllerTest, Blocks)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockAbstractController controller(controller_manager, 0);
 
 	controller.SetBlocks(1);
 	EXPECT_EQ(1, controller.GetBlocks());
@@ -192,7 +251,9 @@ TEST(AbstractControllerTest, Blocks)
 
 TEST(AbstractControllerTest, Length)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockAbstractController controller(controller_manager, 0);
 
 	EXPECT_FALSE(controller.HasValidLength());
 
@@ -203,7 +264,9 @@ TEST(AbstractControllerTest, Length)
 
 TEST(AbstractControllerTest, UpdateOffsetAndLength)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockAbstractController controller(controller_manager, 0);
 
     EXPECT_FALSE(controller.HasValidLength());
 
@@ -213,7 +276,9 @@ TEST(AbstractControllerTest, UpdateOffsetAndLength)
 
 TEST(AbstractControllerTest, Offset)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockAbstractController controller(controller_manager, 0);
 
     controller.ResetOffset();
     EXPECT_EQ(0, controller.GetOffset());
diff --git a/cpp/test/bus_test.cpp b/cpp/test/bus_test.cpp
index 02df56e7..c2cf5a18 100644
--- a/cpp/test/bus_test.cpp
+++ b/cpp/test/bus_test.cpp
@@ -8,7 +8,38 @@
 //---------------------------------------------------------------------------
 
 #include "mocks.h"
-#include "bus.h"
+#include "hal/bus.h"
+
+TEST(BusTest, GetCommandByteCount)
+{
+	unordered_set<int> opcodes;
+
+	EXPECT_EQ(16, BUS::GetCommandByteCount(0x88));
+	opcodes.insert(0x88);
+	EXPECT_EQ(16, BUS::GetCommandByteCount(0x8a));
+	opcodes.insert(0x8a);
+	EXPECT_EQ(16, BUS::GetCommandByteCount(0x8f));
+	opcodes.insert(0x8f);
+	EXPECT_EQ(16, BUS::GetCommandByteCount(0x91));
+	opcodes.insert(0x91);
+	EXPECT_EQ(16, BUS::GetCommandByteCount(0x9e));
+	opcodes.insert(0x9e);
+	EXPECT_EQ(16, BUS::GetCommandByteCount(0x9f));
+	opcodes.insert(0x9f);
+	EXPECT_EQ(12, BUS::GetCommandByteCount(0xa0));
+	opcodes.insert(0xa0);
+
+	for (int i = 0x20; i <= 0x7d; i++) {
+		EXPECT_EQ(10, BUS::GetCommandByteCount(i));
+		opcodes.insert(i);
+	}
+
+	for (int i = 0; i < 256; i++) {
+		if (opcodes.find(i) == opcodes.end()) {
+			EXPECT_EQ(6, BUS::GetCommandByteCount(i));
+		}
+	}
+}
 
 TEST(BusTest, GetPhase)
 {
diff --git a/cpp/test/controller_manager_test.cpp b/cpp/test/controller_manager_test.cpp
index fd0fa564..996b2f0a 100644
--- a/cpp/test/controller_manager_test.cpp
+++ b/cpp/test/controller_manager_test.cpp
@@ -17,33 +17,33 @@ TEST(ControllerManagerTest, LifeCycle)
 	const int LUN1 = 0;
 	const int LUN2 = 3;
 
-	auto bus_ptr = make_shared<MockBus>();
-	ControllerManager controller_manager(bus_ptr);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
 	DeviceFactory device_factory;
 
-	auto device = device_factory.CreateDevice(controller_manager, SCHS, -1, "");
-	EXPECT_FALSE(controller_manager.AttachToScsiController(ID, device));
+	auto device = device_factory.CreateDevice(SCHS, -1, "");
+	EXPECT_FALSE(controller_manager->AttachToScsiController(ID, device));
 
-	device = device_factory.CreateDevice(controller_manager, SCHS, LUN1, "");
-	EXPECT_TRUE(controller_manager.AttachToScsiController(ID, device));
-	auto controller = controller_manager.FindController(ID);
+	device = device_factory.CreateDevice(SCHS, LUN1, "");
+	EXPECT_TRUE(controller_manager->AttachToScsiController(ID, device));
+	auto controller = controller_manager->FindController(ID);
 	EXPECT_NE(nullptr, controller);
 	EXPECT_EQ(1, controller->GetLunCount());
-	EXPECT_NE(nullptr, controller_manager.IdentifyController(1 << ID));
-	EXPECT_EQ(nullptr, controller_manager.IdentifyController(0));
-	EXPECT_EQ(nullptr, controller_manager.FindController(0));
-	EXPECT_NE(nullptr, controller_manager.GetDeviceByIdAndLun(ID, LUN1));
-	EXPECT_EQ(nullptr, controller_manager.GetDeviceByIdAndLun(0, 0));
+	EXPECT_NE(nullptr, controller_manager->IdentifyController(1 << ID));
+	EXPECT_EQ(nullptr, controller_manager->IdentifyController(0));
+	EXPECT_EQ(nullptr, controller_manager->FindController(0));
+	EXPECT_NE(nullptr, controller_manager->GetDeviceByIdAndLun(ID, LUN1));
+	EXPECT_EQ(nullptr, controller_manager->GetDeviceByIdAndLun(0, 0));
 
-	device = device_factory.CreateDevice(controller_manager, SCHS, LUN2, "");
-	EXPECT_TRUE(controller_manager.AttachToScsiController(ID, device));
-	controller = controller_manager.FindController(ID);
-	EXPECT_TRUE(controller_manager.DeleteController(controller));
-	EXPECT_EQ(nullptr, controller_manager.FindController(ID));
+	device = device_factory.CreateDevice(SCHS, LUN2, "");
+	EXPECT_TRUE(controller_manager->AttachToScsiController(ID, device));
+	controller = controller_manager->FindController(ID);
+	EXPECT_TRUE(controller_manager->DeleteController(controller));
+	EXPECT_EQ(nullptr, controller_manager->FindController(ID));
 
-	controller_manager.DeleteAllControllers();
-	EXPECT_EQ(nullptr, controller_manager.FindController(ID));
-	EXPECT_EQ(nullptr, controller_manager.GetDeviceByIdAndLun(ID, LUN1));
+	controller_manager->DeleteAllControllers();
+	EXPECT_EQ(nullptr, controller_manager->FindController(ID));
+	EXPECT_EQ(nullptr, controller_manager->GetDeviceByIdAndLun(ID, LUN1));
 }
 
 TEST(ControllerManagerTest, AttachToScsiController)
@@ -52,33 +52,15 @@ TEST(ControllerManagerTest, AttachToScsiController)
 	const int LUN1 = 3;
 	const int LUN2 = 0;
 
-	auto bus_ptr = make_shared<MockBus>();
-	ControllerManager controller_manager(bus_ptr);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
 	DeviceFactory device_factory;
 
-	auto device1 = device_factory.CreateDevice(controller_manager, SCHS, LUN1, "");
-	EXPECT_FALSE(controller_manager.AttachToScsiController(ID, device1)) << "LUN 0 is missing";
+	auto device1 = device_factory.CreateDevice(SCHS, LUN1, "");
+	EXPECT_FALSE(controller_manager->AttachToScsiController(ID, device1)) << "LUN 0 is missing";
 
-	auto device2 = device_factory.CreateDevice(controller_manager, SCLP, LUN2, "");
-	EXPECT_TRUE(controller_manager.AttachToScsiController(ID, device2));
-	EXPECT_TRUE(controller_manager.AttachToScsiController(ID, device1));
-	EXPECT_FALSE(controller_manager.AttachToScsiController(ID, device1));
-}
-
-TEST(ControllerManagerTest, ResetAllControllers)
-{
-	const int ID = 2;
-
-	auto bus_ptr = make_shared<MockBus>();
-	ControllerManager controller_manager(bus_ptr);
-	DeviceFactory device_factory;
-
-	auto device = device_factory.CreateDevice(controller_manager, SCHS, 0, "");
-	EXPECT_TRUE(controller_manager.AttachToScsiController(ID, device));
-	auto controller = controller_manager.FindController(ID);
-	EXPECT_NE(nullptr, controller);
-
-	controller->SetStatus(status::RESERVATION_CONFLICT);
-	controller_manager.ResetAllControllers();
-	EXPECT_EQ(status::GOOD, controller->GetStatus());
+	auto device2 = device_factory.CreateDevice(SCLP, LUN2, "");
+	EXPECT_TRUE(controller_manager->AttachToScsiController(ID, device2));
+	EXPECT_TRUE(controller_manager->AttachToScsiController(ID, device1));
+	EXPECT_FALSE(controller_manager->AttachToScsiController(ID, device1));
 }
diff --git a/cpp/test/device_factory_test.cpp b/cpp/test/device_factory_test.cpp
index c976d2fc..c63bfba4 100644
--- a/cpp/test/device_factory_test.cpp
+++ b/cpp/test/device_factory_test.cpp
@@ -122,27 +122,27 @@ TEST(DeviceFactoryTest, GetDefaultParams)
 
 TEST(DeviceFactoryTest, UnknownDeviceType)
 {
-	auto bus_ptr = make_shared<MockBus>();
 	DeviceFactory device_factory;
-	ControllerManager controller_manager(bus_ptr);
 
-	auto device1 = device_factory.CreateDevice(controller_manager, UNDEFINED, 0, "test");
+	auto device1 = device_factory.CreateDevice(UNDEFINED, 0, "test");
 	EXPECT_EQ(nullptr, device1);
 
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
-	auto device2 = device_factory.CreateDevice(controller_manager, SAHD, 0, "test");
+	auto device2 = device_factory.CreateDevice(SAHD, 0, "test");
 #pragma GCC diagnostic pop
 	EXPECT_EQ(nullptr, device2);
 }
 
 TEST(DeviceFactoryTest, SCHD_Device_Defaults)
 {
-	auto bus_ptr = make_shared<MockBus>();
 	DeviceFactory device_factory;
-	ControllerManager controller_manager(bus_ptr);
 
-	auto device = device_factory.CreateDevice(controller_manager, UNDEFINED, 0, "test.hda");
+	auto device = device_factory.CreateDevice(UNDEFINED, 0, "test.hda");
+
+	const unordered_map<string, string> params;
+	device->Init(params);
+
 	EXPECT_NE(nullptr, device);
 	EXPECT_EQ(SCHD, device->GetType());
 	EXPECT_TRUE(device->SupportsFile());
@@ -162,26 +162,26 @@ TEST(DeviceFactoryTest, SCHD_Device_Defaults)
 	EXPECT_EQ(string(rascsi_get_version_string()).substr(0, 2) + string(rascsi_get_version_string()).substr(3, 2),
 			device->GetRevision());
 
-	device = device_factory.CreateDevice(controller_manager, UNDEFINED, 0, "test.hds");
+	device = device_factory.CreateDevice(UNDEFINED, 0, "test.hds");
 	EXPECT_NE(nullptr, device);
 	EXPECT_EQ(SCHD, device->GetType());
 
-	device = device_factory.CreateDevice(controller_manager, UNDEFINED, 0, "test.hdi");
+	device = device_factory.CreateDevice(UNDEFINED, 0, "test.hdi");
 	EXPECT_NE(nullptr, device);
 	EXPECT_EQ(SCHD, device->GetType());
 
-	device = device_factory.CreateDevice(controller_manager, UNDEFINED, 0, "test.nhd");
+	device = device_factory.CreateDevice(UNDEFINED, 0, "test.nhd");
 	EXPECT_NE(nullptr, device);
 	EXPECT_EQ(SCHD, device->GetType());
 }
 
 void TestRemovableDrive(PbDeviceType type, const string& filename, const string& product)
 {
-	auto bus_ptr = make_shared<MockBus>();
 	DeviceFactory device_factory;
-	ControllerManager controller_manager(bus_ptr);
+	auto device = device_factory.CreateDevice(UNDEFINED, 0, filename);
+	const unordered_map<string, string> params;
+	device->Init(params);
 
-	auto device = device_factory.CreateDevice(controller_manager, UNDEFINED, 0, filename);
 	EXPECT_NE(nullptr, device);
 	EXPECT_EQ(type, device->GetType());
 	EXPECT_TRUE(device->SupportsFile());
@@ -215,11 +215,13 @@ TEST(DeviceFactoryTest, SCMO_Device_Defaults)
 
 TEST(DeviceFactoryTest, SCCD_Device_Defaults)
 {
-	auto bus_ptr = make_shared<MockBus>();
 	DeviceFactory device_factory;
-	ControllerManager controller_manager(bus_ptr);
 
-	auto device = device_factory.CreateDevice(controller_manager, UNDEFINED, 0, "test.iso");
+	auto device = device_factory.CreateDevice(UNDEFINED, 0, "test.iso");
+
+	const unordered_map<string, string> params;
+	device->Init(params);
+
 	EXPECT_NE(nullptr, device);
 	EXPECT_EQ(SCCD, device->GetType());
 	EXPECT_TRUE(device->SupportsFile());
@@ -242,11 +244,13 @@ TEST(DeviceFactoryTest, SCCD_Device_Defaults)
 
 TEST(DeviceFactoryTest, SCBR_Device_Defaults)
 {
-	auto bus_ptr = make_shared<MockBus>();
 	DeviceFactory device_factory;
-	ControllerManager controller_manager(bus_ptr);
 
-	auto device = device_factory.CreateDevice(controller_manager, UNDEFINED, 0, "bridge");
+	auto device = device_factory.CreateDevice(UNDEFINED, 0, "bridge");
+
+	const unordered_map<string, string> params;
+	device->Init(params);
+
 	EXPECT_NE(nullptr, device);
 	EXPECT_EQ(SCBR, device->GetType());
 	EXPECT_FALSE(device->SupportsFile());
@@ -269,11 +273,13 @@ TEST(DeviceFactoryTest, SCBR_Device_Defaults)
 
 TEST(DeviceFactoryTest, SCDP_Device_Defaults)
 {
-	auto bus_ptr = make_shared<MockBus>();
 	DeviceFactory device_factory;
-	ControllerManager controller_manager(bus_ptr);
 
-	auto device = device_factory.CreateDevice(controller_manager, UNDEFINED, 0, "daynaport");
+	auto device = device_factory.CreateDevice(UNDEFINED, 0, "daynaport");
+
+	const unordered_map<string, string> params;
+	device->Init(params);
+
 	EXPECT_NE(nullptr, device);
 	EXPECT_EQ(SCDP, device->GetType());
 	EXPECT_FALSE(device->SupportsFile());
@@ -295,11 +301,13 @@ TEST(DeviceFactoryTest, SCDP_Device_Defaults)
 
 TEST(DeviceFactoryTest, SCHS_Device_Defaults)
 {
-	auto bus_ptr = make_shared<MockBus>();
 	DeviceFactory device_factory;
-	ControllerManager controller_manager(bus_ptr);
 
-	auto device = device_factory.CreateDevice(controller_manager, UNDEFINED, 0, "services");
+	auto device = device_factory.CreateDevice(UNDEFINED, 0, "services");
+
+	const unordered_map<string, string> params;
+	device->Init(params);
+
 	EXPECT_NE(nullptr, device);
 	EXPECT_EQ(SCHS, device->GetType());
 	EXPECT_FALSE(device->SupportsFile());
@@ -322,11 +330,13 @@ TEST(DeviceFactoryTest, SCHS_Device_Defaults)
 
 TEST(DeviceFactoryTest, SCLP_Device_Defaults)
 {
-	auto bus_ptr = make_shared<MockBus>();
 	DeviceFactory device_factory;
-	ControllerManager controller_manager(bus_ptr);
 
-	auto device = device_factory.CreateDevice(controller_manager, UNDEFINED, 0, "printer");
+	auto device = device_factory.CreateDevice(UNDEFINED, 0, "printer");
+
+	const unordered_map<string, string> params;
+	device->Init(params);
+
 	EXPECT_NE(nullptr, device);
 	EXPECT_EQ(SCLP, device->GetType());
 	EXPECT_FALSE(device->SupportsFile());
diff --git a/cpp/test/disk_test.cpp b/cpp/test/disk_test.cpp
index cb5d28a2..405071bb 100644
--- a/cpp/test/disk_test.cpp
+++ b/cpp/test/disk_test.cpp
@@ -17,21 +17,38 @@ using namespace scsi_command_util;
 
 TEST(DiskTest, Dispatch)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
-	const unordered_set<uint32_t> sector_sizes;
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-    controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
-	EXPECT_FALSE(disk->Dispatch(scsi_command::eCmdIcd)) << "Command is not supported by this class";
+	disk->SetRemovable(true);
+	disk->SetMediumChanged(false);
+	disk->SetReady(true);
+	EXPECT_CALL(*controller, Status);
+	disk->Dispatch(scsi_command::eCmdTestUnitReady);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
+
+	disk->SetMediumChanged(true);
+	EXPECT_CALL(*controller, Error);
+	disk->Dispatch(scsi_command::eCmdTestUnitReady);
+	EXPECT_FALSE(disk->IsMediumChanged());
 }
 
 TEST(DiskTest, Rezero)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-    controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
 	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdRezero); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::NOT_READY),
@@ -40,64 +57,77 @@ TEST(DiskTest, Rezero)
 
     disk->SetReady(true);
 
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdRezero));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, Status);
+	disk->Dispatch(scsi_command::eCmdRezero);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 }
 
 TEST(DiskTest, FormatUnit)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-	controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 
-	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdFormat); }, Throws<scsi_exception>(AllOf(
+	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdFormatUnit); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::NOT_READY),
 			Property(&scsi_exception::get_asc, asc::MEDIUM_NOT_PRESENT))))
 		<< "FORMAT UNIT must fail because drive is not ready";
 
 	disk->SetReady(true);
 
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdFormat));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, Status);
+	disk->Dispatch(scsi_command::eCmdFormatUnit);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 
 	cmd[1] = 0x10;
 	cmd[4] = 1;
-	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdFormat); }, Throws<scsi_exception>(AllOf(
+	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdFormatUnit); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::ILLEGAL_REQUEST),
 			Property(&scsi_exception::get_asc, asc::INVALID_FIELD_IN_CDB))));
 }
 
 TEST(DiskTest, ReassignBlocks)
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+{
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-    controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
-	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdReassign); }, Throws<scsi_exception>(AllOf(
+	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdReassignBlocks); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::NOT_READY),
 			Property(&scsi_exception::get_asc, asc::MEDIUM_NOT_PRESENT))))
 		<< "REASSIGN must fail because drive is not ready";
 
     disk->SetReady(true);
 
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdReassign));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, Status);
+	disk->Dispatch(scsi_command::eCmdReassignBlocks);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 }
 
 TEST(DiskTest, Seek6)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-    controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 
 	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdSeek6); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::ILLEGAL_REQUEST),
@@ -114,19 +144,23 @@ TEST(DiskTest, Seek6)
 
 	disk->SetReady(true);
 
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdSeek6));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, Status);
+	disk->Dispatch(scsi_command::eCmdSeek6);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 }
 
 TEST(DiskTest, Seek10)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-	controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 
 	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdSeek10); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::ILLEGAL_REQUEST),
@@ -143,17 +177,21 @@ TEST(DiskTest, Seek10)
 
     disk->SetReady(true);
 
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdSeek10));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, Status);
+	disk->Dispatch(scsi_command::eCmdSeek10);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 }
 
 TEST(DiskTest, ReadCapacity10)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-	controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
 	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdReadCapacity10); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::NOT_READY),
@@ -167,28 +205,32 @@ TEST(DiskTest, ReadCapacity10)
 		<< "READ CAPACITY(10) must fail because the medium has no capacity";
 
 	disk->SetBlockCount(0x12345678);
-	EXPECT_CALL(controller, DataIn());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdReadCapacity10));
-	auto& buf = controller.GetBuffer();
+	EXPECT_CALL(*controller, DataIn);
+	disk->Dispatch(scsi_command::eCmdReadCapacity10);
+	auto& buf = controller->GetBuffer();
 	EXPECT_EQ(0x1234, GetInt16(buf, 0));
 	EXPECT_EQ(0x5677, GetInt16(buf, 2));
 
 	disk->SetBlockCount(0x1234567887654321);
-	EXPECT_CALL(controller, DataIn());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdReadCapacity10));
-	buf = controller.GetBuffer();
+	EXPECT_CALL(*controller, DataIn);
+	disk->Dispatch(scsi_command::eCmdReadCapacity10);
+	buf = controller->GetBuffer();
 	EXPECT_EQ(0xffff, GetInt16(buf, 0));
 	EXPECT_EQ(0xffff, GetInt16(buf, 2));
 }
 
 TEST(DiskTest, ReadCapacity16)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-    controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 
 	cmd[1] = 0x00;
 	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdReadCapacity16_ReadLong16); }, Throws<scsi_exception>(AllOf(
@@ -211,9 +253,9 @@ TEST(DiskTest, ReadCapacity16)
 
 	disk->SetBlockCount(0x1234567887654321);
 	disk->SetSectorSizeInBytes(1024);
-	EXPECT_CALL(controller, DataIn());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdReadCapacity16_ReadLong16));
-	const auto& buf = controller.GetBuffer();
+	EXPECT_CALL(*controller, DataIn);
+	disk->Dispatch(scsi_command::eCmdReadCapacity16_ReadLong16);
+	const auto& buf = controller->GetBuffer();
 	EXPECT_EQ(0x1234, GetInt16(buf, 0));
 	EXPECT_EQ(0x5678, GetInt16(buf, 2));
 	EXPECT_EQ(0x8765, GetInt16(buf, 4));
@@ -224,10 +266,14 @@ TEST(DiskTest, ReadCapacity16)
 
 TEST(DiskTest, Read6)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-    controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
 	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdRead6); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::ILLEGAL_REQUEST),
@@ -239,10 +285,14 @@ TEST(DiskTest, Read6)
 
 TEST(DiskTest, Read10)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-	controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
 	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdRead10); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::ILLEGAL_REQUEST),
@@ -250,19 +300,23 @@ TEST(DiskTest, Read10)
 		<< "READ(10) must fail for a medium with 0 blocks";
 
 	disk->SetBlockCount(1);
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdRead10));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, Status);
+	disk->Dispatch(scsi_command::eCmdRead10);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 
 	// Further testing requires filesystem access
 }
 
 TEST(DiskTest, Read16)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-	controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
 	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdRead16); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::ILLEGAL_REQUEST),
@@ -270,34 +324,50 @@ TEST(DiskTest, Read16)
 		<< "READ(16) must fail for a medium with 0 blocks";
 
 	disk->SetBlockCount(1);
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdRead16));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, Status);
+	disk->Dispatch(scsi_command::eCmdRead16);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 
 	// Further testing requires filesystem access
 }
 
 TEST(DiskTest, Write6)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-	controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
 	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdWrite6); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::ILLEGAL_REQUEST),
 			Property(&scsi_exception::get_asc, asc::LBA_OUT_OF_RANGE))))
-		<< "WRIte(6) must fail for a medium with 0 blocks";
+		<< "WRITE(6) must fail for a medium with 0 blocks";
+
+	disk->SetBlockCount(1);
+	disk->SetReady(true);
+	disk->SetProtectable(true);
+	disk->SetProtected(true);
+	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdWrite6); }, Throws<scsi_exception>(AllOf(
+			Property(&scsi_exception::get_sense_key, sense_key::DATA_PROTECT),
+			Property(&scsi_exception::get_asc, asc::WRITE_PROTECTED))));
 
 	// Further testing requires filesystem access
 }
 
 TEST(DiskTest, Write10)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-	controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
 	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdWrite10); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::ILLEGAL_REQUEST),
@@ -305,19 +375,23 @@ TEST(DiskTest, Write10)
 		<< "WRITE(10) must fail for a medium with 0 blocks";
 
 	disk->SetBlockCount(1);
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdWrite10));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, Status);
+	disk->Dispatch(scsi_command::eCmdWrite10);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 
 	// Further testing requires filesystem access
 }
 
 TEST(DiskTest, Write16)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-	controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
 	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdWrite16); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::ILLEGAL_REQUEST),
@@ -325,19 +399,23 @@ TEST(DiskTest, Write16)
 		<< "WRITE(16) must fail for a medium with 0 blocks";
 
 	disk->SetBlockCount(1);
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdWrite16));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, Status);
+	disk->Dispatch(scsi_command::eCmdWrite16);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 
 	// Further testing requires filesystem access
 }
 
 TEST(DiskTest, Verify10)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-	controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
 	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdVerify10); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::ILLEGAL_REQUEST),
@@ -345,19 +423,23 @@ TEST(DiskTest, Verify10)
 		<< "VERIFY(10) must fail for a medium with 0 blocks";
 
 	disk->SetBlockCount(1);
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdVerify10));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, Status);
+	disk->Dispatch(scsi_command::eCmdVerify10);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 
 	// Further testing requires filesystem access
 }
 
 TEST(DiskTest, Verify16)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-	controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
 	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdVerify16); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::ILLEGAL_REQUEST),
@@ -365,25 +447,29 @@ TEST(DiskTest, Verify16)
 		<< "VERIFY(16) must fail for a medium with 0 blocks";
 
 	disk->SetBlockCount(1);
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdVerify16));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, Status);
+	disk->Dispatch(scsi_command::eCmdVerify16);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 
 	// Further testing requires filesystem access
 }
 
 TEST(DiskTest, ReadLong10)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-	controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdReadLong10));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, Status);
+	disk->Dispatch(scsi_command::eCmdReadLong10);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 
 	cmd[2] = 1;
 	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdReadLong10); }, Throws<scsi_exception>(AllOf(
@@ -401,12 +487,16 @@ TEST(DiskTest, ReadLong10)
 
 TEST(DiskTest, ReadLong16)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-	controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 
 	// READ LONG(16), not READ CAPACITY(16)
 	cmd[1] = 0x11;
@@ -417,9 +507,9 @@ TEST(DiskTest, ReadLong16)
 		<< "READ LONG(16) must fail because the capacity is exceeded";
 	cmd[2] = 0;
 
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdReadCapacity16_ReadLong16));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, Status);
+	disk->Dispatch(scsi_command::eCmdReadCapacity16_ReadLong16);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 
 	cmd[13] = 1;
 	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdReadCapacity16_ReadLong16); }, Throws<scsi_exception>(AllOf(
@@ -430,16 +520,20 @@ TEST(DiskTest, ReadLong16)
 
 TEST(DiskTest, WriteLong10)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-	controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdWriteLong10));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, Status);
+	disk->Dispatch(scsi_command::eCmdWriteLong10);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 
 	cmd[2] = 1;
 	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdWriteLong10); }, Throws<scsi_exception>(AllOf(
@@ -457,12 +551,16 @@ TEST(DiskTest, WriteLong10)
 
 TEST(DiskTest, WriteLong16)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-	controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 
 	cmd[2] = 1;
 	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdWriteLong16); }, Throws<scsi_exception>(AllOf(
@@ -471,9 +569,9 @@ TEST(DiskTest, WriteLong16)
 		<< "WRITE LONG(16) must fail because the capacity is exceeded";
 	cmd[2] = 0;
 
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdWriteLong16));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, Status);
+	disk->Dispatch(scsi_command::eCmdWriteLong16);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 
 	cmd[13] = 1;
 	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdWriteLong16); }, Throws<scsi_exception>(AllOf(
@@ -484,30 +582,35 @@ TEST(DiskTest, WriteLong16)
 
 TEST(DiskTest, StartStopUnit)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
+
 	disk->SetRemovable(true);
 
-    controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 
 	// Stop/Unload
 	disk->SetReady(true);
-	EXPECT_CALL(controller, Status());
+	EXPECT_CALL(*controller, Status);
 	EXPECT_CALL(*disk, FlushCache);
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdStartStop));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	disk->Dispatch(scsi_command::eCmdStartStop);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 	EXPECT_TRUE(disk->IsStopped());
 
 	// Stop/Load
 	cmd[4] = 0x02;
 	disk->SetReady(true);
 	disk->SetLocked(false);
-	EXPECT_CALL(controller, Status());
+	EXPECT_CALL(*controller, Status);
 	EXPECT_CALL(*disk, FlushCache);
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdStartStop));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	disk->Dispatch(scsi_command::eCmdStartStop);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 
 	disk->SetReady(false);
 	EXPECT_CALL(*disk, FlushCache).Times(0);
@@ -524,43 +627,47 @@ TEST(DiskTest, StartStopUnit)
 
 	// Start/Unload
 	cmd[4] = 0x01;
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdStartStop));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, Status);
+	disk->Dispatch(scsi_command::eCmdStartStop);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 	EXPECT_FALSE(disk->IsStopped());
 
 	// Start/Load
 	cmd[4] = 0x03;
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdStartStop));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, Status);
+	disk->Dispatch(scsi_command::eCmdStartStop);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 }
 
 TEST(DiskTest, PreventAllowMediumRemoval)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-    controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 
-	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdRemoval); }, Throws<scsi_exception>(AllOf(
+	EXPECT_THAT([&] { disk->Dispatch(scsi_command::eCmdPreventAllowMediumRemoval); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::NOT_READY),
 			Property(&scsi_exception::get_asc, asc::MEDIUM_NOT_PRESENT))))
 		<< "REMOVAL must fail because drive is not ready";
 
     disk->SetReady(true);
 
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdRemoval));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, Status);
+	disk->Dispatch(scsi_command::eCmdPreventAllowMediumRemoval);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 	EXPECT_FALSE(disk->IsLocked());
 
 	cmd[4] = 1;
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdRemoval));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, Status);
+	disk->Dispatch(scsi_command::eCmdPreventAllowMediumRemoval);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 	EXPECT_TRUE(disk->IsLocked());
 }
 
@@ -593,9 +700,9 @@ TEST(DiskTest, Eject)
 	EXPECT_TRUE(disk.Eject(true));
 }
 
-void DiskTest_ValidateFormatPage(AbstractController& controller, int offset)
+void DiskTest_ValidateFormatPage(shared_ptr<AbstractController> controller, int offset)
 {
-	const auto& buf = controller.GetBuffer();
+	const auto& buf = controller->GetBuffer();
 	EXPECT_EQ(0x08, buf[offset + 3]) << "Wrong number of trackes in one zone";
 	EXPECT_EQ(25, GetInt16(buf, offset + 10)) << "Wrong number of sectors per track";
 	EXPECT_EQ(1024, GetInt16(buf, offset + 12)) << "Wrong number of bytes per sector";
@@ -606,18 +713,18 @@ void DiskTest_ValidateFormatPage(AbstractController& controller, int offset)
 	EXPECT_TRUE(buf[offset + 20] & 0x40) << "Wrong hard-sectored flag";
 }
 
-void DiskTest_ValidateDrivePage(AbstractController& controller, int offset)
+void DiskTest_ValidateDrivePage(shared_ptr<AbstractController> controller, int offset)
 {
-	const auto& buf = controller.GetBuffer();
+	const auto& buf = controller->GetBuffer();
 	EXPECT_EQ(0x17, buf[offset + 2]);
 	EXPECT_EQ(0x4d3b, GetInt16(buf, offset + 3));
 	EXPECT_EQ(8, buf[offset + 5]) << "Wrong number of heads";
 	EXPECT_EQ(7200, GetInt16(buf, offset + 20)) << "Wrong medium rotation rate";
 }
 
-void DiskTest_ValidateCachePage(AbstractController& controller, int offset)
+void DiskTest_ValidateCachePage(shared_ptr<AbstractController> controller, int offset)
 {
-	const auto& buf = controller.GetBuffer();
+	const auto& buf = controller->GetBuffer();
 	EXPECT_EQ(0xffff, GetInt16(buf, offset + 4)) << "Wrong pre-fetch transfer length";
 	EXPECT_EQ(0xffff, GetInt16(buf, offset + 8)) << "Wrong maximum pre-fetch";
 	EXPECT_EQ(0xffff, GetInt16(buf, offset + 10)) << "Wrong maximum pre-fetch ceiling";
@@ -625,12 +732,16 @@ void DiskTest_ValidateCachePage(AbstractController& controller, int offset)
 
 TEST(DiskTest, ModeSense6)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<NiceMock<MockAbstractController>>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-    controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 
 	// Drive must be ready on order to return all data
 	disk->SetReady(true);
@@ -638,19 +749,19 @@ TEST(DiskTest, ModeSense6)
 	cmd[2] = 0x3f;
 	// ALLOCATION LENGTH
 	cmd[4] = 255;
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdModeSense6));
-	EXPECT_EQ(0x08, controller.GetBuffer()[3]) << "Wrong block descriptor length";
+	disk->Dispatch(scsi_command::eCmdModeSense6);
+	EXPECT_EQ(0x08, controller->GetBuffer()[3]) << "Wrong block descriptor length";
 
 	// No block descriptor
 	cmd[1] = 0x08;
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdModeSense6));
-	EXPECT_EQ(0x00, controller.GetBuffer()[2]) << "Wrong device-specific parameter";
+	disk->Dispatch(scsi_command::eCmdModeSense6);
+	EXPECT_EQ(0x00, controller->GetBuffer()[2]) << "Wrong device-specific parameter";
 
 	disk->SetReadOnly(false);
 	disk->SetProtectable(true);
 	disk->SetProtected(true);
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdModeSense6));
-	const auto& buf = controller.GetBuffer();
+	disk->Dispatch(scsi_command::eCmdModeSense6);
+	const auto& buf = controller->GetBuffer();
 	EXPECT_EQ(0x80, buf[2]) << "Wrong device-specific parameter";
 
 	// Return block descriptor
@@ -659,29 +770,33 @@ TEST(DiskTest, ModeSense6)
 	// Format page
 	cmd[2] = 3;
 	disk->SetSectorSizeInBytes(1024);
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdModeSense6));
+	disk->Dispatch(scsi_command::eCmdModeSense6);
 	DiskTest_ValidateFormatPage(controller, 12);
 
 	// Rigid disk drive page
 	cmd[2] = 4;
 	disk->SetBlockCount(0x12345678);
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdModeSense6));
+	disk->Dispatch(scsi_command::eCmdModeSense6);
 	DiskTest_ValidateDrivePage(controller, 12);
 
 	// Cache page
 	cmd[2] = 8;
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdModeSense6));
+	disk->Dispatch(scsi_command::eCmdModeSense6);
 	DiskTest_ValidateCachePage(controller, 12);
 }
 
 TEST(DiskTest, ModeSense10)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<NiceMock<MockAbstractController>>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-	controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 
 	// Drive must be ready on order to return all data
 	disk->SetReady(true);
@@ -689,28 +804,28 @@ TEST(DiskTest, ModeSense10)
 	cmd[2] = 0x3f;
 	// ALLOCATION LENGTH
 	cmd[8] = 255;
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdModeSense10));
-	EXPECT_EQ(0x08, controller.GetBuffer()[7]) << "Wrong block descriptor length";
+	disk->Dispatch(scsi_command::eCmdModeSense10);
+	EXPECT_EQ(0x08, controller->GetBuffer()[7]) << "Wrong block descriptor length";
 
 	// No block descriptor
 	cmd[1] = 0x08;
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdModeSense10));
-	auto& buf = controller.GetBuffer();
-	EXPECT_EQ(0x00, controller.GetBuffer()[3]) << "Wrong device-specific parameter";
+	disk->Dispatch(scsi_command::eCmdModeSense10);
+	auto& buf = controller->GetBuffer();
+	EXPECT_EQ(0x00, controller->GetBuffer()[3]) << "Wrong device-specific parameter";
 
 	disk->SetReadOnly(false);
 	disk->SetProtectable(true);
 	disk->SetProtected(true);
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdModeSense10));
-	buf = controller.GetBuffer();
+	disk->Dispatch(scsi_command::eCmdModeSense10);
+	buf = controller->GetBuffer();
 	EXPECT_EQ(0x80, buf[3]) << "Wrong device-specific parameter";
 
 	// Return short block descriptor
 	cmd[1] = 0x00;
 	disk->SetBlockCount(0x1234);
 	disk->SetSectorSizeInBytes(1024);
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdModeSense10));
-	buf = controller.GetBuffer();
+	disk->Dispatch(scsi_command::eCmdModeSense10);
+	buf = controller->GetBuffer();
 	EXPECT_EQ(0x00, buf[4]) << "Wrong LONGLBA field";
 	EXPECT_EQ(0x08, buf[7]) << "Wrong block descriptor length";
 	EXPECT_EQ(0x00, GetInt16(buf, 8));
@@ -721,8 +836,8 @@ TEST(DiskTest, ModeSense10)
 	// Return long block descriptor
 	cmd[1] = 0x10;
 	disk->SetBlockCount((uint64_t)0xffffffff + 1);
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdModeSense10));
-	buf = controller.GetBuffer();
+	disk->Dispatch(scsi_command::eCmdModeSense10);
+	buf = controller->GetBuffer();
 	EXPECT_EQ(0x01, buf[4]) << "Wrong LONGLBA field";
 	EXPECT_EQ(0x10, buf[7]) << "Wrong block descriptor length";
 	EXPECT_EQ(0x00, GetInt16(buf, 8));
@@ -736,49 +851,57 @@ TEST(DiskTest, ModeSense10)
 	// Format page
 	cmd[2] = 3;
 	disk->SetSectorSizeInBytes(1024);
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdModeSense10));
+	disk->Dispatch(scsi_command::eCmdModeSense10);
 	DiskTest_ValidateFormatPage(controller, 16);
 
 	// Rigid disk drive page
 	cmd[2] = 4;
 	disk->SetBlockCount(0x12345678);
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdModeSense10));
+	disk->Dispatch(scsi_command::eCmdModeSense10);
 	DiskTest_ValidateDrivePage(controller, 16);
 
 	// Cache page
 	cmd[2] = 8;
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdModeSense10));
+	disk->Dispatch(scsi_command::eCmdModeSense10);
 	DiskTest_ValidateCachePage(controller, 16);
 }
 
 TEST(DiskTest, SynchronizeCache)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-	controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
-	EXPECT_CALL(*disk, FlushCache());
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdSynchronizeCache10));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*disk, FlushCache);
+	EXPECT_CALL(*controller, Status);
+	disk->Dispatch(scsi_command::eCmdSynchronizeCache10);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 
-	EXPECT_CALL(*disk, FlushCache());
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdSynchronizeCache16));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*disk, FlushCache);
+	EXPECT_CALL(*controller, Status);
+	disk->Dispatch(scsi_command::eCmdSynchronizeCache16);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 }
 
 TEST(DiskTest, ReadDefectData)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto disk = make_shared<MockDisk>();
+	const unordered_map<string, string> params;
+	disk->Init(params);
 
-    controller.AddDevice(disk);
+	controller->AddDevice(disk);
 
-	EXPECT_CALL(controller, DataIn());
-	EXPECT_TRUE(disk->Dispatch(scsi_command::eCmdReadDefectData10));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, DataIn);
+	disk->Dispatch(scsi_command::eCmdReadDefectData10);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 }
 
 TEST(DiskTest, SectorSize)
diff --git a/cpp/test/gpiobus_test.cpp b/cpp/test/gpiobus_test.cpp
deleted file mode 100644
index faaf920e..00000000
--- a/cpp/test/gpiobus_test.cpp
+++ /dev/null
@@ -1,44 +0,0 @@
-//---------------------------------------------------------------------------
-//
-// SCSI Target Emulator RaSCSI Reloaded
-// for Raspberry Pi
-//
-// Copyright (C) 2022 Uwe Seimet
-//
-//---------------------------------------------------------------------------
-
-#include <gtest/gtest.h>
-
-#include "hal/gpiobus.h"
-#include <unordered_set>
-
-TEST(GpioBusTest, GetCommandByteCount)
-{
-	unordered_set<int> opcodes;
-
-	EXPECT_EQ(16, GPIOBUS::GetCommandByteCount(0x88));
-	opcodes.insert(0x88);
-	EXPECT_EQ(16, GPIOBUS::GetCommandByteCount(0x8a));
-	opcodes.insert(0x8a);
-	EXPECT_EQ(16, GPIOBUS::GetCommandByteCount(0x8f));
-	opcodes.insert(0x8f);
-	EXPECT_EQ(16, GPIOBUS::GetCommandByteCount(0x91));
-	opcodes.insert(0x91);
-	EXPECT_EQ(16, GPIOBUS::GetCommandByteCount(0x9e));
-	opcodes.insert(0x9e);
-	EXPECT_EQ(16, GPIOBUS::GetCommandByteCount(0x9f));
-	opcodes.insert(0x9f);
-	EXPECT_EQ(12, GPIOBUS::GetCommandByteCount(0xa0));
-	opcodes.insert(0xa0);
-
-	for (int i = 0x20; i <= 0x7d; i++) {
-		EXPECT_EQ(10, GPIOBUS::GetCommandByteCount(i));
-		opcodes.insert(i);
-	}
-
-	for (int i = 0; i < 256; i++) {
-		if (opcodes.find(i) == opcodes.end()) {
-			EXPECT_EQ(6, GPIOBUS::GetCommandByteCount(i));
-		}
-	}
-}
diff --git a/cpp/test/host_services_test.cpp b/cpp/test/host_services_test.cpp
index f41cedcc..b0f434be 100644
--- a/cpp/test/host_services_test.cpp
+++ b/cpp/test/host_services_test.cpp
@@ -20,19 +20,16 @@ void HostServices_SetUpModePages(map<int, vector<byte>>& pages)
 	EXPECT_EQ(10, pages[32].size());
 }
 
-TEST(HostServicesTest, Dispatch)
-{
-	TestDispatch(SCHS);
-}
-
 TEST(HostServicesTest, TestUnitReady)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto services = CreateDevice(SCHS, controller);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
+	auto services = CreateDevice(SCHS, *controller);
 
-    EXPECT_CALL(controller, Status());
-    EXPECT_TRUE(services->Dispatch(scsi_command::eCmdTestUnitReady)) << "TEST UNIT READY must never fail";
-    EXPECT_EQ(status::GOOD, controller.GetStatus());
+    EXPECT_CALL(*controller, Status());
+    services->Dispatch(scsi_command::eCmdTestUnitReady);
+    EXPECT_EQ(status::GOOD, controller->GetStatus());
 }
 
 TEST(HostServicesTest, Inquiry)
@@ -42,30 +39,32 @@ TEST(HostServicesTest, Inquiry)
 
 TEST(HostServicesTest, StartStopUnit)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto services = CreateDevice(SCHS, controller);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
+	auto services = CreateDevice(SCHS, *controller);
 
-    vector<int>& cmd = controller.GetCmd();
+    auto& cmd = controller->GetCmd();
 
     // STOP
-    EXPECT_CALL(controller, ScheduleShutdown(AbstractController::rascsi_shutdown_mode::STOP_RASCSI));
-    EXPECT_CALL(controller, Status());
-    EXPECT_TRUE(services->Dispatch(scsi_command::eCmdStartStop));
-    EXPECT_EQ(status::GOOD, controller.GetStatus());
+    EXPECT_CALL(*controller, ScheduleShutdown(AbstractController::rascsi_shutdown_mode::STOP_RASCSI));
+    EXPECT_CALL(*controller, Status());
+    services->Dispatch(scsi_command::eCmdStartStop);
+    EXPECT_EQ(status::GOOD, controller->GetStatus());
 
     // LOAD
     cmd[4] = 0x02;
-    EXPECT_CALL(controller, ScheduleShutdown(AbstractController::rascsi_shutdown_mode::STOP_PI));
-    EXPECT_CALL(controller, Status());
-    EXPECT_TRUE(services->Dispatch(scsi_command::eCmdStartStop));
-    EXPECT_EQ(status::GOOD, controller.GetStatus());
+    EXPECT_CALL(*controller, ScheduleShutdown(AbstractController::rascsi_shutdown_mode::STOP_PI));
+    EXPECT_CALL(*controller, Status());
+    services->Dispatch(scsi_command::eCmdStartStop);
+    EXPECT_EQ(status::GOOD, controller->GetStatus());
 
     // UNLOAD
     cmd[4] = 0x03;
-    EXPECT_CALL(controller, ScheduleShutdown(AbstractController::rascsi_shutdown_mode::RESTART_PI));
-    EXPECT_CALL(controller, Status());
-    EXPECT_TRUE(services->Dispatch(scsi_command::eCmdStartStop));
-    EXPECT_EQ(status::GOOD, controller.GetStatus());
+    EXPECT_CALL(*controller, ScheduleShutdown(AbstractController::rascsi_shutdown_mode::RESTART_PI));
+    EXPECT_CALL(*controller, Status());
+    services->Dispatch(scsi_command::eCmdStartStop);
+    EXPECT_EQ(status::GOOD, controller->GetStatus());
 
     // START
     cmd[4] = 0x01;
@@ -76,10 +75,14 @@ TEST(HostServicesTest, StartStopUnit)
 
 TEST(HostServicesTest, ModeSense6)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto services = CreateDevice(SCHS, controller);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
+	auto services = CreateDevice(SCHS, *controller);
+	const unordered_map<string, string> params;
+	services->Init(params);
 
-    vector<int>& cmd = controller.GetCmd();
+    auto& cmd = controller->GetCmd();
 
 	EXPECT_THAT([&] { services->Dispatch(scsi_command::eCmdModeSense6); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::ILLEGAL_REQUEST),
@@ -95,9 +98,9 @@ TEST(HostServicesTest, ModeSense6)
     cmd[1] = 0x08;
     // ALLOCATION LENGTH
     cmd[4] = 255;
-    EXPECT_CALL(controller, DataIn());
-    EXPECT_TRUE(services->Dispatch(scsi_command::eCmdModeSense6));
-	vector<uint8_t>& buffer = controller.GetBuffer();
+    EXPECT_CALL(*controller, DataIn());
+    services->Dispatch(scsi_command::eCmdModeSense6);
+	vector<uint8_t>& buffer = controller->GetBuffer();
 	// Major version 1
 	EXPECT_EQ(0x01, buffer[6]);
 	// Minor version 0
@@ -109,18 +112,22 @@ TEST(HostServicesTest, ModeSense6)
 
     // ALLOCATION LENGTH
     cmd[4] = 2;
-    EXPECT_CALL(controller, DataIn());
-    EXPECT_TRUE(services->Dispatch(scsi_command::eCmdModeSense6));
-    buffer = controller.GetBuffer();
-    EXPECT_EQ(0x02, buffer[0]);
+    EXPECT_CALL(*controller, DataIn());
+    services->Dispatch(scsi_command::eCmdModeSense6);
+	buffer = controller->GetBuffer();
+	EXPECT_EQ(0x02, buffer[0]);
 }
 
 TEST(HostServicesTest, ModeSense10)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto services = CreateDevice(SCHS, controller);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
+	auto services = CreateDevice(SCHS, *controller);
+	const unordered_map<string, string> params;
+	services->Init(params);
 
-    vector<int>& cmd = controller.GetCmd();
+    auto& cmd = controller->GetCmd();
 
 	EXPECT_THAT([&] { services->Dispatch(scsi_command::eCmdModeSense10); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::ILLEGAL_REQUEST),
@@ -136,9 +143,9 @@ TEST(HostServicesTest, ModeSense10)
     cmd[1] = 0x08;
     // ALLOCATION LENGTH
     cmd[8] = 255;
-    EXPECT_CALL(controller, DataIn());
-    EXPECT_TRUE(services->Dispatch(scsi_command::eCmdModeSense10));
-	vector<uint8_t>& buffer = controller.GetBuffer();
+    EXPECT_CALL(*controller, DataIn());
+    services->Dispatch(scsi_command::eCmdModeSense10);
+	vector<uint8_t>& buffer = controller->GetBuffer();
 	// Major version 1
 	EXPECT_EQ(0x01, buffer[10]);
 	// Minor version 0
@@ -150,16 +157,15 @@ TEST(HostServicesTest, ModeSense10)
 
     // ALLOCATION LENGTH
     cmd[8] = 2;
-    EXPECT_CALL(controller, DataIn());
-    EXPECT_TRUE(services->Dispatch(scsi_command::eCmdModeSense10));
-    buffer = controller.GetBuffer();
-    EXPECT_EQ(0x02, buffer[1]);
+    EXPECT_CALL(*controller, DataIn());
+    services->Dispatch(scsi_command::eCmdModeSense10);
+	buffer = controller->GetBuffer();
+	EXPECT_EQ(0x02, buffer[1]);
 }
 
 TEST(HostServicesTest, SetUpModePages)
 {
-	ControllerManager controller_manager(make_shared<MockBus>());
-	MockHostServices services(0, controller_manager);
+	MockHostServices services(0);
 	map<int, vector<byte>> pages;
 
 	// Non changeable
diff --git a/cpp/test/mocks.h b/cpp/test/mocks.h
index be73de62..90d1c159 100644
--- a/cpp/test/mocks.h
+++ b/cpp/test/mocks.h
@@ -12,7 +12,7 @@
 #include <gmock/gmock.h>
 
 #include "test_shared.h"
-#include "bus.h"
+#include "hal/bus.h"
 #include "controllers/scsi_controller.h"
 #include "devices/host_services.h"
 #include "devices/primary_device.h"
@@ -23,6 +23,7 @@
 #include "devices/scsimo.h"
 #include "rascsi/command_context.h"
 #include "rascsi/rascsi_executor.h"
+#include <fcntl.h>
 
 using namespace testing;
 
@@ -130,6 +131,7 @@ class MockAbstractController : public AbstractController // NOSONAR Having many
 	FRIEND_TEST(AbstractControllerTest, Length);
 	FRIEND_TEST(AbstractControllerTest, UpdateOffsetAndLength);
 	FRIEND_TEST(AbstractControllerTest, Offset);
+	FRIEND_TEST(ScsiControllerTest, Selection);
 	FRIEND_TEST(PrimaryDeviceTest, Inquiry);
 	FRIEND_TEST(PrimaryDeviceTest, TestUnitReady);
 	FRIEND_TEST(PrimaryDeviceTest, RequestSense);
@@ -194,14 +196,15 @@ class MockAbstractController : public AbstractController // NOSONAR Having many
 	MOCK_METHOD(void, MsgOut, (), ());
 	MOCK_METHOD(void, ScheduleShutdown, (rascsi_shutdown_mode), (override));
 
-	explicit MockAbstractController(shared_ptr<MockBus> bus, int target_id) : AbstractController(bus, target_id, 32) {
+	explicit MockAbstractController(shared_ptr<ControllerManager> controller_manager, int target_id)
+		: AbstractController(controller_manager, target_id, 32) {
 		AllocateBuffer(512);
 	}
 	~MockAbstractController() override = default;
 
 	// Permit access to all tests without the need for numerous FRIEND_TEST
 	vector<int>& GetCmd() { return AbstractController::GetCmd(); } //NOSONAR Hides function on purpose
-	shared_ptr<BUS> GetBus() { return AbstractController::GetBus(); } //NOSONAR Hides function on purpose
+	BUS& GetBus() { return AbstractController::GetBus(); } //NOSONAR Hides function on purpose
 };
 
 class MockScsiController : public ScsiController
@@ -230,9 +233,10 @@ class MockScsiController : public ScsiController
 	MOCK_METHOD(void, Execute, (), ());
 
 	using ScsiController::ScsiController;
-	explicit MockScsiController(shared_ptr<NiceMock<MockBus>> bus, int target_id) : ScsiController(bus, target_id) {}
-	explicit MockScsiController(shared_ptr<MockBus> bus, int target_id) : ScsiController(bus, target_id) {}
-	explicit MockScsiController(shared_ptr<MockBus> bus) : ScsiController(bus, 0) {}
+	MockScsiController(shared_ptr<ControllerManager> controller_manager, int target_id)
+		: ScsiController(controller_manager, target_id) {}
+	explicit MockScsiController(shared_ptr<ControllerManager> controller_manager)
+		: ScsiController(controller_manager, 0) {}
 	~MockScsiController() override = default;
 
 };
@@ -271,7 +275,7 @@ class MockPrimaryDevice : public PrimaryDevice
     MOCK_METHOD(void, Reset, (), ());
     MOCK_METHOD(vector<byte>, InquiryInternal, (), (const));
 
-	MOCK_METHOD(vector<byte>, InquiryInternal, (), (const));
+	MOCK_METHOD(vector<uint8_t>, InquiryInternal, (), (const));
 
 	explicit MockPrimaryDevice(int lun) : PrimaryDevice(UNDEFINED, lun) {}
 	~MockPrimaryDevice() override = default;
@@ -288,7 +292,7 @@ class MockModePageDevice : public ModePageDevice
     MOCK_METHOD(int, ModeSense6, (const vector<int> &, vector<BYTE> &), (const override));
     MOCK_METHOD(int, ModeSense10, (const vector<int> &, vector<BYTE> &), (const override));
 
-	MOCK_METHOD(vector<byte>, InquiryInternal, (), (const));
+	MOCK_METHOD(vector<uint8_t>, InquiryInternal, (), (const));
 	MOCK_METHOD(int, ModeSense6, (const vector<int>&, vector<uint8_t>&), (const override));
 	MOCK_METHOD(int, ModeSense10, (const vector<int>&, vector<uint8_t>&), (const override));
 
@@ -330,7 +334,7 @@ class MockStorageDevice : public StorageDevice
 
 public:
 
-	MOCK_METHOD(vector<byte>, InquiryInternal, (), (const));
+	MOCK_METHOD(vector<uint8_t>, InquiryInternal, (), (const));
 	MOCK_METHOD(void, Open, (), (override));
 	MOCK_METHOD(int, ModeSense6, (const vector<int>&, vector<uint8_t>&), (const override));
 	MOCK_METHOD(int, ModeSense10, (const vector<int>&, vector<uint8_t>&), (const override));
@@ -378,7 +382,7 @@ class MockDisk : public Disk
     MOCK_METHOD(vector<byte>, InquiryInternal, (), (const));
     MOCK_METHOD(void, FlushCache, (), (override));
 
-	MOCK_METHOD(vector<byte>, InquiryInternal, (), (const));
+	MOCK_METHOD(vector<uint8_t>, InquiryInternal, (), (const));
 	MOCK_METHOD(void, FlushCache, (), (override));
 	MOCK_METHOD(void, Open, (), (override));
 
diff --git a/cpp/test/mode_page_device_test.cpp b/cpp/test/mode_page_device_test.cpp
index 13afcb9b..ff4447c2 100644
--- a/cpp/test/mode_page_device_test.cpp
+++ b/cpp/test/mode_page_device_test.cpp
@@ -78,36 +78,34 @@ TEST(ModePageDeviceTest, AddVendorPage)
 	EXPECT_TRUE(pages.empty()) << "There must not be any default vendor page";
 }
 
-TEST(ModePageDeviceTest, Dispatch)
-{
-    MockAbstractController controller(make_shared<MockBus>(), 0);
-	auto device = make_shared<NiceMock<MockModePageDevice>>();
-
-    controller.AddDevice(device);
-
-    EXPECT_FALSE(device->Dispatch(scsi_command::eCmdIcd)) << "Command is not supported by this class";
-}
-
 TEST(ModePageDeviceTest, ModeSense6)
 {
-    MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto device = make_shared<NiceMock<MockModePageDevice>>();
+	const unordered_map<string, string> params;
+	device->Init(params);
 
-    controller.AddDevice(device);
+    controller->AddDevice(device);
 
-    EXPECT_CALL(controller, DataIn());
-    EXPECT_TRUE(device->Dispatch(scsi_command::eCmdModeSense6));
+    EXPECT_CALL(*controller, DataIn());
+    device->Dispatch(scsi_command::eCmdModeSense6);
 }
 
 TEST(ModePageDeviceTest, ModeSense10)
 {
-    MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto device = make_shared<NiceMock<MockModePageDevice>>();
+	const unordered_map<string, string> params;
+	device->Init(params);
 
-    controller.AddDevice(device);
+    controller->AddDevice(device);
 
-    EXPECT_CALL(controller, DataIn());
-    EXPECT_TRUE(device->Dispatch(scsi_command::eCmdModeSense10));
+    EXPECT_CALL(*controller, DataIn());
+    device->Dispatch(scsi_command::eCmdModeSense10);
 }
 
 TEST(ModePageDeviceTest, ModeSelect)
@@ -128,15 +126,19 @@ TEST(ModePageDeviceTest, ModeSelect)
 
 TEST(ModePageDeviceTest, ModeSelect6)
 {
-    MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto device = make_shared<MockModePageDevice>();
+	const unordered_map<string, string> params;
+	device->Init(params);
 
-    controller.AddDevice(device);
+    controller->AddDevice(device);
 
-    vector<int>& cmd = controller.GetCmd();
+    auto& cmd = controller->GetCmd();
 
-    EXPECT_CALL(controller, DataOut());
-    EXPECT_TRUE(device->Dispatch(scsi_command::eCmdModeSelect6));
+    EXPECT_CALL(*controller, DataOut());
+    device->Dispatch(scsi_command::eCmdModeSelect6);
 
     cmd[1] = 0x01;
 	EXPECT_THAT([&] { device->Dispatch(scsi_command::eCmdModeSelect6); }, Throws<scsi_exception>(AllOf(
@@ -147,15 +149,19 @@ TEST(ModePageDeviceTest, ModeSelect6)
 
 TEST(ModePageDeviceTest, ModeSelect10)
 {
-    MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto device = make_shared<MockModePageDevice>();
+	const unordered_map<string, string> params;
+	device->Init(params);
 
-    controller.AddDevice(device);
+    controller->AddDevice(device);
 
-    vector<int>& cmd = controller.GetCmd();
+    auto& cmd = controller->GetCmd();
 
-    EXPECT_CALL(controller, DataOut());
-    EXPECT_TRUE(device->Dispatch(scsi_command::eCmdModeSelect10));
+    EXPECT_CALL(*controller, DataOut());
+    device->Dispatch(scsi_command::eCmdModeSelect10);
 
     cmd[1] = 0x01;
 	EXPECT_THAT([&] { device->Dispatch(scsi_command::eCmdModeSelect10); }, Throws<scsi_exception>(AllOf(
diff --git a/cpp/test/primary_device_test.cpp b/cpp/test/primary_device_test.cpp
index 958d8277..9dbfb7ce 100644
--- a/cpp/test/primary_device_test.cpp
+++ b/cpp/test/primary_device_test.cpp
@@ -8,51 +8,61 @@
 //---------------------------------------------------------------------------
 
 #include "devices/device_factory.h"
-#include "devices/primary_device.h"
-#include "mocks.h"
-#include "rascsi_exceptions.h"
-#include "scsi.h"
+#include "devices/scsi_command_util.h"
 
 using namespace scsi_defs;
+using namespace scsi_command_util;
 
 TEST(PrimaryDeviceTest, GetId)
 {
     const int ID = 5;
 
-	MockAbstractController controller(make_shared<MockBus>(), ID);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<NiceMock<MockAbstractController>>(controller_manager, ID);
 	auto device = make_shared<MockPrimaryDevice>(0);
+	const unordered_map<string, string> params;
+	device->Init(params);
 
     EXPECT_EQ(-1, device->GetId()) << "Device ID cannot be known without assignment to a controller";
 
-    controller.AddDevice(device);
-    EXPECT_EQ(ID, device->GetId());
+	controller->AddDevice(device);
+	EXPECT_EQ(ID, device->GetId());
 }
 
 TEST(PrimaryDeviceTest, PhaseChange)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto device = make_shared<MockPrimaryDevice>(0);
+	const unordered_map<string, string> params;
+	device->Init(params);
 
-    controller.AddDevice(device);
+	controller->AddDevice(device);
 
-	EXPECT_CALL(controller, Status());
+	EXPECT_CALL(*controller, Status);
 	device->EnterStatusPhase();
 
-	EXPECT_CALL(controller, DataIn());
+	EXPECT_CALL(*controller, DataIn);
 	device->EnterDataInPhase();
 
-	EXPECT_CALL(controller, DataOut());
+	EXPECT_CALL(*controller, DataOut);
 	device->EnterDataOutPhase();
 }
 
 TEST(PrimaryDeviceTest, Reset)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<NiceMock<MockAbstractController>>(controller_manager, 0);
 	auto device = make_shared<MockPrimaryDevice>(0);
+	const unordered_map<string, string> params;
+	device->Init(params);
 
-	controller.AddDevice(device);
+	controller->AddDevice(device);
 
-	EXPECT_TRUE(device->Dispatch(scsi_command::eCmdReserve6));
+	device->Dispatch(scsi_command::eCmdReserve6);
 	EXPECT_FALSE(device->CheckReservation(1, scsi_command::eCmdTestUnitReady, false))
 		<< "Device must be reserved for initiator ID 1";
 	device->Reset();
@@ -62,15 +72,19 @@ TEST(PrimaryDeviceTest, Reset)
 
 TEST(PrimaryDeviceTest, CheckReservation)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<NiceMock<MockAbstractController>>(controller_manager, 0);
 	auto device = make_shared<MockPrimaryDevice>(0);
+	const unordered_map<string, string> params;
+	device->Init(params);
 
-	controller.AddDevice(device);
+	controller->AddDevice(device);
 
 	EXPECT_TRUE(device->CheckReservation(0, scsi_command::eCmdTestUnitReady, false))
 		<< "Device must not be reserved for initiator ID 0";
 
-	EXPECT_TRUE(device->Dispatch(scsi_command::eCmdReserve6));
+	device->Dispatch(scsi_command::eCmdReserve6);
 	EXPECT_TRUE(device->CheckReservation(0, scsi_command::eCmdTestUnitReady, false))
 		<< "Device must not be reserved for initiator ID 0";
 	EXPECT_FALSE(device->CheckReservation(1, scsi_command::eCmdTestUnitReady, false))
@@ -84,45 +98,53 @@ TEST(PrimaryDeviceTest, CheckReservation)
 	EXPECT_TRUE(device->CheckReservation(1, scsi_command::eCmdRelease6, false))
 		<< "Device must not be reserved for RELEASE (6)";
 
-	EXPECT_TRUE(device->CheckReservation(1, scsi_command::eCmdRemoval, false))
+	EXPECT_TRUE(device->CheckReservation(1, scsi_command::eCmdPreventAllowMediumRemoval, false))
 		<< "Device must not be reserved for PREVENT ALLOW MEDIUM REMOVAL with prevent bit not set";
-	EXPECT_FALSE(device->CheckReservation(1, scsi_command::eCmdRemoval, true))
+	EXPECT_FALSE(device->CheckReservation(1, scsi_command::eCmdPreventAllowMediumRemoval, true))
 		<< "Device must be reserved for PREVENT ALLOW MEDIUM REMOVAL with prevent bit set";
 }
 
 TEST(PrimaryDeviceTest, ReserveReleaseUnit)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<NiceMock<MockAbstractController>>(controller_manager, 0);
 	auto device = make_shared<MockPrimaryDevice>(0);
+	const unordered_map<string, string> params;
+	device->Init(params);
 
-	controller.AddDevice(device);
+	controller->AddDevice(device);
 
-	EXPECT_TRUE(device->Dispatch(scsi_command::eCmdReserve6));
+	device->Dispatch(scsi_command::eCmdReserve6);
 	EXPECT_FALSE(device->CheckReservation(1, scsi_command::eCmdTestUnitReady, false))
 		<< "Device must be reserved for initiator ID 1";
 
-	EXPECT_TRUE(device->Dispatch(scsi_command::eCmdRelease6));
+	device->Dispatch(scsi_command::eCmdRelease6);
 	EXPECT_TRUE(device->CheckReservation(1, scsi_command::eCmdTestUnitReady, false))
 		<< "Device must not be reserved anymore for initiator ID 1";
 
-	ON_CALL(controller, GetInitiatorId).WillByDefault(Return(-1));
-	EXPECT_TRUE(device->Dispatch(scsi_command::eCmdReserve6));
+	ON_CALL(*controller, GetInitiatorId).WillByDefault(Return(-1));
+	device->Dispatch(scsi_command::eCmdReserve6);
 	EXPECT_FALSE(device->CheckReservation(1, scsi_command::eCmdTestUnitReady, false))
 		<< "Device must be reserved for unknown initiator";
 
-	EXPECT_TRUE(device->Dispatch(scsi_command::eCmdRelease6));
+	device->Dispatch(scsi_command::eCmdRelease6);
 	EXPECT_TRUE(device->CheckReservation(1, scsi_command::eCmdTestUnitReady, false))
 		<< "Device must not be reserved anymore for unknown initiator";
 }
 
 TEST(PrimaryDeviceTest, DiscardReservation)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<NiceMock<MockAbstractController>>(controller_manager, 0);
 	auto device = make_shared<MockPrimaryDevice>(0);
+	const unordered_map<string, string> params;
+	device->Init(params);
 
-	controller.AddDevice(device);
+	controller->AddDevice(device);
 
-	EXPECT_TRUE(device->Dispatch(scsi_command::eCmdReserve6));
+	device->Dispatch(scsi_command::eCmdReserve6);
 	EXPECT_FALSE(device->CheckReservation(1, scsi_command::eCmdTestUnitReady, false))
 		<< "Device must be reserved for initiator ID 1";
 	device->DiscardReservation();
@@ -132,76 +154,85 @@ TEST(PrimaryDeviceTest, DiscardReservation)
 
 TEST(PrimaryDeviceTest, TestUnitReady)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto device = make_shared<MockPrimaryDevice>(0);
+	const unordered_map<string, string> params;
+	device->Init(params);
 
-    controller.AddDevice(device);
+	controller->AddDevice(device);
 
 	device->SetReset(true);
 	device->SetAttn(true);
 	device->SetReady(false);
-	EXPECT_CALL(controller, DataIn()).Times(0);
+	EXPECT_CALL(*controller, DataIn).Times(0);
 	EXPECT_THAT([&] { device->Dispatch(scsi_command::eCmdTestUnitReady); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::UNIT_ATTENTION),
 			Property(&scsi_exception::get_asc, asc::POWER_ON_OR_RESET))));
 
 	device->SetReset(false);
-	EXPECT_CALL(controller, DataIn()).Times(0);
+	EXPECT_CALL(*controller, DataIn).Times(0);
 	EXPECT_THAT([&] { device->Dispatch(scsi_command::eCmdTestUnitReady); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::UNIT_ATTENTION),
 			Property(&scsi_exception::get_asc, asc::NOT_READY_TO_READY_CHANGE))));
 
 	device->SetReset(true);
 	device->SetAttn(false);
-	EXPECT_CALL(controller, DataIn()).Times(0);
+	EXPECT_CALL(*controller, DataIn).Times(0);
 	EXPECT_THAT([&] { device->Dispatch(scsi_command::eCmdTestUnitReady); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::UNIT_ATTENTION),
 			Property(&scsi_exception::get_asc, asc::POWER_ON_OR_RESET))));
 
 	device->SetReset(false);
 	device->SetAttn(true);
-	EXPECT_CALL(controller, DataIn()).Times(0);
+	EXPECT_CALL(*controller, DataIn).Times(0);
 	EXPECT_THAT([&] { device->Dispatch(scsi_command::eCmdTestUnitReady); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::UNIT_ATTENTION),
 			Property(&scsi_exception::get_asc, asc::NOT_READY_TO_READY_CHANGE))));
 
 	device->SetAttn(false);
-	EXPECT_CALL(controller, DataIn()).Times(0);
+	EXPECT_CALL(*controller, DataIn).Times(0);
 	EXPECT_THAT([&] { device->Dispatch(scsi_command::eCmdTestUnitReady); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::NOT_READY),
 			Property(&scsi_exception::get_asc, asc::MEDIUM_NOT_PRESENT))));
 
 	device->SetReady(true);
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(device->Dispatch(scsi_command::eCmdTestUnitReady));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, Status);
+	device->Dispatch(scsi_command::eCmdTestUnitReady);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 }
 
 TEST(PrimaryDeviceTest, Inquiry)
 {
-	auto controller = make_shared<NiceMock<MockAbstractController>>(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<NiceMock<MockAbstractController>>(controller_manager, 0);
 	auto device = make_shared<MockPrimaryDevice>(0);
+	const unordered_map<string, string> params;
+	device->Init(params);
 
 	controller->AddDevice(device);
 
-	vector<int>& cmd = controller->GetCmd();
+	auto& cmd = controller->GetCmd();
+
 	// ALLOCATION LENGTH
 	cmd[4] = 255;
 
 	ON_CALL(*device, InquiryInternal()).WillByDefault([&device]() {
 		return device->HandleInquiry(device_type::PROCESSOR, scsi_level::SPC_3, false);
 	});
-	EXPECT_CALL(*device, InquiryInternal());
-	EXPECT_CALL(*controller, DataIn());
+	EXPECT_CALL(*device, InquiryInternal);
+	EXPECT_CALL(*controller, DataIn);
 	ON_CALL(*controller, GetEffectiveLun()).WillByDefault(Return(1));
-	EXPECT_TRUE(device->Dispatch(scsi_command::eCmdInquiry));
+	device->Dispatch(scsi_command::eCmdInquiry);
 	EXPECT_EQ(0x7f, controller->GetBuffer()[0]) << "Invalid LUN was not reported";
 	ON_CALL(*controller, GetEffectiveLun()).WillByDefault(Return(0));
 
 	EXPECT_FALSE(controller->AddDevice(make_shared<MockPrimaryDevice>(0))) << "Duplicate LUN was not rejected";
-	EXPECT_CALL(*device, InquiryInternal());
-	EXPECT_CALL(*controller, DataIn());
-	EXPECT_TRUE(device->Dispatch(scsi_command::eCmdInquiry));
+	EXPECT_CALL(*device, InquiryInternal);
+	EXPECT_CALL(*controller, DataIn);
+	device->Dispatch(scsi_command::eCmdInquiry);
 	EXPECT_EQ(device_type::PROCESSOR, (device_type)controller->GetBuffer()[0]);
 	EXPECT_EQ(0x00, controller->GetBuffer()[1]) << "Device was not reported as non-removable";
 	EXPECT_EQ(scsi_level::SPC_3, (scsi_level)controller->GetBuffer()[2]) << "Wrong SCSI level";
@@ -211,9 +242,9 @@ TEST(PrimaryDeviceTest, Inquiry)
 	ON_CALL(*device, InquiryInternal()).WillByDefault([&device]() {
 		return device->HandleInquiry(device_type::DIRECT_ACCESS, scsi_level::SCSI_1_CCS, true);
 	});
-	EXPECT_CALL(*device, InquiryInternal());
-	EXPECT_CALL(*controller, DataIn());
-	EXPECT_TRUE(device->Dispatch(scsi_command::eCmdInquiry));
+	EXPECT_CALL(*device, InquiryInternal);
+	EXPECT_CALL(*controller, DataIn);
+	device->Dispatch(scsi_command::eCmdInquiry);
 	EXPECT_EQ(device_type::DIRECT_ACCESS, (device_type)controller->GetBuffer()[0]);
 	EXPECT_EQ(0x80, controller->GetBuffer()[1]) << "Device was not reported as removable";
 	EXPECT_EQ(scsi_level::SCSI_1_CCS, (scsi_level)controller->GetBuffer()[2]) << "Wrong SCSI level";
@@ -221,14 +252,14 @@ TEST(PrimaryDeviceTest, Inquiry)
 	EXPECT_EQ(0x1f, controller->GetBuffer()[4]) << "Wrong additional data size";
 
 	cmd[1] = 0x01;
-	EXPECT_CALL(*controller, DataIn()).Times(0);
+	EXPECT_CALL(*controller, DataIn).Times(0);
 	EXPECT_THAT([&] { device->Dispatch(scsi_command::eCmdInquiry); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::ILLEGAL_REQUEST),
 			Property(&scsi_exception::get_asc, asc::INVALID_FIELD_IN_CDB))))
 		<< "EVPD bit is not supported";
 
 	cmd[2] = 0x01;
-	EXPECT_CALL(*controller, DataIn()).Times(0);
+	EXPECT_CALL(*controller, DataIn).Times(0);
 	EXPECT_THAT([&] { device->Dispatch(scsi_command::eCmdInquiry); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::ILLEGAL_REQUEST),
 			Property(&scsi_exception::get_asc, asc::INVALID_FIELD_IN_CDB))))
@@ -238,21 +269,25 @@ TEST(PrimaryDeviceTest, Inquiry)
 	cmd[2] = 0x00;
 	// ALLOCATION LENGTH
 	cmd[4] = 1;
-	EXPECT_CALL(*device, InquiryInternal());
-	EXPECT_CALL(*controller, DataIn());
-	EXPECT_TRUE(device->Dispatch(scsi_command::eCmdInquiry));
+	EXPECT_CALL(*device, InquiryInternal);
+	EXPECT_CALL(*controller, DataIn);
+	device->Dispatch(scsi_command::eCmdInquiry);
 	EXPECT_EQ(0x1f, controller->GetBuffer()[4]) << "Wrong additional data size";
 	EXPECT_EQ(1, controller->GetLength()) << "Wrong ALLOCATION LENGTH handling";
 }
 
 TEST(PrimaryDeviceTest, RequestSense)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<NiceMock<MockAbstractController>>(controller_manager, 0);
 	auto device = make_shared<MockPrimaryDevice>(0);
+	const unordered_map<string, string> params;
+	device->Init(params);
 
-    controller.AddDevice(device);
+	controller->AddDevice(device);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 	// ALLOCATION LENGTH
 	cmd[4] = 255;
 
@@ -262,39 +297,43 @@ TEST(PrimaryDeviceTest, RequestSense)
 			Property(&scsi_exception::get_asc, asc::MEDIUM_NOT_PRESENT))));
 
 	device->SetReady(true);
-	EXPECT_CALL(controller, DataIn());
-	EXPECT_TRUE(device->Dispatch(scsi_command::eCmdRequestSense));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, DataIn);
+	device->Dispatch(scsi_command::eCmdRequestSense);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 }
 
 TEST(PrimaryDeviceTest, SendDiagnostic)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto device = make_shared<MockPrimaryDevice>(0);
+	const unordered_map<string, string> params;
+	device->Init(params);
 
-	controller.AddDevice(device);
+	controller->AddDevice(device);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(device->Dispatch(scsi_command::eCmdSendDiag));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, Status);
+	device->Dispatch(scsi_command::eCmdSendDiagnostic);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 
 	cmd[1] = 0x10;
-	EXPECT_THAT([&] { device->Dispatch(scsi_command::eCmdSendDiag); }, Throws<scsi_exception>(AllOf(
+	EXPECT_THAT([&] { device->Dispatch(scsi_command::eCmdSendDiagnostic); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::ILLEGAL_REQUEST),
 			Property(&scsi_exception::get_asc, asc::INVALID_FIELD_IN_CDB))))
 		<< "SEND DIAGNOSTIC must fail because PF bit is not supported";
 	cmd[1] = 0;
 
 	cmd[3] = 1;
-	EXPECT_THAT([&] { device->Dispatch(scsi_command::eCmdSendDiag); }, Throws<scsi_exception>(AllOf(
+	EXPECT_THAT([&] { device->Dispatch(scsi_command::eCmdSendDiagnostic); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::ILLEGAL_REQUEST),
 			Property(&scsi_exception::get_asc, asc::INVALID_FIELD_IN_CDB))))
 		<< "SEND DIAGNOSTIC must fail because parameter list is not supported";
 	cmd[3] = 0;
 	cmd[4] = 1;
-	EXPECT_THAT([&] { device->Dispatch(scsi_command::eCmdSendDiag); }, Throws<scsi_exception>(AllOf(
+	EXPECT_THAT([&] { device->Dispatch(scsi_command::eCmdSendDiagnostic); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::ILLEGAL_REQUEST),
 			Property(&scsi_exception::get_asc, asc::INVALID_FIELD_IN_CDB))))
 		<< "SEND DIAGNOSTIC must fail because parameter list is not supported";
@@ -305,42 +344,37 @@ TEST(PrimaryDeviceTest, ReportLuns)
     const int LUN1 = 1;
     const int LUN2 = 4;
 
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	auto device1 = make_shared<MockPrimaryDevice>(LUN1);
 	auto device2 = make_shared<MockPrimaryDevice>(LUN2);
+	const unordered_map<string, string> params;
+	device1->Init(params);
+	device2->Init(params);
 
-    controller.AddDevice(device1);
-    EXPECT_TRUE(controller.HasDeviceForLun(LUN1));
-    controller.AddDevice(device2);
-    EXPECT_TRUE(controller.HasDeviceForLun(LUN2));
+	controller->AddDevice(device1);
+	EXPECT_TRUE(controller->HasDeviceForLun(LUN1));
+	controller->AddDevice(device2);
+	EXPECT_TRUE(controller->HasDeviceForLun(LUN2));
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 	// ALLOCATION LENGTH
 	cmd[9] = 255;
 
-	EXPECT_CALL(controller, DataIn());
-	EXPECT_TRUE(device1->Dispatch(scsi_command::eCmdReportLuns));
-	const vector<uint8_t>& buffer = controller.GetBuffer();
-	EXPECT_EQ(0x00, buffer[0]) << "Wrong data length";
-	EXPECT_EQ(0x00, buffer[1]) << "Wrong data length";
-	EXPECT_EQ(0x00, buffer[2]) << "Wrong data length";
-	EXPECT_EQ(0x10, buffer[3]) << "Wrong data length";
-	EXPECT_EQ(0x00, buffer[8]) << "Wrong LUN1 number";
-	EXPECT_EQ(0x00, buffer[9]) << "Wrong LUN1 number";
-	EXPECT_EQ(0x00, buffer[10]) << "Wrong LUN1 number";
-	EXPECT_EQ(0x00, buffer[11]) << "Wrong LUN1 number";
-	EXPECT_EQ(0x00, buffer[12]) << "Wrong LUN1 number";
-	EXPECT_EQ(0x00, buffer[13]) << "Wrong LUN1 number";
-	EXPECT_EQ(0x00, buffer[14]) << "Wrong LUN1 number";
-	EXPECT_EQ(LUN1, buffer[15]) << "Wrong LUN1 number";
-	EXPECT_EQ(0x00, buffer[16]) << "Wrong LUN2 number";
-	EXPECT_EQ(0x00, buffer[17]) << "Wrong LUN2 number";
-	EXPECT_EQ(0x00, buffer[18]) << "Wrong LUN2 number";
-	EXPECT_EQ(0x00, buffer[19]) << "Wrong LUN2 number";
-	EXPECT_EQ(0x00, buffer[20]) << "Wrong LUN2 number";
-	EXPECT_EQ(0x00, buffer[21]) << "Wrong LUN2 number";
-	EXPECT_EQ(0x00, buffer[22]) << "Wrong LUN2 number";
-	EXPECT_EQ(LUN2, buffer[23]) << "Wrong LUN2 number";
+	EXPECT_CALL(*controller, DataIn);
+	device1->Dispatch(scsi_command::eCmdReportLuns);
+	const vector<uint8_t>& buffer = controller->GetBuffer();
+	EXPECT_EQ(0, GetInt16(buffer, 0)) << "Wrong data length";
+	EXPECT_EQ(16, GetInt16(buffer, 2)) << "Wrong data length";
+	EXPECT_EQ(0, GetInt16(buffer, 8)) << "Wrong LUN1 number";
+	EXPECT_EQ(0, GetInt16(buffer, 10)) << "Wrong LUN1 number";
+	EXPECT_EQ(0, GetInt16(buffer, 12)) << "Wrong LUN1 number";
+	EXPECT_EQ(LUN1, GetInt16(buffer, 14)) << "Wrong LUN1 number";
+	EXPECT_EQ(0, GetInt16(buffer, 16)) << "Wrong LUN2 number";
+	EXPECT_EQ(0, GetInt16(buffer, 18)) << "Wrong LUN2 number";
+	EXPECT_EQ(0, GetInt16(buffer, 20)) << "Wrong LUN2 number";
+	EXPECT_EQ(LUN2, GetInt16(buffer, 22)) << "Wrong LUN2 number";
 
 	cmd[2] = 0x01;
 	EXPECT_THAT([&] { device1->Dispatch(scsi_command::eCmdReportLuns); }, Throws<scsi_exception>(AllOf(
@@ -349,24 +383,18 @@ TEST(PrimaryDeviceTest, ReportLuns)
 		<< "Only SELECT REPORT mode 0 is supported";
 }
 
-TEST(PrimaryDeviceTest, UnknownCommand)
-{
-	MockAbstractController controller(make_shared<MockBus>(), 0);
-	auto device = make_shared<MockPrimaryDevice>(0);
-
-    controller.AddDevice(device);
-
-    EXPECT_FALSE(device->Dispatch((scsi_command)0xFF));
-}
-
 TEST(PrimaryDeviceTest, Dispatch)
 {
-    MockAbstractController controller(make_shared<MockBus>(), 0);
-	auto device = make_shared<NiceMock<MockPrimaryDevice>>(0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
+	auto device = make_shared<MockPrimaryDevice>(0);
+	const unordered_map<string, string> params;
+	device->Init(params);
 
-    controller.AddDevice(device);
+	controller->AddDevice(device);
 
-    EXPECT_FALSE(device->Dispatch(scsi_command::eCmdIcd)) << "Command is not supported by this class";
+	EXPECT_THROW(device->Dispatch(static_cast<scsi_command>(0x1f)), scsi_exception) << "Unknown command";
 }
 
 TEST(PrimaryDeviceTest, WriteByteSequence)
@@ -391,7 +419,7 @@ TEST(PrimaryDeviceTest, Init)
     unordered_map<string, string> params;
     MockPrimaryDevice device(0);
 
-	EXPECT_FALSE(device.Init(params)) << "Initialization of primary device must fail";
+	EXPECT_TRUE(device.Init(params)) << "Initialization of primary device must not fail";
 }
 
 TEST(PrimaryDeviceTest, FlushCache)
diff --git a/cpp/test/protobuf_util_test.cpp b/cpp/test/protobuf_util_test.cpp
index 60eb139f..d72cdd3b 100644
--- a/cpp/test/protobuf_util_test.cpp
+++ b/cpp/test/protobuf_util_test.cpp
@@ -22,7 +22,7 @@ void TestSpecialDevice(const string& name)
 	EXPECT_EQ("", GetParam(device, "interfaces"));
 }
 
-TEST(CommandUtil, AddGetParam)
+TEST(ProtobufUtil, AddGetParam)
 {
 	PbCommand command;
 	SetParam(command, "key", "value");
@@ -40,7 +40,7 @@ TEST(CommandUtil, AddGetParam)
 	EXPECT_EQ("value", it->second);
 }
 
-TEST(CommandUtil, ParseParameters)
+TEST(ProtobufUtil, ParseParameters)
 {
 	PbDeviceDefinition device1;
 	ParseParameters(device1, "a=b:c=d:e");
@@ -48,11 +48,10 @@ TEST(CommandUtil, ParseParameters)
 	EXPECT_EQ("d", GetParam(device1, "c"));
 	EXPECT_EQ("", GetParam(device1, "e"));
 
-	// Old style parameters
+	// Old style parameter
 	PbDeviceDefinition device2;
 	ParseParameters(device2, "a");
 	EXPECT_EQ("a", GetParam(device2, "file"));
-	EXPECT_EQ("a", GetParam(device2, "interfaces"));
 
 	TestSpecialDevice("bridge");
 	TestSpecialDevice("daynaport");
@@ -60,7 +59,7 @@ TEST(CommandUtil, ParseParameters)
 	TestSpecialDevice("services");
 }
 
-TEST(CommandUtil, SetPatternParams)
+TEST(ProtobufUtil, SetPatternParams)
 {
 	PbCommand command1;
 	SetPatternParams(command1, "file");
diff --git a/cpp/test/rascsi_executor_test.cpp b/cpp/test/rascsi_executor_test.cpp
index 17d3f75d..f16e05ee 100644
--- a/cpp/test/rascsi_executor_test.cpp
+++ b/cpp/test/rascsi_executor_test.cpp
@@ -40,12 +40,10 @@ TEST_F(RascsiExecutorTest, ProcessDeviceCmd)
 	const int LUN = 0;
 
 	auto bus = make_shared<MockBus>();
-	DeviceFactory device_factory;
-	MockAbstractController controller(bus, ID);
-	ControllerManager controller_manager(bus);
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockAbstractController controller(controller_manager, ID);
 	RascsiImage rascsi_image;
-	RascsiResponse rascsi_response(device_factory, controller_manager, 32);
-	auto executor = make_shared<MockRascsiExecutor>(rascsi_response, rascsi_image, device_factory, controller_manager);
+	auto executor = make_shared<MockRascsiExecutor>(rascsi_image, *controller_manager);
 	PbDeviceDefinition definition;
 	PbCommand command;
 	MockCommandContext context;
@@ -68,19 +66,19 @@ TEST_F(RascsiExecutorTest, ProcessDeviceCmd)
 	EXPECT_FALSE(executor->ProcessDeviceCmd(context, definition, command, true)) << "Operation for unknown device type must fail";
 
 	auto device1 = make_shared<MockPrimaryDevice>(LUN);
-	EXPECT_TRUE(controller_manager.AttachToScsiController(ID, device1));
+	EXPECT_TRUE(controller_manager->AttachToScsiController(ID, device1));
 
 	definition.set_type(SCHS);
 	command.set_operation(INSERT);
 	EXPECT_FALSE(executor->ProcessDeviceCmd(context, definition, command, true)) << "Operation unsupported by device must fail";
-	controller_manager.DeleteAllControllers();
+	controller_manager->DeleteAllControllers();
 	definition.set_type(SCRM);
 
 	auto device2 = make_shared<MockSCSIHD_NEC>(LUN);
 	device2->SetRemovable(true);
 	device2->SetProtectable(true);
 	device2->SetReady(true);
-	EXPECT_TRUE(controller_manager.AttachToScsiController(ID, device2));
+	EXPECT_TRUE(controller_manager->AttachToScsiController(ID, device2));
 
 	command.set_operation(ATTACH);
 	EXPECT_FALSE(executor->ProcessDeviceCmd(context, definition, command, true)) << "ID and LUN already exist";
@@ -113,7 +111,7 @@ TEST_F(RascsiExecutorTest, ProcessDeviceCmd)
 	command.set_operation(DETACH);
 	EXPECT_TRUE(executor->ProcessDeviceCmd(context, definition, command, true));
 	EXPECT_TRUE(executor->ProcessDeviceCmd(context, definition, command, false));
-	EXPECT_TRUE(controller_manager.AttachToScsiController(ID, device2));
+	EXPECT_TRUE(controller_manager->AttachToScsiController(ID, device2));
 
 	command.set_operation(CHECK_AUTHENTICATION);
 	EXPECT_TRUE(executor->ProcessDeviceCmd(context, definition, command, true));
@@ -152,13 +150,11 @@ TEST_F(RascsiExecutorTest, ProcessDeviceCmd)
 
 TEST_F(RascsiExecutorTest, ProcessCmd)
 {
-	shared_ptr<MockBus> bus_ptr;
-	DeviceFactory device_factory;
-	MockAbstractController controller(bus_ptr, 0);
-	ControllerManager controller_manager(bus_ptr);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockAbstractController controller(controller_manager, 0);
 	RascsiImage rascsi_image;
-	RascsiResponse rascsi_response(device_factory, controller_manager, 32);
-	auto executor = make_shared<MockRascsiExecutor>(rascsi_response, rascsi_image, device_factory, controller_manager);
+	auto executor = make_shared<MockRascsiExecutor>(rascsi_image, *controller_manager);
 	PbCommand command1;
 	PbCommand command2;
 	MockCommandContext context;
@@ -219,11 +215,11 @@ TEST_F(RascsiExecutorTest, ProcessCmd)
 
 TEST_F(RascsiExecutorTest, SetLogLevel)
 {
-	DeviceFactory device_factory;
-	ControllerManager controller_manager(make_shared<MockBus>());
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockAbstractController controller(controller_manager, 0);
 	RascsiImage rascsi_image;
-	RascsiResponse rascsi_response(device_factory, controller_manager, 32);
-	RascsiExecutor executor(rascsi_response, rascsi_image, device_factory, controller_manager);
+	RascsiExecutor executor(rascsi_image, *controller_manager);
 
 	EXPECT_TRUE(executor.SetLogLevel("trace"));
 	EXPECT_TRUE(executor.SetLogLevel("debug"));
@@ -240,17 +236,17 @@ TEST_F(RascsiExecutorTest, Attach)
 	const int LUN = 0;
 
 	DeviceFactory device_factory;
-	ControllerManager controller_manager(make_shared<MockBus>());
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
 	RascsiImage rascsi_image;
-	RascsiResponse rascsi_response(device_factory, controller_manager, 32);
-	RascsiExecutor executor(rascsi_response, rascsi_image, device_factory, controller_manager);
+	RascsiExecutor executor(rascsi_image, *controller_manager);
 	PbDeviceDefinition definition;
 	MockCommandContext context;
 
 	definition.set_unit(32);
 	EXPECT_FALSE(executor.Attach(context, definition, false));
 
-	auto device = device_factory.CreateDevice(controller_manager, SCHD, LUN, "");
+	auto device = device_factory.CreateDevice(SCHD, LUN, "");
 	definition.set_id(ID);
 	definition.set_unit(LUN);
 
@@ -262,7 +258,7 @@ TEST_F(RascsiExecutorTest, Attach)
 
 	definition.set_type(PbDeviceType::SCHS);
 	EXPECT_TRUE(executor.Attach(context, definition, false));
-	controller_manager.DeleteAllControllers();
+	controller_manager->DeleteAllControllers();
 
 	definition.set_type(PbDeviceType::SCHD);
 	EXPECT_FALSE(executor.Attach(context, definition, false)) << "Drive without sectors not rejected";
@@ -290,7 +286,7 @@ TEST_F(RascsiExecutorTest, Attach)
 	bool result = executor.Attach(context, definition, false);
 	remove(filename);
 	EXPECT_TRUE(result);
-	controller_manager.DeleteAllControllers();
+	controller_manager->DeleteAllControllers();
 
 	filename = CreateTempFile(513);
 	SetParam(definition, "file", filename.c_str());
@@ -315,20 +311,20 @@ TEST_F(RascsiExecutorTest, Attach)
 	remove(filename);
 	EXPECT_TRUE(result);
 
-	controller_manager.DeleteAllControllers();
+	controller_manager->DeleteAllControllers();
 }
 
 TEST_F(RascsiExecutorTest, Insert)
 {
 	DeviceFactory device_factory;
-	ControllerManager controller_manager(make_shared<MockBus>());
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
 	RascsiImage rascsi_image;
-	RascsiResponse rascsi_response(device_factory, controller_manager, 32);
-	RascsiExecutor executor(rascsi_response, rascsi_image, device_factory, controller_manager);
+	RascsiExecutor executor(rascsi_image, *controller_manager);
 	PbDeviceDefinition definition;
 	MockCommandContext context;
 
-	auto device = device_factory.CreateDevice(controller_manager, SCRM, 0, "test");
+	auto device = device_factory.CreateDevice(SCRM, 0, "test");
 
 	device->SetRemoved(false);
 	EXPECT_FALSE(executor.Insert(context, definition, device, false)) << "Medium is not removed";
@@ -381,23 +377,23 @@ TEST_F(RascsiExecutorTest, Detach)
 	const int LUN2 = 1;
 
 	DeviceFactory device_factory;
-	ControllerManager controller_manager(make_shared<MockBus>());
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
 	RascsiImage rascsi_image;
-	RascsiResponse rascsi_response(device_factory, controller_manager, 32);
-	RascsiExecutor executor(rascsi_response, rascsi_image, device_factory, controller_manager);
+	RascsiExecutor executor(rascsi_image, *controller_manager);
 	MockCommandContext context;
 
-	auto device1 = device_factory.CreateDevice(controller_manager, SCHS, LUN1, "");
-	EXPECT_TRUE(controller_manager.AttachToScsiController(ID, device1));
-	auto device2 = device_factory.CreateDevice(controller_manager, SCHS, LUN2, "");
-	EXPECT_TRUE(controller_manager.AttachToScsiController(ID, device2));
+	auto device1 = device_factory.CreateDevice(SCHS, LUN1, "");
+	EXPECT_TRUE(controller_manager->AttachToScsiController(ID, device1));
+	auto device2 = device_factory.CreateDevice(SCHS, LUN2, "");
+	EXPECT_TRUE(controller_manager->AttachToScsiController(ID, device2));
 
-	auto d1 = controller_manager.GetDeviceByIdAndLun(ID, LUN1);
+	auto d1 = controller_manager->GetDeviceByIdAndLun(ID, LUN1);
 	EXPECT_FALSE(executor.Detach(context, d1, false)) << "LUNs > 0 have to be detached first";
-	auto d2 = controller_manager.GetDeviceByIdAndLun(ID, LUN2);
+	auto d2 = controller_manager->GetDeviceByIdAndLun(ID, LUN2);
 	EXPECT_TRUE(executor.Detach(context, d2, false));
 	EXPECT_TRUE(executor.Detach(context, d1, false));
-	EXPECT_TRUE(controller_manager.GetAllDevices().empty());
+	EXPECT_TRUE(controller_manager->GetAllDevices().empty());
 
 	EXPECT_FALSE(executor.Detach(context, d1, false));
 }
@@ -407,28 +403,27 @@ TEST_F(RascsiExecutorTest, DetachAll)
 	const int ID = 4;
 
 	DeviceFactory device_factory;
-	ControllerManager controller_manager(make_shared<MockBus>());
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
 	RascsiImage rascsi_image;
-	RascsiResponse rascsi_response(device_factory, controller_manager, 32);
-	RascsiExecutor executor(rascsi_response, rascsi_image, device_factory, controller_manager);
+	RascsiExecutor executor(rascsi_image, *controller_manager);
 
-	auto device = device_factory.CreateDevice(controller_manager, SCHS, 0, "");
-	EXPECT_TRUE(controller_manager.AttachToScsiController(ID, device));
-	EXPECT_NE(nullptr, controller_manager.FindController(ID));
-	EXPECT_FALSE(controller_manager.GetAllDevices().empty());
+	auto device = device_factory.CreateDevice(SCHS, 0, "");
+	EXPECT_TRUE(controller_manager->AttachToScsiController(ID, device));
+	EXPECT_NE(nullptr, controller_manager->FindController(ID));
+	EXPECT_FALSE(controller_manager->GetAllDevices().empty());
 
 	executor.DetachAll();
-	EXPECT_EQ(nullptr, controller_manager.FindController(ID));
-	EXPECT_TRUE(controller_manager.GetAllDevices().empty());
+	EXPECT_EQ(nullptr, controller_manager->FindController(ID));
+	EXPECT_TRUE(controller_manager->GetAllDevices().empty());
 }
 
 TEST_F(RascsiExecutorTest, ShutDown)
 {
-	DeviceFactory device_factory;
-	ControllerManager controller_manager(make_shared<MockBus>());
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
 	RascsiImage rascsi_image;
-	RascsiResponse rascsi_response(device_factory, controller_manager, 32);
-	RascsiExecutor executor(rascsi_response, rascsi_image, device_factory, controller_manager);
+	RascsiExecutor executor(rascsi_image, *controller_manager);
 	MockCommandContext context;
 
 	EXPECT_FALSE(executor.ShutDown(context, ""));
@@ -441,10 +436,10 @@ TEST_F(RascsiExecutorTest, ShutDown)
 TEST_F(RascsiExecutorTest, SetReservedIds)
 {
 	DeviceFactory device_factory;
-	ControllerManager controller_manager(make_shared<MockBus>());
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
 	RascsiImage rascsi_image;
-	RascsiResponse rascsi_response(device_factory, controller_manager, 32);
-	RascsiExecutor executor(rascsi_response, rascsi_image, device_factory, controller_manager);
+	RascsiExecutor executor(rascsi_image, *controller_manager);
 
 	string error = executor.SetReservedIds("xyz");
 	EXPECT_FALSE(error.empty());
@@ -472,8 +467,8 @@ TEST_F(RascsiExecutorTest, SetReservedIds)
 	EXPECT_NE(reserved_ids.end(), reserved_ids.find(5));
 	EXPECT_NE(reserved_ids.end(), reserved_ids.find(7));
 
-	auto device = device_factory.CreateDevice(controller_manager, SCHS, 0, "");
-	EXPECT_TRUE(controller_manager.AttachToScsiController(5, device));
+	auto device = device_factory.CreateDevice(SCHS, 0, "");
+	EXPECT_TRUE(controller_manager->AttachToScsiController(5, device));
 	error = executor.SetReservedIds("5");
 	EXPECT_FALSE(error.empty());
 }
@@ -481,14 +476,14 @@ TEST_F(RascsiExecutorTest, SetReservedIds)
 TEST_F(RascsiExecutorTest, ValidateImageFile)
 {
 	DeviceFactory device_factory;
-	ControllerManager controller_manager(make_shared<MockBus>());
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
 	RascsiImage rascsi_image;
-	RascsiResponse rascsi_response(device_factory, controller_manager, 32);
-	RascsiExecutor executor(rascsi_response, rascsi_image, device_factory, controller_manager);
+	RascsiExecutor executor(rascsi_image, *controller_manager);
 	MockCommandContext context;
 
 	string full_path;
-	auto device = dynamic_pointer_cast<StorageDevice>(device_factory.CreateDevice(controller_manager, SCHD, 0, "test"));
+	auto device = dynamic_pointer_cast<StorageDevice>(device_factory.CreateDevice(SCHD, 0, "test"));
 	EXPECT_TRUE(executor.ValidateImageFile(context, *device, "", full_path));
 	EXPECT_TRUE(full_path.empty());
 
@@ -499,10 +494,10 @@ TEST_F(RascsiExecutorTest, ValidateImageFile)
 TEST_F(RascsiExecutorTest, ValidateLunSetup)
 {
 	DeviceFactory device_factory;
-	ControllerManager controller_manager(make_shared<MockBus>());
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
 	RascsiImage rascsi_image;
-	RascsiResponse rascsi_response(device_factory, controller_manager, 32);
-	RascsiExecutor executor(rascsi_response, rascsi_image, device_factory, controller_manager);
+	RascsiExecutor executor(rascsi_image, *controller_manager);
 	PbCommand command;
 
 	auto device1 = command.add_devices();
@@ -514,8 +509,8 @@ TEST_F(RascsiExecutorTest, ValidateLunSetup)
 	error = executor.ValidateLunSetup(command);
 	EXPECT_FALSE(error.empty());
 
-	auto device2 = device_factory.CreateDevice(controller_manager, SCHS, 0, "");
-	EXPECT_TRUE(controller_manager.AttachToScsiController(0, device2));
+	auto device2 = device_factory.CreateDevice(SCHS, 0, "");
+	EXPECT_TRUE(controller_manager->AttachToScsiController(0, device2));
 	error = executor.ValidateLunSetup(command);
 	EXPECT_TRUE(error.empty());
 }
@@ -527,26 +522,25 @@ TEST_F(RascsiExecutorTest, VerifyExistingIdAndLun)
 	const int LUN2 = 3;
 
 	DeviceFactory device_factory;
-	ControllerManager controller_manager(make_shared<MockBus>());
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
 	RascsiImage rascsi_image;
-	RascsiResponse rascsi_response(device_factory, controller_manager, 32);
-	RascsiExecutor executor(rascsi_response, rascsi_image, device_factory, controller_manager);
+	RascsiExecutor executor(rascsi_image, *controller_manager);
 	MockCommandContext context;
 
 	EXPECT_FALSE(executor.VerifyExistingIdAndLun(context, ID, LUN1));
-	auto device = device_factory.CreateDevice(controller_manager, SCHS, LUN1, "");
-	EXPECT_TRUE(controller_manager.AttachToScsiController(ID, device));
+	auto device = device_factory.CreateDevice(SCHS, LUN1, "");
+	EXPECT_TRUE(controller_manager->AttachToScsiController(ID, device));
 	EXPECT_TRUE(executor.VerifyExistingIdAndLun(context, ID, LUN1));
 	EXPECT_FALSE(executor.VerifyExistingIdAndLun(context, ID, LUN2));
 }
 
 TEST_F(RascsiExecutorTest, CreateDevice)
 {
-	DeviceFactory device_factory;
-	ControllerManager controller_manager(make_shared<MockBus>());
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
 	RascsiImage rascsi_image;
-	RascsiResponse rascsi_response(device_factory, controller_manager, 32);
-	RascsiExecutor executor(rascsi_response, rascsi_image, device_factory, controller_manager);
+	RascsiExecutor executor(rascsi_image, *controller_manager);
 	MockCommandContext context;
 
 	EXPECT_EQ(nullptr, executor.CreateDevice(context, UNDEFINED, 0, ""));
@@ -560,11 +554,10 @@ TEST_F(RascsiExecutorTest, CreateDevice)
 
 TEST_F(RascsiExecutorTest, SetSectorSize)
 {
-	DeviceFactory device_factory;
-	ControllerManager controller_manager(make_shared<MockBus>());
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
 	RascsiImage rascsi_image;
-	RascsiResponse rascsi_response(device_factory, controller_manager, 32);
-	RascsiExecutor executor(rascsi_response, rascsi_image, device_factory, controller_manager);
+	RascsiExecutor executor(rascsi_image, *controller_manager);
 	MockCommandContext context;
 
 	unordered_set<uint32_t> sizes;
@@ -580,11 +573,10 @@ TEST_F(RascsiExecutorTest, SetSectorSize)
 
 TEST_F(RascsiExecutorTest, ValidateOperationAgainstDevice)
 {
-	DeviceFactory device_factory;
-	ControllerManager controller_manager(make_shared<MockBus>());
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
 	RascsiImage rascsi_image;
-	RascsiResponse rascsi_response(device_factory, controller_manager, 32);
-	RascsiExecutor executor(rascsi_response, rascsi_image, device_factory, controller_manager);
+	RascsiExecutor executor(rascsi_image, *controller_manager);
 	MockCommandContext context;
 
 	auto device = make_shared<MockPrimaryDevice>(0);
@@ -634,11 +626,10 @@ TEST_F(RascsiExecutorTest, ValidateOperationAgainstDevice)
 
 TEST_F(RascsiExecutorTest, ValidateIdAndLun)
 {
-	DeviceFactory device_factory;
-	ControllerManager controller_manager(make_shared<MockBus>());
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
 	RascsiImage rascsi_image;
-	RascsiResponse rascsi_response(device_factory, controller_manager, 32);
-	RascsiExecutor executor(rascsi_response, rascsi_image, device_factory, controller_manager);
+	RascsiExecutor executor(rascsi_image, *controller_manager);
 	MockCommandContext context;
 
 	EXPECT_FALSE(executor.ValidateIdAndLun(context, -1, 0));
@@ -651,11 +642,10 @@ TEST_F(RascsiExecutorTest, ValidateIdAndLun)
 
 TEST_F(RascsiExecutorTest, SetProductData)
 {
-	DeviceFactory device_factory;
-	ControllerManager controller_manager(make_shared<MockBus>());
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
 	RascsiImage rascsi_image;
-	RascsiResponse rascsi_response(device_factory, controller_manager, 32);
-	RascsiExecutor executor(rascsi_response, rascsi_image, device_factory, controller_manager);
+	RascsiExecutor executor(rascsi_image, *controller_manager);
 	MockCommandContext context;
 	PbDeviceDefinition definition;
 
diff --git a/cpp/test/rascsi_response_test.cpp b/cpp/test/rascsi_response_test.cpp
index 263bbee8..f773571f 100644
--- a/cpp/test/rascsi_response_test.cpp
+++ b/cpp/test/rascsi_response_test.cpp
@@ -18,10 +18,7 @@ using namespace rascsi_interface;
 
 TEST(RascsiResponseTest, Operation_Count)
 {
-	auto bus_ptr = make_shared<MockBus>();
-	ControllerManager controller_manager(bus_ptr);
-	DeviceFactory device_factory;
-	RascsiResponse response(device_factory, controller_manager, 32);
+	RascsiResponse response;
 	PbResult result;
 
 	const auto info = response.GetOperationInfo(result, 0);
@@ -30,29 +27,32 @@ TEST(RascsiResponseTest, Operation_Count)
 
 void TestNonDiskDevice(PbDeviceType type, int default_param_count)
 {
-	auto bus_ptr = make_shared<MockBus>();
-	ControllerManager controller_manager(bus_ptr);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
 	DeviceFactory device_factory;
-	RascsiResponse response(device_factory, controller_manager, 32);
+	RascsiResponse response;
 
-	auto d = device_factory.CreateDevice(controller_manager, type, 0, "");
-	EXPECT_TRUE(controller_manager.AttachToScsiController(0, d));
+	auto d = device_factory.CreateDevice(type, 0, "");
+	const unordered_map<string, string> params;
+	d->Init(params);
+	EXPECT_TRUE(controller_manager->AttachToScsiController(0, d));
 
 	PbServerInfo info;
-	response.GetDevices(info, "image_folder");
+	response.GetDevices(controller_manager->GetAllDevices(), info, "image_folder");
 
 	EXPECT_EQ(1, info.devices_info().devices().size());
+
 	const auto& device = info.devices_info().devices()[0];
 	EXPECT_FALSE(device.properties().read_only());
 	EXPECT_FALSE(device.properties().protectable());
 	EXPECT_FALSE(device.properties().stoppable());
 	EXPECT_FALSE(device.properties().removable());
 	EXPECT_FALSE(device.properties().lockable());
-	EXPECT_EQ(0, device.params().size());
 	EXPECT_EQ(32, device.properties().luns());
 	EXPECT_EQ(0, device.block_size());
 	EXPECT_EQ(0, device.block_count());
 	EXPECT_EQ(default_param_count, device.properties().default_params().size());
+	EXPECT_EQ(default_param_count, device.params().size());
 	EXPECT_FALSE(device.properties().supports_file());
 	if (default_param_count) {
 		EXPECT_TRUE(device.properties().supports_params());
@@ -70,9 +70,7 @@ TEST(RascsiResponseTest, GetDevices)
 
 TEST(RascsiResponseTest, GetImageFile)
 {
-	ControllerManager controller_manager(make_shared<MockBus>());
-	DeviceFactory device_factory;
-	RascsiResponse response(device_factory, controller_manager, 32);
+	RascsiResponse response;
 	PbImageFile image_file;
 
 	EXPECT_FALSE(response.GetImageFile(image_file, "default_folder", ""));
@@ -85,10 +83,7 @@ TEST(RascsiResponseTest, GetImageFile)
 
 TEST(RascsiResponseTest, GetReservedIds)
 {
-	auto bus_ptr = make_shared<MockBus>();
-	ControllerManager controller_manager(bus_ptr);
-	DeviceFactory device_factory;
-	RascsiResponse response(device_factory, controller_manager, 32);
+	RascsiResponse response;
 	unordered_set<int> ids;
 	PbResult result;
 
@@ -110,20 +105,20 @@ TEST(RascsiResponseTest, GetDevicesInfo)
 	const int LUN2 = 5;
 	const int LUN3 = 6;
 
-	ControllerManager controller_manager(make_shared<MockBus>());
-	DeviceFactory device_factory;
-	RascsiResponse response(device_factory, controller_manager, 32);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	RascsiResponse response;
 	PbCommand command;
 	PbResult result;
 
-	response.GetDevicesInfo(result, command, "");
+	response.GetDevicesInfo(controller_manager->GetAllDevices(), result, command, "");
 	EXPECT_TRUE(result.status());
 	EXPECT_TRUE(result.devices_info().devices().empty());
 
-	auto device1 = make_shared<MockHostServices>(LUN1, controller_manager);
-	EXPECT_TRUE(controller_manager.AttachToScsiController(ID, device1));
+	auto device1 = make_shared<MockHostServices>(LUN1);
+	EXPECT_TRUE(controller_manager->AttachToScsiController(ID, device1));
 
-	response.GetDevicesInfo(result, command, "");
+	response.GetDevicesInfo(controller_manager->GetAllDevices(), result, command, "");
 	EXPECT_TRUE(result.status());
 	auto& devices1 = result.devices_info().devices();
 	EXPECT_EQ(1, devices1.size());
@@ -132,9 +127,9 @@ TEST(RascsiResponseTest, GetDevicesInfo)
 	EXPECT_EQ(LUN1, devices1[0].unit());
 
 	auto device2 = make_shared<MockSCSIHD_NEC>(LUN2);
-	EXPECT_TRUE(controller_manager.AttachToScsiController(ID, device2));
+	EXPECT_TRUE(controller_manager->AttachToScsiController(ID, device2));
 
-	response.GetDevicesInfo(result, command, "");
+	response.GetDevicesInfo(controller_manager->GetAllDevices(), result, command, "");
 	EXPECT_TRUE(result.status());
 	auto& devices2 = result.devices_info().devices();
 	EXPECT_EQ(2, devices2.size()) << "Data for all devices must be returned";
@@ -142,7 +137,7 @@ TEST(RascsiResponseTest, GetDevicesInfo)
 	auto requested_device = command.add_devices();
 	requested_device->set_id(ID);
 	requested_device->set_unit(LUN1);
-	response.GetDevicesInfo(result, command, "");
+	response.GetDevicesInfo(controller_manager->GetAllDevices(), result, command, "");
 	EXPECT_TRUE(result.status());
 	auto& devices3 = result.devices_info().devices();
 	EXPECT_EQ(1, devices3.size()) << "Only data for the specified ID and LUN must be returned";
@@ -152,15 +147,13 @@ TEST(RascsiResponseTest, GetDevicesInfo)
 
 	requested_device->set_id(ID);
 	requested_device->set_unit(LUN3);
-	response.GetDevicesInfo(result, command, "");
+	response.GetDevicesInfo(controller_manager->GetAllDevices(), result, command, "");
 	EXPECT_FALSE(result.status()) << "Only data for the specified ID and LUN must be returned";
 }
 
 TEST(RascsiResponseTest, GetDeviceTypesInfo)
 {
-	ControllerManager controller_manager(make_shared<MockBus>());
-	DeviceFactory device_factory;
-	RascsiResponse response(device_factory, controller_manager, 32);
+	RascsiResponse response;
 	PbResult result;
 
 	const auto& info = response.GetDeviceTypesInfo(result);
@@ -170,13 +163,14 @@ TEST(RascsiResponseTest, GetDeviceTypesInfo)
 
 TEST(RascsiResponseTest, GetServerInfo)
 {
-	ControllerManager controller_manager(make_shared<MockBus>());
-	DeviceFactory device_factory;
-	RascsiResponse response(device_factory, controller_manager, 32);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	RascsiResponse response;
+	const unordered_set<shared_ptr<PrimaryDevice>> devices;
 	const unordered_set<int> ids = { 1, 3 };
 	PbResult result;
 
-	const auto& info = response.GetServerInfo(result, ids, "log_level", "default_folder", "", "", 1234);
+	const auto& info = response.GetServerInfo(devices, result, ids, "log_level", "default_folder", "", "", 1234);
 	EXPECT_TRUE(result.status());
 	EXPECT_EQ(rascsi_major_version, info->version_info().major_version());
 	EXPECT_EQ(rascsi_minor_version, info->version_info().minor_version());
@@ -189,9 +183,7 @@ TEST(RascsiResponseTest, GetServerInfo)
 
 TEST(RascsiResponseTest, GetVersionInfo)
 {
-	ControllerManager controller_manager(make_shared<MockBus>());
-	DeviceFactory device_factory;
-	RascsiResponse response(device_factory, controller_manager, 32);
+	RascsiResponse response;
 	PbResult result;
 
 	const auto& info = response.GetVersionInfo(result);
@@ -203,9 +195,7 @@ TEST(RascsiResponseTest, GetVersionInfo)
 
 TEST(RascsiResponseTest, GetLogLevelInfo)
 {
-	ControllerManager controller_manager(make_shared<MockBus>());
-	DeviceFactory device_factory;
-	RascsiResponse response(device_factory, controller_manager, 32);
+	RascsiResponse response;
 	PbResult result;
 
 	const auto& info = response.GetLogLevelInfo(result, "level");
@@ -216,9 +206,7 @@ TEST(RascsiResponseTest, GetLogLevelInfo)
 
 TEST(RascsiResponseTest, GetNetworkInterfacesInfo)
 {
-	ControllerManager controller_manager(make_shared<MockBus>());
-	DeviceFactory device_factory;
-	RascsiResponse response(device_factory, controller_manager, 32);
+	RascsiResponse response;
 	PbResult result;
 
 	const auto& info = response.GetNetworkInterfacesInfo(result);
@@ -228,9 +216,7 @@ TEST(RascsiResponseTest, GetNetworkInterfacesInfo)
 
 TEST(RascsiResponseTest, GetMappingInfo)
 {
-	ControllerManager controller_manager(make_shared<MockBus>());
-	DeviceFactory device_factory;
-	RascsiResponse response(device_factory, controller_manager, 32);
+	RascsiResponse response;
 	PbResult result;
 
 	const auto& info = response.GetMappingInfo(result);
diff --git a/cpp/test/scsi_command_util_test.cpp b/cpp/test/scsi_command_util_test.cpp
index a336fbb1..d198413b 100644
--- a/cpp/test/scsi_command_util_test.cpp
+++ b/cpp/test/scsi_command_util_test.cpp
@@ -21,12 +21,9 @@ TEST(ScsiCommandUtilTest, ModeSelect6)
 	vector<int> cdb(6);
 	vector<uint8_t> buf(LENGTH);
 
-	// PF (vendor-specific parameter format)
+	// PF (vendor-specific parameter format) must not fail but be ignored
 	cdb[1] = 0x00;
-	EXPECT_THAT([&] { ModeSelect(scsi_command::eCmdModeSelect6, cdb, buf, LENGTH, 0); }, Throws<scsi_exception>(AllOf(
-			Property(&scsi_exception::get_sense_key, sense_key::ILLEGAL_REQUEST),
-			Property(&scsi_exception::get_asc, asc::INVALID_FIELD_IN_PARAMETER_LIST))))
-		<< "Vendor-specific parameters are not supported";
+	ModeSelect(scsi_command::eCmdModeSelect6, cdb, buf, LENGTH, 0);
 
 	cdb[0] = 0x15;
 	// PF (standard parameter format)
@@ -71,12 +68,9 @@ TEST(ScsiCommandUtilTest, ModeSelect10)
 	vector<int> cdb(10);
 	vector<uint8_t> buf(LENGTH);
 
-	// PF (vendor-specific parameter format)
+	// PF (vendor-specific parameter format) must not fail but be ignored
 	cdb[1] = 0x00;
-	EXPECT_THAT([&] { ModeSelect(scsi_command::eCmdModeSelect10, cdb, buf, LENGTH, 0); }, Throws<scsi_exception>(AllOf(
-			Property(&scsi_exception::get_sense_key, sense_key::ILLEGAL_REQUEST),
-			Property(&scsi_exception::get_asc, asc::INVALID_FIELD_IN_PARAMETER_LIST))))
-		<< "Vendor-specific parameters are not supported";
+	ModeSelect(scsi_command::eCmdModeSelect10, cdb, buf, LENGTH, 0);
 
 	// PF (standard parameter format)
 	cdb[1] = 0x10;
diff --git a/cpp/test/scsi_controller_test.cpp b/cpp/test/scsi_controller_test.cpp
index 6306228b..88a4c315 100644
--- a/cpp/test/scsi_controller_test.cpp
+++ b/cpp/test/scsi_controller_test.cpp
@@ -19,7 +19,9 @@ TEST(ScsiControllerTest, GetInitiatorId)
 {
 	const int ID = 2;
 
-	MockScsiController controller(make_shared<NiceMock<MockBus>>());
+	auto bus = make_shared<NiceMock<MockBus>>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockScsiController controller(controller_manager, 0);
 
 	controller.Process(ID);
 	EXPECT_EQ(ID, controller.GetInitiatorId());
@@ -30,7 +32,8 @@ TEST(ScsiControllerTest, GetInitiatorId)
 TEST(ScsiControllerTest, Process)
 {
 	auto bus = make_shared<NiceMock<MockBus>>();
-	MockScsiController controller(bus);
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockScsiController controller(controller_manager, 0);
 
 	controller.SetPhase(BUS::phase_t::reserved);
 	ON_CALL(*bus, GetRST).WillByDefault(Return(true));
@@ -56,7 +59,9 @@ TEST(ScsiControllerTest, Process)
 
 TEST(ScsiControllerTest, BusFree)
 {
-	MockScsiController controller(make_shared<NiceMock<MockBus>>());
+	auto bus = make_shared<NiceMock<MockBus>>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockScsiController controller(controller_manager, 0);
 
 	controller.SetPhase(BUS::phase_t::busfree);
 	controller.BusFree();
@@ -88,63 +93,65 @@ TEST(ScsiControllerTest, BusFree)
 TEST(ScsiControllerTest, Selection)
 {
 	auto bus = make_shared<NiceMock<MockBus>>();
-	MockScsiController controller(bus, 0);
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockScsiController>(controller_manager, 0);
 
-	controller.SetPhase(BUS::phase_t::selection);
+	controller->SetPhase(BUS::phase_t::selection);
 	ON_CALL(*bus, GetSEL).WillByDefault(Return(true));
 	ON_CALL(*bus, GetBSY).WillByDefault(Return(true));
 	EXPECT_CALL(*bus, GetATN).Times(0);
-	controller.Selection();
-	EXPECT_EQ(BUS::phase_t::selection, controller.GetPhase());
+	controller->Selection();
+	EXPECT_EQ(BUS::phase_t::selection, controller->GetPhase());
 
 	ON_CALL(*bus, GetSEL).WillByDefault(Return(true));
 	ON_CALL(*bus, GetBSY).WillByDefault(Return(false));
 	EXPECT_CALL(*bus, GetATN).Times(0);
-	EXPECT_CALL(controller, Status);
-	controller.Selection();
-	EXPECT_EQ(BUS::phase_t::selection, controller.GetPhase());
+	EXPECT_CALL(*controller, Status);
+	controller->Selection();
+	EXPECT_EQ(BUS::phase_t::selection, controller->GetPhase());
 
 	ON_CALL(*bus, GetSEL).WillByDefault(Return(false));
 	ON_CALL(*bus, GetBSY).WillByDefault(Return(false));
 	EXPECT_CALL(*bus, GetATN).Times(0);
-	controller.Selection();
-	EXPECT_EQ(BUS::phase_t::selection, controller.GetPhase());
+	controller->Selection();
+	EXPECT_EQ(BUS::phase_t::selection, controller->GetPhase());
 
 	ON_CALL(*bus, GetSEL).WillByDefault(Return(false));
 	ON_CALL(*bus, GetBSY).WillByDefault(Return(true));
 	ON_CALL(*bus, GetATN).WillByDefault(Return(false));
 	EXPECT_CALL(*bus, GetATN);
-	controller.Selection();
-	EXPECT_EQ(BUS::phase_t::command, controller.GetPhase());
+	controller->Selection();
+	EXPECT_EQ(BUS::phase_t::command, controller->GetPhase());
 
-	controller.SetPhase(BUS::phase_t::selection);
+	controller->SetPhase(BUS::phase_t::selection);
 	ON_CALL(*bus, GetSEL).WillByDefault(Return(false));
 	ON_CALL(*bus, GetBSY).WillByDefault(Return(true));
 	ON_CALL(*bus, GetATN).WillByDefault(Return(true));
 	EXPECT_CALL(*bus, GetATN);
-	controller.Selection();
-	EXPECT_EQ(BUS::phase_t::msgout, controller.GetPhase());
+	controller->Selection();
+	EXPECT_EQ(BUS::phase_t::msgout, controller->GetPhase());
 
-	controller.SetPhase(BUS::phase_t::reserved);
+	controller->SetPhase(BUS::phase_t::reserved);
 	ON_CALL(*bus, GetDAT).WillByDefault(Return(0));
-	controller.Selection();
-	EXPECT_EQ(BUS::phase_t::reserved, controller.GetPhase());
+	controller->Selection();
+	EXPECT_EQ(BUS::phase_t::reserved, controller->GetPhase());
 
 	ON_CALL(*bus, GetDAT).WillByDefault(Return(1));
-	controller.Selection();
-	EXPECT_EQ(BUS::phase_t::reserved, controller.GetPhase()) << "There is no device that can be selected";
+	controller->Selection();
+	EXPECT_EQ(BUS::phase_t::reserved, controller->GetPhase()) << "There is no device that can be selected";
 
 	auto device = make_shared<MockPrimaryDevice>(0);
-	controller.AddDevice(device);
+	controller->AddDevice(device);
 	EXPECT_CALL(*bus, SetBSY(true));
-	controller.Selection();
-	EXPECT_EQ(BUS::phase_t::selection, controller.GetPhase());
+	controller->Selection();
+	EXPECT_EQ(BUS::phase_t::selection, controller->GetPhase());
 }
 
 TEST(ScsiControllerTest, Command)
 {
 	auto bus = make_shared<NiceMock<MockBus>>();
-	MockScsiController controller(bus, 0);
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockScsiController controller(controller_manager, 0);
 
 	controller.SetPhase(BUS::phase_t::command);
 	EXPECT_CALL(controller, Status);
@@ -171,7 +178,8 @@ TEST(ScsiControllerTest, Command)
 TEST(ScsiControllerTest, MsgIn)
 {
 	auto bus = make_shared<NiceMock<MockBus>>();
-	MockScsiController controller(bus, 0);
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockScsiController controller(controller_manager, 0);
 
 	controller.SetPhase(BUS::phase_t::reserved);
 	EXPECT_CALL(*bus, SetMSG(true));
@@ -186,7 +194,8 @@ TEST(ScsiControllerTest, MsgIn)
 TEST(ScsiControllerTest, MsgOut)
 {
 	auto bus = make_shared<NiceMock<MockBus>>();
-	MockScsiController controller(bus, 0);
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockScsiController controller(controller_manager, 0);
 
 	controller.SetPhase(BUS::phase_t::reserved);
 	EXPECT_CALL(*bus, SetMSG(true));
@@ -201,7 +210,8 @@ TEST(ScsiControllerTest, MsgOut)
 TEST(ScsiControllerTest, DataIn)
 {
 	auto bus = make_shared<NiceMock<MockBus>>();
-	MockScsiController controller(bus, 0);
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockScsiController controller(controller_manager, 0);
 
 	controller.SetPhase(BUS::phase_t::reserved);
 	controller.SetLength(0);
@@ -221,7 +231,8 @@ TEST(ScsiControllerTest, DataIn)
 TEST(ScsiControllerTest, DataOut)
 {
 	auto bus = make_shared<NiceMock<MockBus>>();
-	MockScsiController controller(bus, 0);
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockScsiController controller(controller_manager, 0);
 
 	controller.SetPhase(BUS::phase_t::reserved);
 	controller.SetLength(0);
@@ -241,7 +252,8 @@ TEST(ScsiControllerTest, DataOut)
 TEST(ScsiControllerTest, Error)
 {
 	auto bus = make_shared<NiceMock<MockBus>>();
-	MockScsiController controller(bus, 0);
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	MockScsiController controller(controller_manager, 0);
 
 	ON_CALL(*bus, GetRST).WillByDefault(Return(true));
 	controller.SetPhase(BUS::phase_t::reserved);
@@ -283,19 +295,23 @@ TEST(ScsiControllerTest, Error)
 
 TEST(ScsiControllerTest, RequestSense)
 {
-	MockScsiController controller(make_shared<NiceMock<MockBus>>());
+	auto bus = make_shared<NiceMock<MockBus>>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockScsiController>(controller_manager, 0);
 	auto device = make_shared<MockPrimaryDevice>(0);
+	const unordered_map<string, string> params;
+	device->Init(params);
 
-    controller.AddDevice(device);
+	controller->AddDevice(device);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 	// ALLOCATION LENGTH
 	cmd[4] = 255;
 	// Non-existing LUN
 	cmd[1] = 0x20;
 
 	device->SetReady(true);
-	EXPECT_CALL(controller, Status);
-	EXPECT_TRUE(device->Dispatch(scsi_command::eCmdRequestSense));
-	EXPECT_EQ(status::GOOD, controller.GetStatus()) << "Illegal CHECK CONDITION for non-exsting LUN";
+	EXPECT_CALL(*controller, Status);
+	device->Dispatch(scsi_command::eCmdRequestSense);
+	EXPECT_EQ(status::GOOD, controller->GetStatus()) << "Wrong CHECK CONDITION for non-existing LUN";
 }
diff --git a/cpp/test/scsi_daynaport_test.cpp b/cpp/test/scsi_daynaport_test.cpp
index aaf53184..12fab173 100644
--- a/cpp/test/scsi_daynaport_test.cpp
+++ b/cpp/test/scsi_daynaport_test.cpp
@@ -16,31 +16,27 @@ TEST(ScsiDaynaportTest, Inquiry)
 	TestInquiry(SCDP, device_type::PROCESSOR, scsi_level::SCSI_2, "Dayna   SCSI/Link       1.4a", 0x20, false);
 }
 
-TEST(ScsiDaynaportTest, Dispatch)
-{
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto daynaport = CreateDevice(SCDP, controller);
-
-	EXPECT_FALSE(daynaport->Dispatch(scsi_command::eCmdIcd)) << "Command is not supported by this class";
-}
-
 TEST(ScsiDaynaportTest, TestUnitReady)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto daynaport = CreateDevice(SCDP, controller);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
+	auto daynaport = CreateDevice(SCDP, *controller);
 
-    EXPECT_CALL(controller, Status());
-    EXPECT_TRUE(daynaport->Dispatch(scsi_command::eCmdTestUnitReady)) << "TEST UNIT READY must never fail";
-    EXPECT_EQ(status::GOOD, controller.GetStatus());
+    EXPECT_CALL(*controller, Status());
+    daynaport->Dispatch(scsi_command::eCmdTestUnitReady);
+    EXPECT_EQ(status::GOOD, controller->GetStatus());
 }
 
 TEST(ScsiDaynaportTest, Read)
 {
 	vector<uint8_t> buf(0);
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto daynaport = dynamic_pointer_cast<SCSIDaynaPort>(CreateDevice(SCDP, controller));
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
+	auto daynaport = dynamic_pointer_cast<SCSIDaynaPort>(CreateDevice(SCDP, *controller));
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 
 	// ALLOCATION LENGTH
 	cmd[4] = 1;
@@ -50,10 +46,12 @@ TEST(ScsiDaynaportTest, Read)
 TEST(ScsiDaynaportTest, WriteBytes)
 {
 	vector<uint8_t> buf(0);
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto daynaport = dynamic_pointer_cast<SCSIDaynaPort>(CreateDevice(SCDP, controller));
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
+	auto daynaport = dynamic_pointer_cast<SCSIDaynaPort>(CreateDevice(SCDP, *controller));
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 
 	// Unknown data format
 	cmd[5] = 0xff;
@@ -62,10 +60,12 @@ TEST(ScsiDaynaportTest, WriteBytes)
 
 TEST(ScsiDaynaportTest, Read6)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto daynaport = CreateDevice(SCDP, controller);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
+	auto daynaport = CreateDevice(SCDP, *controller);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 
 	cmd[5] = 0xff;
     EXPECT_THAT([&] { daynaport->Dispatch(scsi_command::eCmdRead6); }, Throws<scsi_exception>(AllOf(
@@ -76,10 +76,12 @@ TEST(ScsiDaynaportTest, Read6)
 
 TEST(ScsiDaynaportTest, Write6)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto daynaport = CreateDevice(SCDP, controller);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
+	auto daynaport = CreateDevice(SCDP, *controller);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 
 	cmd[5] = 0x00;
     EXPECT_THAT([&] { daynaport->Dispatch(scsi_command::eCmdWrite6); }, Throws<scsi_exception>(AllOf(
@@ -106,23 +108,27 @@ TEST(ScsiDaynaportTest, Write6)
 
 TEST(ScsiDaynaportTest, TestRetrieveStats)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto daynaport = CreateDevice(SCDP, controller);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
+	auto daynaport = CreateDevice(SCDP, *controller);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 
     // ALLOCATION LENGTH
     cmd[4] = 255;
-    EXPECT_CALL(controller, DataIn());
-	EXPECT_TRUE(daynaport->Dispatch(scsi_command::eCmdRetrieveStats));
+    EXPECT_CALL(*controller, DataIn());
+	daynaport->Dispatch(scsi_command::eCmdRetrieveStats);
 }
 
 TEST(ScsiDaynaportTest, SetInterfaceMode)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto daynaport = CreateDevice(SCDP, controller);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
+	auto daynaport = CreateDevice(SCDP, *controller);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 
 	// Unknown interface command
     EXPECT_THAT([&] { daynaport->Dispatch(scsi_command::eCmdSetIfaceMode); }, Throws<scsi_exception>(AllOf(
@@ -131,13 +137,13 @@ TEST(ScsiDaynaportTest, SetInterfaceMode)
 
 	// Not implemented, do nothing
 	cmd[5] = SCSIDaynaPort::CMD_SCSILINK_SETMODE;
-	EXPECT_CALL(controller, Status());
-	EXPECT_TRUE(daynaport->Dispatch(scsi_command::eCmdSetIfaceMode));
-	EXPECT_EQ(status::GOOD, controller.GetStatus());
+	EXPECT_CALL(*controller, Status());
+	daynaport->Dispatch(scsi_command::eCmdSetIfaceMode);
+	EXPECT_EQ(status::GOOD, controller->GetStatus());
 
 	cmd[5] = SCSIDaynaPort::CMD_SCSILINK_SETMAC;
-	EXPECT_CALL(controller, DataOut());
-	EXPECT_TRUE(daynaport->Dispatch(scsi_command::eCmdSetIfaceMode));
+	EXPECT_CALL(*controller, DataOut());
+	daynaport->Dispatch(scsi_command::eCmdSetIfaceMode);
 
 	// Not implemented
 	cmd[5] = SCSIDaynaPort::CMD_SCSILINK_STATS;
@@ -160,10 +166,12 @@ TEST(ScsiDaynaportTest, SetInterfaceMode)
 
 TEST(ScsiDaynaportTest, SetMcastAddr)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto daynaport = CreateDevice(SCDP, controller);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
+	auto daynaport = CreateDevice(SCDP, *controller);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 
 	EXPECT_THAT([&] { daynaport->Dispatch(scsi_command::eCmdSetMcastAddr); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::ILLEGAL_REQUEST),
@@ -171,16 +179,18 @@ TEST(ScsiDaynaportTest, SetMcastAddr)
 		<< "Length of 0 is not supported";
 
 	cmd[4] = 1;
-    EXPECT_CALL(controller, DataOut());
-	EXPECT_TRUE(daynaport->Dispatch(scsi_command::eCmdSetMcastAddr));
+    EXPECT_CALL(*controller, DataOut());
+	daynaport->Dispatch(scsi_command::eCmdSetMcastAddr);
 }
 
 TEST(ScsiDaynaportTest, EnableInterface)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto daynaport = CreateDevice(SCDP, controller);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
+	auto daynaport = CreateDevice(SCDP, *controller);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 
 	// Enable
 	EXPECT_THAT([&] { daynaport->Dispatch(scsi_command::eCmdEnableInterface); }, Throws<scsi_exception>(AllOf(
@@ -196,7 +206,9 @@ TEST(ScsiDaynaportTest, EnableInterface)
 
 TEST(ScsiDaynaportTest, GetSendDelay)
 {
-    SCSIDaynaPort daynaport(0);
+	SCSIDaynaPort daynaport(0);
+	const unordered_map<string, string> params;
+	daynaport.Init(params);
 
     EXPECT_EQ(6, daynaport.GetSendDelay());
 }
diff --git a/cpp/test/scsi_host_bridge_test.cpp b/cpp/test/scsi_host_bridge_test.cpp
index 7a820fea..23ca3f82 100644
--- a/cpp/test/scsi_host_bridge_test.cpp
+++ b/cpp/test/scsi_host_bridge_test.cpp
@@ -13,8 +13,3 @@ TEST(ScsiHostBridgeTest, Inquiry)
 {
 	TestInquiry(SCBR, device_type::COMMUNICATIONS, scsi_level::SCSI_2, "RaSCSI  RASCSI BRIDGE   ", 0x27, false);
 }
-
-TEST(ScsiHostBridgeTest, Dispatch)
-{
-	TestDispatch(SCBR);
-}
diff --git a/cpp/test/scsi_printer_test.cpp b/cpp/test/scsi_printer_test.cpp
index c1790241..f6f61035 100644
--- a/cpp/test/scsi_printer_test.cpp
+++ b/cpp/test/scsi_printer_test.cpp
@@ -16,8 +16,10 @@ using namespace std;
 
 TEST(ScsiPrinterTest, Init)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto printer = CreateDevice(SCLP, controller);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<NiceMock<MockAbstractController>>(controller_manager, 0);
+	auto printer = CreateDevice(SCLP, *controller);
 
 	unordered_map<string, string> params;
 	EXPECT_TRUE(printer->Init(params));
@@ -29,19 +31,16 @@ TEST(ScsiPrinterTest, Init)
 	EXPECT_TRUE(printer->Init(params));
 }
 
-TEST(ScsiPrinterTest, Dispatch)
-{
-	TestDispatch(SCLP);
-}
-
 TEST(ScsiPrinterTest, TestUnitReady)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto printer = CreateDevice(SCLP, controller);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<NiceMock<MockAbstractController>>(controller_manager, 0);
+	auto printer = CreateDevice(SCLP, *controller);
 
-    EXPECT_CALL(controller, Status());
-    EXPECT_TRUE(printer->Dispatch(scsi_command::eCmdTestUnitReady));
-    EXPECT_EQ(status::GOOD, controller.GetStatus());
+    EXPECT_CALL(*controller, Status());
+    printer->Dispatch(scsi_command::eCmdTestUnitReady);
+    EXPECT_EQ(status::GOOD, controller->GetStatus());
 }
 
 TEST(ScsiPrinterTest, Inquiry)
@@ -51,43 +50,51 @@ TEST(ScsiPrinterTest, Inquiry)
 
 TEST(ScsiPrinterTest, ReserveUnit)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto printer = CreateDevice(SCLP, controller);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<NiceMock<MockAbstractController>>(controller_manager, 0);
+	auto printer = CreateDevice(SCLP, *controller);
 
-    EXPECT_CALL(controller, Status()).Times(1);
-    EXPECT_TRUE(printer->Dispatch(scsi_command::eCmdReserve6));
-    EXPECT_EQ(status::GOOD, controller.GetStatus());
+    EXPECT_CALL(*controller, Status()).Times(1);
+    printer->Dispatch(scsi_command::eCmdReserve6);
+    EXPECT_EQ(status::GOOD, controller->GetStatus());
 }
 
 TEST(ScsiPrinterTest, ReleaseUnit)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto printer = CreateDevice(SCLP, controller);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<NiceMock<MockAbstractController>>(controller_manager, 0);
+	auto printer = CreateDevice(SCLP, *controller);
 
-    EXPECT_CALL(controller, Status()).Times(1);
-    EXPECT_TRUE(printer->Dispatch(scsi_command::eCmdRelease6));
-    EXPECT_EQ(status::GOOD, controller.GetStatus());
+    EXPECT_CALL(*controller, Status()).Times(1);
+    printer->Dispatch(scsi_command::eCmdRelease6);
+    EXPECT_EQ(status::GOOD, controller->GetStatus());
 }
 
 TEST(ScsiPrinterTest, SendDiagnostic)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto printer = CreateDevice(SCLP, controller);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<NiceMock<MockAbstractController>>(controller_manager, 0);
+	auto printer = CreateDevice(SCLP, *controller);
 
-    EXPECT_CALL(controller, Status()).Times(1);
-    EXPECT_TRUE(printer->Dispatch(scsi_command::eCmdSendDiag));
-    EXPECT_EQ(status::GOOD, controller.GetStatus());
+    EXPECT_CALL(*controller, Status()).Times(1);
+    printer->Dispatch(scsi_command::eCmdSendDiagnostic);
+    EXPECT_EQ(status::GOOD, controller->GetStatus());
 }
 
 TEST(ScsiPrinterTest, Print)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto printer = CreateDevice(SCLP, controller);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<NiceMock<MockAbstractController>>(controller_manager, 0);
+	auto printer = CreateDevice(SCLP, *controller);
 
-	vector<int>& cmd = controller.GetCmd();
+	auto& cmd = controller->GetCmd();
 
-    EXPECT_CALL(controller, DataOut());
-    EXPECT_TRUE(printer->Dispatch(scsi_command::eCmdPrint));
+    EXPECT_CALL(*controller, DataOut());
+    printer->Dispatch(scsi_command::eCmdPrint);
 
     cmd[3] = 0xff;
     cmd[4] = 0xff;
@@ -99,18 +106,22 @@ TEST(ScsiPrinterTest, Print)
 
 TEST(ScsiPrinterTest, StopPrint)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto printer = CreateDevice(SCLP, controller);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<NiceMock<MockAbstractController>>(controller_manager, 0);
+	auto printer = CreateDevice(SCLP, *controller);
 
-    EXPECT_CALL(controller, Status());
-    EXPECT_TRUE(printer->Dispatch(scsi_command::eCmdStopPrint));
-    EXPECT_EQ(status::GOOD, controller.GetStatus());
+    EXPECT_CALL(*controller, Status());
+    printer->Dispatch(scsi_command::eCmdStopPrint);
+    EXPECT_EQ(status::GOOD, controller->GetStatus());
 }
 
 TEST(ScsiPrinterTest, SynchronizeBuffer)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto printer = CreateDevice(SCLP, controller);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<NiceMock<MockAbstractController>>(controller_manager, 0);
+	auto printer = CreateDevice(SCLP, *controller);
 
     EXPECT_THAT([&] { printer->Dispatch(scsi_command::eCmdSynchronizeBuffer); }, Throws<scsi_exception>(AllOf(
     		Property(&scsi_exception::get_sense_key, sense_key::ABORTED_COMMAND),
@@ -122,10 +133,11 @@ TEST(ScsiPrinterTest, SynchronizeBuffer)
 
 TEST(ScsiPrinterTest, WriteByteSequence)
 {
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto printer = dynamic_pointer_cast<SCSIPrinter>(CreateDevice(SCLP, controller));
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<NiceMock<MockAbstractController>>(controller_manager, 0);
+	auto printer = CreateDevice(SCLP, *controller);
 
 	vector<uint8_t> buf(1);
 	EXPECT_TRUE(printer->WriteByteSequence(buf, buf.size()));
-	printer->Cleanup();
 }
diff --git a/cpp/test/scsicd_test.cpp b/cpp/test/scsicd_test.cpp
index 76228310..bb03e1c6 100644
--- a/cpp/test/scsicd_test.cpp
+++ b/cpp/test/scsicd_test.cpp
@@ -32,11 +32,6 @@ TEST(ScsiCdTest, Inquiry)
 	TestInquiry(SCCD, device_type::CD_ROM, scsi_level::SCSI_2, "RaSCSI  SCSI CD-ROM     ", 0x1f, true);
 }
 
-TEST(ScsiCdTest, Dispatch)
-{
-	TestDispatch(SCCD);
-}
-
 TEST(ScsiCdTest, SetUpModePages)
 {
 	map<int, vector<byte>> pages;
@@ -117,11 +112,15 @@ TEST(ScsiCdTest, Open)
 
 TEST(ScsiCdTest, ReadToc)
 {
-	MockAbstractController controller(make_shared<MockBus>(), 0);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<MockAbstractController>(controller_manager, 0);
 	const unordered_set<uint32_t> sector_sizes;
 	auto cd = make_shared<MockSCSICD>(0, sector_sizes);
+	const unordered_map<string, string> params;
+	cd->Init(params);
 
-	controller.AddDevice(cd);
+	controller->AddDevice(cd);
 
 	EXPECT_THAT([&] { cd->Dispatch(scsi_command::eCmdReadToc); }, Throws<scsi_exception>(AllOf(
 			Property(&scsi_exception::get_sense_key, sense_key::NOT_READY),
diff --git a/cpp/test/scsihd_test.cpp b/cpp/test/scsihd_test.cpp
index 7459fd50..4d9d6cd9 100644
--- a/cpp/test/scsihd_test.cpp
+++ b/cpp/test/scsihd_test.cpp
@@ -117,8 +117,3 @@ TEST(ScsiHdTest, ModeSelect)
 	buf[20] = 0x02;
 	EXPECT_NO_THROW(hd.ModeSelect(scsi_command::eCmdModeSelect10, cmd, buf, 255)) << "MODE SELECT(10) is supported";
 }
-
-TEST(ScsiHdTest, Dispatch)
-{
-	TestDispatch(SCHD);
-}
diff --git a/cpp/test/scsimo_test.cpp b/cpp/test/scsimo_test.cpp
index 86ba4004..2ac0cb93 100644
--- a/cpp/test/scsimo_test.cpp
+++ b/cpp/test/scsimo_test.cpp
@@ -148,9 +148,3 @@ TEST(ScsiMoTest, ModeSelect)
 	buf[20] = 0x08;
 	EXPECT_NO_THROW(mo.ModeSelect(scsi_command::eCmdModeSelect10, cmd, buf, 255)) << "MODE SELECT(10) is supported";
 }
-
-TEST(ScsiMoTest, Dispatch)
-{
-	TestDispatch(SCMO);
-}
-
diff --git a/cpp/test/storage_device_test.cpp b/cpp/test/storage_device_test.cpp
index 651859ae..d9e4f9b3 100644
--- a/cpp/test/storage_device_test.cpp
+++ b/cpp/test/storage_device_test.cpp
@@ -151,14 +151,3 @@ TEST(StorageDeviceTest, GetFileSize)
 	device.SetFilename("/non_existing_file");
 	EXPECT_THROW(device.GetFileSize(), io_exception);
 }
-
-TEST(StorageDeviceTest, Dispatch)
-{
-    MockAbstractController controller(make_shared<MockBus>(), 0);
-	auto device = make_shared<NiceMock<MockStorageDevice>>();
-
-    controller.AddDevice(device);
-
-    EXPECT_FALSE(device->Dispatch(scsi_command::eCmdIcd)) << "Command is not supported by this class";
-}
-
diff --git a/cpp/test/test_shared.cpp b/cpp/test/test_shared.cpp
index 35569723..0892cadc 100644
--- a/cpp/test/test_shared.cpp
+++ b/cpp/test/test_shared.cpp
@@ -7,6 +7,10 @@
 //
 //---------------------------------------------------------------------------
 
+#include "mocks.h"
+#include "controllers/controller_manager.h"
+#include "rascsi_exceptions.h"
+#include "rascsi_version.h"
 #include "test_shared.h"
 #include <unistd.h>
 #include <vector>
@@ -19,9 +23,10 @@ using namespace filesystem;
 shared_ptr<PrimaryDevice> CreateDevice(PbDeviceType type, MockAbstractController& controller, const string& extension)
 {
 	DeviceFactory device_factory;
-	auto controller_manager = make_shared<ControllerManager>(controller.GetBus());
 
-	auto device = device_factory.CreateDevice(*controller_manager, type, 0, extension);
+	auto device = device_factory.CreateDevice(type, 0, extension);
+	unordered_map<string, string> params;
+	device->Init(params);
 
     controller.AddDevice(device);
 
@@ -31,16 +36,18 @@ shared_ptr<PrimaryDevice> CreateDevice(PbDeviceType type, MockAbstractController
 void TestInquiry(PbDeviceType type, device_type t, scsi_level l, const string& ident, int additional_length,
 		bool removable, const string& extension)
 {
-    NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-    auto device = CreateDevice(type, controller, extension);
+	auto bus = make_shared<MockBus>();
+	auto controller_manager = make_shared<ControllerManager>(*bus);
+	auto controller = make_shared<NiceMock<MockAbstractController>>(controller_manager, 0);
+    auto device = CreateDevice(type, *controller, extension);
 
-    vector<int>& cmd = controller.GetCmd();
+    auto& cmd = controller->GetCmd();
 
     // ALLOCATION LENGTH
     cmd[4] = 255;
-    EXPECT_CALL(controller, DataIn());
-    EXPECT_TRUE(device->Dispatch(scsi_command::eCmdInquiry));
-	const vector<uint8_t>& buffer = controller.GetBuffer();
+    EXPECT_CALL(*controller, DataIn());
+    device->Dispatch(scsi_command::eCmdInquiry);
+	const vector<uint8_t>& buffer = controller->GetBuffer();
 	EXPECT_EQ(t, static_cast<device_type>(buffer[0]));
 	EXPECT_EQ(removable ? 0x80: 0x00, buffer[1]);
 	EXPECT_EQ(l, static_cast<scsi_level>(buffer[2]));
@@ -58,14 +65,6 @@ void TestInquiry(PbDeviceType type, device_type t, scsi_level l, const string& i
 	EXPECT_TRUE(!memcmp(product_data.c_str(), &buffer[8], 28));
 }
 
-void TestDispatch(PbDeviceType type)
-{
-	NiceMock<MockAbstractController> controller(make_shared<MockBus>(), 0);
-	auto device = CreateDevice(type, controller);
-
-    EXPECT_FALSE(device->Dispatch(scsi_command::eCmdIcd)) << "Command is not supported by this class";
-}
-
 pair<int, path> OpenTempFile()
 {
 	const string filename = string(temp_directory_path()) + "/rascsi_test-XXXXXX"; //NOSONAR Publicly writable directory is fine here
diff --git a/cpp/test/test_shared.h b/cpp/test/test_shared.h
index 0bc07118..e38212f7 100644
--- a/cpp/test/test_shared.h
+++ b/cpp/test/test_shared.h
@@ -27,8 +27,6 @@ shared_ptr<PrimaryDevice> CreateDevice(PbDeviceType, MockAbstractController&, co
 void TestInquiry(PbDeviceType, scsi_defs::device_type, scsi_defs::scsi_level, const string&,
 		int, bool, const string& = "");
 
-void TestDispatch(PbDeviceType);
-
 pair<int, path> OpenTempFile();
 path CreateTempFile(int);
 
diff --git a/easyinstall.sh b/easyinstall.sh
index bfed348f..c3e5cac0 100755
--- a/easyinstall.sh
+++ b/easyinstall.sh
@@ -78,6 +78,10 @@ function initialChecks() {
 
 # checks that the current user has sudoers privileges
 function sudoCheck() {
+    if [[ $HEADLESS ]]; then
+        echo "Skipping password check in headless mode"
+        return 0
+    fi
     echo "Input your password to allow this script to make the above changes."
     sudo -v
 }
@@ -128,8 +132,6 @@ function installPackagesStandalone() {
 # cache the pip packages
 function cachePipPackages(){
     pushd $WEB_INSTALL_PATH
-    # Refresh the sudo authentication, which shouldn't trigger another password prompt
-    sudo -v
     sudo pip3 install -r ./requirements.txt
     popd
 }
@@ -141,8 +143,6 @@ function compileRaScsi() {
     echo "Compiling with ${CORES:-1} simultaneous cores..."
     make clean </dev/null
 
-    # Refresh the sudo authentication, which shouldn't trigger another password prompt
-    sudo -v
     make -j "${CORES:-1}" all CONNECT_TYPE="${CONNECT_TYPE:-FULLSPEC}" </dev/null
 }
 
@@ -615,9 +615,9 @@ function installHfdisk() {
 
 # Fetch HFS drivers that the Web Interface uses
 function fetchHardDiskDrivers() {
-    if [ ! -f "$BASE/mac-hard-disk-drivers" ]; then
+    if [ ! -d "$BASE/mac-hard-disk-drivers" ]; then
         cd "$BASE" || exit 1
-        wget https://macintoshgarden.org/sites/macintoshgarden.org/files/apps/mac-hard-disk-drivers.zip
+        wget -r https://www.dropbox.com/s/gcs4v5pcmk7rxtb/mac-hard-disk-drivers.zip?dl=0
         unzip -d mac-hard-disk-drivers mac-hard-disk-drivers.zip
         rm mac-hard-disk-drivers.zip
     fi