Merge branch 'master' of ssh://webhost.codesrc.com/home/michael/projects/SCSI2SD
[SCSI2SD.git] / software / SCSI2SD / src / scsi.c
index 74fe177..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
@@ -26,6 +28,9 @@
 #include "led.h"\r
 #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
@@ -36,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
@@ -50,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
@@ -70,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
@@ -112,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
@@ -142,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
@@ -197,10 +212,12 @@ static void process_DataOut()
                scsiRead(scsiDev.data + scsiDev.dataPtr, len);\r
                scsiDev.dataPtr += len;\r
 \r
-               if (scsiDev.parityError && config->enableParity)\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
@@ -247,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)\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
@@ -283,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
@@ -292,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
@@ -340,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
@@ -369,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
@@ -396,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
@@ -429,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
@@ -448,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
@@ -464,6 +507,10 @@ static void enter_SelectionPhase()
        scsiDev.status = GOOD;\r
        scsiDev.phase = SELECTION;\r
        scsiDev.lun = -1;\r
+       scsiDev.discPriv = 0;\r
+\r
+       scsiDev.initiatorId = -1;\r
+       scsiDev.target = NULL;\r
 \r
        transfer.blocks = 0;\r
        transfer.currentBlock = 0;\r
@@ -473,29 +520,60 @@ static void enter_SelectionPhase()
 \r
 static void process_SelectionPhase()\r
 {\r
-       int sel = SCSI_ReadPin(SCSI_In_SEL);\r
-       int bsy = SCSI_ReadPin(SCSI_In_BSY);\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
        // Only read these pins AFTER SEL and BSY - we don't want to catch them\r
        // during a transition period.\r
        uint8 mask = scsiReadDBxPins();\r
        int maskBitCount = countBits(mask);\r
        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) && (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
-               scsiDev.atnFlag = SCSI_ReadPin(SCSI_ATN_INT);\r
-               \r
-               // Unit attention breaks many older SCSI hosts. Disable it completely for\r
-               // SCSI-1 (and older) hosts, regardless of our configured setting.\r
+               scsiDev.atnFlag = atnFlag;\r
+\r
+               // Unit attention breaks many older SCSI hosts. Disable it completely\r
+               // for SCSI-1 (and older) hosts, regardless of our configured setting.\r
+               // Enable the compatability mode also as many SASI and SCSI1\r
+               // controllers don't generate parity bits.\r
                if (!scsiDev.atnFlag)\r
                {\r
-                       scsiDev.unitAttention = 0;\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
@@ -510,9 +588,9 @@ 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_ReadPin(SCSI_In_SEL))\r
+                       if (!SCSI_ReadFilt(SCSI_Filt_SEL))\r
                        {\r
                                break;\r
                        }\r
@@ -521,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
@@ -535,10 +612,6 @@ static void process_SelectionPhase()
                                }\r
                        }\r
                }\r
-               else\r
-               {\r
-                       scsiDev.initiatorId = -1;\r
-               }\r
 \r
                scsiDev.phase = COMMAND;\r
        }\r
@@ -557,12 +630,14 @@ static void process_MessageOut()
        scsiDev.msgOut = scsiReadByte();\r
        scsiDev.msgCount++;\r
 \r
-       if (scsiDev.parityError)\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
                // same set of messages.\r
-               while (SCSI_ReadPin(SCSI_ATN_INT) && !scsiDev.resetFlag)\r
+               while (SCSI_ReadFilt(SCSI_Filt_ATN) && !scsiDev.resetFlag)\r
                {\r
                        scsiReadByte();\r
                }\r
@@ -588,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
@@ -630,7 +705,6 @@ static void process_MessageOut()
        else if (scsiDev.msgOut & 0x80) // 0x80 -> 0xFF\r
        {\r
                // IDENTIFY\r
-               // We don't disconnect, so ignore disconnect privilege.\r
                if ((scsiDev.msgOut & 0x18) || // Reserved bits set.\r
                        (scsiDev.msgOut & 0x20))  // We don't have any target routines!\r
                {\r
@@ -638,7 +712,9 @@ static void process_MessageOut()
                }\r
 \r
                scsiDev.lun = scsiDev.msgOut & 0x7;\r
-               //scsiDev.allowDisconnect = scsiDev.msgOut & 0x40;\r
+               scsiDev.discPriv = \r
+                       ((scsiDev.msgOut & 0x40) && (scsiDev.initiatorId >= 0))\r
+                               ? 1 : 0;\r
        }\r
        else if (scsiDev.msgOut >= 0x20 && scsiDev.msgOut <= 0x2F)\r
        {\r
@@ -652,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
@@ -673,15 +761,15 @@ static void process_MessageOut()
        }\r
 \r
        // Re-check the ATN flag in case it stays asserted.\r
