/* usbms.c - USB Mass Storage Support.  */
/*
 *  VAS_EBOOT  --  GRand Unified Bootloader
 *  Copyright (C) 2008  Free Software Foundation, Inc.
 *
 *  VAS_EBOOT is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  VAS_EBOOT is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with VAS_EBOOT.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <VasEBoot/dl.h>
#include <VasEBoot/mm.h>
#include <VasEBoot/usb.h>
#include <VasEBoot/scsi.h>
#include <VasEBoot/scsicmd.h>
#include <VasEBoot/misc.h>

VAS_EBOOT_MOD_LICENSE ("GPLv3+");

#define VAS_EBOOT_USBMS_DIRECTION_BIT	7

/* Length of CBI command should be always 12 bytes */
#define VAS_EBOOT_USBMS_CBI_CMD_SIZE         12
/* CBI class-specific USB request ADSC - it sends CBI (scsi) command to
 * device in DATA stage */
#define VAS_EBOOT_USBMS_CBI_ADSC_REQ         0x00

/* The USB Mass Storage Command Block Wrapper.  */
struct VasEBoot_usbms_cbw
{
  VasEBoot_uint32_t signature;
  VasEBoot_uint32_t tag;
  VasEBoot_uint32_t transfer_length;
  VasEBoot_uint8_t flags;
  VasEBoot_uint8_t lun;
  VasEBoot_uint8_t length;
  VasEBoot_uint8_t cbwcb[16];
} VAS_EBOOT_PACKED;

struct VasEBoot_usbms_csw
{
  VasEBoot_uint32_t signature;
  VasEBoot_uint32_t tag;
  VasEBoot_uint32_t residue;
  VasEBoot_uint8_t status;
} VAS_EBOOT_PACKED;

struct VasEBoot_usbms_dev
{
  struct VasEBoot_usb_device *dev;

  int luns;

  int config;
  int interface;
  struct VasEBoot_usb_desc_endp *in;
  struct VasEBoot_usb_desc_endp *out;

  int subclass;
  int protocol;
  struct VasEBoot_usb_desc_endp *intrpt;
};
typedef struct VasEBoot_usbms_dev *VasEBoot_usbms_dev_t;

/* FIXME: remove limit.  */
#define MAX_USBMS_DEVICES 128
static VasEBoot_usbms_dev_t VasEBoot_usbms_devices[MAX_USBMS_DEVICES];
static int first_available_slot = 0;

static VasEBoot_usb_err_t
VasEBoot_usbms_cbi_cmd (VasEBoot_usb_device_t dev, int interface,
                    VasEBoot_uint8_t *cbicb)
{
  return VasEBoot_usb_control_msg (dev,
                               VAS_EBOOT_USB_REQTYPE_CLASS_INTERFACE_OUT,
                               VAS_EBOOT_USBMS_CBI_ADSC_REQ, 0, interface,
                               VAS_EBOOT_USBMS_CBI_CMD_SIZE, (char*)cbicb);
}

static VasEBoot_usb_err_t
VasEBoot_usbms_cbi_reset (VasEBoot_usb_device_t dev, int interface)
{
  /* Prepare array with Command Block Reset (=CBR) */
  /* CBI specific communication reset command should be send to device
   * via CBI USB class specific request ADCS */
  struct VasEBoot_cbi_reset
    {
      VasEBoot_uint8_t opcode; /* 0x1d = SEND DIAGNOSTIC */
      VasEBoot_uint8_t lun; /* 7-5 LUN, 4-0 flags - for CBR always = 0x04 */
      VasEBoot_uint8_t pad[10];
      /* XXX: There is collision between CBI and UFI specifications:
       *      CBI says 0xff, UFI says 0x00 ... probably it does
       *      not matter ... (?) */
    } cbicb = { 0x1d, 0x04,
                { 0xff, 0xff, 0xff, 0xff, 0xff,
                  0xff, 0xff, 0xff, 0xff, 0xff }
              };

  return VasEBoot_usbms_cbi_cmd (dev, interface, (VasEBoot_uint8_t *)&cbicb);
}

static VasEBoot_usb_err_t
VasEBoot_usbms_bo_reset (VasEBoot_usb_device_t dev, int interface)
{
  return VasEBoot_usb_control_msg (dev, 0x21, 255, 0, interface, 0, 0);
}

