drivers/usb/function/msc_bulk_function.c

Go to the documentation of this file.

#include <assert.h>
#include <atomic.h>
#include <byteorder.h>
#include <dmapool.h>
#include <interrupt.h>
#include <physmem.h>
#include <status-codes.h>
#include <string.h>
#include <types.h>
#include <util.h>
#include <block/device.h>
#include <scsi/cdb.h>
#include <scsi/spc_protocol.h>
#include <scsi/sbc_protocol.h>
#include <usb/function_core.h>
#include <usb/msc_function.h>
#include <usb/msc_protocol.h>
#include <usb/request.h>
#include <usb/usb_protocol.h>
#include <usb/udc.h>

#include <app/config_usb.h>

#ifndef CONFIG_MSC_FS_BULK_EP_SIZE
# define CONFIG_MSC_FS_BULK_EP_SIZE     64
#endif

#define MSC_DATA_BUFFER_SIZE    CONFIG_DMAPOOL_LARGE_OBJ_SIZE
#define MSC_MAX_NR_BUFFERS      (CONFIG_DMAPOOL_NR_LARGE_OBJS / 8)
#define MSC_MAX_DATA_LEN        (MSC_MAX_NR_BUFFERS * MSC_DATA_BUFFER_SIZE)

#ifdef CONFIG_USE_AES
/* Due to a weakness in the AES block device layer, reduced buffer depth
 * is needed in order to ensure stable operation. */
# define MSC_MAX_NR_SEGS        (1)
#else
# define MSC_MAX_NR_SEGS        (2)
#endif

/* The serial number may be at most 28 characters */
#define MSC_VPD_SERIAL_BUF_SIZE (MSC_MAX_SERIAL_LEN + SCSI_VPD_HEADER_SIZE)

/*
 * Size of the Device Identification VPD:
 *  - 4 bytes for the header
 *  - 4 bytes for the ID descriptor header
 *  - 8 bytes for the T10 vendor ID
 *  - 16 bytes for the product ID
 *  - MSC_MAX_SERIAL_LEN bytes for the serial number
 */
#define MSC_VPD_ID_BUF_SIZE     (SCSI_VPD_HEADER_SIZE                   \
                                        + SCSI_VPD_ID_HEADER_SIZE       \
                                        + 8 + 16                        \
                                        + MSC_MAX_SERIAL_LEN)

struct msc_bulk_iface_block {
        struct usb_interface_descriptor iface;
        struct usb_endpoint_descriptor  in_ep;
        struct usb_endpoint_descriptor  out_ep;
};

struct msc_func {
        struct usb_func_iface   iface;
        struct udc              *udc;
        struct block_device     *bdev;
        usb_ep_id_t             bulk_in_ep;
        usb_ep_id_t             bulk_out_ep;
        uint8_t                 *sense_data;
        phys_addr_t             sense_data_phys;
        uint32_t                blocks_queued;
        uint32_t                blocks_total;
        uint32_t                first_lba;
        atomic_t                blk_segs_pending;
        atomic_t                usb_reqs_pending;
        struct usb_msc_cbw      *cbw;
        struct usb_msc_csw      *csw;
        struct usb_request      *cbw_csw_req;
        struct block_request    *block_req;
        uint16_t                busy_asc;
        void                    (*busy_cb)(void *);
        void                    *busy_cb_data;
        bool                    queue_locked;
        bool                    not_ready;
        bool                    xfer_in_progress;
};

static inline struct msc_func *msc_func_of(struct usb_func_iface *iface)
{
        return container_of(iface, struct msc_func, iface);
}

union msc_mode_param_header {
        struct scsi_mode_param_header6  h6;
        struct scsi_mode_param_header10 h10;
};
#define MSC_MODE_PARAM_HDR_BUF_LEN                      \
        (sizeof(union msc_mode_param_header)            \
                + sizeof(struct sbc_slba_block_desc))

static const struct msc_bulk_iface_block msc_bulk_fs_iface = {
        .iface = {
                .bLength = sizeof(struct usb_interface_descriptor),
                .bDescriptorType        = USB_DT_INTERFACE,
                .bInterfaceNumber       = CONFIG_MSC_BULK_INTERFACE_ID,
                .bAlternateSetting      = 0,
                .bNumEndpoints          = 2,
                .bInterfaceClass        = USB_INTERFACE_CLASS_MSC,
                /* Use INQUIRY to determine command set */
                .bInterfaceSubClass     = USB_MSC_SUBCLASS_TRANSPARENT,
                /* BBB is the only protocol recommended for new designs */
                .bInterfaceProtocol     = USB_MSC_PROTOCOL_BULK,
        },
        .in_ep = {
                .bLength = sizeof(struct usb_endpoint_descriptor),
                .bDescriptorType        = USB_DT_ENDPOINT,
                .bEndpointAddress       = USB_DIR_IN | CONFIG_MSC_BULK_IN_EP,
                .bmAttributes           = USB_EP_XFER_BULK,
                .wMaxPacketSize         = LE16(CONFIG_MSC_FS_BULK_EP_SIZE),
        },
        .out_ep = {
                .bLength = sizeof(struct usb_endpoint_descriptor),
                .bDescriptorType        = USB_DT_ENDPOINT,
                .bEndpointAddress       = USB_DIR_OUT | CONFIG_MSC_BULK_OUT_EP,
                .bmAttributes           = USB_EP_XFER_BULK,
                .wMaxPacketSize         = LE16(CONFIG_MSC_FS_BULK_EP_SIZE),
        },
};

#ifdef CONFIG_UDC_HIGH_SPEED
static const struct msc_bulk_iface_block msc_bulk_hs_iface = {
        .iface = {
                .bLength = sizeof(struct usb_interface_descriptor),
                .bDescriptorType        = USB_DT_INTERFACE,
                .bInterfaceNumber       = CONFIG_MSC_BULK_INTERFACE_ID,
                .bAlternateSetting      = 0,
                .bNumEndpoints          = 2,
                .bInterfaceClass        = USB_INTERFACE_CLASS_MSC,
                /* Use INQUIRY to determine command set */
                .bInterfaceSubClass     = USB_MSC_SUBCLASS_TRANSPARENT,
                /* BBB is the only protocol recommended for new designs */
                .bInterfaceProtocol     = USB_MSC_PROTOCOL_BULK,
        },
        .in_ep = {
                .bLength = sizeof(struct usb_endpoint_descriptor),
                .bDescriptorType        = USB_DT_ENDPOINT,
                .bEndpointAddress       = USB_DIR_IN | CONFIG_MSC_BULK_IN_EP,
                .bmAttributes           = USB_EP_XFER_BULK,
                .wMaxPacketSize         = LE16(512),
        },
        .out_ep = {
                .bLength = sizeof(struct usb_endpoint_descriptor),
                .bDescriptorType        = USB_DT_ENDPOINT,
                .bEndpointAddress       = USB_DIR_OUT | CONFIG_MSC_BULK_OUT_EP,
                .bmAttributes           = USB_EP_XFER_BULK,
                .wMaxPacketSize         = LE16(512),
        },
};
#endif

static const struct scsi_inquiry_data msc_inquiry_data = {
        .pq_pdt         = SCSI_INQ_PQ_CONNECTED | SCSI_INQ_DT_DIR_ACCESS,
        .flags1         = 0,
        .version        = SCSI_INQ_VER_SPC3,
        .flags3         = SCSI_INQ_HISUP | SCSI_INQ_RSP_SPC2,
        .addl_len       = SCSI_INQ_ADDL_LEN(sizeof(msc_inquiry_data)),
        .vendor_id      = { CONFIG_MSC_INQ_VENDOR_ID },
        .product_id     = { CONFIG_MSC_INQ_PRODUCT_ID },
        .product_rev    = { CONFIG_MSC_INQ_PRODUCT_VERSION },
};

static struct usb_msc_cbw *msc_alloc_cbw(struct msc_func *msc,
                phys_addr_t *phys)
{
        return dma_alloc(phys, sizeof(struct usb_msc_cbw));
}

static void msc_free_cbw(struct msc_func *msc, struct usb_msc_cbw *cbw)
{
        dma_free(cbw, sizeof(struct usb_msc_cbw));
}

static struct usb_msc_csw *msc_alloc_csw(struct msc_func *msc,
                phys_addr_t *phys)
{
        struct usb_msc_csw      *csw;

        csw = dma_alloc(phys, sizeof(struct usb_msc_csw));
        if (csw)
                csw->dCSWSignature = LE32(USB_CSW_SIGNATURE);

        return csw;
}

static void msc_free_csw(struct msc_func *msc, struct usb_msc_csw *csw)
{
        dma_free(csw, sizeof(struct usb_msc_csw));
}

