diff -crB grub2-1.98~experimental.20100120/bus/usb/ohci.c grub2-1.98~my_patched.20100312/bus/usb/ohci.c
*** grub2-1.98~experimental.20100120/bus/usb/ohci.c	2010-01-20 23:42:30.000000000 +0100
--- grub2-1.98~my_patched.20100312/bus/usb/ohci.c	2010-03-12 21:03:30.000000000 +0100
***************
*** 84,98 ****
    GRUB_OHCI_REG_INTENA,
    GRUB_OHCI_REG_INTDIS,
    GRUB_OHCI_REG_HCCA,
!   GRUB_OHCI_REG_PERIODIC,
    GRUB_OHCI_REG_CONTROLHEAD,
    GRUB_OHCI_REG_CONTROLCURR,
    GRUB_OHCI_REG_BULKHEAD,
    GRUB_OHCI_REG_BULKCURR,
    GRUB_OHCI_REG_DONEHEAD,
    GRUB_OHCI_REG_FRAME_INTERVAL,
!   GRUB_OHCI_REG_RHUBA = 18,
!   GRUB_OHCI_REG_RHUBPORT = 21
  } grub_ohci_reg_t;
  
  static grub_uint32_t
--- 84,118 ----
    GRUB_OHCI_REG_INTENA,
    GRUB_OHCI_REG_INTDIS,
    GRUB_OHCI_REG_HCCA,
!   GRUB_OHCI_REG_PERIODICCURR,
    GRUB_OHCI_REG_CONTROLHEAD,
    GRUB_OHCI_REG_CONTROLCURR,
    GRUB_OHCI_REG_BULKHEAD,
    GRUB_OHCI_REG_BULKCURR,
    GRUB_OHCI_REG_DONEHEAD,
    GRUB_OHCI_REG_FRAME_INTERVAL,
!   GRUB_OHCI_REG_FRAME_REMAINING,
!   GRUB_OHCI_REG_FRAME_NUMBER,
!   GRUB_OHCI_REG_PERIODIC_START,
!   GRUB_OHCI_REG_LS_TRESHOLD,
!   GRUB_OHCI_REG_RHUB_DESC_A,
!   GRUB_OHCI_REG_RHUB_DESC_B,
!   GRUB_OHCI_REG_RHUB_STATUS,
!   GRUB_OHCI_REG_RHUB_PORT_STAT_1,
!   GRUB_OHCI_REG_RHUB_PORT_STAT_2,
!   GRUB_OHCI_REG_RHUB_PORT_STAT_3,
!   GRUB_OHCI_REG_RHUB_PORT_STAT_4,
!   GRUB_OHCI_REG_RHUB_PORT_STAT_5,
!   GRUB_OHCI_REG_RHUB_PORT_STAT_6,
!   GRUB_OHCI_REG_RHUB_PORT_STAT_7,
!   GRUB_OHCI_REG_RHUB_PORT_STAT_8,
!   GRUB_OHCI_REG_RHUB_PORT_STAT_9,
!   GRUB_OHCI_REG_RHUB_PORT_STAT_10,
!   GRUB_OHCI_REG_RHUB_PORT_STAT_11,
!   GRUB_OHCI_REG_RHUB_PORT_STAT_12,
!   GRUB_OHCI_REG_RHUB_PORT_STAT_13,
!   GRUB_OHCI_REG_RHUB_PORT_STAT_14,
!   GRUB_OHCI_REG_RHUB_PORT_STAT_15
  } grub_ohci_reg_t;
  
  static grub_uint32_t
***************
*** 125,130 ****
--- 145,151 ----
    struct grub_ohci *o;
    grub_uint32_t revision;
    grub_uint32_t frame_interval;
+   grub_uint32_t pom32;
  
    addr = grub_pci_make_address (dev, GRUB_PCI_REG_CLASS);
    class_code = grub_pci_read (addr) >> 8;
***************
*** 148,162 ****
  #endif
  
    /* Allocate memory for the controller and register it.  */
!   o = grub_malloc (sizeof (*o));
    if (! o)
      return 1;
  
    o->iobase = (grub_uint32_t *) base;
  
    /* Reserve memory for the HCCA.  */
!   o->hcca = (struct grub_ohci_hcca *) grub_memalign (256, 256);
! 
    grub_dprintf ("ohci", "class=0x%02x 0x%02x interface 0x%02x base=%p\n",
   		class, subclass, interf, o->iobase);
  
--- 169,192 ----
  #endif
  
    /* Allocate memory for the controller and register it.  */
!   o = grub_malloc (sizeof (struct grub_ohci));
    if (! o)
      return 1;
  
    o->iobase = (grub_uint32_t *) base;
  
    /* Reserve memory for the HCCA.  */
!   /* !!!!!!!!!!!!!!!!!!!!!!!!!!!!! */
!   /* There is something wrong with grub_memalign - it returns not aligned ptr! */
!   /* Some simple workaround - probably not portable. */
!   /* Also freeing of corrected address is not possible (but not needed...) */
!   pom32 = (grub_uint32_t) grub_memalign (256, 512);
!   /*
!   grub_dprintf ("ohci", "Original HCCA pointer=0x%08x\n", pom32);
!   */
!   pom32 += 255;
!   o->hcca = (struct grub_ohci_hcca *)(pom32 & 0xffffff00);
!   
    grub_dprintf ("ohci", "class=0x%02x 0x%02x interface 0x%02x base=%p\n",
   		class, subclass, interf, o->iobase);
  
***************
*** 172,192 ****
    /* Suspend the OHCI by issuing a reset.  */
    grub_ohci_writereg32 (o, GRUB_OHCI_REG_CMDSTATUS, 1); /* XXX: Magic.  */
    grub_millisleep (1);
    grub_dprintf ("ohci", "OHCI reset\n");
  
    /* Restore the frame interval register.  */
    grub_ohci_writereg32 (o, GRUB_OHCI_REG_FRAME_INTERVAL, frame_interval);
  
    /* Setup the HCCA.  */
    grub_ohci_writereg32 (o, GRUB_OHCI_REG_HCCA, (grub_uint32_t) o->hcca);
!   grub_dprintf ("ohci", "OHCI HCCA\n");
  
    /* Enable the OHCI.  */
    grub_ohci_writereg32 (o, GRUB_OHCI_REG_CONTROL,
  			(2 << 6));
    grub_dprintf ("ohci", "OHCI enable: 0x%02x\n",
  		(grub_ohci_readreg32 (o, GRUB_OHCI_REG_CONTROL) >> 6) & 3);
! 
    /* Link to ohci now that initialisation is successful.  */
    o->next = ohci;
    ohci = o;
--- 202,226 ----
    /* Suspend the OHCI by issuing a reset.  */
    grub_ohci_writereg32 (o, GRUB_OHCI_REG_CMDSTATUS, 1); /* XXX: Magic.  */
    grub_millisleep (1);
+   /*
    grub_dprintf ("ohci", "OHCI reset\n");
+   */
  
    /* Restore the frame interval register.  */
    grub_ohci_writereg32 (o, GRUB_OHCI_REG_FRAME_INTERVAL, frame_interval);
  
    /* Setup the HCCA.  */
    grub_ohci_writereg32 (o, GRUB_OHCI_REG_HCCA, (grub_uint32_t) o->hcca);
!   /*
!   grub_dprintf ("ohci", "OHCI HCCA = 0x%08x\n", (grub_uint32_t) o->hcca);
!   * */
  
    /* Enable the OHCI.  */
    grub_ohci_writereg32 (o, GRUB_OHCI_REG_CONTROL,
  			(2 << 6));
    grub_dprintf ("ohci", "OHCI enable: 0x%02x\n",
  		(grub_ohci_readreg32 (o, GRUB_OHCI_REG_CONTROL) >> 6) & 3);
!   
    /* Link to ohci now that initialisation is successful.  */
    o->next = ohci;
    ohci = o;
***************
*** 235,243 ****
    grub_uint32_t buffer;
    grub_uint32_t buffer_end;
  
    grub_dprintf ("ohci", "OHCI transaction td=%p type=%d, toggle=%d, size=%d\n",
  		td, type, toggle, size);
