WebSVN – freemyipod – Diff – /apps/diskmode/main.c

 //
 //
 //    Copyright 2013 TheSeven
+//    Copyright 2014 user890104
 //
 //
 //    This file is part of emCORE.
 //
 //    emCORE is free software: you can redistribute it and/or
 #define DEVICE_REMOVABLE            0x80
 #define SCSI_FORMAT_CAPACITY_FORMATTED_MEDIA 0x02000000
-#define RAMDISK_SECTORSIZE 512
+struct storage_info storage_info;
-#define RAMDISK_SECTORS 2048
+enum storage_operation {
+    OP_READ,
+    OP_WRITE
+} pending_op;
+unsigned long pending_op_start;
+int pending_op_count;
+struct wakeup pending_op_wakeup;
 static const struct usb_devicedescriptor usb_devicedescriptor =
+{
     .bLength = sizeof(struct usb_devicedescriptor),
     .bDescriptorType = USB_DESCRIPTOR_TYPE_DEVICE,
     .wString = { 'f', 'r', 'e', 'e', 'm', 'y', 'i', 'p', 'o', 'd', '.', 'o', 'r', 'g' },
 };
 static const struct usb_stringdescriptor usb_string_product =
+{
-    .bLength = sizeof(usb_string_product) + sizeof(*usb_string_product.wString) * 28,
+    .bLength = sizeof(usb_string_product) + sizeof(*usb_string_product.wString) * 16,
     .bDescriptorType = USB_DESCRIPTOR_TYPE_STRING,
-    .wString = { 'e', 'm', 'C', 'O', 'R', 'E', ' ', 'U', 'S', 'B', ' ', 'm', 'a', 's', 's', ' ', 's', 't', 'o', 'r', 'a', 'g', 'e', ' ', 't', 'e', 's', 't' },
+    .wString = { 'e', 'm', 'C', 'O', 'R', 'E', ' ', 'D', 'i', 's', 'k', ' ', 'm', 'o', 'd', 'e' },
 };
 static const struct usb_stringdescriptor* usb_stringdescriptors[] =
+{
     &usb_string_language,
 static int maxlen;
 static int length;
 static bool locked;
 static const struct usb_instance* usb;
 static volatile bool ejected = false;
-static uint8_t __attribute__((aligned(32))) ramdisk[RAMDISK_SECTORS][RAMDISK_SECTORSIZE];
+static uint8_t __attribute__((aligned(32))) storage_buffer[0x10000];
 static void listen()
+{
     usb_start_rx(usb, outep, &cbw, sizeof(cbw));
+}
         cur_sense_data.sense_key = SENSE_MEDIUM_ERROR;
         cur_sense_data.asc = ASC_READ_ERROR;
         cur_sense_data.ascq = 0;
         return;
+    }
-    send_block_data(ramdisk[cur_cmd.sector], MIN(maxlen, cur_cmd.count * RAMDISK_SECTORSIZE));
-    cur_cmd.sector += maxlen / RAMDISK_SECTORSIZE;
+    pending_op_start = cur_cmd.sector;
-    cur_cmd.count -= MIN(cur_cmd.count, maxlen / RAMDISK_SECTORSIZE);
+    pending_op_count = cur_cmd.count;
+    pending_op = OP_READ;
+    wakeup_signal(&pending_op_wakeup);
+}
 static void copy_padded(char* dest, char* src, int len)
+{
    int i = 0;
+}
 static void fill_inquiry()