static void msc_free_dma_buf_list(struct slist *buf_list)
{
        struct buffer   *buf;

        while (!slist_is_empty(buf_list)) {
                buf = slist_pop_head(buf_list, struct buffer, node);
                buffer_dma_free(buf, MSC_DATA_BUFFER_SIZE);
        }
}

static void msc_queue_empty(struct msc_func *msc)
{
        unsigned long   iflags;

        iflags = cpu_irq_save();

        dbg_verbose("msc: queue empty, not_ready=%d busy_cb=%p\n",
                        msc->not_ready, msc->busy_cb);

        msc->xfer_in_progress = false;
        if (msc->not_ready && msc->busy_cb) {
                void (*busy_cb)(void *) = msc->busy_cb;
                void *busy_data = msc->busy_cb_data;

                msc->busy_cb = NULL;
                cpu_irq_restore(iflags);
                busy_cb(busy_data);
        } else {
                cpu_irq_restore(iflags);
        }
}

static void msc_init_sense(struct msc_func *msc, uint8_t sense_key,
                uint16_t add_sense, uint32_t lba)
{
        uint8_t         *data = msc->sense_data;

        memset(data, 0, 18);
        data[0] = SCSI_SENSE_VALID | SCSI_SENSE_CURRENT;
        data[2] = sense_key;
        data[3] = lba >> 24;
        data[4] = lba >> 16;
        data[5] = lba >> 8;
        data[6] = lba;
        data[7] = SCSI_SENSE_ADDL_LEN(18);
        data[12] = add_sense >> 8;
        data[13] = add_sense;
}

static unsigned int msc_sense_len(struct msc_func *msc)
{
        return msc->sense_data[7] + 8;
}

static void msc_cbw_received(struct udc *udc, struct usb_request *req);
static void msc_csw_sent(struct udc *udc, struct usb_request *req);

static void msc_prepare_csw(struct msc_func *msc, uint32_t residue,
                uint8_t status)
{
        struct usb_msc_csw      *csw;
        struct usb_msc_cbw      *cbw;
        struct usb_request      *req;
        struct buffer           *buf;

        csw = msc->csw;
        cbw = msc->cbw;
        csw->dCSWTag = cbw->dCBWTag;
        csw->dCSWDataResidue = cpu_to_le32(residue);
        csw->bCSWStatus = status;

        req = msc->cbw_csw_req;
        req->bytes_xfered = 0;
        req->req_done = msc_csw_sent;
        buf = usb_req_get_first_buffer(req);
        buffer_init_tx(buf, csw, sizeof(struct usb_msc_csw));

        dbg_verbose("msc: CSW t%08x r%u s%u\n", le32_to_cpu(csw->dCSWTag),
                        residue, status);
}

static void msc_request_data_done(struct udc *udc, struct msc_func *msc)
{
        if (msc->csw->dCSWDataResidue != LE32(0) && msc->bulk_in_ep) {
                if (msc->cbw->bmCBWFlags & USB_CBW_DIRECTION_IN)
                        udc_ep_set_halt(udc, msc->bulk_in_ep);
                else
                        udc_ep_set_halt(udc, msc->bulk_out_ep);
                udc_ep_submit_in_req(udc, msc->bulk_in_ep, msc->cbw_csw_req);
        }

        dbg_verbose("msc data done: t%08x r%u s%u %s\n",
                        le32_to_cpu(msc->csw->dCSWTag),
                        le32_to_cpu(msc->csw->dCSWDataResidue),
                        msc->csw->bCSWStatus,
                        (msc->cbw->bmCBWFlags & USB_CBW_DIRECTION_IN)
                        ? "IN" : "OUT");
}

static void msc_request_done(struct udc *udc, struct msc_func *msc,
                uint32_t residue)
{
        /*
         * If we're transferring exactly the amount of data that the
         * host requested, queue the CSW immediately. Otherwise, we
         * need to stall after we've sent all the data we have, and
         * send the CSW after that.
         */
        if (!residue)
                udc_ep_submit_in_req(udc, msc->bulk_in_ep, msc->cbw_csw_req);

        dbg_verbose("msc req done\n");
}

static void msc_request_done_nodata(struct udc *udc, struct msc_func *msc,
                uint32_t residue)
{
        if (residue) {
                if (msc->cbw->bmCBWFlags & USB_CBW_DIRECTION_IN)
                        udc_ep_set_halt(udc, msc->bulk_in_ep);
                else
                        udc_ep_set_halt(udc, msc->bulk_out_ep);
        }

        udc_ep_submit_in_req(udc, msc->bulk_in_ep, msc->cbw_csw_req);
}

static void msc_request_failed(struct msc_func *msc, uint32_t residue,
                uint8_t csw_status, uint8_t sense_key, uint16_t add_sense)
{
        dbg_warning("msc: req %02x failed: SK %02x ASC(Q) %04x\n",
                        scsi_cdb_get_opcode(msc->cbw->CDB),
                        sense_key, add_sense);

        msc_init_sense(msc, sense_key, add_sense, 0);
        msc_prepare_csw(msc, residue, csw_status);
        msc_request_done_nodata(msc->udc, msc, residue);
}

static void msc_phase_error(struct msc_func *msc)
{
        struct usb_msc_cbw      *cbw = msc->cbw;
        uint32_t                cbw_xfer_len;

        cbw_xfer_len = le32_to_cpu(cbw->dCBWDataTransferLength);

        dbg_error("msc: Phase Error (opcode %02x)\n",
                        scsi_cdb_get_opcode(cbw->CDB));
        dbg_verbose("msc:   CBW bmCBWFlags = 0x%02x\n", cbw->bmCBWFlags);
        dbg_verbose("msc:   CBW dCBWDataTransferLength = 0x%x\n", cbw_xfer_len);

        msc_prepare_csw(msc, cbw_xfer_len, USB_CSW_STATUS_PE);
        msc_request_done_nodata(msc->udc, msc, cbw_xfer_len);
}

static void msc_out_of_memory(struct msc_func *msc)
{
        dbg_error("msc: Out of memory!\n");
        msc_phase_error(msc);
}

static void msc_csw_sent(struct udc *udc, struct usb_request *req)
{
        struct msc_func         *msc = req->context;
        struct buffer           *buf;

        buf = usb_req_get_first_buffer(req);
        assert(buf->addr.ptr == msc->csw);

        dbg_verbose("msc: CSW sent, status=%d\n", req->status);

        msc_queue_empty(msc);

        /* Don't submit any buffers when the endpoint is unavailable */
        if (req->status)
                return;

        /* Re-use the USB request and buffer for the CBW */
        buffer_init_rx(buf, msc->cbw, sizeof(*msc->cbw));
        req->bytes_xfered = 0;
        req->req_done = msc_cbw_received;

        udc_ep_submit_out_req(udc, msc->bulk_out_ep, req);
}

static void msc_data_sent(struct udc *udc, struct usb_request *req)
{
        struct msc_func         *msc = req->context;
        int                     status = req->status;

        usb_req_free(req);

        if (!status)
                msc_request_data_done(udc, msc);
}

static void msc_buf_sent(struct udc *udc, struct usb_request *req)
{
        struct buffer           *buf;

        buf = usb_req_get_first_buffer(req);
        buffer_free(buf);
        msc_data_sent(udc, req);
}

static void msc_capacity_sent(struct udc *udc, struct usb_request *req)
{
        struct buffer   *buf;

        buf = usb_req_get_first_buffer(req);
        buffer_dma_free(buf, sizeof(struct sbc_read_capacity10_data));
        msc_data_sent(udc, req);
}

static void msc_vpd_serial_sent(struct udc *udc, struct usb_request *req)
{
        struct buffer   *buf;

        buf = usb_req_get_first_buffer(req);
        buffer_dma_free(buf, MSC_VPD_SERIAL_BUF_SIZE);
        msc_data_sent(udc, req);
}

static void msc_vpd_id_sent(struct udc *udc, struct usb_request *req)
{
        struct buffer   *buf;

        buf = usb_req_get_first_buffer(req);
        buffer_dma_free(buf, MSC_VPD_ID_BUF_SIZE);
        msc_data_sent(udc, req);
}

static void msc_sense_data_sent(struct udc *udc, struct usb_request *req)
{
        struct msc_func *msc = req->context;
        struct buffer   *buf;
        int             status = req->status;

        dbg_verbose("msc sense data sent: %zu bytes, status %d\n",
                        req->bytes_xfered, req->status);

        msc_init_sense(msc, SCSI_SK_NO_SENSE,
                        SCSI_ASC_NO_ADDITIONAL_SENSE_INFO, 0);
        buf = usb_req_get_first_buffer(req);
        buffer_free(buf);
        usb_req_free(req);

        if (!status)
                msc_request_data_done(udc, msc);
}

