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