Implement WRITE BUFFER and WRITE WITH VERIFY commands
[SCSI2SD-V6.git] / software / SCSI2SD / src / disk.c
index 003f5f3..af1c50a 100755 (executable)
@@ -15,6 +15,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
@@ -38,7 +40,7 @@ static int doSdInit()
        if (blockDev.state & DISK_PRESENT)\r
        {\r
                result = sdInit();\r
-       \r
+\r
                if (result)\r
                {\r
                        blockDev.state = blockDev.state | DISK_INITIALISED;\r
@@ -167,8 +169,8 @@ static void doReadCapacity()
 \r
 static void doWrite(uint32 lba, uint32 blocks)\r
 {\r
-       if ((blockDev.state & DISK_WP) ||\r
-               (scsiDev.target->cfg->deviceType == CONFIG_OPTICAL))\r
+       if (unlikely(blockDev.state & DISK_WP) ||\r
+               unlikely(scsiDev.target->cfg->deviceType == CONFIG_OPTICAL))\r
 \r
        {\r
                scsiDev.status = CHECK_CONDITION;\r
@@ -176,12 +178,13 @@ static void doWrite(uint32 lba, uint32 blocks)
                scsiDev.target->sense.asc = WRITE_PROTECTED;\r
                scsiDev.phase = STATUS;\r
        }\r
-       else if (((uint64) lba) + blocks >\r
+       else if (unlikely(((uint64) lba) + blocks >\r
                getScsiCapacity(\r
                        scsiDev.target->cfg->sdSectorStart,\r
                        scsiDev.target->liveCfg.bytesPerSector,\r
                        scsiDev.target->cfg->scsiSectors\r
-                       ))\r
+                       )\r
+               ))\r
        {\r
                scsiDev.status = CHECK_CONDITION;\r
                scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
@@ -201,7 +204,7 @@ static void doWrite(uint32 lba, uint32 blocks)
                // No need for single-block writes atm.  Overhead of the\r
                // multi-block write is minimal.\r
                transfer.multiBlock = 1;\r
-               \r
+\r
                sdWriteMultiSectorPrep();\r
        }\r
 }\r
@@ -213,7 +216,7 @@ static void doRead(uint32 lba, uint32 blocks)
                scsiDev.target->cfg->sdSectorStart,\r
                scsiDev.target->liveCfg.bytesPerSector,\r
                scsiDev.target->cfg->scsiSectors);\r
