From 474d7ac80f033c5f4977623f6510bb083d3fb368 Mon Sep 17 00:00:00 2001
From: Michael McMaster <michael@codesrc.com>
Date: Tue, 1 Dec 2020 11:24:50 +0000
Subject: [PATCH] Refactor code to make getCapacity calls not specific to SD

---
 Makefile                                      |   1 +
 src/firmware/cdrom.c                          |  15 +-
 src/firmware/config.c                         |  24 +-
 src/firmware/device.c                         |  51 +++
 src/firmware/device.h                         |  81 +++++
 src/firmware/diagnostic.c                     |  22 +-
 src/firmware/disk.c                           |  66 ++--
 src/firmware/geometry.c                       |  10 +-
 src/firmware/geometry.h                       |   3 +-
 src/firmware/hwversion.c                      |  11 +-
 src/firmware/inquiry.c                        |   8 +-
 src/firmware/mode.c                           |  23 +-
 src/firmware/scsi.c                           | 331 ++++++------------
 src/firmware/scsi.h                           |  35 +-
 src/firmware/scsiPhy.c                        |  30 +-
 src/firmware/sd.c                             |  80 ++++-
 src/firmware/sd.h                             |  14 +-
 src/firmware/usb_device/usbd_msc_storage_sd.c | 100 +++---
 18 files changed, 490 insertions(+), 415 deletions(-)
 create mode 100644 src/firmware/device.c
 create mode 100644 src/firmware/device.h

diff --git a/Makefile b/Makefile
index df9d40c6..c92e3dbd 100644
--- a/Makefile
+++ b/Makefile
@@ -115,6 +115,7 @@ SRC = \
 	src/firmware/bsp.c \
 	src/firmware/cdrom.c \
 	src/firmware/config.c \
+	src/firmware/device.c \
 	src/firmware/disk.c \
 	src/firmware/diagnostic.c \
 	src/firmware/fpga.c \
diff --git a/src/firmware/cdrom.c b/src/firmware/cdrom.c
index c2dc148f..de94ca77 100755
--- a/src/firmware/cdrom.c
+++ b/src/firmware/cdrom.c
@@ -153,8 +153,8 @@ static void doReadTOC(int MSF, uint8_t track, uint16_t allocationLength)
 	if (track > 1)
 	{
 		scsiDev.status = CHECK_CONDITION;
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+		scsiDev.target->state.sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->state.sense.asc = INVALID_FIELD_IN_CDB;
 		scsiDev.phase = STATUS;
 	}
 	else
@@ -163,8 +163,9 @@ static void doReadTOC(int MSF, uint8_t track, uint16_t allocationLength)
 		memcpy(scsiDev.data, SimpleTOC, len);
 
 		uint32_t capacity = getScsiCapacity(
+			scsiDev.target->device,
 			scsiDev.target->cfg->sdSectorStart,
-			scsiDev.target->liveCfg.bytesPerSector,
+			scsiDev.target->state.bytesPerSector,
 			scsiDev.target->cfg->scsiSectors);
 
 		// Replace start of leadout track
@@ -214,8 +215,8 @@ static void doReadFullTOC(int convertBCD, uint8_t session, uint16_t allocationLe
 	if (session > 1)
 	{
 		scsiDev.status = CHECK_CONDITION;
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+		scsiDev.target->state.sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->state.sense.asc = INVALID_FIELD_IN_CDB;
 		scsiDev.phase = STATUS;
 	}
 	else
@@ -298,8 +299,8 @@ int scsiCDRomCommand()
 			default:
 			{
 				scsiDev.status = CHECK_CONDITION;
-				scsiDev.target->sense.code = ILLEGAL_REQUEST;
-				scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+				scsiDev.target->state.sense.code = ILLEGAL_REQUEST;
+				scsiDev.target->state.sense.asc = INVALID_FIELD_IN_CDB;
 				scsiDev.phase = STATUS;
 			}
 		}
diff --git a/src/firmware/config.c b/src/firmware/config.c
index df19f9a1..1c7b21c3 100755
--- a/src/firmware/config.c
+++ b/src/firmware/config.c
@@ -97,12 +97,12 @@ void s2s_configInit(S2S_BoardCfg* config)
 		memcpy(config, s2s_cfg, sizeof(S2S_BoardCfg));
 	}
 
-	else if ((blockDev.state & DISK_PRESENT) && sdDev.capacity)
+	else if ((blockDev.state & DISK_PRESENT) && sdCard.capacity)
 	{
 		int cfgSectors = (S2S_CFG_SIZE + 511) / 512;
 		BSP_SD_ReadBlocks_DMA(
 			(uint32_t*) &s2s_cfg[0],
-			(sdDev.capacity - cfgSectors) * 512ll,
+			(sdCard.capacity - cfgSectors) * 512ll,
 			512,
 			cfgSectors);
 
@@ -170,9 +170,9 @@ pingCommand()
 static void
 sdInfoCommand()
 {
-	uint8_t response[sizeof(sdDev.csd) + sizeof(sdDev.cid)];
-	memcpy(response, sdDev.csd, sizeof(sdDev.csd));
-	memcpy(response + sizeof(sdDev.csd), sdDev.cid, sizeof(sdDev.cid));
+	uint8_t response[sizeof(sdCard.csd) + sizeof(sdCard.cid)];
+	memcpy(response, sdCard.csd, sizeof(sdCard.csd));
+	memcpy(response + sizeof(sdCard.csd), sdCard.cid, sizeof(sdCard.cid));
 
 	hidPacket_send(response, sizeof(response));
 }
@@ -197,10 +197,10 @@ scsiDevInfoCommand()
 	{
 		FIRMWARE_VERSION >> 8,
 		FIRMWARE_VERSION & 0xff,
-		sdDev.capacity >> 24,
-		sdDev.capacity >> 16,
-		sdDev.capacity >> 8,
-		sdDev.capacity,
+		sdCard.capacity >> 24,
+		sdCard.capacity >> 16,
+		sdCard.capacity >> 8,
+		sdCard.capacity,
 		1 // useSdConfig, always true for V6.
 	};
 	hidPacket_send(response, sizeof(response));
@@ -217,8 +217,8 @@ debugCommand()
 	response[15] = scsiDev.lastSense;
 	response[16] = scsiDev.phase;
 	response[17] = *SCSI_STS_SCSI;
-	response[18] = scsiDev.target != NULL ? scsiDev.target->syncOffset : 0;
-	response[19] = scsiDev.target != NULL ? scsiDev.target->syncPeriod : 0;
+	response[18] = scsiDev.target != NULL ? scsiDev.target->state.syncOffset : 0;
+	response[19] = scsiDev.target != NULL ? scsiDev.target->state.syncPeriod : 0;
 	response[20] = scsiDev.minSyncPeriod;
 	response[21] = scsiDev.rstCount;
 	response[22] = scsiDev.selCount;
@@ -444,7 +444,7 @@ void s2s_configSave(int scsiId, uint16_t bytesPerSector)
 
 	BSP_SD_WriteBlocks_DMA(
 		(uint32_t*) &s2s_cfg[0],
-		(sdDev.capacity - S2S_CFG_SIZE) * 512ll,
+		(sdCard.capacity - S2S_CFG_SIZE) * 512ll,
 		512,
 		(S2S_CFG_SIZE + 511) / 512);
 }
diff --git a/src/firmware/device.c b/src/firmware/device.c
new file mode 100644
index 00000000..e3ff9ea0
--- /dev/null
+++ b/src/firmware/device.c
@@ -0,0 +1,51 @@
+//	Copyright (C) 2020 Michael McMaster <michael@codesrc.com>
+//
+//	This file is part of SCSI2SD.
+//
+//	SCSI2SD is free software: you can redistribute it and/or modify
+//	it under the terms of the GNU General Public License as published by
+//	the Free Software Foundation, either version 3 of the License, or
+//	(at your option) any later version.
+//
+//	SCSI2SD is distributed in the hope that it will be useful,
+//	but WITHOUT ANY WARRANTY; without even the implied warranty of
+//	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//	GNU General Public License for more details.
+//
+//	You should have received a copy of the GNU General Public License
+//	along with SCSI2SD.  If not, see <http://www.gnu.org/licenses/>.
+#include "device.h"
+
+#include "sd.h"
+
+#include <stddef.h>
+
+S2S_Target* s2s_DeviceFindByScsiId(int scsiId)
+{
+	int deviceCount;
+	S2S_Device* devices = s2s_GetDevices(&deviceCount);
+	for (int deviceIdx = 0; deviceIdx < deviceCount; ++deviceIdx)
+	{
+		int targetCount;
+		S2S_Target* targets = devices[deviceIdx].getTargets(devices + deviceIdx, &targetCount);
+		for (int targetIdx = 0; targetIdx < targetCount; ++targetIdx)
+		{
+			S2S_Target* target = targets + targetIdx;
+			if (target &&
+				(target->cfg->scsiId & S2S_CFG_TARGET_ENABLED) &&
+				((target->cfg->scsiId & 7) == scsiId))
+			{
+				return target;
+			}
+		}
+	}
+
+	return NULL;
+}
+
+S2S_Device* s2s_GetDevices(int* count)
+{
+	*count = 1;
+	return sdDevice;
+}
+
diff --git a/src/firmware/device.h b/src/firmware/device.h
new file mode 100644
index 00000000..324c266c
--- /dev/null
+++ b/src/firmware/device.h
@@ -0,0 +1,81 @@
+//	Copyright (C) 2020 Michael McMaster <michael@codesrc.com>
+//
+//	This file is part of SCSI2SD.
+//
+//	SCSI2SD is free software: you can redistribute it and/or modify
+//	it under the terms of the GNU General Public License as published by
+//	the Free Software Foundation, either version 3 of the License, or
+//	(at your option) any later version.
+//
+//	SCSI2SD is distributed in the hope that it will be useful,
+//	but WITHOUT ANY WARRANTY; without even the implied warranty of
+//	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//	GNU General Public License for more details.
+//
+//	You should have received a copy of the GNU General Public License
+//	along with SCSI2SD.  If not, see <http://www.gnu.org/licenses/>.
+#ifndef S2S_DEVICE_H
+#define S2S_DEVICE_H
+
+#include "scsi2sd.h"
+#include "sense.h"
+
+#include <stdint.h>
+
+struct S2S_DeviceStruct;
+typedef struct S2S_DeviceStruct S2S_Device;
+
+struct S2S_TargetStruct;
+typedef struct S2S_TargetStruct S2S_Target;
+
+struct S2S_TargetStateStruct;
+typedef struct S2S_TargetStateStruct S2S_TargetState;
+
+struct S2S_TargetStateStruct
+{
+	ScsiSense sense;
+
+	uint16_t unitAttention; // Set to the sense qualifier key to be returned.
+
+	// Only let the reserved initiator talk to us.
+	// A 3rd party may be sending the RESERVE/RELEASE commands
+	int reservedId; // 0 -> 7 if reserved. -1 if not reserved.
+	int reserverId; // 0 -> 7 if reserved. -1 if not reserved.
+
+	uint8_t syncOffset;
+	uint8_t syncPeriod;
+
+	// Shadow parameters, possibly not saved to flash yet.
+	// Set via Mode Select
+	uint16_t bytesPerSector;
+};
+
+struct S2S_TargetStruct
+{
+	uint8_t id;
+
+	S2S_Device* device;
+	S2S_TargetCfg* cfg;
+
+	S2S_TargetState state;
+};
+
+struct S2S_DeviceStruct
+{
+	const S2S_BoardCfg* (*getBoardConfig)(S2S_Device* dev);
+	//const S2S_Target* (*findByScsiId)(S2S_Device* dev, int scsiId);
+	S2S_Target* (*getTargets)(S2S_Device* dev, int* count);
+
+	// Get the number of 512 byte blocks
+	uint32_t (*getCapacity)(S2S_Device* dev);
+
+};
+
+void s2s_DeviceGetBoardConfig(S2S_BoardCfg* config);
+S2S_Target* s2s_DeviceFindByScsiId(int scsiId);
+
+S2S_Device* s2s_GetDevices(int* count);
+
+#endif
+
+
diff --git a/src/firmware/diagnostic.c b/src/firmware/diagnostic.c
index aeead0fc..819af1ae 100755
--- a/src/firmware/diagnostic.c
+++ b/src/firmware/diagnostic.c
@@ -50,8 +50,8 @@ void scsiSendDiagnostic()
 			// Nowhere to store this data!
 			// Shouldn't happen - our buffer should be many magnitudes larger
 			// than the required size for diagnostic parameters.