static void msc_mode_params_sent(struct udc *udc, struct usb_request *req)
{
        struct buffer   *buf;

        buf = usb_req_get_first_buffer(req);
        buffer_dma_free(buf, MSC_MODE_PARAM_HDR_BUF_LEN);
        msc_data_sent(udc, req);
}

static long msc_validate_req(struct msc_func *msc, struct usb_msc_cbw *cbw,
                uint32_t alloc_len, uint8_t dir_flag)
{
        uint32_t        cbw_len;

        cbw_len = le32_to_cpu(cbw->dCBWDataTransferLength);

        /*
         * The following cases should result in a phase error:
         *  - Case  2: Hn < Di
         *  - Case  3: Hn < Do
         *  - Case  7: Hi < Di
         *  - Case  8: Hi <> Do
         *  - Case 10: Ho <> Di
         *  - Case 13: Ho < Do
         */
        if ((cbw->bmCBWFlags ^ dir_flag) & USB_CBW_DIRECTION_IN
                        || cbw_len < alloc_len) {
                msc_phase_error(msc);
                return -1;
        }

        /*
         * The following cases should result in a stall and nonzero
         * residue:
         *  - Case  4: Hi > Dn
         *  - Case  5: Hi > Di
         *  - Case  9: Ho > Dn
         *  - Case 11: Ho > Do
         */
        return cbw_len - alloc_len;
}

static void msc_test_unit_ready(struct msc_func *msc, struct udc *udc,
                uint32_t cbw_data_len)
{
        dbg_verbose("msc TEST UNIT READY len %u\n", cbw_data_len);

        if (msc->not_ready) {
                msc_request_failed(msc, cbw_data_len, USB_CSW_STATUS_FAIL,
                                SCSI_SK_NOT_READY,
                                msc->busy_asc);
        } else if (test_bit(BDEV_PRESENT, &msc->bdev->flags)) {
                msc_prepare_csw(msc, cbw_data_len, USB_CSW_STATUS_PASS);
                msc_request_done_nodata(udc, msc, cbw_data_len);
        } else {
                msc_request_failed(msc, cbw_data_len, USB_CSW_STATUS_FAIL,
                                SCSI_SK_NOT_READY,
                                SCSI_ASC_MEDIUM_NOT_PRESENT);
        }
}

static void msc_request_sense(struct msc_func *msc, struct udc *udc,
                struct usb_msc_cbw *cbw)
{
        struct usb_request      *req;
        struct buffer           *buf;
        const uint8_t           *cdb = cbw->CDB;
        long                    residue;
        uint8_t                 alloc_len;

        alloc_len = scsi_cdb_get_alloc_len(cdb, SCSI_CMD_REQUEST_SENSE);

        dbg_verbose("msc: REQUEST SENSE len %u (sense len %u)\n",
                        alloc_len, msc_sense_len(msc));

        residue = msc_validate_req(msc, cbw, alloc_len, USB_CBW_DIRECTION_IN);
        if (residue < 0)
                return;

        if (alloc_len > 0) {
                /*
                 * Just send the sense buffer. It should be
                 * initialized by the last command that failed; after
                 * it has been successfully sent, it gets reset to
                 * NO_SENSE.
                 */
                req = usb_req_alloc();
                if (!req)
                        goto err_req_alloc;
                buf = buffer_alloc();
                if (!buf)
                        goto err_buf_alloc;

                buffer_init_tx(buf, msc->sense_data,
                                min(alloc_len, msc_sense_len(msc)));
                usb_req_add_buffer(req, buf);
                req->context = msc;
                req->req_done = msc_sense_data_sent;

                residue += alloc_len - buf->len;
                msc_prepare_csw(msc, residue, USB_CSW_STATUS_PASS);
                udc_ep_submit_in_req(udc, msc->bulk_in_ep, req);

                msc_request_done(udc, msc, residue);
        } else {
                /*
                 * REQUEST SENSE always resets the sense data, even if
                 * no data was actually transfered.
                 */
                msc_init_sense(msc, SCSI_SK_NO_SENSE,
                                SCSI_ASC_NO_ADDITIONAL_SENSE_INFO, 0);
                msc_prepare_csw(msc, residue, USB_CSW_STATUS_PASS);
                msc_request_done_nodata(udc, msc, residue);
        }

        return;

err_buf_alloc:
        usb_req_free(req);
err_req_alloc:
        msc_out_of_memory(msc);
}

static int msc_vpd_supported_pages(struct msc_func *msc,
                struct usb_request *req, size_t alloc_len)
{
        static const uint8_t vpd_page[] = {
                SCSI_INQ_PQ_CONNECTED | SCSI_INQ_DT_DIR_ACCESS,
                SCSI_VPD_SUPPORTED_PAGES,
                0x00,
                3,
                SCSI_VPD_SUPPORTED_PAGES,
                SCSI_VPD_UNIT_SERIAL_NUMBER,
                SCSI_VPD_DEVICE_IDENTIFICATION,
        };
        struct buffer   *buf;
        size_t          total_len = 0;

        if (alloc_len) {
                buf = buffer_alloc();
                if (!buf)
                        return -1;

                total_len = min(alloc_len, sizeof(vpd_page));
                buffer_init_tx(buf, &vpd_page, total_len);
                usb_req_add_buffer(req, buf);
                req->req_done = msc_buf_sent;
        }

        return total_len;
}

static int msc_vpd_serial_number(struct msc_func *msc,
                struct usb_request *req, size_t alloc_len)
{
        struct buffer   *buf;
        uint8_t         *vpd_page;
        const char      *serial;
        size_t          serial_len;
        size_t          total_len = 0;

        if (alloc_len) {
                serial = get_serial_number();
                serial_len = strlen(serial);

                /*
                 * The application must verify that the serial number
                 * is valid by calling msc_serial_number_is_valid() at
                 * startup. An invalid serial number is considered a
                 * serious configuration error.
                 */
                assert(serial_len <= MSC_MAX_SERIAL_LEN);

                buf = buffer_dma_alloc(MSC_VPD_SERIAL_BUF_SIZE);
                if (!buf)
                        return -1;

                vpd_page = buf->addr.ptr;
                vpd_page[0] = SCSI_INQ_PQ_CONNECTED | SCSI_INQ_DT_DIR_ACCESS;
                vpd_page[1] = SCSI_VPD_UNIT_SERIAL_NUMBER;
                vpd_page[2] = 0;
                vpd_page[3] = MSC_MAX_SERIAL_LEN;
                memset(vpd_page + SCSI_VPD_HEADER_SIZE,
                                ' ', MSC_MAX_SERIAL_LEN - serial_len);
                memcpy(vpd_page + MSC_VPD_SERIAL_BUF_SIZE - serial_len,
                                serial, serial_len);

                total_len = MSC_VPD_SERIAL_BUF_SIZE;
                if (alloc_len < total_len) {
                        total_len = alloc_len;
                        buffer_resize(buf, total_len);
                }

                usb_req_add_buffer(req, buf);
                req->req_done = msc_vpd_serial_sent;
        }

        return total_len;
}

static int msc_vpd_device_id(struct msc_func *msc,
                struct usb_request *req, size_t alloc_len)
{
        struct buffer   *buf;
        const char      *serial;
        size_t          serial_len;
        uint8_t         *vpd_page;
        uint8_t         *p;
        size_t          total_len = 0;

        if (alloc_len) {
                serial = get_serial_number();
                serial_len = strlen(serial);

                /*
                 * The application must verify that the serial number
                 * is valid by calling msc_serial_number_is_valid() at
                 * startup. An invalid serial number is considered a
                 * serious configuration error.
                 */
                assert(serial_len <= MSC_MAX_SERIAL_LEN);

                buf = buffer_dma_alloc(MSC_VPD_ID_BUF_SIZE);
                if (!buf)
                        return -1;

                vpd_page = buf->addr.ptr;
                vpd_page[0] = SCSI_INQ_PQ_CONNECTED | SCSI_INQ_DT_DIR_ACCESS;
                vpd_page[1] = SCSI_VPD_DEVICE_IDENTIFICATION;
                vpd_page[2] = 0;
                vpd_page[3] = MSC_VPD_ID_BUF_SIZE - SCSI_VPD_HEADER_SIZE;
                vpd_page[4] = SCSI_VPD_CODE_SET_ASCII;
                vpd_page[5] = SCSI_VPD_ID_TYPE_T10;
                vpd_page[6] = 0;
                vpd_page[7] = MSC_VPD_ID_BUF_SIZE - SCSI_VPD_HEADER_SIZE
                                - SCSI_VPD_ID_HEADER_SIZE;

                /* Copy Vendor ID and Product ID in one go */
                p = vpd_page + SCSI_VPD_HEADER_SIZE + SCSI_VPD_ID_HEADER_SIZE;
                memcpy(p, msc_inquiry_data.vendor_id, 8 + 16);

                /* And finally, the right-aligned serial number */
                p += 8 + 16;
                memset(p, ' ', MSC_MAX_SERIAL_LEN - serial_len);
                p += MSC_MAX_SERIAL_LEN - serial_len;
                memcpy(p, serial, serial_len);

                total_len = MSC_VPD_ID_BUF_SIZE;
                if (alloc_len < total_len) {
                        total_len = alloc_len;
                        buffer_resize(buf, total_len);
                }

                usb_req_add_buffer(req, buf);
                req->req_done = msc_vpd_id_sent;
        }

