Fix USB bug with > 4gb drives
[SCSI2SD-V6.git] / src / firmware / mode.c
1 //      Copyright (C) 2013 Michael McMaster <michael@codesrc.com>\r
2 //  Copyright (C) 2014 Doug Brown <doug@downtowndougbrown.com>\r
3 //\r
4 //      This file is part of SCSI2SD.\r
5 //\r
6 //      SCSI2SD is free software: you can redistribute it and/or modify\r
7 //      it under the terms of the GNU General Public License as published by\r
8 //      the Free Software Foundation, either version 3 of the License, or\r
9 //      (at your option) any later version.\r
10 //\r
11 //      SCSI2SD is distributed in the hope that it will be useful,\r
12 //      but WITHOUT ANY WARRANTY; without even the implied warranty of\r
13 //      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
14 //      GNU General Public License for more details.\r
15 //\r
16 //      You should have received a copy of the GNU General Public License\r
17 //      along with SCSI2SD.  If not, see <http://www.gnu.org/licenses/>.\r
18 \r
19 #include "scsi.h"\r
20 #include "mode.h"\r
21 #include "disk.h"\r
22 \r
23 #include <string.h>\r
24 \r
25 static const uint8_t ReadWriteErrorRecoveryPage[] =\r
26 {\r
27 0x01, // Page code\r
28 0x0A, // Page length\r
29 \r
30 // VMS 5.5-2 is very particular regarding the mode page values.\r
31 // The required values for a SCSI2/NoTCQ device are:\r
32 // AWRE=0 ARRE=0 TB=1 RC=0 EER=? PER=1 DTE=1 DCR=?\r
33 // See ftp://www.digiater.nl/openvms/decus/vms94b/net94b/scsi_params_dkdriver.txt\r
34 // X-Newsgroups: comp.os.vms\r
35 // Subject: Re: VMS 6.1 vs. Seagate Disk Drives\r
36 // Message-Id: <32g87h$8q@nntpd.lkg.dec.com>\r
37 // From: weber@evms.enet.dec.com (Ralph O. Weber -- OpenVMS AXP)\r
38 // Date: 12 Aug 1994 16:32:49 GMT\r
39 0x26,\r
40 \r
41 0x00, // Don't try recovery algorithm during reads\r
42 0x00, // Correction span 0\r
43 0x00, // Head offset count 0,\r
44 0x00, // Data strobe offset count 0,\r
45 0x00, // Reserved\r
46 0x00, // Don't try recovery algorithm during writes\r
47 0x00, // Reserved\r
48 0x00, 0x00 // Recovery time limit 0 (use default)*/\r
49 };\r
50 \r
51 static const uint8_t ReadWriteErrorRecoveryPage_SCSI1[] =\r
52 {\r
53 0x01, // Page code\r
54 0x06, // Page length\r
55 0x26,\r
56 0x00, // Don't try recovery algorithm during reads\r
57 0x00, // Correction span 0\r
58 0x00, // Head offset count 0,\r
59 0x00, // Data strobe offset count 0,\r
60 0xFF // Reserved\r
61 };\r
62 \r
63 static const uint8_t DisconnectReconnectPage[] =\r
64 {\r
65 0x02, // Page code\r
66 0x0E, // Page length\r
67 0, // Buffer full ratio\r
68 0, // Buffer empty ratio\r
69 0x00, 10, // Bus inactivity limit, 100us increments. Allow 1ms.\r
70 0x00, 0x00, // Disconnect time limit\r
71 0x00, 0x00, // Connect time limit\r
72 0x00, 0x00, // Maximum burst size\r
73 0x00 ,// DTDC. Not used.\r
74 0x00, 0x00, 0x00 // Reserved\r
75 };\r
76 \r
77 static const uint8_t DisconnectReconnectPage_SCSI1[] =\r
78 {\r
79 0x02, // Page code\r
80 0x0A, // Page length\r
81 0, // Buffer full ratio\r
82 0, // Buffer empty ratio\r
83 0x00, 10, // Bus inactivity limit, 100us increments. Allow 1ms.\r
84 0x00, 0x00, // Disconnect time limit\r
85 0x00, 0x00, // Connect time limit\r
86 0x00, 0x00 // Maximum burst size\r
87 };\r
88 \r
89 static const uint8_t FormatDevicePage[] =\r
90 {\r
91 0x03 | 0x80, // Page code | PS (persist) bit.\r
92 0x16, // Page length\r
93 0x00, 0x00, // Single zone\r
94 0x00, 0x00, // No alternate sectors\r
95 0x00, 0x00, // No alternate tracks\r
96 0x00, 0x00, // No alternate tracks per lun\r
97 0x00, 0x00, // Sectors per track, configurable\r
98 0xFF, 0xFF, // Data bytes per physical sector. Configurable.\r
99 0x00, 0x01, // Interleave\r
100 0x00, 0x00, // Track skew factor\r
101 0x00, 0x00, // Cylinder skew factor\r
102 0xC0, // SSEC(set) HSEC(set) RMB SURF\r
103 0x00, 0x00, 0x00 // Reserved\r
104 };\r
105 \r
106 static const uint8_t RigidDiskDriveGeometry[] =\r
107 {\r
108 0x04, // Page code\r
109 0x16, // Page length\r
110 0xFF, 0xFF, 0xFF, // Number of cylinders\r
111 0x00, // Number of heads (replaced by configured value)\r
112 0xFF, 0xFF, 0xFF, // Starting cylinder-write precompensation\r
113 0xFF, 0xFF, 0xFF, // Starting cylinder-reduced write current\r
114 0x00, 0x1, // Drive step rate (units of 100ns)\r
115 0x00, 0x00, 0x00, // Landing zone cylinder\r
116 0x00, // RPL\r
117 0x00, // Rotational offset\r
118 0x00, // Reserved\r
119 5400 >> 8, 5400 & 0xFF, // Medium rotation rate (RPM)\r
120 0x00, 0x00 // Reserved\r
121 };\r
122 \r
123 static const uint8_t RigidDiskDriveGeometry_SCSI1[] =\r
124 {\r
125 0x04, // Page code\r
126 0x12, // Page length\r
127 0xFF, 0xFF, 0xFF, // Number of cylinders\r
128 0x00, // Number of heads (replaced by configured value)\r
129 0xFF, 0xFF, 0xFF, // Starting cylinder-write precompensation\r
130 0xFF, 0xFF, 0xFF, // Starting cylinder-reduced write current\r
131 0x00, 0x1, // Drive step rate (units of 100ns)\r
132 0x00, 0x00, 0x00, // Landing zone cylinder\r
133 0x00, // RPL\r
134 0x00, // Rotational offset\r
135 0x00 // Reserved\r
136 };\r
137 \r
138 static const uint8_t CachingPage[] =\r
139 {\r
140 0x08, // Page Code\r
141 0x0A, // Page length\r
142 0x01, // Read cache disable\r
143 0x00, // No useful rention policy.\r
144 0x00, 0x00, // Pre-fetch always disabled\r
145 0x00, 0x00, // Minimum pre-fetch\r
146 0x00, 0x00, // Maximum pre-fetch\r
147 0x00, 0x00, // Maximum pre-fetch ceiling\r
148 };\r
149 \r
150 // Old CCS SCSI-1 cache page\r
151 static const uint8_t CCSCachingPage[] =\r
152 {\r
153 0x38, // Page Code\r
154 0x0E, // Page length\r
155 0x00, // Read cache disable\r
156 0x00, // Prefetch threshold\r
157 0x00, 0x00, // Max threshold / multiplier\r
158 0x00, 0x00, // Min threshold / multiplier\r
159 0x00, 0x00, // Reserved\r
160 0x00, 0x00,\r
161 0x00, 0x00,\r
162 0x00, 0x00,\r
163 };\r
164 \r
165 static const uint8_t ControlModePage[] =\r
166 {\r
167 0x0A, // Page code\r
168 0x06, // Page length\r
169 0x00, // No logging\r
170 0x01, // Disable tagged queuing\r
171 0x00, // No async event notifications\r
172 0x00, // Reserved\r
173 0x00, 0x00 // AEN holdoff period.\r
174 };\r
175 \r
176 static const uint8_t SequentialDeviceConfigPage[] =\r
177 {\r
178 0x10, // page code\r
179 0x0E, // Page length\r
180 0x00, // CAP, CAF, Active Format\r
181 0x00, // Active partition\r
182 0x00, // Write buffer full ratio\r
183 0x00, // Read buffer empty ratio\r
184 0x00,0x01, // Write delay time, in 100ms units\r
185 0x00, // Default gap size\r
186 0x10, // auto-generation of default eod (end of data)\r
187 0x00,0x00,0x00, // buffer-size at early warning\r
188 0x00, // No data compression\r
189 0x00 // reserved\r
190 };\r
191 \r
192 // Allow Apple 68k Drive Setup to format this drive.\r
193 // Code\r
194 static const uint8_t AppleVendorPage[] =\r
195 {\r
196 0x30, // Page code\r
197 28, // Page length\r
198 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,\r
199 'A','P','P','L','E',' ','C','O','M','P','U','T','E','R',',',' ','I','N','C','.'\r
200 };\r
201 \r
202 static void pageIn(int pc, int dataIdx, const uint8_t* pageData, int pageLen)\r
203 {\r
204         memcpy(&scsiDev.data[dataIdx], pageData, pageLen);\r
205 \r
206         if (pc == 0x01) // Mask out (un)changable values\r
207         {\r
208                 memset(&scsiDev.data[dataIdx+2], 0, pageLen - 2);\r
209         }\r
210 }\r
211 \r
212 static void doModeSense(\r
213         int sixByteCmd, int dbd, int pc, int pageCode, int allocLength)\r
214 {\r
215         ////////////// Mode Parameter Header\r
216         ////////////////////////////////////\r
217 \r
218         // Skip the Mode Data Length, we set that last.\r
219         int idx = 1;\r
220         if (!sixByteCmd) ++idx;\r
221 \r
222         uint8_t mediumType = 0;\r
223         uint8_t deviceSpecificParam = 0;\r
224         uint8_t density = 0;\r
225         switch (scsiDev.target->cfg->deviceType)\r
226         {\r
227         case S2S_CFG_FIXED:\r
228         case S2S_CFG_REMOVEABLE:\r
229                 mediumType = 0; // We should support various floppy types here!\r
230                 // Contains cache bits (0) and a Write-Protect bit.\r
231                 deviceSpecificParam =\r
232                         (blockDev.state & DISK_WP) ? 0x80 : 0;\r
233                 density = 0; // reserved for direct access\r
234                 break;\r
235 \r
236         case S2S_CFG_FLOPPY_14MB:\r
237                 mediumType = 0x1E; // 90mm/3.5"\r
238                 deviceSpecificParam =\r
239                         (blockDev.state & DISK_WP) ? 0x80 : 0;\r
240                 density = 0; // reserved for direct access\r
241                 break;\r
242 \r
243         case S2S_CFG_OPTICAL:\r
244                 mediumType = 0x02; // 120mm CDROM, data only.\r
245                 deviceSpecificParam = 0;\r
246                 density = 0x01; // User data only, 2048bytes per sector.\r
247                 break;\r
248 \r
249         case S2S_CFG_SEQUENTIAL:\r
250                 mediumType = 0; // reserved\r
251                 deviceSpecificParam =\r
252                         (blockDev.state & DISK_WP) ? 0x80 : 0;\r
253                 density = 0x13; // DAT Data Storage, X3B5/88-185A \r
254                 break;\r
255 \r
256         case S2S_CFG_MO:\r
257         mediumType = 0x03; // Optical reversible or erasable medium\r
258                 deviceSpecificParam =\r
259                         (blockDev.state & DISK_WP) ? 0x80 : 0;\r
260                 density = 0x00; // Default\r
261                 break;\r
262 \r
263         };\r
264 \r
265         scsiDev.data[idx++] = mediumType;\r
266         scsiDev.data[idx++] = deviceSpecificParam;\r
267 \r
268         if (sixByteCmd)\r
269         {\r
270                 if (dbd)\r
271                 {\r
272                         scsiDev.data[idx++] = 0; // No block descriptor\r
273                 }\r
274                 else\r
275                 {\r
276                         // One block descriptor of length 8 bytes.\r
277                         scsiDev.data[idx++] = 8;\r
278                 }\r
279         }\r
280         else\r
281         {\r
282                 scsiDev.data[idx++] = 0; // Reserved\r
283                 scsiDev.data[idx++] = 0; // Reserved\r
284                 if (dbd)\r
285                 {\r
286                         scsiDev.data[idx++] = 0; // No block descriptor\r
287                         scsiDev.data[idx++] = 0; // No block descriptor\r
288                 }\r
289                 else\r
290                 {\r
291                         // One block descriptor of length 8 bytes.\r
292                         scsiDev.data[idx++] = 0;\r
293                         scsiDev.data[idx++] = 8;\r
294                 }\r
295         }\r
296 \r
297         ////////////// Block Descriptor\r
298         ////////////////////////////////////\r
299         if (!dbd)\r
300         {\r
301                 scsiDev.data[idx++] = density;\r
302                 // Number of blocks\r
303                 // Zero == all remaining blocks shall have the medium\r
304                 // characteristics specified.\r
305                 scsiDev.data[idx++] = 0;\r
306                 scsiDev.data[idx++] = 0;\r
307                 scsiDev.data[idx++] = 0;\r
308 \r
309                 scsiDev.data[idx++] = 0; // reserved\r
310 \r
311                 // Block length\r
312                 uint32_t bytesPerSector = scsiDev.target->liveCfg.bytesPerSector;\r
313                 scsiDev.data[idx++] = bytesPerSector >> 16;\r
314                 scsiDev.data[idx++] = bytesPerSector >> 8;\r
315                 scsiDev.data[idx++] = bytesPerSector & 0xFF;\r
316         }\r
317 \r
318         int pageFound = 0;\r
319 \r
320         if (pageCode == 0x01 || pageCode == 0x3F)\r
321         {\r
322                 pageFound = 1;\r
323                 if ((scsiDev.compatMode >= COMPAT_SCSI2))\r
324                 {\r
325                         pageIn(pc, idx, ReadWriteErrorRecoveryPage, sizeof(ReadWriteErrorRecoveryPage));\r
326                         idx += sizeof(ReadWriteErrorRecoveryPage);\r
327                 }\r
328                 else\r
329                 {\r
330                         pageIn(pc, idx, ReadWriteErrorRecoveryPage_SCSI1, sizeof(ReadWriteErrorRecoveryPage_SCSI1));\r
331                         idx += sizeof(ReadWriteErrorRecoveryPage_SCSI1);\r
332                 }\r
333         }\r
334 \r
335         if (pageCode == 0x02 || pageCode == 0x3F)\r
336         {\r
337                 pageFound = 1;\r
338                 if ((scsiDev.compatMode >= COMPAT_SCSI2))\r
339                 {\r
340                         pageIn(pc, idx, DisconnectReconnectPage, sizeof(DisconnectReconnectPage));\r
341                         idx += sizeof(DisconnectReconnectPage);\r
342                 }\r
343                 else\r
344                 {\r
345                         pageIn(pc, idx, DisconnectReconnectPage_SCSI1, sizeof(DisconnectReconnectPage_SCSI1));\r
346                         idx += sizeof(DisconnectReconnectPage_SCSI1);\r
347                 }\r
348         }\r
349 \r
350         if (pageCode == 0x03 || pageCode == 0x3F)\r
351         {\r
352                 pageFound = 1;\r
353                 pageIn(pc, idx, FormatDevicePage, sizeof(FormatDevicePage));\r
354                 if (pc != 0x01)\r
355                 {\r
356                         uint16_t sectorsPerTrack = scsiDev.target->cfg->sectorsPerTrack;\r
357                         scsiDev.data[idx+10] = sectorsPerTrack >> 8;\r
358                         scsiDev.data[idx+11] = sectorsPerTrack & 0xFF;\r
359 \r
360                         // Fill out the configured bytes-per-sector\r
361                         uint32_t bytesPerSector = scsiDev.target->liveCfg.bytesPerSector;\r
362                         scsiDev.data[idx+12] = bytesPerSector >> 8;\r
363                         scsiDev.data[idx+13] = bytesPerSector & 0xFF;\r
364                 }\r
365                 else\r
366                 {\r
367                         // Set a mask for the changeable values.\r
368                         scsiDev.data[idx+12] = 0xFF;\r
369                         scsiDev.data[idx+13] = 0xFF;\r
370                 }\r
371 \r
372                 idx += sizeof(FormatDevicePage);\r
373         }\r
374 \r
375         if (pageCode == 0x04 || pageCode == 0x3F)\r
376         {\r
377                 pageFound = 1;\r
378                 if ((scsiDev.compatMode >= COMPAT_SCSI2))\r
379                 {\r
380                         pageIn(pc, idx, RigidDiskDriveGeometry, sizeof(RigidDiskDriveGeometry));\r
381                 }\r
382                 else\r
383                 {\r
384                         pageIn(pc, idx, RigidDiskDriveGeometry_SCSI1, sizeof(RigidDiskDriveGeometry_SCSI1));\r
385                 }\r
386 \r
387                 if (pc != 0x01)\r
388                 {\r
389                         // Need to fill out the number of cylinders.\r
390                         uint32_t cyl;\r
391                         uint8_t head;\r
392                         uint32_t sector;\r
393                         LBA2CHS(\r
394                                 getScsiCapacity(\r
395                                         scsiDev.target->cfg->sdSectorStart,\r
396                                         scsiDev.target->liveCfg.bytesPerSector,\r
397                                         scsiDev.target->cfg->scsiSectors),\r
398                                 &cyl,\r
399                                 &head,\r
400                                 &sector,\r
401                                 scsiDev.target->cfg->headsPerCylinder,\r
402                                 scsiDev.target->cfg->sectorsPerTrack);\r
403 \r
404                         scsiDev.data[idx+2] = cyl >> 16;\r
405                         scsiDev.data[idx+3] = cyl >> 8;\r
406                         scsiDev.data[idx+4] = cyl;\r
407 \r
408                         memcpy(&scsiDev.data[idx+6], &scsiDev.data[idx+2], 3);\r
409                         memcpy(&scsiDev.data[idx+9], &scsiDev.data[idx+2], 3);\r
410 \r
411                         scsiDev.data[idx+5] = scsiDev.target->cfg->headsPerCylinder;\r
412                 }\r
413 \r
414                 if ((scsiDev.compatMode >= COMPAT_SCSI2))\r
415                 {\r
416                         idx += sizeof(RigidDiskDriveGeometry);\r
417                 }\r
418                 else\r
419                 {\r
420                         idx += sizeof(RigidDiskDriveGeometry_SCSI1);\r
421                 }\r
422         }\r
423 \r
424         // DON'T output the following pages for SCSI1 hosts. They get upset when\r
425         // we have more data to send than the allocation length provided.\r
426         // (ie. Try not to output any more pages below this comment)\r
427 \r
428 \r
429         if ((scsiDev.compatMode >= COMPAT_SCSI2) &&\r
430                 (pageCode == 0x08 || pageCode == 0x3F))\r
431         {\r
432                 pageFound = 1;\r
433                 pageIn(pc, idx, CachingPage, sizeof(CachingPage));\r
434                 idx += sizeof(CachingPage);\r
435         }\r
436 \r
437         if ((scsiDev.compatMode >= COMPAT_SCSI2)\r
438                 && (pageCode == 0x0A || pageCode == 0x3F))\r
439         {\r
440                 pageFound = 1;\r
441                 pageIn(pc, idx, ControlModePage, sizeof(ControlModePage));\r
442                 idx += sizeof(ControlModePage);\r
443         }\r
444 \r
445         if ((scsiDev.target->cfg->deviceType == S2S_CFG_SEQUENTIAL) &&\r
446                 (pageCode == 0x10 || pageCode == 0x3F))\r
447         {\r
448                 pageFound = 1;\r
449                 pageIn(\r
450                         pc,\r
451                         idx,\r
452                         SequentialDeviceConfigPage,\r
453                         sizeof(SequentialDeviceConfigPage));\r
454                 idx += sizeof(SequentialDeviceConfigPage);\r
455         }\r
456 \r
457         if ((\r
458                         (scsiDev.target->cfg->quirks == S2S_CFG_QUIRKS_APPLE) ||\r
459                         (idx + sizeof(AppleVendorPage) <= allocLength)\r
460                 ) &&\r
461                 (pageCode == 0x30 || pageCode == 0x3F))\r
462         {\r
463                 pageFound = 1;\r
464                 pageIn(pc, idx, AppleVendorPage, sizeof(AppleVendorPage));\r
465                 idx += sizeof(AppleVendorPage);\r
466         }\r
467 \r
468         if (pageCode == 0x38) // Don't send unless requested\r
469         {\r
470                 pageFound = 1;\r
471                 pageIn(pc, idx, CCSCachingPage, sizeof(CCSCachingPage));\r
472                 idx += sizeof(CCSCachingPage);\r
473         }\r
474 \r
475         if (!pageFound)\r
476         {\r
477                 // Unknown Page Code\r
478                 pageFound = 0;\r
479                 scsiDev.status = CHECK_CONDITION;\r
480                 scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
481                 scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;\r
482                 scsiDev.phase = STATUS;\r
483         }\r
484         else\r
485         {\r
486                 // Go back and fill out the mode data length\r
487                 if (sixByteCmd)\r
488                 {\r
489                         // Cannot currently exceed limits. yay\r
490                         scsiDev.data[0] = idx - 1;\r
491                 }\r
492                 else\r
493                 {\r
494                         scsiDev.data[0] = ((idx - 2) >> 8);\r
495                         scsiDev.data[1] = (idx - 2);\r
496                 }\r
497 \r
498                 scsiDev.dataLen = idx > allocLength ? allocLength : idx;\r
499                 scsiDev.phase = DATA_IN;\r
500         }\r
501 }\r
502 \r
503 \r
504 // Callback after the DATA OUT phase is complete.\r
505 static void doModeSelect(void)\r
506 {\r
507         if (scsiDev.status == GOOD) // skip if we've already encountered an error\r
508         {\r
509                 // scsiDev.dataLen bytes are in scsiDev.data\r
510 \r
511                 int idx;\r
512                 int blockDescLen;\r
513                 if (scsiDev.cdb[0] == 0x55)\r
514                 {\r
515                         blockDescLen =\r
516                                 (((uint16_t)scsiDev.data[6]) << 8) |scsiDev.data[7];\r
517                         idx = 8;\r
518                 }\r
519                 else\r
520                 {\r
521                         blockDescLen = scsiDev.data[3];\r
522                         idx = 4;\r
523                 }\r
524 \r
525                 // The unwritten rule.  Blocksizes are normally set using the\r
526                 // block descriptor value, not by changing page 0x03.\r
527                 if (blockDescLen >= 8)\r
528                 {\r
529                         uint32_t bytesPerSector =\r
530                                 (((uint32_t)scsiDev.data[idx+5]) << 16) |\r
531                                 (((uint32_t)scsiDev.data[idx+6]) << 8) |\r
532                                 scsiDev.data[idx+7];\r
533                         if ((bytesPerSector < MIN_SECTOR_SIZE) ||\r
534                                 (bytesPerSector > MAX_SECTOR_SIZE))\r
535                         {\r
536                                 goto bad;\r
537                         }\r
538                         else\r
539                         {\r
540                                 scsiDev.target->liveCfg.bytesPerSector = bytesPerSector;\r
541                                 if (bytesPerSector != scsiDev.target->cfg->bytesPerSector)\r
542                                 {\r
543                                         s2s_configSave(scsiDev.target->targetId, bytesPerSector);\r
544                                 }\r
545                         }\r
546                 }\r
547                 idx += blockDescLen;\r
548 \r
549                 while (idx < scsiDev.dataLen)\r
550                 {\r
551                         int pageLen = scsiDev.data[idx + 1];\r
552                         if (idx + 2 + pageLen > scsiDev.dataLen) goto bad;\r
553 \r
554                         int pageCode = scsiDev.data[idx] & 0x3F;\r
555                         switch (pageCode)\r
556                         {\r
557                         case 0x03: // Format Device Page\r
558                         {\r
559                                 if (pageLen != 0x16) goto bad;\r
560 \r
561                                 // Fill out the configured bytes-per-sector\r
562                                 uint16_t bytesPerSector =\r
563                                         (((uint16_t)scsiDev.data[idx+12]) << 8) |\r
564                                         scsiDev.data[idx+13];\r
565 \r
566                                 // Sane values only, ok ?\r
567                                 if ((bytesPerSector < MIN_SECTOR_SIZE) ||\r
568                                         (bytesPerSector > MAX_SECTOR_SIZE))\r
569                                 {\r
570                                         goto bad;\r
571                                 }\r
572 \r
573                                 scsiDev.target->liveCfg.bytesPerSector = bytesPerSector;\r
574                                 if (scsiDev.cdb[1] & 1) // SP Save Pages flag\r
575                                 {\r
576                                         s2s_configSave(scsiDev.target->targetId, bytesPerSector);\r
577                                 }\r
578                         }\r
579                         break;\r
580                         //default:\r
581 \r
582                                 // Easiest to just ignore for now. We'll get here when changing\r
583                                 // the SCSI block size via the descriptor header.\r
584                         }\r
585                         idx += 2 + pageLen;\r
586                 }\r
587         }\r
588 \r
589         goto out;\r
590 bad:\r
591         scsiDev.status = CHECK_CONDITION;\r
592         scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
593         scsiDev.target->sense.asc = INVALID_FIELD_IN_PARAMETER_LIST;\r
594 \r
595 out:\r
596         scsiDev.phase = STATUS;\r
597 }\r
598 \r
599 int scsiModeCommand()\r
600 {\r
601         int commandHandled = 1;\r
602 \r
603         uint8_t command = scsiDev.cdb[0];\r
604 \r
605         // We don't currently support the setting of any parameters.\r
606         // (ie. no MODE SELECT(6) or MODE SELECT(10) commands)\r
607 \r
608         if (command == 0x1A)\r
609         {\r
610                 // MODE SENSE(6)\r
611                 int dbd = scsiDev.cdb[1] & 0x08; // Disable block descriptors\r
612                 int pc = scsiDev.cdb[2] >> 6; // Page Control\r
613                 int pageCode = scsiDev.cdb[2] & 0x3F;\r
614                 int allocLength = scsiDev.cdb[4];\r
615 \r
616                 // SCSI1 standard: (CCS X3T9.2/86-52)\r
617                 // "An Allocation Length of zero indicates that no MODE SENSE data shall\r
618                 // be transferred. This condition shall not be considered as an error."\r
619                 doModeSense(1, dbd, pc, pageCode, allocLength);\r
620         }\r
621         else if (command == 0x5A)\r
622         {\r
623                 // MODE SENSE(10)\r
624                 int dbd = scsiDev.cdb[1] & 0x08; // Disable block descriptors\r
625                 int pc = scsiDev.cdb[2] >> 6; // Page Control\r
626                 int pageCode = scsiDev.cdb[2] & 0x3F;\r
627                 int allocLength =\r
628                         (((uint16_t) scsiDev.cdb[7]) << 8) +\r
629                         scsiDev.cdb[8];\r
630                 doModeSense(0, dbd, pc, pageCode, allocLength);\r
631         }\r
632         else if (command == 0x15)\r
633         {\r
634                 // MODE SELECT(6)\r
635                 int len = scsiDev.cdb[4];\r
636                 if (len == 0)\r
637                 {\r
638                         // If len == 0, then transfer no data. From the SCSI 2 standard:\r
639                         //      A parameter list length of zero indicates that no data shall\r
640                         //      be transferred. This condition shall not be considered as an\r
641                         //              error.\r
642                         scsiDev.phase = STATUS;\r
643                 }\r
644                 else\r
645                 {\r
646                         scsiDev.dataLen = len;\r
647                         scsiDev.phase = DATA_OUT;\r
648                         scsiDev.postDataOutHook = doModeSelect;\r
649                 }\r
650         }\r
651         else if (command == 0x55)\r
652         {\r
653                 // MODE SELECT(10)\r
654                 int allocLength = (((uint16_t) scsiDev.cdb[7]) << 8) + scsiDev.cdb[8];\r
655                 if (allocLength == 0)\r
656                 {\r
657                         // If len == 0, then transfer no data. From the SCSI 2 standard:\r
658                         //      A parameter list length of zero indicates that no data shall\r
659                         //      be transferred. This condition shall not be considered as an\r
660                         //              error.\r
661                         scsiDev.phase = STATUS;\r
662                 }\r
663                 else\r
664                 {\r
665                         scsiDev.dataLen = allocLength;\r
666                         scsiDev.phase = DATA_OUT;\r
667                         scsiDev.postDataOutHook = doModeSelect;\r
668                 }\r
669         }\r
670         else\r
671         {\r
672                 commandHandled = 0;\r
673         }\r
674 \r
675         return commandHandled;\r
676 }\r
677 \r