Merge branch 'master' of ssh://webhost.codesrc.com/home/michael/projects/SCSI2SD
[SCSI2SD.git] / software / SCSI2SD / src / scsi.c
index 8d64270..48b4c6d 100755 (executable)
@@ -14,6 +14,8 @@
 //\r
 //     You should have received a copy of the GNU General Public License\r
 //     along with SCSI2SD.  If not, see <http://www.gnu.org/licenses/>.\r
+#pragma GCC push_options\r
+#pragma GCC optimize("-flto")\r
 \r
 #include "device.h"\r
 #include "scsi.h"\r
@@ -27,6 +29,8 @@
 #include "mode.h"\r
 #include "disk.h"\r
 #include "time.h"\r
+#include "cdrom.h"\r
+#include "debug.h"\r
 \r
 #include <string.h>\r
 \r
@@ -37,9 +41,7 @@ static void enter_SelectionPhase(void);
 static void process_SelectionPhase(void);\r
 static void enter_BusFree(void);\r
 static void enter_MessageIn(uint8 message);\r
-static void process_MessageIn(void);\r
 static void enter_Status(uint8 status);\r
-static void process_Status(void);\r
 static void enter_DataIn(int len);\r
 static void process_DataIn(void);\r
 static void process_DataOut(void);\r
@@ -51,7 +53,16 @@ static void enter_BusFree()
 {\r
        // This delay probably isn't needed for most SCSI hosts, but it won't\r
        // hurt either. It's possible some of the samplers needed this delay.\r
-       CyDelayUs(2);\r
+       if (scsiDev.compatMode < COMPAT_SCSI2)\r
+       {\r
+               CyDelayUs(2);\r
+       }\r
+\r
+       if (scsiDev.status != GOOD && isDebugEnabled())\r
+       {\r
+               // We want to capture debug information for failure cases.\r
+               CyDelay(64);\r
+       }\r
 \r
        SCSI_ClearPin(SCSI_Out_BSY);\r
        // We now have a Bus Clear Delay of 800ns to release remaining signals.\r
@@ -71,12 +82,12 @@ static void enter_MessageIn(uint8 message)
        scsiDev.phase = MESSAGE_IN;\r
 }\r
 \r