static VasEBoot_usb_err_t
VasEBoot_usbms_reset (VasEBoot_usbms_dev_t dev)
{
  if (dev->protocol == VAS_EBOOT_USBMS_PROTOCOL_BULK)
    return VasEBoot_usbms_bo_reset (dev->dev, dev->interface);
  else
    return VasEBoot_usbms_cbi_reset (dev->dev, dev->interface);
}

static void
VasEBoot_usbms_detach (VasEBoot_usb_device_t usbdev, int config, int interface)
{
  unsigned i;
  for (i = 0; i < ARRAY_SIZE (VasEBoot_usbms_devices); i++)
    if (VasEBoot_usbms_devices[i] && VasEBoot_usbms_devices[i]->dev == usbdev
	&& VasEBoot_usbms_devices[i]->interface == interface
	&& VasEBoot_usbms_devices[i]->config == config)
      {
	VasEBoot_free (VasEBoot_usbms_devices[i]);
	VasEBoot_usbms_devices[i] = 0;
      }
}

static int
VasEBoot_usbms_attach (VasEBoot_usb_device_t usbdev, int configno, int interfno)
{
  struct VasEBoot_usb_desc_if *interf
    = usbdev->config[configno].interf[interfno].descif;
  int j;
  VasEBoot_uint8_t luns = 0;
  unsigned curnum;
  VasEBoot_usb_err_t err = VAS_EBOOT_USB_ERR_NONE;

  VasEBoot_boot_time ("Attaching USB mass storage");

  if (first_available_slot == ARRAY_SIZE (VasEBoot_usbms_devices))
    return 0;

  curnum = first_available_slot;
  first_available_slot++;

  interf = usbdev->config[configno].interf[interfno].descif;

  if ((interf->subclass != VAS_EBOOT_USBMS_SUBCLASS_BULK
       /* Experimental support of RBC, MMC-2, UFI, SFF-8070i devices */
       && interf->subclass != VAS_EBOOT_USBMS_SUBCLASS_RBC
       && interf->subclass != VAS_EBOOT_USBMS_SUBCLASS_MMC2
       && interf->subclass != VAS_EBOOT_USBMS_SUBCLASS_UFI
       && interf->subclass != VAS_EBOOT_USBMS_SUBCLASS_SFF8070 )
      || (interf->protocol != VAS_EBOOT_USBMS_PROTOCOL_BULK
          && interf->protocol != VAS_EBOOT_USBMS_PROTOCOL_CBI
          && interf->protocol != VAS_EBOOT_USBMS_PROTOCOL_CB))
    return 0;

  VasEBoot_usbms_devices[curnum] = VasEBoot_zalloc (sizeof (struct VasEBoot_usbms_dev));
  if (! VasEBoot_usbms_devices[curnum])
    return 0;

  VasEBoot_usbms_devices[curnum]->dev = usbdev;
  VasEBoot_usbms_devices[curnum]->interface = interfno;
  VasEBoot_usbms_devices[curnum]->subclass = interf->subclass;
  VasEBoot_usbms_devices[curnum]->protocol = interf->protocol;

  VasEBoot_dprintf ("usbms", "alive\n");

  /* Iterate over all endpoints of this interface, at least a
     IN and OUT bulk endpoint are required.  */
  for (j = 0; j < interf->endpointcnt; j++)
    {
      struct VasEBoot_usb_desc_endp *endp;
      endp = &usbdev->config[0].interf[interfno].descendp[j];

      if ((endp->endp_addr & 128) && (endp->attrib & 3) == 2)
	/* Bulk IN endpoint.  */
	VasEBoot_usbms_devices[curnum]->in = endp;
      else if (!(endp->endp_addr & 128) && (endp->attrib & 3) == 2)
        /* Bulk OUT endpoint.  */
	VasEBoot_usbms_devices[curnum]->out = endp;
      else if ((endp->endp_addr & 128) && (endp->attrib & 3) == 3)
        /* Interrupt (IN) endpoint.  */
	VasEBoot_usbms_devices[curnum]->intrpt = endp;
    }

  if (!VasEBoot_usbms_devices[curnum]->in || !VasEBoot_usbms_devices[curnum]->out
      || ((VasEBoot_usbms_devices[curnum]->protocol == VAS_EBOOT_USBMS_PROTOCOL_CBI)
          && !VasEBoot_usbms_devices[curnum]->intrpt))
    {
      VasEBoot_free (VasEBoot_usbms_devices[curnum]);
      VasEBoot_usbms_devices[curnum] = 0;
      return 0;
    }

  VasEBoot_dprintf ("usbms", "alive\n");

  /* XXX: Activate the first configuration.  */
  VasEBoot_usb_set_configuration (usbdev, 1);

  /* Query the amount of LUNs.  */
  if (VasEBoot_usbms_devices[curnum]->protocol == VAS_EBOOT_USBMS_PROTOCOL_BULK)
    { /* Only Bulk only devices support Get Max LUN command */
      err = VasEBoot_usb_control_msg (usbdev, 0xA1, 254, 0, interfno, 1, (char *) &luns);

      if (err)
        {
          /* In case of a stall, clear the stall.  */
          if (err == VAS_EBOOT_USB_ERR_STALL)
	    {
	      VasEBoot_usb_clear_halt (usbdev, VasEBoot_usbms_devices[curnum]->in->endp_addr);
	      VasEBoot_usb_clear_halt (usbdev, VasEBoot_usbms_devices[curnum]->out->endp_addr);
	    }
          /* Just set the amount of LUNs to one.  */
          VasEBoot_errno = VAS_EBOOT_ERR_NONE;
          VasEBoot_usbms_devices[curnum]->luns = 1;
        }
      else
        /* luns = 0 means one LUN with ID 0 present ! */
        /* We get from device not number of LUNs but highest
         * LUN number. LUNs are numbered from 0,
         * i.e. number of LUNs is luns+1 ! */
        VasEBoot_usbms_devices[curnum]->luns = luns + 1;
    }
  else
    /* XXX: Does CBI devices support multiple LUNs ?
     * I.e., should we detect number of device's LUNs ? (How?) */
    VasEBoot_usbms_devices[curnum]->luns = 1;

  VasEBoot_dprintf ("usbms", "alive\n");

  usbdev->config[configno].interf[interfno].detach_hook = VasEBoot_usbms_detach;

  VasEBoot_boot_time ("Attached USB mass storage");

#if 0 /* All this part should be probably deleted.
       * This make trouble on some devices if they are not in
       * Phase Error state - and there they should be not in such state...
       * Bulk only mass storage reset procedure should be used only
       * on place and in time when it is really necessary. */
  /* Reset recovery procedure */
  /* Bulk-Only Mass Storage Reset, after the reset commands
     will be accepted.  */
  VasEBoot_usbms_reset (usbdev, i);
  VasEBoot_usb_clear_halt (usbdev, usbms->in->endp_addr);
  VasEBoot_usb_clear_halt (usbdev, usbms->out->endp_addr);
#endif

  return 1;
}