        return total_len;
}

static void msc_inquiry(struct msc_func *msc, struct udc *udc,
                struct usb_msc_cbw *cbw)
{
        struct usb_request      *req;
        struct buffer           *buf;
        long                    residue;
        long                    ret;
        uint16_t                alloc_len;
        const uint8_t           *cdb = cbw->CDB;
        size_t                  total_len;
        uint8_t                 page_code;

        alloc_len = scsi_cdb_get_u16(cdb, 3);

        dbg_verbose("msc: INQUIRY %u (inq len %zu)\n",
                        alloc_len, sizeof(msc_inquiry_data));

        residue = msc_validate_req(msc, cbw, alloc_len, USB_CBW_DIRECTION_IN);
        if (residue < 0)
                return;

        req = usb_req_alloc();
        if (!req)
                goto err_req_alloc;
        req->context = msc;

        total_len = 0;
        page_code = cdb[2];

        if (cdb[1] & SCSI_INQ_REQ_EVPD) {
                /* Vital Product Data page requested */
                switch (page_code) {
                case SCSI_VPD_SUPPORTED_PAGES:
                        ret = msc_vpd_supported_pages(msc, req, alloc_len);
                        break;
                case SCSI_VPD_UNIT_SERIAL_NUMBER:
                        ret = msc_vpd_serial_number(msc, req, alloc_len);
                        break;
                case SCSI_VPD_DEVICE_IDENTIFICATION:
                        ret = msc_vpd_device_id(msc, req, alloc_len);
                        break;
                default:
                        dbg_info("msc: unsupported VPD page %02x requested\n",
                                        page_code);
                        goto illegal_request;
                }

                if (ret < 0)
                        goto err_buf_alloc;

                total_len = ret;
        } else if (page_code != 0) {
                /* Nonzero PC is illegal when EVPD is not set */
                dbg_info("msc: INQUIRY PC=%02x but EVPD not set\n", page_code);
                goto illegal_request;
        } else if (alloc_len) {
                /* Send the normal INQUIRY data */
                buf = buffer_alloc();
                if (!buf)
                        goto err_buf_alloc;

                total_len = min(alloc_len, sizeof(msc_inquiry_data));
                buffer_init_tx(buf, &msc_inquiry_data, total_len);
                usb_req_add_buffer(req, buf);
                req->req_done = msc_buf_sent;
        }

        residue += alloc_len - total_len;
        msc_prepare_csw(msc, residue, USB_CSW_STATUS_PASS);

        if (total_len) {
                udc_ep_submit_in_req(udc, msc->bulk_in_ep, req);
                msc_request_done(udc, msc, residue);
        } else {
                usb_req_free(req);
                msc_request_done_nodata(udc, msc, residue);
        }

        return;

illegal_request:
        msc_request_failed(msc, alloc_len + residue,
                        USB_CSW_STATUS_FAIL,
                        SCSI_SK_ILLEGAL_REQUEST,
                        SCSI_ASC_INVALID_FIELD_IN_CDB);
        usb_req_free(req);
        return;

err_buf_alloc:
        usb_req_free(req);
err_req_alloc:
        msc_out_of_memory(msc);
}

static long msc_add_mode_pages(struct msc_func *msc,
                struct usb_request *req, uint32_t avail_len,
                const uint8_t *cdb, uint32_t cbw_data_len)
{
        /* Changeable values are not supported */
        if (scsi_mode_sense_get_pc(cdb) == SCSI_MS_PC_CHANGEABLE) {
                dbg_info("msc: changeable mode pages not supported\n");
                goto invalid_request;
        }

        /* Saved values are not supported either, but we're allowed to
         * return current values instead. Also, since we don't support
         * changeable values, current values equal default values */

        /* Now, send any mode pages that were requested */
        switch (scsi_mode_sense_get_page_code(cdb)) {
        case 0:
                /* Just return the header and block descriptors */
                break;
        case SCSI_MS_PAGE_ALL:
                /* We don't support any actual mode pages yet */
                break;
        default:
                dbg_info("msc: unsupported mode page 0x%x\n",
                                scsi_mode_sense_get_page_code(cdb));
                goto invalid_request;
        }

        return 0;

invalid_request:
        msc_request_failed(msc, cbw_data_len, USB_CSW_STATUS_FAIL,
                        SCSI_SK_ILLEGAL_REQUEST,
                        SCSI_ASC_INVALID_FIELD_IN_CDB);
        return -1;
}

static void msc_mode_sense(struct msc_func *msc, struct udc *udc,
                struct usb_msc_cbw *cbw, uint32_t alloc_len)
{
        union msc_mode_param_header *header;
        struct usb_request      *req;
        struct buffer           *buf;
        const uint8_t           *cdb = cbw->CDB;
        long                    residue;
        long                    ret;
        size_t                  total_size;

        dbg_verbose("msc MODE SENSE(N) page %u PC%u len %u\n",
                        scsi_mode_sense_get_page_code(cdb),
                        scsi_mode_sense_get_pc(cdb),
                        alloc_len);

        residue = msc_validate_req(msc, cbw, alloc_len, USB_CBW_DIRECTION_IN);
        if (residue < 0)
                return;

        req = usb_req_alloc();
        if (!req)
                goto err_req_alloc;
        req->context = msc;
        req->req_done = msc_mode_params_sent;

        /*
         * Always send a header. Also allocate room for a block
         * descriptor while we're at it.
         */
        buf = buffer_dma_alloc(MSC_MODE_PARAM_HDR_BUF_LEN);
        if (!buf)
                goto err_buf_alloc;
        usb_req_add_buffer(req, buf);

        header = buf->addr.ptr;
        memset(header, 0, MSC_MODE_PARAM_HDR_BUF_LEN);

        /* The block device determines WP status */
        if (scsi_cdb_get_opcode(cdb) == SCSI_CMD_MODE_SENSE6) {
                if (!test_bit(BDEV_WRITEABLE, &msc->bdev->flags))
                        header->h6.device_specific_parameter = SCSI_MS_SBC_WP;
                total_size = sizeof(header->h6);
        } else {
                if (!test_bit(BDEV_WRITEABLE, &msc->bdev->flags))
                        header->h10.device_specific_parameter = SCSI_MS_SBC_WP;
                total_size = sizeof(header->h10);
        }

        /*
         * If the Disable Block Descriptors bit isn't set, add a
         * block descriptor.
         */
        if (!scsi_mode_sense_dbd_is_set(cdb)) {
                struct sbc_slba_block_desc      *desc;

                total_size += sizeof(*desc);
                if (scsi_cdb_get_opcode(cdb) == SCSI_CMD_MODE_SENSE6) {
                        header->h6.block_descriptor_length = sizeof(*desc);
                        desc = (struct sbc_slba_block_desc *)(&header->h6 + 1);
                } else {
                        header->h10.block_descriptor_length
                                = cpu_to_be16(sizeof(*desc));
                        desc = (struct sbc_slba_block_desc *)(&header->h10 + 1);
                }
                desc->nr_blocks = cpu_to_be32(msc->bdev->nr_blocks);
                desc->block_len = cpu_to_be32(msc->bdev->block_size);
                assert(!(be32_to_cpu(desc->block_len)
                                        & ~SBC_SLBA_BLOCK_LEN_MASK));
        }

        buffer_resize(buf, total_size);

        /* Add the mode pages to the response */
        ret = msc_add_mode_pages(msc, req, alloc_len - total_size, cdb,
                        alloc_len + residue);
        if (ret < 0) {
                usb_req_free(req);
                buffer_dma_free(buf, MSC_MODE_PARAM_HDR_BUF_LEN);
                return;
        }

        total_size += ret;
        if (scsi_cdb_get_opcode(cdb) == SCSI_CMD_MODE_SENSE6)
                header->h6.mode_data_length = total_size - 1;
        else
                header->h10.mode_data_length = cpu_to_be16(total_size - 2);

        /*
         * Finish up the USB request and send it if it ends up
         * containing any data.
         */
        residue += alloc_len - min(alloc_len, total_size);
        msc_prepare_csw(msc, residue, USB_CSW_STATUS_PASS);
        if (alloc_len > 0) {
                udc_ep_submit_in_req(udc, msc->bulk_in_ep, req);
                msc_request_done(udc, msc, residue);
        } else {
                usb_req_free(req);
                buffer_dma_free(buf, MSC_MODE_PARAM_HDR_BUF_LEN);
                msc_request_done_nodata(udc, msc, residue);
        }

