Implement WRITE BUFFER and WRITE WITH VERIFY commands
[SCSI2SD-V6.git] / software / SCSI2SD / src / diagnostic.c
index c518d75..709836c 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
@@ -162,4 +164,54 @@ void scsiReadBuffer()
                        (allocLength > MAX_SECTOR_SIZE) ? MAX_SECTOR_SIZE : allocLength;\r
                scsiDev.phase = DATA_IN;\r
        }\r
+       else\r
+       {\r
+               // error.\r
+               scsiDev.status = CHECK_CONDITION;\r
+               scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
+               scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;\r
+               scsiDev.phase = STATUS;\r
+       }\r
+}\r
+\r
+// Callback after the DATA OUT phase is complete.\r
+static void doWriteBuffer(void)\r
+{\r
+       if (scsiDev.status == GOOD) // skip if we've already encountered an error\r
+       {\r
+               // scsiDev.dataLen bytes are in scsiDev.data\r
+               // Don't shift it down 4 bytes ... this space is taken by\r
+               // the read buffer header anyway\r
+               scsiDev.phase = STATUS;\r
+       }\r
 }\r
+\r
+void scsiWriteBuffer()\r
+{\r
+       // WRITE BUFFER\r
+       // Used for testing the speed of the SCSI interface.\r
+       uint8 mode = scsiDev.data[1] & 7;\r
+\r
+       int allocLength =\r
+               (((uint32) scsiDev.cdb[6]) << 16) +\r
+               (((uint32) scsiDev.cdb[7]) << 8) +\r
+               scsiDev.cdb[8];\r
+\r
+       if (mode == 0 && allocLength <= sizeof(scsiDev.data))\r
+       {\r
+               scsiDev.dataLen = allocLength;\r
+               scsiDev.phase = DATA_OUT;\r
+               scsiDev.postDataOutHook = doWriteBuffer;\r
+       }\r
+       else\r
+       {\r
+               // error.\r
+               scsiDev.status = CHECK_CONDITION;\r
+               scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
+               scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;\r
+               scsiDev.phase = STATUS;\r
+       }\r
+}\r
+\r
+\r
+#pragma GCC pop_options\r