Implement WRITE BUFFER and WRITE WITH VERIFY commands
[SCSI2SD-V6.git] / software / SCSI2SD / src / diagnostic.c
1 //      Copyright (C) 2013 Michael McMaster <michael@codesrc.com>\r
2 //\r
3 //      This file is part of SCSI2SD.\r
4 //\r
5 //      SCSI2SD is free software: you can redistribute it and/or modify\r
6 //      it under the terms of the GNU General Public License as published by\r
7 //      the Free Software Foundation, either version 3 of the License, or\r
8 //      (at your option) any later version.\r
9 //\r
10 //      SCSI2SD is distributed in the hope that it will be useful,\r
11 //      but WITHOUT ANY WARRANTY; without even the implied warranty of\r
12 //      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
13 //      GNU General Public License for more details.\r
14 //\r
15 //      You should have received a copy of the GNU General Public License\r
16 //      along with SCSI2SD.  If not, see <http://www.gnu.org/licenses/>.\r
17 #pragma GCC push_options\r
18 #pragma GCC optimize("-flto")\r
19 \r
20 #include "device.h"\r
21 #include "scsi.h"\r
22 #include "diagnostic.h"\r
23 \r
24 #include <string.h>\r
25 \r
26 static const uint8 SupportedDiagnosticPages[] =\r
27 {\r
28 0x00, // Page Code\r
29 0x00, // Reserved\r
30 0x02, // Page length\r
31 0x00, // Support "Supported diagnostic page"\r
32 0x40  // Support "Translate address page"\r
33 };\r
34 \r
35 void scsiSendDiagnostic()\r
36 {\r
37         // SEND DIAGNOSTIC\r
38         // Pretend to do self-test. Actual data is returned via the\r
39         // RECEIVE DIAGNOSTIC RESULTS command.\r
40         int selfTest = scsiDev.cdb[1] & 0x04;\r
41         uint32 paramLength =\r
42                 (((uint32) scsiDev.cdb[3]) << 8) +\r
43                 scsiDev.cdb[4];\r
44 \r
45         if (!selfTest)\r
46         {\r
47                 // Initiator sends us page data.\r
48                 scsiDev.dataLen = paramLength;\r
49                 scsiDev.phase = DATA_OUT;\r
50 \r
51                 if (scsiDev.dataLen > sizeof (scsiDev.data))\r
52                 {\r
53                         // Nowhere to store this data!\r
54                         // Shouldn't happen - our buffer should be many magnitudes larger\r
55                         // than the required size for diagnostic parameters.\r
56                         scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
57                         scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;\r
58                         scsiDev.status = CHECK_CONDITION;\r
59                         scsiDev.phase = STATUS;\r
60                 }\r
61         }\r
62         else\r
63         {\r
64                 // Default command result will be a status of GOOD anyway.\r
65         }\r
66 }\r
67 \r
68 void scsiReceiveDiagnostic()\r
69 {\r
70         // RECEIVE DIAGNOSTIC RESULTS\r
71         // We assume scsiDev.data contains the contents of a previous\r
72         // SEND DIAGNOSTICS command.  We only care about the page-code part\r
73         // of the parameter list.\r
74         uint8 pageCode = scsiDev.data[0];\r
75 \r
76         int allocLength =\r
77                 (((uint16) scsiDev.cdb[3]) << 8) +\r
78                 scsiDev.cdb[4];\r
79 \r
80 \r
81         if (pageCode == 0x00)\r
82         {\r
83                 memcpy(\r
84                         scsiDev.data,\r
85                         SupportedDiagnosticPages,\r
86                         sizeof(SupportedDiagnosticPages));\r
87                 scsiDev.dataLen = sizeof(SupportedDiagnosticPages);\r
88                 scsiDev.phase = DATA_IN;\r
89         }\r
90         else if (pageCode == 0x40)\r
91         {\r
92                 // Translate between logical block address, physical sector address, or\r
93                 // physical bytes.\r
94                 uint8 suppliedFmt = scsiDev.data[4] & 0x7;\r
95                 uint8 translateFmt = scsiDev.data[5] & 0x7;\r
96 \r
97                 // Convert each supplied address back to a simple\r
98                 // 64bit linear address, then convert back again.\r
99                 uint64 fromByteAddr =\r
100                         scsiByteAddress(\r
101                                 scsiDev.target->liveCfg.bytesPerSector,\r
102                                 suppliedFmt,\r
103                                 &scsiDev.data[6]);\r
104 \r
105                 scsiSaveByteAddress(\r
106                         scsiDev.target->liveCfg.bytesPerSector,\r
107                         translateFmt,\r
108                         fromByteAddr,\r
109                         &scsiDev.data[6]);\r
110 \r
111                 // Fill out the rest of the response.\r
112                 // (Clear out any optional bits).\r
113                 scsiDev.data[4] = suppliedFmt;\r
114                 scsiDev.data[5] = translateFmt;\r
115 \r
116                 scsiDev.dataLen = 14;\r
117                 scsiDev.phase = DATA_IN;\r
118         }\r
119         else\r
120         {\r
121                 // error.\r
122                 scsiDev.status = CHECK_CONDITION;\r
123                 scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
124                 scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;\r
125                 scsiDev.phase = STATUS;\r
126         }\r
127 \r
128         if (scsiDev.phase == DATA_IN && scsiDev.dataLen > allocLength)\r
129         {\r
130                 // simply truncate the response.\r
131                 scsiDev.dataLen = allocLength;\r
132         }\r
133 \r
134         {\r
135                 // Set the first byte to indicate LUN presence.\r
136                 if (scsiDev.lun) // We only support lun 0\r
137                 {\r
138                         scsiDev.data[0] = 0x7F;\r
139                 }\r
140         }\r
141 }\r
142 \r
143 void scsiReadBuffer()\r
144 {\r
145         // READ BUFFER\r
146         // Used for testing the speed of the SCSI interface.\r
147         uint8 mode = scsiDev.data[1] & 7;\r
148 \r
149         int allocLength =\r
150                 (((uint32) scsiDev.cdb[6]) << 16) +\r
151                 (((uint32) scsiDev.cdb[7]) << 8) +\r
152                 scsiDev.cdb[8];\r
153 \r
154         if (mode == 0)\r
155         {\r
156                 uint32_t maxSize = MAX_SECTOR_SIZE - 4;\r
157                 // 4 byte header\r
158                 scsiDev.data[0] = 0;\r
159                 scsiDev.data[1] = (maxSize >> 16) & 0xff;\r
160                 scsiDev.data[2] = (maxSize >> 8) & 0xff;\r
161                 scsiDev.data[3] = maxSize & 0xff;\r
162 \r
163                 scsiDev.dataLen =\r
164                         (allocLength > MAX_SECTOR_SIZE) ? MAX_SECTOR_SIZE : allocLength;\r
165                 scsiDev.phase = DATA_IN;\r
166         }\r
167         else\r
168         {\r
169                 // error.\r
170                 scsiDev.status = CHECK_CONDITION;\r
171                 scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
172                 scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;\r
173                 scsiDev.phase = STATUS;\r
174         }\r
175 }\r
176 \r
177 // Callback after the DATA OUT phase is complete.\r
178 static void doWriteBuffer(void)\r
179 {\r
180         if (scsiDev.status == GOOD) // skip if we've already encountered an error\r
181         {\r
182                 // scsiDev.dataLen bytes are in scsiDev.data\r
183                 // Don't shift it down 4 bytes ... this space is taken by\r
184                 // the read buffer header anyway\r
185                 scsiDev.phase = STATUS;\r
186         }\r
187 }\r
188 \r
189 void scsiWriteBuffer()\r
190 {\r
191         // WRITE BUFFER\r
192         // Used for testing the speed of the SCSI interface.\r
193         uint8 mode = scsiDev.data[1] & 7;\r
194 \r
195         int allocLength =\r
196                 (((uint32) scsiDev.cdb[6]) << 16) +\r
197                 (((uint32) scsiDev.cdb[7]) << 8) +\r
198                 scsiDev.cdb[8];\r
199 \r
200         if (mode == 0 && allocLength <= sizeof(scsiDev.data))\r
201         {\r
202                 scsiDev.dataLen = allocLength;\r
203                 scsiDev.phase = DATA_OUT;\r
204                 scsiDev.postDataOutHook = doWriteBuffer;\r
205         }\r
206         else\r
207         {\r
208                 // error.\r
209                 scsiDev.status = CHECK_CONDITION;\r
210                 scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
211                 scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;\r
212                 scsiDev.phase = STATUS;\r
213         }\r
214 }\r
215 \r
216 \r
217 #pragma GCC pop_options\r