        return;

err_buf_alloc:
        usb_req_free(req);
err_req_alloc:
        msc_out_of_memory(msc);
}

static void msc_read_capacity(struct msc_func *msc, struct udc *udc,
                struct usb_msc_cbw *cbw)
{
        struct sbc_read_capacity10_data *response;
        struct usb_request      *req;
        struct buffer           *buf;
        uint32_t                residue;

        build_assert(sizeof(*response) == 8);

        dbg_verbose("msc READ CAPACITY LBA %x blklen %u\n",
                        msc->bdev->nr_blocks - 1, msc->bdev->block_size);

        residue = msc_validate_req(msc, cbw, 8, USB_CBW_DIRECTION_IN);
        if (residue < 0)
                return;

        req = usb_req_alloc();
        if (!req)
                goto err_req_alloc;
        req->req_done = msc_capacity_sent;
        req->context = msc;

        buf = buffer_dma_alloc(sizeof(*response));
        if (!buf)
                goto err_buf_alloc;
        usb_req_add_buffer(req, buf);

        response = buf->addr.ptr;
        response->max_lba = cpu_to_be32(msc->bdev->nr_blocks - 1);
        response->block_len = cpu_to_be32(msc->bdev->block_size);

        msc_prepare_csw(msc, residue, USB_CSW_STATUS_PASS);
        udc_ep_submit_in_req(udc, msc->bulk_in_ep, req);
        msc_request_done(udc, msc, residue);

        return;

err_buf_alloc:
        usb_req_free(req);
err_req_alloc:
        msc_out_of_memory(msc);
}

static uint32_t msc_fill_buffer_list(struct slist *buf_list,
                unsigned int block_size, uint32_t nr_blocks)
{
        uint32_t                blocks_remaining;
        uint32_t                blocks_per_buf;
        unsigned int            i;

        blocks_remaining = nr_blocks;
        blocks_per_buf = MSC_DATA_BUFFER_SIZE / block_size;

        for (i = 0; i < MSC_MAX_NR_BUFFERS; i++) {
                struct buffer           *buf;

                buf = buffer_dma_alloc(MSC_DATA_BUFFER_SIZE);
                if (!buf)
                        break;

                if (blocks_per_buf > blocks_remaining) {
                        blocks_per_buf = blocks_remaining;
                        buffer_resize(buf, blocks_per_buf * block_size);
                }

                slist_insert_tail(buf_list, &buf->node);
                blocks_remaining -= blocks_per_buf;
                if (!blocks_remaining)
                        break;
        }

        return nr_blocks - blocks_remaining;
}

static int msc_submit_read_buffers(struct msc_func *msc,
                struct block_device *bdev, struct block_request *breq,
                uint32_t nr_blocks)
{
        struct slist            buf_list;
        uint32_t                blocks_queued;

        slist_init(&buf_list);
        blocks_queued = msc_fill_buffer_list(&buf_list, bdev->block_size,
                                nr_blocks);

        dbg_verbose("msc: blocks %u/%u queued for read\n", blocks_queued,
                        nr_blocks);

        if (unlikely(!blocks_queued))
                return 0;

        /*
         * If the request failed before we got around to submitting the
         * buffer list, abort without submitting anything.
         */
        atomic_inc(&msc->blk_segs_pending);
        if (block_submit_buf_list(bdev, breq, &buf_list)) {
                atomic_dec(&msc->blk_segs_pending);
                msc_free_dma_buf_list(&buf_list);
                return 0;
        }

        msc->blocks_queued += blocks_queued;

        return blocks_queued;
}

static void msc_read_worker(struct msc_func *msc)
{
        struct block_device     *bdev = msc->bdev;
        struct block_request    *breq = msc->block_req;
        uint32_t                blocks_remaining;
        uint32_t                submitted;

        cpu_irq_disable();
        dbg_verbose("msc: blk pending %u locked %d\n",
                        atomic_read(&msc->blk_segs_pending), msc->queue_locked);
        while (atomic_read(&msc->blk_segs_pending) < MSC_MAX_NR_SEGS
                        && !msc->queue_locked) {
                dbg_verbose("msc: read worker: q%u <= t%u s %d\n",
                                msc->blocks_queued, msc->blocks_total,
                                breq->status);
                assert(msc->blocks_queued <= msc->blocks_total);
                blocks_remaining = msc->blocks_total - msc->blocks_queued;
                if (!blocks_remaining)
                        break;

                msc->queue_locked = true;
                cpu_irq_enable();

                submitted = msc_submit_read_buffers(msc, bdev, breq,
                                blocks_remaining);

                cpu_irq_disable();
                msc->queue_locked = false;

                if (!submitted)
                        break;
        }
        cpu_irq_enable();

        dbg_verbose("msc read worker done\n");
}

static void msc_read_data_sent(struct udc *udc, struct usb_request *req)
{
        struct msc_func         *msc = req->context;
        uint32_t                bytes_xfered;
        uint32_t                blocks_remaining;
        int                     status;

        dbg_verbose("msc: data sent: first=%p last=%p\n",
                        slist_peek_head_node(&req->buf_list),
                        slist_peek_tail_node(&req->buf_list));

        msc_free_dma_buf_list(&req->buf_list);
        status = req->status;
        bytes_xfered = req->bytes_xfered;
        usb_req_free(req);

        assert(atomic_read(&msc->usb_reqs_pending) > 0);
        atomic_dec(&msc->usb_reqs_pending);

        /*
         * If the USB transfer failed, we were probably disconnected
         * or reset. Abort the operation.
         */
        if (status) {
                block_abort_req(msc->bdev, msc->block_req);
                return;
        }

        blocks_remaining = msc->blocks_total - msc->blocks_queued;
        if (!blocks_remaining)
                msc_request_data_done(udc, msc);
        else
                msc_read_worker(msc);
}

/*
 * This function is called when the block request has moved to the head
 * of the queue and the first batch of buffers have been submitted for
 * DMA. If we start submitting buffers before this, they will be
 * combined with the first batch, and we won't get a separate completion
 * callback for them.
 */
static void msc_block_read_started(struct block_device *bdev,
                struct block_request *breq)
{
        struct msc_func         *msc = breq->context;

        if (msc->blocks_queued < msc->blocks_total)
                msc_read_worker(msc);
}

static void msc_block_read_done(struct block_device *bdev,
                struct block_request *breq)
{
        struct msc_func         *msc = breq->context;
        uint32_t                residue;

        assert(breq == msc->block_req);

        residue = le32_to_cpu(msc->csw->dCSWDataResidue);

        if (breq->status) {
                struct usb_msc_csw      *csw = msc->csw;
                uint32_t                blocks_xfered;

                blocks_xfered = blk_req_get_blocks_xfered(bdev, breq);

                dbg_warning("msc: block read failed: %d (after %u blocks)\n",
                                breq->status, blocks_xfered);

                residue = le32_to_cpu(msc->cbw->dCBWDataTransferLength);
                residue -= bdev->block_size * blocks_xfered;
                csw->dCSWDataResidue = cpu_to_le32(residue);
                csw->bCSWStatus = USB_CSW_STATUS_FAIL;

                msc_init_sense(msc, SCSI_SK_MEDIUM_ERROR,
                                SCSI_ASC_UNRECOVERED_READ_ERROR,
                                msc->first_lba + blocks_xfered);
        }

        msc_request_done(msc->udc, msc, residue);
}

static void msc_block_read_buffers_done(struct block_device *bdev,
                struct block_request *breq, struct slist *buf_list)
{
        struct msc_func         *msc = breq->context;
        struct usb_request      *req;

        assert(!slist_is_empty(buf_list));

        dbg_verbose("msc: read bufs done: status %d\n", breq->status);

        if (breq->status != -STATUS_IN_PROGRESS || !msc->bulk_in_ep) {
                dbg_verbose("  request terminated, discarding buffers\n");
                msc_free_dma_buf_list(buf_list);
                return;
        }

        req = usb_req_alloc();
        if (!req) {
                block_abort_req(bdev, breq);
                msc_free_dma_buf_list(buf_list);
                msc_out_of_memory(msc);
                return;
        }

        assert(atomic_read(&msc->blk_segs_pending) > 0);
        atomic_inc(&msc->usb_reqs_pending);
        atomic_dec(&msc->blk_segs_pending);

        slist_move_to_tail(&req->buf_list, buf_list);
        req->req_done = msc_read_data_sent;
        req->context = msc;
        dbg_verbose("  submitting IN request...\n");
        udc_ep_submit_in_req(msc->udc, msc->bulk_in_ep, req);
}