+{
     memset(&tb.inquiry, 0, sizeof(tb.inquiry));
-    copy_padded(tb.inquiry.VendorId, "emCORE", sizeof(tb.inquiry.VendorId));
+    copy_padded(tb.inquiry.VendorId, storage_info.vendor, sizeof(tb.inquiry.VendorId));
-    copy_padded(tb.inquiry.ProductId, "UMS Test", sizeof(tb.inquiry.ProductId));
+    copy_padded(tb.inquiry.ProductId, storage_info.product, sizeof(tb.inquiry.ProductId));
-    copy_padded(tb.inquiry.ProductRevisionLevel, "0.1", sizeof(tb.inquiry.ProductRevisionLevel));
+    copy_padded(tb.inquiry.ProductRevisionLevel, storage_info.revision, sizeof(tb.inquiry.ProductRevisionLevel));
     tb.inquiry.DeviceType = DIRECT_ACCESS_DEVICE;
     tb.inquiry.AdditionalLength = 0x1f;
     tb.inquiry.Versions = 4;  // SPC-2
     tb.inquiry.Format = 2;  // SPC-2/3 inquiry format
                     tb.ms_data_10.block_descriptor_length = htobe16(sizeof(tb.ms_data_10.block_descriptor));
                     memset(tb.ms_data_10.block_descriptor.reserved, 0, 4);
                     memset(tb.ms_data_10.block_descriptor.num_blocks, 0, 8);
-                    tb.ms_data_10.block_descriptor.num_blocks[4] = (RAMDISK_SECTORS & 0xff000000) >> 24;
+                    tb.ms_data_10.block_descriptor.num_blocks[4] = (storage_info.num_sectors & 0xff000000) >> 24;
-                    tb.ms_data_10.block_descriptor.num_blocks[5] = (RAMDISK_SECTORS & 0x00ff0000) >> 16;
+                    tb.ms_data_10.block_descriptor.num_blocks[5] = (storage_info.num_sectors & 0x00ff0000) >> 16;
-                    tb.ms_data_10.block_descriptor.num_blocks[6] = (RAMDISK_SECTORS & 0x0000ff00) >> 8;
+                    tb.ms_data_10.block_descriptor.num_blocks[6] = (storage_info.num_sectors & 0x0000ff00) >> 8;
-                    tb.ms_data_10.block_descriptor.num_blocks[7] = (RAMDISK_SECTORS & 0x000000ff);
+                    tb.ms_data_10.block_descriptor.num_blocks[7] = (storage_info.num_sectors & 0x000000ff);
-                    tb.ms_data_10.block_descriptor.block_size[0] = (RAMDISK_SECTORSIZE & 0xff000000) >> 24;
+                    tb.ms_data_10.block_descriptor.block_size[0] = (storage_info.sector_size & 0xff000000) >> 24;
-                    tb.ms_data_10.block_descriptor.block_size[1] = (RAMDISK_SECTORSIZE & 0x00ff0000) >> 16;
+                    tb.ms_data_10.block_descriptor.block_size[1] = (storage_info.sector_size & 0x00ff0000) >> 16;
-                    tb.ms_data_10.block_descriptor.block_size[2] = (RAMDISK_SECTORSIZE & 0x0000ff00) >> 8;
+                    tb.ms_data_10.block_descriptor.block_size[2] = (storage_info.sector_size & 0x0000ff00) >> 8;
-                    tb.ms_data_10.block_descriptor.block_size[3] = (RAMDISK_SECTORSIZE & 0x000000ff);
+                    tb.ms_data_10.block_descriptor.block_size[3] = (storage_info.sector_size & 0x000000ff);
                     send_command_result(&tb.ms_data_10, MIN(sizeof(tb.ms_data_10), length));
                     break;
                 default:
                     send_command_failed_result();
                     cur_sense_data.sense_key = SENSE_ILLEGAL_REQUEST;
                     tb.ms_data_6.medium_type = 0;
                     tb.ms_data_6.device_specific = 0;
                     tb.ms_data_6.block_descriptor_length = sizeof(tb.ms_data_6.block_descriptor);
                     tb.ms_data_6.block_descriptor.density_code = 0;
                     tb.ms_data_6.block_descriptor.reserved = 0;
-                    if (RAMDISK_SECTORS > 0xffffff)
+                    if (storage_info.num_sectors > 0xffffff)
+                    {
                         tb.ms_data_6.block_descriptor.num_blocks[0] = 0xff;
                         tb.ms_data_6.block_descriptor.num_blocks[1] = 0xff;
                         tb.ms_data_6.block_descriptor.num_blocks[2] = 0xff;
+                    }
                     else
+                    {
-                        tb.ms_data_6.block_descriptor.num_blocks[0] = (RAMDISK_SECTORS & 0xff0000) >> 16;
+                        tb.ms_data_6.block_descriptor.num_blocks[0] = (storage_info.num_sectors & 0xff0000) >> 16;
-                        tb.ms_data_6.block_descriptor.num_blocks[1] = (RAMDISK_SECTORS & 0x00ff00) >> 8;
+                        tb.ms_data_6.block_descriptor.num_blocks[1] = (storage_info.num_sectors & 0x00ff00) >> 8;
-                        tb.ms_data_6.block_descriptor.num_blocks[2] = (RAMDISK_SECTORS & 0x0000ff);
+                        tb.ms_data_6.block_descriptor.num_blocks[2] = (storage_info.num_sectors & 0x0000ff);
+                    }
-                    tb.ms_data_6.block_descriptor.block_size[0] = (RAMDISK_SECTORSIZE & 0xff0000) >> 16;
+                    tb.ms_data_6.block_descriptor.block_size[0] = (storage_info.sector_size & 0xff0000) >> 16;
-                    tb.ms_data_6.block_descriptor.block_size[1] = (RAMDISK_SECTORSIZE & 0x00ff00) >> 8;
+                    tb.ms_data_6.block_descriptor.block_size[1] = (storage_info.sector_size & 0x00ff00) >> 8;
-                    tb.ms_data_6.block_descriptor.block_size[2] = (RAMDISK_SECTORSIZE & 0x0000ff);
+                    tb.ms_data_6.block_descriptor.block_size[2] = (storage_info.sector_size & 0x0000ff);
                     send_command_result(&tb.ms_data_6, MIN(sizeof(tb.ms_data_6), length));
                     break;
                 default:
                     send_command_failed_result();
                     cur_sense_data.sense_key = SENSE_ILLEGAL_REQUEST;
         case SCSI_READ_FORMAT_CAPACITY:
+        {
             if (!ejected)
+            {
                 tb.format_capacity_data.following_length = 0x08000000;
-                tb.format_capacity_data.block_count = htobe32(RAMDISK_SECTORS - 1);
+                tb.format_capacity_data.block_count = htobe32(storage_info.num_sectors - 1);
-                tb.format_capacity_data.block_size = htobe32(RAMDISK_SECTORSIZE);
+                tb.format_capacity_data.block_size = htobe32(storage_info.sector_size);
                 tb.format_capacity_data.block_size |= htobe32(SCSI_FORMAT_CAPACITY_FORMATTED_MEDIA);
                 send_command_result(&tb.format_capacity_data, MIN(sizeof(tb.format_capacity_data), length));
+           }
            else
+           {
         case SCSI_READ_CAPACITY:
+        {
             if (!ejected)
+            {
-                tb.capacity_data.block_count = htobe32(RAMDISK_SECTORS - 1);
+                tb.capacity_data.block_count = htobe32(storage_info.num_sectors - 1);
-                tb.capacity_data.block_size = htobe32(RAMDISK_SECTORSIZE);
+                tb.capacity_data.block_size = htobe32(storage_info.sector_size);
                 send_command_result(&tb.capacity_data, MIN(sizeof(tb.capacity_data), length));
+            }
             else
+            {
                 send_command_failed_result();
             cur_cmd.sector = (cbw->command_block[2] << 24 | cbw->command_block[3] << 16
                             | cbw->command_block[4] << 8 | cbw->command_block[5]);
             cur_cmd.count = (cbw->command_block[7] << 8 | cbw->command_block[8]);
             cur_cmd.orig_count = cur_cmd.count;
-            if ((cur_cmd.sector + cur_cmd.count) > RAMDISK_SECTORS)
+            if ((cur_cmd.sector + cur_cmd.count) > storage_info.num_sectors)
+            {
                 send_csw(1);
                 cur_sense_data.sense_key = SENSE_ILLEGAL_REQUEST;
                 cur_sense_data.asc = ASC_LBA_OUT_OF_RANGE;
                 cur_sense_data.ascq = 0;
             cur_cmd.sector = (cbw->command_block[2] << 24 | cbw->command_block[3] << 16
                             | cbw->command_block[4] << 8 | cbw->command_block[5]);
             cur_cmd.count = (cbw->command_block[7] << 8 | cbw->command_block[8]);
             cur_cmd.orig_count = cur_cmd.count;
-            if ((cur_cmd.sector + cur_cmd.count) > RAMDISK_SECTORS)
+            if ((cur_cmd.sector + cur_cmd.count) > storage_info.num_sectors)
+            {
                 send_csw(1);
                 cur_sense_data.sense_key = SENSE_ILLEGAL_REQUEST;
                 cur_sense_data.asc = ASC_LBA_OUT_OF_RANGE;
                 cur_sense_data.ascq = 0;
+            }
-            else
+            else {
-                receive_block_data(ramdisk[cur_cmd.sector], MIN(maxlen, cur_cmd.count * RAMDISK_SECTORSIZE));
+                receive_block_data(storage_buffer, MIN(maxlen, cur_cmd.count * storage_info.sector_size));
+            }
             break;
         case SCSI_WRITE_BUFFER:
             break;
+{
     length -= bytesleft;
     switch (state)
+    {
         case RECEIVING_BLOCKS:
-            if (length != RAMDISK_SECTORSIZE * cur_cmd.count && length != maxlen) break;
+            if (length != storage_info.sector_size * cur_cmd.count && length != maxlen) break;
-            cur_cmd.sector += (maxlen / RAMDISK_SECTORSIZE);
-            cur_cmd.count -= MIN(cur_cmd.count, maxlen / RAMDISK_SECTORSIZE);
-            if (cur_cmd.count)
+            pending_op_start = cur_cmd.sector;
-                receive_block_data(ramdisk[cur_cmd.sector], MIN(maxlen, cur_cmd.count * RAMDISK_SECTORSIZE));
+            pending_op_count = cur_cmd.count;
-            else send_csw(0);
+            pending_op = OP_WRITE;
+            wakeup_signal(&pending_op_wakeup);
             break;
         case WAITING_FOR_CSW_COMPLETION:
             state = WAITING_FOR_COMMAND;
             listen();
             break;
     &usb_c1,
 };
 static void main(int argc, const char** argv)
+{
+    storage_get_info(0, &storage_info);
+    if (!storage_info.sector_size) panicf(PANIC_KILLTHREAD, "Sector size is zero!\n");
+    if (!storage_info.num_sectors) panicf(PANIC_KILLTHREAD, "Number of sectors is zero!\n");
+    disk_unmount(0);
     int i;
     uint32_t eps = usbmanager_get_available_endpoints();
     int ep_out = 0;
     int ep_in = 0;
     for (i = 1; i < 16; i++)
     usb_c1_i0_a0_ep1in.number.number = ep_in;
     outep.number = ep_out;
     inep.number = ep_in;
     int rc = usbmanager_install_custom(&usb_devicedescriptor, ARRAYLEN(usb_configurations), usb_configurations,
                                        ARRAYLEN(usb_stringdescriptors), usb_stringdescriptors, true);
+    if (IS_ERR(rc)) {
+        disk_mount(0);
-    if (IS_ERR(rc)) panicf(PANIC_KILLTHREAD, "Failed to register USB handler: %08X\n", rc);
+        panicf(PANIC_KILLTHREAD, "Failed to register USB handler: %08X\n", rc);
+    }
+    wakeup_init(&pending_op_wakeup);
-    while (!ejected && usbmanager_get_connected()) sleep(200000);
+    while (!ejected && usbmanager_get_connected()) {
+        if (wakeup_wait(&pending_op_wakeup, 200000) == THREAD_OK) {
+            if (pending_op == OP_READ) {
+                //cprintf(3, "R(%u, %u)\n", pending_op_start, pending_op_count);
+                storage_read_sectors_md(0, pending_op_start, pending_op_count, storage_buffer);
+                send_block_data(storage_buffer, MIN(maxlen, cur_cmd.count * storage_info.sector_size));
+            }
+            if (pending_op == OP_WRITE) {
+                //cprintf(3, "W(%u, %u)\n", pending_op_start, pending_op_count);
+                storage_write_sectors_md(0, pending_op_start, pending_op_count, storage_buffer);
+            }
+            cur_cmd.sector += maxlen / storage_info.sector_size;
+            cur_cmd.count -= MIN(cur_cmd.count, maxlen / storage_info.sector_size);
+            if (pending_op == OP_WRITE) {
+                if (cur_cmd.count) {
+                    receive_block_data(storage_buffer, MIN(maxlen, cur_cmd.count * storage_info.sector_size));
+                }
+                else send_csw(0);
+            }
+        }
+    }
     usbmanager_uninstall_custom();
+    disk_mount(0);
+}
-EMCORE_APP_HEADER("USB mass storage test", main, 127)
+EMCORE_APP_HEADER("Disk mode", main, 127)

Subversion Repositories freemyipod

(root)/apps/diskmode/main.c – Rev 913 → 914