static int
VasEBoot_usbms_iterate (VasEBoot_scsi_dev_iterate_hook_t hook, void *hook_data,
		    VasEBoot_disk_pull_t pull)
{
  unsigned i;

  if (pull != VAS_EBOOT_DISK_PULL_NONE)
    return 0;

  VasEBoot_usb_poll_devices (1);

  for (i = 0; i < ARRAY_SIZE (VasEBoot_usbms_devices); i++)
    if (VasEBoot_usbms_devices[i])
      {
	if (hook (VAS_EBOOT_SCSI_SUBSYSTEM_USBMS, i, VasEBoot_usbms_devices[i]->luns,
		  hook_data))
	  return 1;
      }

  return 0;
}

static VasEBoot_err_t
VasEBoot_usbms_transfer_bo (struct VasEBoot_scsi *scsi, VasEBoot_size_t cmdsize, char *cmd,
		        VasEBoot_size_t size, char *buf, int read_write)
{
  struct VasEBoot_usbms_cbw cbw;
  VasEBoot_usbms_dev_t dev = (VasEBoot_usbms_dev_t) scsi->data;
  struct VasEBoot_usbms_csw status;
  static VasEBoot_uint32_t tag = 0;
  VasEBoot_usb_err_t err = VAS_EBOOT_USB_ERR_NONE;
  VasEBoot_usb_err_t errCSW = VAS_EBOOT_USB_ERR_NONE;
  int retrycnt = 3 + 1;

  tag++;

 retry:
  retrycnt--;
  if (retrycnt == 0)
    return VasEBoot_error (VAS_EBOOT_ERR_IO, "USB Mass Storage stalled");

  /* Setup the request.  */
  VasEBoot_memset (&cbw, 0, sizeof (cbw));
  cbw.signature = VasEBoot_cpu_to_le32_compile_time (0x43425355);
  cbw.tag = tag;
  cbw.transfer_length = VasEBoot_cpu_to_le32 (size);
  cbw.flags = (!read_write) << VAS_EBOOT_USBMS_DIRECTION_BIT;
  cbw.lun = scsi->lun; /* In USB MS CBW are LUN bits on another place than in SCSI CDB, both should be set correctly. */
  cbw.length = cmdsize;
  VasEBoot_memcpy (cbw.cbwcb, cmd, cmdsize);

  /* Debug print of CBW content. */
  VasEBoot_dprintf ("usb", "CBW: sign=0x%08x tag=0x%08x len=0x%08x\n",
  	cbw.signature, cbw.tag, cbw.transfer_length);
  VasEBoot_dprintf ("usb", "CBW: flags=0x%02x lun=0x%02x CB_len=0x%02x\n",
  	cbw.flags, cbw.lun, cbw.length);
  VasEBoot_dprintf ("usb", "CBW: cmd:\n %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\n",
  	cbw.cbwcb[ 0], cbw.cbwcb[ 1], cbw.cbwcb[ 2], cbw.cbwcb[ 3],
  	cbw.cbwcb[ 4], cbw.cbwcb[ 5], cbw.cbwcb[ 6], cbw.cbwcb[ 7],
  	cbw.cbwcb[ 8], cbw.cbwcb[ 9], cbw.cbwcb[10], cbw.cbwcb[11],
  	cbw.cbwcb[12], cbw.cbwcb[13], cbw.cbwcb[14], cbw.cbwcb[15]);

  /* Write the request.
   * XXX: Error recovery is maybe still not fully correct. */
  err = VasEBoot_usb_bulk_write (dev->dev, dev->out,
			     sizeof (cbw), (char *) &cbw);
  if (err)
    {
      if (err == VAS_EBOOT_USB_ERR_STALL)
	{
	  VasEBoot_usb_clear_halt (dev->dev, dev->out->endp_addr);
	  goto CheckCSW;
	}
      goto retry;
    }

  /* Read/write the data, (maybe) according to specification.  */
  if (size && (read_write == 0))
    {
      err = VasEBoot_usb_bulk_read (dev->dev, dev->in, size, buf);
      VasEBoot_dprintf ("usb", "read: %d %d\n", err, VAS_EBOOT_USB_ERR_STALL);
      if (err)
        {
          if (err == VAS_EBOOT_USB_ERR_STALL)
	    VasEBoot_usb_clear_halt (dev->dev, dev->in->endp_addr);
          goto CheckCSW;
        }
      /* Debug print of received data. */
      VasEBoot_dprintf ("usb", "buf:\n");
      if (size <= 64)
	{
	  unsigned i;
	  for (i = 0; i < size; i++)
	    VasEBoot_dprintf ("usb", "0x%02x: 0x%02x\n", i, buf[i]);
	}
      else
          VasEBoot_dprintf ("usb", "Too much data for debug print...\n");
    }
  else if (size)
    {
      err = VasEBoot_usb_bulk_write (dev->dev, dev->out, size, buf);
      VasEBoot_dprintf ("usb", "write: %d %d\n", err, VAS_EBOOT_USB_ERR_STALL);
      VasEBoot_dprintf ("usb", "First 16 bytes of sent data:\n %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\n",
  	buf[ 0], buf[ 1], buf[ 2], buf[ 3],
  	buf[ 4], buf[ 5], buf[ 6], buf[ 7],
  	buf[ 8], buf[ 9], buf[10], buf[11],
  	buf[12], buf[13], buf[14], buf[15]);
      if (err)
        {
          if (err == VAS_EBOOT_USB_ERR_STALL)
	    VasEBoot_usb_clear_halt (dev->dev, dev->out->endp_addr);
          goto CheckCSW;
        }
      /* Debug print of sent data. */
      if (size <= 256)
	{
	  unsigned i;
	  for (i=0; i<size; i++)
	    VasEBoot_dprintf ("usb", "0x%02x: 0x%02x\n", i, buf[i]);
	}
      else
          VasEBoot_dprintf ("usb", "Too much data for debug print...\n");
    }

  /* Read the status - (maybe) according to specification.  */
CheckCSW:
  errCSW = VasEBoot_usb_bulk_read (dev->dev, dev->in,
		    sizeof (status), (char *) &status);
  if (errCSW)
    {
      VasEBoot_usb_clear_halt (dev->dev, dev->in->endp_addr);
      errCSW = VasEBoot_usb_bulk_read (dev->dev, dev->in,
			        sizeof (status), (char *) &status);
      if (errCSW)
        { /* Bulk-only reset device. */
          VasEBoot_dprintf ("usb", "Bulk-only reset device - errCSW\n");
          VasEBoot_usbms_reset (dev);
          VasEBoot_usb_clear_halt (dev->dev, dev->in->endp_addr);
          VasEBoot_usb_clear_halt (dev->dev, dev->out->endp_addr);
	  goto retry;
        }
    }

  /* Debug print of CSW content. */
  VasEBoot_dprintf ("usb", "CSW: sign=0x%08x tag=0x%08x resid=0x%08x\n",
  	status.signature, status.tag, status.residue);
  VasEBoot_dprintf ("usb", "CSW: status=0x%02x\n", status.status);

  /* If phase error or not valid signature, do bulk-only reset device. */
  if ((status.status == 2) ||
      (status.signature != VasEBoot_cpu_to_le32_compile_time(0x53425355)))
    { /* Bulk-only reset device. */
      VasEBoot_dprintf ("usb", "Bulk-only reset device - bad status\n");
      VasEBoot_usbms_reset (dev);
      VasEBoot_usb_clear_halt (dev->dev, dev->in->endp_addr);
      VasEBoot_usb_clear_halt (dev->dev, dev->out->endp_addr);

      goto retry;
    }

  /* If "command failed" status or data transfer failed -> error */
  if ((status.status || err) && !read_write)
    return VasEBoot_error (VAS_EBOOT_ERR_READ_ERROR,
		       "error communication with USB Mass Storage device");
  else if ((status.status || err) && read_write)
    return VasEBoot_error (VAS_EBOOT_ERR_WRITE_ERROR,
		       "error communication with USB Mass Storage device");

  return VAS_EBOOT_ERR_NONE;
}

