diff options
| -rw-r--r-- | payloads/libpayload/drivers/usb/usbmsc.c | 41 | ||||
| -rw-r--r-- | payloads/libpayload/include/usb/usbmsc.h | 7 |
2 files changed, 28 insertions, 20 deletions
diff --git a/payloads/libpayload/drivers/usb/usbmsc.c b/payloads/libpayload/drivers/usb/usbmsc.c index 3b5206ef85..62428b6ebe 100644 --- a/payloads/libpayload/drivers/usb/usbmsc.c +++ b/payloads/libpayload/drivers/usb/usbmsc.c @@ -170,10 +170,10 @@ reset_transport (usbdev_t *dev) } /* device may stall this command, so beware! */ -static int -get_max_luns (usbdev_t *dev) +static void +initialize_luns (usbdev_t *dev) { - unsigned char luns = 75; + usbmsc_inst_t *msc = MSC_INST (dev); dev_req_t dr; dr.bmRequestType = 0; dr.data_dir = device_to_host; @@ -185,23 +185,24 @@ get_max_luns (usbdev_t *dev) dr.wValue = 0; dr.wIndex = 0; dr.wLength = 1; - if (dev->controller->control (dev, IN, sizeof (dr), &dr, 1, &luns) < 0) - luns = 0; // assume only 1 lun if req fails - return luns; + if (dev->controller->control (dev, IN, sizeof (dr), &dr, + sizeof (msc->num_luns), &msc->num_luns) < 0) + msc->num_luns = 0; /* assume only 1 lun if req fails */ + msc->num_luns++; /* Get Max LUN returns number of last LUN */ + msc->lun = 0; } unsigned int tag; -unsigned char lun = 0; static void wrap_cbw (cbw_t *cbw, int datalen, cbw_direction dir, const u8 *cmd, - int cmdlen) + int cmdlen, u8 lun) { memset (cbw, 0, sizeof (cbw_t)); cbw->dCBWSignature = cbw_signature; cbw->dCBWTag = ++tag; - cbw->bCBWLUN = lun; // static value per device + cbw->bCBWLUN = lun; cbw->dCBWDataTransferLength = datalen; cbw->bmCBWFlags = dir; @@ -236,7 +237,7 @@ execute_command (usbdev_t *dev, cbw_direction dir, const u8 *cb, int cblen, if ((cb[0] == 0x1b) && (cb[4] == 1)) { //start command, always succeed always_succeed = 1; } - wrap_cbw (&cbw, buflen, dir, cb, cblen); + wrap_cbw (&cbw, buflen, dir, cb, cblen, MSC_INST (dev)->lun); if (dev->controller-> bulk (MSC_INST (dev)->bulk_out, sizeof (cbw), (u8 *) &cbw, 0) < 0) { return reset_transport (dev); @@ -623,7 +624,6 @@ usb_msc_init (usbdev_t *dev) if (!dev->data) fatal("Not enough memory for USB MSC device.\n"); - MSC_INST (dev)->protocol = interface->bInterfaceSubClass; MSC_INST (dev)->bulk_in = 0; MSC_INST (dev)->bulk_out = 0; MSC_INST (dev)->usbdisk_created = 0; @@ -655,7 +655,8 @@ usb_msc_init (usbdev_t *dev) MSC_INST (dev)->bulk_in->endpoint, MSC_INST (dev)->bulk_out->endpoint); - usb_debug (" has %d luns\n", get_max_luns (dev) + 1); + initialize_luns (dev); + usb_debug (" has %d luns\n", MSC_INST (dev)->num_luns); /* Test if unit is ready (nothing to do if it isn't). */ if (usb_msc_test_unit_ready (dev) != USB_MSC_READY) @@ -668,16 +669,22 @@ usb_msc_init (usbdev_t *dev) static void usb_msc_poll (usbdev_t *dev) { - int prev_ready = MSC_INST (dev)->ready; + usbmsc_inst_t *msc = MSC_INST (dev); + int prev_ready = msc->ready; if (usb_msc_test_unit_ready (dev) == USB_MSC_DETACHED) return; - if (!prev_ready && MSC_INST (dev)->ready) { - usb_debug ("usb msc: not ready -> ready\n"); + if (!prev_ready && msc->ready) { + usb_debug ("usb msc: not ready -> ready (lun %d)\n", msc->lun); usb_msc_create_disk (dev); - } else if (prev_ready && !MSC_INST (dev)->ready) { - usb_debug ("usb msc: ready -> not ready\n"); + } else if (prev_ready && !msc->ready) { + usb_debug ("usb msc: ready -> not ready (lun %d)\n", msc->lun); usb_msc_remove_disk (dev); + } else if (!prev_ready && !msc->ready) { + u8 new_lun = (msc->lun + 1) % msc->num_luns; + usb_debug("usb msc: not ready (lun %d) -> lun %d\n", msc->lun, + new_lun); + msc->lun = new_lun; } } diff --git a/payloads/libpayload/include/usb/usbmsc.h b/payloads/libpayload/include/usb/usbmsc.h index 8930156619..f4562a56d5 100644 --- a/payloads/libpayload/include/usb/usbmsc.h +++ b/payloads/libpayload/include/usb/usbmsc.h @@ -32,11 +32,12 @@ typedef struct { unsigned int blocksize; unsigned int numblocks; - unsigned int protocol; endpoint_t *bulk_in; endpoint_t *bulk_out; - int usbdisk_created; - int ready; + u8 usbdisk_created; + s8 ready; + u8 lun; + u8 num_luns; void *data; /* For use by consumers of libpayload. */ } usbmsc_inst_t; |