! 
    switch (type)
      {
      case GRUB_USB_TRANSFER_TYPE_SETUP:
--- 269,278 ----
    grub_uint32_t buffer;
    grub_uint32_t buffer_end;
  
+   /*
    grub_dprintf ("ohci", "OHCI transaction td=%p type=%d, toggle=%d, size=%d\n",
  		td, type, toggle, size);
!   */
    switch (type)
      {
      case GRUB_USB_TRANSFER_TYPE_SETUP:
***************
*** 254,273 ****
        break;
      }
  
!   /* Generate no interrupts.  */
    token |= 7 << 21;
  
    /* Set the token.  */
    token |= toggle << 24;
    token |= 1 << 25;
  
!   buffer = (grub_uint32_t) data;
!   buffer_end = buffer + size - 1;
  
    td->token = grub_cpu_to_le32 (token);
-   td->buffer = grub_cpu_to_le32 (buffer);
    td->next_td = 0;
!   td->buffer_end = grub_cpu_to_le32 (buffer_end);
  }
  
  static grub_usb_err_t
--- 289,327 ----
        break;
      }
  
!   /* Generate no interrupts. Except last TD in ED but it is handled in
!    *  grub_ohci_transfer */
    token |= 7 << 21;
  
    /* Set the token.  */
    token |= toggle << 24;
    token |= 1 << 25;
  
!   /* Set "Not accessed" error code */
!   token |= 15 << 28;
!   
!   /* Set correct buffer values in TD if zero transfer occurs */
!   if (size) 
!      {
! 	 buffer = (grub_uint32_t) data;
! 	 buffer_end = buffer + size - 1;
!          td->buffer = grub_cpu_to_le32 (buffer);
!          td->buffer_end = grub_cpu_to_le32 (buffer_end);
!      }
!   else 
!      {
! 	td->buffer = 0;
! 	td->buffer_end = 0;
!      }
  
+   /* Set the rest of TD */
    td->token = grub_cpu_to_le32 (token);
    td->next_td = 0;
! 
!   /*
!   grub_dprintf ("ohci", "TD: token=0x%08x, buf=0x%08x, buf_end=0x%08x\n",
! 		td->token, td->buffer, td->buffer_end);
!   */
  }
  
  static grub_usb_err_t
***************
*** 276,288 ****
  {
    struct grub_ohci *o = (struct grub_ohci *) dev->data;
    grub_ohci_ed_t ed;
!   grub_ohci_td_t td_list;
    grub_uint32_t target;
    grub_uint32_t td_tail;
    grub_uint32_t td_head;
    grub_uint32_t status;
    grub_uint32_t control;
    grub_usb_err_t err;
    int i;
  
    /* Allocate an Endpoint Descriptor.  */
--- 330,345 ----
  {
    struct grub_ohci *o = (struct grub_ohci *) dev->data;
    grub_ohci_ed_t ed;
!   grub_ohci_td_t td_list /*, pom_td */;
    grub_uint32_t target;
    grub_uint32_t td_tail;
    grub_uint32_t td_head;
    grub_uint32_t status;
    grub_uint32_t control;
    grub_usb_err_t err;
+ 
+   grub_uint8_t errcode = 0;
+   grub_ohci_td_t tderr = NULL;
    int i;
  
    /* Allocate an Endpoint Descriptor.  */
***************
*** 297,303 ****
        return GRUB_USB_ERR_INTERNAL;
      }
  
!   grub_dprintf ("ohci", "alloc=%p\n", td_list);
  
    /* Setup all Transfer Descriptors.  */
    for (i = 0; i < transfer->transcnt; i++)
--- 354,361 ----
        return GRUB_USB_ERR_INTERNAL;
      }
  
!   grub_dprintf ("ohci", "alloc=%p, transcnt=0x%02x\n",
!                 td_list, transfer->transcnt);
  
    /* Setup all Transfer Descriptors.  */
    for (i = 0; i < transfer->transcnt; i++)
***************
*** 310,315 ****
--- 368,385 ----
        td_list[i].next_td = grub_cpu_to_le32 (&td_list[i + 1]);
      }
  
+   /* The last-1 TD token we should change to enable interrupt when TD finishes.
+    * As OHCI interrupts are disabled, it does only setting of WDH bit in
+    * HcInterruptStatus register - and that is what we want to safely detect
+    * end of all transactions. */
+   td_list[transfer->transcnt - 1].token &= ~(7 << 21);
+ 
+   td_list[transfer->transcnt].token = 0;
+   td_list[transfer->transcnt].buffer = 0;
+   td_list[transfer->transcnt].buffer_end = 0;
+   td_list[transfer->transcnt].next_td =
+     (grub_uint32_t) &td_list[transfer->transcnt];
+   
    /* Setup the Endpoint Descriptor.  */
  
    /* Set the device address.  */