static void msc_do_read(struct msc_func *msc, struct udc *udc,
                struct usb_msc_cbw *cbw, uint32_t lba, uint32_t nr_blocks)
{
        struct block_device     *bdev = msc->bdev;
        struct block_request    *breq;
        long                    residue;
        uint32_t                cdb_data_len;
        uint32_t                blocks_queued;
        unsigned long           iflags;

        dbg_verbose("msc READ(x) %u blocks, LBA %u\n", nr_blocks, lba);

        assert(!msc->xfer_in_progress);

        /*
         * READ(10) allows max 65535 blocks. We allow max 65535 for
         * block_size, so this cannot overflow.
         */
        cdb_data_len = nr_blocks * bdev->block_size;

        residue = msc_validate_req(msc, cbw, cdb_data_len,
                        USB_CBW_DIRECTION_IN);
        if (unlikely(residue < 0))
                return;

        iflags = cpu_irq_save();
        if (msc->not_ready) {
                cpu_irq_restore(iflags);
                msc_request_failed(msc,
                                le32_to_cpu(cbw->dCBWDataTransferLength),
                                USB_CSW_STATUS_FAIL,
                                SCSI_SK_NOT_READY, msc->busy_asc);
                return;
        }

        msc->xfer_in_progress = true;
        cpu_irq_restore(iflags);

        msc_prepare_csw(msc, residue, USB_CSW_STATUS_PASS);

        /* If no data was requested, we're done processing the command */
        if (unlikely(nr_blocks == 0)) {
                msc_request_done_nodata(udc, msc, residue);
                return;
        }

        msc->blocks_total = nr_blocks;
        msc->blocks_queued = 0;
        msc->queue_locked = true;
        atomic_write(&msc->blk_segs_pending, 0);
        atomic_write(&msc->usb_reqs_pending, 0);

        breq = msc->block_req;
        breq->req_started = msc_block_read_started;
        breq->req_done = msc_block_read_done;
        breq->buf_list_done = msc_block_read_buffers_done;
        breq->context = msc;
        block_queue_req(bdev, breq, lba, nr_blocks, BLK_OP_READ);

        blocks_queued = msc_submit_read_buffers(msc, bdev, breq, nr_blocks);
        if (blocks_queued == 0) {
                block_abort_req(bdev, breq);
                msc_out_of_memory(msc);
        }
        msc->queue_locked = false;
}

static void msc_write_data_received(struct udc *udc, struct usb_request *req);

static int msc_submit_write_data_req(struct msc_func *msc,
                struct block_device *bdev, uint32_t nr_blocks)
{
        struct usb_request      *req;
        uint32_t                blocks_queued;

        req = usb_req_alloc();
        if (!req)
                return 0;

        req->context = msc;
        req->req_done = msc_write_data_received;

        blocks_queued = msc_fill_buffer_list(&req->buf_list, bdev->block_size,
                                nr_blocks);

        dbg_verbose("msc: blocks %u/%u queued for write\n", blocks_queued,
                        nr_blocks);

        if (unlikely(!blocks_queued)) {
                usb_req_free(req);
                return 0;
        }

        msc->blocks_queued += blocks_queued;

        atomic_inc(&msc->usb_reqs_pending);
        udc_ep_submit_out_req(msc->udc, msc->bulk_out_ep, req);

        return blocks_queued;
}

static void msc_write_worker(void *data)
{
        struct msc_func         *msc = data;
        struct block_device     *bdev = msc->bdev;
        uint32_t                blocks_remaining;
        uint32_t                submitted;

        while (atomic_read(&msc->usb_reqs_pending) < MSC_MAX_NR_SEGS
                        && !msc->queue_locked) {
                dbg_verbose("msc: write worker: q%u <= 5%u s %d\n",
                                msc->blocks_queued, msc->blocks_total,
                                msc->block_req->status);
                assert(msc->blocks_queued <= msc->blocks_total);
                blocks_remaining = msc->blocks_total - msc->blocks_queued;
                if (!blocks_remaining)
                        break;

                msc->queue_locked = true;
                cpu_irq_enable();

                submitted = msc_submit_write_data_req(msc, bdev,
                                blocks_remaining);

                cpu_irq_disable();
                msc->queue_locked = false;

                if (!submitted)
                        break;
        }
}

static void msc_block_write_started(struct block_device *bdev,
                struct block_request *breq)
{
        struct msc_func         *msc = breq->context;

        if (msc->blocks_queued < msc->blocks_total)
                msc_write_worker(msc);
}

static void msc_block_write_done(struct block_device *bdev,
                struct block_request *breq)
{
        struct msc_func         *msc = breq->context;
        uint32_t                residue;

        assert(breq == msc->block_req);

        residue = le32_to_cpu(msc->csw->dCSWDataResidue);

        if (breq->status) {
                struct usb_msc_csw      *csw = msc->csw;
                uint32_t                blocks_xfered;

                blocks_xfered = blk_req_get_blocks_xfered(bdev, breq);

                dbg_warning("msc: block write failed: %d (after %u blocks)\n",
                                breq->status, blocks_xfered);

                residue = le32_to_cpu(msc->cbw->dCBWDataTransferLength);
                residue -= bdev->block_size * blocks_xfered;
                csw->dCSWDataResidue = cpu_to_le32(residue);
                csw->bCSWStatus = USB_CSW_STATUS_FAIL;

                msc_init_sense(msc, SCSI_SK_MEDIUM_ERROR,
                                SCSI_ASC_WRITE_ERROR,
                                msc->first_lba + blocks_xfered);
        }

        msc_request_done(msc->udc, msc, residue);
        msc_request_data_done(msc->udc, msc);
}

static void msc_block_write_buffers_done(struct block_device *bdev,
                struct block_request *breq, struct slist *buf_list)
{
        struct msc_func         *msc = breq->context;

        msc_free_dma_buf_list(buf_list);

        assert(atomic_read(&msc->blk_segs_pending) > 0);
        atomic_dec(&msc->blk_segs_pending);

        assert(msc->blocks_queued <= msc->blocks_total);
        if (msc->blocks_queued < msc->blocks_total)
                msc_write_worker(msc);
}

static void msc_write_data_received(struct udc *udc, struct usb_request *req)
{
        struct slist            buf_list;
        struct msc_func         *msc = req->context;
        struct block_device     *bdev;
        struct block_request    *breq;
        int                     status;

        status = req->status;
        slist_init(&buf_list);
        slist_move_to_tail(&buf_list, &req->buf_list);
        usb_req_free(req);

        bdev = msc->bdev;
        breq = msc->block_req;

        if (!status) {
                atomic_inc(&msc->blk_segs_pending);
                atomic_dec(&msc->usb_reqs_pending);

                if (block_submit_buf_list(bdev, breq, &buf_list)) {
                        atomic_dec(&msc->blk_segs_pending);
                        msc_free_dma_buf_list(&buf_list);
                }
        } else {
                block_abort_req(bdev, breq);
        }
}

static void msc_do_write(struct msc_func *msc, struct udc *udc,
                struct usb_msc_cbw *cbw, uint32_t lba, uint32_t nr_blocks)
{
        struct block_device     *bdev = msc->bdev;
        struct block_request    *breq;
        long                    residue;
        uint32_t                cdb_data_len;
        uint32_t                blocks_queued;
        unsigned long           iflags;

        dbg_verbose("msc WRITE(x) %u blocks, LBA %u\n", nr_blocks, lba);

        assert(!msc->xfer_in_progress);

        /*
         * WRITE(10) allows max 65535 blocks. We allow max 65535 for
         * block_size, so this cannot overflow.
         */
        cdb_data_len = nr_blocks * bdev->block_size;

        residue = msc_validate_req(msc, cbw, cdb_data_len, 0);
        if (unlikely(residue < 0))
                return;

        iflags = cpu_irq_save();
        if (msc->not_ready) {
                cpu_irq_restore(iflags);
                msc_request_failed(msc,
                                le32_to_cpu(cbw->dCBWDataTransferLength),
                                USB_CSW_STATUS_FAIL,
                                SCSI_SK_NOT_READY, msc->busy_asc);
                return;
        }

        msc->xfer_in_progress = true;
        cpu_irq_restore(iflags);

        msc_prepare_csw(msc, residue, USB_CSW_STATUS_PASS);

        /* If no data was requested, we're done processing the command */
        if (unlikely(nr_blocks == 0)) {
                msc_request_done_nodata(udc, msc, residue);
                return;
        }

        msc->blocks_total = nr_blocks;
        msc->blocks_queued = 0;
        msc->queue_locked = true;
        atomic_write(&msc->blk_segs_pending, 0);
        atomic_write(&msc->usb_reqs_pending, 0);

        breq = msc->block_req;
        breq->req_started = msc_block_write_started;
        breq->req_done = msc_block_write_done;
        breq->buf_list_done = msc_block_write_buffers_done;
        breq->context = msc;
        block_queue_req(bdev, breq, lba, nr_blocks, BLK_OP_WRITE);

