usb4bsd: Use NULL for pointers.
[dragonfly.git] / sys / bus / u4b / storage / ustorage_fs.c
1 /* $FreeBSD$ */
2 /*-
3  * Copyright (C) 2003-2005 Alan Stern
4  * Copyright (C) 2008 Hans Petter Selasky
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  * 1. Redistributions of source code must retain the above copyright
11  *    notice, this list of conditions, and the following disclaimer,
12  *    without modification.
13  * 2. Redistributions in binary form must reproduce the above copyright
14  *    notice, this list of conditions and the following disclaimer in the
15  *    documentation and/or other materials provided with the distribution.
16  * 3. The names of the above-listed copyright holders may not be used
17  *    to endorse or promote products derived from this software without
18  *    specific prior written permission.
19  *
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
22  * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
23  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
24  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
25  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
26  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
27  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
28  * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
29  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
30  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
31  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32  */
33
34 /*
35  * NOTE: Much of the SCSI statemachine handling code derives from the
36  * Linux USB gadget stack.
37  */
38
39 #include <sys/stdint.h>
40 #include <sys/param.h>
41 #include <sys/queue.h>
42 #include <sys/types.h>
43 #include <sys/systm.h>
44 #include <sys/kernel.h>
45 #include <sys/bus.h>
46 #include <sys/module.h>
47 #include <sys/lock.h>
48 #include <sys/mutex.h>
49 #include <sys/condvar.h>
50 #include <sys/sysctl.h>
51 #include <sys/unistd.h>
52 #include <sys/callout.h>
53 #include <sys/malloc.h>
54 #include <sys/priv.h>
55
56 #include <bus/u4b/usb.h>
57 #include <bus/u4b/usbdi.h>
58 #include <bus/u4b/usbdevs.h>
59 #include "usb_if.h"
60
61 #define USB_DEBUG_VAR ustorage_fs_debug
62 #include <bus/u4b/usb_debug.h>
63
64 #ifdef USB_DEBUG
65 static int ustorage_fs_debug = 0;
66
67 SYSCTL_NODE(_hw_usb, OID_AUTO, ustorage_fs, CTLFLAG_RW, 0, "USB ustorage_fs");
68 SYSCTL_INT(_hw_usb_ustorage_fs, OID_AUTO, debug, CTLFLAG_RW,
69     &ustorage_fs_debug, 0, "ustorage_fs debug level");
70 #endif
71
72 /* Define some limits */
73
74 #ifndef USTORAGE_FS_BULK_SIZE 
75 #define USTORAGE_FS_BULK_SIZE (1UL << 17)       /* bytes */
76 #endif
77
78 #ifndef USTORAGE_FS_MAX_LUN
79 #define USTORAGE_FS_MAX_LUN     8       /* units */
80 #endif
81
82 #ifndef USTORAGE_QDATA_MAX
83 #define USTORAGE_QDATA_MAX      40      /* bytes */
84 #endif
85
86 #define sc_cmd_data sc_cbw.CBWCDB
87
88 /*
89  * The SCSI ID string must be exactly 28 characters long
90  * exluding the terminating zero.
91  */
92 #ifndef USTORAGE_FS_ID_STRING
93 #define USTORAGE_FS_ID_STRING \
94         "FreeBSD " /* 8 */ \
95         "File-Stor Gadget" /* 16 */ \
96         "0101" /* 4 */
97 #endif
98
99 /*
100  * The following macro defines the number of
101  * sectors to be allocated for the RAM disk:
102  */
103 #ifndef USTORAGE_FS_RAM_SECT
104 #define USTORAGE_FS_RAM_SECT (1UL << 13)
105 #endif
106
107 static uint8_t *ustorage_fs_ramdisk;
108
109 /* USB transfer definitions */
110
111 #define USTORAGE_FS_T_BBB_COMMAND     0
112 #define USTORAGE_FS_T_BBB_DATA_DUMP   1
113 #define USTORAGE_FS_T_BBB_DATA_READ   2
114 #define USTORAGE_FS_T_BBB_DATA_WRITE  3
115 #define USTORAGE_FS_T_BBB_STATUS      4
116 #define USTORAGE_FS_T_BBB_MAX         5
117
118 /* USB data stage direction */
119
120 #define DIR_NONE        0
121 #define DIR_READ        1
122 #define DIR_WRITE       2
123
124 /* USB interface specific control request */
125
126 #define UR_BBB_RESET            0xff    /* Bulk-Only reset */
127 #define UR_BBB_GET_MAX_LUN      0xfe    /* Get maximum lun */
128
129 /* Command Block Wrapper */
130 typedef struct {
131         uDWord  dCBWSignature;
132 #define CBWSIGNATURE    0x43425355
133         uDWord  dCBWTag;
134         uDWord  dCBWDataTransferLength;
135         uByte   bCBWFlags;
136 #define CBWFLAGS_OUT    0x00
137 #define CBWFLAGS_IN     0x80
138         uByte   bCBWLUN;
139         uByte   bCDBLength;
140 #define CBWCDBLENGTH    16
141         uByte   CBWCDB[CBWCDBLENGTH];
142 } __packed ustorage_fs_bbb_cbw_t;
143
144 #define USTORAGE_FS_BBB_CBW_SIZE        31
145
146 /* Command Status Wrapper */
147 typedef struct {
148         uDWord  dCSWSignature;
149 #define CSWSIGNATURE    0x53425355
150         uDWord  dCSWTag;
151         uDWord  dCSWDataResidue;
152         uByte   bCSWStatus;
153 #define CSWSTATUS_GOOD  0x0
154 #define CSWSTATUS_FAILED        0x1
155 #define CSWSTATUS_PHASE 0x2
156 } __packed ustorage_fs_bbb_csw_t;
157
158 #define USTORAGE_FS_BBB_CSW_SIZE        13
159
160 struct ustorage_fs_lun {
161
162         uint8_t *memory_image;
163
164         uint32_t num_sectors;
165         uint32_t sense_data;
166         uint32_t sense_data_info;
167         uint32_t unit_attention_data;
168
169         uint8_t read_only:1;
170         uint8_t prevent_medium_removal:1;
171         uint8_t info_valid:1;
172         uint8_t removable:1;
173 };
174
175 struct ustorage_fs_softc {
176
177         ustorage_fs_bbb_cbw_t sc_cbw;   /* Command Wrapper Block */
178         ustorage_fs_bbb_csw_t sc_csw;   /* Command Status Block */
179
180         struct lock sc_lock;
181
182         struct ustorage_fs_lun sc_lun[USTORAGE_FS_MAX_LUN];
183
184         struct {
185                 uint8_t *data_ptr;
186                 struct ustorage_fs_lun *currlun;
187
188                 uint32_t data_rem;      /* bytes, as reported by the command
189                                          * block wrapper */
190                 uint32_t offset;        /* bytes */
191
192                 uint8_t cbw_dir;
193                 uint8_t cmd_dir;
194                 uint8_t lun;
195                 uint8_t cmd_len;
196                 uint8_t data_short:1;
197                 uint8_t data_error:1;
198         }       sc_transfer;
199
200         device_t sc_dev;
201         struct usb_device *sc_udev;
202         struct usb_xfer *sc_xfer[USTORAGE_FS_T_BBB_MAX];
203
204         uint8_t sc_iface_no;            /* interface number */
205         uint8_t sc_last_lun;
206         uint8_t sc_last_xfer_index;
207         uint8_t sc_qdata[USTORAGE_QDATA_MAX];
208 };
209
210 /* prototypes */
211
212 static device_probe_t ustorage_fs_probe;
213 static device_attach_t ustorage_fs_attach;
214 static device_detach_t ustorage_fs_detach;
215 static device_suspend_t ustorage_fs_suspend;
216 static device_resume_t ustorage_fs_resume;
217 static usb_handle_request_t ustorage_fs_handle_request;
218
219 static usb_callback_t ustorage_fs_t_bbb_command_callback;
220 static usb_callback_t ustorage_fs_t_bbb_data_dump_callback;
221 static usb_callback_t ustorage_fs_t_bbb_data_read_callback;
222 static usb_callback_t ustorage_fs_t_bbb_data_write_callback;
223 static usb_callback_t ustorage_fs_t_bbb_status_callback;
224
225 static void ustorage_fs_transfer_start(struct ustorage_fs_softc *sc, uint8_t xfer_index);
226 static void ustorage_fs_transfer_stop(struct ustorage_fs_softc *sc);
227
228 static uint8_t ustorage_fs_verify(struct ustorage_fs_softc *sc);
229 static uint8_t ustorage_fs_inquiry(struct ustorage_fs_softc *sc);
230 static uint8_t ustorage_fs_request_sense(struct ustorage_fs_softc *sc);
231 static uint8_t ustorage_fs_read_capacity(struct ustorage_fs_softc *sc);
232 static uint8_t ustorage_fs_mode_sense(struct ustorage_fs_softc *sc);
233 static uint8_t ustorage_fs_start_stop(struct ustorage_fs_softc *sc);
234 static uint8_t ustorage_fs_prevent_allow(struct ustorage_fs_softc *sc);
235 static uint8_t ustorage_fs_read_format_capacities(struct ustorage_fs_softc *sc);
236 static uint8_t ustorage_fs_mode_select(struct ustorage_fs_softc *sc);
237 static uint8_t ustorage_fs_min_len(struct ustorage_fs_softc *sc, uint32_t len, uint32_t mask);
238 static uint8_t ustorage_fs_read(struct ustorage_fs_softc *sc);
239 static uint8_t ustorage_fs_write(struct ustorage_fs_softc *sc);
240 static uint8_t ustorage_fs_check_cmd(struct ustorage_fs_softc *sc, uint8_t cmd_size, uint16_t mask, uint8_t needs_medium);
241 static uint8_t ustorage_fs_do_cmd(struct ustorage_fs_softc *sc);
242
243 static device_method_t ustorage_fs_methods[] = {
244         /* USB interface */
245         DEVMETHOD(usb_handle_request, ustorage_fs_handle_request),
246
247         /* Device interface */
248         DEVMETHOD(device_probe, ustorage_fs_probe),
249         DEVMETHOD(device_attach, ustorage_fs_attach),
250         DEVMETHOD(device_detach, ustorage_fs_detach),
251         DEVMETHOD(device_suspend, ustorage_fs_suspend),
252         DEVMETHOD(device_resume, ustorage_fs_resume),
253
254         {0, 0}
255 };
256
257 static driver_t ustorage_fs_driver = {
258         .name = "ustorage_fs",
259         .methods = ustorage_fs_methods,
260         .size = sizeof(struct ustorage_fs_softc),
261 };
262
263 static devclass_t ustorage_fs_devclass;
264
265 DRIVER_MODULE(ustorage_fs, uhub, ustorage_fs_driver, ustorage_fs_devclass, NULL, NULL);
266 MODULE_VERSION(ustorage_fs, 0);
267 MODULE_DEPEND(ustorage_fs, usb, 1, 1, 1);
268
269 static struct usb_config ustorage_fs_bbb_config[USTORAGE_FS_T_BBB_MAX] = {
270
271         [USTORAGE_FS_T_BBB_COMMAND] = {
272                 .type = UE_BULK,
273                 .endpoint = UE_ADDR_ANY,
274                 .direction = UE_DIR_OUT,
275                 .bufsize = sizeof(ustorage_fs_bbb_cbw_t),
276                 .flags = {.ext_buffer = 1,},
277                 .callback = &ustorage_fs_t_bbb_command_callback,
278                 .usb_mode = USB_MODE_DEVICE,
279         },
280
281         [USTORAGE_FS_T_BBB_DATA_DUMP] = {
282                 .type = UE_BULK,
283                 .endpoint = UE_ADDR_ANY,
284                 .direction = UE_DIR_OUT,
285                 .bufsize = 0,   /* use wMaxPacketSize */
286                 .flags = {.proxy_buffer = 1,.short_xfer_ok = 1,},
287                 .callback = &ustorage_fs_t_bbb_data_dump_callback,
288                 .usb_mode = USB_MODE_DEVICE,
289         },
290
291         [USTORAGE_FS_T_BBB_DATA_READ] = {
292                 .type = UE_BULK,
293                 .endpoint = UE_ADDR_ANY,
294                 .direction = UE_DIR_OUT,
295                 .bufsize = USTORAGE_FS_BULK_SIZE,
296                 .flags = {.proxy_buffer = 1,.short_xfer_ok = 1,.ext_buffer = 1},
297                 .callback = &ustorage_fs_t_bbb_data_read_callback,
298                 .usb_mode = USB_MODE_DEVICE,
299         },
300
301         [USTORAGE_FS_T_BBB_DATA_WRITE] = {
302                 .type = UE_BULK,
303                 .endpoint = UE_ADDR_ANY,
304                 .direction = UE_DIR_IN,
305                 .bufsize = USTORAGE_FS_BULK_SIZE,
306                 .flags = {.proxy_buffer = 1,.short_xfer_ok = 1,.ext_buffer = 1},
307                 .callback = &ustorage_fs_t_bbb_data_write_callback,
308                 .usb_mode = USB_MODE_DEVICE,
309         },
310
311         [USTORAGE_FS_T_BBB_STATUS] = {
312                 .type = UE_BULK,
313                 .endpoint = UE_ADDR_ANY,
314                 .direction = UE_DIR_IN,
315                 .bufsize = sizeof(ustorage_fs_bbb_csw_t),
316                 .flags = {.short_xfer_ok = 1,.ext_buffer = 1,},
317                 .callback = &ustorage_fs_t_bbb_status_callback,
318                 .usb_mode = USB_MODE_DEVICE,
319         },
320 };
321
322 /*
323  * USB device probe/attach/detach
324  */
325
326 static int
327 ustorage_fs_probe(device_t dev)
328 {
329         struct usb_attach_arg *uaa = device_get_ivars(dev);
330         struct usb_interface_descriptor *id;
331
332         if (uaa->usb_mode != USB_MODE_DEVICE) {
333                 return (ENXIO);
334         }
335         /* Check for a standards compliant device */
336         id = usbd_get_interface_descriptor(uaa->iface);
337         if ((id == NULL) ||
338             (id->bInterfaceClass != UICLASS_MASS) ||
339             (id->bInterfaceSubClass != UISUBCLASS_SCSI) ||
340             (id->bInterfaceProtocol != UIPROTO_MASS_BBB)) {
341                 return (ENXIO);
342         }
343         return (BUS_PROBE_GENERIC);
344 }
345
346 static int
347 ustorage_fs_attach(device_t dev)
348 {
349         struct ustorage_fs_softc *sc = device_get_softc(dev);
350         struct usb_attach_arg *uaa = device_get_ivars(dev);
351         struct usb_interface_descriptor *id;
352         int err;
353         int unit;
354
355         /*
356          * NOTE: the softc struct is cleared in device_set_driver.
357          * We can safely call ustorage_fs_detach without specifically
358          * initializing the struct.
359          */
360
361         sc->sc_dev = dev;
362         sc->sc_udev = uaa->device;
363         unit = device_get_unit(dev);
364
365         /* enable power saving mode */
366         usbd_set_power_mode(uaa->device, USB_POWER_MODE_SAVE);
367
368         if (unit == 0) {
369                 if (ustorage_fs_ramdisk == NULL) {
370                         /*
371                          * allocate a memory image for our ramdisk until
372                          * further
373                          */
374                         ustorage_fs_ramdisk =
375                             kmalloc(USTORAGE_FS_RAM_SECT << 9, M_USB,
376                             M_ZERO | M_WAITOK);
377
378                         if (ustorage_fs_ramdisk == NULL) {
379                                 return (ENOMEM);
380                         }
381                 }
382                 sc->sc_lun[0].memory_image = ustorage_fs_ramdisk;
383                 sc->sc_lun[0].num_sectors = USTORAGE_FS_RAM_SECT;
384                 sc->sc_lun[0].removable = 1;
385         }
386
387         device_set_usb_desc(dev);
388
389         lockinit(&sc->sc_lock, "USTORAGE_FS lock", 0, LK_CANRECURSE);
390
391         /* get interface index */
392
393         id = usbd_get_interface_descriptor(uaa->iface);
394         if (id == NULL) {
395                 device_printf(dev, "failed to get "
396                     "interface number\n");
397                 goto detach;
398         }
399         sc->sc_iface_no = id->bInterfaceNumber;
400
401         err = usbd_transfer_setup(uaa->device,
402             &uaa->info.bIfaceIndex, sc->sc_xfer, ustorage_fs_bbb_config,
403             USTORAGE_FS_T_BBB_MAX, sc, &sc->sc_lock);
404         if (err) {
405                 device_printf(dev, "could not setup required "
406                     "transfers, %s\n", usbd_errstr(err));
407                 goto detach;
408         }
409         /* start Mass Storage State Machine */
410
411         lockmgr(&sc->sc_lock, LK_EXCLUSIVE);
412         ustorage_fs_transfer_start(sc, USTORAGE_FS_T_BBB_COMMAND);
413         lockmgr(&sc->sc_lock, LK_RELEASE);
414
415         return (0);                     /* success */
416
417 detach:
418         ustorage_fs_detach(dev);
419         return (ENXIO);                 /* failure */
420 }
421
422 static int
423 ustorage_fs_detach(device_t dev)
424 {
425         struct ustorage_fs_softc *sc = device_get_softc(dev);
426
427         /* teardown our statemachine */
428
429         usbd_transfer_unsetup(sc->sc_xfer, USTORAGE_FS_T_BBB_MAX);
430
431         lockuninit(&sc->sc_lock);
432
433         return (0);                     /* success */
434 }
435
436 static int
437 ustorage_fs_suspend(device_t dev)
438 {
439         device_printf(dev, "suspending\n");
440         return (0);                     /* success */
441 }
442
443 static int
444 ustorage_fs_resume(device_t dev)
445 {
446         device_printf(dev, "resuming\n");
447         return (0);                     /* success */
448 }
449
450 /*
451  * Generic functions to handle transfers
452  */
453
454 static void
455 ustorage_fs_transfer_start(struct ustorage_fs_softc *sc, uint8_t xfer_index)
456 {
457         if (sc->sc_xfer[xfer_index]) {
458                 sc->sc_last_xfer_index = xfer_index;
459                 usbd_transfer_start(sc->sc_xfer[xfer_index]);
460         }
461 }
462
463 static void
464 ustorage_fs_transfer_stop(struct ustorage_fs_softc *sc)
465 {
466         usbd_transfer_stop(sc->sc_xfer[sc->sc_last_xfer_index]);
467         lockmgr(&sc->sc_lock, LK_RELEASE);
468         usbd_transfer_drain(sc->sc_xfer[sc->sc_last_xfer_index]);
469         lockmgr(&sc->sc_lock, LK_EXCLUSIVE);
470 }
471
472 static int
473 ustorage_fs_handle_request(device_t dev,
474     const void *preq, void **pptr, uint16_t *plen,
475     uint16_t offset, uint8_t *pstate)
476 {
477         struct ustorage_fs_softc *sc = device_get_softc(dev);
478         const struct usb_device_request *req = preq;
479         uint8_t is_complete = *pstate;
480
481         if (!is_complete) {
482                 if ((req->bmRequestType == UT_WRITE_CLASS_INTERFACE) &&
483                     (req->bRequest == UR_BBB_RESET)) {
484                         *plen = 0;
485                         lockmgr(&sc->sc_lock, LK_EXCLUSIVE);
486                         ustorage_fs_transfer_stop(sc);
487                         sc->sc_transfer.data_error = 1;
488                         ustorage_fs_transfer_start(sc,
489                             USTORAGE_FS_T_BBB_COMMAND);
490                         lockmgr(&sc->sc_lock, LK_RELEASE);
491                         return (0);
492                 } else if ((req->bmRequestType == UT_READ_CLASS_INTERFACE) &&
493                            (req->bRequest == UR_BBB_GET_MAX_LUN)) {
494                         if (offset == 0) {
495                                 *plen = 1;
496                                 *pptr = &sc->sc_last_lun;
497                         } else {
498                                 *plen = 0;
499                         }
500                         return (0);
501                 }
502         }
503         return (ENXIO);                 /* use builtin handler */
504 }
505
506 static void
507 ustorage_fs_t_bbb_command_callback(struct usb_xfer *xfer, usb_error_t error)
508 {
509         struct ustorage_fs_softc *sc = usbd_xfer_softc(xfer);
510         uint32_t tag;
511         uint8_t err = 0;
512
513         DPRINTF("\n");
514
515         switch (USB_GET_STATE(xfer)) {
516         case USB_ST_TRANSFERRED:
517
518                 tag = UGETDW(sc->sc_cbw.dCBWSignature);
519
520                 if (tag != CBWSIGNATURE) {
521                         /* do nothing */
522                         DPRINTF("invalid signature 0x%08x\n", tag);
523                         break;
524                 }
525                 tag = UGETDW(sc->sc_cbw.dCBWTag);
526
527                 /* echo back tag */
528                 USETDW(sc->sc_csw.dCSWTag, tag);
529
530                 /* reset status */
531                 sc->sc_csw.bCSWStatus = 0;
532
533                 /* reset data offset, data length and data remainder */
534                 sc->sc_transfer.offset = 0;
535                 sc->sc_transfer.data_rem =
536                     UGETDW(sc->sc_cbw.dCBWDataTransferLength);
537
538                 /* reset data flags */
539                 sc->sc_transfer.data_short = 0;
540
541                 /* extract LUN */
542                 sc->sc_transfer.lun = sc->sc_cbw.bCBWLUN;
543
544                 if (sc->sc_transfer.data_rem == 0) {
545                         sc->sc_transfer.cbw_dir = DIR_NONE;
546                 } else {
547                         if (sc->sc_cbw.bCBWFlags & CBWFLAGS_IN) {
548                                 sc->sc_transfer.cbw_dir = DIR_WRITE;
549                         } else {
550                                 sc->sc_transfer.cbw_dir = DIR_READ;
551                         }
552                 }
553
554                 sc->sc_transfer.cmd_len = sc->sc_cbw.bCDBLength;
555                 if ((sc->sc_transfer.cmd_len > sizeof(sc->sc_cbw.CBWCDB)) ||
556                     (sc->sc_transfer.cmd_len == 0)) {
557                         /* just halt - this is invalid */
558                         DPRINTF("invalid command length %d bytes\n",
559                             sc->sc_transfer.cmd_len);
560                         break;
561                 }
562
563                 err = ustorage_fs_do_cmd(sc);
564                 if (err) {
565                         /* got an error */
566                         DPRINTF("command failed\n");
567                         break;
568                 }
569                 if ((sc->sc_transfer.data_rem > 0) &&
570                     (sc->sc_transfer.cbw_dir != sc->sc_transfer.cmd_dir)) {
571                         /* contradicting data transfer direction */
572                         err = 1;
573                         DPRINTF("data direction mismatch\n");
574                         break;
575                 }
576                 switch (sc->sc_transfer.cbw_dir) {
577                 case DIR_READ:
578                         ustorage_fs_transfer_start(sc, USTORAGE_FS_T_BBB_DATA_READ);
579                         break;
580                 case DIR_WRITE:
581                         ustorage_fs_transfer_start(sc, USTORAGE_FS_T_BBB_DATA_WRITE);
582                         break;
583                 default:
584                         ustorage_fs_transfer_start(sc,
585                             USTORAGE_FS_T_BBB_STATUS);
586                         break;
587                 }
588                 break;
589
590         case USB_ST_SETUP:
591 tr_setup:
592                 if (sc->sc_transfer.data_error) {
593                         sc->sc_transfer.data_error = 0;
594                         usbd_xfer_set_stall(xfer);
595                         DPRINTF("stall pipe\n");
596                 }
597
598                 usbd_xfer_set_frame_data(xfer, 0, &sc->sc_cbw,
599                     sizeof(sc->sc_cbw));
600                 usbd_transfer_submit(xfer);
601                 break;
602
603         default:                        /* Error */
604                 DPRINTF("error\n");
605                 if (error == USB_ERR_CANCELLED) {
606                         break;
607                 }
608                 /* If the pipe is already stalled, don't do another stall */
609                 if (!usbd_xfer_is_stalled(xfer))
610                         sc->sc_transfer.data_error = 1;
611
612                 /* try again */
613                 goto tr_setup;
614         }
615         if (err) {
616                 if (sc->sc_csw.bCSWStatus == 0) {
617                         /* set some default error code */
618                         sc->sc_csw.bCSWStatus = CSWSTATUS_FAILED;
619                 }
620                 if (sc->sc_transfer.cbw_dir == DIR_READ) {
621                         /* dump all data */
622                         ustorage_fs_transfer_start(sc,
623                             USTORAGE_FS_T_BBB_DATA_DUMP);
624                         return;
625                 }
626                 if (sc->sc_transfer.cbw_dir == DIR_WRITE) {
627                         /* need to stall before status */
628                         sc->sc_transfer.data_error = 1;
629                 }
630                 ustorage_fs_transfer_start(sc, USTORAGE_FS_T_BBB_STATUS);
631         }
632 }
633
634 static void
635 ustorage_fs_t_bbb_data_dump_callback(struct usb_xfer *xfer, usb_error_t error)
636 {
637         struct ustorage_fs_softc *sc = usbd_xfer_softc(xfer);
638         uint32_t max_bulk = usbd_xfer_max_len(xfer);
639         int actlen, sumlen;
640
641         usbd_xfer_status(xfer, &actlen, &sumlen, NULL, NULL);
642
643         DPRINTF("\n");
644
645         switch (USB_GET_STATE(xfer)) {
646         case USB_ST_TRANSFERRED:
647                 sc->sc_transfer.data_rem -= actlen;
648                 sc->sc_transfer.offset += actlen;
649
650                 if (actlen != sumlen || sc->sc_transfer.data_rem == 0) {
651                         /* short transfer or end of data */
652                         ustorage_fs_transfer_start(sc,
653                             USTORAGE_FS_T_BBB_STATUS);
654                         break;
655                 }
656                 /* Fallthrough */
657
658         case USB_ST_SETUP:
659 tr_setup:
660                 if (max_bulk > sc->sc_transfer.data_rem) {
661                         max_bulk = sc->sc_transfer.data_rem;
662                 }
663                 if (sc->sc_transfer.data_error) {
664                         sc->sc_transfer.data_error = 0;
665                         usbd_xfer_set_stall(xfer);
666                 }
667                 usbd_xfer_set_frame_len(xfer, 0, max_bulk);
668                 usbd_transfer_submit(xfer);
669                 break;
670
671         default:                        /* Error */
672                 if (error == USB_ERR_CANCELLED) {
673                         break;
674                 }
675                 /*
676                  * If the pipe is already stalled, don't do another stall:
677                  */
678                 if (!usbd_xfer_is_stalled(xfer))
679                         sc->sc_transfer.data_error = 1;
680
681                 /* try again */
682                 goto tr_setup;
683         }
684 }
685
686 static void
687 ustorage_fs_t_bbb_data_read_callback(struct usb_xfer *xfer, usb_error_t error)
688 {
689         struct ustorage_fs_softc *sc = usbd_xfer_softc(xfer);
690         uint32_t max_bulk = usbd_xfer_max_len(xfer);
691         int actlen, sumlen;
692
693         usbd_xfer_status(xfer, &actlen, &sumlen, NULL, NULL);
694
695         DPRINTF("\n");
696
697         switch (USB_GET_STATE(xfer)) {
698         case USB_ST_TRANSFERRED:
699                 sc->sc_transfer.data_rem -= actlen;
700                 sc->sc_transfer.data_ptr += actlen;
701                 sc->sc_transfer.offset += actlen;
702
703                 if (actlen != sumlen || sc->sc_transfer.data_rem == 0) {
704                         /* short transfer or end of data */
705                         ustorage_fs_transfer_start(sc,
706                             USTORAGE_FS_T_BBB_STATUS);
707                         break;
708                 }
709                 /* Fallthrough */
710
711         case USB_ST_SETUP:
712 tr_setup:
713                 if (max_bulk > sc->sc_transfer.data_rem) {
714                         max_bulk = sc->sc_transfer.data_rem;
715                 }
716                 if (sc->sc_transfer.data_error) {
717                         sc->sc_transfer.data_error = 0;
718                         usbd_xfer_set_stall(xfer);
719                 }
720
721                 usbd_xfer_set_frame_data(xfer, 0, sc->sc_transfer.data_ptr,
722                     max_bulk);
723                 usbd_transfer_submit(xfer);
724                 break;
725
726         default:                        /* Error */
727                 if (error == USB_ERR_CANCELLED) {
728                         break;
729                 }
730                 /* If the pipe is already stalled, don't do another stall */
731                 if (!usbd_xfer_is_stalled(xfer))
732                         sc->sc_transfer.data_error = 1;
733
734                 /* try again */
735                 goto tr_setup;
736         }
737 }
738
739 static void
740 ustorage_fs_t_bbb_data_write_callback(struct usb_xfer *xfer, usb_error_t error)
741 {
742         struct ustorage_fs_softc *sc = usbd_xfer_softc(xfer);
743         uint32_t max_bulk = usbd_xfer_max_len(xfer);
744         int actlen, sumlen;
745
746         usbd_xfer_status(xfer, &actlen, &sumlen, NULL, NULL);
747
748         DPRINTF("\n");
749
750         switch (USB_GET_STATE(xfer)) {
751         case USB_ST_TRANSFERRED:
752                 sc->sc_transfer.data_rem -= actlen;
753                 sc->sc_transfer.data_ptr += actlen;
754                 sc->sc_transfer.offset += actlen;
755
756                 if (actlen != sumlen || sc->sc_transfer.data_rem == 0) {
757                         /* short transfer or end of data */
758                         ustorage_fs_transfer_start(sc,
759                             USTORAGE_FS_T_BBB_STATUS);
760                         break;
761                 }
762         case USB_ST_SETUP:
763 tr_setup:
764                 if (max_bulk >= sc->sc_transfer.data_rem) {
765                         max_bulk = sc->sc_transfer.data_rem;
766                         if (sc->sc_transfer.data_short)
767                                 usbd_xfer_set_flag(xfer, USB_FORCE_SHORT_XFER);
768                         else
769                                 usbd_xfer_clr_flag(xfer, USB_FORCE_SHORT_XFER);
770                 } else
771                         usbd_xfer_clr_flag(xfer, USB_FORCE_SHORT_XFER);
772
773                 if (sc->sc_transfer.data_error) {
774                         sc->sc_transfer.data_error = 0;
775                         usbd_xfer_set_stall(xfer);
776                 }
777
778                 usbd_xfer_set_frame_data(xfer, 0, sc->sc_transfer.data_ptr,
779                     max_bulk);
780                 usbd_transfer_submit(xfer);
781                 break;
782
783         default:                        /* Error */
784                 if (error == USB_ERR_CANCELLED) {
785                         break;
786                 }
787                 /*
788                  * If the pipe is already stalled, don't do another
789                  * stall
790                  */
791                 if (!usbd_xfer_is_stalled(xfer))
792                         sc->sc_transfer.data_error = 1;
793
794                 /* try again */
795                 goto tr_setup;
796         }
797 }
798
799 static void
800 ustorage_fs_t_bbb_status_callback(struct usb_xfer *xfer, usb_error_t error)
801 {
802         struct ustorage_fs_softc *sc = usbd_xfer_softc(xfer);
803
804         DPRINTF("\n");
805
806         switch (USB_GET_STATE(xfer)) {
807         case USB_ST_TRANSFERRED:
808                 ustorage_fs_transfer_start(sc, USTORAGE_FS_T_BBB_COMMAND);
809                 break;
810
811         case USB_ST_SETUP:
812 tr_setup:
813                 USETDW(sc->sc_csw.dCSWSignature, CSWSIGNATURE);
814                 USETDW(sc->sc_csw.dCSWDataResidue, sc->sc_transfer.data_rem);
815
816                 if (sc->sc_transfer.data_error) {
817                         sc->sc_transfer.data_error = 0;
818                         usbd_xfer_set_stall(xfer);
819                 }
820
821                 usbd_xfer_set_frame_data(xfer, 0, &sc->sc_csw,
822                     sizeof(sc->sc_csw));
823                 usbd_transfer_submit(xfer);
824                 break;
825
826         default:
827                 if (error == USB_ERR_CANCELLED) {
828                         break;
829                 }
830                 /* If the pipe is already stalled, don't do another stall */
831                 if (!usbd_xfer_is_stalled(xfer))
832                         sc->sc_transfer.data_error = 1;
833
834                 /* try again */
835                 goto tr_setup;
836         }
837 }
838
839 /* SCSI commands that we recognize */
840 #define SC_FORMAT_UNIT                  0x04
841 #define SC_INQUIRY                      0x12
842 #define SC_MODE_SELECT_6                0x15
843 #define SC_MODE_SELECT_10               0x55
844 #define SC_MODE_SENSE_6                 0x1a
845 #define SC_MODE_SENSE_10                0x5a
846 #define SC_PREVENT_ALLOW_MEDIUM_REMOVAL 0x1e
847 #define SC_READ_6                       0x08
848 #define SC_READ_10                      0x28
849 #define SC_READ_12                      0xa8
850 #define SC_READ_CAPACITY                0x25
851 #define SC_READ_FORMAT_CAPACITIES       0x23
852 #define SC_RELEASE                      0x17
853 #define SC_REQUEST_SENSE                0x03
854 #define SC_RESERVE                      0x16
855 #define SC_SEND_DIAGNOSTIC              0x1d
856 #define SC_START_STOP_UNIT              0x1b
857 #define SC_SYNCHRONIZE_CACHE            0x35
858 #define SC_TEST_UNIT_READY              0x00
859 #define SC_VERIFY                       0x2f
860 #define SC_WRITE_6                      0x0a
861 #define SC_WRITE_10                     0x2a
862 #define SC_WRITE_12                     0xaa
863
864 /* SCSI Sense Key/Additional Sense Code/ASC Qualifier values */
865 #define SS_NO_SENSE                             0
866 #define SS_COMMUNICATION_FAILURE                0x040800
867 #define SS_INVALID_COMMAND                      0x052000
868 #define SS_INVALID_FIELD_IN_CDB                 0x052400
869 #define SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE   0x052100
870 #define SS_LOGICAL_UNIT_NOT_SUPPORTED           0x052500
871 #define SS_MEDIUM_NOT_PRESENT                   0x023a00
872 #define SS_MEDIUM_REMOVAL_PREVENTED             0x055302
873 #define SS_NOT_READY_TO_READY_TRANSITION        0x062800
874 #define SS_RESET_OCCURRED                       0x062900
875 #define SS_SAVING_PARAMETERS_NOT_SUPPORTED      0x053900
876 #define SS_UNRECOVERED_READ_ERROR               0x031100
877 #define SS_WRITE_ERROR                          0x030c02
878 #define SS_WRITE_PROTECTED                      0x072700
879
880 #define SK(x)           ((uint8_t) ((x) >> 16)) /* Sense Key byte, etc. */
881 #define ASC(x)          ((uint8_t) ((x) >> 8))
882 #define ASCQ(x)         ((uint8_t) (x))
883
884 /* Routines for unaligned data access */
885
886 static uint16_t
887 get_be16(uint8_t *buf)
888 {
889         return ((uint16_t)buf[0] << 8) | ((uint16_t)buf[1]);
890 }
891
892 static uint32_t
893 get_be32(uint8_t *buf)
894 {
895         return ((uint32_t)buf[0] << 24) | ((uint32_t)buf[1] << 16) |
896         ((uint32_t)buf[2] << 8) | ((uint32_t)buf[3]);
897 }
898
899 static void
900 put_be16(uint8_t *buf, uint16_t val)
901 {
902         buf[0] = val >> 8;
903         buf[1] = val;
904 }
905
906 static void
907 put_be32(uint8_t *buf, uint32_t val)
908 {
909         buf[0] = val >> 24;
910         buf[1] = val >> 16;
911         buf[2] = val >> 8;
912         buf[3] = val & 0xff;
913 }
914
915 /*------------------------------------------------------------------------*
916  *      ustorage_fs_verify
917  *
918  * Returns:
919  *    0: Success
920  * Else: Failure
921  *------------------------------------------------------------------------*/
922 static uint8_t
923 ustorage_fs_verify(struct ustorage_fs_softc *sc)
924 {
925         struct ustorage_fs_lun *currlun = sc->sc_transfer.currlun;
926         uint32_t lba;
927         uint32_t vlen;
928         uint64_t file_offset;
929         uint64_t amount_left;
930
931         /*
932          * Get the starting Logical Block Address
933          */
934         lba = get_be32(&sc->sc_cmd_data[2]);
935
936         /*
937          * We allow DPO (Disable Page Out = don't save data in the cache)
938          * but we don't implement it.
939          */
940         if ((sc->sc_cmd_data[1] & ~0x10) != 0) {
941                 currlun->sense_data = SS_INVALID_FIELD_IN_CDB;
942                 return (1);
943         }
944         vlen = get_be16(&sc->sc_cmd_data[7]);
945         if (vlen == 0) {
946                 goto done;
947         }
948         /* No default reply */
949
950         /* Prepare to carry out the file verify */
951         amount_left = vlen;
952         amount_left <<= 9;
953         file_offset = lba;
954         file_offset <<= 9;
955
956         /* Range check */
957         vlen += lba;
958
959         if ((vlen < lba) ||
960             (vlen > currlun->num_sectors) ||
961             (lba >= currlun->num_sectors)) {
962                 currlun->sense_data = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
963                 return (1);
964         }
965         /* XXX TODO: verify that data is readable */
966 done:
967         return (ustorage_fs_min_len(sc, 0, 0 - 1));
968 }
969
970 /*------------------------------------------------------------------------*
971  *      ustorage_fs_inquiry
972  *
973  * Returns:
974  *    0: Success
975  * Else: Failure
976  *------------------------------------------------------------------------*/
977 static uint8_t
978 ustorage_fs_inquiry(struct ustorage_fs_softc *sc)
979 {
980         uint8_t *buf = sc->sc_transfer.data_ptr;
981
982         struct ustorage_fs_lun *currlun = sc->sc_transfer.currlun;
983
984         if (!sc->sc_transfer.currlun) {
985                 /* Unsupported LUNs are okay */
986                 memset(buf, 0, 36);
987                 buf[0] = 0x7f;
988                 /* Unsupported, no device - type */
989                 return (ustorage_fs_min_len(sc, 36, 0 - 1));
990         }
991         memset(buf, 0, 8);
992         /* Non - removable, direct - access device */
993         if (currlun->removable)
994                 buf[1] = 0x80;
995         buf[2] = 2;
996         /* ANSI SCSI level 2 */
997         buf[3] = 2;
998         /* SCSI - 2 INQUIRY data format */
999         buf[4] = 31;
1000         /* Additional length */
1001         /* No special options */
1002         /* Copy in ID string */
1003         memcpy(buf + 8, USTORAGE_FS_ID_STRING, 28);
1004
1005 #if (USTORAGE_QDATA_MAX < 36)
1006 #error "(USTORAGE_QDATA_MAX < 36)"
1007 #endif
1008         return (ustorage_fs_min_len(sc, 36, 0 - 1));
1009 }
1010
1011 /*------------------------------------------------------------------------*
1012  *      ustorage_fs_request_sense
1013  *
1014  * Returns:
1015  *    0: Success
1016  * Else: Failure
1017  *------------------------------------------------------------------------*/
1018 static uint8_t
1019 ustorage_fs_request_sense(struct ustorage_fs_softc *sc)
1020 {
1021         uint8_t *buf = sc->sc_transfer.data_ptr;
1022         struct ustorage_fs_lun *currlun = sc->sc_transfer.currlun;
1023         uint32_t sd;
1024         uint32_t sdinfo;
1025         uint8_t valid;
1026
1027         /*
1028          * From the SCSI-2 spec., section 7.9 (Unit attention condition):
1029          *
1030          * If a REQUEST SENSE command is received from an initiator
1031          * with a pending unit attention condition (before the target
1032          * generates the contingent allegiance condition), then the
1033          * target shall either:
1034          *   a) report any pending sense data and preserve the unit
1035          *      attention condition on the logical unit, or,
1036          *   b) report the unit attention condition, may discard any
1037          *      pending sense data, and clear the unit attention
1038          *      condition on the logical unit for that initiator.
1039          *
1040          * FSG normally uses option a); enable this code to use option b).
1041          */
1042 #if 0
1043         if (currlun && currlun->unit_attention_data != SS_NO_SENSE) {
1044                 currlun->sense_data = currlun->unit_attention_data;
1045                 currlun->unit_attention_data = SS_NO_SENSE;
1046         }
1047 #endif
1048
1049         if (!currlun) {
1050                 /* Unsupported LUNs are okay */
1051                 sd = SS_LOGICAL_UNIT_NOT_SUPPORTED;
1052                 sdinfo = 0;
1053                 valid = 0;
1054         } else {
1055                 sd = currlun->sense_data;
1056                 sdinfo = currlun->sense_data_info;
1057                 valid = currlun->info_valid << 7;
1058                 currlun->sense_data = SS_NO_SENSE;
1059                 currlun->sense_data_info = 0;
1060                 currlun->info_valid = 0;
1061         }
1062
1063         memset(buf, 0, 18);
1064         buf[0] = valid | 0x70;
1065         /* Valid, current error */
1066         buf[2] = SK(sd);
1067         put_be32(&buf[3], sdinfo);
1068         /* Sense information */
1069         buf[7] = 18 - 8;
1070         /* Additional sense length */
1071         buf[12] = ASC(sd);
1072         buf[13] = ASCQ(sd);
1073
1074 #if (USTORAGE_QDATA_MAX < 18)
1075 #error "(USTORAGE_QDATA_MAX < 18)"
1076 #endif
1077         return (ustorage_fs_min_len(sc, 18, 0 - 1));
1078 }
1079
1080 /*------------------------------------------------------------------------*
1081  *      ustorage_fs_read_capacity
1082  *
1083  * Returns:
1084  *    0: Success
1085  * Else: Failure
1086  *------------------------------------------------------------------------*/
1087 static uint8_t
1088 ustorage_fs_read_capacity(struct ustorage_fs_softc *sc)
1089 {
1090         uint8_t *buf = sc->sc_transfer.data_ptr;
1091         struct ustorage_fs_lun *currlun = sc->sc_transfer.currlun;
1092         uint32_t lba = get_be32(&sc->sc_cmd_data[2]);
1093         uint8_t pmi = sc->sc_cmd_data[8];
1094
1095         /* Check the PMI and LBA fields */
1096         if ((pmi > 1) || ((pmi == 0) && (lba != 0))) {
1097                 currlun->sense_data = SS_INVALID_FIELD_IN_CDB;
1098                 return (1);
1099         }
1100         /* Max logical block */
1101         put_be32(&buf[0], currlun->num_sectors - 1);
1102         /* Block length */
1103         put_be32(&buf[4], 512);
1104
1105 #if (USTORAGE_QDATA_MAX < 8)
1106 #error "(USTORAGE_QDATA_MAX < 8)"
1107 #endif
1108         return (ustorage_fs_min_len(sc, 8, 0 - 1));
1109 }
1110
1111 /*------------------------------------------------------------------------*
1112  *      ustorage_fs_mode_sense
1113  *
1114  * Returns:
1115  *    0: Success
1116  * Else: Failure
1117  *------------------------------------------------------------------------*/
1118 static uint8_t
1119 ustorage_fs_mode_sense(struct ustorage_fs_softc *sc)
1120 {
1121         uint8_t *buf = sc->sc_transfer.data_ptr;
1122         struct ustorage_fs_lun *currlun = sc->sc_transfer.currlun;
1123         uint8_t *buf0;
1124         uint16_t len;
1125         uint16_t limit;
1126         uint8_t mscmnd = sc->sc_cmd_data[0];
1127         uint8_t pc;
1128         uint8_t page_code;
1129         uint8_t changeable_values;
1130         uint8_t all_pages;
1131
1132         buf0 = buf;
1133
1134         if ((sc->sc_cmd_data[1] & ~0x08) != 0) {
1135                 /* Mask away DBD */
1136                 currlun->sense_data = SS_INVALID_FIELD_IN_CDB;
1137                 return (1);
1138         }
1139         pc = sc->sc_cmd_data[2] >> 6;
1140         page_code = sc->sc_cmd_data[2] & 0x3f;
1141         if (pc == 3) {
1142                 currlun->sense_data = SS_SAVING_PARAMETERS_NOT_SUPPORTED;
1143                 return (1);
1144         }
1145         changeable_values = (pc == 1);
1146         all_pages = (page_code == 0x3f);
1147
1148         /*
1149          * Write the mode parameter header.  Fixed values are: default
1150          * medium type, no cache control (DPOFUA), and no block descriptors.
1151          * The only variable value is the WriteProtect bit.  We will fill in
1152          * the mode data length later.
1153          */
1154         memset(buf, 0, 8);
1155         if (mscmnd == SC_MODE_SENSE_6) {
1156                 buf[2] = (currlun->read_only ? 0x80 : 0x00);
1157                 /* WP, DPOFUA */
1158                 buf += 4;
1159                 limit = 255;
1160         } else {
1161                 /* SC_MODE_SENSE_10 */
1162                 buf[3] = (currlun->read_only ? 0x80 : 0x00);
1163                 /* WP, DPOFUA */
1164                 buf += 8;
1165                 limit = 65535;
1166                 /* Should really be mod_data.buflen */
1167         }
1168
1169         /* No block descriptors */
1170
1171         /*
1172          * The mode pages, in numerical order.
1173          */
1174         if ((page_code == 0x08) || all_pages) {
1175                 buf[0] = 0x08;
1176                 /* Page code */
1177                 buf[1] = 10;
1178                 /* Page length */
1179                 memset(buf + 2, 0, 10);
1180                 /* None of the fields are changeable */
1181
1182                 if (!changeable_values) {
1183                         buf[2] = 0x04;
1184                         /* Write cache enable, */
1185                         /* Read cache not disabled */
1186                         /* No cache retention priorities */
1187                         put_be16(&buf[4], 0xffff);
1188                         /* Don 't disable prefetch */
1189                         /* Minimum prefetch = 0 */
1190                         put_be16(&buf[8], 0xffff);
1191                         /* Maximum prefetch */
1192                         put_be16(&buf[10], 0xffff);
1193                         /* Maximum prefetch ceiling */
1194                 }
1195                 buf += 12;
1196         }
1197         /*
1198          * Check that a valid page was requested and the mode data length
1199          * isn't too long.
1200          */
1201         len = buf - buf0;
1202         if (len > limit) {
1203                 currlun->sense_data = SS_INVALID_FIELD_IN_CDB;
1204                 return (1);
1205         }
1206         /* Store the mode data length */
1207         if (mscmnd == SC_MODE_SENSE_6)
1208                 buf0[0] = len - 1;
1209         else
1210                 put_be16(buf0, len - 2);
1211
1212 #if (USTORAGE_QDATA_MAX < 24)
1213 #error "(USTORAGE_QDATA_MAX < 24)"
1214 #endif
1215         return (ustorage_fs_min_len(sc, len, 0 - 1));
1216 }
1217
1218 /*------------------------------------------------------------------------*
1219  *      ustorage_fs_start_stop
1220  *
1221  * Returns:
1222  *    0: Success
1223  * Else: Failure
1224  *------------------------------------------------------------------------*/
1225 static uint8_t
1226 ustorage_fs_start_stop(struct ustorage_fs_softc *sc)
1227 {
1228         struct ustorage_fs_lun *currlun = sc->sc_transfer.currlun;
1229         uint8_t loej;
1230         uint8_t start;
1231         uint8_t immed;
1232
1233         if (!currlun->removable) {
1234                 currlun->sense_data = SS_INVALID_COMMAND;
1235                 return (1);
1236         }
1237         immed = sc->sc_cmd_data[1] & 0x01;
1238         loej = sc->sc_cmd_data[4] & 0x02;
1239         start = sc->sc_cmd_data[4] & 0x01;
1240
1241         if (immed || loej || start) {
1242                 /* compile fix */
1243         }
1244         return (0);
1245 }
1246
1247 /*------------------------------------------------------------------------*
1248  *      ustorage_fs_prevent_allow
1249  *
1250  * Returns:
1251  *    0: Success
1252  * Else: Failure
1253  *------------------------------------------------------------------------*/
1254 static uint8_t
1255 ustorage_fs_prevent_allow(struct ustorage_fs_softc *sc)
1256 {
1257         struct ustorage_fs_lun *currlun = sc->sc_transfer.currlun;
1258         uint8_t prevent;
1259
1260         if (!currlun->removable) {
1261                 currlun->sense_data = SS_INVALID_COMMAND;
1262                 return (1);
1263         }
1264         prevent = sc->sc_cmd_data[4] & 0x01;
1265         if ((sc->sc_cmd_data[4] & ~0x01) != 0) {
1266                 /* Mask away Prevent */
1267                 currlun->sense_data = SS_INVALID_FIELD_IN_CDB;
1268                 return (1);
1269         }
1270         if (currlun->prevent_medium_removal && !prevent) {
1271                 //fsync_sub(currlun);
1272         }
1273         currlun->prevent_medium_removal = prevent;
1274         return (0);
1275 }
1276
1277 /*------------------------------------------------------------------------*
1278  *      ustorage_fs_read_format_capacities
1279  *
1280  * Returns:
1281  *    0: Success
1282  * Else: Failure
1283  *------------------------------------------------------------------------*/
1284 static uint8_t
1285 ustorage_fs_read_format_capacities(struct ustorage_fs_softc *sc)
1286 {
1287         uint8_t *buf = sc->sc_transfer.data_ptr;
1288         struct ustorage_fs_lun *currlun = sc->sc_transfer.currlun;
1289
1290         buf[0] = buf[1] = buf[2] = 0;
1291         buf[3] = 8;
1292         /* Only the Current / Maximum Capacity Descriptor */
1293         buf += 4;
1294
1295         /* Number of blocks */
1296         put_be32(&buf[0], currlun->num_sectors);
1297         /* Block length */
1298         put_be32(&buf[4], 512);
1299         /* Current capacity */
1300         buf[4] = 0x02;
1301
1302 #if (USTORAGE_QDATA_MAX < 12)
1303 #error "(USTORAGE_QDATA_MAX < 12)"
1304 #endif
1305         return (ustorage_fs_min_len(sc, 12, 0 - 1));
1306 }
1307
1308 /*------------------------------------------------------------------------*
1309  *      ustorage_fs_mode_select
1310  *
1311  * Return values:
1312  *    0: Success
1313  * Else: Failure
1314  *------------------------------------------------------------------------*/
1315 static uint8_t
1316 ustorage_fs_mode_select(struct ustorage_fs_softc *sc)
1317 {
1318         struct ustorage_fs_lun *currlun = sc->sc_transfer.currlun;
1319
1320         /* We don't support MODE SELECT */
1321         currlun->sense_data = SS_INVALID_COMMAND;
1322         return (1);
1323 }
1324
1325 /*------------------------------------------------------------------------*
1326  *      ustorage_fs_synchronize_cache
1327  *
1328  * Return values:
1329  *    0: Success
1330  * Else: Failure
1331  *------------------------------------------------------------------------*/
1332 static uint8_t
1333 ustorage_fs_synchronize_cache(struct ustorage_fs_softc *sc)
1334 {
1335 #if 0
1336         struct ustorage_fs_lun *currlun = sc->sc_transfer.currlun;
1337         uint8_t rc;
1338
1339         /*
1340          * We ignore the requested LBA and write out all dirty data buffers.
1341          */
1342         rc = 0;
1343         if (rc) {
1344                 currlun->sense_data = SS_WRITE_ERROR;
1345         }
1346 #endif
1347         return (0);
1348 }
1349
1350 /*------------------------------------------------------------------------*
1351  *      ustorage_fs_read - read data from disk
1352  *
1353  * Return values:
1354  *    0: Success
1355  * Else: Failure
1356  *------------------------------------------------------------------------*/
1357 static uint8_t
1358 ustorage_fs_read(struct ustorage_fs_softc *sc)
1359 {
1360         struct ustorage_fs_lun *currlun = sc->sc_transfer.currlun;
1361         uint64_t file_offset;
1362         uint32_t lba;
1363         uint32_t len;
1364
1365         /*
1366          * Get the starting Logical Block Address and check that it's not
1367          * too big
1368          */
1369         if (sc->sc_cmd_data[0] == SC_READ_6) {
1370                 lba = (((uint32_t)sc->sc_cmd_data[1]) << 16) |
1371                     get_be16(&sc->sc_cmd_data[2]);
1372         } else {
1373                 lba = get_be32(&sc->sc_cmd_data[2]);
1374
1375                 /*
1376                  * We allow DPO (Disable Page Out = don't save data in the
1377                  * cache) and FUA (Force Unit Access = don't read from the
1378                  * cache), but we don't implement them.
1379                  */
1380                 if ((sc->sc_cmd_data[1] & ~0x18) != 0) {
1381                         currlun->sense_data = SS_INVALID_FIELD_IN_CDB;
1382                         return (1);
1383                 }
1384         }
1385         len = sc->sc_transfer.data_rem >> 9;
1386         len += lba;
1387
1388         if ((len < lba) ||
1389             (len > currlun->num_sectors) ||
1390             (lba >= currlun->num_sectors)) {
1391                 currlun->sense_data = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
1392                 return (1);
1393         }
1394         file_offset = lba;
1395         file_offset <<= 9;
1396
1397         sc->sc_transfer.data_ptr = currlun->memory_image + file_offset;
1398
1399         return (0);
1400 }
1401
1402 /*------------------------------------------------------------------------*
1403  *      ustorage_fs_write - write data to disk
1404  *
1405  * Return values:
1406  *    0: Success
1407  * Else: Failure
1408  *------------------------------------------------------------------------*/
1409 static uint8_t
1410 ustorage_fs_write(struct ustorage_fs_softc *sc)
1411 {
1412         struct ustorage_fs_lun *currlun = sc->sc_transfer.currlun;
1413         uint64_t file_offset;
1414         uint32_t lba;
1415         uint32_t len;
1416
1417         if (currlun->read_only) {
1418                 currlun->sense_data = SS_WRITE_PROTECTED;
1419                 return (1);
1420         }
1421         /* XXX clear SYNC */
1422
1423         /*
1424          * Get the starting Logical Block Address and check that it's not
1425          * too big.
1426          */
1427         if (sc->sc_cmd_data[0] == SC_WRITE_6)
1428                 lba = (((uint32_t)sc->sc_cmd_data[1]) << 16) |
1429                     get_be16(&sc->sc_cmd_data[2]);
1430         else {
1431                 lba = get_be32(&sc->sc_cmd_data[2]);
1432
1433                 /*
1434                  * We allow DPO (Disable Page Out = don't save data in the
1435                  * cache) and FUA (Force Unit Access = write directly to the
1436                  * medium).  We don't implement DPO; we implement FUA by
1437                  * performing synchronous output.
1438                  */
1439                 if ((sc->sc_cmd_data[1] & ~0x18) != 0) {
1440                         currlun->sense_data = SS_INVALID_FIELD_IN_CDB;
1441                         return (1);
1442                 }
1443                 if (sc->sc_cmd_data[1] & 0x08) {
1444                         /* FUA */
1445                         /* XXX set SYNC flag here */
1446                 }
1447         }
1448
1449         len = sc->sc_transfer.data_rem >> 9;
1450         len += lba;
1451
1452         if ((len < lba) ||
1453             (len > currlun->num_sectors) ||
1454             (lba >= currlun->num_sectors)) {
1455                 currlun->sense_data = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;
1456                 return (1);
1457         }
1458         file_offset = lba;
1459         file_offset <<= 9;
1460
1461         sc->sc_transfer.data_ptr = currlun->memory_image + file_offset;
1462
1463         return (0);
1464 }
1465
1466 /*------------------------------------------------------------------------*
1467  *      ustorage_fs_min_len
1468  *
1469  * Return values:
1470  *    0: Success
1471  * Else: Failure
1472  *------------------------------------------------------------------------*/
1473 static uint8_t
1474 ustorage_fs_min_len(struct ustorage_fs_softc *sc, uint32_t len, uint32_t mask)
1475 {
1476         if (len != sc->sc_transfer.data_rem) {
1477
1478                 if (sc->sc_transfer.cbw_dir == DIR_READ) {
1479                         /*
1480                          * there must be something wrong about this SCSI
1481                          * command
1482                          */
1483                         sc->sc_csw.bCSWStatus = CSWSTATUS_PHASE;
1484                         return (1);
1485                 }
1486                 /* compute the minimum length */
1487
1488                 if (sc->sc_transfer.data_rem > len) {
1489                         /* data ends prematurely */
1490                         sc->sc_transfer.data_rem = len;
1491                         sc->sc_transfer.data_short = 1;
1492                 }
1493                 /* check length alignment */
1494
1495                 if (sc->sc_transfer.data_rem & ~mask) {
1496                         /* data ends prematurely */
1497                         sc->sc_transfer.data_rem &= mask;
1498                         sc->sc_transfer.data_short = 1;
1499                 }
1500         }
1501         return (0);
1502 }
1503
1504 /*------------------------------------------------------------------------*
1505  *      ustorage_fs_check_cmd - check command routine
1506  *
1507  * Check whether the command is properly formed and whether its data
1508  * size and direction agree with the values we already have.
1509  *
1510  * Return values:
1511  *    0: Success
1512  * Else: Failure
1513  *------------------------------------------------------------------------*/
1514 static uint8_t
1515 ustorage_fs_check_cmd(struct ustorage_fs_softc *sc, uint8_t min_cmd_size,
1516     uint16_t mask, uint8_t needs_medium)
1517 {
1518         struct ustorage_fs_lun *currlun;
1519         uint8_t lun = (sc->sc_cmd_data[1] >> 5);
1520         uint8_t i;
1521
1522         /* Verify the length of the command itself */
1523         if (min_cmd_size > sc->sc_transfer.cmd_len) {
1524                 DPRINTF("%u > %u\n",
1525                     min_cmd_size, sc->sc_transfer.cmd_len);
1526                 sc->sc_csw.bCSWStatus = CSWSTATUS_PHASE;
1527                 return (1);
1528         }
1529         /* Mask away the LUN */
1530         sc->sc_cmd_data[1] &= 0x1f;
1531
1532         /* Check if LUN is correct */
1533         if (lun != sc->sc_transfer.lun) {
1534
1535         }
1536         /* Check the LUN */
1537         if (sc->sc_transfer.lun <= sc->sc_last_lun) {
1538                 sc->sc_transfer.currlun = currlun =
1539                     sc->sc_lun + sc->sc_transfer.lun;
1540                 if (sc->sc_cmd_data[0] != SC_REQUEST_SENSE) {
1541                         currlun->sense_data = SS_NO_SENSE;
1542                         currlun->sense_data_info = 0;
1543                         currlun->info_valid = 0;
1544                 }
1545                 /*
1546                  * If a unit attention condition exists, only INQUIRY
1547                  * and REQUEST SENSE commands are allowed. Anything
1548                  * else must fail!
1549                  */
1550                 if ((currlun->unit_attention_data != SS_NO_SENSE) &&
1551                     (sc->sc_cmd_data[0] != SC_INQUIRY) &&
1552                     (sc->sc_cmd_data[0] != SC_REQUEST_SENSE)) {
1553                         currlun->sense_data = currlun->unit_attention_data;
1554                         currlun->unit_attention_data = SS_NO_SENSE;
1555                         return (1);
1556                 }
1557         } else {
1558                 sc->sc_transfer.currlun = currlun = NULL;
1559
1560                 /*
1561                  * INQUIRY and REQUEST SENSE commands are explicitly allowed
1562                  * to use unsupported LUNs; all others may not.
1563                  */
1564                 if ((sc->sc_cmd_data[0] != SC_INQUIRY) &&
1565                     (sc->sc_cmd_data[0] != SC_REQUEST_SENSE)) {
1566                         return (1);
1567                 }
1568         }
1569
1570         /*
1571          * Check that only command bytes listed in the mask are
1572          * non-zero.
1573          */
1574         for (i = 0; i != min_cmd_size; i++) {
1575                 if (sc->sc_cmd_data[i] && !(mask & (1UL << i))) {
1576                         if (currlun) {
1577                                 currlun->sense_data = SS_INVALID_FIELD_IN_CDB;
1578                         }
1579                         return (1);
1580                 }
1581         }
1582
1583         /*
1584          * If the medium isn't mounted and the command needs to access
1585          * it, return an error.
1586          */
1587         if (currlun && (!currlun->memory_image) && needs_medium) {
1588                 currlun->sense_data = SS_MEDIUM_NOT_PRESENT;
1589                 return (1);
1590         }
1591         return (0);
1592 }
1593
1594 /*------------------------------------------------------------------------*
1595  *      ustorage_fs_do_cmd - do command
1596  *
1597  * Return values:
1598  *    0: Success
1599  * Else: Failure
1600  *------------------------------------------------------------------------*/
1601 static uint8_t
1602 ustorage_fs_do_cmd(struct ustorage_fs_softc *sc)
1603 {
1604         uint8_t error = 1;
1605         uint8_t i;
1606         uint32_t temp;
1607         const uint32_t mask9 = (0xFFFFFFFFUL >> 9) << 9;
1608
1609         /* set default data transfer pointer */
1610         sc->sc_transfer.data_ptr = sc->sc_qdata;
1611
1612         DPRINTF("cmd_data[0]=0x%02x, data_rem=0x%08x\n",
1613             sc->sc_cmd_data[0], sc->sc_transfer.data_rem);
1614
1615         switch (sc->sc_cmd_data[0]) {
1616         case SC_INQUIRY:
1617                 sc->sc_transfer.cmd_dir = DIR_WRITE;
1618                 error = ustorage_fs_min_len(sc, sc->sc_cmd_data[4], 0 - 1);
1619                 if (error) {
1620                         break;
1621                 }
1622                 error = ustorage_fs_check_cmd(sc, 6,
1623                     (1UL << 4) | 1, 0);
1624                 if (error) {
1625                         break;
1626                 }
1627                 error = ustorage_fs_inquiry(sc);
1628
1629                 break;
1630
1631         case SC_MODE_SELECT_6:
1632                 sc->sc_transfer.cmd_dir = DIR_READ;
1633                 error = ustorage_fs_min_len(sc, sc->sc_cmd_data[4], 0 - 1);
1634                 if (error) {
1635                         break;
1636                 }
1637                 error = ustorage_fs_check_cmd(sc, 6,
1638                     (1UL << 1) | (1UL << 4) | 1, 0);
1639                 if (error) {
1640                         break;
1641                 }
1642                 error = ustorage_fs_mode_select(sc);
1643
1644                 break;
1645
1646         case SC_MODE_SELECT_10:
1647                 sc->sc_transfer.cmd_dir = DIR_READ;
1648                 error = ustorage_fs_min_len(sc,
1649                     get_be16(&sc->sc_cmd_data[7]), 0 - 1);
1650                 if (error) {
1651                         break;
1652                 }
1653                 error = ustorage_fs_check_cmd(sc, 10,
1654                     (1UL << 1) | (3UL << 7) | 1, 0);
1655                 if (error) {
1656                         break;
1657                 }
1658                 error = ustorage_fs_mode_select(sc);
1659
1660                 break;
1661
1662         case SC_MODE_SENSE_6:
1663                 sc->sc_transfer.cmd_dir = DIR_WRITE;
1664                 error = ustorage_fs_min_len(sc, sc->sc_cmd_data[4], 0 - 1);
1665                 if (error) {
1666                         break;
1667                 }
1668                 error = ustorage_fs_check_cmd(sc, 6,
1669                     (1UL << 1) | (1UL << 2) | (1UL << 4) | 1, 0);
1670                 if (error) {
1671                         break;
1672                 }
1673                 error = ustorage_fs_mode_sense(sc);
1674
1675                 break;
1676
1677         case SC_MODE_SENSE_10:
1678                 sc->sc_transfer.cmd_dir = DIR_WRITE;
1679                 error = ustorage_fs_min_len(sc,
1680                     get_be16(&sc->sc_cmd_data[7]), 0 - 1);
1681                 if (error) {
1682                         break;
1683                 }
1684                 error = ustorage_fs_check_cmd(sc, 10,
1685                     (1UL << 1) | (1UL << 2) | (3UL << 7) | 1, 0);
1686                 if (error) {
1687                         break;
1688                 }
1689                 error = ustorage_fs_mode_sense(sc);
1690
1691                 break;
1692
1693         case SC_PREVENT_ALLOW_MEDIUM_REMOVAL:
1694                 error = ustorage_fs_min_len(sc, 0, 0 - 1);
1695                 if (error) {
1696                         break;
1697                 }
1698                 error = ustorage_fs_check_cmd(sc, 6,
1699                     (1UL << 4) | 1, 0);
1700                 if (error) {
1701                         break;
1702                 }
1703                 error = ustorage_fs_prevent_allow(sc);
1704
1705                 break;
1706
1707         case SC_READ_6:
1708                 i = sc->sc_cmd_data[4];
1709                 sc->sc_transfer.cmd_dir = DIR_WRITE;
1710                 temp = ((i == 0) ? 256UL : i);
1711                 error = ustorage_fs_min_len(sc, temp << 9, mask9);
1712                 if (error) {
1713                         break;
1714                 }
1715                 error = ustorage_fs_check_cmd(sc, 6,
1716                     (7UL << 1) | (1UL << 4) | 1, 1);
1717                 if (error) {
1718                         break;
1719                 }
1720                 error = ustorage_fs_read(sc);
1721
1722                 break;
1723
1724         case SC_READ_10:
1725                 sc->sc_transfer.cmd_dir = DIR_WRITE;
1726                 temp = get_be16(&sc->sc_cmd_data[7]);
1727                 error = ustorage_fs_min_len(sc, temp << 9, mask9);
1728                 if (error) {
1729                         break;
1730                 }
1731                 error = ustorage_fs_check_cmd(sc, 10,
1732                     (1UL << 1) | (0xfUL << 2) | (3UL << 7) | 1, 1);
1733                 if (error) {
1734                         break;
1735                 }
1736                 error = ustorage_fs_read(sc);
1737
1738                 break;
1739
1740         case SC_READ_12:
1741                 sc->sc_transfer.cmd_dir = DIR_WRITE;
1742                 temp = get_be32(&sc->sc_cmd_data[6]);
1743                 if (temp >= (1UL << (32 - 9))) {
1744                         /* numerical overflow */
1745                         sc->sc_csw.bCSWStatus = CSWSTATUS_FAILED;
1746                         error = 1;
1747                         break;
1748                 }
1749                 error = ustorage_fs_min_len(sc, temp << 9, mask9);
1750                 if (error) {
1751                         break;
1752                 }
1753                 error = ustorage_fs_check_cmd(sc, 12,
1754                     (1UL << 1) | (0xfUL << 2) | (0xfUL << 6) | 1, 1);
1755                 if (error) {
1756                         break;
1757                 }
1758                 error = ustorage_fs_read(sc);
1759
1760                 break;
1761
1762         case SC_READ_CAPACITY:
1763                 sc->sc_transfer.cmd_dir = DIR_WRITE;
1764                 error = ustorage_fs_check_cmd(sc, 10,
1765                     (0xfUL << 2) | (1UL << 8) | 1, 1);
1766                 if (error) {
1767                         break;
1768                 }
1769                 error = ustorage_fs_read_capacity(sc);
1770
1771                 break;
1772
1773         case SC_READ_FORMAT_CAPACITIES:
1774                 sc->sc_transfer.cmd_dir = DIR_WRITE;
1775                 error = ustorage_fs_min_len(sc,
1776                     get_be16(&sc->sc_cmd_data[7]), 0 - 1);
1777                 if (error) {
1778                         break;
1779                 }
1780                 error = ustorage_fs_check_cmd(sc, 10,
1781                     (3UL << 7) | 1, 1);
1782                 if (error) {
1783                         break;
1784                 }
1785                 error = ustorage_fs_read_format_capacities(sc);
1786
1787                 break;
1788
1789         case SC_REQUEST_SENSE:
1790                 sc->sc_transfer.cmd_dir = DIR_WRITE;
1791                 error = ustorage_fs_min_len(sc, sc->sc_cmd_data[4], 0 - 1);
1792                 if (error) {
1793                         break;
1794                 }
1795                 error = ustorage_fs_check_cmd(sc, 6,
1796                     (1UL << 4) | 1, 0);
1797                 if (error) {
1798                         break;
1799                 }
1800                 error = ustorage_fs_request_sense(sc);
1801
1802                 break;
1803
1804         case SC_START_STOP_UNIT:
1805                 error = ustorage_fs_min_len(sc, 0, 0 - 1);
1806                 if (error) {
1807                         break;
1808                 }
1809                 error = ustorage_fs_check_cmd(sc, 6,
1810                     (1UL << 1) | (1UL << 4) | 1, 0);
1811                 if (error) {
1812                         break;
1813                 }
1814                 error = ustorage_fs_start_stop(sc);
1815
1816                 break;
1817
1818         case SC_SYNCHRONIZE_CACHE:
1819                 error = ustorage_fs_min_len(sc, 0, 0 - 1);
1820                 if (error) {
1821                         break;
1822                 }
1823                 error = ustorage_fs_check_cmd(sc, 10,
1824                     (0xfUL << 2) | (3UL << 7) | 1, 1);
1825                 if (error) {
1826                         break;
1827                 }
1828                 error = ustorage_fs_synchronize_cache(sc);
1829
1830                 break;
1831
1832         case SC_TEST_UNIT_READY:
1833                 error = ustorage_fs_min_len(sc, 0, 0 - 1);
1834                 if (error) {
1835                         break;
1836                 }
1837                 error = ustorage_fs_check_cmd(sc, 6,
1838                     0 | 1, 1);
1839                 break;
1840
1841                 /*
1842                  * Although optional, this command is used by MS-Windows.
1843                  * We support a minimal version: BytChk must be 0.
1844                  */
1845         case SC_VERIFY:
1846                 error = ustorage_fs_min_len(sc, 0, 0 - 1);
1847                 if (error) {
1848                         break;
1849                 }
1850                 error = ustorage_fs_check_cmd(sc, 10,
1851                     (1UL << 1) | (0xfUL << 2) | (3UL << 7) | 1, 1);
1852                 if (error) {
1853                         break;
1854                 }
1855                 error = ustorage_fs_verify(sc);
1856
1857                 break;
1858
1859         case SC_WRITE_6:
1860                 i = sc->sc_cmd_data[4];
1861                 sc->sc_transfer.cmd_dir = DIR_READ;
1862                 temp = ((i == 0) ? 256UL : i);
1863                 error = ustorage_fs_min_len(sc, temp << 9, mask9);
1864                 if (error) {
1865                         break;
1866                 }
1867                 error = ustorage_fs_check_cmd(sc, 6,
1868                     (7UL << 1) | (1UL << 4) | 1, 1);
1869                 if (error) {
1870                         break;
1871                 }
1872                 error = ustorage_fs_write(sc);
1873
1874                 break;
1875
1876         case SC_WRITE_10:
1877                 sc->sc_transfer.cmd_dir = DIR_READ;
1878                 temp = get_be16(&sc->sc_cmd_data[7]);
1879                 error = ustorage_fs_min_len(sc, temp << 9, mask9);
1880                 if (error) {
1881                         break;
1882                 }
1883                 error = ustorage_fs_check_cmd(sc, 10,
1884                     (1UL << 1) | (0xfUL << 2) | (3UL << 7) | 1, 1);
1885                 if (error) {
1886                         break;
1887                 }
1888                 error = ustorage_fs_write(sc);
1889
1890                 break;
1891
1892         case SC_WRITE_12:
1893                 sc->sc_transfer.cmd_dir = DIR_READ;
1894                 temp = get_be32(&sc->sc_cmd_data[6]);
1895                 if (temp > (mask9 >> 9)) {
1896                         /* numerical overflow */
1897                         sc->sc_csw.bCSWStatus = CSWSTATUS_FAILED;
1898                         error = 1;
1899                         break;
1900                 }
1901                 error = ustorage_fs_min_len(sc, temp << 9, mask9);
1902                 if (error) {
1903                         break;
1904                 }
1905                 error = ustorage_fs_check_cmd(sc, 12,
1906                     (1UL << 1) | (0xfUL << 2) | (0xfUL << 6) | 1, 1);
1907                 if (error) {
1908                         break;
1909                 }
1910                 error = ustorage_fs_write(sc);
1911
1912                 break;
1913
1914                 /*
1915                  * Some mandatory commands that we recognize but don't
1916                  * implement.  They don't mean much in this setting.
1917                  * It's left as an exercise for anyone interested to
1918                  * implement RESERVE and RELEASE in terms of Posix
1919                  * locks.
1920                  */
1921         case SC_FORMAT_UNIT:
1922         case SC_RELEASE:
1923         case SC_RESERVE:
1924         case SC_SEND_DIAGNOSTIC:
1925                 /* Fallthrough */
1926
1927         default:
1928                 error = ustorage_fs_min_len(sc, 0, 0 - 1);
1929                 if (error) {
1930                         break;
1931                 }
1932                 error = ustorage_fs_check_cmd(sc, sc->sc_transfer.cmd_len,
1933                     0xff, 0);
1934                 if (error) {
1935                         break;
1936                 }
1937                 sc->sc_transfer.currlun->sense_data =
1938                     SS_INVALID_COMMAND;
1939                 error = 1;
1940
1941                 break;
1942         }
1943         return (error);
1944 }