***************
*** 334,365 ****
    ed->next_ed = grub_cpu_to_le32 (0);
  
    grub_dprintf ("ohci", "program OHCI\n");
  
    /* Program the OHCI to actually transfer.  */
    switch (transfer->type)
      {
      case GRUB_USB_TRANSACTION_TYPE_BULK:
        {
  	grub_dprintf ("ohci", "add to bulk list\n");
  
! 	status = grub_ohci_readreg32 (o, GRUB_OHCI_REG_CMDSTATUS);
! 	control = grub_ohci_readreg32 (o, GRUB_OHCI_REG_CONTROL);
! 
! 	/* Disable the Control and Bulk lists.  */
! 	control &= ~(3 << 4);
! 	grub_ohci_writereg32 (o, GRUB_OHCI_REG_CONTROL, control);
! 
! 	/* Clear BulkListFilled.  */
! 	status &= ~(1 << 2);
! 	grub_ohci_writereg32 (o, GRUB_OHCI_REG_CMDSTATUS, status);
! 
  	grub_ohci_writereg32 (o, GRUB_OHCI_REG_BULKHEAD, (grub_uint32_t) ed);
  
  	/* Enable the Bulk list.  */
  	control |= 1 << 5;
  	grub_ohci_writereg32 (o, GRUB_OHCI_REG_CONTROL, control);
  
  	/* Set BulkListFilled.  */
  	status |= 1 << 2;
  	grub_ohci_writereg32 (o, GRUB_OHCI_REG_CMDSTATUS, status);
  
--- 404,464 ----
    ed->next_ed = grub_cpu_to_le32 (0);
  
    grub_dprintf ("ohci", "program OHCI\n");
+   /* Additional debug. To be removed. */
+   /*
+   grub_dprintf ("ohci", "HCCA_reg=0x%08x HCCA=0x%08x\n",
+     grub_ohci_readreg32 (o, GRUB_OHCI_REG_HCCA),
+     (grub_uint32_t) o->hcca);
+   grub_dprintf ("ohci", "DoneHead_reg=0x%08x DoneHead_HCCA=0x%08x\n",
+     grub_ohci_readreg32 (o, GRUB_OHCI_REG_DONEHEAD),
+     grub_le_to_cpu32 (o->hcca->donehead));
+   */
  
    /* Program the OHCI to actually transfer.  */
+ 
+   /* Disable the Control and Bulk lists.  */
+   control = grub_ohci_readreg32 (o, GRUB_OHCI_REG_CONTROL);
+   control &= ~(3 << 4);
+   grub_ohci_writereg32 (o, GRUB_OHCI_REG_CONTROL, control);
+ 
+   /* Clear BulkListFilled and ControlListFilled.  */
+   status = grub_ohci_readreg32 (o, GRUB_OHCI_REG_CMDSTATUS);
+   status &= ~(3 << 1);
+   grub_ohci_writereg32 (o, GRUB_OHCI_REG_CMDSTATUS, status);
+ 
+   /* Now we should wait for start of next frame. Because we are not using
+    * interrupt, we reset SF bit and wait when it goes to 1. */
+   /* SF bit reset. (SF bit indicates Start Of Frame (SOF) packet) */
+   grub_ohci_writereg32 (o, GRUB_OHCI_REG_INTSTATUS, (1<<2));
+   /* Wait for new SOF */
+   while ((grub_ohci_readreg32 (o, GRUB_OHCI_REG_INTSTATUS) & 0x4) == 0);
+   /* Now it should be safe to change CONTROL and BULK lists. */
+ 
+   /* This we do for safety's sake - it should be done in previous call
+    * of grub_ohci_transfer and nobody should change it in meantime...
+    * It should be done before start of control or bulk OHCI list. */   
+   o->hcca->donehead = 0;
+   grub_ohci_writereg32 (o, GRUB_OHCI_REG_INTSTATUS, (1 << 1)); /* Clears WDH */
+ 
    switch (transfer->type)
      {
      case GRUB_USB_TRANSACTION_TYPE_BULK:
        {
+ 	/*
  	grub_dprintf ("ohci", "add to bulk list\n");
+ 	*/
  
! 	/* Set BulkList Head and Current */
  	grub_ohci_writereg32 (o, GRUB_OHCI_REG_BULKHEAD, (grub_uint32_t) ed);
+ 	grub_ohci_writereg32 (o, GRUB_OHCI_REG_BULKCURR, 0);
  
  	/* Enable the Bulk list.  */
+ 	control = grub_ohci_readreg32 (o, GRUB_OHCI_REG_CONTROL);
  	control |= 1 << 5;
  	grub_ohci_writereg32 (o, GRUB_OHCI_REG_CONTROL, control);
  
  	/* Set BulkListFilled.  */
+ 	status = grub_ohci_readreg32 (o, GRUB_OHCI_REG_CMDSTATUS);
  	status |= 1 << 2;
  	grub_ohci_writereg32 (o, GRUB_OHCI_REG_CMDSTATUS, status);
  
***************
*** 368,389 ****
  
      case GRUB_USB_TRANSACTION_TYPE_CONTROL:
        {
  	grub_dprintf ("ohci", "add to control list\n");
! 	status = grub_ohci_readreg32 (o, GRUB_OHCI_REG_CMDSTATUS);
! 	control = grub_ohci_readreg32 (o, GRUB_OHCI_REG_CONTROL);
! 
! 	/* Disable the Control and Bulk lists.  */
! 	control &= ~(3 << 4);
! 	grub_ohci_writereg32 (o, GRUB_OHCI_REG_CONTROL, control);
! 
! 	/* Clear ControlListFilled.  */
! 	status &= ~(1 << 1);
! 	grub_ohci_writereg32 (o, GRUB_OHCI_REG_CMDSTATUS, status);
  
  	grub_ohci_writereg32 (o, GRUB_OHCI_REG_CONTROLHEAD,
  			      (grub_uint32_t) ed);
! 	grub_ohci_writereg32 (o, GRUB_OHCI_REG_CONTROLHEAD+1,
! 			      (grub_uint32_t) ed);
  
  	/* Enable the Control list.  */
  	control |= 1 << 4;
--- 467,481 ----
  
      case GRUB_USB_TRANSACTION_TYPE_CONTROL:
        {
+ 	/*
  	grub_dprintf ("ohci", "add to control list\n");
! 	*/
  
+ 	/* Set ControlList Head and Current */
  	grub_ohci_writereg32 (o, GRUB_OHCI_REG_CONTROLHEAD,
  			      (grub_uint32_t) ed);
! 	grub_ohci_writereg32 (o, GRUB_OHCI_REG_CONTROLCURR,
! 			      0);
  
  	/* Enable the Control list.  */
  	control |= 1 << 4;
***************
*** 397,434 ****
      }
  
    grub_dprintf ("ohci", "wait for completion\n");
    grub_dprintf ("ohci", "control=0x%02x status=0x%02x\n",
  		grub_ohci_readreg32 (o, GRUB_OHCI_REG_CONTROL),
  		grub_ohci_readreg32 (o, GRUB_OHCI_REG_CMDSTATUS));
  
    /* Wait until the transfer is completed or STALLs.  */
!   while ((ed->td_head & ~0xf) != (ed->td_tail & ~0xf))
      {
!       grub_cpu_idle ();
! 
!       grub_dprintf ("ohci", "head=0x%02x tail=0x%02x\n", ed->td_head, ed->td_tail);
! 
!       /* Detected a STALL.  */
!       if (ed->td_head & 1)
! 	break;
      }
  
!   grub_dprintf ("ohci", "complete\n");
! 
! /*   if (ed->td_head & 1) */
! /*     err = GRUB_USB_ERR_STALL; */
! /*   else if (ed->td */
! 
! 
!   if (ed->td_head & 1)
      {
!       grub_uint8_t errcode;
!       grub_ohci_td_t tderr;
! 
!       tderr = (grub_ohci_td_t) grub_ohci_readreg32 (o,
! 						    GRUB_OHCI_REG_DONEHEAD);
!       errcode = tderr->token >> 28;
  
        switch (errcode)
  	{
  	case 0:
--- 490,589 ----
      }
  
    grub_dprintf ("ohci", "wait for completion\n");
+   /*
    grub_dprintf ("ohci", "control=0x%02x status=0x%02x\n",
  		grub_ohci_readreg32 (o, GRUB_OHCI_REG_CONTROL),
  		grub_ohci_readreg32 (o, GRUB_OHCI_REG_CMDSTATUS));
+   */
  
    /* Wait until the transfer is completed or STALLs.  */
!   do
      {
!       /* Some additional diagnostic. To be removed. */
!       /*
!       grub_dprintf ("ohci", "control=0x%02x status=0x%02x\n",
! 	grub_ohci_readreg32 (o, GRUB_OHCI_REG_CONTROL),
! 	grub_ohci_readreg32 (o, GRUB_OHCI_REG_CMDSTATUS));
!       grub_dprintf ("ohci", "ED=0x%08x TDHead=0x%08x TDTail=0x%08x\n",
!       	(grub_uint32_t) ed, ed->td_head, ed->td_tail);
!       grub_dprintf ("ohci", "Target=0x%08x\n", ed->target);
!       grub_dprintf ("ohci", "BulkHeadED=0x%08x, BulkCurrED=0x%08x\n",
!       	grub_ohci_readreg32 (o, GRUB_OHCI_REG_BULKHEAD),
!       	grub_ohci_readreg32 (o, GRUB_OHCI_REG_BULKCURR));
!       grub_dprintf ("ohci", "ControlHeadED=0x%08x, ControlCurrED=0x%08x\n",
!       	grub_ohci_readreg32 (o, GRUB_OHCI_REG_CONTROLHEAD),
!       	grub_ohci_readreg32 (o, GRUB_OHCI_REG_CONTROLCURR));
! 
!       grub_dprintf ("ohci", "DoneHeadTD=0x%08x, RHUB_STATUS=0x%08x, RHUB_PORT_STAT_1=0x%08x\n",
!       	grub_ohci_readreg32 (o, GRUB_OHCI_REG_DONEHEAD),
!       	grub_ohci_readreg32 (o, GRUB_OHCI_REG_RHUB_STATUS),
!       	grub_ohci_readreg32 (o, GRUB_OHCI_REG_RHUB_PORT_STAT_1));
!       */	
! 
!       /* Removed grub_cpu_idle(), it is totally useless here... */
! 
!       /* Detected a HALT.  */
!       if (grub_le_to_cpu32 (&ed->td_head) & 1)
!         break;
!   
!   /* The original style of detection of finished transaction looks hazardeous.
!   while ((grub_le_to_cpu32 (grub_ohci_read_mem32 (&ed->td_head)) & ~0xf)
!          != (grub_le_to_cpu32 (grub_ohci_read_mem32 (&ed->td_tail)) & ~0xf));
!   */
!   /* This should be according to OHCI specification:
!    * TD is finished and ED is updated when TD is retired and HcDoneHead
!    * register updated and, if allowed by WDH bit, written into HccaDoneHead.
!    * So we should:
!    * - allow WritebackDoneHead interrupt (WDH) by proper setting of last TD
!    *   token - it is done above in transaction settings
!    * - detect setting of WDH bit in HcInterruptStatus register
!    * - compare HccaDoneHead value with address of last-1 TD. If it is not
!    *   equal, check ED for halt and if not so, reset WDH bit and wait again
!    *   - but it should not happen - debug it!
!    */
!       if ((grub_ohci_readreg32 (o, GRUB_OHCI_REG_INTSTATUS) & 0x2) != 0)
!         {
!           if ((grub_le_to_cpu32 (o->hcca->donehead) & ~0xf)
!                == (grub_uint32_t) &td_list[transfer->transcnt - 1])
!             break;
! 
!           /* Done Head can be updated on some another place if ED is halted. */          
!           if (grub_le_to_cpu32 (&ed->td_head) & 1)
!             break;
! 
!           /* If there is not HALT in ED, it is not correct, so debug it, reset
!            * donehead and WDH and continue waiting. */
!           grub_dprintf ("ohci", "Incorrect HccaDoneHead=0x%08x",
!                         o->hcca->donehead);
!           o->hcca->donehead = 0;
!           grub_ohci_writereg32 (o, GRUB_OHCI_REG_INTSTATUS, (1 << 1));
!           continue;
!         }
      }
+   while (1);
  
!   /*
!   grub_dprintf ("ohci", "completed\n");
!   */
!   
!   /* Additional debug - to be removed. */
!   /*
!   grub_dprintf ("ohci", "DoneHead_reg=0x%08x DoneHead_HCCA=0x%08x\n",
!     grub_ohci_readreg32 (o, GRUB_OHCI_REG_DONEHEAD),
!     grub_le_to_cpu32 (o->hcca->donehead));
!   */
!   
!   if (grub_le_to_cpu32 (ed->td_head) & 1)
      {
!       tderr = (grub_ohci_td_t)
!                 (grub_ohci_readreg32 (o, GRUB_OHCI_REG_DONEHEAD) & ~0xf);
!       if (tderr == 0)
!         /* If DONEHEAD==0 it means that correct address is in HCCA.
!          * It should be always now! */
!         tderr = (grub_ohci_td_t) (grub_le_to_cpu32 (o->hcca->donehead) & ~0xf);
  
+       errcode = grub_le_to_cpu32 (tderr->token) >> 28;
+       
        switch (errcode)
  	{
  	case 0:
***************
*** 508,513 ****
--- 663,698 ----
    else
      err = GRUB_USB_ERR_NONE;
  
+   /*
+   grub_dprintf ("ohci", "tderr=0x%08x errcode=0x%1x err=0x%08x\n",
+     (grub_uint32_t) tderr, errcode, err);
+   */
+   
+   /*
+   if (err)
+     {
+       grub_dprintf ("ohci", "ED=0x%08x TDHead=0x%08x TDTail=0x%08x\n",
+       	(grub_uint32_t) ed, grub_le_to_cpu32 (ed->td_head),
+       	grub_le_to_cpu32 (ed->td_tail));
+       grub_dprintf ("ohci", "Target=0x%08x DoneHead_reg=0x%08x\n",
+         ed->target, grub_ohci_readreg32 (o, GRUB_OHCI_REG_DONEHEAD));
+       grub_dprintf ("ohci", "DoneHead_HCCA=0x%08x\n",
+         grub_le_to_cpu32 (o->hcca->donehead));
+       pom_td = (grub_ohci_td_t) (grub_le_to_cpu32 (ed->td_head) & ~0xf);
+       grub_dprintf ("ohci", "ED_HeadTD=0x%08x ED_HeadTD_Token=0x%08x\n",
+         (grub_uint32_t) pom_td, grub_le_to_cpu32 (pom_td->token));
+       for (i = 0; i < transfer->transcnt; i++)
+         {
+           grub_dprintf ("ohci", "Token[%d]=0x%08x buffer[%d]=0x%08x\n",
+             i, grub_le_to_cpu32 (td_list[i].token),
+             i, grub_le_to_cpu32 (td_list[i].buffer));
+           grub_dprintf ("ohci", "buffer_end[%d]=0x%08x next_td[%d]=0x%08x\n",
+             i, grub_le_to_cpu32 (td_list[i].buffer_end),
+             i, grub_le_to_cpu32 (td_list[i].next_td));
+         }
+     }
+    */
+ 
    /* Disable the Control and Bulk lists.  */
    control = grub_ohci_readreg32 (o, GRUB_OHCI_REG_CONTROL);
    control &= ~(3 << 4);
***************
*** 515,523 ****
  
    /* Clear BulkListFilled and ControlListFilled.  */
    status = grub_ohci_readreg32 (o, GRUB_OHCI_REG_CMDSTATUS);
!   status &= ~((1 << 2) | (1 << 3));
    grub_ohci_writereg32 (o, GRUB_OHCI_REG_CMDSTATUS, status);
  
    /* XXX */
    grub_free (td_list);
    grub_free (ed);
--- 700,737 ----
  
    /* Clear BulkListFilled and ControlListFilled.  */
    status = grub_ohci_readreg32 (o, GRUB_OHCI_REG_CMDSTATUS);
!   status &= ~(3 << 1);
    grub_ohci_writereg32 (o, GRUB_OHCI_REG_CMDSTATUS, status);
+   
+   /* Set ED to be skipped - for safety */
+   ed->target |= grub_cpu_to_le32 (1 << 14);
+ 
+   /* Now we should wait for start of next frame. Because we are not using
+    * interrupt, we reset SF bit and wait when it goes to 1. */
+   /* SF bit reset. (SF bit indicates Start Of Frame (SOF) packet) */
+   grub_ohci_writereg32 (o, GRUB_OHCI_REG_INTSTATUS, (1<<2));
+   /* Wait for new SOF */
+   while ((grub_ohci_readreg32 (o, GRUB_OHCI_REG_INTSTATUS) & 0x4) == 0);
+   /* Now it should be safe to change CONTROL and BULK lists. */
+   
+   /* Important cleaning. */
+   o->hcca->donehead = 0;
+   /* Additional debug print. to be removed.
+   grub_dprintf ("ohci", "Before reset of WDH: INTSTATUS=0x%08x\n",
+     grub_ohci_readreg32 (o, GRUB_OHCI_REG_INTSTATUS));
+     */
+   grub_ohci_writereg32 (o, GRUB_OHCI_REG_INTSTATUS, (1 << 1)); /* Clears WDH */
+   /* Additional debug print. to be removed.
+   grub_dprintf ("ohci", "After reset of WDH: INTSTATUS=0x%08x\n",
+     grub_ohci_readreg32 (o, GRUB_OHCI_REG_INTSTATUS));
+     */
+   grub_ohci_writereg32 (o, GRUB_OHCI_REG_CONTROLHEAD, 0);
+   grub_ohci_writereg32 (o, GRUB_OHCI_REG_CONTROLCURR, 0);
+   grub_ohci_writereg32 (o, GRUB_OHCI_REG_BULKHEAD, 0);
+   grub_ohci_writereg32 (o, GRUB_OHCI_REG_BULKCURR, 0);
  
+   grub_dprintf ("ohci", "OHCI finished, freeing, err=0x%02x\n", err);
+   
    /* XXX */
    grub_free (td_list);
    grub_free (ed);
***************
*** 533,556 ****
     grub_uint32_t status;
  
     /* Reset the port.  */
!    status = grub_ohci_readreg32 (o, GRUB_OHCI_REG_RHUBPORT + port);
     status |= (1 << 4); /* XXX: Magic.  */
!    grub_ohci_writereg32 (o, GRUB_OHCI_REG_RHUBPORT + port, status);
     grub_millisleep (100);
  
     /* End the reset signaling.  */
!    status = grub_ohci_readreg32 (o, GRUB_OHCI_REG_RHUBPORT + port);
     status |= (1 << 20); /* XXX: Magic.  */
!    grub_ohci_writereg32 (o, GRUB_OHCI_REG_RHUBPORT + port, status);
     grub_millisleep (10);
  
     /* Enable the port.  */
!    status = grub_ohci_readreg32 (o, GRUB_OHCI_REG_RHUBPORT + port);
     status |= (enable << 1); /* XXX: Magic.  */
!    grub_ohci_writereg32 (o, GRUB_OHCI_REG_RHUBPORT + port, status);
  
!    status = grub_ohci_readreg32 (o, GRUB_OHCI_REG_RHUBPORT + port);
!    grub_dprintf ("ohci", "portstatus=0x%02x\n", status);
  
     return GRUB_ERR_NONE;
  }
--- 747,770 ----
     grub_uint32_t status;
  
     /* Reset the port.  */
!    status = grub_ohci_readreg32 (o, GRUB_OHCI_REG_RHUB_PORT_STAT_1 + port);
     status |= (1 << 4); /* XXX: Magic.  */
!    grub_ohci_writereg32 (o, GRUB_OHCI_REG_RHUB_PORT_STAT_1 + port, status);
     grub_millisleep (100);
  
     /* End the reset signaling.  */
!    status = grub_ohci_readreg32 (o, GRUB_OHCI_REG_RHUB_PORT_STAT_1 + port);
     status |= (1 << 20); /* XXX: Magic.  */
!    grub_ohci_writereg32 (o, GRUB_OHCI_REG_RHUB_PORT_STAT_1 + port, status);
     grub_millisleep (10);
  
     /* Enable the port.  */
!    status = grub_ohci_readreg32 (o, GRUB_OHCI_REG_RHUB_PORT_STAT_1 + port);
     status |= (enable << 1); /* XXX: Magic.  */
!    grub_ohci_writereg32 (o, GRUB_OHCI_REG_RHUB_PORT_STAT_1 + port, status);
  
!    status = grub_ohci_readreg32 (o, GRUB_OHCI_REG_RHUB_PORT_STAT_1 + port);
!    grub_dprintf ("ohci", "port=0x%02x, portstatus=0x%02x\n", port, status);
  
     return GRUB_ERR_NONE;
  }
***************
*** 561,569 ****
     struct grub_ohci *o = (struct grub_ohci *) dev->data;
     grub_uint32_t status;
  
!    status = grub_ohci_readreg32 (o, GRUB_OHCI_REG_RHUBPORT + port);
  
!    grub_dprintf ("ohci", "detect_dev status=0x%02x\n", status);
  
     if (! (status & 1))
       return GRUB_USB_SPEED_NONE;
--- 775,783 ----
     struct grub_ohci *o = (struct grub_ohci *) dev->data;
     grub_uint32_t status;
  
!    status = grub_ohci_readreg32 (o, GRUB_OHCI_REG_RHUB_PORT_STAT_1 + port);
  
!    grub_dprintf ("ohci", "detect_dev port=0x%02x, status=0x%02x\n", port, status);
  
     if (! (status & 1))
       return GRUB_USB_SPEED_NONE;
***************
*** 579,585 ****
    struct grub_ohci *o = (struct grub_ohci *) dev->data;
    grub_uint32_t portinfo;
  
!   portinfo = grub_ohci_readreg32 (o, GRUB_OHCI_REG_RHUBA);
  
    grub_dprintf ("ohci", "root hub ports=%d\n", portinfo & 0xFF);
  
--- 793,799 ----
    struct grub_ohci *o = (struct grub_ohci *) dev->data;
    grub_uint32_t portinfo;
  
!   portinfo = grub_ohci_readreg32 (o, GRUB_OHCI_REG_RHUB_DESC_A);
  
    grub_dprintf ("ohci", "root hub ports=%d\n", portinfo & 0xFF);
  
diff -crB grub2-1.98~experimental.20100120/bus/usb/usbtrans.c grub2-1.98~my_patched.20100312/bus/usb/usbtrans.c
*** grub2-1.98~experimental.20100120/bus/usb/usbtrans.c	2010-01-20 23:42:30.000000000 +0100
--- grub2-1.98~my_patched.20100312/bus/usb/usbtrans.c	2010-03-11 22:12:40.000000000 +0100
***************
*** 54,59 ****
--- 54,65 ----
      max = 64;
  
    datablocks = (size + max - 1) / max;
+   /* Note: This, together with :simple and effective" toggling of toggle bit,
+ 	 * is not safe, it will work only in the case if
+    * OHCI will realy send one TD as one packet - I think it is not guaranted!
+    * Fortunately, control messages are very short, so prabably we do never
+    * need to split it. But another case are bulk transfers - see note about
+    * toggle bit and data blocks in bulk transfer function.	*/
  
    /* XXX: Discriminate between different types of control
       messages.  */
***************
*** 103,109 ****
        size -= max;
      }
  
!   /* End with an empty OUT transaction.  */
    transfer->transactions[datablocks + 1].size = 0;
    transfer->transactions[datablocks + 1].data = NULL;
    if (reqtype & 128)
--- 109,115 ----
        size -= max;
      }
  
!   /* End with an empty OUT/IN transaction (reverse direction than data).  */
    transfer->transactions[datablocks + 1].size = 0;
    transfer->transactions[datablocks + 1].data = NULL;
    if (reqtype & 128)
***************
*** 133,138 ****
--- 138,147 ----
    grub_usb_err_t err;
    int toggle = dev->toggle[endpoint];
  
+   grub_dprintf ("usb",
+ 		"bulk: endpoint=0x%02x type=0x%02x size=%d\n",
+ 		endpoint, type, size);
+ 
    /* Use the maximum packet size given in the endpoint descriptor.  */
    if (dev->initialized)
      {
***************
*** 178,183 ****
--- 187,208 ----
        tr->size = (size > max) ? max : size;
        /* XXX: Use the right most bit as the data toggle.  Simple and
  	 effective.  */
+ 	    /* Addendum to XXX: This is simple and effective but it is really not
+ 			 * good idea! Why:
+ 	     * 1. It is a wasting of memory - for each 64 (or less!) bytes we need
+ 	     * one TD = 16 bytes! TD can transfer up to 8k if buffer is 4k page
+ 	     * aligned, or 4k if buffer is not aligned (because there can be only one
+ 	     * 4k page crossing.)			 	     
+ 	     * 2. I think there is not guaranted that OHCI will send each TD as one
+ 	     * packet even if it has buffer size up to max packet size. So we can
+ 	     * lost toggle bit synchronization!
+ 			 * Proposal enhancement (to do...):
+ 			 * A. Change toggle bit control in the way that it is controled by OHCI
+ 			 * 		itself during TD transfers and final toggle value is read from
+ 			 * 		OHCI. This is MUST if we want to do next point B. 			 	 			 			 			 
+ 			 * B. Change max. buffer size in one TD to 4k, i.e. split data to 4k
+ 			 *    blocks instead of "max" (<=64).
+ 			 */			 
        tr->toggle = toggle;
        toggle = toggle ? 0 : 1;
        tr->pid = type;
Pouze v grub2-1.98~my_patched.20100312/conf: gcry.rmk
diff -crB grub2-1.98~experimental.20100120/disk/scsi.c grub2-1.98~my_patched.20100312/disk/scsi.c
*** grub2-1.98~experimental.20100120/disk/scsi.c	2010-01-20 23:42:31.000000000 +0100
--- grub2-1.98~my_patched.20100312/disk/scsi.c	2010-03-09 22:27:16.000000000 +0100
***************
*** 50,55 ****
--- 50,106 ----
  }
  
  
+ /* Check result of previous operation.  */
+ static grub_err_t
+ grub_scsi_request_sense (grub_scsi_t scsi)
+ {
+   struct grub_scsi_request_sense rs;
+   struct grub_scsi_request_sense_data rsd;
+   grub_err_t err;
+ 
+   rs.opcode = grub_scsi_cmd_request_sense;
+   rs.lun = scsi->lun << GRUB_SCSI_LUN_SHIFT;
+   rs.reserved1 = 0;
+   rs.reserved2 = 0;
+   rs.alloc_length = 0x12; /* XXX: Hardcoded for now */
+   rs.control = 0;
+ 
+   err = scsi->dev->read (scsi, sizeof (rs), (char *) &rs,
+ 			 sizeof (rsd), (char *) &rsd);
+   if (err)
+     return err;
+ 
+   return GRUB_ERR_NONE;
+ }
+ /* Self commenting... */
+ static grub_err_t
+ grub_scsi_test_unit_ready (grub_scsi_t scsi)
+ {
+   struct grub_scsi_test_unit_ready tur;
+   grub_err_t err;
+   grub_err_t err_sense;
+   
+   tur.opcode = grub_scsi_cmd_test_unit_ready;
+   tur.lun = scsi->lun << GRUB_SCSI_LUN_SHIFT;
+   tur.reserved1 = 0;
+   tur.reserved2 = 0;
+   tur.reserved3 = 0;
+   tur.control = 0;
+ 
+   err = scsi->dev->read (scsi, sizeof (tur), (char *) &tur,
+ 			 0, NULL);
+ 
+   /* Each SCSI command should be followed by Request Sense.
+      If not so, many devices STALLs or definitely freezes. */
+   err_sense = grub_scsi_request_sense (scsi);
+   /* err_sense is ignored for now and Request Sense Data also... */
+   
+   if (err)
+     return err;
+ 
+   return GRUB_ERR_NONE;
+ }
+ 
  /* Determine the the device is removable and the type of the device
     SCSI.  */
  static grub_err_t
***************
*** 58,72 ****
    struct grub_scsi_inquiry iq;
    struct grub_scsi_inquiry_data iqd;
    grub_err_t err;
  
    iq.opcode = grub_scsi_cmd_inquiry;
    iq.lun = scsi->lun << GRUB_SCSI_LUN_SHIFT;
    iq.reserved = 0;
    iq.alloc_length = 0x24; /* XXX: Hardcoded for now */
!   iq.reserved2 = 0;
  
    err = scsi->dev->read (scsi, sizeof (iq), (char *) &iq,
  			 sizeof (iqd), (char *) &iqd);
    if (err)
      return err;
  
--- 109,131 ----
    struct grub_scsi_inquiry iq;
    struct grub_scsi_inquiry_data iqd;
    grub_err_t err;
+   grub_err_t err_sense;
  
    iq.opcode = grub_scsi_cmd_inquiry;
    iq.lun = scsi->lun << GRUB_SCSI_LUN_SHIFT;
+   iq.page = 0;
    iq.reserved = 0;
    iq.alloc_length = 0x24; /* XXX: Hardcoded for now */
!   iq.control = 0;
  
    err = scsi->dev->read (scsi, sizeof (iq), (char *) &iq,
  			 sizeof (iqd), (char *) &iqd);
+ 
+   /* Each SCSI command should be followed by Request Sense.
+      If not so, many devices STALLs or definitely freezes. */
+   err_sense = grub_scsi_request_sense (scsi);
+   /* err_sense is ignored for now and Request Sense Data also... */
+ 
    if (err)
      return err;
  
***************
*** 83,95 ****
    struct grub_scsi_read_capacity rc;
    struct grub_scsi_read_capacity_data rcd;
    grub_err_t err;
  
    rc.opcode = grub_scsi_cmd_read_capacity;
    rc.lun = scsi->lun << GRUB_SCSI_LUN_SHIFT;
!   grub_memset (rc.reserved, 0, sizeof (rc.reserved));
  
    err = scsi->dev->read (scsi, sizeof (rc), (char *) &rc,
  			 sizeof (rcd), (char *) &rcd);
    if (err)
      return err;
  
--- 142,165 ----
    struct grub_scsi_read_capacity rc;
    struct grub_scsi_read_capacity_data rcd;
    grub_err_t err;
+   grub_err_t err_sense;
  
    rc.opcode = grub_scsi_cmd_read_capacity;
    rc.lun = scsi->lun << GRUB_SCSI_LUN_SHIFT;
!   rc.logical_block_addr = 0;
!   rc.reserved1 = 0;
!   rc.reserved2 = 0;
!   rc.PMI = 0;
!   rc.control = 0;
  
    err = scsi->dev->read (scsi, sizeof (rc), (char *) &rc,
  			 sizeof (rcd), (char *) &rcd);
+ 
+   /* Each SCSI command should be followed by Request Sense.
+      If not so, many devices STALLs or definitely freezes. */
+   err_sense = grub_scsi_request_sense (scsi);
+   /* err_sense is ignored for now and Request Sense Data also... */
+ 
    if (err)
      return err;
  
***************
*** 107,112 ****
--- 177,184 ----
  {
    grub_scsi_t scsi;
    struct grub_scsi_read10 rd;
+   grub_err_t err;
+   grub_err_t err_sense;
  
    scsi = disk->data;
  
***************
*** 118,124 ****
    rd.reserved2 = 0;
    rd.pad = 0;
  
!   return scsi->dev->read (scsi, sizeof (rd), (char *) &rd, size * scsi->blocksize, buf);
  }
  
  /* Send a SCSI request for DISK: read SIZE sectors starting with
--- 190,203 ----
    rd.reserved2 = 0;
    rd.pad = 0;
  
!   err = scsi->dev->read (scsi, sizeof (rd), (char *) &rd, size * scsi->blocksize, buf);
! 
!   /* Each SCSI command should be followed by Request Sense.
!      If not so, many devices STALLs or definitely freezes. */
!   err_sense = grub_scsi_request_sense (scsi);
!   /* err_sense is ignored for now and Request Sense Data also... */
! 
!   return err;
  }
  
  /* Send a SCSI request for DISK: read SIZE sectors starting with
***************
*** 129,134 ****
--- 208,215 ----
  {
    grub_scsi_t scsi;
    struct grub_scsi_read12 rd;
+   grub_err_t err;
+   grub_err_t err_sense;
  
    scsi = disk->data;
  
***************
*** 139,145 ****
    rd.reserved = 0;
    rd.control = 0;
  
!   return scsi->dev->read (scsi, sizeof (rd), (char *) &rd, size * scsi->blocksize, buf);
  }
  
  #if 0
--- 220,233 ----
    rd.reserved = 0;
    rd.control = 0;
  
!   err = scsi->dev->read (scsi, sizeof (rd), (char *) &rd, size * scsi->blocksize, buf);
! 
!   /* Each SCSI command should be followed by Request Sense.
!      If not so, many devices STALLs or definitely freezes. */
!   err_sense = grub_scsi_request_sense (scsi);
!   /* err_sense is ignored for now and Request Sense Data also... */
! 
!   return err;
  }
  
  #if 0
***************
*** 151,156 ****
--- 239,246 ----
  {
    grub_scsi_t scsi;
    struct grub_scsi_write10 wr;
+   grub_err_t err;
+   grub_err_t err_sense;
  
    scsi = disk->data;
  
***************
*** 162,168 ****
    wr.reserved2 = 0;
    wr.pad = 0;
  
!   return scsi->dev->write (scsi, sizeof (wr), (char *) &wr, size * scsi->blocksize, buf);
  }
  
  /* Send a SCSI request for DISK: write the data stored in BUF to SIZE
--- 252,265 ----
    wr.reserved2 = 0;
    wr.pad = 0;
  
!   err = scsi->dev->write (scsi, sizeof (wr), (char *) &wr, size * scsi->blocksize, buf);
! 
!   /* Each SCSI command should be followed by Request Sense.
!      If not so, many devices STALLs or definitely freezes. */
!   err_sense = grub_scsi_request_sense (scsi);
!   /* err_sense is ignored for now and Request Sense Data also... */
! 
!   return err;
  }
  
  /* Send a SCSI request for DISK: write the data stored in BUF to SIZE
***************
*** 173,178 ****
--- 270,277 ----
  {
    grub_scsi_t scsi;
    struct grub_scsi_write10 wr;
+   grub_err_t err;
+   grub_err_t err_sense;
  
    scsi = disk->data;
  
***************
*** 183,189 ****
    wr.reserved = 0;
    wr.pad = 0;
  
!   return scsi->dev->write (scsi, sizeof (wr), (char *) &wr, size * scsi->blocksize, buf);
  }
  #endif
  
--- 282,295 ----
    wr.reserved = 0;
    wr.pad = 0;
  
!   err = scsi->dev->write (scsi, sizeof (wr), (char *) &wr, size * scsi->blocksize, buf);
! 
!   /* Each SCSI command should be followed by Request Sense.
!      If not so, many devices STALLs or definitely freezes. */
!   err_sense = grub_scsi_request_sense (scsi);
!   /* err_sense is ignored for now and Request Sense Data also... */
! 
!   return err;
  }
  #endif
  
