Diff: USBHostMSD/USBHostMSD.cpp

Revision:

4:b320d68e98e7

Parent:

0:a554658735bf

Child:

5:e48791a1ef18

diff -r 0f5c32575eb8 -r b320d68e98e7 USBHostMSD/USBHostMSD.cpp
--- a/USBHostMSD/USBHostMSD.cpp	Wed Mar 06 17:50:07 2013 +0000
+++ b/USBHostMSD/USBHostMSD.cpp	Tue Mar 12 17:23:37 2013 +0000
@@ -28,7 +28,8 @@
 #define DEVICE_TO_HOST  0x80
 #define HOST_TO_DEVICE  0x00
 
-#define GET_MAX_LUN     (0xFE)
+#define GET_MAX_LUN             (0xFE)
+#define BO_MASS_STORAGE_RESET   (0xFF)
 
 USBHostMSD::USBHostMSD(const char * rootdir) : FATFileSystem(rootdir)
 {
@@ -48,6 +49,7 @@
     msd_device_found = false;
     disk_init = false;
     dev_connected = false;
+    nb_ep = 0;
 }
 
 
@@ -67,6 +69,8 @@
     for (i = 0; i < MAX_DEVICE_CONNECTED; i++) {
         if ((dev = host->getDevice(i)) != NULL) {
             
+            USB_DBG("Trying to connect MSD device\r\n");
+            
             if(host->enumerate(dev, this))
                 break;
 
@@ -75,10 +79,10 @@
                 bulk_out = dev->getEndpoint(msd_intf, BULK_ENDPOINT, OUT);
                 
                 if (!bulk_in || !bulk_out)
-                    break;
+                    continue;
                 
-                USB_INFO("New MSD device: VID:%04x PID:%04x [dev: %p]", dev->getVid(), dev->getPid(), dev);
-                dev->setName("MSD");
+                USB_INFO("New MSD device: VID:%04x PID:%04x [dev: %p - intf: %d]", dev->getVid(), dev->getPid(), dev, msd_intf);
+                dev->setName("MSD", msd_intf);
                 host->registerDriver(dev, msd_intf, this, &USBHostMSD::init);
 
                 dev_connected = true;
@@ -86,6 +90,7 @@
             }
         } //if()
     } //for()
+    init();
     return false;
 }
 