-static void process_MessageIn()\r
+void process_MessageIn()\r
 {\r
        scsiEnterPhase(MESSAGE_IN);\r
        scsiWriteByte(scsiDev.msgIn);\r
 \r
-       if (scsiDev.atnFlag)\r
+       if (unlikely(scsiDev.atnFlag))\r
        {\r
                // If there was a parity error, we go\r
                // back to MESSAGE_OUT first, get out parity error message, then come\r
@@ -113,10 +124,11 @@ static void enter_Status(uint8 status)
        scsiDev.phase = STATUS;\r
 \r
        scsiDev.lastStatus = scsiDev.status;\r
-       scsiDev.lastSense = scsiDev.sense.code;\r
+       scsiDev.lastSense = scsiDev.target->sense.code;\r
+       scsiDev.lastSenseASC = scsiDev.target->sense.asc;\r
 }\r
 \r
-static void process_Status()\r
+void process_Status()\r
 {\r
        scsiEnterPhase(STATUS);\r
 \r
@@ -143,7 +155,9 @@ static void process_Status()
        scsiWriteByte(scsiDev.status);\r
 \r
        scsiDev.lastStatus = scsiDev.status;\r
-       scsiDev.lastSense = scsiDev.sense.code;\r
+       scsiDev.lastSense = scsiDev.target->sense.code;\r
+       scsiDev.lastSenseASC = scsiDev.target->sense.asc;\r
+\r
 \r
        // Command Complete occurs AFTER a valid status has been\r
        // sent. then we go bus-free.\r
@@ -198,10 +212,12 @@ static void process_DataOut()
                scsiRead(scsiDev.data + scsiDev.dataPtr, len);\r
                scsiDev.dataPtr += len;\r
 \r
-               if (scsiDev.parityError && config->enableParity && !scsiDev.compatMode)\r
+               if (scsiDev.parityError &&\r
+                       (scsiDev.target->cfg->flags & CONFIG_ENABLE_PARITY) &&\r
+                       (scsiDev.compatMode >= COMPAT_SCSI2))\r
                {\r
-                       scsiDev.sense.code = ABORTED_COMMAND;\r
-                       scsiDev.sense.asc = SCSI_PARITY_ERROR;\r
+                       scsiDev.target->sense.code = ABORTED_COMMAND;\r
+                       scsiDev.target->sense.asc = SCSI_PARITY_ERROR;\r
                        enter_Status(CHECK_CONDITION);\r
                }\r
        }\r
@@ -248,25 +264,28 @@ static void process_Command()
        control = scsiDev.cdb[scsiDev.cdbLen - 1];\r
 \r
        scsiDev.cmdCount++;\r
+       TargetConfig* cfg = scsiDev.target->cfg;\r
 \r
-       if (scsiDev.resetFlag)\r
+       if (unlikely(scsiDev.resetFlag))\r
        {\r
                // Don't log bogus commands\r
                scsiDev.cmdCount--;\r
                memset(scsiDev.cdb, 0xff, sizeof(scsiDev.cdb));\r
                return;\r
        }\r
-       else if (scsiDev.parityError && config->enableParity && !scsiDev.compatMode)\r
+       else if (scsiDev.parityError &&\r
+               (cfg->flags & CONFIG_ENABLE_PARITY) &&\r
+               (scsiDev.compatMode >= COMPAT_SCSI2))\r
        {\r
-               scsiDev.sense.code = ABORTED_COMMAND;\r
-               scsiDev.sense.asc = SCSI_PARITY_ERROR;\r
+               scsiDev.target->sense.code = ABORTED_COMMAND;\r
+               scsiDev.target->sense.asc = SCSI_PARITY_ERROR;\r
                enter_Status(CHECK_CONDITION);\r
        }\r
        else if ((control & 0x02) && ((control & 0x01) == 0))\r
        {\r
                // FLAG set without LINK flag.\r
-               scsiDev.sense.code = ILLEGAL_REQUEST;\r
-               scsiDev.sense.asc = INVALID_FIELD_IN_CDB;\r
+               scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
+               scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;\r
                enter_Status(CHECK_CONDITION);\r
        }\r
        else if (command == 0x12)\r
@@ -284,7 +303,7 @@ static void process_Command()
 \r
                memset(scsiDev.data, 0, 256); // Max possible alloc length\r
                scsiDev.data[0] = 0xF0;\r
-               scsiDev.data[2] = scsiDev.sense.code & 0x0F;\r
+               scsiDev.data[2] = scsiDev.target->sense.code & 0x0F;\r
 \r
                scsiDev.data[3] = transfer.lba >> 24;\r
                scsiDev.data[4] = transfer.lba >> 16;\r
@@ -293,46 +312,61 @@ static void process_Command()
 \r
                // Additional bytes if there are errors to report\r
                scsiDev.data[7] = 10; // additional length\r
-               scsiDev.data[12] = scsiDev.sense.asc >> 8;\r
-               scsiDev.data[13] = scsiDev.sense.asc;\r
+               scsiDev.data[12] = scsiDev.target->sense.asc >> 8;\r
+               scsiDev.data[13] = scsiDev.target->sense.asc;\r
 \r
                // Silently truncate results. SCSI-2 spec 8.2.14.\r
                enter_DataIn(allocLength);\r
 \r
                // This is a good time to clear out old sense information.\r
-               scsiDev.sense.code = NO_SENSE;\r
-               scsiDev.sense.asc = NO_ADDITIONAL_SENSE_INFORMATION;\r
+               scsiDev.target->sense.code = NO_SENSE;\r
+               scsiDev.target->sense.asc = NO_ADDITIONAL_SENSE_INFORMATION;\r
        }\r
        // Some old SCSI drivers do NOT properly support\r
        // unitAttention. eg. the Mac Plus would trigger a SCSI reset\r
        // on receiving the unit attention response on boot, thus\r
        // triggering another unit attention condition.\r