-			scsiDev.target->sense.code = ILLEGAL_REQUEST;
-			scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+			scsiDev.target->state.sense.code = ILLEGAL_REQUEST;
+			scsiDev.target->state.sense.asc = INVALID_FIELD_IN_CDB;
 			scsiDev.status = CHECK_CONDITION;
 			scsiDev.phase = STATUS;
 		}
@@ -95,14 +95,14 @@ void scsiReceiveDiagnostic()
 		// 64bit linear address, then convert back again.
 		uint64_t fromByteAddr =
 			scsiByteAddress(
-				scsiDev.target->liveCfg.bytesPerSector,
+				scsiDev.target->state.bytesPerSector,
 				scsiDev.target->cfg->headsPerCylinder,
 				scsiDev.target->cfg->sectorsPerTrack,
 				suppliedFmt,
 				&scsiDev.data[6]);
 
 		scsiSaveByteAddress(
-			scsiDev.target->liveCfg.bytesPerSector,
+			scsiDev.target->state.bytesPerSector,
 			scsiDev.target->cfg->headsPerCylinder,
 			scsiDev.target->cfg->sectorsPerTrack,
 			translateFmt,
@@ -121,8 +121,8 @@ void scsiReceiveDiagnostic()
 	{
 		// error.
 		scsiDev.status = CHECK_CONDITION;
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+		scsiDev.target->state.sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->state.sense.asc = INVALID_FIELD_IN_CDB;
 		scsiDev.phase = STATUS;
 	}
 
@@ -189,8 +189,8 @@ void scsiReadBuffer()
 	{
 		// error.
 		scsiDev.status = CHECK_CONDITION;
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+		scsiDev.target->state.sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->state.sense.asc = INVALID_FIELD_IN_CDB;
 		scsiDev.phase = STATUS;
 	}
 }
@@ -228,8 +228,8 @@ void scsiWriteBuffer()
 	{
 		// error.
 		scsiDev.status = CHECK_CONDITION;
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+		scsiDev.target->state.sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->state.sense.asc = INVALID_FIELD_IN_CDB;
 		scsiDev.phase = STATUS;
 	}
 }
@@ -239,7 +239,7 @@ void scsiWriteBuffer()
 // Section 4.3.14
 void scsiWriteSectorBuffer()
 {
-	scsiDev.dataLen = scsiDev.target->liveCfg.bytesPerSector;
+	scsiDev.dataLen = scsiDev.target->state.bytesPerSector;
 	scsiDev.phase = DATA_OUT;
 	scsiDev.postDataOutHook = doWriteBuffer;
 }
diff --git a/src/firmware/disk.c b/src/firmware/disk.c
index f1fd8c86..79951b0a 100755
--- a/src/firmware/disk.c
+++ b/src/firmware/disk.c
@@ -95,8 +95,8 @@ static void doFormatUnitHeader(void)
 	{
 		// Save the "MODE SELECT savable parameters"
 		s2s_configSave(
-			scsiDev.target->targetId,
-			scsiDev.target->liveCfg.bytesPerSector);
+			scsiDev.target->id,
+			scsiDev.target->state.bytesPerSector);
 	}
 
 	if (IP)
@@ -126,8 +126,9 @@ static void doReadCapacity()
 	int pmi = scsiDev.cdb[8] & 1;
 
 	uint32_t capacity = getScsiCapacity(
+		scsiDev.target->device,
 		scsiDev.target->cfg->sdSectorStart,
-		scsiDev.target->liveCfg.bytesPerSector,
+		scsiDev.target->state.bytesPerSector,
 		scsiDev.target->cfg->scsiSectors);
 
 	if (!pmi && lba)
@@ -137,8 +138,8 @@ static void doReadCapacity()
 		// assume that delays are constant across each block. But the spec
 		// says we must return this error if pmi is specified incorrectly.
 		scsiDev.status = CHECK_CONDITION;
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+		scsiDev.target->state.sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->state.sense.asc = INVALID_FIELD_IN_CDB;
 		scsiDev.phase = STATUS;
 	}
 	else if (capacity > 0)
@@ -150,7 +151,7 @@ static void doReadCapacity()
 		scsiDev.data[2] = highestBlock >> 8;
 		scsiDev.data[3] = highestBlock;
 
-		uint32_t bytesPerSector = scsiDev.target->liveCfg.bytesPerSector;
+		uint32_t bytesPerSector = scsiDev.target->state.bytesPerSector;
 		scsiDev.data[4] = bytesPerSector >> 24;
 		scsiDev.data[5] = bytesPerSector >> 16;
 		scsiDev.data[6] = bytesPerSector >> 8;
@@ -161,8 +162,8 @@ static void doReadCapacity()
 	else
 	{
 		scsiDev.status = CHECK_CONDITION;
-		scsiDev.target->sense.code = NOT_READY;
-		scsiDev.target->sense.asc = MEDIUM_NOT_PRESENT;
+		scsiDev.target->state.sense.code = NOT_READY;
+		scsiDev.target->state.sense.asc = MEDIUM_NOT_PRESENT;
 		scsiDev.phase = STATUS;
 	}
 }
@@ -175,19 +176,20 @@ static void doWrite(uint32_t lba, uint32_t blocks)
 		s2s_delay_ms(10);
 	}
 
-	uint32_t bytesPerSector = scsiDev.target->liveCfg.bytesPerSector;
+	uint32_t bytesPerSector = scsiDev.target->state.bytesPerSector;
 
 	if (unlikely(blockDev.state & DISK_WP) ||
 		unlikely(scsiDev.target->cfg->deviceType == S2S_CFG_OPTICAL))
 
 	{
 		scsiDev.status = CHECK_CONDITION;
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = WRITE_PROTECTED;
+		scsiDev.target->state.sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->state.sense.asc = WRITE_PROTECTED;
 		scsiDev.phase = STATUS;
 	}
 	else if (unlikely(((uint64_t) lba) + blocks >
 		getScsiCapacity(
+			scsiDev.target->device,
 			scsiDev.target->cfg->sdSectorStart,
 			bytesPerSector,
 			scsiDev.target->cfg->scsiSectors
@@ -195,8 +197,8 @@ static void doWrite(uint32_t lba, uint32_t blocks)
 		))
 	{
 		scsiDev.status = CHECK_CONDITION;
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
+		scsiDev.target->state.sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->state.sense.asc = LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
 		scsiDev.phase = STATUS;
 	}
 	else
@@ -233,14 +235,15 @@ static void doRead(uint32_t lba, uint32_t blocks)
 	}
 
 	uint32_t capacity = getScsiCapacity(
+		scsiDev.target->device,
 		scsiDev.target->cfg->sdSectorStart,
-		scsiDev.target->liveCfg.bytesPerSector,
+		scsiDev.target->state.bytesPerSector,
 		scsiDev.target->cfg->scsiSectors);
 	if (unlikely(((uint64_t) lba) + blocks > capacity))
 	{
 		scsiDev.status = CHECK_CONDITION;
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
+		scsiDev.target->state.sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->state.sense.asc = LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
 		scsiDev.phase = STATUS;
 	}
 	else
@@ -251,7 +254,7 @@ static void doRead(uint32_t lba, uint32_t blocks)
 		scsiDev.phase = DATA_IN;
 		scsiDev.dataLen = 0; // No data yet
 
-		uint32_t bytesPerSector = scsiDev.target->liveCfg.bytesPerSector;
+		uint32_t bytesPerSector = scsiDev.target->state.bytesPerSector;
 		uint32_t sdSectorPerSCSISector = SDSectorsPerSCSISector(bytesPerSector);
 		uint32_t sdSectors =
 			blocks * sdSectorPerSCSISector;