***************
*** 290,295 ****
--- 396,411 ----
        else
  	disk->has_partitions = 1;
  
+ 
+       /* According to USB MS tests, issue Test Unit Ready until OK */
+       /* XXX: there should be some timeout... */
+       do
+         {
+           err = grub_scsi_test_unit_ready (scsi);
+         }
+       while (err == GRUB_ERR_READ_ERROR);
+ 
+       /* Read capacity of media */
        err = grub_scsi_read_capacity (scsi);
        if (err)
  	{
***************
*** 300,311 ****
  
        /* SCSI blocks can be something else than 512, although GRUB
  	 wants 512 byte blocks.  */
!       disk->total_sectors = ((scsi->size * scsi->blocksize)
! 			     << GRUB_DISK_SECTOR_BITS);
! 
!       grub_dprintf ("scsi", "capacity=%llu, blksize=%d\n",
! 		    (unsigned long long) disk->total_sectors,
! 		    scsi->blocksize);
  
        return GRUB_ERR_NONE;
      }
--- 416,429 ----
  
        /* SCSI blocks can be something else than 512, although GRUB
  	 wants 512 byte blocks.  */
!       disk->total_sectors = ((grub_uint64_t)scsi->size
!                              * (grub_uint64_t)scsi->blocksize)
! 			    >> GRUB_DISK_SECTOR_BITS;
! 
!       grub_dprintf ("scsi", "blocks=%u, blocksize=%u\n",
! 		    scsi->size, scsi->blocksize);
!       grub_dprintf ("scsi", "Disk total 512 sectors = %llu\n",
! 		    disk->total_sectors);
  
        return GRUB_ERR_NONE;
      }
