[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[cdi-devel] [PATCH 5/5] USB MSD
From: Max Reitz <max@xxxxxxxxxx>
+ Added USB mass storage driver (currently just bulk-only devices
supported)
Signed-off-by: Max Reitz <max@xxxxxxxxxx>
Signed-off-by: Kevin Wolf <kevin@xxxxxxxxxx>
---
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