@@ -286,14 +289,15 @@ static void doSeek(uint32_t lba)
 {
 	if (lba >=
 		getScsiCapacity(
+			scsiDev.target->device,
 			scsiDev.target->cfg->sdSectorStart,
-			scsiDev.target->liveCfg.bytesPerSector,
+			scsiDev.target->state.bytesPerSector,
 			scsiDev.target->cfg->scsiSectors)
 		)
 	{
 		scsiDev.status = CHECK_CONDITION;
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
+		scsiDev.target->state.sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->state.sense.asc = LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
 		scsiDev.phase = STATUS;
 	}
 	else
@@ -313,24 +317,24 @@ static int doTestUnitReady()
 	{
 		ready = 0;
 		scsiDev.status = CHECK_CONDITION;
-		scsiDev.target->sense.code = NOT_READY;
-		scsiDev.target->sense.asc = LOGICAL_UNIT_NOT_READY_INITIALIZING_COMMAND_REQUIRED;
+		scsiDev.target->state.sense.code = NOT_READY;
+		scsiDev.target->state.sense.asc = LOGICAL_UNIT_NOT_READY_INITIALIZING_COMMAND_REQUIRED;
 		scsiDev.phase = STATUS;
 	}
 	else if (unlikely(!(blockDev.state & DISK_PRESENT)))
 	{
 		ready = 0;
 		scsiDev.status = CHECK_CONDITION;
-		scsiDev.target->sense.code = NOT_READY;
-		scsiDev.target->sense.asc = MEDIUM_NOT_PRESENT;
+		scsiDev.target->state.sense.code = NOT_READY;
+		scsiDev.target->state.sense.asc = MEDIUM_NOT_PRESENT;
 		scsiDev.phase = STATUS;
 	}
 	else if (unlikely(!(blockDev.state & DISK_INITIALISED)))
 	{
 		ready = 0;
 		scsiDev.status = CHECK_CONDITION;
-		scsiDev.target->sense.code = NOT_READY;
-		scsiDev.target->sense.asc = LOGICAL_UNIT_NOT_READY_CAUSE_NOT_REPORTABLE;
+		scsiDev.target->state.sense.code = NOT_READY;
+		scsiDev.target->state.sense.asc = LOGICAL_UNIT_NOT_READY_CAUSE_NOT_REPORTABLE;
 		scsiDev.phase = STATUS;
 	}
 	return ready;
@@ -516,8 +520,8 @@ int scsiDiskCommand()
 			// TODO. This means they are supplying data to verify against.
 			// Technically we should probably grab the data and compare it.
 			scsiDev.status = CHECK_CONDITION;
-			scsiDev.target->sense.code = ILLEGAL_REQUEST;
-			scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+			scsiDev.target->state.sense.code = ILLEGAL_REQUEST;
+			scsiDev.target->state.sense.asc = INVALID_FIELD_IN_CDB;
 			scsiDev.phase = STATUS;
 		}
 	}
@@ -575,7 +579,7 @@ calcReadahead(uint32_t totalBytes, uint32_t sdSpeedKBs, uint32_t scsiSpeedKBs)
 
 void scsiDiskPoll()
 {
-	uint32_t bytesPerSector = scsiDev.target->liveCfg.bytesPerSector;
+	uint32_t bytesPerSector = scsiDev.target->state.bytesPerSector;
 
 	if (scsiDev.phase == DATA_IN &&
 		transfer.currentBlock != transfer.blocks)
@@ -925,8 +929,8 @@ void scsiDiskPoll()
 			if (parityError &&
 				(scsiDev.boardCfg.flags & S2S_CFG_ENABLE_PARITY))
 			{
-				scsiDev.target->sense.code = ABORTED_COMMAND;
-				scsiDev.target->sense.asc = SCSI_PARITY_ERROR;
+				scsiDev.target->state.sense.code = ABORTED_COMMAND;
+				scsiDev.target->state.sense.asc = SCSI_PARITY_ERROR;
 				scsiDev.status = CHECK_CONDITION;;
 			}
 			scsiDev.phase = STATUS;
diff --git a/src/firmware/geometry.c b/src/firmware/geometry.c
index 44a2e82b..314964a8 100755
--- a/src/firmware/geometry.c
+++ b/src/firmware/geometry.c
@@ -23,20 +23,22 @@
 #include <string.h>
 
 uint32_t getScsiCapacity(
+	S2S_Device* device,
 	uint32_t sdSectorStart,
 	uint16_t bytesPerSector,
 	uint32_t scsiSectors)
 {
+	uint32_t devCapacity = device->getCapacity(device);
+
 	uint32_t capacity =
-		(sdDev.capacity - sdSectorStart - S2S_CFG_SIZE) /
+		(devCapacity - sdSectorStart - S2S_CFG_SIZE) /
 			SDSectorsPerSCSISector(bytesPerSector);
 
-
-	if (sdDev.capacity == 0)
+	if (devCapacity == 0)
 	{
 		capacity = 0;
 	}
-	else if (sdSectorStart >= (sdDev.capacity - S2S_CFG_SIZE))
+	else if (sdSectorStart >= (devCapacity - S2S_CFG_SIZE))
 	{
 		capacity = 0;
 	}
diff --git a/src/firmware/geometry.h b/src/firmware/geometry.h
index 83de38a7..c1d38ca6 100755
--- a/src/firmware/geometry.h
+++ b/src/firmware/geometry.h
@@ -18,7 +18,7 @@
 #define GEOMETRY_H
 
 #include "config.h"
-// TODO #include "sd.h"
+#include "device.h"
 #define SD_SECTOR_SIZE 512
 
 typedef enum
@@ -34,6 +34,7 @@ static inline int SDSectorsPerSCSISector(uint16_t bytesPerSector)
 }
 
 uint32_t getScsiCapacity(
+	S2S_Device* device,
 	uint32_t sdSectorStart,
 	uint16_t bytesPerSector,
 	uint32_t scsiSectors);
diff --git a/src/firmware/hwversion.c b/src/firmware/hwversion.c
index 9cd06cc4..9330f995 100755
--- a/src/firmware/hwversion.c
+++ b/src/firmware/hwversion.c
@@ -28,15 +28,11 @@
 // Values for STM32F401RE
 const size_t OTP_SIZE = 512;
 const size_t OTP_BLOCKS = 16;
-const size_t OTP_BLOCK_SIZE = OTP_SIZE / OTP_BLOCKS;
+const size_t OTP_BLOCK_SIZE = 32;//OTP_SIZE / OTP_BLOCKS;
 
 const size_t OTP_BLOCK_NUM = 0;
 
-// Define some pointers for writing, but also to allow easy reading back values
 #define FLASH_OTP_BASE 0x1FFF7800
-const uint8_t *otp = (uint8_t*)(FLASH_OTP_BASE + OTP_BLOCK_NUM * OTP_BLOCK_SIZE);
-const uint32_t *otp32 = (uint32_t*)(FLASH_OTP_BASE + OTP_BLOCK_NUM * OTP_BLOCK_SIZE);
-const uint8_t *lock = (uint8_t*)(FLASH_OTP_BASE + OTP_SIZE + OTP_BLOCK_NUM);
 
 const uint32_t marker = 0x06002020;
 
@@ -59,6 +55,11 @@ s2s_checkHwVersion()
 {
 	checkHwSensePins();
 
+	// Define some pointers for writing, but also to allow easy reading back values
+	const uint8_t *otp = (uint8_t*)(FLASH_OTP_BASE + OTP_BLOCK_NUM * OTP_BLOCK_SIZE);
+	const uint32_t *otp32 = (uint32_t*)(FLASH_OTP_BASE + OTP_BLOCK_NUM * OTP_BLOCK_SIZE);
+	const uint8_t *lock = (uint8_t*)(FLASH_OTP_BASE + OTP_SIZE + OTP_BLOCK_NUM);
+
 	// Write a marker to flash that can be read by dfu-util now that we know
 	// the version is correct.
 	if (*otp32 != marker)
diff --git a/src/firmware/inquiry.c b/src/firmware/inquiry.c
index 138e8c21..a538be79 100755
--- a/src/firmware/inquiry.c
+++ b/src/firmware/inquiry.c
@@ -98,8 +98,8 @@ void s2s_scsiInquiry()
 		{
 			// error.
 			scsiDev.status = CHECK_CONDITION;
-			scsiDev.target->sense.code = ILLEGAL_REQUEST;
-			scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+			scsiDev.target->state.sense.code = ILLEGAL_REQUEST;
+			scsiDev.target->state.sense.asc = INVALID_FIELD_IN_CDB;
 			scsiDev.phase = STATUS;
 		}
 		else
@@ -147,8 +147,8 @@ void s2s_scsiInquiry()
 	{
 		// error.
 		scsiDev.status = CHECK_CONDITION;
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+		scsiDev.target->state.sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->state.sense.asc = INVALID_FIELD_IN_CDB;
 		scsiDev.phase = STATUS;
 	}
 
diff --git a/src/firmware/mode.c b/src/firmware/mode.c
index 5235e448..64823e7b 100755
--- a/src/firmware/mode.c
+++ b/src/firmware/mode.c
@@ -356,7 +356,7 @@ static void doModeSense(
 		scsiDev.data[idx++] = 0; // reserved
 
 		// Block length
-		uint32_t bytesPerSector = scsiDev.target->liveCfg.bytesPerSector;
+		uint32_t bytesPerSector = scsiDev.target->state.bytesPerSector;
 		scsiDev.data[idx++] = bytesPerSector >> 16;
 		scsiDev.data[idx++] = bytesPerSector >> 8;
 		scsiDev.data[idx++] = bytesPerSector & 0xFF;
@@ -405,7 +405,7 @@ static void doModeSense(
 			scsiDev.data[idx+11] = sectorsPerTrack & 0xFF;
 
 			// Fill out the configured bytes-per-sector
-			uint32_t bytesPerSector = scsiDev.target->liveCfg.bytesPerSector;
+			uint32_t bytesPerSector = scsiDev.target->state.bytesPerSector;
 			scsiDev.data[idx+12] = bytesPerSector >> 8;
 			scsiDev.data[idx+13] = bytesPerSector & 0xFF;
 		}
@@ -439,8 +439,9 @@ static void doModeSense(
 			uint32_t sector;
 			LBA2CHS(
 				getScsiCapacity(
+					scsiDev.target->device,
 					scsiDev.target->cfg->sdSectorStart,
-					scsiDev.target->liveCfg.bytesPerSector,
+					scsiDev.target->state.bytesPerSector,
 					scsiDev.target->cfg->scsiSectors),
 				&cyl,
 				&head,
@@ -547,8 +548,8 @@ static void doModeSense(
 		// Unknown Page Code
 		pageFound = 0;
 		scsiDev.status = CHECK_CONDITION;
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+		scsiDev.target->state.sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->state.sense.asc = INVALID_FIELD_IN_CDB;
 		scsiDev.phase = STATUS;
 	}
 	else
@@ -607,10 +608,10 @@ static void doModeSelect(void)
 			}
 			else
 			{
-				scsiDev.target->liveCfg.bytesPerSector = bytesPerSector;
+				scsiDev.target->state.bytesPerSector = bytesPerSector;
 				if (bytesPerSector != scsiDev.target->cfg->bytesPerSector)
 				{
-					s2s_configSave(scsiDev.target->targetId, bytesPerSector);
+					s2s_configSave(scsiDev.target->id, bytesPerSector);
 				}
 			}
 		}
@@ -640,10 +641,10 @@ static void doModeSelect(void)
 					goto bad;
 				}
 
-				scsiDev.target->liveCfg.bytesPerSector = bytesPerSector;
+				scsiDev.target->state.bytesPerSector = bytesPerSector;
 				if (scsiDev.cdb[1] & 1) // SP Save Pages flag
 				{
-					s2s_configSave(scsiDev.target->targetId, bytesPerSector);
+					s2s_configSave(scsiDev.target->id, bytesPerSector);
 				}
 			}
 			break;
@@ -659,8 +660,8 @@ static void doModeSelect(void)
 	goto out;
 bad:
 	scsiDev.status = CHECK_CONDITION;
-	scsiDev.target->sense.code = ILLEGAL_REQUEST;
-	scsiDev.target->sense.asc = INVALID_FIELD_IN_PARAMETER_LIST;
+	scsiDev.target->state.sense.code = ILLEGAL_REQUEST;
+	scsiDev.target->state.sense.asc = INVALID_FIELD_IN_PARAMETER_LIST;
 
 out:
 	scsiDev.phase = STATUS;
diff --git a/src/firmware/scsi.c b/src/firmware/scsi.c
index 016ecbb5..6369fb38 100755
--- a/src/firmware/scsi.c
+++ b/src/firmware/scsi.c
@@ -131,8 +131,8 @@ static void enter_Status(uint8_t status)
 	scsiDev.phase = STATUS;
 
 	scsiDev.lastStatus = scsiDev.status;
-	scsiDev.lastSense = scsiDev.target->sense.code;
-	scsiDev.lastSenseASC = scsiDev.target->sense.asc;
+	scsiDev.lastSense = scsiDev.target->state.sense.code;
+	scsiDev.lastSenseASC = scsiDev.target->state.sense.asc;
 }
 
 void process_Status()