diff -crB grub2-1.98~experimental.20100120/disk/usbms.c grub2-1.98~my_patched.20100312/disk/usbms.c
*** grub2-1.98~experimental.20100120/disk/usbms.c	2010-01-20 23:42:31.000000000 +0100
--- grub2-1.98~my_patched.20100312/disk/usbms.c	2010-03-09 21:46:32.000000000 +0100
***************
*** 125,138 ****
  		{
  		  /* Bulk IN endpoint.  */
  		  usbms->in = endp;
! 		  grub_usb_clear_halt (usbdev, endp->endp_addr & 128);
  		  usbms->in_maxsz = endp->maxpacket;
  		}
  	      else if (!(endp->endp_addr & 128) && (endp->attrib & 3) == 2)
  		{
  		  /* Bulk OUT endpoint.  */
  		  usbms->out = endp;
! 		  grub_usb_clear_halt (usbdev, endp->endp_addr & 128);
  		  usbms->out_maxsz = endp->maxpacket;
  		}
  	    }
--- 128,143 ----
  		{
  		  /* Bulk IN endpoint.  */
  		  usbms->in = endp;
! 		  /* Clear Halt is not possible yet! */
! 		  /* grub_usb_clear_halt (usbdev, endp->endp_addr); */
  		  usbms->in_maxsz = endp->maxpacket;
  		}
  	      else if (!(endp->endp_addr & 128) && (endp->attrib & 3) == 2)
  		{
  		  /* Bulk OUT endpoint.  */
  		  usbms->out = endp;
! 		  /* Clear Halt is not possible yet! */
! 		  /* grub_usb_clear_halt (usbdev, endp->endp_addr); */
  		  usbms->out_maxsz = endp->maxpacket;
  		}
  	    }