-       scsiDev.atnFlag |= SCSI_ReadPin(SCSI_ATN_INT);\r
+       scsiDev.atnFlag |= SCSI_ReadFilt(SCSI_Filt_ATN);\r
 }\r
 \r
 void scsiPoll(void)\r
 {\r
-       if (scsiDev.resetFlag)\r
+       if (unlikely(scsiDev.resetFlag))\r
        {\r
                scsiReset();\r
-               if ((scsiDev.resetFlag = SCSI_ReadPin(SCSI_RST_INT)))\r
+               if ((scsiDev.resetFlag = SCSI_ReadFilt(SCSI_Filt_RST)))\r
                {\r
                        // Still in reset phase. Do not try and process any commands.\r
                        return;\r
@@ -691,7 +779,7 @@ void scsiPoll(void)
        switch (scsiDev.phase)\r
        {\r
        case BUS_FREE:\r
-               if (SCSI_ReadPin(SCSI_In_BSY))\r
+               if (SCSI_ReadFilt(SCSI_Filt_BSY))\r
                {\r
                        scsiDev.phase = BUS_BUSY;\r
                }\r
@@ -699,7 +787,7 @@ void scsiPoll(void)
                // one initiator in the chain. Support this by moving\r
                // straight to selection if SEL is asserted.\r
                // ie. the initiator won't assert BSY and it's own ID before moving to selection.\r
-               else if (SCSI_ReadPin(SCSI_In_SEL))\r
+               else if (SCSI_ReadFilt(SCSI_Filt_SEL))\r
                {\r
                        enter_SelectionPhase();\r
                }\r
@@ -708,11 +796,11 @@ void scsiPoll(void)
        case BUS_BUSY:\r
                // Someone is using the bus. Perhaps they are trying to\r
                // select us.\r
-               if (SCSI_ReadPin(SCSI_In_SEL))\r
+               if (SCSI_ReadFilt(SCSI_Filt_SEL))\r
                {\r
                        enter_SelectionPhase();\r
                }\r
-               else if (!SCSI_ReadPin(SCSI_In_BSY))\r
+               else if (!SCSI_ReadFilt(SCSI_Filt_BSY))\r
                {\r
                        scsiDev.phase = BUS_FREE;\r
                }\r
@@ -745,7 +833,7 @@ void scsiPoll(void)
        break;\r
 \r
        case DATA_IN:\r
-               scsiDev.atnFlag |= SCSI_ReadPin(SCSI_ATN_INT);\r
+               scsiDev.atnFlag |= SCSI_ReadFilt(SCSI_Filt_ATN);\r
                if (scsiDev.atnFlag)\r
                {\r
                        process_MessageOut();\r
@@ -757,7 +845,7 @@ void scsiPoll(void)
        break;\r
 \r
        case DATA_OUT:\r
-               scsiDev.atnFlag |= SCSI_ReadPin(SCSI_ATN_INT);\r
+               scsiDev.atnFlag |= SCSI_ReadFilt(SCSI_Filt_ATN);\r
                if (scsiDev.atnFlag)\r
                {\r
                        process_MessageOut();\r
@@ -769,7 +857,7 @@ void scsiPoll(void)
        break;\r
 \r
        case STATUS:\r
-               scsiDev.atnFlag |= SCSI_ReadPin(SCSI_ATN_INT);\r
+               scsiDev.atnFlag |= SCSI_ReadFilt(SCSI_Filt_ATN);\r
                if (scsiDev.atnFlag)\r
                {\r
                        process_MessageOut();\r
@@ -781,7 +869,7 @@ void scsiPoll(void)
        break;\r
 \r
        case MESSAGE_IN:\r
-               scsiDev.atnFlag |= SCSI_ReadPin(SCSI_ATN_INT);\r
+               scsiDev.atnFlag |= SCSI_ReadFilt(SCSI_Filt_ATN);\r
                if (scsiDev.atnFlag)\r
                {\r
                        process_MessageOut();\r
@@ -801,13 +889,137 @@ 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
+{\r
+       scsiEnterPhase(MESSAGE_IN);\r
+       scsiWriteByte(0x02); // save data pointer\r
+       scsiWriteByte(0x04); // disconnect msg.\r
+\r
+       // For now, the caller is responsible for tracking the disconnected\r
+       // state, and calling scsiReconnect.\r
+       // Ideally the client would exit their loop and we'd implement this\r
+       // as part of scsiPoll\r
+       int phase = scsiDev.phase;\r
+       enter_BusFree();\r
+       scsiDev.phase = phase;\r
+}\r
+\r
+int scsiReconnect()\r
+{\r
+       int reconnected = 0;\r
+\r
+       int sel = SCSI_ReadFilt(SCSI_Filt_SEL);\r
+       int bsy = SCSI_ReadFilt(SCSI_Filt_BSY);\r
+       if (!sel && !bsy)\r
+       {\r
+               CyDelayUs(1);\r
+               sel = SCSI_ReadFilt(SCSI_Filt_SEL);\r
+               bsy = SCSI_ReadFilt(SCSI_Filt_BSY);\r
+       }\r
+\r
+       if (!sel && !bsy)\r
+       {\r
+               // Arbitrate.\r
+               ledOn();\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
+               CyDelayUs(3); // arbitrate delay. 2.4us.\r
+\r
+               uint8_t dbx = scsiReadDBxPins();\r
+               sel = SCSI_ReadFilt(SCSI_Filt_SEL);\r
+               if (sel || ((dbx ^ scsiIdMask) > scsiIdMask))\r
+               {\r
+                       // Lost arbitration.\r
+                       SCSI_Out_Ctl_Write(0);\r
+                       SCSI_ClearPin(SCSI_Out_BSY);\r
+                       ledOff();\r
+               }\r
+               else\r
+               {\r
+                       // Won arbitration\r
+                       SCSI_SetPin(SCSI_Out_SEL);\r
+                       CyDelayUs(1); // Bus clear + Bus settle.\r
+\r
+                       // Reselection phase\r
+                       SCSI_CTL_PHASE_Write(__scsiphase_io);\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
+                       CyDelayUs(1);  // Bus Settle Delay\r
+\r
+                       uint32_t waitStart_ms = getTime_ms();\r
+                       bsy = SCSI_ReadFilt(SCSI_Filt_BSY);\r
+                       // Wait for initiator.\r
+                       while (\r
+                               !bsy &&\r
+                               !scsiDev.resetFlag &&\r
+                               (elapsedTime_ms(waitStart_ms) < 250))\r
+                       {\r
+                               bsy = SCSI_ReadFilt(SCSI_Filt_BSY);\r
+                       }\r
+\r
+                       if (bsy)\r
+                       {\r
+                               SCSI_SetPin(SCSI_Out_BSY);\r
+                               scsiDeskewDelay(); // 2 deskew delays\r
+                               scsiDeskewDelay(); // 2 deskew delays\r
+                               SCSI_ClearPin(SCSI_Out_SEL);\r
+\r
+                               // Prepare for the initial IDENTIFY message.\r
+                               SCSI_Out_Ctl_Write(0);\r
+                               scsiEnterPhase(MESSAGE_IN);\r
+\r
+                               // Send identify command\r
+                               scsiWriteByte(0x80);\r
+\r
+                               scsiEnterPhase(scsiDev.phase);\r
+                               reconnected = 1;\r
+                       }\r
+                       else\r
+                       {\r
+                               // reselect timeout.\r
+                               SCSI_Out_Ctl_Write(0);\r
+                               SCSI_ClearPin(SCSI_Out_SEL);\r
+                               SCSI_CTL_PHASE_Write(0);\r
+                               ledOff();\r
+                       }\r
+               }\r
+       }\r
+       return reconnected;\r
 }\r
 \r
+#pragma GCC pop_options\r