@@ -190,7 +190,7 @@ void process_Status()
 	}
 	else if (scsiDev.target->cfg->quirks == S2S_CFG_QUIRKS_OMTI)
 	{
-		scsiDev.status |= (scsiDev.target->targetId & 0x03) << 5;
+		scsiDev.status |= (scsiDev.target->id & 0x03) << 5;
 		scsiWriteByte(scsiDev.status);
 	}
 	else
@@ -199,8 +199,8 @@ void process_Status()
 	}
 
 	scsiDev.lastStatus = scsiDev.status;
-	scsiDev.lastSense = scsiDev.target->sense.code;
-	scsiDev.lastSenseASC = scsiDev.target->sense.asc;
+	scsiDev.lastSense = scsiDev.target->state.sense.code;
+	scsiDev.lastSenseASC = scsiDev.target->state.sense.asc;
 
 	// Command Complete occurs AFTER a valid status has been
 	// sent. then we go bus-free.
@@ -258,8 +258,8 @@ static void process_DataOut()
 		if (parityError &&
 			(scsiDev.boardCfg.flags & S2S_CFG_ENABLE_PARITY))
 		{
-			scsiDev.target->sense.code = ABORTED_COMMAND;
-			scsiDev.target->sense.asc = SCSI_PARITY_ERROR;
+			scsiDev.target->state.sense.code = ABORTED_COMMAND;
+			scsiDev.target->state.sense.asc = SCSI_PARITY_ERROR;
 			enter_Status(CHECK_CONDITION);
 		}
 	}
@@ -323,15 +323,11 @@ static void process_Command()
 	// http://bitsavers.trailing-edge.com/pdf/xebec/104524C_S1410Man_Aug83.pdf
 	if ((scsiDev.lun > 0) && (scsiDev.boardCfg.flags & S2S_CFG_MAP_LUNS_TO_IDS))
 	{
-		int tgtIndex;
-		for (tgtIndex = 0; tgtIndex < S2S_MAX_TARGETS; ++tgtIndex)
+		S2S_Target* lunTarget = s2s_DeviceFindByScsiId(scsiDev.lun);
+		if (lunTarget != NULL)
 		{
-			if (scsiDev.targets[tgtIndex].targetId == scsiDev.lun)
-			{
-				scsiDev.target = &scsiDev.targets[tgtIndex];
-				scsiDev.lun = 0;
-				break;
-			}
+			scsiDev.target = lunTarget;
+			scsiDev.lun = 0;
 		}
 	}
 
@@ -350,8 +346,8 @@ static void process_Command()
 	else if (parityError &&
 		(scsiDev.boardCfg.flags & S2S_CFG_ENABLE_PARITY))
 	{
-		scsiDev.target->sense.code = ABORTED_COMMAND;
-		scsiDev.target->sense.asc = SCSI_PARITY_ERROR;
+		scsiDev.target->state.sense.code = ABORTED_COMMAND;
+		scsiDev.target->state.sense.asc = SCSI_PARITY_ERROR;
 		enter_Status(CHECK_CONDITION);
 	}
 	else if ((control & 0x02) && ((control & 0x01) == 0) &&
@@ -359,8 +355,8 @@ static void process_Command()
 		likely(scsiDev.target->cfg->quirks != S2S_CFG_QUIRKS_XEBEC))
 	{
 		// FLAG set without LINK flag.
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+		scsiDev.target->state.sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->state.sense.asc = INVALID_FIELD_IN_CDB;
 		enter_Status(CHECK_CONDITION);
 	}
 	else if (command == 0x12)
@@ -376,11 +372,11 @@ static void process_Command()
 		{
 			// Completely non-standard
 			allocLength = 4;
-			if (scsiDev.target->sense.code == NO_SENSE)
+			if (scsiDev.target->state.sense.code == NO_SENSE)
 				scsiDev.data[0] = 0;
-			else if (scsiDev.target->sense.code == ILLEGAL_REQUEST)
+			else if (scsiDev.target->state.sense.code == ILLEGAL_REQUEST)
 				scsiDev.data[0] = 0x20; // Illegal command
-			else if (scsiDev.target->sense.code == NOT_READY)
+			else if (scsiDev.target->state.sense.code == NOT_READY)
 				scsiDev.data[0] = 0x04; // Drive not ready
 			else
 				scsiDev.data[0] = 0x11;  // Uncorrectable data error
@@ -397,7 +393,7 @@ static void process_Command()
 
 			memset(scsiDev.data, 0, 256); // Max possible alloc length
 			scsiDev.data[0] = 0xF0;
-			scsiDev.data[2] = scsiDev.target->sense.code & 0x0F;
+			scsiDev.data[2] = scsiDev.target->state.sense.code & 0x0F;
 
 			scsiDev.data[3] = transfer.lba >> 24;
 			scsiDev.data[4] = transfer.lba >> 16;
@@ -406,45 +402,45 @@ static void process_Command()
 
 			// Additional bytes if there are errors to report
 			scsiDev.data[7] = 10; // additional length
-			scsiDev.data[12] = scsiDev.target->sense.asc >> 8;
-			scsiDev.data[13] = scsiDev.target->sense.asc;
+			scsiDev.data[12] = scsiDev.target->state.sense.asc >> 8;
+			scsiDev.data[13] = scsiDev.target->state.sense.asc;
 		}
 
 		// Silently truncate results. SCSI-2 spec 8.2.14.
 		enter_DataIn(allocLength);
 
 		// This is a good time to clear out old sense information.
-		scsiDev.target->sense.code = NO_SENSE;
-		scsiDev.target->sense.asc = NO_ADDITIONAL_SENSE_INFORMATION;
+		scsiDev.target->state.sense.code = NO_SENSE;
+		scsiDev.target->state.sense.asc = NO_ADDITIONAL_SENSE_INFORMATION;
 	}
 	// Some old SCSI drivers do NOT properly support
 	// unitAttention. eg. the Mac Plus would trigger a SCSI reset
 	// on receiving the unit attention response on boot, thus
 	// triggering another unit attention condition.
-	else if (scsiDev.target->unitAttention &&
+	else if (scsiDev.target->state.unitAttention &&
 		(scsiDev.boardCfg.flags & S2S_CFG_ENABLE_UNIT_ATTENTION))
 	{
-		scsiDev.target->sense.code = UNIT_ATTENTION;
-		scsiDev.target->sense.asc = scsiDev.target->unitAttention;
+		scsiDev.target->state.sense.code = UNIT_ATTENTION;
+		scsiDev.target->state.sense.asc = scsiDev.target->state.unitAttention;
 
 		// If initiator doesn't do REQUEST SENSE for the next command, then
 		// data is lost.
-		scsiDev.target->unitAttention = 0;
+		scsiDev.target->state.unitAttention = 0;
 
 		enter_Status(CHECK_CONDITION);
 	}
 	else if (scsiDev.lun)
 	{
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = LOGICAL_UNIT_NOT_SUPPORTED;
+		scsiDev.target->state.sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->state.sense.asc = LOGICAL_UNIT_NOT_SUPPORTED;
 		enter_Status(CHECK_CONDITION);
 	}
 	else if (command == 0x17 || command == 0x16)
 	{
 		doReserveRelease();
 	}
-	else if ((scsiDev.target->reservedId >= 0) &&
-		(scsiDev.target->reservedId != scsiDev.initiatorId))
+	else if ((scsiDev.target->state.reservedId >= 0) &&
+		(scsiDev.target->state.reservedId != scsiDev.initiatorId))
 	{
 		enter_Status(CONFLICT);
 	}
@@ -485,8 +481,8 @@ static void process_Command()
 	}
 	else if (!scsiModeCommand() && !scsiVendorCommand())
 	{
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = INVALID_COMMAND_OPERATION_CODE;
+		scsiDev.target->state.sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->state.sense.asc = INVALID_COMMAND_OPERATION_CODE;
 		enter_Status(CHECK_CONDITION);
 	}
 
