Implement WRITE BUFFER and WRITE WITH VERIFY commands
[SCSI2SD-V6.git] / software / SCSI2SD / src / sd.c
index e0649bd..00163f3 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
@@ -37,10 +39,6 @@ static int sdIOState = SD_IDLE;
 static uint8 sdDMARxChan = CY_DMA_INVALID_CHANNEL;\r
 static uint8 sdDMATxChan = CY_DMA_INVALID_CHANNEL;\r
 \r
-// DMA descriptors\r
-static uint8 sdDMARxTd[2] = { CY_DMA_INVALID_TD, CY_DMA_INVALID_TD };\r
-static uint8 sdDMATxTd[3] = { CY_DMA_INVALID_TD, CY_DMA_INVALID_TD, CY_DMA_INVALID_TD };\r
-\r
 // Dummy location for DMA to send unchecked CRC bytes to\r
 static uint8 discardBuffer;\r
 \r
@@ -53,18 +51,18 @@ static uint8_t writeStartToken = 0xFC;
 // Source of dummy SPI bytes for DMA\r
 static uint8 dummyBuffer = 0xFF;\r
 \r
-volatile static uint8 rxDMAComplete;\r
-volatile static uint8 txDMAComplete;\r
+volatile uint8_t sdRxDMAComplete;\r
+volatile uint8_t sdTxDMAComplete;\r
 \r
 CY_ISR_PROTO(sdRxISR);\r
 CY_ISR(sdRxISR)\r
 {\r
-       rxDMAComplete = 1;\r
+       sdRxDMAComplete = 1;\r
 }\r
 CY_ISR_PROTO(sdTxISR);\r
 CY_ISR(sdTxISR)\r
 {\r
-       txDMAComplete = 1;\r
+       sdTxDMAComplete = 1;\r
 }\r
 \r
 static uint8 sdCrc7(uint8* chr, uint8 cnt, uint8 crc)\r