-       else if (scsiDev.unitAttention && config->enableUnitAttention)\r
+       else if (scsiDev.target->unitAttention &&\r
+               (cfg->flags & CONFIG_ENABLE_UNIT_ATTENTION))\r
        {\r
-               scsiDev.sense.code = UNIT_ATTENTION;\r
-               scsiDev.sense.asc = scsiDev.unitAttention;\r
+               scsiDev.target->sense.code = UNIT_ATTENTION;\r
+               scsiDev.target->sense.asc = scsiDev.target->unitAttention;\r
 \r
                // If initiator doesn't do REQUEST SENSE for the next command, then\r
                // data is lost.\r
-               scsiDev.unitAttention = 0;\r
+               scsiDev.target->unitAttention = 0;\r
 \r
                enter_Status(CHECK_CONDITION);\r
        }\r
        else if (scsiDev.lun)\r
        {\r
-               scsiDev.sense.code = ILLEGAL_REQUEST;\r
-               scsiDev.sense.asc = LOGICAL_UNIT_NOT_SUPPORTED;\r
+               scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
+               scsiDev.target->sense.asc = LOGICAL_UNIT_NOT_SUPPORTED;\r
                enter_Status(CHECK_CONDITION);\r
        }\r
        else if (command == 0x17 || command == 0x16)\r
        {\r
                doReserveRelease();\r
        }\r
-       else if ((scsiDev.reservedId >= 0) &&\r
-               (scsiDev.reservedId != scsiDev.initiatorId))\r
+       else if ((scsiDev.target->reservedId >= 0) &&\r
+               (scsiDev.target->reservedId != scsiDev.initiatorId))\r
        {\r
                enter_Status(CONFLICT);\r
        }\r
