target/linux/brcm2708/patches-3.10/0072-USB-fix-using-a-FIQ-to-implement-split-transactions.patch

   1 From d0f04f1be88d3ad42f3c875f9ab45a1cdb2129bd Mon Sep 17 00:00:00 2001
   2 From: Gordon Hollingworth <gordon@holliweb.co.uk>
   3 Date: Thu, 4 Apr 2013 11:05:21 +0100
   4 Subject: [PATCH 072/174] USB fix using a FIQ to implement split transactions
   5
   6 This commit adds a FIQ implementaion that schedules
   7 the split transactions using a FIQ so we don't get
   8 held off by the interrupt latency of Linux
   9 ---
  10  .../usb/host/dwc_common_port/dwc_common_linux.c    |   7 +-
  11  drivers/usb/host/dwc_otg/dwc_otg_cil_intr.c        |  37 +-
  12  drivers/usb/host/dwc_otg/dwc_otg_driver.c          |   6 +-
  13  drivers/usb/host/dwc_otg/dwc_otg_hcd.c             | 125 ++++-
  14  drivers/usb/host/dwc_otg/dwc_otg_hcd.h             |  20 +-
  15  drivers/usb/host/dwc_otg/dwc_otg_hcd_intr.c        | 538 ++++++++++++++++++---
  16  drivers/usb/host/dwc_otg/dwc_otg_hcd_linux.c       |  18 +-
  17  drivers/usb/host/dwc_otg/dwc_otg_hcd_queue.c       |  31 +-
  18  drivers/usb/host/dwc_otg/dwc_otg_mphi_fix.h        |  26 +-
  19  drivers/usb/host/dwc_otg/dwc_otg_pcd_intr.c        |   2 +-
  20  10 files changed, 696 insertions(+), 114 deletions(-)
  21
  22 --- a/drivers/usb/host/dwc_common_port/dwc_common_linux.c
  23 +++ b/drivers/usb/host/dwc_common_port/dwc_common_linux.c
  24 @@ -580,7 +580,12 @@ void DWC_WRITE_REG64(uint64_t volatile *
  25
  26  void DWC_MODIFY_REG32(uint32_t volatile *reg, uint32_t clear_mask, uint32_t set_mask)
  27  {
  28 +       unsigned long flags;
  29 +
  30 +       local_irq_save(flags);
  31 +       local_fiq_disable();
  32         writel((readl(reg) & ~clear_mask) | set_mask, reg);
  33 +       local_irq_restore(flags);
  34  }
  35
  36  #if 0
  37 @@ -1301,7 +1306,7 @@ EXPORT_SYMBOL(DWC_EXCEPTION);
  38  EXPORT_SYMBOL(__DWC_DEBUG);
  39  #endif
  40
  41 -EXPORT_SYMBOL(__DWC_DMA_ALLOC);\r
  42 +EXPORT_SYMBOL(__DWC_DMA_ALLOC);
  43  EXPORT_SYMBOL(__DWC_DMA_ALLOC_ATOMIC);
  44  EXPORT_SYMBOL(__DWC_DMA_FREE);
  45  EXPORT_SYMBOL(__DWC_ALLOC);
  46 --- a/drivers/usb/host/dwc_otg/dwc_otg_cil_intr.c
  47 +++ b/drivers/usb/host/dwc_otg/dwc_otg_cil_intr.c
  48 @@ -47,8 +47,6 @@
  49  #include "dwc_otg_hcd.h"
  50  #include "dwc_otg_mphi_fix.h"
  51
  52 -extern bool fiq_fix_enable;
  53 -
  54  #ifdef DEBUG
  55  inline const char *op_state_str(dwc_otg_core_if_t * core_if)
  56  {
  57 @@ -1321,7 +1319,7 @@ static int32_t dwc_otg_handle_lpm_intr(d
  58  /**
  59   * This function returns the Core Interrupt register.
  60   */
  61 -static inline uint32_t dwc_otg_read_common_intr(dwc_otg_core_if_t * core_if)
  62 +static inline uint32_t dwc_otg_read_common_intr(dwc_otg_core_if_t * core_if, gintmsk_data_t *reenable_gintmsk)
  63  {
  64         gahbcfg_data_t gahbcfg = {.d32 = 0 };
  65         gintsts_data_t gintsts;
  66 @@ -1338,19 +1336,33 @@ static inline uint32_t dwc_otg_read_comm
  67         gintmsk_common.b.lpmtranrcvd = 1;
  68  #endif
  69         gintmsk_common.b.restoredone = 1;
  70 -       /** @todo: The port interrupt occurs while in device
  71 -         * mode. Added code to CIL to clear the interrupt for now!
  72 -         */
  73 -       gintmsk_common.b.portintr = 1;
  74 -
  75 +       if(dwc_otg_is_device_mode(core_if))
  76 +       {
  77 +               /** @todo: The port interrupt occurs while in device
  78 +                * mode. Added code to CIL to clear the interrupt for now!
  79 +                */
  80 +               gintmsk_common.b.portintr = 1;
  81 +       }
  82         gintsts.d32 = DWC_READ_REG32(&core_if->core_global_regs->gintsts);
  83         gintmsk.d32 = DWC_READ_REG32(&core_if->core_global_regs->gintmsk);
  84 +       {
  85 +               unsigned long flags;
  86 +
  87 +               // Re-enable the saved interrupts
  88 +               local_irq_save(flags);
  89 +               local_fiq_disable();
  90 +               gintmsk.d32 |= gintmsk_common.d32;
  91 +               gintsts_saved.d32 &= ~gintmsk_common.d32;
  92 +               reenable_gintmsk->d32 = gintmsk.d32;
  93 +               local_irq_restore(flags);
  94 +       }
  95 +
  96         gahbcfg.d32 = DWC_READ_REG32(&core_if->core_global_regs->gahbcfg);
  97
  98  #ifdef DEBUG
  99         /* if any common interrupts set */
 100         if (gintsts.d32 & gintmsk_common.d32) {
 101 -               DWC_DEBUGPL(DBG_ANY, "gintsts=%08x  gintmsk=%08x\n",
 102 +               DWC_DEBUGPL(DBG_ANY, "common_intr: gintsts=%08x  gintmsk=%08x\n",
 103                             gintsts.d32, gintmsk.d32);
 104         }
 105  #endif
 106 @@ -1394,6 +1406,7 @@ int32_t dwc_otg_handle_common_intr(void
 107  {
 108         int retval = 0;
 109         gintsts_data_t gintsts;
 110 +       gintmsk_data_t reenable_gintmsk;
 111         gpwrdn_data_t gpwrdn = {.d32 = 0 };
 112         dwc_otg_device_t *otg_dev = dev;
 113         dwc_otg_core_if_t *core_if = otg_dev->core_if;
 114 @@ -1415,7 +1428,7 @@ int32_t dwc_otg_handle_common_intr(void
 115         }
 116
 117         if (core_if->hibernation_suspend <= 0) {
 118 -               gintsts.d32 = dwc_otg_read_common_intr(core_if);
 119 +               gintsts.d32 = dwc_otg_read_common_intr(core_if, &reenable_gintmsk);
 120
 121                 if (gintsts.b.modemismatch) {
 122                         retval |= dwc_otg_handle_mode_mismatch_intr(core_if);
 123 @@ -1512,8 +1525,12 @@ int32_t dwc_otg_handle_common_intr(void
 124                         gintsts.b.portintr = 1;
 125                         DWC_WRITE_REG32(&core_if->core_global_regs->gintsts,gintsts.d32);
 126                         retval |= 1;
 127 +                       reenable_gintmsk.b.portintr = 1;
 128
 129                 }
 130 +
 131 +               DWC_WRITE_REG32(&core_if->core_global_regs->gintmsk, reenable_gintmsk.d32);
 132 +
 133         } else {
 134                 DWC_DEBUGPL(DBG_ANY, "gpwrdn=%08x\n", gpwrdn.d32);
 135
 136 --- a/drivers/usb/host/dwc_otg/dwc_otg_driver.c
 137 +++ b/drivers/usb/host/dwc_otg/dwc_otg_driver.c
 138 @@ -242,7 +242,8 @@ static struct dwc_otg_driver_module_para
 139
 140  //Global variable to switch the fiq fix on or off (declared in bcm2708.c)
 141  extern bool fiq_fix_enable;
 142 -
 143 +// Global variable to enable the split transaction fix
 144 +bool fiq_split_enable = true;
 145  //Global variable to switch the nak holdoff on or off
 146  bool nak_holdoff_enable = true;
 147
 148 @@ -1090,6 +1091,7 @@ static int __init dwc_otg_driver_init(vo
 149         }
 150         printk(KERN_DEBUG "dwc_otg: FIQ %s\n", fiq_fix_enable ? "enabled":"disabled");
 151         printk(KERN_DEBUG "dwc_otg: NAK holdoff %s\n", nak_holdoff_enable ? "enabled":"disabled");
 152 +       printk(KERN_DEBUG "dwc_otg: FIQ split fix %s\n", fiq_split_enable ? "enabled":"disabled");
 153
 154         error = driver_create_file(drv, &driver_attr_version);
 155  #ifdef DEBUG
 156 @@ -1374,6 +1376,8 @@ module_param(fiq_fix_enable, bool, 0444)
 157  MODULE_PARM_DESC(fiq_fix_enable, "Enable the fiq fix");
 158  module_param(nak_holdoff_enable, bool, 0444);
 159  MODULE_PARM_DESC(nak_holdoff_enable, "Enable the NAK holdoff");
 160 +module_param(fiq_split_enable, bool, 0444);
 161 +MODULE_PARM_DESC(fiq_split_enable, "Enable the FIQ fix on split transactions");
 162
 163  /** @page "Module Parameters"
 164   *
 165 --- a/drivers/usb/host/dwc_otg/dwc_otg_hcd.c
 166 +++ b/drivers/usb/host/dwc_otg/dwc_otg_hcd.c
 167 @@ -45,6 +45,7 @@
 168
 169  #include "dwc_otg_hcd.h"
 170  #include "dwc_otg_regs.h"
 171 +#include "dwc_otg_mphi_fix.h"
 172
 173  extern bool microframe_schedule, nak_holdoff_enable;
 174
 175 @@ -581,6 +582,8 @@ int dwc_otg_hcd_urb_dequeue(dwc_otg_hcd_
 176                          */
 177                         dwc_otg_hc_halt(hcd->core_if, qh->channel,
 178                                         DWC_OTG_HC_XFER_URB_DEQUEUE);
 179 +
 180 +                       dwc_otg_hcd_release_port(hcd, qh);
 181                 }
 182         }
 183
 184 @@ -716,6 +719,8 @@ static void completion_tasklet_func(void
 185
 186                 usb_hcd_giveback_urb(hcd->priv, urb, urb->status);
 187
 188 +               fiq_print(FIQDBG_PORTHUB, "COMPLETE");
 189 +
 190                 DWC_SPINLOCK_IRQSAVE(hcd->lock, &flags);
 191         }
 192         DWC_SPINUNLOCK_IRQRESTORE(hcd->lock, flags);
 193 @@ -979,6 +984,10 @@ int dwc_otg_hcd_init(dwc_otg_hcd_t * hcd
 194         hcd->frame_list = NULL;
 195         hcd->frame_list_dma = 0;
 196         hcd->periodic_qh_count = 0;
 197 +
 198 +       DWC_MEMSET(hcd->hub_port, 0, sizeof(hcd->hub_port));
 199 +       DWC_MEMSET(hcd->hub_port_alloc, -1, sizeof(hcd->hub_port_alloc));
 200 +
 201  out:
 202         return retval;
 203  }
 204 @@ -1124,7 +1133,12 @@ static void assign_and_init_hc(dwc_otg_h
 205                 uint32_t hub_addr, port_addr;
 206                 hc->do_split = 1;
 207                 hc->xact_pos = qtd->isoc_split_pos;
 208 -               hc->complete_split = qtd->complete_split;
 209 +               /* We don't need to do complete splits anymore */
 210 +               if(fiq_split_enable)
 211 +                       hc->complete_split = qtd->complete_split = 0;
 212 +               else
 213 +                       hc->complete_split = qtd->complete_split;
 214 +
 215                 hcd->fops->hub_info(hcd, urb->priv, &hub_addr, &port_addr);
 216                 hc->hub_addr = (uint8_t) hub_addr;
 217                 hc->port_addr = (uint8_t) port_addr;
 218 @@ -1271,6 +1285,62 @@ static void assign_and_init_hc(dwc_otg_h
 219         hc->qh = qh;
 220  }
 221
 222 +/*
 223 +** Check the transaction to see if the port / hub has already been assigned for
 224 +** a split transaction
 225 +**
 226 +** Return 0 - Port is already in use
 227 +*/
 228 +int dwc_otg_hcd_allocate_port(dwc_otg_hcd_t * hcd, dwc_otg_qh_t *qh)
 229 +{
 230 +       uint32_t hub_addr, port_addr;
 231 +
 232 +       if(!fiq_split_enable)
 233 +               return 0;
 234 +
 235 +       hcd->fops->hub_info(hcd, DWC_CIRCLEQ_FIRST(&qh->qtd_list)->urb->priv, &hub_addr, &port_addr);
 236 +
 237 +       if(hcd->hub_port[hub_addr] & (1 << port_addr))
 238 +       {
 239 +               fiq_print(FIQDBG_PORTHUB, "H%dP%d:S%02d", hub_addr, port_addr, qh->skip_count);
 240 +
 241 +               qh->skip_count++;
 242 +
 243 +               if(qh->skip_count > 40000)
 244 +               {
 245 +                       printk_once(KERN_ERR "Error: Having to skip port allocation");
 246 +                       local_fiq_disable();
 247 +                       BUG();
 248 +                       return 0;
 249 +               }
 250 +               return 1;
 251 +       }
 252 +       else
 253 +       {
 254 +               qh->skip_count = 0;
 255 +               hcd->hub_port[hub_addr] |= 1 << port_addr;
 256 +               fiq_print(FIQDBG_PORTHUB, "H%dP%d:A %d", hub_addr, port_addr, DWC_CIRCLEQ_FIRST(&qh->qtd_list)->urb->pipe_info.ep_num);
 257 +               hcd->hub_port_alloc[hub_addr * 16 + port_addr] = dwc_otg_hcd_get_frame_number(hcd);
 258 +               return 0;
 259 +       }
 260 +}
 261 +void dwc_otg_hcd_release_port(dwc_otg_hcd_t * hcd, dwc_otg_qh_t *qh)
 262 +{
 263 +       uint32_t hub_addr, port_addr;
 264 +
 265 +       if(!fiq_split_enable)
 266 +               return;
 267 +
 268 +       hcd->fops->hub_info(hcd, DWC_CIRCLEQ_FIRST(&qh->qtd_list)->urb->priv, &hub_addr, &port_addr);
 269 +
 270 +       hcd->hub_port[hub_addr] &= ~(1 << port_addr);
 271 +       hcd->hub_port_alloc[hub_addr * 16 + port_addr] = -1;
 272 +
 273 +       fiq_print(FIQDBG_PORTHUB, "H%dP%d:RO%d", hub_addr, port_addr, DWC_CIRCLEQ_FIRST(&qh->qtd_list)->urb->pipe_info.ep_num);
 274 +
 275 +}
 276 +
 277 +
 278  /**
 279   * This function selects transactions from the HCD transfer schedule and
 280   * assigns them to available host channels. It is called from HCD interrupt
 281 @@ -1304,11 +1374,22 @@ dwc_otg_transaction_type_e dwc_otg_hcd_s
 282
 283         while (qh_ptr != &hcd->periodic_sched_ready &&
 284                !DWC_CIRCLEQ_EMPTY(&hcd->free_hc_list)) {
 285 +
 286 +               qh = DWC_LIST_ENTRY(qh_ptr, dwc_otg_qh_t, qh_list_entry);
 287 +
 288 +               if(qh->do_split && dwc_otg_hcd_allocate_port(hcd, qh))
 289 +               {
 290 +                       qh_ptr = DWC_LIST_NEXT(qh_ptr);
 291 +                       g_next_sched_frame = dwc_frame_num_inc(dwc_otg_hcd_get_frame_number(hcd), 1);
 292 +                       continue;
 293 +               }
 294 +
 295                 if (microframe_schedule) {
 296                         // Make sure we leave one channel for non periodic transactions.
 297                         DWC_SPINLOCK_IRQSAVE(channel_lock, &flags);
 298                         if (hcd->available_host_channels <= 1) {
 299                                 DWC_SPINUNLOCK_IRQRESTORE(channel_lock, flags);
 300 +                               if(qh->do_split) dwc_otg_hcd_release_port(hcd, qh);
 301                                 break;
 302                         }
 303                         hcd->available_host_channels--;
 304 @@ -1329,8 +1410,6 @@ dwc_otg_transaction_type_e dwc_otg_hcd_s
 305                 DWC_LIST_MOVE_HEAD(&hcd->periodic_sched_assigned,
 306                                    &qh->qh_list_entry);
 307                 DWC_SPINUNLOCK_IRQRESTORE(channel_lock, flags);
 308 -
 309 -               ret_val = DWC_OTG_TRANSACTION_PERIODIC;
 310         }
 311
 312         /*
 313 @@ -1369,10 +1448,19 @@ dwc_otg_transaction_type_e dwc_otg_hcd_s
 314                                 qh->nak_frame = 0xffff;
 315                         }
 316                 }
 317 +
 318 +               if (qh->do_split && dwc_otg_hcd_allocate_port(hcd, qh))
 319 +               {
 320 +                       g_next_sched_frame = dwc_frame_num_inc(dwc_otg_hcd_get_frame_number(hcd), 1);
 321 +                       qh_ptr = DWC_LIST_NEXT(qh_ptr);
 322 +                       continue;
 323 +               }
 324 +
 325                 if (microframe_schedule) {
 326                                 DWC_SPINLOCK_IRQSAVE(channel_lock, &flags);
 327                                 if (hcd->available_host_channels < 1) {
 328                                         DWC_SPINUNLOCK_IRQRESTORE(channel_lock, flags);
 329 +                                       if(qh->do_split) dwc_otg_hcd_release_port(hcd, qh);
 330                                         break;
 331                                 }
 332                                 hcd->available_host_channels--;
 333 @@ -1396,16 +1484,17 @@ dwc_otg_transaction_type_e dwc_otg_hcd_s
 334
 335                 g_np_sent++;
 336
 337 -               if (ret_val == DWC_OTG_TRANSACTION_NONE) {
 338 -                       ret_val = DWC_OTG_TRANSACTION_NON_PERIODIC;
 339 -               } else {
 340 -                       ret_val = DWC_OTG_TRANSACTION_ALL;
 341 -               }
 342 -
 343                 if (!microframe_schedule)
 344                         hcd->non_periodic_channels++;
 345         }
 346
 347 +       if(!DWC_LIST_EMPTY(&hcd->periodic_sched_assigned))
 348 +               ret_val |= DWC_OTG_TRANSACTION_PERIODIC;
 349 +
 350 +       if(!DWC_LIST_EMPTY(&hcd->non_periodic_sched_active))
 351 +               ret_val |= DWC_OTG_TRANSACTION_NON_PERIODIC;
 352 +
 353 +
 354  #ifdef DEBUG_HOST_CHANNELS
 355         last_sel_trans_num_avail_hc_at_end = hcd->available_host_channels;
 356  #endif /* DEBUG_HOST_CHANNELS */
 357 @@ -1522,6 +1611,15 @@ static void process_periodic_channels(dw
 358
 359                 qh = DWC_LIST_ENTRY(qh_ptr, dwc_otg_qh_t, qh_list_entry);
 360
 361 +               // Do not send a split start transaction any later than frame .6
 362 +               // Note, we have to schedule a periodic in .5 to make it go in .6
 363 +               if(fiq_split_enable && qh->do_split && ((dwc_otg_hcd_get_frame_number(hcd) + 1) & 7) > 6)
 364 +               {
 365 +                       qh_ptr = qh_ptr->next;
 366 +                       g_next_sched_frame = dwc_otg_hcd_get_frame_number(hcd) | 7;
 367 +                       continue;
 368 +               }
 369 +
 370                 /*
 371                  * Set a flag if we're queuing high-bandwidth in slave mode.
 372                  * The flag prevents any halts to get into the request queue in
 373 @@ -1651,6 +1749,15 @@ static void process_non_periodic_channel
 374
 375                 qh = DWC_LIST_ENTRY(hcd->non_periodic_qh_ptr, dwc_otg_qh_t,
 376                                     qh_list_entry);
 377 +
 378 +               // Do not send a split start transaction any later than frame .5
 379 +               // non periodic transactions will start immediately in this uframe
 380 +               if(fiq_split_enable && qh->do_split && ((dwc_otg_hcd_get_frame_number(hcd) + 1) & 7) > 6)
 381 +               {
 382 +                       g_next_sched_frame = dwc_otg_hcd_get_frame_number(hcd) | 7;
 383 +                       break;
 384 +               }
 385 +
 386                 status =
 387                     queue_transaction(hcd, qh->channel,
 388                                       tx_status.b.nptxfspcavail);
 389 --- a/drivers/usb/host/dwc_otg/dwc_otg_hcd.h
 390 +++ b/drivers/usb/host/dwc_otg/dwc_otg_hcd.h
 391 @@ -168,10 +168,10 @@ typedef enum dwc_otg_control_phase {
 392
 393  /** Transaction types. */
 394  typedef enum dwc_otg_transaction_type {
 395 -       DWC_OTG_TRANSACTION_NONE,
 396 -       DWC_OTG_TRANSACTION_PERIODIC,
 397 -       DWC_OTG_TRANSACTION_NON_PERIODIC,
 398 -       DWC_OTG_TRANSACTION_ALL
 399 +       DWC_OTG_TRANSACTION_NONE          = 0,
 400 +       DWC_OTG_TRANSACTION_PERIODIC      = 1,
 401 +       DWC_OTG_TRANSACTION_NON_PERIODIC  = 2,
 402 +       DWC_OTG_TRANSACTION_ALL           = DWC_OTG_TRANSACTION_PERIODIC + DWC_OTG_TRANSACTION_NON_PERIODIC
 403  } dwc_otg_transaction_type_e;
 404
 405  struct dwc_otg_qh;
 406 @@ -370,6 +370,8 @@ typedef struct dwc_otg_qh {
 407
 408         uint16_t speed;
 409         uint16_t frame_usecs[8];
 410 +
 411 +       uint32_t skip_count;
 412  } dwc_otg_qh_t;
 413
 414  DWC_CIRCLEQ_HEAD(hc_list, dwc_hc);
 415 @@ -574,6 +576,10 @@ struct dwc_otg_hcd {
 416         /** Frame List */
 417         uint32_t *frame_list;
 418
 419 +       /** Hub - Port assignment */
 420 +       int hub_port[16];
 421 +       int hub_port_alloc[256];
 422 +
 423         /** Frame List DMA address */
 424         dma_addr_t frame_list_dma;
 425
 426 @@ -604,12 +610,16 @@ extern dwc_otg_transaction_type_e dwc_ot
 427  extern void dwc_otg_hcd_queue_transactions(dwc_otg_hcd_t * hcd,
 428                                            dwc_otg_transaction_type_e tr_type);
 429
 430 +int dwc_otg_hcd_allocate_port(dwc_otg_hcd_t * hcd, dwc_otg_qh_t *qh);
 431 +void dwc_otg_hcd_release_port(dwc_otg_hcd_t * dwc_otg_hcd, dwc_otg_qh_t *qh);
 432 +
 433 +
 434  /** @} */
 435
 436  /** @name Interrupt Handler Functions */
 437  /** @{ */
 438  extern int32_t dwc_otg_hcd_handle_intr(dwc_otg_hcd_t * dwc_otg_hcd);
 439 -extern int32_t dwc_otg_hcd_handle_sof_intr(dwc_otg_hcd_t * dwc_otg_hcd, int32_t);
 440 +extern int32_t dwc_otg_hcd_handle_sof_intr(dwc_otg_hcd_t * dwc_otg_hcd);
 441  extern int32_t dwc_otg_hcd_handle_rx_status_q_level_intr(dwc_otg_hcd_t *
 442                                                          dwc_otg_hcd);
 443  extern int32_t dwc_otg_hcd_handle_np_tx_fifo_empty_intr(dwc_otg_hcd_t *
 444 --- a/drivers/usb/host/dwc_otg/dwc_otg_hcd_intr.c
 445 +++ b/drivers/usb/host/dwc_otg/dwc_otg_hcd_intr.c
 446 @@ -38,6 +38,7 @@
 447
 448  #include <linux/jiffies.h>
 449  #include <mach/hardware.h>
 450 +#include <asm/fiq.h>
 451
 452
 453  extern bool microframe_schedule;
 454 @@ -52,21 +53,295 @@ extern bool microframe_schedule;
 455
 456  void * dummy_send;
 457  mphi_regs_t c_mphi_regs;
 458 +volatile void *dwc_regs_base;
 459  int fiq_done, int_done;
 460 -int g_next_sched_frame, g_np_count, g_np_sent, g_work_expected;
 461 -static int mphi_int_count = 0 ;
 462
 463 -extern bool fiq_fix_enable, nak_holdoff_enable;
 464 +gintsts_data_t  gintsts_saved = {.d32 = 0};
 465 +hcint_data_t    hcint_saved[MAX_EPS_CHANNELS];
 466 +hcintmsk_data_t hcintmsk_saved[MAX_EPS_CHANNELS];
 467 +int             split_out_xfersize[MAX_EPS_CHANNELS];
 468 +haint_data_t    haint_saved;
 469 +
 470 +int g_next_sched_frame, g_np_count, g_np_sent;
 471 +static int mphi_int_count = 0 ;
 472
 473  hcchar_data_t nak_hcchar;
 474  hctsiz_data_t nak_hctsiz;
 475  hcsplt_data_t nak_hcsplt;
 476  int nak_count;
 477
 478 +int complete_sched[MAX_EPS_CHANNELS] = { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1};
 479 +int split_start_frame[MAX_EPS_CHANNELS];
 480 +int queued_port[MAX_EPS_CHANNELS];
 481 +
 482 +#ifdef FIQ_DEBUG
 483 +char buffer[1000*16];
 484 +int wptr;
 485 +void _fiq_print(FIQDBG_T dbg_lvl, char *fmt, ...)
 486 +{
 487 +       FIQDBG_T dbg_lvl_req = FIQDBG_PORTHUB;
 488 +       va_list args;
 489 +       char text[17];
 490 +       hfnum_data_t hfnum = { .d32 = FIQ_READ(dwc_regs_base + 0x408) };
 491 +       unsigned long flags;
 492 +
 493 +       local_irq_save(flags);
 494 +       local_fiq_disable();
 495 +       if(dbg_lvl & dbg_lvl_req || dbg_lvl == FIQDBG_ERR)
 496 +       {
 497 +               snprintf(text, 9, "%4d%d:%d ", hfnum.b.frnum/8, hfnum.b.frnum%8, 8 - hfnum.b.frrem/937);
 498 +               va_start(args, fmt);
 499 +               vsnprintf(text+8, 9, fmt, args);
 500 +               va_end(args);
 501 +
 502 +               memcpy(buffer + wptr, text, 16);
 503 +               wptr = (wptr + 16) % sizeof(buffer);
 504 +       }
 505 +       local_irq_restore(flags);
 506 +}
 507 +#endif
 508 +
 509 +void fiq_queue_request(int channel, int odd_frame)
 510 +{
 511 +       hcchar_data_t   hcchar   = { .d32 = FIQ_READ(dwc_regs_base + 0x500 + (channel * 0x20) + 0x0)  };
 512 +       hcsplt_data_t   hcsplt   = { .d32 = FIQ_READ(dwc_regs_base + 0x500 + (channel * 0x20) + 0x4)  };
 513 +       hctsiz_data_t   hctsiz   = { .d32 = FIQ_READ(dwc_regs_base + 0x500 + (channel * 0x20) + 0x10) };
 514 +
 515 +       if(hcsplt.b.spltena     == 0)
 516 +       {
 517 +               fiq_print(FIQDBG_ERR, "SPLTENA ");
 518 +               BUG();
 519 +       }
 520 +
 521 +       if(hcchar.b.epdir == 1)
 522 +       {
 523 +               fiq_print(FIQDBG_SCHED, "IN  Ch %d", channel);
 524 +       }
 525 +       else
 526 +       {
 527 +               hctsiz.b.xfersize = 0;
 528 +               fiq_print(FIQDBG_SCHED, "OUT Ch %d", channel);
 529 +       }
 530 +       FIQ_WRITE((dwc_regs_base + 0x500 + (channel * 0x20) + 0x10), hctsiz.d32);
 531 +
 532 +       hcsplt.b.compsplt = 1;
 533 +       FIQ_WRITE((dwc_regs_base + 0x500 + (channel * 0x20) + 0x4), hcsplt.d32);
 534 +
 535 +       // Send the Split complete
 536 +       hcchar.b.chen = 1;
 537 +       hcchar.b.oddfrm = odd_frame ? 1 : 0;
 538 +
 539 +       // Post this for transmit on the next frame for periodic or this frame for non-periodic
 540 +       fiq_print(FIQDBG_SCHED, "SND_%s", odd_frame ? "ODD " : "EVEN");
 541 +
 542 +       FIQ_WRITE((dwc_regs_base + 0x500 + (channel * 0x20) + 0x0), hcchar.d32);
 543 +}
 544 +
 545 +static int last_sof = -1;
 546 +
 547 +/*
 548 +** Function to handle the start of frame interrupt, choose whether we need to do anything and
 549 +** therefore trigger the main interrupt
 550 +**
 551 +** returns int != 0 - interrupt has been handled
 552 +*/
 553 +int diff;
 554 +
 555 +int fiq_sof_handle(hfnum_data_t hfnum)
 556 +{
 557 +       int handled = 0;
 558 +       int i;
 559 +
 560 +       // Just check that once we're running we don't miss a SOF
 561 +       /*if(last_sof != -1 && (hfnum.b.frnum != ((last_sof + 1) & 0x3fff)))
 562 +       {
 563 +               fiq_print(FIQDBG_ERR, "LASTSOF ");
 564 +               fiq_print(FIQDBG_ERR, "%4d%d   ", last_sof / 8, last_sof & 7);
 565 +               fiq_print(FIQDBG_ERR, "%4d%d   ", hfnum.b.frnum / 8, hfnum.b.frnum & 7);
 566 +               BUG();
 567 +       }*/
 568 +
 569 +       // Only start remembering the last sof when the interrupt has been
 570 +       // enabled (we don't check the mask to come in here...)
 571 +       if(last_sof != -1 || FIQ_READ(dwc_regs_base + 0x18) & (1<<3))
 572 +               last_sof = hfnum.b.frnum;
 573 +
 574 +       for(i = 0; i < MAX_EPS_CHANNELS; i++)
 575 +       {
 576 +               if(complete_sched[i] != -1)
 577 +               {
 578 +                       if(complete_sched[i] <= hfnum.b.frnum || (complete_sched[i] > 0x3f00 && hfnum.b.frnum < 0xf0))
 579 +                       {
 580 +                               fiq_queue_request(i, hfnum.b.frnum & 1);
 581 +                               complete_sched[i] = -1;
 582 +                       }
 583 +               }
 584 +
 585 +               if(complete_sched[i] != -1)
 586 +               {
 587 +                       // This is because we've seen a split complete occur with no start...
 588 +                       // most likely because missed the complete 0x3fff frames ago!
 589 +
 590 +                       diff = (hfnum.b.frnum + 0x3fff - complete_sched[i]) & 0x3fff ;
 591 +                       if(diff > 32 && diff < 0x3f00)
 592 +                       {
 593 +                               fiq_print(FIQDBG_ERR, "SPLTMISS");
 594 +                               BUG();
 595 +                       }
 596 +               }
 597 +       }
 598 +
 599 +       if(g_np_count == g_np_sent && dwc_frame_num_gt(g_next_sched_frame, hfnum.b.frnum))
 600 +       {
 601 +               /*
 602 +                * If np_count != np_sent that means we need to queue non-periodic (bulk) packets this packet
 603 +                * g_next_sched_frame is the next frame we have periodic packets for
 604 +                *
 605 +                * if neither of these are required for this frame then just clear the interrupt
 606 +                */
 607 +               handled = 1;
 608 +
 609 +       }
 610 +
 611 +       return handled;
 612 +}
 613 +
 614 +int port_id(hcsplt_data_t hcsplt)
 615 +{
 616 +       return hcsplt.b.prtaddr + (hcsplt.b.hubaddr << 8);
 617 +}
 618 +
 619 +int fiq_hcintr_handle(int channel, hfnum_data_t hfnum)
 620 +{
 621 +       hcchar_data_t   hcchar   = { .d32 = FIQ_READ(dwc_regs_base + 0x500 + (channel * 0x20) + 0x0) };
 622 +       hcsplt_data_t   hcsplt   = { .d32 = FIQ_READ(dwc_regs_base + 0x500 + (channel * 0x20) + 0x4) };
 623 +       hcint_data_t    hcint    = { .d32 = FIQ_READ(dwc_regs_base + 0x500 + (channel * 0x20) + 0x8) };
 624 +       hcintmsk_data_t hcintmsk = { .d32 = FIQ_READ(dwc_regs_base + 0x500 + (channel * 0x20) + 0xc) };
 625 +       hctsiz_data_t   hctsiz   = { .d32 = FIQ_READ(dwc_regs_base + 0x500 + (channel * 0x20) + 0x10)};
 626 +
 627 +       hcint_saved[channel].d32 |= hcint.d32;
 628 +       hcintmsk_saved[channel].d32 =  hcintmsk.d32;
 629 +
 630 +       if(hcsplt.b.spltena)
 631 +       {
 632 +               fiq_print(FIQDBG_PORTHUB, "ph: %4x", port_id(hcsplt));
 633 +               if(hcint.b.chhltd)
 634 +               {
 635 +                       fiq_print(FIQDBG_SCHED, "CH HLT %d", channel);
 636 +                       fiq_print(FIQDBG_SCHED, "%08x", hcint_saved[channel]);
 637 +               }
 638 +               if(hcint.b.stall || hcint.b.xacterr || hcint.b.bblerr || hcint.b.frmovrun || hcint.b.datatglerr)
 639 +               {
 640 +                       queued_port[channel] = 0;
 641 +                       fiq_print(FIQDBG_ERR, "CHAN ERR");
 642 +               }
 643 +               if(hcint.b.xfercomp)
 644 +               {
 645 +                       // Clear the port allocation and transmit anything also on this port
 646 +                       queued_port[channel] = 0;
 647 +                       fiq_print(FIQDBG_SCHED, "XFERCOMP");
 648 +               }
 649 +               if(hcint.b.nak)
 650 +               {
 651 +                       queued_port[channel] = 0;
 652 +                       fiq_print(FIQDBG_SCHED, "NAK");
 653 +               }
 654 +               if(hcint.b.ack && !hcsplt.b.compsplt)
 655 +               {
 656 +                       int i;
 657 +
 658 +                       // Do not complete isochronous out transactions
 659 +                       if(hcchar.b.eptype == 1 && hcchar.b.epdir == 0)
 660 +                       {
 661 +                               queued_port[channel] = 0;
 662 +                               fiq_print(FIQDBG_SCHED, "ISOC_OUT");
 663 +                       }
 664 +                       else
 665 +                       {
 666 +                               // Make sure we check the port / hub combination that we sent this split on.
 667 +                               // Do not queue a second request to the same port
 668 +                               for(i = 0; i < MAX_EPS_CHANNELS; i++)
 669 +                               {
 670 +                                       if(port_id(hcsplt) == queued_port[i])
 671 +                                       {
 672 +                                               fiq_print(FIQDBG_ERR, "PORTERR ");
 673 +                                               //BUG();
 674 +                                       }
 675 +                               }
 676 +
 677 +                               split_start_frame[channel] = (hfnum.b.frnum + 1) & ~7;
 678 +
 679 +                               // Note, the size of an OUT is in the start split phase, not
 680 +                               // the complete split
 681 +                               split_out_xfersize[channel] = hctsiz.b.xfersize;
 682 +
 683 +                               hcint_saved[channel].b.chhltd = 0;
 684 +                               hcint_saved[channel].b.ack = 0;
 685 +
 686 +                               queued_port[channel] = port_id(hcsplt);
 687 +
 688 +                               if(hcchar.b.eptype & 1)
 689 +                               {
 690 +                                       // Send the periodic complete in the same oddness frame as the ACK went...
 691 +                                       fiq_queue_request(channel, !(hfnum.b.frnum & 1));
 692 +       //                              complete_sched[channel] = dwc_frame_num_inc(hfnum.b.frnum, 1);
 693 +                               }
 694 +                               else
 695 +                               {
 696 +                                       // Schedule the split complete to occur later
 697 +                                       complete_sched[channel] = dwc_frame_num_inc(hfnum.b.frnum, 2);
 698 +                                       fiq_print(FIQDBG_SCHED, "ACK%04d%d", complete_sched[channel]/8, complete_sched[channel]%8);
 699 +                               }
 700 +                       }
 701 +               }
 702 +               if(hcint.b.nyet)
 703 +               {
 704 +                       fiq_print(FIQDBG_ERR, "NYETERR1");
 705 +                       //BUG();
 706 +                       // Can transmit a split complete up to uframe .0 of the next frame
 707 +                       if(hfnum.b.frnum <= dwc_frame_num_inc(split_start_frame[channel], 8))
 708 +                       {
 709 +                               // Send it next frame
 710 +                               if(hcchar.b.eptype & 1) // type 1 & 3 are interrupt & isoc
 711 +                               {
 712 +                                       fiq_print(FIQDBG_SCHED, "NYT:SEND");
 713 +                                       fiq_queue_request(channel, !(hfnum.b.frnum & 1));
 714 +                               }
 715 +                               else
 716 +                               {
 717 +                                       // Schedule non-periodic access for next frame (the odd-even bit doesn't effect NP)
 718 +                                       complete_sched[channel] = dwc_frame_num_inc(hfnum.b.frnum, 1);
 719 +                                       fiq_print(FIQDBG_SCHED, "NYT%04d%d", complete_sched[channel]/8, complete_sched[channel]%8);
 720 +                               }
 721 +                               hcint_saved[channel].b.chhltd = 0;
 722 +                               hcint_saved[channel].b.nyet = 0;
 723 +                       }
 724 +                       else
 725 +                       {
 726 +                               queued_port[channel] = 0;
 727 +                               fiq_print(FIQDBG_ERR, "NYETERR2");
 728 +                               //BUG();
 729 +                       }
 730 +               }
 731 +       }
 732 +
 733 +       // Clear the interrupt, this will also clear the HAINT bit
 734 +       FIQ_WRITE((dwc_regs_base + 0x500 + (channel * 0x20) + 0x8), hcint.d32);
 735 +       return hcint_saved[channel].d32 == 0;
 736 +}
 737 +
 738 +gintsts_data_t gintsts;
 739 +gintmsk_data_t gintmsk;
 740 +// triggered: The set of interrupts that were triggered
 741 +// handled:   The set of interrupts that have been handled (no IRQ is
 742 +//            required)
 743 +// keep:      The set of interrupts we want to keep unmasked even though we
 744 +//            want to trigger an IRQ to handle it (SOF and HCINTR)
 745 +gintsts_data_t triggered, handled, keep;
 746 +hfnum_data_t hfnum;
 747 +
 748  void __attribute__ ((naked)) dwc_otg_hcd_handle_fiq(void)
 749  {
 750 -       gintsts_data_t gintsts;
 751 -       hfnum_data_t hfnum;
 752
 753         /* entry takes care to store registers we will be treading on here */
 754         asm __volatile__ (
 755 @@ -74,43 +349,112 @@ void __attribute__ ((naked)) dwc_otg_hcd
 756                 /* stash FIQ and normal regs */
 757                 "stmdb  sp!, {r0-r12,  lr};"
 758                 /* !! THIS SETS THE FRAME, adjust to > sizeof locals */
 759 -               "sub     fp, ip, #256 ;"
 760 +               "sub     fp, ip, #512 ;"
 761                 );
 762
 763 -       fiq_done++;
 764 -       gintsts.d32 = FIQ_READ_IO_ADDRESS(USB_BASE + 0x14) & FIQ_READ_IO_ADDRESS(USB_BASE + 0x18);
 765 -       hfnum.d32 =   FIQ_READ_IO_ADDRESS(USB_BASE + 0x408);
 766 -
 767 -       if(gintsts.d32)
 768 +       // Cannot put local variables at the beginning of the function
 769 +       // because otherwise 'C' will play with the stack pointer. any locals
 770 +       // need to be inside the following block
 771 +       do
 772         {
 773 -               if(gintsts.b.sofintr && g_np_count == g_np_sent && dwc_frame_num_gt(g_next_sched_frame, hfnum.b.frnum))
 774 +               fiq_done++;
 775 +               gintsts.d32 = FIQ_READ(dwc_regs_base + 0x14);
 776 +               gintmsk.d32 = FIQ_READ(dwc_regs_base + 0x18);
 777 +               hfnum.d32 =   FIQ_READ(dwc_regs_base + 0x408);
 778 +               triggered.d32 = gintsts.d32 & gintmsk.d32;
 779 +               handled.d32 = 0;
 780 +               keep.d32 = 0;
 781 +               fiq_print(FIQDBG_INT, "FIQ     ");
 782 +               fiq_print(FIQDBG_INT, "%08x", gintsts.d32);
 783 +               fiq_print(FIQDBG_INT, "%08x", gintmsk.d32);
 784 +               if(gintsts.d32)
 785                 {
 786 -                       /*
 787 -                        * If np_count != np_sent that means we need to queue non-periodic (bulk) packets this packet
 788 -                        * g_next_sched_frame is the next frame we have periodic packets for
 789 -                        *
 790 -                        * if neither of these are required for this frame then just clear the interrupt
 791 -                        */
 792 -                       gintsts.d32 = 0;
 793 -                       gintsts.b.sofintr = 1;
 794 -                       FIQ_WRITE_IO_ADDRESS((USB_BASE + 0x14), gintsts.d32);
 795 +                       // If port enabled
 796 +                       if((FIQ_READ(dwc_regs_base + 0x440) & 0xf) == 0x5)
 797 +                       {
 798 +                               if(gintsts.b.sofintr)
 799 +                               {
 800 +                                       if(fiq_sof_handle(hfnum))
 801 +                                       {
 802 +                                               handled.b.sofintr = 1; /* Handled in FIQ */
 803 +                                       }
 804 +                                       else
 805 +                                       {
 806 +                                               /* Keer interrupt unmasked */
 807 +                                               keep.b.sofintr = 1;
 808 +                                       }
 809 +                                       {
 810 +                                               // Need to make sure the read and clearing of the SOF interrupt is as close as possible to avoid the possibility of missing
 811 +                                               // a start of frame interrupt
 812 +                                               gintsts_data_t gintsts = { .b.sofintr = 1 };
 813 +                                               FIQ_WRITE((dwc_regs_base + 0x14), gintsts.d32);
 814 +                                       }
 815 +                               }
 816 +
 817 +                               if(fiq_split_enable && gintsts.b.hcintr)
 818 +                               {
 819 +                                       int i;
 820 +                                       haint_data_t    haint;
 821 +                                       haintmsk_data_t haintmsk;
 822 +
 823 +                                       haint.d32 = FIQ_READ(dwc_regs_base + 0x414);
 824 +                                       haintmsk.d32 = FIQ_READ(dwc_regs_base + 0x418);
 825 +                                       haint.d32 &= haintmsk.d32;
 826 +                                       haint_saved.d32 |= haint.d32;
 827 +
 828 +                                       fiq_print(FIQDBG_INT, "hcintr");
 829 +                                       fiq_print(FIQDBG_INT, "%08x", FIQ_READ(dwc_regs_base + 0x414));
 830 +
 831 +                                       // Go through each channel that has an enabled interrupt
 832 +                                       for(i = 0; i < 16; i++)
 833 +                                               if((haint.d32 >> i) & 1)
 834 +                                                       if(fiq_hcintr_handle(i, hfnum))
 835 +                                                               haint_saved.d32 &= ~(1 << i); /* this was handled */
 836 +
 837 +                                       /* If we've handled all host channel interrupts then don't trigger the interrupt */
 838 +                                       if(haint_saved.d32 == 0)
 839 +                                       {
 840 +                                               handled.b.hcintr = 1;
 841 +                                       }
 842 +                                       else
 843 +                                       {
 844 +                                               /* Make sure we keep the channel interrupt unmasked when triggering the IRQ */
 845 +                                               keep.b.hcintr = 1;
 846 +                                       }
 847
 848 -                       g_work_expected = 0;
 849 +                                       {
 850 +                                               gintsts_data_t gintsts = { .b.hcintr = 1 };
 851 +
 852 +                                               // Always clear the channel interrupt
 853 +                                               FIQ_WRITE((dwc_regs_base + 0x14), gintsts.d32);
 854 +                                       }
 855 +                               }
 856 +                       }
 857 +                       else
 858 +                       {
 859 +                               last_sof = -1;
 860 +                       }
 861                 }
 862 -               else
 863 +
 864 +               // Mask out the interrupts triggered - those handled - don't mask out the ones we want to keep
 865 +               gintmsk.d32 = keep.d32 | (gintmsk.d32 & ~(triggered.d32 & ~handled.d32));
 866 +               // Save those that were triggered but not handled
 867 +               gintsts_saved.d32 |= triggered.d32 & ~handled.d32;
 868 +               FIQ_WRITE(dwc_regs_base + 0x18, gintmsk.d32);
 869 +
 870 +               // Clear and save any unhandled interrupts and trigger the interrupt
 871 +               if(gintsts_saved.d32)
 872                 {
 873 -                       g_work_expected = 1;
 874                         /* To enable the MPHI interrupt  (INT 32)
 875                          */
 876 -                       FIQ_WRITE(  c_mphi_regs.outdda, (int) dummy_send);
 877 +                       FIQ_WRITE( c_mphi_regs.outdda, (int) dummy_send);
 878                         FIQ_WRITE( c_mphi_regs.outddb, (1 << 29));
 879
 880                         mphi_int_count++;
 881 -                       /* Clear the USB global interrupt so we don't just sit in the FIQ */
 882 -                       FIQ_MODIFY_IO_ADDRESS((USB_BASE + 0x8),1,0);
 883 -
 884                 }
 885         }
 886 +       while(0);
 887 +
 888         mb();
 889
 890         /* exit back to normal mode restoring everything */
 891 @@ -133,6 +477,7 @@ int32_t dwc_otg_hcd_handle_intr(dwc_otg_
 892
 893         dwc_otg_core_if_t *core_if = dwc_otg_hcd->core_if;
 894         gintsts_data_t gintsts;
 895 +       gintmsk_data_t gintmsk;
 896         hfnum_data_t hfnum;
 897
 898  #ifdef DEBUG
 899 @@ -140,6 +485,9 @@ int32_t dwc_otg_hcd_handle_intr(dwc_otg_
 900
 901  #endif
 902
 903 +       gintsts.d32 = DWC_READ_REG32(&core_if->core_global_regs->gintsts);
 904 +       gintmsk.d32 = DWC_READ_REG32(&core_if->core_global_regs->gintmsk);
 905 +
 906         /* Exit from ISR if core is hibernated */
 907         if (core_if->hibernation_suspend == 1) {
 908                 goto exit_handler_routine;
 909 @@ -147,11 +495,18 @@ int32_t dwc_otg_hcd_handle_intr(dwc_otg_
 910         DWC_SPINLOCK(dwc_otg_hcd->lock);
 911         /* Check if HOST Mode */
 912         if (dwc_otg_is_host_mode(core_if)) {
 913 -               gintsts.d32 = dwc_otg_read_core_intr(core_if);
 914 +               local_fiq_disable();
 915 +               gintmsk.d32 |= gintsts_saved.d32;
 916 +               gintsts.d32 |= gintsts_saved.d32;
 917 +               gintsts_saved.d32 = 0;
 918 +               local_fiq_enable();
 919                 if (!gintsts.d32) {
 920                         goto exit_handler_routine;
 921                 }
 922 +               gintsts.d32 &= gintmsk.d32;
 923 +
 924  #ifdef DEBUG
 925 +               // We should be OK doing this because the common interrupts should already have been serviced
 926                 /* Don't print debug message in the interrupt handler on SOF */
 927  #ifndef DEBUG_SOF
 928                 if (gintsts.d32 != DWC_SOF_INTR_MASK)
 929 @@ -171,11 +526,12 @@ int32_t dwc_otg_hcd_handle_intr(dwc_otg_
 930                 if (gintsts.b.sofintr && g_np_count == g_np_sent && dwc_frame_num_gt(g_next_sched_frame, hfnum.b.frnum))
 931                 {
 932                         /* Note, we should never get here if the FIQ is doing it's job properly*/
 933 -                       retval |= dwc_otg_hcd_handle_sof_intr(dwc_otg_hcd, g_work_expected);
 934 +                       retval |= dwc_otg_hcd_handle_sof_intr(dwc_otg_hcd);
 935                 }
 936                 else if (gintsts.b.sofintr) {
 937 -                       retval |= dwc_otg_hcd_handle_sof_intr(dwc_otg_hcd, g_work_expected);
 938 +                       retval |= dwc_otg_hcd_handle_sof_intr(dwc_otg_hcd);
 939                 }
 940 +
 941                 if (gintsts.b.rxstsqlvl) {
 942                         retval |=
 943                             dwc_otg_hcd_handle_rx_status_q_level_intr
 944 @@ -190,7 +546,10 @@ int32_t dwc_otg_hcd_handle_intr(dwc_otg_
 945                         /** @todo Implement i2cintr handler. */
 946                 }
 947                 if (gintsts.b.portintr) {
 948 +
 949 +                       gintmsk_data_t gintmsk = { .b.portintr = 1};
 950                         retval |= dwc_otg_hcd_handle_port_intr(dwc_otg_hcd);
 951 +                       DWC_MODIFY_REG32(&core_if->core_global_regs->gintmsk, 0, gintmsk.d32);
 952                 }
 953                 if (gintsts.b.hcintr) {
 954                         retval |= dwc_otg_hcd_handle_hc_intr(dwc_otg_hcd);
 955 @@ -227,26 +586,35 @@ exit_handler_routine:
 956
 957         if (fiq_fix_enable)
 958         {
 959 -               /* Clear the MPHI interrupt */
 960 -               DWC_WRITE_REG32(c_mphi_regs.intstat, (1<<16));
 961 -               if (mphi_int_count >= 60)
 962 +               local_fiq_disable();
 963 +               // Make sure that we don't clear the interrupt if we've still got pending work to do
 964 +               if(gintsts_saved.d32 == 0)
 965                 {
 966 -                       DWC_WRITE_REG32(c_mphi_regs.ctrl, ((1<<31) + (1<<16)));
 967 -                       while(!(DWC_READ_REG32(c_mphi_regs.ctrl) & (1 << 17)))
 968 -                               ;
 969 -                       DWC_WRITE_REG32(c_mphi_regs.ctrl, (1<<31));
 970 -                       mphi_int_count = 0;
 971 +                       /* Clear the MPHI interrupt */
 972 +                       DWC_WRITE_REG32(c_mphi_regs.intstat, (1<<16));
 973 +                       if (mphi_int_count >= 60)
 974 +                       {
 975 +                               DWC_WRITE_REG32(c_mphi_regs.ctrl, ((1<<31) + (1<<16)));
 976 +                               while(!(DWC_READ_REG32(c_mphi_regs.ctrl) & (1 << 17)))
 977 +                                       ;
 978 +                               DWC_WRITE_REG32(c_mphi_regs.ctrl, (1<<31));
 979 +                               mphi_int_count = 0;
 980 +                       }
 981 +                       int_done++;
 982                 }
 983 -               int_done++;
 984 +
 985 +               // Unmask handled interrupts
 986 +               FIQ_WRITE(dwc_regs_base + 0x18, gintmsk.d32);
 987 +               //DWC_MODIFY_REG32((uint32_t *)IO_ADDRESS(USB_BASE + 0x8), 0 , 1);
 988 +
 989 +               local_fiq_enable();
 990 +
 991                 if((jiffies / HZ) > last_time)
 992                 {
 993                         /* Once a second output the fiq and irq numbers, useful for debug */
 994                         last_time = jiffies / HZ;
 995                         DWC_DEBUGPL(DBG_USER, "int_done = %d fiq_done = %d\n", int_done, fiq_done);
 996                 }
 997 -
 998 -               /* Re-Enable FIQ interrupt from USB peripheral */
 999 -               DWC_MODIFY_REG32((uint32_t *)IO_ADDRESS(USB_BASE + 0x8), 0 , 1);
1000         }
1001
1002         DWC_SPINUNLOCK(dwc_otg_hcd->lock);
1003 @@ -294,13 +662,12 @@ static inline void track_missed_sofs(uin
1004   * (micro)frame. Periodic transactions may be queued to the controller for the
1005   * next (micro)frame.
1006   */
1007 -int32_t dwc_otg_hcd_handle_sof_intr(dwc_otg_hcd_t * hcd, int32_t work_expected)
1008 +int32_t dwc_otg_hcd_handle_sof_intr(dwc_otg_hcd_t * hcd)
1009  {
1010         hfnum_data_t hfnum;
1011         dwc_list_link_t *qh_entry;
1012         dwc_otg_qh_t *qh;
1013         dwc_otg_transaction_type_e tr_type;
1014 -       gintsts_data_t gintsts = {.d32 = 0 };
1015         int did_something = 0;
1016         int32_t next_sched_frame = -1;
1017
1018 @@ -326,6 +693,7 @@ int32_t dwc_otg_hcd_handle_sof_intr(dwc_
1019                 qh = DWC_LIST_ENTRY(qh_entry, dwc_otg_qh_t, qh_list_entry);
1020                 qh_entry = qh_entry->next;
1021                 if (dwc_frame_num_le(qh->sched_frame, hcd->frame_number)) {
1022 +
1023                         /*
1024                          * Move QH to the ready list to be executed next
1025                          * (micro)frame.
1026 @@ -351,15 +719,10 @@ int32_t dwc_otg_hcd_handle_sof_intr(dwc_
1027                 dwc_otg_hcd_queue_transactions(hcd, tr_type);
1028                 did_something = 1;
1029         }
1030 -       if(work_expected && !did_something)
1031 -               DWC_DEBUGPL(DBG_USER, "Nothing to do !! frame = %x, g_next_sched_frame = %x\n", (int) hfnum.b.frnum, g_next_sched_frame);
1032 -       if(!work_expected && did_something)
1033 -               DWC_DEBUGPL(DBG_USER, "Unexpected work done !! frame = %x, g_next_sched_frame = %x\n", (int) hfnum.b.frnum, g_next_sched_frame);
1034 -
1035
1036         /* Clear interrupt */
1037 -       gintsts.b.sofintr = 1;
1038 -       DWC_WRITE_REG32(&hcd->core_if->core_global_regs->gintsts, gintsts.d32);
1039 +       //gintsts.b.sofintr = 1;
1040 +       //DWC_WRITE_REG32(&hcd->core_if->core_global_regs->gintsts, gintsts.d32);
1041
1042         return 1;
1043  }
1044 @@ -643,6 +1006,15 @@ int32_t dwc_otg_hcd_handle_hc_intr(dwc_o
1045
1046         haint.d32 = dwc_otg_read_host_all_channels_intr(dwc_otg_hcd->core_if);
1047
1048 +       // Overwrite with saved interrupts from fiq handler
1049 +       if(fiq_split_enable)
1050 +       {
1051 +               local_fiq_disable();
1052 +               haint.d32 = haint_saved.d32;
1053 +               haint_saved.d32 = 0;
1054 +               local_fiq_enable();
1055 +       }
1056 +
1057         for (i = 0; i < dwc_otg_hcd->core_if->core_params->host_channels; i++) {
1058                 if (haint.b2.chint & (1 << i)) {
1059                         retval |= dwc_otg_hcd_handle_hc_n_intr(dwc_otg_hcd, i);
1060 @@ -683,7 +1055,10 @@ static uint32_t get_actual_xfer_length(d
1061                                 *short_read = (hctsiz.b.xfersize != 0);
1062                         }
1063                 } else if (hc->qh->do_split) {
1064 -                       length = qtd->ssplit_out_xfer_count;
1065 +                       if(fiq_split_enable)
1066 +                               length = split_out_xfersize[hc->hc_num];
1067 +                       else
1068 +                               length = qtd->ssplit_out_xfer_count;
1069                 } else {
1070                         length = hc->xfer_len;
1071                 }
1072 @@ -727,7 +1102,6 @@ static int update_urb_state_xfer_comp(dw
1073                                              DWC_OTG_HC_XFER_COMPLETE,
1074                                              &short_read);
1075
1076 -
1077         /* non DWORD-aligned buffer case handling. */
1078         if (hc->align_buff && xfer_length && hc->ep_is_in) {
1079                 dwc_memcpy(urb->buf + urb->actual_length, hc->qh->dw_align_buf,
1080 @@ -930,6 +1304,9 @@ static void release_channel(dwc_otg_hcd_
1081         int free_qtd;
1082         dwc_irqflags_t flags;
1083         dwc_spinlock_t *channel_lock = hcd->channel_lock;
1084 +#ifdef FIQ_DEBUG
1085 +       int endp = qtd->urb ? qtd->urb->pipe_info.ep_num : 0;
1086 +#endif
1087
1088         DWC_DEBUGPL(DBG_HCDV, "  %s: channel %d, halt_status %d, xfer_len %d\n",
1089                     __func__, hc->hc_num, halt_status, hc->xfer_len);
1090 @@ -1008,9 +1385,24 @@ cleanup:
1091
1092                 DWC_SPINLOCK_IRQSAVE(channel_lock, &flags);
1093                 hcd->available_host_channels++;
1094 +               fiq_print(FIQDBG_PORTHUB, "AHC = %d ", hcd->available_host_channels);
1095                 DWC_SPINUNLOCK_IRQRESTORE(channel_lock, flags);
1096         }
1097
1098 +       if(fiq_split_enable && hc->do_split)
1099 +       {
1100 +               if(!(hcd->hub_port[hc->hub_addr] & (1 << hc->port_addr)))
1101 +               {
1102 +                       fiq_print(FIQDBG_ERR, "PRTNOTAL");
1103 +                       //BUG();
1104 +               }
1105 +
1106 +               hcd->hub_port[hc->hub_addr] &= ~(1 << hc->port_addr);
1107 +               hcd->hub_port_alloc[hc->hub_addr * 16 + hc->port_addr] = -1;
1108 +
1109 +               fiq_print(FIQDBG_PORTHUB, "H%dP%d:RR%d", hc->hub_addr, hc->port_addr, endp);
1110 +       }
1111 +
1112         /* Try to queue more transfers now that there's a free channel. */
1113         tr_type = dwc_otg_hcd_select_transactions(hcd);
1114         if (tr_type != DWC_OTG_TRANSACTION_NONE) {
1115 @@ -1633,8 +2025,10 @@ static int32_t handle_hc_nyet_intr(dwc_o
1116                     hc->ep_type == DWC_OTG_EP_TYPE_ISOC) {
1117                         int frnum = dwc_otg_hcd_get_frame_number(hcd);
1118
1119 +                       // With the FIQ running we only ever see the failed NYET
1120                         if (dwc_full_frame_num(frnum) !=
1121 -                           dwc_full_frame_num(hc->qh->sched_frame)) {
1122 +                           dwc_full_frame_num(hc->qh->sched_frame) ||
1123 +                           fiq_split_enable) {
1124                                 /*
1125                                  * No longer in the same full speed frame.
1126                                  * Treat this as a transaction error.
1127 @@ -2012,10 +2406,10 @@ static inline int halt_status_ok(dwc_otg
1128  static void handle_hc_chhltd_intr_dma(dwc_otg_hcd_t * hcd,
1129                                       dwc_hc_t * hc,
1130                                       dwc_otg_hc_regs_t * hc_regs,
1131 -                                     dwc_otg_qtd_t * qtd)
1132 +                                     dwc_otg_qtd_t * qtd,
1133 +                                     hcint_data_t hcint,
1134 +                                     hcintmsk_data_t hcintmsk)
1135  {
1136 -       hcint_data_t hcint;
1137 -       hcintmsk_data_t hcintmsk;
1138         int out_nak_enh = 0;
1139
1140         /* For core with OUT NAK enhancement, the flow for high-
1141 @@ -2047,8 +2441,11 @@ static void handle_hc_chhltd_intr_dma(dw
1142         }
1143
1144         /* Read the HCINTn register to determine the cause for the halt. */
1145 -       hcint.d32 = DWC_READ_REG32(&hc_regs->hcint);
1146 -       hcintmsk.d32 = DWC_READ_REG32(&hc_regs->hcintmsk);
1147 +       if(!fiq_split_enable)
1148 +       {
1149 +               hcint.d32 = DWC_READ_REG32(&hc_regs->hcint);
1150 +               hcintmsk.d32 = DWC_READ_REG32(&hc_regs->hcintmsk);
1151 +       }
1152
1153         if (hcint.b.xfercomp) {
1154                 /** @todo This is here because of a possible hardware bug.  Spec
1155 @@ -2161,13 +2558,15 @@ static void handle_hc_chhltd_intr_dma(dw
1156  static int32_t handle_hc_chhltd_intr(dwc_otg_hcd_t * hcd,
1157                                      dwc_hc_t * hc,
1158                                      dwc_otg_hc_regs_t * hc_regs,
1159 -                                    dwc_otg_qtd_t * qtd)
1160 +                                    dwc_otg_qtd_t * qtd,
1161 +                                    hcint_data_t hcint,
1162 +                                    hcintmsk_data_t hcintmsk)
1163  {
1164         DWC_DEBUGPL(DBG_HCDI, "--Host Channel %d Interrupt: "
1165                     "Channel Halted--\n", hc->hc_num);
1166
1167         if (hcd->core_if->dma_enable) {
1168 -               handle_hc_chhltd_intr_dma(hcd, hc, hc_regs, qtd);
1169 +               handle_hc_chhltd_intr_dma(hcd, hc, hc_regs, qtd, hcint, hcintmsk);
1170         } else {
1171  #ifdef DEBUG
1172                 if (!halt_status_ok(hcd, hc, hc_regs, qtd)) {
1173 @@ -2184,7 +2583,7 @@ static int32_t handle_hc_chhltd_intr(dwc
1174  int32_t dwc_otg_hcd_handle_hc_n_intr(dwc_otg_hcd_t * dwc_otg_hcd, uint32_t num)
1175  {
1176         int retval = 0;
1177 -       hcint_data_t hcint;
1178 +       hcint_data_t hcint, hcint_orig;
1179         hcintmsk_data_t hcintmsk;
1180         dwc_hc_t *hc;
1181         dwc_otg_hc_regs_t *hc_regs;
1182 @@ -2197,12 +2596,23 @@ int32_t dwc_otg_hcd_handle_hc_n_intr(dwc
1183         qtd = DWC_CIRCLEQ_FIRST(&hc->qh->qtd_list);
1184
1185         hcint.d32 = DWC_READ_REG32(&hc_regs->hcint);
1186 +       hcint_orig = hcint;
1187         hcintmsk.d32 = DWC_READ_REG32(&hc_regs->hcintmsk);
1188         DWC_DEBUGPL(DBG_HCDV,
1189                     "  hcint 0x%08x, hcintmsk 0x%08x, hcint&hcintmsk 0x%08x\n",
1190                     hcint.d32, hcintmsk.d32, (hcint.d32 & hcintmsk.d32));
1191         hcint.d32 = hcint.d32 & hcintmsk.d32;
1192
1193 +       if(fiq_split_enable)
1194 +       {
1195 +               // replace with the saved interrupts from the fiq handler
1196 +               local_fiq_disable();
1197 +               hcint_orig.d32 = hcint_saved[num].d32;
1198 +               hcint.d32 = hcint_orig.d32 & hcintmsk_saved[num].d32;
1199 +               hcint_saved[num].d32 = 0;
1200 +               local_fiq_enable();
1201 +       }
1202 +
1203         if (!dwc_otg_hcd->core_if->dma_enable) {
1204                 if (hcint.b.chhltd && hcint.d32 != 0x2) {
1205                         hcint.b.chhltd = 0;
1206 @@ -2220,7 +2630,7 @@ int32_t dwc_otg_hcd_handle_hc_n_intr(dwc
1207                 hcint.b.nyet = 0;
1208         }
1209         if (hcint.b.chhltd) {
1210 -               retval |= handle_hc_chhltd_intr(dwc_otg_hcd, hc, hc_regs, qtd);
1211 +               retval |= handle_hc_chhltd_intr(dwc_otg_hcd, hc, hc_regs, qtd, hcint_orig, hcintmsk_saved[num]);
1212         }
1213         if (hcint.b.ahberr) {
1214                 retval |= handle_hc_ahberr_intr(dwc_otg_hcd, hc, hc_regs, qtd);
1215 --- a/drivers/usb/host/dwc_otg/dwc_otg_hcd_linux.c
1216 +++ b/drivers/usb/host/dwc_otg/dwc_otg_hcd_linux.c
1217 @@ -392,7 +392,11 @@ static struct dwc_otg_hcd_function_ops h
1218  static struct fiq_handler fh = {
1219    .name = "usb_fiq",
1220  };
1221 -static uint8_t fiqStack[1024];
1222 +struct fiq_stack_s {
1223 +       int magic1;
1224 +       uint8_t stack[2048];
1225 +       int magic2;
1226 +} fiq_stack;
1227
1228  extern mphi_regs_t c_mphi_regs;
1229  /**
1230 @@ -434,9 +438,11 @@ int hcd_init(dwc_bus_dev_t *_dev)
1231                 memset(&regs,0,sizeof(regs));
1232                 regs.ARM_r8 = (long)dwc_otg_hcd_handle_fiq;
1233                 regs.ARM_r9 = (long)0;
1234 -               regs.ARM_sp = (long)fiqStack + sizeof(fiqStack) - 4;
1235 +               regs.ARM_sp = (long)fiq_stack.stack + sizeof(fiq_stack.stack) - 4;
1236                 set_fiq_regs(&regs);
1237 -               }
1238 +               fiq_stack.magic1 = 0xdeadbeef;
1239 +               fiq_stack.magic2 = 0xaa995566;
1240 +       }
1241
1242         /*
1243          * Allocate memory for the base HCD plus the DWC OTG HCD.
1244 @@ -459,6 +465,8 @@ int hcd_init(dwc_bus_dev_t *_dev)
1245
1246         if (fiq_fix_enable)
1247         {
1248 +               volatile extern void *dwc_regs_base;
1249 +
1250                 //Set the mphi periph to  the required registers
1251                 c_mphi_regs.base    = otg_dev->os_dep.mphi_base;
1252                 c_mphi_regs.ctrl    = otg_dev->os_dep.mphi_base + 0x4c;
1253 @@ -466,6 +474,8 @@ int hcd_init(dwc_bus_dev_t *_dev)
1254                 c_mphi_regs.outddb  = otg_dev->os_dep.mphi_base + 0x2c;
1255                 c_mphi_regs.intstat = otg_dev->os_dep.mphi_base + 0x50;
1256
1257 +               dwc_regs_base = otg_dev->os_dep.base;
1258 +
1259                 //Enable mphi peripheral
1260                 writel((1<<31),c_mphi_regs.ctrl);
1261  #ifdef DEBUG
1262 @@ -839,6 +849,8 @@ static int dwc_otg_urb_dequeue(struct us
1263                  usb_hcd_unlink_urb_from_ep(hcd, urb);
1264  #endif
1265                 DWC_SPINUNLOCK_IRQRESTORE(dwc_otg_hcd->lock, flags);
1266 +
1267 +
1268  #if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28)
1269                  usb_hcd_giveback_urb(hcd, urb);
1270  #else
1271 --- a/drivers/usb/host/dwc_otg/dwc_otg_hcd_queue.c
1272 +++ b/drivers/usb/host/dwc_otg/dwc_otg_hcd_queue.c
1273 @@ -41,6 +41,7 @@
1274
1275  #include "dwc_otg_hcd.h"
1276  #include "dwc_otg_regs.h"
1277 +#include "dwc_otg_mphi_fix.h"
1278
1279  extern bool microframe_schedule;
1280
1281 @@ -191,6 +192,7 @@ void qh_init(dwc_otg_hcd_t * hcd, dwc_ot
1282                             dwc_otg_hcd_get_ep_num(&urb->pipe_info), hub_addr,
1283                             hub_port);
1284                 qh->do_split = 1;
1285 +               qh->skip_count = 0;
1286         }
1287
1288         if (qh->ep_type == UE_INTERRUPT || qh->ep_type == UE_ISOCHRONOUS) {
1289 @@ -737,6 +739,9 @@ void dwc_otg_hcd_qh_remove(dwc_otg_hcd_t
1290                             hcd->non_periodic_qh_ptr->next;
1291                 }
1292                 DWC_LIST_REMOVE_INIT(&qh->qh_list_entry);
1293 +
1294 +               // If we've removed the last non-periodic entry then there are none left!
1295 +               g_np_count = g_np_sent;
1296         } else {
1297                 deschedule_periodic(hcd, qh);
1298                 hcd->periodic_qh_count--;
1299 @@ -766,21 +771,21 @@ void dwc_otg_hcd_qh_deactivate(dwc_otg_h
1300  {
1301         if (dwc_qh_is_non_per(qh)) {
1302
1303 -       dwc_otg_qh_t *qh_tmp;
1304 -       dwc_list_link_t *qh_list;
1305 -       DWC_LIST_FOREACH(qh_list, &hcd->non_periodic_sched_inactive)
1306 -       {
1307 -               qh_tmp = DWC_LIST_ENTRY(qh_list, struct dwc_otg_qh, qh_list_entry);
1308 -               if(qh_tmp == qh)
1309 +               dwc_otg_qh_t *qh_tmp;
1310 +               dwc_list_link_t *qh_list;
1311 +               DWC_LIST_FOREACH(qh_list, &hcd->non_periodic_sched_inactive)
1312                 {
1313 -                       /*
1314 -                        *  FIQ is being disabled because this one nevers gets a np_count increment
1315 -                        *  This is still not absolutely correct, but it should fix itself with
1316 -                        *  just an unnecessary extra interrupt
1317 -                        */
1318 -                       g_np_sent = g_np_count;
1319 +                       qh_tmp = DWC_LIST_ENTRY(qh_list, struct dwc_otg_qh, qh_list_entry);
1320 +                       if(qh_tmp == qh)
1321 +                       {
1322 +                               /*
1323 +                                *  FIQ is being disabled because this one nevers gets a np_count increment
1324 +                                *  This is still not absolutely correct, but it should fix itself with
1325 +                                *  just an unnecessary extra interrupt
1326 +                                */
1327 +                               g_np_sent = g_np_count;
1328 +                       }
1329                 }
1330 -       }
1331
1332
1333                 dwc_otg_hcd_qh_remove(hcd, qh);
1334 --- a/drivers/usb/host/dwc_otg/dwc_otg_mphi_fix.h
1335 +++ b/drivers/usb/host/dwc_otg/dwc_otg_mphi_fix.h
1336 @@ -1,10 +1,7 @@
1337  #ifndef __DWC_OTG_MPHI_FIX_H__
1338  #define __DWC_OTG_MPHI_FIX_H__
1339 -
1340 -#define FIQ_WRITE_IO_ADDRESS(_addr_,_data_) *(volatile uint32_t *) IO_ADDRESS(_addr_) = _data_
1341 -#define FIQ_READ_IO_ADDRESS(_addr_) *(volatile uint32_t *) IO_ADDRESS(_addr_)
1342 -#define FIQ_MODIFY_IO_ADDRESS(_addr_,_clear_,_set_) FIQ_WRITE_IO_ADDRESS(_addr_ , (FIQ_READ_IO_ADDRESS(_addr_)&~_clear_)|_set_)
1343 -#define FIQ_WRITE(_addr_,_data_) *(volatile uint32_t *) _addr_ = _data_
1344 +#define FIQ_WRITE(_addr_,_data_) (*(volatile uint32_t *) (_addr_) = (_data_))
1345 +#define FIQ_READ(_addr_) (*(volatile uint32_t *) (_addr_))
1346
1347  typedef struct {
1348         volatile void* base;
1349 @@ -12,13 +9,13 @@ typedef struct {
1350         volatile void* outdda;
1351         volatile void* outddb;
1352         volatile void* intstat;
1353 -} mphi_regs_t;
1354 +} mphi_regs_t;
1355
1356  void dwc_debug_print_core_int_reg(gintsts_data_t gintsts, const char* function_name);
1357  void dwc_debug_core_int_mask(gintsts_data_t gintmsk, const char* function_name);
1358  void dwc_debug_otg_int(gotgint_data_t gotgint, const char* function_name);
1359
1360 -
1361 +extern gintsts_data_t gintsts_saved;
1362
1363  #ifdef DEBUG
1364  #define DWC_DBG_PRINT_CORE_INT(_arg_) dwc_debug_print_core_int_reg(_arg_,__func__)
1365 @@ -30,7 +27,22 @@ void dwc_debug_otg_int(gotgint_data_t go
1366  #define DWC_DBG_PRINT_CORE_INT_MASK(_arg_)
1367  #define DWC_DBG_PRINT_OTG_INT(_arg_)
1368
1369 +#endif
1370
1371 +typedef enum {
1372 +       FIQDBG_SCHED = (1 << 0),
1373 +       FIQDBG_INT   = (1 << 1),
1374 +       FIQDBG_ERR   = (1 << 2),
1375 +       FIQDBG_PORTHUB = (1 << 3),
1376 +} FIQDBG_T;
1377 +
1378 +void _fiq_print(FIQDBG_T dbg_lvl, char *fmt, ...);
1379 +#ifdef FIQ_DEBUG
1380 +#define fiq_print _fiq_print
1381 +#else
1382 +#define fiq_print(x, y, ...)
1383  #endif
1384
1385 +extern bool fiq_fix_enable, nak_holdoff_enable, fiq_split_enable;
1386 +
1387  #endif
1388 --- a/drivers/usb/host/dwc_otg/dwc_otg_pcd_intr.c
1389 +++ b/drivers/usb/host/dwc_otg/dwc_otg_pcd_intr.c
1390 @@ -4276,7 +4276,7 @@ do { \
1391                                                                         && (pcd->ep0state == EP0_OUT_DATA_PHASE))
1392                                                                         status.d32 = core_if->dev_if->out_desc_addr->status.d32;
1393                                                                 if (pcd->ep0state == EP0_OUT_STATUS_PHASE)
1394 -                                                                       status.d32 = status.d32 = core_if->dev_if->
1395 +                                                                       status.d32 = core_if->dev_if->
1396                                                                         out_desc_addr->status.d32;
1397
1398                                                                 if (status.b.sr) {