@@ -98,14 +96,23 @@ static uint16_t sdDoCommand(
        int useCRC,\r
        int use2byteResponse)\r
 {\r
-       uint8_t send[7];\r
+       int waitWhileBusy = (cmd != SD_GO_IDLE_STATE) && (cmd != SD_STOP_TRANSMISSION);\r
+\r
+       // "busy" probe. We'll examine the results later.\r
+       if (waitWhileBusy)\r
+       {\r
+               SDCard_WriteTxData(0xFF);\r
+       }\r
 \r
+       // send is static as the address must remain consistent for the static\r
+       // DMA descriptors to work.\r
+       static uint8_t send[7];\r
        send[0] = cmd | 0x40;\r
        send[1] = param >> 24;\r
        send[2] = param >> 16;\r
        send[3] = param >> 8;\r
        send[4] = param;\r
-       if (useCRC)\r
+       if (unlikely(useCRC))\r
        {\r
                send[5] = (sdCrc7(send, 5, 0) << 1) | 1;\r
        }\r
@@ -115,31 +122,52 @@ static uint16_t sdDoCommand(
        }\r
        send[6] = 0xFF; // Result code or stuff byte.\r
 \r
-       CyDmaTdSetConfiguration(sdDMATxTd[0], sizeof(send), CY_DMA_DISABLE_TD, TD_INC_SRC_ADR|SD_TX_DMA__TD_TERMOUT_EN);\r
-       CyDmaTdSetAddress(sdDMATxTd[0], LO16((uint32)&send), LO16((uint32)SDCard_TXDATA_PTR));\r
-       CyDmaTdSetConfiguration(sdDMARxTd[0], sizeof(send), CY_DMA_DISABLE_TD, SD_RX_DMA__TD_TERMOUT_EN);\r
-       CyDmaTdSetAddress(sdDMARxTd[0], LO16((uint32)SDCard_RXDATA_PTR), LO16((uint32)&discardBuffer));\r
+       static uint8_t dmaRxTd = CY_DMA_INVALID_TD;\r
+       static uint8_t dmaTxTd = CY_DMA_INVALID_TD;\r
+       if (unlikely(dmaRxTd == CY_DMA_INVALID_TD))\r
+       {\r
+               dmaRxTd = CyDmaTdAllocate();\r
+               dmaTxTd = CyDmaTdAllocate();\r
+               CyDmaTdSetConfiguration(dmaTxTd, sizeof(send), CY_DMA_DISABLE_TD, TD_INC_SRC_ADR|SD_TX_DMA__TD_TERMOUT_EN);\r
+               CyDmaTdSetAddress(dmaTxTd, LO16((uint32)&send), LO16((uint32)SDCard_TXDATA_PTR));\r
+               CyDmaTdSetConfiguration(dmaRxTd, sizeof(send), CY_DMA_DISABLE_TD, SD_RX_DMA__TD_TERMOUT_EN);\r
+               CyDmaTdSetAddress(dmaRxTd, LO16((uint32)SDCard_RXDATA_PTR), LO16((uint32)&discardBuffer));\r
+       }\r
+\r
+       sdTxDMAComplete = 0;\r
+       sdRxDMAComplete = 0;\r
+\r
+       CyDmaChSetInitialTd(sdDMARxChan, dmaRxTd);\r
+       CyDmaChSetInitialTd(sdDMATxChan, dmaTxTd);\r
+\r
+       // Some Samsung cards enter a busy-state after single-sector reads.\r
+       // But we also need to wait for R1B to complete from the multi-sector\r
+       // reads.\r
+       if (waitWhileBusy)\r
+       {\r
+               while (!(SDCard_ReadRxStatus() & SDCard_STS_RX_FIFO_NOT_EMPTY)) {}\r
+               int busy = SDCard_ReadRxData() != 0xFF;\r
+               if (unlikely(busy))\r
+               {\r
+                       while (sdSpiByte(0xFF) != 0xFF) {}\r
+               }\r
+       }\r
+\r
        // The DMA controller is a bit trigger-happy. It will retain\r
        // a drq request that was triggered while the channel was\r
        // disabled.\r
        CyDmaClearPendingDrq(sdDMATxChan);\r
        CyDmaClearPendingDrq(sdDMARxChan);\r
 \r
-       txDMAComplete = 0;\r
-       rxDMAComplete = 0;\r
-\r
-       CyDmaChSetInitialTd(sdDMARxChan, sdDMARxTd[0]);\r
-       CyDmaChSetInitialTd(sdDMATxChan, sdDMATxTd[0]);\r
-\r
        // There is no flow control, so we must ensure we can read the bytes\r
        // before we start transmitting\r
        CyDmaChEnable(sdDMARxChan, 1);\r
        CyDmaChEnable(sdDMATxChan, 1);\r
 \r
-       while (!(txDMAComplete && rxDMAComplete)) {}\r
+       while (!(sdTxDMAComplete && sdRxDMAComplete)) { __WFI(); }\r
 \r
        uint16_t response = discardBuffer;\r
-       if (cmd == SD_STOP_TRANSMISSION)\r
+       if (unlikely(cmd == SD_STOP_TRANSMISSION))\r
        {\r
                // Stuff byte is required for this command only.\r
                // Part 1 Simplified standard 3.01\r
@@ -149,11 +177,11 @@ static uint16_t sdDoCommand(
        }\r
 \r
        uint32_t start = getTime_ms();\r
-       while ((response & 0x80) && (diffTime_ms(start, getTime_ms()) <= 200))\r
+       while ((response & 0x80) && likely(elapsedTime_ms(start) <= 200))\r
        {\r
                response = sdSpiByte(0xFF);\r
        }\r
-       if (use2byteResponse)\r
+       if (unlikely(use2byteResponse))\r
        {\r
                response = (response << 8) | sdSpiByte(0xFF);\r
        }\r
@@ -161,21 +189,13 @@ static uint16_t sdDoCommand(
 }\r
 \r
 \r
-static uint16_t sdCommandAndResponse(uint8_t cmd, uint32_t param)\r
+static inline uint16_t sdCommandAndResponse(uint8_t cmd, uint32_t param)\r
 {\r
-       // Some Samsung cards enter a busy-state after single-sector reads.\r
-       // But we also need to wait for R1B to complete from the multi-sector\r
-       // reads.\r
-       while (sdSpiByte(0xFF) == 0x00) {}\r
        return sdDoCommand(cmd, param, 0, 0);\r
 }\r
 \r
-static uint16_t sdCRCCommandAndResponse(uint8_t cmd, uint32_t param)\r
+static inline uint16_t sdCRCCommandAndResponse(uint8_t cmd, uint32_t param)\r
 {\r
-       // Some Samsung cards enter a busy-state after single-sector reads.\r
-       // But we also need to wait for R1B to complete from the multi-sector\r
-       // reads.\r
-       while (sdSpiByte(0xFF) == 0x00) {}\r
        return sdDoCommand(cmd, param, 1, 0);\r
 }\r
 \r
@@ -203,14 +223,14 @@ sdReadMultiSectorPrep()
                sdLBA = sdLBA * SD_SECTOR_SIZE;\r
        }\r
        v = sdCommandAndResponse(SD_READ_MULTIPLE_BLOCK, sdLBA);\r
-       if (v)\r
+       if (unlikely(v))\r
        {\r
                scsiDiskReset();\r
                sdClearStatus();\r
 \r
                scsiDev.status = CHECK_CONDITION;\r
                scsiDev.target->sense.code = HARDWARE_ERROR;\r
-               scsiDev.target->sense.asc = LOGICAL_UNIT_NOT_READY_CAUSE_NOT_REPORTABLE;\r
+               scsiDev.target->sense.asc = LOGICAL_UNIT_COMMUNICATION_FAILURE;\r
                scsiDev.phase = STATUS;\r
        }\r
        else\r
@@ -226,16 +246,16 @@ dmaReadSector(uint8_t* outputBuffer)
        // Don't wait more than 200ms.  The standard recommends 100ms.\r
        uint32_t start = getTime_ms();\r
        uint8_t token = sdSpiByte(0xFF);\r
-       while (token != 0xFE && (diffTime_ms(start, getTime_ms()) <= 200))\r
+       while (token != 0xFE && likely(elapsedTime_ms(start) <= 200))\r
        {\r
-               if (token && ((token & 0xE0) == 0))\r
+               if (unlikely(token && ((token & 0xE0) == 0)))\r
                {\r
                        // Error token!\r
                        break;\r
                }\r
                token = sdSpiByte(0xFF);\r
        }\r
-       if (token != 0xFE)\r
+       if (unlikely(token != 0xFE))\r
        {\r
                if (transfer.multiBlock)\r
                {\r
@@ -245,31 +265,41 @@ dmaReadSector(uint8_t* outputBuffer)
                {\r
                        scsiDev.status = CHECK_CONDITION;\r
                        scsiDev.target->sense.code = HARDWARE_ERROR;\r
-                       scsiDev.target->sense.asc = 0x4400 | token;\r
+                       scsiDev.target->sense.asc = UNRECOVERED_READ_ERROR;\r
                        scsiDev.phase = STATUS;\r
                }\r
                sdClearStatus();\r
                return;\r
        }\r
 \r
-       // Receive 512 bytes of data and then 2 bytes CRC.\r
-       CyDmaTdSetConfiguration(sdDMARxTd[0], SD_SECTOR_SIZE, sdDMARxTd[1], TD_INC_DST_ADR);\r
-       CyDmaTdSetAddress(sdDMARxTd[0], LO16((uint32)SDCard_RXDATA_PTR), LO16((uint32)outputBuffer));\r
-       CyDmaTdSetConfiguration(sdDMARxTd[1], 2, CY_DMA_DISABLE_TD, SD_RX_DMA__TD_TERMOUT_EN);\r
-       CyDmaTdSetAddress(sdDMARxTd[1], LO16((uint32)SDCard_RXDATA_PTR), LO16((uint32)&discardBuffer));\r
+       static uint8_t dmaRxTd[2] = { CY_DMA_INVALID_TD, CY_DMA_INVALID_TD};\r
+       static uint8_t dmaTxTd = CY_DMA_INVALID_TD;\r
+       if (unlikely(dmaRxTd[0] == CY_DMA_INVALID_TD))\r
+       {\r
+               dmaRxTd[0] = CyDmaTdAllocate();\r
+               dmaRxTd[1] = CyDmaTdAllocate();\r
+               dmaTxTd = CyDmaTdAllocate();\r
+               \r
+               // Receive 512 bytes of data and then 2 bytes CRC.\r
+               CyDmaTdSetConfiguration(dmaRxTd[0], SD_SECTOR_SIZE, dmaRxTd[1], TD_INC_DST_ADR);\r
+               CyDmaTdSetConfiguration(dmaRxTd[1], 2, CY_DMA_DISABLE_TD, SD_RX_DMA__TD_TERMOUT_EN);\r
+               CyDmaTdSetAddress(dmaRxTd[1], LO16((uint32)SDCard_RXDATA_PTR), LO16((uint32)&discardBuffer));\r
+       \r
+               CyDmaTdSetConfiguration(dmaTxTd, SD_SECTOR_SIZE + 2, CY_DMA_DISABLE_TD, SD_TX_DMA__TD_TERMOUT_EN);\r
+               CyDmaTdSetAddress(dmaTxTd, LO16((uint32)&dummyBuffer), LO16((uint32)SDCard_TXDATA_PTR));\r
 \r
-       CyDmaTdSetConfiguration(sdDMATxTd[0], SD_SECTOR_SIZE + 2, CY_DMA_DISABLE_TD, SD_TX_DMA__TD_TERMOUT_EN);\r
-       CyDmaTdSetAddress(sdDMATxTd[0], LO16((uint32)&dummyBuffer), LO16((uint32)SDCard_TXDATA_PTR));\r
+       }\r
+       CyDmaTdSetAddress(dmaRxTd[0], LO16((uint32)SDCard_RXDATA_PTR), LO16((uint32)outputBuffer));\r
 \r
        sdIOState = SD_DMA;\r
-       txDMAComplete = 0;\r
-       rxDMAComplete = 0;\r
+       sdTxDMAComplete = 0;\r
+       sdRxDMAComplete = 0;\r
 \r
        // Re-loading the initial TD's here is very important, or else\r
        // we'll be re-using the last-used TD, which would be the last\r
        // in the chain (ie. CRC TD)\r
-       CyDmaChSetInitialTd(sdDMARxChan, sdDMARxTd[0]);\r
-       CyDmaChSetInitialTd(sdDMATxChan, sdDMATxTd[0]);\r
+       CyDmaChSetInitialTd(sdDMARxChan, dmaRxTd[0]);\r
+       CyDmaChSetInitialTd(sdDMATxChan, dmaTxTd);\r
 \r
        // The DMA controller is a bit trigger-happy. It will retain\r
        // a drq request that was triggered while the channel was\r
@@ -286,7 +316,7 @@ dmaReadSector(uint8_t* outputBuffer)
 int\r
 sdReadSectorDMAPoll()\r
 {\r
-       if (rxDMAComplete && txDMAComplete)\r
+       if (sdRxDMAComplete && sdTxDMAComplete)\r
        {\r
                // DMA transfer is complete\r
                sdIOState = SD_IDLE;\r
@@ -306,14 +336,14 @@ void sdReadSingleSectorDMA(uint32_t lba, uint8_t* outputBuffer)
                lba = lba * SD_SECTOR_SIZE;\r
        }\r
        v = sdCommandAndResponse(SD_READ_SINGLE_BLOCK, lba);\r
-       if (v)\r
+       if (unlikely(v))\r
        {\r
                scsiDiskReset();\r
                sdClearStatus();\r
 \r
                scsiDev.status = CHECK_CONDITION;\r
                scsiDev.target->sense.code = HARDWARE_ERROR;\r
-               scsiDev.target->sense.asc = LOGICAL_UNIT_DOES_NOT_RESPOND_TO_SELECTION;\r
+               scsiDev.target->sense.asc = LOGICAL_UNIT_COMMUNICATION_FAILURE;\r
                scsiDev.phase = STATUS;\r
        }\r
        else\r
@@ -332,29 +362,26 @@ sdReadMultiSectorDMA(uint8_t* outputBuffer)
 \r
 void sdCompleteRead()\r
 {\r
-       if (sdIOState != SD_IDLE)\r
+       if (unlikely(sdIOState != SD_IDLE))\r
        {\r
                // Not much choice but to wait until we've completed the transfer.\r
                // Cancelling the transfer can't be done as we have no way to reset\r
                // the SD card.\r
                while (!sdReadSectorDMAPoll()) { /* spin */ }\r
        }\r
-       transfer.inProgress = 0;\r
-\r
-       // We cannot send even a single "padding" byte, as we normally would when\r
-       // sending a command.  If we've just finished reading the very last block\r
-       // on the card, then reading an additional dummy byte will just trigger\r
-       // an error condition as we're trying to read past-the-end of the storage\r
-       // device.\r
-       // ie. do not use sdCommandAndResponse here.\r
-       uint8 r1b = sdDoCommand(SD_STOP_TRANSMISSION, 0, 0, 0);\r
-\r
-       if (r1b)\r
+       \r
+       if (transfer.inProgress)\r
        {\r
-               scsiDev.status = CHECK_CONDITION;\r
-               scsiDev.target->sense.code = HARDWARE_ERROR;\r
-               scsiDev.target->sense.asc = UNRECOVERED_READ_ERROR | r1b;\r
-               scsiDev.phase = STATUS;\r
+               transfer.inProgress = 0;\r
+               uint8 r1b = sdCommandAndResponse(SD_STOP_TRANSMISSION, 0);\r
+\r
+               if (unlikely(r1b))\r
+               {\r
+                       scsiDev.status = CHECK_CONDITION;\r
+                       scsiDev.target->sense.code = HARDWARE_ERROR;\r
+                       scsiDev.target->sense.asc = UNRECOVERED_READ_ERROR;\r
+                       scsiDev.phase = STATUS;\r
+               }\r
        }\r
 \r
        // R1b has an optional trailing "busy" signal, but we defer waiting on this.\r
@@ -374,22 +401,34 @@ static void sdWaitWriteBusy()
 void\r
 sdWriteMultiSectorDMA(uint8_t* outputBuffer)\r
 {\r
-       // Transmit 512 bytes of data and then 2 bytes CRC, and then get the response byte\r
-       // We need to do this without stopping the clock\r
-       CyDmaTdSetConfiguration(sdDMATxTd[0], 1, sdDMATxTd[1], TD_INC_SRC_ADR);\r
-       CyDmaTdSetAddress(sdDMATxTd[0], LO16((uint32)&writeStartToken), LO16((uint32)SDCard_TXDATA_PTR));\r
+       static uint8_t dmaRxTd[2] = { CY_DMA_INVALID_TD, CY_DMA_INVALID_TD};\r
+       static uint8_t dmaTxTd[3] = { CY_DMA_INVALID_TD, CY_DMA_INVALID_TD, CY_DMA_INVALID_TD};\r
+       if (unlikely(dmaRxTd[0] == CY_DMA_INVALID_TD))\r
+       {\r
+               dmaRxTd[0] = CyDmaTdAllocate();\r
+               dmaRxTd[1] = CyDmaTdAllocate();\r
+               dmaTxTd[0] = CyDmaTdAllocate();\r
+               dmaTxTd[1] = CyDmaTdAllocate();\r
+               dmaTxTd[2] = CyDmaTdAllocate();\r
+               \r
+               // Transmit 512 bytes of data and then 2 bytes CRC, and then get the response byte\r
+               // We need to do this without stopping the clock\r
+               CyDmaTdSetConfiguration(dmaTxTd[0], 1, dmaTxTd[1], TD_INC_SRC_ADR);\r
+               CyDmaTdSetAddress(dmaTxTd[0], LO16((uint32)&writeStartToken), LO16((uint32)SDCard_TXDATA_PTR));\r
 \r
-       CyDmaTdSetConfiguration(sdDMATxTd[1], SD_SECTOR_SIZE, sdDMATxTd[2], TD_INC_SRC_ADR);\r
-       CyDmaTdSetAddress(sdDMATxTd[1], LO16((uint32)outputBuffer), LO16((uint32)SDCard_TXDATA_PTR));\r
+               CyDmaTdSetConfiguration(dmaTxTd[1], SD_SECTOR_SIZE, dmaTxTd[2], TD_INC_SRC_ADR);\r
+\r
+               CyDmaTdSetConfiguration(dmaTxTd[2], 2 + sizeof(writeResponseBuffer), CY_DMA_DISABLE_TD, SD_TX_DMA__TD_TERMOUT_EN);\r
+               CyDmaTdSetAddress(dmaTxTd[2], LO16((uint32)&dummyBuffer), LO16((uint32)SDCard_TXDATA_PTR));\r
+\r
+               CyDmaTdSetConfiguration(dmaRxTd[0], SD_SECTOR_SIZE + 3, dmaRxTd[1], 0);\r
+               CyDmaTdSetAddress(dmaRxTd[0], LO16((uint32)SDCard_RXDATA_PTR), LO16((uint32)&discardBuffer));\r
+               CyDmaTdSetConfiguration(dmaRxTd[1], sizeof(writeResponseBuffer), CY_DMA_DISABLE_TD, SD_RX_DMA__TD_TERMOUT_EN|TD_INC_DST_ADR);\r
+               CyDmaTdSetAddress(dmaRxTd[1], LO16((uint32)SDCard_RXDATA_PTR), LO16((uint32)&writeResponseBuffer));\r
+       }\r
+       CyDmaTdSetAddress(dmaTxTd[1], LO16((uint32)outputBuffer), LO16((uint32)SDCard_TXDATA_PTR));\r
 \r
-       CyDmaTdSetConfiguration(sdDMATxTd[2], 2 + sizeof(writeResponseBuffer), CY_DMA_DISABLE_TD, SD_TX_DMA__TD_TERMOUT_EN);\r
-       CyDmaTdSetAddress(sdDMATxTd[2], LO16((uint32)&dummyBuffer), LO16((uint32)SDCard_TXDATA_PTR));\r
 \r
-       CyDmaTdSetConfiguration(sdDMARxTd[0], SD_SECTOR_SIZE + 3, sdDMARxTd[1], 0);\r
-       CyDmaTdSetAddress(sdDMARxTd[0], LO16((uint32)SDCard_RXDATA_PTR), LO16((uint32)&discardBuffer));\r
-       CyDmaTdSetConfiguration(sdDMARxTd[1], sizeof(writeResponseBuffer), CY_DMA_DISABLE_TD, SD_RX_DMA__TD_TERMOUT_EN|TD_INC_DST_ADR);\r
-       CyDmaTdSetAddress(sdDMARxTd[1], LO16((uint32)SDCard_RXDATA_PTR), LO16((uint32)&writeResponseBuffer));\r
-       \r
        sdIOState = SD_DMA;\r
        // The DMA controller is a bit trigger-happy. It will retain\r
        // a drq request that was triggered while the channel was\r
@@ -397,14 +436,14 @@ sdWriteMultiSectorDMA(uint8_t* outputBuffer)
        CyDmaClearPendingDrq(sdDMATxChan);\r
        CyDmaClearPendingDrq(sdDMARxChan);\r
 \r
-       txDMAComplete = 0;\r
-       rxDMAComplete = 0;\r
+       sdTxDMAComplete = 0;\r
+       sdRxDMAComplete = 0;\r
 \r
        // Re-loading the initial TD's here is very important, or else\r
        // we'll be re-using the last-used TD, which would be the last\r
        // in the chain (ie. CRC TD)\r
-       CyDmaChSetInitialTd(sdDMARxChan, sdDMARxTd[0]);\r
-       CyDmaChSetInitialTd(sdDMATxChan, sdDMATxTd[0]);\r
+       CyDmaChSetInitialTd(sdDMARxChan, dmaRxTd[0]);\r
+       CyDmaChSetInitialTd(sdDMATxChan, dmaTxTd[0]);\r
 \r
        // There is no flow control, so we must ensure we can read the bytes\r
        // before we start transmitting\r
@@ -415,7 +454,7 @@ sdWriteMultiSectorDMA(uint8_t* outputBuffer)
 int\r
 sdWriteSectorDMAPoll(int sendStopToken)\r
 {\r
-       if (rxDMAComplete && txDMAComplete)\r
+       if (sdRxDMAComplete && sdTxDMAComplete)\r
        {\r
                if (sdIOState == SD_DMA)\r
                {\r
@@ -431,7 +470,7 @@ sdWriteSectorDMAPoll(int sendStopToken)
 \r
                        // At this point we should either have an accepted token, or we'll\r
                        // timeout and proceed into the error case below.\r
-                       if (((dataToken & 0x1F) >> 1) != 0x2) // Accepted.\r
+                       if (unlikely(((dataToken & 0x1F) >> 1) != 0x2)) // Accepted.\r
                        {\r
                                sdIOState = SD_IDLE;\r
 \r
@@ -445,7 +484,7 @@ sdWriteSectorDMAPoll(int sendStopToken)
 \r
                                scsiDev.status = CHECK_CONDITION;\r
                                scsiDev.target->sense.code = HARDWARE_ERROR;\r
-                               scsiDev.target->sense.asc = 0x6900 | dataToken;\r
+                               scsiDev.target->sense.asc = LOGICAL_UNIT_COMMUNICATION_FAILURE;\r
                                scsiDev.phase = STATUS;\r
                        }\r
                        else\r
@@ -492,7 +531,7 @@ sdWriteSectorDMAPoll(int sendStopToken)
 \r
 void sdCompleteWrite()\r
 {\r
-       if (sdIOState != SD_IDLE)\r
+       if (unlikely(sdIOState != SD_IDLE))\r
        {\r
                // Not much choice but to wait until we've completed the transfer.\r
                // Cancelling the transfer can't be done as we have no way to reset\r
@@ -500,13 +539,10 @@ void sdCompleteWrite()
                while (!sdWriteSectorDMAPoll(1)) { /* spin */ }\r
        }\r
 \r
-       transfer.inProgress = 0;\r
-\r
-       if (scsiDev.phase == DATA_OUT)\r
+       if (transfer.inProgress && likely(scsiDev.phase == DATA_OUT))\r
        {\r
-               sdSpiByte(0xFF);\r
                uint16_t r2 = sdDoCommand(SD_SEND_STATUS, 0, 0, 1);\r
-               if (r2)\r
+               if (unlikely(r2))\r
                {\r
                        sdClearStatus();\r
                        scsiDev.status = CHECK_CONDITION;\r
@@ -515,6 +551,7 @@ void sdCompleteWrite()
                        scsiDev.phase = STATUS;\r
                }\r
        }\r
+       transfer.inProgress = 0;\r
 }\r
 \r
 \r
@@ -569,7 +606,7 @@ static int sdOpCond()
                sdClearStatus();\r
 \r
        // Spec says to poll for 1 second.\r
-       } while ((status != 0) && (diffTime_ms(start, getTime_ms()) < 1000));\r
+       } while ((status != 0) && (elapsedTime_ms(start) < 1000));\r
 \r
        return status == 0;\r
 }\r
@@ -598,7 +635,7 @@ static int sdReadOCR()
 \r
        } while (!status &&\r
                !complete &&\r
-               (diffTime_ms(start, getTime_ms()) < 1000));\r
+               (elapsedTime_ms(start) < 1000));\r
 \r
        return (status == 0) && complete;\r
 }\r
@@ -702,12 +739,6 @@ static void sdInitDMA()
                CyDmaChDisable(sdDMATxChan);\r
                CyDmaChDisable(sdDMARxChan);\r
 \r
-               sdDMARxTd[0] = CyDmaTdAllocate();\r
-               sdDMARxTd[1] = CyDmaTdAllocate();\r
-               sdDMATxTd[0] = CyDmaTdAllocate();\r
-               sdDMATxTd[1] = CyDmaTdAllocate();\r
-               sdDMATxTd[2] = CyDmaTdAllocate();\r
-\r
                SD_RX_DMA_COMPLETE_StartEx(sdRxISR);\r
                SD_TX_DMA_COMPLETE_StartEx(sdTxISR);\r
        }\r
@@ -791,6 +822,7 @@ int sdInit()
        goto out;\r
 \r
 bad:\r
+       SD_Data_Clk_SetDivider(clkDiv25MHz); // Restore the clock for our next retry\r
        sdDev.capacity = 0;\r
 \r
 out:\r
@@ -826,13 +858,13 @@ void sdWriteMultiSectorPrep()
                sdLBA = sdLBA * SD_SECTOR_SIZE;\r
        }\r
        v = sdCommandAndResponse(SD_WRITE_MULTIPLE_BLOCK, sdLBA);\r