static VasEBoot_err_t
VasEBoot_usbms_transfer_cbi (struct VasEBoot_scsi *scsi, VasEBoot_size_t cmdsize, char *cmd,
		        VasEBoot_size_t size, char *buf, int read_write)
{
  VasEBoot_usbms_dev_t dev = (VasEBoot_usbms_dev_t) scsi->data;
  int retrycnt = 3 + 1;
  VasEBoot_usb_err_t err = VAS_EBOOT_USB_ERR_NONE;
  VasEBoot_uint8_t cbicb[VAS_EBOOT_USBMS_CBI_CMD_SIZE];
  VasEBoot_uint16_t status;

 retry:
  retrycnt--;
  if (retrycnt == 0)
    return VasEBoot_error (VAS_EBOOT_ERR_IO, "USB Mass Storage CBI failed");

  /* Setup the request.  */
  VasEBoot_memset (cbicb, 0, sizeof (cbicb));
  VasEBoot_memcpy (cbicb, cmd,
               cmdsize >= VAS_EBOOT_USBMS_CBI_CMD_SIZE
                 ? VAS_EBOOT_USBMS_CBI_CMD_SIZE
                 : cmdsize);

  /* Debug print of CBIcb content. */
  VasEBoot_dprintf ("usb", "cbicb:\n %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\n",
  	cbicb[ 0], cbicb[ 1], cbicb[ 2], cbicb[ 3],
  	cbicb[ 4], cbicb[ 5], cbicb[ 6], cbicb[ 7],
  	cbicb[ 8], cbicb[ 9], cbicb[10], cbicb[11]);

  /* Write the request.
   * XXX: Error recovery is maybe not correct. */
  err = VasEBoot_usbms_cbi_cmd (dev->dev, dev->interface, cbicb);
  if (err)
    {
      VasEBoot_dprintf ("usb", "CBI cmdcb setup err=%d\n", err);
      if (err == VAS_EBOOT_USB_ERR_STALL)
	{
	  /* Stall in this place probably means bad or unsupported
	   * command, so we will not try it again. */
         return VasEBoot_error (VAS_EBOOT_ERR_IO, "USB Mass Storage CBI request failed");
	}
      else if (dev->protocol == VAS_EBOOT_USBMS_PROTOCOL_CBI)
        {
          /* Try to get status from interrupt pipe */
          err = VasEBoot_usb_bulk_read (dev->dev, dev->intrpt,
                                    2, (char*)&status);
          VasEBoot_dprintf ("usb", "CBI cmdcb setup status: err=%d, status=0x%x\n", err, status);
        }
        /* Any other error could be transport problem, try it again */
        goto retry;
    }

  /* Read/write the data, (maybe) according to specification.  */
  if (size && (read_write == 0))
    {
      err = VasEBoot_usb_bulk_read (dev->dev, dev->in, size, buf);
      VasEBoot_dprintf ("usb", "read: %d\n", err);
      if (err)
        {
          if (err == VAS_EBOOT_USB_ERR_STALL)
            VasEBoot_usb_clear_halt (dev->dev, dev->in->endp_addr);
          goto retry;
        }
    }
  else if (size)
    {
      err = VasEBoot_usb_bulk_write (dev->dev, dev->out, size, buf);
      VasEBoot_dprintf ("usb", "write: %d\n", err);
      if (err)
        {
          if (err == VAS_EBOOT_USB_ERR_STALL)
	    VasEBoot_usb_clear_halt (dev->dev, dev->out->endp_addr);
          goto retry;
        }
    }

  /* XXX: It is not clear to me yet, how to check status of CBI
   * data transfer on devices without interrupt pipe.
   * AFAIK there is probably no status phase to indicate possibly
   * bad transported data.
   * Maybe we should do check on higher level, i.e. issue RequestSense
   * command (we do it already in scsi.c) and check returned values
   * (we do not it yet) - ? */
  if (dev->protocol == VAS_EBOOT_USBMS_PROTOCOL_CBI)
    { /* Check status in interrupt pipe */
      err = VasEBoot_usb_bulk_read (dev->dev, dev->intrpt,
                                2, (char*)&status);
      VasEBoot_dprintf ("usb", "read status: %d\n", err);
      if (err)
        {
          /* Try to reset device, because it is probably not standard
           * situation */
          VasEBoot_usbms_reset (dev);
          VasEBoot_usb_clear_halt (dev->dev, dev->in->endp_addr);
          VasEBoot_usb_clear_halt (dev->dev, dev->out->endp_addr);
          VasEBoot_usb_clear_halt (dev->dev, dev->intrpt->endp_addr);
          goto retry;
        }
      if (dev->subclass == VAS_EBOOT_USBMS_SUBCLASS_UFI)
        {
          /* These devices should return bASC and bASCQ */
          if (status != 0)
            /* Some error, currently we don't care what it is... */
            goto retry;
        }
      else if (dev->subclass == VAS_EBOOT_USBMS_SUBCLASS_RBC)
        {
          /* XXX: I don't understand what returns RBC subclass devices,
           * so I don't check it - maybe somebody helps ? */
        }
      else
        {
          /* Any other device should return bType = 0 and some bValue */
          if (status & 0xff)
            return VasEBoot_error (VAS_EBOOT_ERR_IO, "USB Mass Storage CBI status type != 0");
          status = (status & 0x0300) >> 8;
          switch (status)
            {
              case 0 : /* OK */
                break;
              case 1 : /* Fail */
                goto retry;
                break;
              case 2 : /* Phase error */
              case 3 : /* Persistent Failure */
                VasEBoot_dprintf ("usb", "CBI reset device - phase error or persistent failure\n");
                VasEBoot_usbms_reset (dev);
                VasEBoot_usb_clear_halt (dev->dev, dev->in->endp_addr);
                VasEBoot_usb_clear_halt (dev->dev, dev->out->endp_addr);
                VasEBoot_usb_clear_halt (dev->dev, dev->intrpt->endp_addr);
                goto retry;
                break;
            }
        }
    }

  if (err)
    return VasEBoot_error (VAS_EBOOT_ERR_IO, "USB error %d", err);

  return VAS_EBOOT_ERR_NONE;
}


