[cdi-devel] [PATCH 5/5] USB MSD
Kevin Wolf ·
From: Max Reitz <max at tyndur.org> + Added USB mass storage driver (currently just bulk-only devices supported) Signed-off-by: Max Reitz <max at tyndur.org> Signed-off-by: Kevin Wolf <kevin at tyndur.org> --- usb-msd/include/msd.h | 121 ++++++++++++ usb-msd/main.c | 68 +++++++ usb-msd/msd.c | 485 +++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 674 insertions(+), 0 deletions(-) create mode 100644 usb-msd/include/msd.h create mode 100644 usb-msd/main.c create mode 100644 usb-msd/msd.c diff --git a/usb-msd/include/msd.h b/usb-msd/include/msd.h new file mode 100644 index 0000000..bb6cf51 --- /dev/null +++ b/usb-msd/include/msd.h @@ -0,0 +1,121 @@ +/***************************************************************************** +* Copyright (c) 2009 Max Reitz * +* * +* Permission is hereby granted, free of charge, to any person obtaining a * +* copy of this software and associated documentation files (the "Software"), * +* to deal in the Software without restriction, including without limitation * +* the rights to use, copy, modify, merge, publish, distribute, sublicense, * +* and/or sell copies of the Software, and to permit persons to whom the * +* Software is furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL * +* THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING * +* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER * +* DEALINGS IN THE SOFTWARE. * +*****************************************************************************/ + +#ifndef MSD_H +#define MSD_H + +#include <stdint.h> +#include <stdio.h> + +//#define DEBUG + +#ifdef DEBUG +#define d1printf(...) printf("[usb-msd] " __VA_ARGS__) +#define _d1printf(...) printf(__VA_ARGS__) +#else +#define d1printf(...) +#define _d1printf(...) +#endif + +#define d0printf(...) printf("[usb-msd] " __VA_ARGS__) +#define _d0printf(...) printf(__VA_ARGS__) + +//"USBC" +#define CBW_SIGNATURE 0x43425355 +struct command_block_wrapper +{ + uint32_t cbw_signature; + uint32_t cbw_tag; + uint32_t cbw_data_transfer_length; + uint8_t cbw_flags; + uint8_t cbw_lun; + uint8_t cbw_cb_length; + uint8_t cbw_cb[16]; +} __attribute__((packed)); + +//"USBS" +#define CSW_SIGNATURE 0x53425355 +struct command_status_wrapper +{ + uint32_t csw_signature; + uint32_t csw_tag; + uint32_t csw_data_residue; + uint8_t csw_status; +} __attribute__((packed)); + +struct msc_sense +{ + unsigned error : 7; + unsigned valid : 1; + uint8_t rsvd0; + unsigned sense_key : 4; + unsigned rsvd1 : 4; + uint32_t information; + uint8_t additional_length; + uint32_t rsvd2; + uint8_t additional_code; + uint8_t additional_code_qualifier; + uint32_t rsvd; +} __attribute__((packed)); + +struct msd_capacity +{ + uint32_t last_lba; + uint32_t block_length; +} __attribute__((packed)); + +struct cdi_msd +{ + struct cdi_storage_device cdi_device; + struct cdi_usb_device *usb_device; + struct cdi_usb_pipe *bulk_in; + struct cdi_usb_pipe *bulk_out; + uint32_t offset; //Für Partitionen +}; + +struct part_table_entry +{ + uint8_t active; + uint8_t begin_chs[3]; + uint8_t type; + uint8_t end_chs[3]; + uint32_t start; + uint32_t size; +} __attribute__((packed)); + + +#define MSC_CMD_REZERO 0x01 +#define MSC_CMD_SENSE 0x03 +#define MSC_CMD_CAPACITY 0x25 +#define MSC_CMD_READ10 0x28 +#define MSC_CMD_WRITE10 0x2A +#define MSC_CMD_SEEK 0x2B +#define MSC_CMD_READ12 0xA8 +#define MSC_CMD_WRITE12 0xAA + +int check_msd(struct cdi_usb_device *device); +int msd_read_blocks(struct cdi_storage_device *device, uint64_t start, + uint64_t count, void *buffer); +int msd_write_blocks(struct cdi_storage_device *device, uint64_t start, + uint64_t count, void *buffer); + +#endif diff --git a/usb-msd/main.c b/usb-msd/main.c new file mode 100644 index 0000000..c5c401f --- /dev/null +++ b/usb-msd/main.c @@ -0,0 +1,68 @@ +/***************************************************************************** +* Copyright (c) 2009 Max Reitz * +* * +* Permission is hereby granted, free of charge, to any person obtaining a * +* copy of this software and associated documentation files (the "Software"), * +* to deal in the Software without restriction, including without limitation * +* the rights to use, copy, modify, merge, publish, distribute, sublicense, * +* and/or sell copies of the Software, and to permit persons to whom the * +* Software is furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL * +* THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING * +* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER * +* DEALINGS IN THE SOFTWARE. * +*****************************************************************************/ + +#include <cdi/misc.h> +#include <cdi/storage.h> +#include <cdi/usb.h> + +#include "msd.h" + +#define DRIVER_NAME "usb-msd" + +static int init_msd(void); + +struct cdi_storage_driver msc_driver = { + .drv = { + .type = CDI_STORAGE, + .name = DRIVER_NAME, + .devices = NULL, + .init_device = NULL, + .remove_device = NULL, + .init = &init_msd, + .destroy = NULL + }, + .read_blocks = &msd_read_blocks, + .write_blocks = &msd_write_blocks +}; + +CDI_DRIVER(DRIVER_NAME, msc_driver) + +static struct cdi_usb_device_pattern msd_pattern = { + .vendor_id = -1, + .device_id = -1, + .class_id = 8, + .subclass_id = -1, + .protocol_id = -1, + + .take_usb_device = &check_msd +}; + +static int init_msd() +{ + cdi_init(); + + cdi_storage_driver_init(&msc_driver); + + cdi_usb_register_device_pattern(&msd_pattern); + + return 0; +} diff --git a/usb-msd/msd.c b/usb-msd/msd.c new file mode 100644 index 0000000..0385389 --- /dev/null +++ b/usb-msd/msd.c @@ -0,0 +1,485 @@ +/***************************************************************************** +* Copyright (c) 2009 Max Reitz * +* * +* Permission is hereby granted, free of charge, to any person obtaining a * +* copy of this software and associated documentation files (the "Software"), * +* to deal in the Software without restriction, including without limitation * +* the rights to use, copy, modify, merge, publish, distribute, sublicense, * +* and/or sell copies of the Software, and to permit persons to whom the * +* Software is furnished to do so, subject to the following conditions: * +* * +* The above copyright notice and this permission notice shall be included in * +* all copies or substantial portions of the Software. * +* * +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL * +* THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING * +* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER * +* DEALINGS IN THE SOFTWARE. * +*****************************************************************************/ + +#include <stdint.h> +#include <stdlib.h> +#include <string.h> + +#include <cdi/misc.h> +#include <cdi/storage.h> +#include <cdi/usb.h> +#include <cdi/usb-hcd.h> + +#include "msd.h" + +//TODO: CDI-Funktion wäre hier sicher nützlich... +#define CPU_IS_LITTLE_ENDIAN + +#define change_endianess_32(_) ((((_) & \ + 0xFF000000) >> \ + 24) | \ + (((_) & \ + 0xFF0000) >> \ + 8) | \ + (((_) & 0xFF00) << 8) | (((_) & 0xFF) << 24)) +#ifdef CPU_IS_LITTLE_ENDIAN +#define CPU2BE(_) change_endianess_32(_) +#define CPU2LE(_) (_) +#define BE2CPU(_) change_endianess_32(_) +#define LE2CPU(_) (_) +#else +#define CPU2BE(_) (_) +#define CPU2LE(_) change_endianess_32(_) +#define BE2CPU(_) (_) +#define LE2CPU(_) change_endianess_32(_) +#endif + +extern struct cdi_storage_driver msc_driver; +static uint32_t cbw_tag = 1; + +static int msd_cdi_read(struct cdi_storage_device *strgdev, uint64_t start, + uint64_t count, void *buffer); +static int msd_cdi_write(struct cdi_storage_device *strgdev, uint64_t start, + uint64_t count, void *buffer); + +int msd_read_blocks(struct cdi_storage_device *device, uint64_t start, + uint64_t count, void *buffer) +{ + return msd_cdi_read(device, start, count, buffer); +} + +int msd_write_blocks(struct cdi_storage_device *device, uint64_t start, + uint64_t count, void *buffer) +{ + return msd_cdi_write(device, start, count, buffer); +} + +static int msd_get_capacity(struct cdi_usb_device *usbdev, + uint32_t *block_size, uint32_t *block_count); + +int check_msd(struct cdi_usb_device *device) +{ + struct cdi_storage_device *strgdev; + struct cdi_usb_endpoint *ep_desc; + struct cdi_msd *cdimsd; + struct cdi_usb_hcd *hcd; + void *address; + int i; + uint32_t bs, bc; + uint64_t size; + static int msdnum = 0; + + //Nur Bulk-Only + if (device->protocol_id != 0x50) { + return 0; + } + + d0printf("MSD detected.\n"); + + cdimsd = malloc(sizeof(struct cdi_msd)); + if (cdimsd == NULL) { + return 0; + } + cdimsd->usb_device = device; + cdimsd->offset = 0; + strgdev = (struct cdi_storage_device *)cdimsd; + strgdev->dev.name = malloc(10); + if (strgdev->dev.name == NULL) { + free(strgdev); + return 0; + } + sprintf((char *)strgdev->dev.name, "usb-msd%i", msdnum++); + strgdev->dev.driver = (struct cdi_driver *)&msc_driver; + device->driver_data = cdimsd; + cdimsd->bulk_in = malloc(sizeof(*cdimsd->bulk_in)); + cdimsd->bulk_in->device = device; + cdimsd->bulk_in->endpoint = NULL; + cdimsd->bulk_in->data_toggle = 0; + cdimsd->bulk_out = malloc(sizeof(*cdimsd->bulk_out)); + cdimsd->bulk_out->device = device; + cdimsd->bulk_out->endpoint = NULL; + cdimsd->bulk_out->data_toggle = 0; + address = (void *)((uintptr_t)device->interface_desc + + sizeof(struct cdi_usb_interface)); + for (i = 0; i < device->interface_desc->num_endpoints; i++) { + ep_desc = address; + if ((ep_desc->endpoint_address & 0x80) && + (ep_desc->attributes == 0x02) && + (cdimsd->bulk_in->endpoint == NULL)) + { + //BULK-IN + cdimsd->bulk_in->endpoint = ep_desc; + } + else if (!(ep_desc->endpoint_address & 0x80) && + (ep_desc->attributes == 0x02) && + (cdimsd->bulk_out->endpoint == NULL)) + { + //BULK-OUT + cdimsd->bulk_out->endpoint = ep_desc; + } + address = (void *)((uintptr_t)address + + sizeof(struct cdi_usb_endpoint)); + } + if ((cdimsd->bulk_in->endpoint == NULL) || + (cdimsd->bulk_out->endpoint == NULL)) + { + d0printf("Not enough endpoints found.\n"); + return 0; + } + hcd = (struct cdi_usb_hcd *)device->hc->dev.driver; + hcd->add_pipe(cdimsd->bulk_in); + hcd->add_pipe(cdimsd->bulk_out); + if (!msd_get_capacity(device, &bs, &bc)) { + strgdev->block_size = 0; + strgdev->block_count = 0; + d1printf("Wasn't able to determine size of %s.\n", strgdev->dev.name); + } else { + strgdev->block_size = bs; + strgdev->block_count = bc; + size = bs; + size *= bc; + d1printf("%s: %i * %i B (about %d MB).\n", strgdev->dev.name, bc, bs, + (int)(size >> 20)); + } + cdi_storage_device_init(strgdev); + cdi_list_push(msc_driver.drv.devices, strgdev); + + return 1; +} + +static int write_cmd(struct cdi_usb_device *usbdev, void *src) +{ + struct cdi_msd *cdimsd = usbdev->driver_data; + struct cdi_usb_packet cmd_packet = { + .pipe = cdimsd->bulk_out, + .type = CDI_USB_PACKET_OUT, + .data = src, + .length = 0x1F, + .use_toggle = TOGGLE_UNSPECIFIC + }; + + return usbdev->drv->send_packets(&cmd_packet, 1); +} + +/** + * Liest den Status von einem MSD + * + * @param usbdev Das bewusste Gerät + * @param expected_tag Das erwartete Tag + * + * @return Wenn der Status OK ist USB_NO_ERROR, sonst entsprechender Fehler + */ + +static int read_status(struct cdi_usb_device *usbdev, uint32_t expected_tag) +{ + struct cdi_msd *msc = usbdev->driver_data; + struct command_status_wrapper _csw; + struct command_status_wrapper *csw = &_csw; + cdi_usb_status_t error; + + struct cdi_usb_packet status_packet = { + .pipe = msc->bulk_in, + .type = CDI_USB_PACKET_IN, + .data = csw, + .length = 0x0D, + .use_toggle = TOGGLE_UNSPECIFIC + }; + + error = usbdev->drv->send_packets(&status_packet, 1); + if (error != CDI_USB_NO_ERROR) { + return error; + } + if ((csw->csw_signature != CSW_SIGNATURE) || + (csw->csw_tag != expected_tag) || csw->csw_status) + { + d1printf("0x%08X %i==%i 0x%08X\n", csw->csw_signature, csw->csw_tag, + expected_tag, csw->csw_status); + return CDI_USB_TRIVIAL_ERROR; + } + + return CDI_USB_NO_ERROR; +} + +static int msd_get_capacity(struct cdi_usb_device *usbdev, + uint32_t *block_size, uint32_t *block_count) +{ + struct cdi_msd *msc; + struct command_block_wrapper _cbw; + struct command_block_wrapper *cbw = &_cbw; + struct msd_capacity _cap; + struct msd_capacity *cap = &_cap; + uint32_t expected_tag; + + if ((usbdev == NULL) || (block_size == NULL) || (block_count == NULL)) { + return 0; + } + + msc = usbdev->driver_data; + memset(cbw, 0, 0x1F); + cbw->cbw_signature = CBW_SIGNATURE; + cbw->cbw_tag = (expected_tag = cbw_tag++); + cbw->cbw_data_transfer_length = sizeof(struct msd_capacity); + cbw->cbw_flags = 0x80; //IN + cbw->cbw_lun = 0; //Was weiß ich, wie viele LUNs datt Dingens hat? + cbw->cbw_cb_length = 12; + cbw->cbw_cb[0] = MSC_CMD_CAPACITY; + cbw->cbw_cb[1] = 0; //LUN: 0 + + if (write_cmd(usbdev, cbw) != CDI_USB_NO_ERROR) { + return 0; + } + + struct cdi_usb_packet in_packet = { + .pipe = msc->bulk_in, + .type = CDI_USB_PACKET_IN, + .data = cap, + .length = sizeof(*cap), + .use_toggle = TOGGLE_UNSPECIFIC + }; + + if (usbdev->drv->send_packets(&in_packet, 1) != CDI_USB_NO_ERROR) { + return 0; + } + + if (read_status(usbdev, expected_tag) != CDI_USB_NO_ERROR) { + return 0; + } + + *block_size = BE2CPU(cap->block_length); + *block_count = BE2CPU(cap->last_lba) + 1; + + return 1; +} + +#ifdef WAIT_FOR_MSD_READY +static int msd_ready(struct cdi_usb_device *usbdev) +{ + struct msclass_data *msc; + struct command_block_wrapper _cbw; + struct command_block_wrapper *cbw = &_cbw; + uint32_t expected_tag; + + msc = (struct msclass_data *)usbdev->driver_data; + memset(cbw, 0, 0x1F); + cbw->cbw_signature = CBW_SIGNATURE; + cbw->cbw_tag = (expected_tag = cbw_tag++); + cbw->cbw_data_transfer_length = 0; + cbw->cbw_flags = 0x80; //IN + cbw->cbw_lun = 0; //Was weiß ich, wie viele LUNs datt Dingens hat? + cbw->cbw_cb_length = 12; //Alles null, also "Test unit ready" + + if (write_cmd(usbdev, cbw) != USB_NO_ERROR) { + return 0; + } + + if (read_status(usbdev, expected_tag) != USB_NO_ERROR) { + return 0; + } + + return 1; +} +#endif + +#define MAX_ACCESS_BLOCKS 32 + +static uint32_t msd_read(struct cdi_usb_device *usbdev, uint32_t lba, + uint16_t sectors, void *buffer, size_t length); + +static int msd_cdi_read(struct cdi_storage_device *strgdev, uint64_t start, + uint64_t count, void *buffer) +{ + int bs = strgdev->block_size, error; +#ifdef WAIT_FOR_MSD_READY + int i; +#endif + struct cdi_usb_device *usbdev = ((struct cdi_msd *)strgdev)->usb_device; + int j, bbs; + + d1printf("read(%i, %i)\n", (int)start, (int)count); + start += ((struct cdi_msd *)strgdev)->offset; + + if (!count) { + d1printf("Empty read request.\n"); + return 0; + } + for (j = 0; j < (int)count; j += MAX_ACCESS_BLOCKS) { +#ifdef WAIT_FOR_MSD_READY + for (i = 0; !msd_ready(usbdev) && (i < 10); i++) { + cdi_sleep_ms(20); + } +#endif + bbs = (count - j > MAX_ACCESS_BLOCKS) ? MAX_ACCESS_BLOCKS : count - j; + error = msd_read(usbdev, start + j, bbs, + (void *)((uintptr_t)buffer + j * bs), bbs * bs); + if (error != CDI_USB_NO_ERROR) { + d0printf("Read error 0x%X at block %lld.\n", error, start + j); + return -1; + } + } + return 0; +} + +static uint32_t msd_read(struct cdi_usb_device *usbdev, uint32_t lba, + uint16_t sectors, void *buffer, size_t length) +{ + struct cdi_msd *msc; + struct command_block_wrapper _cbw; + struct command_block_wrapper *cbw = &_cbw; + uint32_t expected_tag; + int error; + + if ((buffer == NULL) || !length) { + return CDI_USB_TRIVIAL_ERROR; + } + msc = usbdev->driver_data; + memset(cbw, 0, 0x1F); + cbw->cbw_signature = CBW_SIGNATURE; + cbw->cbw_tag = (expected_tag = cbw_tag++); + cbw->cbw_data_transfer_length = length; + cbw->cbw_flags = 0x80; //IN + cbw->cbw_lun = 0; //Was weiß ich, wie viele LUNs datt Dingens hat? + cbw->cbw_cb_length = 12; + cbw->cbw_cb[0] = MSC_CMD_READ10; + cbw->cbw_cb[1] = 0; //LUN: 0 + cbw->cbw_cb[2] = (lba & 0xFF000000) >> 24; + cbw->cbw_cb[3] = (lba & 0x00FF0000) >> 16; + cbw->cbw_cb[4] = (lba & 0x0000FF00) >> 8; + cbw->cbw_cb[5] = lba & 0x000000FF; + cbw->cbw_cb[7] = (sectors & 0x0000FF00) >> 8; + cbw->cbw_cb[8] = sectors & 0x000000FF; + + error = write_cmd(usbdev, cbw); + if (error != CDI_USB_NO_ERROR) { + return error; + } + + struct cdi_usb_packet in_packet = { + .pipe = msc->bulk_in, + .type = CDI_USB_PACKET_IN, + .data = buffer, + .length = length, + .use_toggle = TOGGLE_UNSPECIFIC + }; + + error = usbdev->drv->send_packets(&in_packet, 1); + if (error != CDI_USB_NO_ERROR) { + return error; + } + + error = read_status(usbdev, expected_tag); + if (error != CDI_USB_NO_ERROR) { + return error; + } + + return CDI_USB_NO_ERROR; +} + +static uint32_t msd_write(struct cdi_usb_device *usbdev, uint32_t lba, + uint16_t sectors, void *buffer, size_t length); + +static int msd_cdi_write(struct cdi_storage_device *strgdev, uint64_t start, + uint64_t count, void *buffer) +{ + int bs = strgdev->block_size, error; +#ifdef WAIT_FOR_MSD_READY + int i; +#endif + struct cdi_usb_device *usbdev = ((struct cdi_msd *)strgdev)->usb_device; + int j, bbs; + + d1printf("write(%i, %i)\n", (int)start, (int)count); + start += ((struct cdi_msd *)strgdev)->offset; + + if (!count) { + d1printf("Empty write request.\n"); + return 0; + } + for (j = 0; j < (int)count; j += MAX_ACCESS_BLOCKS) { +#ifdef WAIT_FOR_MSD_READY + for (i = 0; !msd_ready(usbdev) && (i < 10); i++) + cdi_sleep_ms(20); +#endif + bbs = (count - j > MAX_ACCESS_BLOCKS) ? MAX_ACCESS_BLOCKS : count - j; + error = msd_write(usbdev, start + j, bbs, + (void *)((uintptr_t)buffer + j * bs), bbs * bs); + if (error != CDI_USB_NO_ERROR) { + d0printf("Write error 0x%X at block %lld.\n", error, start + j); + return -1; + } + } + return 0; +} + +static uint32_t msd_write(struct cdi_usb_device *usbdev, uint32_t lba, + uint16_t sectors, void *buffer, size_t length) +{ + struct cdi_msd *msc; + struct command_block_wrapper _cbw; + struct command_block_wrapper* cbw = &_cbw; + uint32_t expected_tag; + int error; + + if ((buffer == NULL) || !length) { + return CDI_USB_TRIVIAL_ERROR; + } + msc = usbdev->driver_data; + memset(cbw, 0, 0x1F); + cbw->cbw_signature = CBW_SIGNATURE; + cbw->cbw_tag = (expected_tag = cbw_tag++); + cbw->cbw_data_transfer_length = length; + cbw->cbw_flags = 0x00; //OUT + cbw->cbw_lun = 0; //Was weiß ich, wie viele LUNs datt Dingens hat? + cbw->cbw_cb_length = 12; + cbw->cbw_cb[0] = MSC_CMD_WRITE10; + cbw->cbw_cb[1] = 0; //LUN: 0 + cbw->cbw_cb[2] = (lba & 0xFF000000) >> 24; + cbw->cbw_cb[3] = (lba & 0x00FF0000) >> 16; + cbw->cbw_cb[4] = (lba & 0x0000FF00) >> 8; + cbw->cbw_cb[5] = lba & 0x000000FF; + cbw->cbw_cb[7] = (sectors & 0x0000FF00) >> 8; + cbw->cbw_cb[8] = sectors & 0x000000FF; + + error = write_cmd(usbdev, cbw); + if (error != CDI_USB_NO_ERROR) { + return error; + } + + struct cdi_usb_packet out_packet = { + .pipe = msc->bulk_out, + .type = CDI_USB_PACKET_OUT, + .data = buffer, + .length = length, + .use_toggle = TOGGLE_UNSPECIFIC + }; + + error = usbdev->drv->send_packets(&out_packet, 1); + if (error != CDI_USB_NO_ERROR) { + return error; + } + + error = read_status(usbdev, expected_tag); + if (error != CDI_USB_NO_ERROR) { + return error; + } + + return CDI_USB_NO_ERROR; +} -- 1.6.0.2