[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]

[cdi-devel] [PATCH] USB mass storage device driver



From: Max Reitz <max@xxxxxxxxxx>

+ Added USB mass storage device driver (currently just for "bulk
  only")

Signed-off-by: Max Reitz <max@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..e29d679
--- /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 <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.type = CDI_STORAGE;
+    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.4.2