@@ -506,25 +502,25 @@ static void doReserveRelease()
 	uint8_t command = scsiDev.cdb[0];
 
 	int canRelease =
-		(!thirdPty && (scsiDev.initiatorId == scsiDev.target->reservedId)) ||
+		(!thirdPty && (scsiDev.initiatorId == scsiDev.target->state.reservedId)) ||
 			(thirdPty &&
-				(scsiDev.target->reserverId == scsiDev.initiatorId) &&
-				(scsiDev.target->reservedId == thirdPtyId)
+				(scsiDev.target->state.reserverId == scsiDev.initiatorId) &&
+				(scsiDev.target->state.reservedId == thirdPtyId)
 			);
 
 	if (extentReservation)
 	{
 		// Not supported.
-		scsiDev.target->sense.code = ILLEGAL_REQUEST;
-		scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;
+		scsiDev.target->state.sense.code = ILLEGAL_REQUEST;
+		scsiDev.target->state.sense.asc = INVALID_FIELD_IN_CDB;
 		enter_Status(CHECK_CONDITION);
 	}
 	else if (command == 0x17) // release
 	{
-		if ((scsiDev.target->reservedId < 0) || canRelease)
+		if ((scsiDev.target->state.reservedId < 0) || canRelease)
 		{
-			scsiDev.target->reservedId = -1;
-			scsiDev.target->reserverId = -1;
+			scsiDev.target->state.reservedId = -1;
+			scsiDev.target->state.reserverId = -1;
 		}
 		else
 		{
@@ -533,16 +529,16 @@ static void doReserveRelease()
 	}
 	else // assume reserve.
 	{
-		if ((scsiDev.target->reservedId < 0) || canRelease)
+		if ((scsiDev.target->state.reservedId < 0) || canRelease)
 		{
-			scsiDev.target->reserverId = scsiDev.initiatorId;
+			scsiDev.target->state.reserverId = scsiDev.initiatorId;
 			if (thirdPty)
 			{
-				scsiDev.target->reservedId = thirdPtyId;
+				scsiDev.target->state.reservedId = thirdPtyId;
 			}
 			else
 			{
-				scsiDev.target->reservedId = scsiDev.initiatorId;
+				scsiDev.target->state.reservedId = scsiDev.initiatorId;
 			}
 		}
 		else
@@ -571,21 +567,28 @@ static void scsiReset()
 
 	if (scsiDev.target)
 	{
-		if (scsiDev.target->unitAttention != POWER_ON_RESET)
+		if (scsiDev.target->state.unitAttention != POWER_ON_RESET)
 		{
-			scsiDev.target->unitAttention = SCSI_BUS_RESET;
+			scsiDev.target->state.unitAttention = SCSI_BUS_RESET;
 		}
-		scsiDev.target->reservedId = -1;
-		scsiDev.target->reserverId = -1;
-		scsiDev.target->sense.code = NO_SENSE;
-		scsiDev.target->sense.asc = NO_ADDITIONAL_SENSE_INFORMATION;
+		scsiDev.target->state.reservedId = -1;
+		scsiDev.target->state.reserverId = -1;
+		scsiDev.target->state.sense.code = NO_SENSE;
+		scsiDev.target->state.sense.asc = NO_ADDITIONAL_SENSE_INFORMATION;
 	}
 	scsiDev.target = NULL;
 
-	for (int i = 0; i < S2S_MAX_TARGETS; ++i)
+	int deviceCount;
+	S2S_Device* allDevices = s2s_GetDevices(&deviceCount);
+	for (int devIdx = 0; devIdx < deviceCount; ++devIdx)
 	{
-		scsiDev.targets[i].syncOffset = 0;
-		scsiDev.targets[i].syncPeriod = 0;
+		int targetCount;
+		S2S_Target* allTargets = allDevices[devIdx].getTargets(allDevices + devIdx, &targetCount);
+		for (int i = 0; i < targetCount; ++i)
+		{
+			allTargets[i].state.syncOffset = 0;
+			allTargets[i].state.syncPeriod = 0;
+		}
 	}
 	scsiDev.minSyncPeriod = 0;
 
@@ -659,16 +662,7 @@ static void process_SelectionPhase()
 		selStatus = scsiDev.selFlag;
 	}
 
-	int tgtIndex;
-	TargetState* target = NULL;
-	for (tgtIndex = 0; tgtIndex < S2S_MAX_TARGETS; ++tgtIndex)
-	{
-		if (scsiDev.targets[tgtIndex].targetId == (selStatus & 7))
-		{
-			target = &scsiDev.targets[tgtIndex];
-			break;
-		}
-	}
+	S2S_Target* target = s2s_DeviceFindByScsiId(selStatus & 7);
 	if ((target != NULL) && (selStatus & 0x40))
 	{
 		// We've been selected!
@@ -694,7 +688,7 @@ static void process_SelectionPhase()
 		// controllers don't generate parity bits.
 		if (!scsiDev.atnFlag)
 		{
-			target->unitAttention = 0;
+			target->state.unitAttention = 0;
 			scsiDev.compatMode = COMPAT_SCSI1;
 		}
 		else if (!(scsiDev.boardCfg.flags & S2S_CFG_ENABLE_SCSI2))
@@ -788,15 +782,15 @@ static void process_MessageOut()
 
 		scsiDiskReset();
 
-		scsiDev.target->unitAttention = SCSI_BUS_RESET;
+		scsiDev.target->state.unitAttention = SCSI_BUS_RESET;
 
 		// ANY initiator can reset the reservation state via this message.
-		scsiDev.target->reservedId = -1;
-		scsiDev.target->reserverId = -1;
+		scsiDev.target->state.reservedId = -1;
+		scsiDev.target->state.reserverId = -1;
 
 		// Cancel any sync negotiation
-		scsiDev.target->syncOffset = 0;
-		scsiDev.target->syncPeriod = 0;
+		scsiDev.target->state.syncOffset = 0;
+		scsiDev.target->state.syncPeriod = 0;
 
 		enter_BusFree();
 	}
@@ -823,8 +817,8 @@ static void process_MessageOut()
 
 		if (wasNeedSyncNegotiationAck)
 		{
-			scsiDev.target->syncOffset = 0;
-			scsiDev.target->syncPeriod = 0;
+			scsiDev.target->state.syncOffset = 0;
+			scsiDev.target->state.syncPeriod = 0;
 		}
 	}
 	else if (scsiDev.msgOut == 0x08)
@@ -839,8 +833,8 @@ static void process_MessageOut()
 
 		if (wasNeedSyncNegotiationAck)
 		{
-			scsiDev.target->syncOffset = 0;
-			scsiDev.target->syncPeriod = 0;
+			scsiDev.target->state.syncOffset = 0;
+			scsiDev.target->state.syncPeriod = 0;
 		}
 	}
 	else if (scsiDev.msgOut & 0x80) // 0x80 -> 0xFF
@@ -890,13 +884,13 @@ static void process_MessageOut()
 			scsiWrite(WDTR, sizeof(WDTR));
 
 			// SDTR becomes invalidated.