        blocks_queued = msc_submit_write_data_req(msc, bdev, nr_blocks);
        if (blocks_queued == 0) {
                block_abort_req(bdev, breq);
                msc_out_of_memory(msc);
        }
        msc->queue_locked = false;
}

static void msc_verify_read(struct msc_func *msc, struct block_device *bdev,
                uint32_t first_lba, uint32_t nr_blocks);
static void msc_verify_bytchk(struct msc_func *msc, struct block_device *bdev,
                uint32_t first_lba, uint32_t nr_blocks);

static void msc_verify_bytchk(struct msc_func *msc, struct block_device *bdev,
                uint32_t first_lba, uint32_t nr_blocks)
{
        msc_request_failed(msc, le32_to_cpu(msc->csw->dCSWDataResidue),
                        USB_CSW_STATUS_FAIL, SCSI_SK_ILLEGAL_REQUEST,
                        SCSI_ASC_INVALID_FIELD_IN_CDB);
}

static void msc_verify_read_done(struct block_device *bdev,
                struct block_request *breq)
{
        struct msc_func         *msc = breq->context;

        assert(breq == msc->block_req);

        if (breq->status) {
                uint32_t        blocks_xfered;

                blocks_xfered = blk_req_get_blocks_xfered(bdev, breq);

                msc->csw->bCSWStatus = USB_CSW_STATUS_FAIL;
                msc_init_sense(msc, SCSI_SK_MEDIUM_ERROR,
                                SCSI_ASC_UNRECOVERED_READ_ERROR,
                                msc->first_lba + blocks_xfered);
        }

        msc_request_done_nodata(msc->udc, msc,
                        le32_to_cpu(msc->csw->dCSWDataResidue));
}

static void msc_verify_read_buffers_done(struct block_device *bdev,
                struct block_request *breq, struct slist *buf_list)
{
        struct slist            new_buf_list;
        struct msc_func         *msc = breq->context;
        unsigned int            block_size;
        uint32_t                blocks_queued;
        uint32_t                blocks_total;
        unsigned int            blocks_per_buf;

        blocks_total = msc->blocks_total;
        blocks_queued = msc->blocks_queued;
        block_size = bdev->block_size;
        blocks_per_buf = MSC_DATA_BUFFER_SIZE / block_size;
        slist_init(&new_buf_list);

        while (!slist_is_empty(buf_list)) {
                struct buffer   *buf;

                assert(blocks_queued <= blocks_total);
                if (blocks_queued == blocks_total)
                        break;

                buf = slist_pop_head(buf_list, struct buffer, node);
                blocks_queued += blocks_per_buf;

                if (blocks_queued > blocks_total) {
                        blocks_per_buf -= blocks_queued - blocks_total;
                        buffer_resize(buf, block_size * blocks_per_buf);
                        blocks_queued = blocks_total;
                }
                slist_insert_tail(&new_buf_list, &buf->node);
        }

        if (!slist_is_empty(&new_buf_list)) {
                if (block_submit_buf_list(bdev, breq, &new_buf_list))
                        msc_free_dma_buf_list(&new_buf_list);
                else
                        msc->blocks_queued = blocks_queued;
        }

        /* Free whatever is left over from the loop above */
        msc_free_dma_buf_list(buf_list);
}

static void msc_verify_read(struct msc_func *msc, struct block_device *bdev,
                uint32_t first_lba, uint32_t nr_blocks)
{
        struct slist            buf_list;
        struct block_request    *breq;
        unsigned int            block_size;
        uint32_t                blocks_queued;

        /*
         * VERIFY isn't as performance critical as READ and WRITE, so
         * we're not doing double-buffering here.
         */
        msc->blocks_total = nr_blocks;
        breq = msc->block_req;
        breq->req_started = NULL;
        breq->req_done = msc_verify_read_done;
        breq->buf_list_done = msc_verify_read_buffers_done;
        breq->context = msc;
        block_queue_req(bdev, breq, first_lba, nr_blocks, BLK_OP_READ);

        block_size = bdev->block_size;
        slist_init(&buf_list);

        blocks_queued = msc_fill_buffer_list(&buf_list, bdev->block_size,
                                nr_blocks);

        if (unlikely(blocks_queued == 0)) {
                block_abort_req(bdev, breq);
                msc_out_of_memory(msc);
                return;
        }

        msc->blocks_queued = blocks_queued;
        if (block_submit_buf_list(bdev, breq, &buf_list)) {
                block_abort_req(bdev, breq);
                msc_free_dma_buf_list(&buf_list);
                msc_out_of_memory(msc);
        }
}

static void msc_do_verify(struct msc_func *msc, struct udc *udc,
                struct usb_msc_cbw *cbw, uint32_t lba, uint32_t nr_blocks,
                bool bytchk)
{
        struct block_device     *bdev = msc->bdev;
        long                    residue;
        uint32_t                cdb_data_len = 0;
        unsigned long           iflags;

        dbg_verbose("msc VERIFY(x) %u blocks, LBA %u\n", nr_blocks, lba);

        /* Only expect to transfer data when doing byte checking */
        if (bytchk)
                cdb_data_len = nr_blocks * bdev->block_size;

        residue = msc_validate_req(msc, cbw, cdb_data_len, 0);
        if (unlikely(residue < 0))
                return;

        iflags = cpu_irq_save();
        if (msc->not_ready) {
                cpu_irq_restore(iflags);
                msc_request_failed(msc,
                                le32_to_cpu(cbw->dCBWDataTransferLength),
                                USB_CSW_STATUS_FAIL,
                                SCSI_SK_NOT_READY, msc->busy_asc);
                return;
        }

        msc->xfer_in_progress = true;
        cpu_irq_restore(iflags);

        msc_prepare_csw(msc, residue, USB_CSW_STATUS_PASS);

        if (unlikely(nr_blocks == 0)) {
                /* Nothing to verify; not an error */
                msc_request_done_nodata(udc, msc, residue);
                return;
        }

        if (bytchk)
                msc_verify_bytchk(msc, bdev, lba, nr_blocks);
        else
                msc_verify_read(msc, bdev, lba, nr_blocks);
}

static void msc_cbw_received(struct udc *udc, struct usb_request *req)
{
        struct msc_func         *msc = req->context;
        struct usb_msc_cbw      *cbw;
        uint8_t                 opcode;

        dbg_verbose("cbw received: status %d len %zu\n",
                        req->status, req->bytes_xfered);

        cbw = msc->cbw;
        assert(req == msc->cbw_csw_req);
        assert(cbw == usb_req_get_first_buffer(req)->addr.ptr);

        /* Don't submit any buffers when the USB transfer failed */
        if (req->status)
                return;

        /* Is this a valid CBW? */
        if (cbw->dCBWSignature != LE32(USB_CBW_SIGNATURE)
                        || req->bytes_xfered != 31) {
                /*
                 * No. Wedge both endpoints -- the host must do a
                 * Bulk-Only Mass Storage Reset to recover.
                 */
                udc_ep_set_wedge(udc, msc->bulk_in_ep);
                udc_ep_set_wedge(udc, msc->bulk_out_ep);
                return;
        }

        opcode = scsi_cdb_get_opcode(cbw->CDB);

        /* Try to handle the request */
        switch (opcode) {
        case SCSI_CMD_TEST_UNIT_READY:
                msc_test_unit_ready(msc, udc,
                                le32_to_cpu(cbw->dCBWDataTransferLength));
                break;

        case SCSI_CMD_REQUEST_SENSE:
                msc_request_sense(msc, udc, cbw);
                break;

        case SCSI_CMD_READ6:
                msc_do_read(msc, udc, cbw, scsi_cdb_get_lba(cbw->CDB, 6),
                                scsi_cdb_get_xfer_len(cbw->CDB, 6));
                break;

        case SCSI_CMD_WRITE6:
                msc_do_write(msc, udc, cbw, scsi_cdb_get_lba(cbw->CDB, 6),
                                scsi_cdb_get_xfer_len(cbw->CDB, 6));
                break;

        case SCSI_CMD_INQUIRY:
                msc_inquiry(msc, udc, cbw);
                break;

        case SCSI_CMD_MODE_SENSE6:
                msc_mode_sense(msc, udc, cbw,
                        scsi_cdb_get_alloc_len(cbw->CDB, SCSI_CMD_MODE_SENSE6));
                break;

        case SCSI_CMD_READ_CAPACITY10:
                msc_read_capacity(msc, udc, cbw);
                break;

        case SCSI_CMD_READ10:
                msc_do_read(msc, udc, cbw, scsi_cdb_get_lba(cbw->CDB, 10),
                                scsi_cdb_get_xfer_len(cbw->CDB, 10));
                break;

        case SCSI_CMD_WRITE10:
                msc_do_write(msc, udc, cbw, scsi_cdb_get_lba(cbw->CDB, 10),
                                scsi_cdb_get_xfer_len(cbw->CDB, 10));
                break;

        case SCSI_CMD_VERIFY10:
                msc_do_verify(msc, udc, cbw, scsi_cdb_get_lba(cbw->CDB, 10),
                                scsi_cdb_get_xfer_len(cbw->CDB, 10),
                                scsi_cdb_bytchk_is_set(cbw->CDB, 10));
                break;

        case SCSI_CMD_MODE_SENSE10:
                msc_mode_sense(msc, udc, cbw,
                                scsi_cdb_get_alloc_len(cbw->CDB,
                                        SCSI_CMD_MODE_SENSE10));
                break;

        default:
                dbg_verbose("MSC: Unhandled opcode %02x\n", opcode);

                msc_request_failed(msc,
                                le32_to_cpu(cbw->dCBWDataTransferLength),
                                USB_CSW_STATUS_FAIL,
                                SCSI_SK_ILLEGAL_REQUEST,
                                SCSI_ASC_INVALID_COMMAND_OPERATION_CODE);
                break;
        }
}