static VasEBoot_err_t
VasEBoot_usbms_transfer (struct VasEBoot_scsi *scsi, VasEBoot_size_t cmdsize, char *cmd,
		        VasEBoot_size_t size, char *buf, int read_write)
{
  VasEBoot_usbms_dev_t dev = (VasEBoot_usbms_dev_t) scsi->data;

  if (dev->protocol == VAS_EBOOT_USBMS_PROTOCOL_BULK)
    return VasEBoot_usbms_transfer_bo (scsi, cmdsize, cmd, size, buf,
                                   read_write);
  else
    return VasEBoot_usbms_transfer_cbi (scsi, cmdsize, cmd, size, buf,
                                    read_write);
}

static VasEBoot_err_t
VasEBoot_usbms_read (struct VasEBoot_scsi *scsi, VasEBoot_size_t cmdsize, char *cmd,
		 VasEBoot_size_t size, char *buf)
{
  return VasEBoot_usbms_transfer (scsi, cmdsize, cmd, size, buf, 0);
}

static VasEBoot_err_t
VasEBoot_usbms_write (struct VasEBoot_scsi *scsi, VasEBoot_size_t cmdsize, char *cmd,
		  VasEBoot_size_t size, const char *buf)
{
  return VasEBoot_usbms_transfer (scsi, cmdsize, cmd, size, (char *) buf, 1);
}