***************
*** 143,148 ****
--- 148,156 ----
  	      return 0;
  	    }
  
+ 	  /* XXX: Activate the first configuration.  */
+ 	  grub_usb_set_configuration (usbdev, 1);
+ 
  	  /* Query the amount of LUNs.  */
  	  err = grub_usb_control_msg (usbdev, 0xA1, 254,
  				      0, i, 1, (char *) &luns);
***************
*** 151,158 ****
  	      /* In case of a stall, clear the stall.  */
  	      if (err == GRUB_USB_ERR_STALL)
  		{
! 		  grub_usb_clear_halt (usbdev, usbms->in->endp_addr & 3);
! 		  grub_usb_clear_halt (usbdev, usbms->out->endp_addr & 3);
  		}
  
  	      /* Just set the amount of LUNs to one.  */
--- 159,166 ----
  	      /* In case of a stall, clear the stall.  */
  	      if (err == GRUB_USB_ERR_STALL)
  		{
! 		  grub_usb_clear_halt (usbdev, usbms->in->endp_addr);
! 		  grub_usb_clear_halt (usbdev, usbms->out->endp_addr);
  		}
  
  	      /* Just set the amount of LUNs to one.  */
***************
*** 164,171 ****
  
  	  /* XXX: Check the magic values, does this really make
  	     sense?  */