-			scsiDev.target->syncOffset = 0;
-			scsiDev.target->syncPeriod = 0;
+			scsiDev.target->state.syncOffset = 0;
+			scsiDev.target->state.syncPeriod = 0;
 		}
 		else if (extmsg[0] == 1 && msgLen == 3) // Synchronous data request
 		{
-			int oldPeriod = scsiDev.target->syncPeriod;
-			int oldOffset = scsiDev.target->syncOffset;
+			int oldPeriod = scsiDev.target->state.syncPeriod;
+			int oldOffset = scsiDev.target->state.syncOffset;
 
 			int transferPeriod = extmsg[1];
 			int offset = extmsg[2];
@@ -918,10 +912,10 @@ static void process_MessageOut()
 				((scsiDev.boardCfg.scsiSpeed != S2S_CFG_SPEED_NoLimit) &&
 					(scsiDev.boardCfg.scsiSpeed <= S2S_CFG_SPEED_ASYNC_50)))
 			{
-				scsiDev.target->syncOffset = 0;
-				scsiDev.target->syncPeriod = 0;
+				scsiDev.target->state.syncOffset = 0;
+				scsiDev.target->state.syncPeriod = 0;
 			} else {
-				scsiDev.target->syncOffset = offset <= 15 ? offset : 15;
+				scsiDev.target->state.syncOffset = offset <= 15 ? offset : 15;
 				// FAST20 / 50ns / 20MHz is disabled for now due to
 				// data corruption while reading data. We can count the
 				// ACK's correctly, but can't save the data to a register
@@ -929,39 +923,39 @@ static void process_MessageOut()
 				if ((scsiDev.boardCfg.scsiSpeed == S2S_CFG_SPEED_TURBO) &&
 					(transferPeriod <= 16))
 				{
-					scsiDev.target->syncPeriod = 16; // 15.6MB/s
+					scsiDev.target->state.syncPeriod = 16; // 15.6MB/s
 				}
 				else if (scsiDev.boardCfg.scsiSpeed == S2S_CFG_SPEED_TURBO)
 				{
-					scsiDev.target->syncPeriod = transferPeriod;
+					scsiDev.target->state.syncPeriod = transferPeriod;
 				}
 				else if (transferPeriod <= 25 &&
 					((scsiDev.boardCfg.scsiSpeed == S2S_CFG_SPEED_NoLimit) ||
 						(scsiDev.boardCfg.scsiSpeed >= S2S_CFG_SPEED_SYNC_10)))
 				{
-					scsiDev.target->syncPeriod = 25; // 100ns, 10MB/s
+					scsiDev.target->state.syncPeriod = 25; // 100ns, 10MB/s
 
 				} else if (transferPeriod < 50 &&
 					((scsiDev.boardCfg.scsiSpeed == S2S_CFG_SPEED_NoLimit) ||
 						(scsiDev.boardCfg.scsiSpeed >= S2S_CFG_SPEED_SYNC_10)))
 				{
-					scsiDev.target->syncPeriod = transferPeriod;
+					scsiDev.target->state.syncPeriod = transferPeriod;
 				} else if (transferPeriod >= 50)
 				{
-					scsiDev.target->syncPeriod = transferPeriod;
+					scsiDev.target->state.syncPeriod = transferPeriod;
 				} else {
-					scsiDev.target->syncPeriod = 50;
+					scsiDev.target->state.syncPeriod = 50;
 				}
 			}
 
 			if (transferPeriod != oldPeriod ||
-				scsiDev.target->syncPeriod != oldPeriod ||
+				scsiDev.target->state.syncPeriod != oldPeriod ||
 				offset != oldOffset ||
-				scsiDev.target->syncOffset != oldOffset ||
+				scsiDev.target->state.syncOffset != oldOffset ||
 				!wasNeedSyncNegotiationAck) // Don't get into infinite loops negotiating.
 			{
 				scsiEnterPhase(MESSAGE_IN);
-				uint8_t SDTR[] = {0x01, 0x03, 0x01, scsiDev.target->syncPeriod, scsiDev.target->syncOffset};
+				uint8_t SDTR[] = {0x01, 0x03, 0x01, scsiDev.target->state.syncPeriod, scsiDev.target->state.syncOffset};
 				scsiWrite(SDTR, sizeof(SDTR));
 				scsiDev.needSyncNegotiationAck = 1; // Check if this message is rejected.
 				scsiDev.sdUnderrunCount = 0;  // reset counter, may work now.
@@ -1133,145 +1127,34 @@ void scsiInit()
 	scsiDev.hostSpeedKBs = 0;
 	scsiDev.hostSpeedMeasured = 0;
 
-	int i;
-	for (i = 0; i < S2S_MAX_TARGETS; ++i)
+	int deviceCount;
+	S2S_Device* devices = s2s_GetDevices(&deviceCount);
+	for (int deviceIdx = 0; deviceIdx < deviceCount; ++deviceIdx)
 	{
-		const S2S_TargetCfg* cfg = s2s_getConfigByIndex(i);
-		if (cfg && (cfg->scsiId & S2S_CFG_TARGET_ENABLED))
-		{
-			scsiDev.targets[i].targetId = cfg->scsiId & S2S_CFG_TARGET_ID_BITS;
-			scsiDev.targets[i].cfg = cfg;
+		int targetCount;
+		S2S_Target* targets = devices[deviceIdx].getTargets(devices + deviceIdx, &targetCount);
 
-			scsiDev.targets[i].liveCfg.bytesPerSector = cfg->bytesPerSector;
-		}
-		else
+		for (int i = 0; i < targetCount; ++i)
 		{
-			scsiDev.targets[i].targetId = 0xff;
-			scsiDev.targets[i].cfg = NULL;
-		}
-		scsiDev.targets[i].reservedId = -1;
-		scsiDev.targets[i].reserverId = -1;
-		if (firstInit)
-		{
-			scsiDev.targets[i].unitAttention = POWER_ON_RESET;
-		}
-		else
-		{
-			scsiDev.targets[i].unitAttention = PARAMETERS_CHANGED;
-		}
-		scsiDev.targets[i].sense.code = NO_SENSE;
-		scsiDev.targets[i].sense.asc = NO_ADDITIONAL_SENSE_INFORMATION;
-
-		scsiDev.targets[i].syncOffset = 0;
-		scsiDev.targets[i].syncPeriod = 0;
-	}
-	firstInit = 0;
-}
-
-/* TODO REENABLE
-void scsiDisconnect()
-{
-	scsiEnterPhase(MESSAGE_IN);
-	scsiWriteByte(0x02); // save data pointer
-	scsiWriteByte(0x04); // disconnect msg.
-
-	// For now, the caller is responsible for tracking the disconnected
-	// state, and calling scsiReconnect.
-	// Ideally the client would exit their loop and we'd implement this
-	// as part of scsiPoll
-	int phase = scsiDev.phase;
-	enter_BusFree();
-	scsiDev.phase = phase;
-}
-*/
-
-/* TODO REENABLE
-int scsiReconnect()
-{
-	int reconnected = 0;
-
-	int sel = SCSI_ReadFilt(SCSI_Filt_SEL);
-	int bsy = SCSI_ReadFilt(SCSI_Filt_BSY);
-	if (!sel && !bsy)
-	{
-		s2s_delay_us(1);
-		sel = SCSI_ReadFilt(SCSI_Filt_SEL);
-		bsy = SCSI_ReadFilt(SCSI_Filt_BSY);
-	}
-
-	if (!sel && !bsy)
-	{
-		// Arbitrate.
-		s2s_ledOn();
-		uint8_t scsiIdMask = 1 << scsiDev.target->targetId;
-		SCSI_Out_Bits_Write(scsiIdMask);
-		SCSI_Out_Ctl_Write(1); // Write bits manually.
-		SCSI_SetPin(SCSI_Out_BSY);
-
-		s2s_delay_us(3); // arbitrate delay. 2.4us.
-
-		uint8_t dbx = scsiReadDBxPins();
-		sel = SCSI_ReadFilt(SCSI_Filt_SEL);
-		if (sel || ((dbx ^ scsiIdMask) > scsiIdMask))
-		{
-			// Lost arbitration.
-			SCSI_Out_Ctl_Write(0);
-			SCSI_ClearPin(SCSI_Out_BSY);
-			s2s_ledOff();
-		}
-		else
-		{
-			// Won arbitration
-			SCSI_SetPin(SCSI_Out_SEL);
-			s2s_delay_us(1); // Bus clear + Bus settle.
-
-			// Reselection phase
-			SCSI_CTL_PHASE_Write(__scsiphase_io);
-			SCSI_Out_Bits_Write(scsiIdMask | (1 << scsiDev.initiatorId));
-			scsiDeskewDelay(); // 2 deskew delays
-			scsiDeskewDelay(); // 2 deskew delays
-			SCSI_ClearPin(SCSI_Out_BSY);
-			s2s_delay_us(1);  // Bus Settle Delay
-
-			uint32_t waitStart_ms = getTime_ms();
-			bsy = SCSI_ReadFilt(SCSI_Filt_BSY);
-			// Wait for initiator.
-			while (
-				!bsy &&
-				!scsiDev.resetFlag &&
-				(elapsedTime_ms(waitStart_ms) < 250))
-			{
-				bsy = SCSI_ReadFilt(SCSI_Filt_BSY);
-			}
-
-			if (bsy)
+			S2S_TargetState* state = &(targets[i].state);
+			state->reservedId = -1;
+			state->reserverId = -1;
+			if (firstInit)
 			{
-				SCSI_SetPin(SCSI_Out_BSY);
-				scsiDeskewDelay(); // 2 deskew delays
-				scsiDeskewDelay(); // 2 deskew delays
-				SCSI_ClearPin(SCSI_Out_SEL);
-
-				// Prepare for the initial IDENTIFY message.
-				SCSI_Out_Ctl_Write(0);
-				scsiEnterPhase(MESSAGE_IN);
-
-				// Send identify command
-				scsiWriteByte(0x80);
-
-				scsiEnterPhase(scsiDev.phase);
-				reconnected = 1;
+				state->unitAttention = POWER_ON_RESET;
 			}
 			else
 			{
-				// reselect timeout.
-				SCSI_Out_Ctl_Write(0);
-				SCSI_ClearPin(SCSI_Out_SEL);
-				SCSI_CTL_PHASE_Write(0);
-				s2s_ledOff();
+				state->unitAttention = PARAMETERS_CHANGED;
 			}
+			state->sense.code = NO_SENSE;
+			state->sense.asc = NO_ADDITIONAL_SENSE_INFORMATION;
+
+			state->syncOffset = 0;
+			state->syncPeriod = 0;
 		}
 	}
-	return reconnected;
+	firstInit = 0;
 }
-*/
+
 
diff --git a/src/firmware/scsi.h b/src/firmware/scsi.h
index 64353ada..ccbba855 100755
--- a/src/firmware/scsi.h
+++ b/src/firmware/scsi.h
@@ -17,8 +17,8 @@
 #ifndef SCSI_H
 #define SCSI_H
 
+#include "device.h"
 #include "geometry.h"
-#include "sense.h"
 
 #include <stdint.h>
 
@@ -75,33 +75,6 @@ typedef enum
 #define MAX_SECTOR_SIZE 8192
 #define MIN_SECTOR_SIZE 64
 