static VasEBoot_err_t
VasEBoot_usbms_open (int id, int devnum, struct VasEBoot_scsi *scsi)
{
  if (id != VAS_EBOOT_SCSI_SUBSYSTEM_USBMS)
    return VasEBoot_error (VAS_EBOOT_ERR_UNKNOWN_DEVICE,
		       "not USB Mass Storage device");

  if (!VasEBoot_usbms_devices[devnum])
    VasEBoot_usb_poll_devices (1);

  if (!VasEBoot_usbms_devices[devnum])
    return VasEBoot_error (VAS_EBOOT_ERR_UNKNOWN_DEVICE,
		       "unknown USB Mass Storage device");

  scsi->data = VasEBoot_usbms_devices[devnum];
  scsi->luns = VasEBoot_usbms_devices[devnum]->luns;

  return VAS_EBOOT_ERR_NONE;
}

static struct VasEBoot_scsi_dev VasEBoot_usbms_dev =
  {
    .iterate = VasEBoot_usbms_iterate,
    .open = VasEBoot_usbms_open,
    .read = VasEBoot_usbms_read,
    .write = VasEBoot_usbms_write
  };

static struct VasEBoot_usb_attach_desc attach_hook =
{
  .class = VAS_EBOOT_USB_CLASS_MASS_STORAGE,
  .hook = VasEBoot_usbms_attach
};

VAS_EBOOT_MOD_INIT(usbms)
{
  VasEBoot_usb_register_attach_hook_class (&attach_hook);
  VasEBoot_scsi_dev_register (&VasEBoot_usbms_dev);
}

VAS_EBOOT_MOD_FINI(usbms)
{
  unsigned i;
  for (i = 0; i < ARRAY_SIZE (VasEBoot_usbms_devices); i++)
    {
      VasEBoot_usbms_devices[i]->dev->config[VasEBoot_usbms_devices[i]->config]
	.interf[VasEBoot_usbms_devices[i]->interface].detach_hook = 0;
      VasEBoot_usbms_devices[i]->dev->config[VasEBoot_usbms_devices[i]->config]
	.interf[VasEBoot_usbms_devices[i]->interface].attached = 0;
    }
  VasEBoot_usb_unregister_attach_hook_class (&attach_hook);
  VasEBoot_scsi_dev_unregister (&VasEBoot_usbms_dev);
}