static int msc_iface_enable(struct udc *udc, struct usb_func_iface *iface)
{
        const struct msc_bulk_iface_block *desc;
        struct msc_func         *msc = msc_func_of(iface);
        struct usb_msc_cbw      *cbw;
        struct usb_request      *req;
        struct buffer           *buf;
        phys_addr_t             phys;

        msc_queue_empty(msc);

        switch (udc->speed) {
        case USB_SPEED_FULL:
                desc = &msc_bulk_fs_iface;
                break;
#ifdef CONFIG_UDC_HIGH_SPEED
        case USB_SPEED_HIGH:
                desc = &msc_bulk_hs_iface;
                break;
#endif
        default:
                return -1;
        }

        msc->udc = udc;

        msc->bulk_in_ep = udc_ep_create(udc, &desc->in_ep,
                        CONFIG_MSC_BULK_NR_BANKS);
        msc->bulk_out_ep = udc_ep_create(udc, &desc->out_ep,
                        CONFIG_MSC_BULK_NR_BANKS);
        if (msc->bulk_in_ep < 0 || msc->bulk_out_ep < 0)
                goto fail;

        msc->block_req = block_alloc_request(msc->bdev);
        assert(msc->block_req);

        msc->csw = msc_alloc_csw(msc, &phys);
        assert(msc->csw);

        cbw = msc_alloc_cbw(msc, &phys);
        assert(cbw);
        msc->cbw = cbw;

        req = usb_req_alloc();
        assert(req);
        req->req_done = msc_cbw_received;
        req->context = msc;
        msc->cbw_csw_req = req;

        buf = buffer_alloc();
        assert(buf);
        buffer_init_rx(buf, cbw, sizeof(*cbw));
        usb_req_add_buffer(req, buf);

        udc_ep_submit_out_req(udc, msc->bulk_out_ep, req);

        return 0;

fail:
        if (msc->bulk_in_ep > 0) {
                usb_ep_id_t ep = msc->bulk_in_ep;
                msc->bulk_in_ep = 0;
                udc_ep_destroy(udc, ep);
        }
        if (msc->bulk_out_ep > 0) {
                usb_ep_id_t ep = msc->bulk_out_ep;
                msc->bulk_out_ep = 0;
                udc_ep_destroy(udc, ep);
        }

        return -1;
}

static void msc_iface_disable(struct udc *udc, struct usb_func_iface *iface)
{
        struct msc_func *msc = msc_func_of(iface);
        usb_ep_id_t     in, out;

        msc_queue_empty(msc);

        in = msc->bulk_in_ep;
        msc->bulk_in_ep = 0;
        out = msc->bulk_out_ep;
        msc->bulk_out_ep = 0;

        if (in > 0)
                udc_ep_destroy(udc, in);
        if (out > 0)
                udc_ep_destroy(udc, out);

        msc_free_cbw(msc, msc->cbw);
        msc_free_csw(msc, msc->csw);
        buffer_free(usb_req_get_first_buffer(msc->cbw_csw_req));
        usb_req_free(msc->cbw_csw_req);
        block_free_request(msc->bdev, msc->block_req);
}

static int msc_bulk_reset(struct udc *udc, struct msc_func *msc)
{
        struct usb_request      *req;
        struct buffer           *buf;

        dbg_info("MSC Bulk Reset\n");

        /*
         * Flush and un-wedge the endpoints. They will still be
         * halted, so the host must still do ClearFeature(HALT) on
         * both endpoints as per section 5.3.4 in the MSC Bulk-Only
         * spec.
         */
        if (msc->bulk_in_ep > 0) {
                udc_ep_flush(udc, msc->bulk_in_ep);
                udc_ep_clear_wedge(udc, msc->bulk_in_ep);
        }
        if (msc->bulk_out_ep > 0) {
                udc_ep_flush(udc, msc->bulk_out_ep);
                udc_ep_clear_wedge(udc, msc->bulk_out_ep);
        }

        msc_queue_empty(msc);

        /* Now, submit a new CBW and keep going */
        req = msc->cbw_csw_req;
        buf = usb_req_get_first_buffer(req);
        usb_req_init(req);

        buffer_init_rx(buf, msc->cbw, sizeof(*msc->cbw));
        usb_req_add_buffer(req, buf);
        req->req_done = msc_cbw_received;

        udc_ep_submit_out_req(udc, msc->bulk_out_ep, req);

        return 0;
}

static int msc_iface_setup(struct udc *udc, struct usb_func_iface *iface,
                struct usb_setup_req *req)
{
        uint16_t        value = le16_to_cpu(req->wValue);
        uint16_t        len = le16_to_cpu(req->wLength);
        uint8_t         byte;

        if (usb_req_type(req) != USB_REQTYPE_CLASS)
                return -1;

        switch (req->bRequest) {
        case USB_MSC_REQ_BULK_RESET:
                if (len || value || usb_req_is_in(req))
                        return -1;

                if (msc_bulk_reset(udc, msc_func_of(iface)))
                        return -1;

                udc_ep0_send_status(udc);
                break;

        case USB_MSC_REQ_GET_MAX_LUN:
                if (len != 1 || value || usb_req_is_out(req))
                        return -1;

                /* TODO: Support multiple LUNs */
                byte = 0;
                udc_ep0_write_sync(udc, &byte, sizeof(byte));
                udc_ep0_expect_status(udc);
                break;

        default:
                return -1;
        }

        return 0;
}

static const struct usb_func_iface_ops msc_iface_ops = {
        .enable         = msc_iface_enable,
        .disable        = msc_iface_disable,
        .setup          = msc_iface_setup,
};

void usb_msc_set_busy(struct usb_func_iface *iface, uint16_t asc,
                void (*queue_empty)(void *data), void *data)
{
        struct msc_func *msc = msc_func_of(iface);
        unsigned long   iflags;

        iflags = cpu_irq_save();
        msc->not_ready = true;
        msc->busy_asc = asc;
        dbg_verbose("msc_set_busy: ASC(Q) %04x in_progress: %d\n",
                        asc, msc->xfer_in_progress);
        if (msc->xfer_in_progress) {
                msc->busy_cb = queue_empty;
                msc->busy_cb_data = data;
                cpu_irq_restore(iflags);
        } else {
                cpu_irq_restore(iflags);
                queue_empty(data);
        }
}

void usb_msc_set_ready(struct usb_func_iface *iface)
{
        struct msc_func *msc = msc_func_of(iface);

        msc->not_ready = false;
}

static struct msc_func msc_func = {
        .iface.nr_settings      = 1,
        .iface.setting[0] = {
                .ops            = &msc_iface_ops,
                .fs_desc        = &msc_bulk_fs_iface.iface,
                .fs_desc_size   = sizeof(msc_bulk_fs_iface),
#ifdef CONFIG_UDC_HIGH_SPEED
                .hs_desc        = &msc_bulk_hs_iface.iface,
                .hs_desc_size   = sizeof(msc_bulk_hs_iface),
#endif
        },
};

struct usb_func_iface *usb_msc_func_init(struct block_device *bdev)
{
        struct msc_func *msc = &msc_func;

        msc->bdev = bdev;

        build_assert(CONFIG_DMAPOOL_SMALL_OBJ_SIZE % 4 == 0);
        build_assert(MSC_DATA_BUFFER_SIZE % 512 == 0);

        msc->sense_data = dma_alloc(&msc->sense_data_phys, 32);
        msc_init_sense(msc, SCSI_SK_NO_SENSE,
                        SCSI_ASC_NO_ADDITIONAL_SENSE_INFO, 0);

        return &msc->iface;
}

---