+       // Handle odd device types first that may override basic read and\r
+       // write commands. Will fall-through to generic disk handling.\r
+       else if (((cfg->deviceType == CONFIG_OPTICAL) && scsiCDRomCommand()) ||\r
+               ((cfg->deviceType == CONFIG_SEQUENTIAL) && scsiTapeCommand()) ||\r
+               ((cfg->deviceType == CONFIG_MO) && scsiMOCommand()))\r
+       {\r
+               // Already handled.\r
+       }\r
+       else if (scsiDiskCommand())\r
+       {\r
+               // Already handled.\r
+               // check for the performance-critical read/write\r
+               // commands ASAP.\r
+       }\r
        else if (command == 0x1C)\r
        {\r
                scsiReceiveDiagnostic();\r
@@ -341,16 +375,18 @@ static void process_Command()
        {\r
                scsiSendDiagnostic();\r
        }\r
+       else if (command == 0x3B)\r
+       {\r
+               scsiWriteBuffer();\r
+       }\r
        else if (command == 0x3C)\r
        {\r
                scsiReadBuffer();\r
        }\r
-       else if (\r
-               !scsiModeCommand() &&\r
-               !scsiDiskCommand())\r
+       else if (!scsiModeCommand())\r
        {\r
-               scsiDev.sense.code = ILLEGAL_REQUEST;\r
-               scsiDev.sense.asc = INVALID_COMMAND_OPERATION_CODE;\r
+               scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
+               scsiDev.target->sense.asc = INVALID_COMMAND_OPERATION_CODE;\r
                enter_Status(CHECK_CONDITION);\r
        }\r
 \r
@@ -370,25 +406,25 @@ static void doReserveRelease()
        uint8 command = scsiDev.cdb[0];\r
 \r
        int canRelease =\r
-               (!thirdPty && (scsiDev.initiatorId == scsiDev.reservedId)) ||\r
+               (!thirdPty && (scsiDev.initiatorId == scsiDev.target->reservedId)) ||\r
                        (thirdPty &&\r
-                               (scsiDev.reserverId == scsiDev.initiatorId) &&\r
-                               (scsiDev.reservedId == thirdPtyId)\r
+                               (scsiDev.target->reserverId == scsiDev.initiatorId) &&\r
+                               (scsiDev.target->reservedId == thirdPtyId)\r
                        );\r
 \r
        if (extentReservation)\r
        {\r
                // Not supported.\r
-               scsiDev.sense.code = ILLEGAL_REQUEST;\r
-               scsiDev.sense.asc = INVALID_FIELD_IN_CDB;\r
+               scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
+               scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;\r
                enter_Status(CHECK_CONDITION);\r
        }\r
        else if (command == 0x17) // release\r
        {\r
-               if ((scsiDev.reservedId < 0) || canRelease)\r
+               if ((scsiDev.target->reservedId < 0) || canRelease)\r
                {\r
-                       scsiDev.reservedId = -1;\r
-                       scsiDev.reserverId = -1;\r
+                       scsiDev.target->reservedId = -1;\r
+                       scsiDev.target->reserverId = -1;\r
                }\r
                else\r
                {\r
@@ -397,16 +433,16 @@ static void doReserveRelease()
        }\r
        else // assume reserve.\r
        {\r
-               if ((scsiDev.reservedId < 0) || canRelease)\r
+               if ((scsiDev.target->reservedId < 0) || canRelease)\r
                {\r
-                       scsiDev.reserverId = scsiDev.initiatorId;\r
+                       scsiDev.target->reserverId = scsiDev.initiatorId;\r
                        if (thirdPty)\r
                        {\r
-                               scsiDev.reservedId = thirdPtyId;\r
+                               scsiDev.target->reservedId = thirdPtyId;\r
                        }\r
                        else\r
                        {\r
-                               scsiDev.reservedId = scsiDev.initiatorId;\r
+                               scsiDev.target->reservedId = scsiDev.initiatorId;\r
                        }\r
                }\r
                else\r
@@ -430,15 +466,20 @@ static void scsiReset()
        scsiDev.atnFlag = 0;\r
        scsiDev.resetFlag = 0;\r
        scsiDev.lun = -1;\r
+       scsiDev.compatMode = COMPAT_UNKNOWN;\r
 \r
-       if (scsiDev.unitAttention != POWER_ON_RESET)\r
+       if (scsiDev.target)\r
        {\r
-               scsiDev.unitAttention = SCSI_BUS_RESET;\r
+               if (scsiDev.target->unitAttention != POWER_ON_RESET)\r
+               {\r
+                       scsiDev.target->unitAttention = SCSI_BUS_RESET;\r
+               }\r
+               scsiDev.target->reservedId = -1;\r
+               scsiDev.target->reserverId = -1;\r
+               scsiDev.target->sense.code = NO_SENSE;\r
+               scsiDev.target->sense.asc = NO_ADDITIONAL_SENSE_INFORMATION;\r
        }\r
-       scsiDev.reservedId = -1;\r
-       scsiDev.reserverId = -1;\r
-       scsiDev.sense.code = NO_SENSE;\r
-       scsiDev.sense.asc = NO_ADDITIONAL_SENSE_INFORMATION;\r
+       scsiDev.target = NULL;\r
        scsiDiskReset();\r
 \r
        scsiDev.postDataOutHook = NULL;\r
@@ -449,8 +490,9 @@ static void scsiReset()
        // There is no guarantee that the RST line will be negated by then.\r
        // NOTE: We could be connected and powered by USB for configuration,\r
        // in which case TERMPWR cannot be supplied, and reset will ALWAYS\r
-       // be true.\r
-       CyDelay(10); // 10ms.\r
+       // be true. Therefore, the sleep here must be slow to avoid slowing\r
+       // USB comms\r
+       CyDelay(1); // 1ms.\r
 }\r
 \r
 static void enter_SelectionPhase()\r
@@ -466,7 +508,9 @@ static void enter_SelectionPhase()
        scsiDev.phase = SELECTION;\r
        scsiDev.lun = -1;\r
        scsiDev.discPriv = 0;\r
-       scsiDev.compatMode = 0;\r
+\r
+       scsiDev.initiatorId = -1;\r
+       scsiDev.target = NULL;\r
 \r
        transfer.blocks = 0;\r
        transfer.currentBlock = 0;\r
@@ -476,6 +520,12 @@ static void enter_SelectionPhase()
 \r
 static void process_SelectionPhase()\r
 {\r
+       if (scsiDev.compatMode < COMPAT_SCSI2)\r
+       {\r
+               // Required for some older SCSI1 devices using a 5380 chip.\r
+               CyDelay(1);\r
+       }\r
+\r
        int sel = SCSI_ReadFilt(SCSI_Filt_SEL);\r
        int bsy = SCSI_ReadFilt(SCSI_Filt_BSY);\r
 \r
@@ -486,11 +536,23 @@ static void process_SelectionPhase()
        int goodParity = (Lookup_OddParity[mask] == SCSI_ReadPin(SCSI_In_DBP));\r
        int atnFlag = SCSI_ReadFilt(SCSI_Filt_ATN);\r
 \r
+       int tgtIndex;\r
+       TargetState* target = NULL;\r
+       for (tgtIndex = 0; tgtIndex < MAX_SCSI_TARGETS; ++tgtIndex)\r
+       {\r
+               if (mask & (1 << scsiDev.targets[tgtIndex].targetId))\r
+               {\r
+                       target = &scsiDev.targets[tgtIndex];\r
+                       break;\r
+               }\r
+       }\r
        if (!bsy && sel &&\r
-               (mask & scsiDev.scsiIdMask) &&\r
-               (goodParity || !config->enableParity || !atnFlag) &&\r
-               (maskBitCount <= 2))\r
+               target &&\r
+               (goodParity || !(target->cfg->flags & CONFIG_ENABLE_PARITY) || !atnFlag) &&\r
+               likely(maskBitCount <= 2))\r
        {\r
+               scsiDev.target = target;\r
+\r
                // Do we enter MESSAGE OUT immediately ? SCSI 1 and 2 standards says\r
                // move to MESSAGE OUT if ATN is true before we assert BSY.\r
                // The initiator should assert ATN with SEL.\r
@@ -502,8 +564,16 @@ static void process_SelectionPhase()
                // controllers don't generate parity bits.\r
                if (!scsiDev.atnFlag)\r
                {\r
-                       scsiDev.unitAttention = 0;\r
-                       scsiDev.compatMode = 1;\r
+                       target->unitAttention = 0;\r
+                       scsiDev.compatMode = COMPAT_SCSI1;\r
+               }\r
+               else if (!(target->cfg->flags & CONFIG_ENABLE_SCSI2))\r
+               {\r
+                       scsiDev.compatMode = COMPAT_SCSI1;\r
+               }\r
+               else if (scsiDev.compatMode == COMPAT_UNKNOWN)\r
+               {\r
+                       scsiDev.compatMode = COMPAT_SCSI2;\r
                }\r
 \r
                // We've been selected!\r
@@ -518,7 +588,7 @@ static void process_SelectionPhase()
                scsiDev.selCount++;\r
 \r
                // Wait until the end of the selection phase.\r
-               while (!scsiDev.resetFlag)\r
+               while (likely(!scsiDev.resetFlag))\r
                {\r
                        if (!SCSI_ReadFilt(SCSI_Filt_SEL))\r
                        {\r
@@ -529,11 +599,10 @@ static void process_SelectionPhase()
                // Save our initiator now that we're no longer in a time-critical\r
                // section.\r
                // SCSI1/SASI initiators may not set their own ID.\r
-               if (maskBitCount == 2)\r
                {\r
                        int i;\r
-                       uint8 initiatorMask = mask ^ scsiDev.scsiIdMask;\r
-                       scsiDev.initiatorId = 0;\r
+                       uint8_t initiatorMask = mask ^ (1 << target->targetId);\r
+                       scsiDev.initiatorId = -1;\r
                        for (i = 0; i < 8; ++i)\r
                        {\r
                                if (initiatorMask & (1 << i))\r
@@ -543,10 +612,6 @@ static void process_SelectionPhase()
                                }\r
                        }\r
                }\r
-               else\r
-               {\r
-                       scsiDev.initiatorId = -1;\r
-               }\r
 \r
                scsiDev.phase = COMMAND;\r
        }\r
@@ -565,7 +630,9 @@ static void process_MessageOut()
        scsiDev.msgOut = scsiReadByte();\r
        scsiDev.msgCount++;\r
 \r
-       if (scsiDev.parityError && config->enableParity && !scsiDev.compatMode)\r
+       if (scsiDev.parityError &&\r
+               (scsiDev.target->cfg->flags & CONFIG_ENABLE_PARITY) &&\r
+               (scsiDev.compatMode >= COMPAT_SCSI2))\r
        {\r
                // Skip the remaining message bytes, and then start the MESSAGE_OUT\r
                // phase again from the start. The initiator will re-send the\r
@@ -596,11 +663,11 @@ static void process_MessageOut()
 \r
                scsiDiskReset();\r
 \r
-               scsiDev.unitAttention = SCSI_BUS_RESET;\r
+               scsiDev.target->unitAttention = SCSI_BUS_RESET;\r
 \r
                // ANY initiator can reset the reservation state via this message.\r
-               scsiDev.reservedId = -1;\r
-               scsiDev.reserverId = -1;\r
+               scsiDev.target->reservedId = -1;\r
+               scsiDev.target->reserverId = -1;\r
                enter_BusFree();\r
        }\r
        else if (scsiDev.msgOut == 0x05)\r
@@ -645,7 +712,7 @@ static void process_MessageOut()
                }\r
 \r
                scsiDev.lun = scsiDev.msgOut & 0x7;\r
-               scsiDev.discPriv =\r
+               scsiDev.discPriv = \r
                        ((scsiDev.msgOut & 0x40) && (scsiDev.initiatorId >= 0))\r
                                ? 1 : 0;\r
        }\r
@@ -661,20 +728,32 @@ static void process_MessageOut()
                // Extended message.\r
                int msgLen = scsiReadByte();\r
                if (msgLen == 0) msgLen = 256;\r
+               uint8_t extmsg[256];\r
                for (i = 0; i < msgLen && !scsiDev.resetFlag; ++i)\r
                {\r
                        // Discard bytes.\r
-                       scsiReadByte();\r
+                       extmsg[i] = scsiReadByte();\r
                }\r
 \r
-               // We don't support ANY extended messages.\r
-               // Modify Data Pointer:  We don't support reselection.\r
-               // Wide Data Transfer Request: No. 8bit only.\r
-               // Synchronous data transfer request. No, we can't do that.\r
-               // We don't support any 2-byte messages either.\r
-               // And we don't support any optional 1-byte messages.\r
-               // In each case, the correct response is MESSAGE REJECT.\r
-               messageReject();\r
+               if (extmsg[0] == 3 && msgLen == 2) // Wide Data Request\r
+               {\r
+                       // Negotiate down to 8bit\r
+                       scsiEnterPhase(MESSAGE_IN);\r
+                       static const uint8_t WDTR[] = {0x01, 0x02, 0x03, 0x00};\r
+                       scsiWrite(WDTR, sizeof(WDTR));\r
+               }\r
+               else if (extmsg[0] == 1 && msgLen == 3) // Synchronous data request\r
+               {\r
+                       // Negotiate back to async\r
+                       scsiEnterPhase(MESSAGE_IN);\r
+                       static const uint8_t SDTR[] = {0x01, 0x03, 0x01, 0x00, 0x00};\r
+                       scsiWrite(SDTR, sizeof(SDTR));\r
+               }\r
+               else\r
+               {\r
+                       // Not supported\r
+                       messageReject();\r
+               }\r
        }\r
        else\r
        {\r
@@ -687,7 +766,7 @@ static void process_MessageOut()
 \r
 void scsiPoll(void)\r
 {\r
-       if (scsiDev.resetFlag)\r
+       if (unlikely(scsiDev.resetFlag))\r
        {\r
                scsiReset();\r
                if ((scsiDev.resetFlag = SCSI_ReadFilt(SCSI_Filt_RST)))\r
@@ -810,14 +889,34 @@ void scsiPoll(void)
 \r
 void scsiInit()\r
 {\r
-       scsiDev.scsiIdMask = 1 << (config->scsiId);\r
-\r
        scsiDev.atnFlag = 0;\r
        scsiDev.resetFlag = 1;\r
        scsiDev.phase = BUS_FREE;\r
-       scsiDev.reservedId = -1;\r
-       scsiDev.reserverId = -1;\r
-       scsiDev.unitAttention = POWER_ON_RESET;\r
+       scsiDev.target = NULL;\r
+       scsiDev.compatMode = COMPAT_UNKNOWN;\r
+\r
+       int i;\r
+       for (i = 0; i < MAX_SCSI_TARGETS; ++i)\r
+       {\r
+               const TargetConfig* cfg = getConfigByIndex(i);\r
+               if (cfg && (cfg->scsiId & CONFIG_TARGET_ENABLED))\r
+               {\r
+                       scsiDev.targets[i].targetId = cfg->scsiId & CONFIG_TARGET_ID_BITS;\r
+                       scsiDev.targets[i].cfg = cfg;\r
+\r
+                       scsiDev.targets[i].liveCfg.bytesPerSector = cfg->bytesPerSector;\r
+               }\r
+               else\r
+               {\r
+                       scsiDev.targets[i].targetId = 0xff;\r
+                       scsiDev.targets[i].cfg = NULL;\r
+               }\r
+               scsiDev.targets[i].reservedId = -1;\r
+               scsiDev.targets[i].reserverId = -1;\r
+               scsiDev.targets[i].unitAttention = POWER_ON_RESET;\r
+               scsiDev.targets[i].sense.code = NO_SENSE;\r
+               scsiDev.targets[i].sense.asc = NO_ADDITIONAL_SENSE_INFORMATION;\r
+       }\r
 }\r
 \r
 void scsiDisconnect()\r
@@ -852,7 +951,8 @@ int scsiReconnect()
        {\r
                // Arbitrate.\r
                ledOn();\r
-               SCSI_Out_Bits_Write(scsiDev.scsiIdMask);\r
+               uint8_t scsiIdMask = 1 << scsiDev.target->targetId;\r
+               SCSI_Out_Bits_Write(scsiIdMask);\r
                SCSI_Out_Ctl_Write(1); // Write bits manually.\r
                SCSI_SetPin(SCSI_Out_BSY);\r
 \r
@@ -860,7 +960,7 @@ int scsiReconnect()
 \r
                uint8_t dbx = scsiReadDBxPins();\r
                sel = SCSI_ReadFilt(SCSI_Filt_SEL);\r
-               if (sel || ((dbx ^ scsiDev.scsiIdMask) > scsiDev.scsiIdMask))\r
+               if (sel || ((dbx ^ scsiIdMask) > scsiIdMask))\r
                {\r
                        // Lost arbitration.\r
                        SCSI_Out_Ctl_Write(0);\r
@@ -875,7 +975,7 @@ int scsiReconnect()
 \r
                        // Reselection phase\r
                        SCSI_CTL_PHASE_Write(__scsiphase_io);\r
-                       SCSI_Out_Bits_Write(scsiDev.scsiIdMask | (1 << scsiDev.initiatorId));\r
+                       SCSI_Out_Bits_Write(scsiIdMask | (1 << scsiDev.initiatorId));\r
                        scsiDeskewDelay(); // 2 deskew delays\r
                        scsiDeskewDelay(); // 2 deskew delays\r
                        SCSI_ClearPin(SCSI_Out_BSY);\r
@@ -887,7 +987,7 @@ int scsiReconnect()
                        while (\r
                                !bsy &&\r
                                !scsiDev.resetFlag &&\r
-                               (diffTime_ms(waitStart_ms, getTime_ms()) < 250))\r
+                               (elapsedTime_ms(waitStart_ms) < 250))\r
                        {\r
                                bsy = SCSI_ReadFilt(SCSI_Filt_BSY);\r
                        }\r
@@ -922,3 +1022,4 @@ int scsiReconnect()
        return reconnected;\r
 }\r
 \r
+#pragma GCC pop_options\r