@@ -110,7 +115,9 @@
 {
     if (intf_nb == msd_intf) {
         if (type == BULK_ENDPOINT) {
-            msd_device_found = true;
+            nb_ep++;
+            if (nb_ep == 2)
+                msd_device_found = true;
             return true;
         }
     }
@@ -119,43 +126,49 @@
 
 
 int USBHostMSD::testUnitReady() {
+    USB_DBG("Test unit ready");
     return SCSITransfer(NULL, 6, DEVICE_TO_HOST, 0, 0);
 }
 
+
 int USBHostMSD::readCapacity() {
+    USB_DBG("Read capacity");
     uint8_t cmd[10] = {0x25,0,0,0,0,0,0,0,0,0};
     uint8_t result[8];
     int status = SCSITransfer(cmd, 10, DEVICE_TO_HOST, result, 8);
     if (status == 0) {
         blockCount = (result[0] << 24) | (result[1] << 16) | (result[2] << 8) | result[3];
         blockSize = (result[4] << 24) | (result[5] << 16) | (result[6] << 8) | result[7];
-        USB_INFO("MSD [dev: %p] - blockCount: %lld, blockSize: %d\r\n", dev, blockCount, blockSize);
+        USB_INFO("MSD [dev: %p] - blockCount: %lld, blockSize: %d, Capacity: %lld\r\n", dev, blockCount, blockSize, blockCount*blockSize);
     }
     return status;
 }
 
 
 int USBHostMSD::SCSIRequestSense() {
-    uint8_t cmd[5] = {0x03,0,0,0,0x13};
-    uint8_t result[19];
-    int status = SCSITransfer(cmd, 5, DEVICE_TO_HOST, result, 19);
+    USB_DBG("Request sense");
+    uint8_t cmd[6] = {0x03,0,0,0,18,0};
+    uint8_t result[18];
+    int status = SCSITransfer(cmd, 6, DEVICE_TO_HOST, result, 18);
     return status;
 }
 
 
 int USBHostMSD::inquiry(uint8_t lun, uint8_t page_code) {
+    USB_DBG("Inquiry");
     uint8_t evpd = (page_code == 0) ? 0 : 1;
     uint8_t cmd[6] = {0x12, (lun << 5) | evpd, page_code, 0, 36, 0};
     uint8_t result[36];
     int status = SCSITransfer(cmd, 6, DEVICE_TO_HOST, result, 36);
     if (status == 0) {
-        char vid_pid[9];
+        char vid_pid[17];
         memcpy(vid_pid, &result[8], 8);
         vid_pid[8] = 0;
         USB_INFO("MSD [dev: %p] - Vendor ID: %s", dev, vid_pid);
 
-        memcpy(vid_pid, &result[16], 8);
-        USB_INFO("MSD [dev: %p] - Produc ID: %s", dev, vid_pid);
+        memcpy(vid_pid, &result[16], 16);
+        vid_pid[16] = 0;
+        USB_INFO("MSD [dev: %p] - Product ID: %s", dev, vid_pid);
 
         memcpy(vid_pid, &result[32], 4);
         vid_pid[4] = 0;
@@ -170,10 +183,7 @@
         res = host->controlWrite(   dev,
                                     USB_RECIPIENT_ENDPOINT | USB_HOST_TO_DEVICE | USB_REQUEST_TYPE_STANDARD,
                                     CLEAR_FEATURE,
-                                    0,
-                                    ep->getAddress(),
-                                    NULL,
-                                    0);
+                                    0, ep->getAddress(), NULL, 0);
         // set state to IDLE if clear feature successful
         if (res == USB_TYPE_OK) {
             ep->setState(USB_TYPE_IDLE);
@@ -203,14 +213,16 @@
     }
 
     // send the cbw
+    USB_DBG("Send CBW");
     res = host->bulkWrite(dev, bulk_out,(uint8_t *)&cbw, 31);
     if (checkResult(res, bulk_out))
         return -1;
 
     // data stage if needed
     if (data) {
+        USB_DBG("data stage");
         if (flags == HOST_TO_DEVICE) {
-
+            
             res = host->bulkWrite(dev, bulk_out, data, transfer_len);
             if (checkResult(res, bulk_out))
                 return -1;
@@ -225,6 +237,7 @@
 
     // status stage
     csw.Signature = 0;
+    USB_DBG("Read CSW");
     res = host->bulkRead(dev, bulk_in,(uint8_t *)&csw, 13);
     if (checkResult(res, bulk_in))
         return -1;
@@ -232,12 +245,37 @@
     if (csw.Signature != CSW_SIGNATURE) {
         return -1;
     }
+    
+    USB_DBG("recv csw: status: %d", csw.Status);
 
     // ModeSense?
-    if ((csw.Status == 1) && (cmd[0] != 0x03))
+    if ((csw.Status == 1) && (cmd[0] != 0x03)) {
+        USB_DBG("request mode sense");
         return SCSIRequestSense();
+    }
+    
+    // perform reset recovery
+    if ((csw.Status == 2) && (cmd[0] != 0x03)) {
+        
+        // send Bulk-Only Mass Storage Reset request
+        res = host->controlWrite(   dev,
+                                    USB_RECIPIENT_INTERFACE | USB_HOST_TO_DEVICE | USB_REQUEST_TYPE_CLASS,
+                                    BO_MASS_STORAGE_RESET,
+                                    0, msd_intf, NULL, 0);
+        
+        // unstall both endpoints
+        res = host->controlWrite(   dev,
+                                    USB_RECIPIENT_ENDPOINT | USB_HOST_TO_DEVICE | USB_REQUEST_TYPE_STANDARD,
+                                    CLEAR_FEATURE,
+                                    0, bulk_in->getAddress(), NULL, 0);
+        
+        res = host->controlWrite(   dev,
+                                    USB_RECIPIENT_ENDPOINT | USB_HOST_TO_DEVICE | USB_REQUEST_TYPE_STANDARD,
+                                    CLEAR_FEATURE,
+                                    0, bulk_out->getAddress(), NULL, 0);
+        
+    }
 
-    USB_DBG("recv csw: status: %d", csw.Status);
     return csw.Status;
 }
 
@@ -258,12 +296,22 @@
     return SCSITransfer(cmd, 10, direction, buf, blockSize*nbBlock);
 }
 
+int USBHostMSD::getMaxLun() {
+    uint8_t buf[1], res;
+    res = host->controlRead(    dev, USB_RECIPIENT_INTERFACE | USB_DEVICE_TO_HOST | USB_REQUEST_TYPE_CLASS,
+                                0xfe, 0, msd_intf, buf, 1);
+    USB_DBG("max lun: %d", buf[0]);
+    return res;
+}
 
 int USBHostMSD::disk_initialize() {
     USB_DBG("FILESYSTEM: init");
-    U8 i, timeout = 10;
+    U16 i, timeout = 10;
+    
+    getMaxLun();
     
     for (i = 0; i < timeout; i++) {
+        Thread::wait(100);
         if (!testUnitReady())
             break;
     }
@@ -272,6 +320,7 @@
         disk_init = false;
         return -1;
     }
+    
     inquiry(0, 0);
     disk_init = 1;
     return readCapacity();
@@ -308,4 +357,3 @@
 }
 
 #endif
-