-// Shadow parameters, possibly not saved to flash yet.
-// Set via Mode Select
-typedef struct
-{
-	uint16_t bytesPerSector;
-} LiveCfg;
-
-typedef struct
-{
-	uint8_t targetId;
-
-	const S2S_TargetCfg* cfg;
-
-	LiveCfg liveCfg;
-
-	ScsiSense sense;
-
-	uint16_t unitAttention; // Set to the sense qualifier key to be returned.
-
-	// Only let the reserved initiator talk to us.
-	// A 3rd party may be sending the RESERVE/RELEASE commands
-	int reservedId; // 0 -> 7 if reserved. -1 if not reserved.
-	int reserverId; // 0 -> 7 if reserved. -1 if not reserved.
-
-	uint8_t syncOffset;
-	uint8_t syncPeriod;
-} TargetState;
 
 typedef struct
 {
@@ -110,10 +83,8 @@ typedef struct
 	// 65536 bytes is the DMA limit
 	uint8_t data[MAX_SECTOR_SIZE * 8];
 
-	TargetState targets[S2S_MAX_TARGETS];
-	TargetState* target;
-	S2S_BoardCfg boardCfg;
-
+	S2S_Target* target;
+	S2S_BoardCfg boardCfg; // TODO remove
 
 	// Set to true (1) if the ATN flag was set, and we need to
 	// enter the MESSAGE_OUT phase.
diff --git a/src/firmware/scsiPhy.c b/src/firmware/scsiPhy.c
index 8da0a098..887b3d08 100755
--- a/src/firmware/scsiPhy.c
+++ b/src/firmware/scsiPhy.c
@@ -576,17 +576,18 @@ uint32_t scsiEnterPhaseImmediate(int newPhase)
 	while (likely(!scsiDev.resetFlag) && scsiStatusACK()) {}
 
 	int oldPhase = *SCSI_CTRL_PHASE;
+	uint8_t syncOffset = scsiDev.target->state.syncOffset;
+	uint8_t syncPeriod = scsiDev.target->state.syncPeriod;
 
 	if (newPhase != oldPhase)
 	{
-		if ((newPhase == DATA_IN || newPhase == DATA_OUT) &&
-			scsiDev.target->syncOffset)
+		if ((newPhase == DATA_IN || newPhase == DATA_OUT) && syncOffset)
 		{
-			if (scsiDev.target->syncPeriod < 23)
+			if (syncPeriod < 23)
 			{
 				scsiSetTiming(SCSI_FAST20_ASSERT, SCSI_FAST20_DESKEW, SCSI_FAST20_HOLD, 1);
 			}
-			else if (scsiDev.target->syncPeriod <= 25)
+			else if (syncPeriod <= 25)
 			{
 				if (newPhase == DATA_IN)
 				{
@@ -602,26 +603,25 @@ uint32_t scsiEnterPhaseImmediate(int newPhase)
 				// Amiga A3000 OS3.9 sets period to 35 and fails with
 				// glitch == 1.
 				int glitch =
-					scsiDev.target->syncPeriod < 35 ? 1 :
-						(scsiDev.target->syncPeriod < 45 ? 2 : 5);
-				int deskew = syncDeskew(scsiDev.target->syncPeriod);
+					syncPeriod < 35 ? 1 : (syncPeriod < 45 ? 2 : 5);
+				int deskew = syncDeskew(syncPeriod);
 				int assertion;
 				if (newPhase == DATA_IN)
 				{
-					assertion = syncAssertionWrite(scsiDev.target->syncPeriod, deskew);
+					assertion = syncAssertionWrite(syncPeriod, deskew);
 				}
 				else
 				{
-					assertion = syncAssertionRead(scsiDev.target->syncPeriod);
+					assertion = syncAssertionRead(syncPeriod);
 				}
 				scsiSetTiming(
 					assertion,
 					deskew,
-					syncHold(scsiDev.target->syncPeriod),
+					syncHold(syncPeriod),
 					glitch);
 			}
 
-			*SCSI_CTRL_SYNC_OFFSET = scsiDev.target->syncOffset;
+			*SCSI_CTRL_SYNC_OFFSET = syncOffset;
 		}
 		else if (newPhase >= 0)
 		{
@@ -681,13 +681,13 @@ uint32_t scsiEnterPhaseImmediate(int newPhase)
 // theoretical speed / 2
 uint32_t s2s_getScsiRateKBs()
 {
-	if (scsiDev.target->syncOffset)
+	if (scsiDev.target->state.syncOffset)
 	{
-		if (scsiDev.target->syncPeriod < 23)
+		if (scsiDev.target->state.syncPeriod < 23)
 		{
 			return 20 / 2;
 		}
-		else if (scsiDev.target->syncPeriod <= 25)
+		else if (scsiDev.target->state.syncPeriod <= 25)
 		{
 			return 10 / 2;
 		}
@@ -695,7 +695,7 @@ uint32_t s2s_getScsiRateKBs()
 		{
 			// 1000000000 / (scsiDev.target->syncPeriod * 4) bytes per second
 			// (1000000000 / (scsiDev.target->syncPeriod * 4)) / 1000  kB/s
-			return (1000000 / (scsiDev.target->syncPeriod * 4)) / 2;
+			return (1000000 / (scsiDev.target->state.syncPeriod * 4)) / 2;
 		}
 	}
 	else
diff --git a/src/firmware/sd.c b/src/firmware/sd.c
index bc400cbc..7f4b7e8b 100755
--- a/src/firmware/sd.c
+++ b/src/firmware/sd.c
@@ -31,8 +31,22 @@
 
 #include <string.h>
 
+static const S2S_BoardCfg* sd_getBoardConfig(S2S_Device* dev);
+//static const S2S_TargetCfg* sd_findByScsiId(S2S_Device* dev, int scsiId);
+static S2S_Target* sd_getTargets(S2S_Device* dev, int* count);
+static uint32_t sd_getCapacity(S2S_Device* dev);
+
 // Global
-SdDevice sdDev;
+SdCard sdCard S2S_DMA_ALIGN = {
+		{
+			sd_getBoardConfig,
+			//sd_findByScsiId,
+			sd_getTargets,
+			sd_getCapacity
+		}
+};
+
+S2S_Device* sdDevice = &(sdCard.dev);
 
 static int sdCmdActive = 0;
 
@@ -73,8 +87,8 @@ void sdReadDMA(uint32_t lba, uint32_t sectors, uint8_t* outputBuffer)
 		scsiDiskReset();
 
 		scsiDev.status = CHECK_CONDITION;
-		scsiDev.target->sense.code = HARDWARE_ERROR;
-		scsiDev.target->sense.asc = LOGICAL_UNIT_COMMUNICATION_FAILURE;
+		scsiDev.target->state.sense.code = HARDWARE_ERROR;
+		scsiDev.target->state.sense.asc = LOGICAL_UNIT_COMMUNICATION_FAILURE;
 		scsiDev.phase = STATUS;
 	}
 	else
@@ -127,10 +141,10 @@ void sdTmpWrite(uint8_t* data, uint32_t lba, int sectors)
 
 static void sdClear()
 {
-	sdDev.version = 0;
-	sdDev.capacity = 0;
-	memset(sdDev.csd, 0, sizeof(sdDev.csd));
-	memset(sdDev.cid, 0, sizeof(sdDev.cid));
+	sdCard.version = 0;
+	sdCard.capacity = 0;
+	memset(sdCard.csd, 0, sizeof(sdCard.csd));
+	memset(sdCard.cid, 0, sizeof(sdCard.cid));
 }
 
 static int sdDoInit()
@@ -143,9 +157,9 @@ static int sdDoInit()
 	int8_t error = BSP_SD_Init();
 	if (error == MSD_OK)
 	{
-		memcpy(sdDev.csd, &SDCardInfo.SD_csd, sizeof(sdDev.csd));
-		memcpy(sdDev.cid, &SDCardInfo.SD_cid, sizeof(sdDev.cid));
-		sdDev.capacity = SDCardInfo.CardCapacity / SD_SECTOR_SIZE;
+		memcpy(sdCard.csd, &SDCardInfo.SD_csd, sizeof(sdCard.csd));
+		memcpy(sdCard.cid, &SDCardInfo.SD_cid, sizeof(sdCard.cid));
+		sdCard.capacity = SDCardInfo.CardCapacity / SD_SECTOR_SIZE;
 		blockDev.state |= DISK_PRESENT | DISK_INITIALISED;
 		result = 1;
 
@@ -181,7 +195,7 @@ static int sdDoInit()
 //bad:
 	blockDev.state &= ~(DISK_PRESENT | DISK_INITIALISED);
 
-	sdDev.capacity = 0;
+	sdCard.capacity = 0;
 
 out:
 	s2s_ledOff();
@@ -256,13 +270,13 @@ int sdInit()
 		}
 		else if (!cs && (blockDev.state & DISK_PRESENT))
 		{
-			sdDev.capacity = 0;
+			sdCard.capacity = 0;
 			blockDev.state &= ~DISK_PRESENT;
 			blockDev.state &= ~DISK_INITIALISED;
 			int i;
 			for (i = 0; i < S2S_MAX_TARGETS; ++i)
 			{
-				scsiDev.targets[i].unitAttention = PARAMETERS_CHANGED;
+				sdCard.targets[i].state.unitAttention = PARAMETERS_CHANGED;
 			}
 
 			HAL_SD_DeInit(&hsd);
@@ -273,3 +287,43 @@ int sdInit()
 	return result;
 }
 
+static const S2S_BoardCfg* sd_getBoardConfig(S2S_Device* dev)
+{
+	SdCard* sdCardDevice = (SdCard*)dev;
+
+	if ((blockDev.state & DISK_PRESENT) && sdCardDevice->capacity)
+	{
+		int cfgSectors = (S2S_CFG_SIZE + 511) / 512;
+		BSP_SD_ReadBlocks_DMA(
+			(uint32_t*) &(sdCardDevice->boardCfg[0]),
+			(sdCardDevice->capacity - cfgSectors) * 512ll,
+			512,
+			cfgSectors);
+
+		S2S_BoardCfg* cfg = (S2S_BoardCfg*) &(sdCardDevice->boardCfg[0]);
+		if (memcmp(cfg->magic, "BCFG", 4))
+		{
+			return NULL;
+		}
+		else
+		{
+			return cfg;
+		}
+	}
+
+	return NULL;
+}
+
+//static const S2S_TargetCfg* sd_findByScsiId(S2S_Device* dev, int scsiId)
+static S2S_Target* sd_getTargets(S2S_Device* dev, int* count)
+{
+	SdCard* sdCardDevice = (SdCard*)dev;
+	*count = S2S_MAX_TARGETS;
+	return sdCardDevice->targets;
+}
+
+static uint32_t sd_getCapacity(S2S_Device* dev)
+{
+	SdCard* sdCardDevice = (SdCard*)dev;
+	return sdCardDevice->capacity;
+}
diff --git a/src/firmware/sd.h b/src/firmware/sd.h
index b82f1192..3a3fd556 100755
--- a/src/firmware/sd.h
+++ b/src/firmware/sd.h
@@ -18,19 +18,29 @@
 #define S2S_SD_H
 
 #include <stdint.h>
+#include "bsp.h"
+#include "device.h"
 
 #define SD_SECTOR_SIZE 512
 
 typedef struct
 {
+	S2S_Device dev;
+
+	uint8_t boardCfg[S2S_CFG_SIZE] S2S_DMA_ALIGN;
+
+	S2S_Target targets[S2S_MAX_TARGETS];
+
 	int version; // SDHC = version 2.
 	uint32_t capacity; // in 512 byte blocks
 
 	uint8_t csd[16]; // Unparsed CSD
 	uint8_t cid[16]; // Unparsed CID
-} SdDevice;
+} SdCard;
+
+extern SdCard sdCard; // TODO move to .c file
 
-extern SdDevice sdDev;
+extern S2S_Device* sdDevice;
 
 int sdInit(void);
 
diff --git a/src/firmware/usb_device/usbd_msc_storage_sd.c b/src/firmware/usb_device/usbd_msc_storage_sd.c
index 8fb6aa89..b9666ad9 100755
--- a/src/firmware/usb_device/usbd_msc_storage_sd.c
+++ b/src/firmware/usb_device/usbd_msc_storage_sd.c
@@ -16,8 +16,6 @@
 //	along with SCSI2SD.  If not, see <http://www.gnu.org/licenses/>.
 
 
-#include "usbd_msc_storage_sd.h"
-#include "stm32f2xx.h"
 #include "../bsp_driver_sd.h"
 #include "../bsp.h"
 #include "../disk.h"
@@ -26,8 +24,10 @@
 #include "../config.h"
 #include "../geometry.h"
 #include "../inquiry.h"
-#include "usb_device.h"
 
+#include "usbd_msc_storage_sd.h"
+#include "stm32f2xx.h"
+#include "usb_device.h"
 
 
 static int8_t s2s_usbd_storage_Init(uint8_t lun);
@@ -69,24 +69,32 @@ USBD_StorageTypeDef USBD_MSC_SD_fops =
 	s2s_usbd_storage_Inquiry
 };
 
-static const S2S_TargetCfg* getUsbConfig(uint8_t lun) {
+static const S2S_Target* getUsbTarget(uint8_t lun) {
 	int count = 0;
-	for (int i = 0; i < S2S_MAX_TARGETS; ++i)
+	int deviceCount;
+	S2S_Device* devices = s2s_GetDevices(&deviceCount);
+	for (int deviceIdx = 0; deviceIdx < deviceCount; ++deviceIdx)
 	{
-		const S2S_TargetCfg* cfg = s2s_getConfigByIndex(i);
-		if (cfg && (cfg->scsiId & S2S_CFG_TARGET_ENABLED))
+		int targetCount;
+		S2S_Target* targets = devices[deviceIdx].getTargets(devices + deviceIdx, &targetCount);
+		for (int targetIdx = 0; targetIdx < targetCount; ++targetIdx)
 		{
-			if (count == lun)
-			{
-				return cfg;
-			}
-			else
+			S2S_Target* target = targets + targetIdx;
+			if (target && (target->cfg->scsiId & S2S_CFG_TARGET_ENABLED))
 			{
-				count++;
+				if (count == lun)
+				{
+					return target;
+				}
+				else
+				{
+					count++;
+				}
 			}
 		}
 	}
-	return s2s_getConfigByIndex(0); // Fallback, try not to crash
+	int dummy;
+	return devices[0].getTargets(devices, &dummy); // Fallback, try not to crash
 }
 
 int8_t s2s_usbd_storage_Init(uint8_t lun)
@@ -96,30 +104,31 @@ int8_t s2s_usbd_storage_Init(uint8_t lun)
 
 int8_t s2s_usbd_storage_GetCapacity (uint8_t lun, uint32_t *block_num, uint16_t *block_size)
 {
-	const S2S_TargetCfg* cfg = getUsbConfig(lun);
+	const S2S_Target* target = getUsbTarget(lun);
 
 	uint32_t capacity = getScsiCapacity(
-		cfg->sdSectorStart,
-		cfg->bytesPerSector,
-		cfg->scsiSectors);
+		target->device,
+		target->cfg->sdSectorStart,
+		target->state.bytesPerSector,
+		target->cfg->scsiSectors);
 
 	*block_num  = capacity;
-	*block_size = cfg->bytesPerSector;
+	*block_size = target->state.bytesPerSector;
 	return (0);
 }
 
 uint32_t s2s_usbd_storage_Inquiry (uint8_t lun, uint8_t* buf, uint8_t maxlen)
 {
-	const S2S_TargetCfg* cfg = getUsbConfig(lun);
+	const S2S_Target* target = getUsbTarget(lun);
 
-	return s2s_getStandardInquiry(cfg, buf, maxlen);
+	return s2s_getStandardInquiry(target->cfg, buf, maxlen);
 }
 
 int8_t s2s_usbd_storage_IsReady (uint8_t lun)
 {
-	const S2S_TargetCfg* cfg = getUsbConfig(lun);
+	const S2S_Target* target = getUsbTarget(lun);
 	return (
-			cfg &&
+			target &&
 			(blockDev.state & DISK_PRESENT) &&
 			(blockDev.state & DISK_INITIALISED)
 			) ? 0 : 1; // inverse logic
@@ -137,24 +146,24 @@ int8_t s2s_usbd_storage_Read (uint8_t lun,
 		uint16_t blk_len)
 {
 	s2s_ledOn();
-	const S2S_TargetCfg* cfg = getUsbConfig(lun);
+	const S2S_Target* target = getUsbTarget(lun);
 
-	if (cfg->bytesPerSector == 512)
+	if (target->state.bytesPerSector == 512)
 	{
 		BSP_SD_ReadBlocks_DMA(
 			(uint32_t*) buf,
-			(cfg->sdSectorStart + blk_addr) * 512ll,
+			(target->cfg->sdSectorStart + blk_addr) * 512ll,
 			512,
 			blk_len);
 	}
 	else
 	{
-		int sdPerScsi = SDSectorsPerSCSISector(cfg->bytesPerSector);
-		int sdSectorNum =  cfg->sdSectorStart + (blk_addr * sdPerScsi);
+		int sdPerScsi = SDSectorsPerSCSISector(target->state.bytesPerSector);
+		int sdSectorNum =  target->cfg->sdSectorStart + (blk_addr * sdPerScsi);
 
 		for (int blk = 0; blk < blk_len; ++blk)
 		{
-			for (int i = 0; i < SDSectorsPerSCSISector(cfg->bytesPerSector); ++i)
+			for (int i = 0; i < SDSectorsPerSCSISector(target->state.bytesPerSector); ++i)
 			{
 				uint8_t partial[512] S2S_DMA_ALIGN;
 				BSP_SD_ReadBlocks_DMA(
@@ -164,16 +173,14 @@ int8_t s2s_usbd_storage_Read (uint8_t lun,
 					1);
 				sdSectorNum++;
 
-				int validBytes = cfg->bytesPerSector % SD_SECTOR_SIZE;
+				int validBytes = target->state.bytesPerSector % SD_SECTOR_SIZE;
 				if (validBytes == 0) validBytes = SD_SECTOR_SIZE;
 
 				memcpy(buf, partial, validBytes);
 
 				buf += validBytes;
 			}
-
 		}
-
 	}
 	s2s_ledOff();
 	return 0;
@@ -185,24 +192,24 @@ int8_t s2s_usbd_storage_Write (uint8_t lun,
 		uint16_t blk_len)
 {
 	s2s_ledOn();
-	const S2S_TargetCfg* cfg = getUsbConfig(lun);
+	const S2S_Target* target = getUsbTarget(lun);
 
-	if (cfg->bytesPerSector == 512)
+	if (target->state.bytesPerSector == 512)
 	{
 		BSP_SD_WriteBlocks_DMA(
 			(uint32_t*) buf,
-			(cfg->sdSectorStart + blk_addr) * 512ll,
+			(target->cfg->sdSectorStart + blk_addr) * 512ll,
 			512,
 			blk_len);
 	}
 	else
 	{
-		int sdPerScsi = SDSectorsPerSCSISector(cfg->bytesPerSector);
-		int sdSectorNum =  cfg->sdSectorStart + (blk_addr * sdPerScsi);
+		int sdPerScsi = SDSectorsPerSCSISector(target->state.bytesPerSector);
+		int sdSectorNum =  target->cfg->sdSectorStart + (blk_addr * sdPerScsi);
 
 		for (int blk = 0; blk < blk_len; ++blk)
 		{
-			for (int i = 0; i < SDSectorsPerSCSISector(cfg->bytesPerSector); ++i)
+			for (int i = 0; i < SDSectorsPerSCSISector(target->state.bytesPerSector); ++i)
 			{
 				uint8_t partial[512] S2S_DMA_ALIGN;
 				memcpy(partial, buf, 512);
@@ -214,7 +221,7 @@ int8_t s2s_usbd_storage_Write (uint8_t lun,
 					1);
 				sdSectorNum++;
 
-				int validBytes = cfg->bytesPerSector % SD_SECTOR_SIZE;
+				int validBytes = target->state.bytesPerSector % SD_SECTOR_SIZE;
 				if (validBytes == 0) validBytes = SD_SECTOR_SIZE;
 
 				buf += validBytes;
@@ -231,12 +238,19 @@ int8_t s2s_usbd_storage_Write (uint8_t lun,
 int8_t s2s_usbd_storage_GetMaxLun (void)
 {
 	int count = 0;
-	for (int i = 0; i < S2S_MAX_TARGETS; ++i)
+	int deviceCount;
+	S2S_Device* devices = s2s_GetDevices(&deviceCount);
+	for (int deviceIdx = 0; deviceIdx < deviceCount; ++deviceIdx)
 	{
-		const S2S_TargetCfg* cfg = s2s_getConfigByIndex(i);
-		if (cfg && (cfg->scsiId & S2S_CFG_TARGET_ENABLED))
+		int targetCount;
+		S2S_Target* targets = devices[deviceIdx].getTargets(devices + deviceIdx, &targetCount);
+		for (int targetIdx = 0; targetIdx < targetCount; ++targetIdx)
 		{
-			count++;
+			S2S_Target* target = targets + targetIdx;
+			if (target && (target->cfg->scsiId & S2S_CFG_TARGET_ENABLED))
+			{
+				count++;
+			}
 		}
 	}
 	return count - 1 < 0 ? 0 : count - 1;
-- 
2.38.5