-       if (v)\r
+       if (unlikely(v))\r
        {\r
                scsiDiskReset();\r
                sdClearStatus();\r
                scsiDev.status = CHECK_CONDITION;\r
                scsiDev.target->sense.code = HARDWARE_ERROR;\r
-               scsiDev.target->sense.asc = 0x8800 | v;\r
+               scsiDev.target->sense.asc = LOGICAL_UNIT_COMMUNICATION_FAILURE;\r
                scsiDev.phase = STATUS;\r
        }\r
        else\r
@@ -854,7 +886,7 @@ void sdPoll()
                // overpower the SD pullup resistor.\r
                SD_CS_Write(0);\r
                SD_CS_SetDriveMode(SD_CS_DM_DIG_HIZ);\r
-               \r
+\r
                CyDelayCycles(64);\r
                uint8_t cs = SD_CS_Read();\r
                SD_CS_SetDriveMode(SD_CS_DM_STRONG)     ;\r
@@ -862,14 +894,14 @@ void sdPoll()
                if (cs && !(blockDev.state & DISK_PRESENT))\r
                {\r
                        static int firstInit = 1;\r
-               \r
+\r
                        // Debounce\r
                        CyDelay(250);\r
-                       \r
+\r
                        if (sdInit())\r
                        {\r
                                blockDev.state |= DISK_PRESENT | DISK_INITIALISED;\r
-                               \r
+\r
                                if (!firstInit)\r
                                {\r
                                        int i;\r
@@ -894,3 +926,5 @@ void sdPoll()
                }\r
        }\r
 }\r
+\r
+#pragma GCC pop_options\r