-       if (((uint64) lba) + blocks > capacity)\r
+       if (unlikely(((uint64) lba) + blocks > capacity))\r
        {\r
                scsiDev.status = CHECK_CONDITION;\r
                scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
@@ -230,7 +233,7 @@ static void doRead(uint32 lba, uint32 blocks)
                scsiDev.dataLen = 0; // No data yet\r
 \r
                if ((blocks == 1) ||\r
-                       (((uint64) lba) + blocks == capacity)\r
+                       unlikely(((uint64) lba) + blocks == capacity)\r
                        )\r
                {\r
                        // We get errors on reading the last sector using a multi-sector\r
@@ -264,7 +267,11 @@ static void doSeek(uint32 lba)
 static int doTestUnitReady()\r
 {\r
        int ready = 1;\r
-       if (!(blockDev.state & DISK_STARTED))\r
+       if (likely(blockDev.state == (DISK_STARTED | DISK_PRESENT | DISK_INITIALISED)))\r
+       {\r
+               // nothing to do.\r
+       }\r
+       else if (unlikely(!(blockDev.state & DISK_STARTED)))\r
        {\r
                ready = 0;\r
                scsiDev.status = CHECK_CONDITION;\r
@@ -272,7 +279,7 @@ static int doTestUnitReady()
                scsiDev.target->sense.asc = LOGICAL_UNIT_NOT_READY_INITIALIZING_COMMAND_REQUIRED;\r
                scsiDev.phase = STATUS;\r
        }\r
-       else if (!(blockDev.state & DISK_PRESENT))\r
+       else if (unlikely(!(blockDev.state & DISK_PRESENT)))\r
        {\r
                ready = 0;\r
                scsiDev.status = CHECK_CONDITION;\r
@@ -280,7 +287,7 @@ static int doTestUnitReady()
                scsiDev.target->sense.asc = MEDIUM_NOT_PRESENT;\r
                scsiDev.phase = STATUS;\r
        }\r
-       else if (!(blockDev.state & DISK_INITIALISED))\r
+       else if (unlikely(!(blockDev.state & DISK_INITIALISED)))\r
        {\r
                ready = 0;\r
                scsiDev.status = CHECK_CONDITION;\r
@@ -297,7 +304,7 @@ int scsiDiskCommand()
        int commandHandled = 1;\r
 \r
        uint8 command = scsiDev.cdb[0];\r
-       if (command == 0x1B)\r
+       if (unlikely(command == 0x1B))\r
        {\r
                // START STOP UNIT\r
                // Enable or disable media access operations.\r
@@ -318,36 +325,16 @@ int scsiDiskCommand()
                        blockDev.state &= ~DISK_STARTED;\r
                }\r
        }\r
-       else if (command == 0x00)\r
+       else if (unlikely(command == 0x00))\r
        {\r
                // TEST UNIT READY\r
                doTestUnitReady();\r
        }\r
-       else if (!doTestUnitReady())\r
+       else if (unlikely(!doTestUnitReady()))\r
        {\r
                // Status and sense codes already set by doTestUnitReady\r
        }\r
-       else if (command == 0x04)\r
-       {\r
-               // FORMAT UNIT\r
-               // We don't really do any formatting, but we need to read the correct\r
-               // number of bytes in the DATA_OUT phase to make the SCSI host happy.\r
-\r
-               int fmtData = (scsiDev.cdb[1] & 0x10) ? 1 : 0;\r
-               if (fmtData)\r
-               {\r
-                       // We need to read the parameter list, but we don't know how\r
-                       // big it is yet. Start with the header.\r
-                       scsiDev.dataLen = 4;\r
-                       scsiDev.phase = DATA_OUT;\r
-                       scsiDev.postDataOutHook = doFormatUnitHeader;\r
-               }\r
-               else\r
-               {\r
-                       // No data to read, we're already finished!\r
-               }\r
-       }\r
-       else if (command == 0x08)\r
+       else if (likely(command == 0x08))\r
        {\r
                // READ(6)\r
                uint32 lba =\r
@@ -355,11 +342,10 @@ int scsiDiskCommand()
                        (((uint32) scsiDev.cdb[2]) << 8) +\r
                        scsiDev.cdb[3];\r
                uint32 blocks = scsiDev.cdb[4];\r
-               if (blocks == 0) blocks = 256;\r
+               if (unlikely(blocks == 0)) blocks = 256;\r
                doRead(lba, blocks);\r
        }\r
-\r
-       else if (command == 0x28)\r
+       else if (likely(command == 0x28))\r
        {\r
                // READ(10)\r
                // Ignore all cache control bits - we don't support a memory cache.\r
@@ -375,90 +361,110 @@ int scsiDiskCommand()
 \r
                doRead(lba, blocks);\r
        }\r
-\r
-       else if (command == 0x25)\r
+       else if (likely(command == 0x0A))\r
        {\r
-               // READ CAPACITY\r
-               doReadCapacity();\r
-       }\r
-\r
-       else if (command == 0x0B)\r
-       {\r
-               // SEEK(6)\r
+               // WRITE(6)\r
                uint32 lba =\r
                        (((uint32) scsiDev.cdb[1] & 0x1F) << 16) +\r
                        (((uint32) scsiDev.cdb[2]) << 8) +\r
                        scsiDev.cdb[3];\r
-\r
-               doSeek(lba);\r
+               uint32 blocks = scsiDev.cdb[4];\r
+               if (unlikely(blocks == 0)) blocks = 256;\r
+               doWrite(lba, blocks);\r
        }\r
-\r
-       else if (command == 0x2B)\r
+       else if (likely(command == 0x2A) || // WRITE(10)\r
+               unlikely(command == 0x2E)) // WRITE AND VERIFY\r
        {\r
-               // SEEK(10)\r
+               // Ignore all cache control bits - we don't support a memory cache.\r
+               // Don't bother verifying either. The SD card likely stores ECC\r
+               // along with each flash row.\r
+\r
                uint32 lba =\r
                        (((uint32) scsiDev.cdb[2]) << 24) +\r
                        (((uint32) scsiDev.cdb[3]) << 16) +\r
                        (((uint32) scsiDev.cdb[4]) << 8) +\r
                        scsiDev.cdb[5];\r
+               uint32 blocks =\r
+                       (((uint32) scsiDev.cdb[7]) << 8) +\r
+                       scsiDev.cdb[8];\r
 \r
-               doSeek(lba);\r
+               doWrite(lba, blocks);\r
        }\r
-       else if (command == 0x0A)\r
+\r
+       else if (unlikely(command == 0x04))\r
        {\r
-               // WRITE(6)\r
+               // FORMAT UNIT\r
+               // We don't really do any formatting, but we need to read the correct\r
+               // number of bytes in the DATA_OUT phase to make the SCSI host happy.\r
+\r
+               int fmtData = (scsiDev.cdb[1] & 0x10) ? 1 : 0;\r
+               if (fmtData)\r
+               {\r
+                       // We need to read the parameter list, but we don't know how\r
+                       // big it is yet. Start with the header.\r
+                       scsiDev.dataLen = 4;\r
+                       scsiDev.phase = DATA_OUT;\r
+                       scsiDev.postDataOutHook = doFormatUnitHeader;\r
+               }\r
+               else\r
+               {\r
+                       // No data to read, we're already finished!\r
+               }\r
+       }\r
+       else if (unlikely(command == 0x25))\r
+       {\r
+               // READ CAPACITY\r
+               doReadCapacity();\r
+       }\r
+       else if (unlikely(command == 0x0B))\r
+       {\r
+               // SEEK(6)\r
                uint32 lba =\r
                        (((uint32) scsiDev.cdb[1] & 0x1F) << 16) +\r
                        (((uint32) scsiDev.cdb[2]) << 8) +\r
                        scsiDev.cdb[3];\r
-               uint32 blocks = scsiDev.cdb[4];\r
-               if (blocks == 0) blocks = 256;\r
-               doWrite(lba, blocks);\r
+\r
+               doSeek(lba);\r
        }\r
 \r
-       else if (command == 0x2A)\r
+       else if (unlikely(command == 0x2B))\r
        {\r
-               // WRITE(10)\r
-               // Ignore all cache control bits - we don't support a memory cache.\r
-\r
+               // SEEK(10)\r
                uint32 lba =\r
                        (((uint32) scsiDev.cdb[2]) << 24) +\r
                        (((uint32) scsiDev.cdb[3]) << 16) +\r
                        (((uint32) scsiDev.cdb[4]) << 8) +\r
                        scsiDev.cdb[5];\r
-               uint32 blocks =\r
-                       (((uint32) scsiDev.cdb[7]) << 8) +\r
-                       scsiDev.cdb[8];\r
 \r
-               doWrite(lba, blocks);\r
+               doSeek(lba);\r
        }\r
-       else if (command == 0x36)\r
+       else if (unlikely(command == 0x36))\r
        {\r
                // LOCK UNLOCK CACHE\r
                // We don't have a cache to lock data into. do nothing.\r
        }\r
-       else if (command == 0x34)\r
+       else if (unlikely(command == 0x34))\r
        {\r
                // PRE-FETCH.\r
                // We don't have a cache to pre-fetch into. do nothing.\r
        }\r
-       else if (command == 0x1E)\r
+       else if (unlikely(command == 0x1E))\r
        {\r
                // PREVENT ALLOW MEDIUM REMOVAL\r
                // Not much we can do to prevent the user removing the SD card.\r
                // do nothing.\r
        }\r
-       else if (command == 0x01)\r
+       else if (unlikely(command == 0x01))\r
        {\r
                // REZERO UNIT\r
                // Set the lun to a vendor-specific state. Ignore.\r
        }\r
-       else if (command == 0x35)\r
+       else if (unlikely(command == 0x35))\r
        {\r
                // SYNCHRONIZE CACHE\r
                // We don't have a cache. do nothing.\r
        }\r
-       else if (command == 0x2F)\r
+       else if (unlikely(command == 0x2F))\r
        {\r
                // VERIFY\r
                // TODO: When they supply data to verify, we should read the data and\r
@@ -512,15 +518,27 @@ void scsiDiskPoll()
                int scsiActive = 0;\r
                int sdActive = 0;\r
                while ((i < totalSDSectors) &&\r
-                       (scsiDev.phase == DATA_IN) &&\r
-                       !scsiDev.resetFlag)\r
+                       likely(scsiDev.phase == DATA_IN) &&\r
+                       likely(!scsiDev.resetFlag))\r
                {\r
-                       if ((sdActive == 1) && sdReadSectorDMAPoll())\r
+                       // Wait for the next DMA interrupt. It's beneficial to halt the\r
+                       // processor to give the DMA controller more memory bandwidth to\r
+                       // work with.\r
+                       // We're optimistically assuming a race condition won't occur\r
+                       // between these checks and the interrupt handers. The 1ms\r
+                       // systick timer interrupt saves us on the event of a race.\r
+                       int scsiBusy = scsiDMABusy();\r
+                       int sdBusy = sdDMABusy();\r
+                       if (scsiBusy && sdBusy) __WFI();\r
+\r
+                       if (sdActive && !sdBusy && sdReadSectorDMAPoll())\r
                        {\r
                                sdActive = 0;\r
                                prep++;\r
                        }\r
-                       else if ((sdActive == 0) && (prep - i < buffers) && (prep < totalSDSectors))\r
+                       else if (!sdActive &&\r
+                               (prep - i < buffers) &&\r
+                               (prep < totalSDSectors))\r
                        {\r
                                // Start an SD transfer if we have space.\r
                                if (transfer.multiBlock)\r
@@ -534,12 +552,12 @@ void scsiDiskPoll()
                                sdActive = 1;\r
                        }\r
 \r
-                       if ((scsiActive == 1) && scsiWriteDMAPoll())\r
+                       if (scsiActive && !scsiBusy && scsiWriteDMAPoll())\r
                        {\r
                                scsiActive = 0;\r
                                ++i;\r
                        }\r
-                       else if ((scsiActive == 0) && ((prep - i) > 0))\r
+                       else if (!scsiActive && ((prep - i) > 0))\r
                        {\r
                                int dmaBytes = SD_SECTOR_SIZE;\r
                                if ((i % sdPerScsi) == (sdPerScsi - 1))\r
@@ -575,16 +593,26 @@ void scsiDiskPoll()
                int sdActive = 0;\r
 \r
                while ((i < totalSDSectors) &&\r
-                       ((scsiDev.phase == DATA_OUT) || // scsiDisconnect keeps our phase.\r
+                       (likely(scsiDev.phase == DATA_OUT) || // scsiDisconnect keeps our phase.\r
                                scsiComplete) &&\r
-                       !scsiDev.resetFlag)\r
+                       likely(!scsiDev.resetFlag))\r
                {\r
-                       if ((sdActive == 1) && sdWriteSectorDMAPoll(i == (totalSDSectors - 1)))\r
+                       // Wait for the next DMA interrupt. It's beneficial to halt the\r
+                       // processor to give the DMA controller more memory bandwidth to\r
+                       // work with.\r
+                       // We're optimistically assuming a race condition won't occur\r
+                       // between these checks and the interrupt handers. The 1ms\r
+                       // systick timer interrupt saves us on the event of a race.\r
+                       int scsiBusy = scsiDMABusy();\r
+                       int sdBusy = sdDMABusy();\r
+                       if (scsiBusy && sdBusy) __WFI();\r
+\r
+                       if (sdActive && !sdBusy && sdWriteSectorDMAPoll(i == (totalSDSectors - 1)))\r
                        {\r
                                sdActive = 0;\r
                                i++;\r
                        }\r
-                       else if ((sdActive == 0) && ((prep - i) > 0))\r
+                       else if (!sdActive && ((prep - i) > 0))\r
                        {\r
                                // Start an SD transfer if we have space.\r
                                sdWriteMultiSectorDMA(&scsiDev.data[SD_SECTOR_SIZE * (i % buffers)]);\r
@@ -593,16 +621,16 @@ void scsiDiskPoll()
 \r
                        uint32_t now = getTime_ms();\r
 \r
-                       if ((scsiActive == 1) && scsiReadDMAPoll())\r
+                       if (scsiActive && !scsiBusy && scsiReadDMAPoll())\r
                        {\r
                                scsiActive = 0;\r
                                ++prep;\r
                                lastActivityTime = now;\r
                        }\r
-                       else if ((scsiActive == 0) &&\r
+                       else if (!scsiActive &&\r
                                ((prep - i) < buffers) &&\r
                                (prep < totalSDSectors) &&\r
-                               !scsiDisconnected)\r
+                               likely(!scsiDisconnected))\r
                        {\r
                                int dmaBytes = SD_SECTOR_SIZE;\r
                                if ((prep % sdPerScsi) == (sdPerScsi - 1))\r
@@ -615,10 +643,10 @@ void scsiDiskPoll()
                        }\r
                        else if (\r
                                (scsiActive == 0) &&\r
-                               !scsiDisconnected &&\r
-                               scsiDev.discPriv &&\r
-                               (diffTime_ms(lastActivityTime, now) >= 20) &&\r
-                               (scsiDev.phase == DATA_OUT))\r
+                               likely(!scsiDisconnected) &&\r
+                               unlikely(scsiDev.discPriv) &&\r
+                               unlikely(diffTime_ms(lastActivityTime, now) >= 20) &&\r
+                               likely(scsiDev.phase == DATA_OUT))\r
                        {\r
                                // We're transferring over the SCSI bus faster than the SD card\r
                                // can write.  There is no more buffer space once we've finished\r
@@ -631,12 +659,12 @@ void scsiDiskPoll()
                                scsiDisconnected = 1;\r
                                lastActivityTime = getTime_ms();\r
                        }\r
-                       else if (scsiDisconnected &&\r
+                       else if (unlikely(scsiDisconnected) &&\r
                                (\r
                                        (prep == i) || // Buffers empty.\r
                                        // Send some messages every 100ms so we don't timeout.\r
                                        // At a minimum, a reselection involves an IDENTIFY message.\r
-                                       (diffTime_ms(lastActivityTime, now) >= 100)\r
+                                       unlikely(diffTime_ms(lastActivityTime, now) >= 100)\r
                                ))\r
                        {\r
                                int reconnected = scsiReconnect();\r
@@ -652,13 +680,13 @@ void scsiDiskPoll()
                                }\r
                        }\r
                        else if (\r
-                               !scsiComplete &&\r
+                               likely(!scsiComplete) &&\r
                                (sdActive == 1) &&\r
                                (prep == totalSDSectors) && // All scsi data read and buffered\r
-                               !scsiDev.discPriv && // Prefer disconnect where possible.\r
-                               (diffTime_ms(lastActivityTime, now) >= 150) &&\r
+                               likely(!scsiDev.discPriv) && // Prefer disconnect where possible.\r
+                               unlikely(diffTime_ms(lastActivityTime, now) >= 150) &&\r
 \r
-                               (scsiDev.phase == DATA_OUT) &&\r
+                               likely(scsiDev.phase == DATA_OUT) &&\r
                                !(scsiDev.cdb[scsiDev.cdbLen - 1] & 0x01) // Not linked command\r
                                )\r
                        {\r
@@ -685,8 +713,8 @@ void scsiDiskPoll()
                }\r
                while (\r
                        !scsiDev.resetFlag &&\r
-                       scsiDisconnected &&\r
-                       (diffTime_ms(lastActivityTime, getTime_ms()) <= 10000))\r
+                       unlikely(scsiDisconnected) &&\r
+                       (elapsedTime_ms(lastActivityTime) <= 10000))\r
                {\r
                        scsiDisconnected = !scsiReconnect();\r
                }\r
@@ -723,7 +751,7 @@ void scsiDiskReset()
        transfer.currentBlock = 0;\r
 \r
        // Cancel long running commands!\r
-       if (transfer.inProgress == 1)\r
+       if (unlikely(transfer.inProgress == 1))\r
        {\r
                if (transfer.dir == TRANSFER_WRITE)\r
                {\r
@@ -736,8 +764,6 @@ void scsiDiskReset()
        }\r
        transfer.inProgress = 0;\r
        transfer.multiBlock = 0;\r
-               //              SD_CS_Write(1);\r
-\r
 }\r
 \r
 void scsiDiskInit()\r
@@ -757,3 +783,4 @@ void scsiDiskInit()
        #endif\r
 }\r
 \r
+#pragma GCC pop_options\r