! 	  grub_usb_control_msg (usbdev, (1 << 6) | 1, 255,
! 				0, i, 0, 0);
  
  	  /* XXX: To make Qemu work?  */
  	  if (usbms->luns == 0)
--- 172,180 ----
  
  	  /* XXX: Check the magic values, does this really make
  	     sense?  */
! 	  /* What is it? Removed. */
! 	  /* grub_usb_control_msg (usbdev, (1 << 6) | 1, 255,
! 				0, i, 0, 0); */
  
  	  /* XXX: To make Qemu work?  */
  	  if (usbms->luns == 0)
***************
*** 174,185 ****
  	  usbms->next = grub_usbms_dev_list;
  	  grub_usbms_dev_list = usbms;
  
! 	  /* XXX: Activate the first configuration.  */
! 	  grub_usb_set_configuration (usbdev, 1);
! 
  	  /* Bulk-Only Mass Storage Reset, after the reset commands
  	     will be accepted.  */
  	  grub_usbms_reset (usbdev, i);
  
  	  return 0;
  	}
--- 183,194 ----
  	  usbms->next = grub_usbms_dev_list;
  	  grub_usbms_dev_list = usbms;
  
! 	  /* Reset recovery procedure */
  	  /* Bulk-Only Mass Storage Reset, after the reset commands
  	     will be accepted.  */
  	  grub_usbms_reset (usbdev, i);
+ 	  grub_usb_clear_halt (usbdev, usbms->in->endp_addr);
+ 	  grub_usb_clear_halt (usbdev, usbms->out->endp_addr);
  
  	  return 0;
  	}
***************
*** 225,230 ****
--- 234,240 ----
    static grub_uint32_t tag = 0;
    grub_usb_err_t err = GRUB_USB_ERR_NONE;
    int retrycnt = 3 + 1;
+   grub_size_t i;
  
   retry:
    retrycnt--;
***************
*** 240,309 ****
    cbw.lun = scsi->lun << GRUB_SCSI_LUN_SHIFT;
    cbw.length = cmdsize;
    grub_memcpy (cbw.cbwcb, cmd, cmdsize);
  
!   /* Write the request.  */
    err = grub_usb_bulk_write (dev->dev, dev->out->endp_addr & 15,
  			     sizeof (cbw), (char *) &cbw);
    if (err)
      {
        if (err == GRUB_USB_ERR_STALL)
  	{
  	  grub_usb_clear_halt (dev->dev, dev->out->endp_addr);
  	  goto retry;
  	}
        return grub_error (GRUB_ERR_IO, "USB Mass Storage request failed");
      }
  
!   /* Read/write the data.  */
!   if (read_write == 0)
      {
        err = grub_usb_bulk_read (dev->dev, dev->in->endp_addr & 15, size, buf);
!       grub_dprintf ("usb", "read: %d %d\n", err, GRUB_USB_ERR_STALL);
!       if (err)
! 	{
! 	  if (err == GRUB_USB_ERR_STALL)
! 	    {
! 	      grub_usb_clear_halt (dev->dev, dev->in->endp_addr);
! 	      goto retry;
! 	    }
! 	  return grub_error (GRUB_ERR_READ_ERROR,
! 			     "can't read from USB Mass Storage device");
! 	}
      }
!   else
      {
!       err = grub_usb_bulk_write (dev->dev, dev->in->endp_addr & 15, size, buf);
        grub_dprintf ("usb", "write: %d %d\n", err, GRUB_USB_ERR_STALL);
!       if (err)
! 	{
! 	  if (err == GRUB_USB_ERR_STALL)
! 	    {
! 	      grub_usb_clear_halt (dev->dev, dev->out->endp_addr);
! 	      goto retry;
! 	    }
! 	  return grub_error (GRUB_ERR_WRITE_ERROR,
! 			     "can't write to USB Mass Storage device");
! 	}
      }
  
!   /* Read the status.  */
    err = grub_usb_bulk_read (dev->dev, dev->in->endp_addr & 15,
  			    sizeof (status), (char *) &status);
    if (err)
      {
!       if (err == GRUB_USB_ERR_STALL)
! 	{
! 	  grub_usb_clear_halt (dev->dev, dev->in->endp_addr);
  	  goto retry;
! 	}
!       return grub_error (GRUB_ERR_READ_ERROR,
! 			 "can't read status from USB Mass Storage device");
      }
  
