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