!   /* XXX: Magic and check this code.  */
    if (status.status == 2)
      {
-       /* XXX: Phase error, reset device.  */
        grub_usbms_reset (dev->dev, dev->interface);
        grub_usb_clear_halt (dev->dev, dev->in->endp_addr);
        grub_usb_clear_halt (dev->dev, dev->out->endp_addr);
--- 250,335 ----
    cbw.lun = scsi->lun << GRUB_SCSI_LUN_SHIFT;
    cbw.length = cmdsize;
    grub_memcpy (cbw.cbwcb, cmd, cmdsize);
+   
+   /* Debug print of CBW content. */
+   grub_dprintf ("usb", "CBW: sign=0x%08x tag=0x%08x len=0x%08x\n",
+   	cbw.signature, cbw.tag, cbw.transfer_length);
+   grub_dprintf ("usb", "CBW: flags=0x%02x lun=0x%02x CB_len=0x%02x\n",
+   	cbw.flags, cbw.lun, cbw.length);
+   grub_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 should be corrected! */
    err = grub_usb_bulk_write (dev->dev, dev->out->endp_addr & 15,
  			     sizeof (cbw), (char *) &cbw);
    if (err)
      {
        if (err == GRUB_USB_ERR_STALL)
  	{
+ 	  grub_usb_clear_halt (dev->dev, dev->in->endp_addr);
  	  grub_usb_clear_halt (dev->dev, dev->out->endp_addr);
  	  goto retry;
  	}
        return grub_error (GRUB_ERR_IO, "USB Mass Storage request failed");
      }
  
!   /* Read/write the data, (maybe) according to specification.  */
!   if (size && (read_write == 0))
      {
        err = grub_usb_bulk_read (dev->dev, dev->in->endp_addr & 15, size, buf);
!       grub_dprintf ("usb", "read: %d %d\n", err, GRUB_USB_ERR_STALL); 
!       if (err) goto CheckCSW;
!       /* Debug print of received data. */
!       grub_dprintf ("usb", "buf:\n");
!       if (size <= 256)
!         for (i=0; i<size; i++)
!           grub_dprintf ("usb", "0x%02x: 0x%02x\n", i, buf[i]);
!       else
!           grub_dprintf ("usb", "Too much data for debug print...\n");
      }
!   else if (size)
      {
!       err = grub_usb_bulk_write (dev->dev, dev->out->endp_addr & 15, size, buf);
        grub_dprintf ("usb", "write: %d %d\n", err, GRUB_USB_ERR_STALL);
!       grub_dprintf ("usb", "buf:\n");
!       /* Debug print of sent data. */
!       if (size <= 256)
!         for (i=0; i<size; i++)
!           grub_dprintf ("usb", "0x%02x: 0x%02x\n", i, buf[i]);
!       else
!           grub_dprintf ("usb", "Too much data for debug print...\n");
!       if (err) goto CheckCSW;
      }
  
!   /* Read the status - (maybe) according to specification.  */
! CheckCSW:
    err = grub_usb_bulk_read (dev->dev, dev->in->endp_addr & 15,
  			    sizeof (status), (char *) &status);
    if (err)
      {
!       grub_usb_clear_halt (dev->dev, dev->in->endp_addr);
!       err = grub_usb_bulk_read (dev->dev, dev->in->endp_addr & 15,
! 			        sizeof (status), (char *) &status);
!       if (err)
!         { /* Bulk-only reset device. */
!           grub_usbms_reset (dev->dev, dev->interface);
!           grub_usb_clear_halt (dev->dev, dev->in->endp_addr);
!           grub_usb_clear_halt (dev->dev, dev->out->endp_addr);
  	  goto retry;
!         }
      }
  
!   /* Debug print of CSW content. */
!   grub_dprintf ("usb", "CSW: sign=0x%08x tag=0x%08x resid=0x%08x\n",
!   	status.signature, status.tag, status.residue);
!   grub_dprintf ("usb", "CSW: status=0x%02x\n", status.status);
!   
!   /* If phase error, do bulk-only reset device. */
    if (status.status == 2)
      {
        grub_usbms_reset (dev->dev, dev->interface);
        grub_usb_clear_halt (dev->dev, dev->in->endp_addr);
        grub_usb_clear_halt (dev->dev, dev->out->endp_addr);
diff -crB grub2-1.98~experimental.20100120/include/grub/scsicmd.h grub2-1.98~my_patched.20100312/include/grub/scsicmd.h
*** grub2-1.98~experimental.20100120/include/grub/scsicmd.h	2010-01-20 23:42:31.000000000 +0100
--- grub2-1.98~my_patched.20100312/include/grub/scsicmd.h	2010-03-09 18:19:07.000000000 +0100
***************
*** 25,38 ****
  #define GRUB_SCSI_REMOVABLE_BIT	7
  #define GRUB_SCSI_LUN_SHIFT	5
  
! struct grub_scsi_inquiry
  {
    grub_uint8_t opcode;
!   grub_uint8_t lun;
!   grub_uint16_t reserved;
!   grub_uint16_t alloc_length;
    grub_uint8_t reserved2;
!   grub_uint8_t pad[5];
  } __attribute__((packed));
  
  struct grub_scsi_inquiry_data
--- 25,48 ----
  #define GRUB_SCSI_REMOVABLE_BIT	7
  #define GRUB_SCSI_LUN_SHIFT	5
  
! struct grub_scsi_test_unit_ready
  {
    grub_uint8_t opcode;
!   grub_uint8_t lun; /* 7-5 LUN, 4-0 reserved */
!   grub_uint8_t reserved1;
    grub_uint8_t reserved2;
!   grub_uint8_t reserved3;
!   grub_uint8_t control;
! } __attribute__((packed));
! 
! struct grub_scsi_inquiry
! {
!   grub_uint8_t opcode;
!   grub_uint8_t lun; /* 7-5 LUN, 4-1 reserved, 0 EVPD */
!   grub_uint8_t page; /* page code if EVPD=1 */
!   grub_uint8_t reserved;
!   grub_uint8_t alloc_length;
!   grub_uint8_t control;
  } __attribute__((packed));
  
  struct grub_scsi_inquiry_data
***************
*** 47,58 ****
    char prodrev[4];
  } __attribute__((packed));
  
  struct grub_scsi_read_capacity
  {
    grub_uint8_t opcode;
!   grub_uint8_t lun;
!   grub_uint8_t reserved[8];
!   grub_uint8_t pad[2];
  } __attribute__((packed));
  
  struct grub_scsi_read_capacity_data
--- 57,96 ----
    char prodrev[4];
  } __attribute__((packed));
  
+ struct grub_scsi_request_sense
+ {
+   grub_uint8_t opcode;
+   grub_uint8_t lun; /* 7-5 LUN, 4-0 reserved */
+   grub_uint8_t reserved1;
+   grub_uint8_t reserved2;
+   grub_uint8_t alloc_length;
+   grub_uint8_t control;
+ } __attribute__((packed));
+ 
+ struct grub_scsi_request_sense_data
+ {
+   grub_uint8_t error_code; /* 7 Valid, 6-0 Err. code */
+   grub_uint8_t segment_number;
+   grub_uint8_t sense_key; /*7 FileMark, 6 EndOfMedia, 5 ILI, 4-0 sense key */
+   grub_uint32_t information;
+   grub_uint8_t additional_sense_length;
+   grub_uint32_t cmd_specific_info;
+   grub_uint8_t additional_sense_code;
+   grub_uint8_t additional_sense_code_qualifier;
+   grub_uint8_t field_replaceable_unit_code;
+   grub_uint8_t sense_key_specific[3];
+   /* there can be additional sense field */
+ } __attribute__((packed));
+ 
  struct grub_scsi_read_capacity
  {
    grub_uint8_t opcode;
!   grub_uint8_t lun; /* 7-5 LUN, 4-1 reserved, 0 reserved */
!   grub_uint32_t logical_block_addr; /* only if PMI=1 */
!   grub_uint8_t reserved1;
!   grub_uint8_t reserved2;
!   grub_uint8_t PMI;
!   grub_uint8_t control;
  } __attribute__((packed));
  
  struct grub_scsi_read_capacity_data
***************
*** 106,116 ****
  typedef enum
    {
      grub_scsi_cmd_inquiry = 0x12,
      grub_scsi_cmd_read_capacity = 0x25,
      grub_scsi_cmd_read10 = 0x28,
      grub_scsi_cmd_write10 = 0x2a,
      grub_scsi_cmd_read12 = 0xa8,
!     grub_scsi_cmd_write12 = 0xaa
    } grub_scsi_cmd_t;
  
  typedef enum
--- 144,156 ----
  typedef enum
    {
      grub_scsi_cmd_inquiry = 0x12,
+     grub_scsi_cmd_test_unit_ready = 0x00,
      grub_scsi_cmd_read_capacity = 0x25,
      grub_scsi_cmd_read10 = 0x28,
      grub_scsi_cmd_write10 = 0x2a,
      grub_scsi_cmd_read12 = 0xa8,
!     grub_scsi_cmd_write12 = 0xaa,
!     grub_scsi_cmd_request_sense = 0x03
    } grub_scsi_cmd_t;
  
  typedef enum
diff -crB grub2-1.98~experimental.20100120/include/grub/usbtrans.h grub2-1.98~my_patched.20100312/include/grub/usbtrans.h
*** grub2-1.98~experimental.20100120/include/grub/usbtrans.h	2010-01-20 23:42:31.000000000 +0100
--- grub2-1.98~my_patched.20100312/include/grub/usbtrans.h	2010-03-06 13:04:12.000000000 +0100
***************
*** 86,94 ****
  
  #define GRUB_USB_REQ_HUB_GET_PORT_STATUS 0x00
  
! #define GRUB_USB_FEATURE_ENDP_HALT	0x01
! #define GRUB_USB_FEATURE_DEV_REMOTE_WU	0x02
! #define GRUB_USB_FEATURE_TEST_MODE	0x04
  
  #define GRUB_USB_HUB_STATUS_CONNECTED	(1 << 0)
  #define GRUB_USB_HUB_STATUS_LOWSPEED	(1 << 9)
--- 86,94 ----
  
  #define GRUB_USB_REQ_HUB_GET_PORT_STATUS 0x00
  
! #define GRUB_USB_FEATURE_ENDP_HALT	0x00
! #define GRUB_USB_FEATURE_DEV_REMOTE_WU	0x01
! #define GRUB_USB_FEATURE_TEST_MODE	0x02
  
  #define GRUB_USB_HUB_STATUS_CONNECTED	(1 << 0)
  #define GRUB_USB_HUB_STATUS_LOWSPEED	(1 << 9)

