/*
 * Copyright (C) 2015-2016 Andrew J. Kroll
 * and
 * Copyright (C) 2011 Circuits At Home, LTD. All rights reserved.
 *
 * This software may be distributed and modified under the terms of the GNU
 * General Public License version 2 (GPL2) as publishe7d by the Free Software
 * Foundation and appearing in the file GPL2.TXT included in the packaging of
 * this file. Please note that GPL2 Section 2[b] requires that all works based
 * on this software must also be made publicly available under the terms of
 * the GPL2 ("Copyleft").
 *
 * Contact information
 * -------------------
 *
 * Circuits At Home, LTD
 * Web      :  https://www.circuitsathome.com
 * e-mail   :  support@circuitsathome.com
 *
 */

#if defined(USB_HOST_SHIELD_H) && !defined(USB_HOST_SHIELD_LOADED)
#define USB_HOST_SHIELD_LOADED

#include <Arduino.h>

#ifndef digitalPinToInterrupt
    #error digitalPinToInterrupt not defined, complain to your board maintainer.
#endif


#if USB_HOST_SHIELD_USE_ISR

  // allow two slots. this makes the maximum allowed shield count TWO
  // for AVRs this is limited to pins 2 and 3 ONLY
  // for all other boards, one odd and one even pin number is allowed.
  static MAX3421E_HOST *ISReven;
  static MAX3421E_HOST *ISRodd;

  static void UHS_NI call_ISReven() {
    ISReven->ISRTask();
  }

  static void UHS_NI call_ISRodd() {
    UHS_PIN_WRITE(LED_BUILTIN, HIGH);
    ISRodd->ISRTask();
  }
#endif


void UHS_NI MAX3421E_HOST::resume_host() {
  // Used on MCU that lack control of IRQ priority (AVR).
  // Resumes ISRs.
  // NOTE: you must track the state yourself!
  #ifdef __AVR__
    noInterrupts();
    if (irq_pin & 1) {
    ISRodd = this;
    attachInterrupt(UHS_GET_DPI(irq_pin), call_ISRodd, IRQ_SENSE);
    } else {
    ISReven = this;
    attachInterrupt(UHS_GET_DPI(irq_pin), call_ISReven, IRQ_SENSE);
    }
    interrupts();
  #endif
}

/* write single byte into MAX3421e register */
void UHS_NI MAX3421E_HOST::regWr(uint8_t reg, uint8_t data) {
  SPIclass.beginTransaction(MAX3421E_SPI_Settings);
  MARLIN_UHS_WRITE_SS(LOW);
  SPIclass.transfer(reg | 0x02);
  SPIclass.transfer(data);
  MARLIN_UHS_WRITE_SS(HIGH);
  SPIclass.endTransaction();
}

/* multiple-byte write                            */

/* returns a pointer to memory position after last written */
uint8_t* UHS_NI MAX3421E_HOST::bytesWr(uint8_t reg, uint8_t nbytes, uint8_t *data_p) {
  SPIclass.beginTransaction(MAX3421E_SPI_Settings);
  MARLIN_UHS_WRITE_SS(LOW);
  SPIclass.transfer(reg | 0x02);
  //printf("%2.2x :", reg);

  while (nbytes) {
    SPIclass.transfer(*data_p);
    //printf("%2.2x ", *data_p);
    nbytes--;
    data_p++; // advance data pointer
  }
  MARLIN_UHS_WRITE_SS(HIGH);
  SPIclass.endTransaction();
  //printf("\r\n");
  return (data_p);
}

/* GPIO write                                           */
/*GPIO byte is split between 2 registers, so two writes are needed to write one byte */

/* GPOUT bits are in the low nybble. 0-3 in IOPINS1, 4-7 in IOPINS2 */
void UHS_NI MAX3421E_HOST::gpioWr(uint8_t data) {
  regWr(rIOPINS1, data);
  data >>= 4;
  regWr(rIOPINS2, data);
  return;
}

/* single host register read    */
uint8_t UHS_NI MAX3421E_HOST::regRd(uint8_t reg) {
  SPIclass.beginTransaction(MAX3421E_SPI_Settings);
  MARLIN_UHS_WRITE_SS(LOW);
  SPIclass.transfer(reg);
  uint8_t rv = SPIclass.transfer(0);
  MARLIN_UHS_WRITE_SS(HIGH);
  SPIclass.endTransaction();
  return (rv);
}
/* multiple-byte register read  */

/* returns a pointer to a memory position after last read   */
uint8_t* UHS_NI MAX3421E_HOST::bytesRd(uint8_t reg, uint8_t nbytes, uint8_t *data_p) {
  SPIclass.beginTransaction(MAX3421E_SPI_Settings);
  MARLIN_UHS_WRITE_SS(LOW);
  SPIclass.transfer(reg);
  while (nbytes) {
    *data_p++ = SPIclass.transfer(0);
    nbytes--;
  }
  MARLIN_UHS_WRITE_SS(HIGH);
  SPIclass.endTransaction();
  return ( data_p);
}

/* GPIO read. See gpioWr for explanation */

/* GPIN pins are in high nybbles of IOPINS1, IOPINS2    */
uint8_t UHS_NI MAX3421E_HOST::gpioRd() {
  uint8_t gpin = 0;
  gpin = regRd(rIOPINS2); // pins 4-7
  gpin &= 0xF0; // clean lower nybble
  gpin |= (regRd(rIOPINS1) >> 4); // shift low bits and OR with upper from previous operation.
  return (gpin);
}

/* reset MAX3421E. Returns number of microseconds it took for PLL to stabilize after reset
or zero if PLL haven't stabilized in 65535 cycles */
uint16_t UHS_NI MAX3421E_HOST::reset() {
  uint16_t i = 0;

  // Initiate chip reset
  regWr(rUSBCTL, bmCHIPRES);
  regWr(rUSBCTL, 0x00);

  int32_t now;
  uint32_t expires = micros() + 65535;

  // Enable full-duplex SPI so we can read rUSBIRQ
  regWr(rPINCTL, bmFDUPSPI);
  while ((int32_t)(micros() - expires) < 0L) {
    if ((regRd(rUSBIRQ) & bmOSCOKIRQ)) {
      break;
    }
  }
  now = (int32_t)(micros() - expires);
  if (now < 0L) {
    i = 65535 + now; // Note this subtracts, as now is negative
  }
  return (i);
}

void UHS_NI MAX3421E_HOST::VBUS_changed() {
  /* modify USB task state because Vbus changed or unknown */
  uint8_t speed = 1;
  //printf("\r\n\r\n\r\n\r\nSTATE %2.2x -> ", usb_task_state);
  switch (vbusState) {
    case LSHOST: // Low speed
    speed = 0;
    // Intentional fall-through
    case FSHOST: // Full speed
      // Start device initialization if we are not initializing
      // Resets to the device cause an IRQ
      // usb_task_state == UHS_USB_HOST_STATE_RESET_NOT_COMPLETE;
      //if ((usb_task_state & UHS_USB_HOST_STATE_MASK) != UHS_USB_HOST_STATE_DETACHED) {
      ReleaseChildren();
      if (!doingreset) {
        if (usb_task_state == UHS_USB_HOST_STATE_RESET_NOT_COMPLETE) {
          usb_task_state = UHS_USB_HOST_STATE_WAIT_BUS_READY;
        } else if (usb_task_state != UHS_USB_HOST_STATE_WAIT_BUS_READY) {
          usb_task_state = UHS_USB_HOST_STATE_DEBOUNCE;
        }
      }
      sof_countdown = 0;
    break;
    case SE1: // illegal state
      sof_countdown = 0;
      doingreset = false;
      ReleaseChildren();
      usb_task_state = UHS_USB_HOST_STATE_ILLEGAL;
      break;
    case SE0: // disconnected
    default:
      sof_countdown = 0;
      doingreset = false;
      ReleaseChildren();
      usb_task_state = UHS_USB_HOST_STATE_IDLE;
      break;
  }
  usb_host_speed = speed;
  //printf("0x%2.2x\r\n\r\n\r\n\r\n", usb_task_state);
  return;
}

/**
 * Probe bus to determine device presence and speed,
 * then switch host to detected speed.
 */
void UHS_NI MAX3421E_HOST::busprobe() {
  uint8_t bus_sample;
  uint8_t tmpdata;
  bus_sample = regRd(rHRSL); // Get J,K status
  bus_sample &= (bmJSTATUS | bmKSTATUS); // zero the rest of the byte
  switch (bus_sample) { // start full-speed or low-speed host
    case bmJSTATUS:
      // Serial.println("J");
      if ((regRd(rMODE) & bmLOWSPEED) == 0) {
      regWr(rMODE, MODE_FS_HOST); // start full-speed host
      vbusState = FSHOST;
      } else {
      regWr(rMODE, MODE_LS_HOST); // start low-speed host
      vbusState = LSHOST;
      }
      #ifdef USB_HOST_MANUAL_POLL
          enable_frame_irq(true);
      #endif
      tmpdata = regRd(rMODE) | bmSOFKAENAB; // start SOF generation
      regWr(rHIRQ, bmFRAMEIRQ); // see data sheet.
      regWr(rMODE, tmpdata);
      break;
    case bmKSTATUS:
      // Serial.println("K");
      if ((regRd(rMODE) & bmLOWSPEED) == 0) {
      regWr(rMODE, MODE_LS_HOST); // start low-speed host
      vbusState = LSHOST;
      } else {
      regWr(rMODE, MODE_FS_HOST); // start full-speed host
      vbusState = FSHOST;
      }
      #ifdef USB_HOST_MANUAL_POLL
        enable_frame_irq(true);
      #endif
      tmpdata = regRd(rMODE) | bmSOFKAENAB; // start SOF generation
      regWr(rHIRQ, bmFRAMEIRQ); // see data sheet.
      regWr(rMODE, tmpdata);
      break;
    case bmSE1: // illegal state
      // Serial.println("I");
      regWr(rMODE, bmDPPULLDN | bmDMPULLDN | bmHOST);
      vbusState = SE1;
      // sofevent = false;
      break;
    case bmSE0: // disconnected state
      // Serial.println("D");
      regWr(rMODE, bmDPPULLDN | bmDMPULLDN | bmHOST);
      vbusState = SE0;
      // sofevent = false;
      break;
  } // end switch ( bus_sample )
}

/**
 * Initialize USB hardware, turn on VBUS
 *
 * @param mseconds Delay energizing VBUS after mseconds, A value of INT16_MIN means no delay.
 * @return 0 on success, -1 on error
 */
int16_t UHS_NI MAX3421E_HOST::Init(int16_t mseconds) {
  usb_task_state = UHS_USB_HOST_STATE_INITIALIZE; //set up state machine
  //Serial.print("MAX3421E 'this' USB Host @ 0x");
  //Serial.println((uint32_t)this, HEX);
  //Serial.print("MAX3421E 'this' USB Host Address Pool @ 0x");
  //Serial.println((uint32_t)GetAddressPool(), HEX);
  Init_dyn_SWI();
  UHS_printf_HELPER_init();
  noInterrupts();
  #ifdef ARDUINO_AVR_ADK
    // For Mega ADK, which has a Max3421e on-board,
    // set MAX_RESET to output mode, and then set it to HIGH
    // PORTJ bit 2
    if (irq_pin == 54) {
      DDRJ |= 0x04; // output
      PORTJ |= 0x04; // HIGH
    }
  #endif
  SPIclass.begin();
  #ifdef ARDUINO_AVR_ADK
    if (irq_pin == 54) {
      DDRE &= ~0x20; // input
      PORTE |= 0x20; // pullup
    } else
  #endif
  pinMode(irq_pin, INPUT_PULLUP);
  //UHS_PIN_WRITE(irq_pin, HIGH);
  pinMode(ss_pin, OUTPUT);
  MARLIN_UHS_WRITE_SS(HIGH);

  #ifdef USB_HOST_SHIELD_TIMING_PIN
    pinMode(USB_HOST_SHIELD_TIMING_PIN, OUTPUT);
    // My counter/timer can't work on an inverted gate signal
    // so we gate using a high pulse -- AJK
    UHS_PIN_WRITE(USB_HOST_SHIELD_TIMING_PIN, LOW);
  #endif
  interrupts();

  #if USB_HOST_SHIELD_USE_ISR
    int intr = digitalPinToInterrupt(irq_pin);
    if (intr == NOT_AN_INTERRUPT) {
      #ifdef ARDUINO_AVR_ADK
        if (irq_pin == 54)
          intr = 6;
        else
      #endif
      return (-2);
    }
    SPIclass.usingInterrupt(intr);
  #else
    SPIclass.usingInterrupt(255);
  #endif
  #ifndef NO_AUTO_SPEED
    // test to get to reset acceptance.
    uint32_t spd = UHS_MAX3421E_SPD;
    again:
      MAX3421E_SPI_Settings = SPISettings(spd, MSBFIRST, SPI_MODE0);
    if (reset() == 0) {
      MAX_HOST_DEBUG(PSTR("Fail SPI speed %lu\r\n"), spd);
      if (spd > 1999999) {
        spd -= 1000000;
        goto again;
      }
      return (-1);
    } else {
      // reset passes, does 64k?
      uint8_t sample_wr = 0;
      uint8_t sample_rd = 0;
      uint8_t gpinpol_copy = regRd(rGPINPOL);
      for (uint16_t j = 0; j < 65535; j++) {
        regWr(rGPINPOL, sample_wr);
        sample_rd = regRd(rGPINPOL);
        if (sample_rd != sample_wr) {
          MAX_HOST_DEBUG(PSTR("Fail SPI speed %lu\r\n"), spd);
          if (spd > 1999999) {
            spd -= 1000000;
            goto again;
          }
          return (-1);
        }
        sample_wr++;
      }
      regWr(rGPINPOL, gpinpol_copy);
    }

    MAX_HOST_DEBUG(PSTR("Pass SPI speed %lu\r\n"), spd);
  #endif

  if (reset() == 0) { // OSCOKIRQ hasn't asserted in time
    MAX_HOST_DEBUG(PSTR("OSCOKIRQ hasn't asserted in time"));
    return ( -1);
  }

  /* MAX3421E - full-duplex SPI, interrupt kind, vbus off */
  regWr(rPINCTL, (bmFDUPSPI | bmIRQ_SENSE | GPX_VBDET));

  // Delay a minimum of 1 second to ensure any capacitors are drained.
  // 1 second is required to make sure we do not smoke a Microdrive!
  if (mseconds != INT16_MIN) {
    if (mseconds < 1000) mseconds = 1000;
    delay(mseconds); // We can't depend on SOF timer here.
  }

  regWr(rMODE, bmDPPULLDN | bmDMPULLDN | bmHOST); // set pull-downs, Host

  // Enable interrupts on the MAX3421e
  regWr(rHIEN, IRQ_CHECK_MASK);
  // Enable interrupt pin on the MAX3421e, set pulse width for edge
  regWr(rCPUCTL, (bmIE | bmPULSEWIDTH));

  /* check if device is connected */
  regWr(rHCTL, bmSAMPLEBUS); // sample USB bus
  while (!(regRd(rHCTL) & bmSAMPLEBUS)); // wait for sample operation to finish

  busprobe(); // check if anything is connected
  VBUS_changed();

  // GPX pin on. This is done here so that a change is detected if we have a switch connected.
  /* MAX3421E - full-duplex SPI, interrupt kind, vbus on */
  regWr(rPINCTL, (bmFDUPSPI | bmIRQ_SENSE));
  regWr(rHIRQ, bmBUSEVENTIRQ); // see data sheet.
  regWr(rHCTL, bmBUSRST); // issue bus reset to force generate yet another possible IRQ

  #if USB_HOST_SHIELD_USE_ISR
    // Attach ISR to service IRQ from MAX3421e
    noInterrupts();
    if (irq_pin & 1) {
      ISRodd = this;
      attachInterrupt(UHS_GET_DPI(irq_pin), call_ISRodd, IRQ_SENSE);
    } else {
      ISReven = this;
      attachInterrupt(UHS_GET_DPI(irq_pin), call_ISReven, IRQ_SENSE);
    }
    interrupts();
  #endif
  //printf("\r\nrPINCTL 0x%2.2X\r\n", rPINCTL);
  //printf("rCPUCTL 0x%2.2X\r\n", rCPUCTL);
  //printf("rHIEN 0x%2.2X\r\n", rHIEN);
  //printf("irq_pin %i\r\n", irq_pin);
  return 0;
}

/**
 * Setup UHS_EpInfo structure
 *
 * @param addr USB device address
 * @param ep Endpoint
 * @param ppep pointer to the pointer to a valid UHS_EpInfo structure
 * @param nak_limit how many NAKs before aborting
 * @return 0 on success
 */
uint8_t UHS_NI MAX3421E_HOST::SetAddress(uint8_t addr, uint8_t ep, UHS_EpInfo **ppep, uint16_t &nak_limit) {
  UHS_Device *p = addrPool.GetUsbDevicePtr(addr);

  if (!p)
    return UHS_HOST_ERROR_NO_ADDRESS_IN_POOL;

  if (!p->epinfo)
    return UHS_HOST_ERROR_NULL_EPINFO;

  *ppep = getEpInfoEntry(addr, ep);

  if (!*ppep)
    return UHS_HOST_ERROR_NO_ENDPOINT_IN_TABLE;

  nak_limit = (0x0001UL << (((*ppep)->bmNakPower > UHS_USB_NAK_MAX_POWER) ? UHS_USB_NAK_MAX_POWER : (*ppep)->bmNakPower));
  nak_limit--;
  /*
  USBTRACE2("\r\nAddress: ", addr);
  USBTRACE2(" EP: ", ep);
  USBTRACE2(" NAK Power: ",(*ppep)->bmNakPower);
  USBTRACE2(" NAK Limit: ", nak_limit);
  USBTRACE("\r\n");
  */
  regWr(rPERADDR, addr); // set peripheral address

  uint8_t mode = regRd(rMODE);

  //Serial.print("\r\nMode: ");
  //Serial.println( mode, HEX);
  //Serial.print("\r\nLS: ");
  //Serial.println(p->speed, HEX);

  // Set bmLOWSPEED and bmHUBPRE in case of low-speed device, reset them otherwise
  regWr(rMODE, (p->speed) ? mode & ~(bmHUBPRE | bmLOWSPEED) : mode | bmLOWSPEED | hub_present);

  return 0;
}

/**
 * Receive a packet
 *
 * @param pep pointer to a valid UHS_EpInfo structure
 * @param nak_limit how many NAKs before aborting
 * @param nbytesptr pointer to maximum number of bytes of data to receive
 * @param data pointer to data buffer
 * @return 0 on success
 */
uint8_t UHS_NI MAX3421E_HOST::InTransfer(UHS_EpInfo *pep, uint16_t nak_limit, uint16_t *nbytesptr, uint8_t *data) {
  uint8_t rcode = 0;
  uint8_t pktsize;

  uint16_t nbytes = *nbytesptr;
  MAX_HOST_DEBUG(PSTR("Requesting %i bytes "), nbytes);
  uint8_t maxpktsize = pep->maxPktSize;

  *nbytesptr = 0;
  regWr(rHCTL, (pep->bmRcvToggle) ? bmRCVTOG1 : bmRCVTOG0); // set toggle value

  // use a 'break' to exit this loop
  while (1) {
    rcode = dispatchPkt(MAX3421E_tokIN, pep->epAddr, nak_limit); // IN packet to EP-'endpoint'. Function takes care of NAKS.
    #if 0
      // This issue should be resolved now.
      if (rcode == UHS_HOST_ERROR_TOGERR) {
      //MAX_HOST_DEBUG(PSTR("toggle wrong\r\n"));
      // yes, we flip it wrong here so that next time it is actually correct!
      pep->bmRcvToggle = (regRd(rHRSL) & bmSNDTOGRD) ? 0 : 1;
      regWr(rHCTL, (pep->bmRcvToggle) ? bmRCVTOG1 : bmRCVTOG0); // set toggle value
      continue;
      }
    #endif
    if (rcode) {
      //MAX_HOST_DEBUG(PSTR(">>>>>>>> Problem! dispatchPkt %2.2x\r\n"), rcode);
      break; // should be 0, indicating ACK. Else return error code.
    }
    /* check for RCVDAVIRQ and generate error if not present */
    /* the only case when absence of RCVDAVIRQ makes sense is when toggle error occurred. Need to add handling for that */
    if ((regRd(rHIRQ) & bmRCVDAVIRQ) == 0) {
      //MAX_HOST_DEBUG(PSTR(">>>>>>>> Problem! NO RCVDAVIRQ!\r\n"));
      rcode = 0xF0; // receive error
      break;
    }
    pktsize = regRd(rRCVBC); // number of received bytes
    MAX_HOST_DEBUG(PSTR("Got %i bytes \r\n"), pktsize);

    if (pktsize > nbytes) { // certain devices send more than asked
      //MAX_HOST_DEBUG(PSTR(">>>>>>>> Warning: wanted %i bytes but got %i.\r\n"), nbytes, pktsize);
      pktsize = nbytes;
    }

    int16_t mem_left = (int16_t)nbytes - *((int16_t*)nbytesptr);

    if (mem_left < 0)
      mem_left = 0;

    data = bytesRd(rRCVFIFO, ((pktsize > mem_left) ? mem_left : pktsize), data);

    regWr(rHIRQ, bmRCVDAVIRQ); // Clear the IRQ & free the buffer
    *nbytesptr += pktsize; // add this packet's byte count to total transfer length

    /* The transfer is complete under two conditions:           */
    /* 1. The device sent a short packet (L.T. maxPacketSize)   */
    /* 2. 'nbytes' have been transferred.                       */
    if ((pktsize < maxpktsize) || (*nbytesptr >= nbytes)) { // have we transferred 'nbytes' bytes?
      // Save toggle value
      pep->bmRcvToggle = ((regRd(rHRSL) & bmRCVTOGRD)) ? 1 : 0;
      //MAX_HOST_DEBUG(PSTR("\r\n"));
      rcode = 0;
      break;
    } // if
  } // while( 1 )
  return (rcode);
}

/**
 * Transmit a packet
 *
 * @param pep pointer to a valid UHS_EpInfo structure
 * @param nak_limit how many NAKs before aborting
 * @param nbytes number of bytes of data to send
 * @param data pointer to data buffer
 * @return 0 on success
 */
uint8_t UHS_NI MAX3421E_HOST::OutTransfer(UHS_EpInfo *pep, uint16_t nak_limit, uint16_t nbytes, uint8_t *data) {
  uint8_t rcode = UHS_HOST_ERROR_NONE;
  uint8_t retry_count;
  uint8_t *data_p = data; // local copy of the data pointer
  uint16_t bytes_tosend;
  uint16_t nak_count;
  uint16_t bytes_left = nbytes;

  uint8_t maxpktsize = pep->maxPktSize;

  if (maxpktsize < 1 || maxpktsize > 64)
  return UHS_HOST_ERROR_BAD_MAX_PACKET_SIZE;

  unsigned long timeout = millis() + UHS_HOST_TRANSFER_MAX_MS;

  regWr(rHCTL, (pep->bmSndToggle) ? bmSNDTOG1 : bmSNDTOG0); // set toggle value

  while (bytes_left) {
    SYSTEM_OR_SPECIAL_YIELD();
    retry_count = 0;
    nak_count = 0;
    bytes_tosend = (bytes_left >= maxpktsize) ? maxpktsize : bytes_left;
    bytesWr(rSNDFIFO, bytes_tosend, data_p); // filling output FIFO
    regWr(rSNDBC, bytes_tosend); // set number of bytes
    regWr(rHXFR, (MAX3421E_tokOUT | pep->epAddr)); // dispatch packet
    while (!(regRd(rHIRQ) & bmHXFRDNIRQ)); // wait for the completion IRQ
    regWr(rHIRQ, bmHXFRDNIRQ); // clear IRQ
    rcode = (regRd(rHRSL) & 0x0F);

    while (rcode && ((long)(millis() - timeout) < 0L)) {
      switch (rcode) {
        case UHS_HOST_ERROR_NAK:
          nak_count++;
          if (nak_limit && (nak_count == nak_limit))
            goto breakout;
          break;
        case UHS_HOST_ERROR_TIMEOUT:
          retry_count++;
          if (retry_count == UHS_HOST_TRANSFER_RETRY_MAXIMUM)
            goto breakout;
          break;
        case UHS_HOST_ERROR_TOGERR:
          // yes, we flip it wrong here so that next time it is actually correct!
          pep->bmSndToggle = (regRd(rHRSL) & bmSNDTOGRD) ? 0 : 1;
          regWr(rHCTL, (pep->bmSndToggle) ? bmSNDTOG1 : bmSNDTOG0); // set toggle value
          break;
        default:
          goto breakout;
      } // switch (rcode

      /* process NAK according to Host out NAK bug */
      regWr(rSNDBC, 0);
      regWr(rSNDFIFO, *data_p);
      regWr(rSNDBC, bytes_tosend);
      regWr(rHXFR, (MAX3421E_tokOUT | pep->epAddr)); // dispatch packet
      while (!(regRd(rHIRQ) & bmHXFRDNIRQ)); // wait for the completion IRQ
      regWr(rHIRQ, bmHXFRDNIRQ); // clear IRQ
      rcode = (regRd(rHRSL) & 0x0F);
      SYSTEM_OR_SPECIAL_YIELD();
    } // while (rcode && ....
    bytes_left -= bytes_tosend;
    data_p += bytes_tosend;
  } // while (bytes_left...
  breakout:

  pep->bmSndToggle = (regRd(rHRSL) & bmSNDTOGRD) ? 1 : 0; // bmSNDTOG1 : bmSNDTOG0;  // update toggle
  return (rcode); // should be 0 in all cases
}

/**
 * Send the actual packet.
 *
 * @param token
 * @param ep Endpoint
 * @param nak_limit how many NAKs before aborting, 0 == exit after timeout
 * @return 0 on success, 0xFF indicates NAK timeout. @see
 */
/* Assumes peripheral address is set and relevant buffer is loaded/empty       */
/* If NAK, tries to re-send up to nak_limit times                                                   */
/* If nak_limit == 0, do not count NAKs, exit after timeout                                         */
/* If bus timeout, re-sends up to USB_RETRY_LIMIT times                                             */

/* return codes 0x00-0x0F are HRSLT( 0x00 being success ), 0xFF means timeout                       */
uint8_t UHS_NI MAX3421E_HOST::dispatchPkt(uint8_t token, uint8_t ep, uint16_t nak_limit) {
  unsigned long timeout = millis() + UHS_HOST_TRANSFER_MAX_MS;
  uint8_t tmpdata;
  uint8_t rcode = UHS_HOST_ERROR_NONE;
  uint8_t retry_count = 0;
  uint16_t nak_count = 0;

  for (;;) {
    regWr(rHXFR, (token | ep)); // launch the transfer
    while (long(millis() - timeout) < 0L) { // wait for transfer completion
      SYSTEM_OR_SPECIAL_YIELD();
      tmpdata = regRd(rHIRQ);

      if (tmpdata & bmHXFRDNIRQ) {
        regWr(rHIRQ, bmHXFRDNIRQ); // clear the interrupt
        //rcode = 0x00;
        break;
      } // if (tmpdata & bmHXFRDNIRQ

    } // while (millis() < timeout

    rcode = (regRd(rHRSL) & 0x0F); // analyze transfer result

    switch (rcode) {
      case UHS_HOST_ERROR_NAK:
        nak_count++;
        if (nak_limit && (nak_count == nak_limit))
          return (rcode);
        delayMicroseconds(200);
        break;
      case UHS_HOST_ERROR_TIMEOUT:
        retry_count++;
        if (retry_count == UHS_HOST_TRANSFER_RETRY_MAXIMUM)
          return (rcode);
        break;
      default:
        return (rcode);
    } // switch (rcode)
  }
}

//
// NULL is error, we don't need to know the reason.
//

UHS_EpInfo * UHS_NI MAX3421E_HOST::ctrlReqOpen(uint8_t addr, uint64_t Request, uint8_t *dataptr) {
  uint8_t rcode;
  UHS_EpInfo *pep = NULL;
  uint16_t nak_limit = 0;
  rcode = SetAddress(addr, 0, &pep, nak_limit);

  if (!rcode) {

    bytesWr(rSUDFIFO, 8, (uint8_t*)(&Request)); // transfer to setup packet FIFO

    rcode = dispatchPkt(MAX3421E_tokSETUP, 0, nak_limit); // dispatch packet
    if (!rcode) {
      if (dataptr != NULL) {
        if (((Request)/* bmReqType*/ & 0x80) == 0x80) {
          pep->bmRcvToggle = 1; //bmRCVTOG1;
        } else {
          pep->bmSndToggle = 1; //bmSNDTOG1;
        }
      }
    } else {
      pep = NULL;
    }
  }
  return pep;
}

uint8_t UHS_NI MAX3421E_HOST::ctrlReqRead(UHS_EpInfo *pep, uint16_t *left, uint16_t *read, uint16_t nbytes, uint8_t *dataptr) {
  *read = 0;
  uint16_t nak_limit = 0;
  MAX_HOST_DEBUG(PSTR("ctrlReqRead left: %i\r\n"), *left);
  if (*left) {
    again:
    *read = nbytes;
    uint8_t rcode = InTransfer(pep, nak_limit, read, dataptr);
    if (rcode == UHS_HOST_ERROR_TOGERR) {
      // yes, we flip it wrong here so that next time it is actually correct!
      pep->bmRcvToggle = (regRd(rHRSL) & bmSNDTOGRD) ? 0 : 1;
      goto again;
    }

    if (rcode) {
      MAX_HOST_DEBUG(PSTR("ctrlReqRead ERROR: %2.2x, left: %i, read %i\r\n"), rcode, *left, *read);
      return rcode;
    }
    *left -= *read;
    MAX_HOST_DEBUG(PSTR("ctrlReqRead left: %i, read %i\r\n"), *left, *read);
  }
  return 0;
}

uint8_t UHS_NI MAX3421E_HOST::ctrlReqClose(UHS_EpInfo *pep, uint8_t bmReqType, uint16_t left, uint16_t nbytes, uint8_t *dataptr) {
  uint8_t rcode = 0;

  //MAX_HOST_DEBUG(PSTR("Closing"));
  if (((bmReqType & 0x80) == 0x80) && pep && left && dataptr) {
    MAX_HOST_DEBUG(PSTR("ctrlReqRead Sinking %i\r\n"), left);
    // If reading, sink the rest of the data.
    while (left) {
      uint16_t read = nbytes;
      rcode = InTransfer(pep, 0, &read, dataptr);
      if (rcode == UHS_HOST_ERROR_TOGERR) {
        // yes, we flip it wrong here so that next time it is actually correct!
        pep->bmRcvToggle = (regRd(rHRSL) & bmSNDTOGRD) ? 0 : 1;
        continue;
      }
      if (rcode) break;
      left -= read;
      if (read < nbytes) break;
    }
  }
  if (!rcode) {
    //Serial.println("Dispatching");
    rcode = dispatchPkt(((bmReqType & 0x80) == 0x80) ? MAX3421E_tokOUTHS : MAX3421E_tokINHS, 0, 0); //GET if direction
  //} else {
    //Serial.println("Bypassed Dispatch");
  }
  return rcode;
}

/**
 * Bottom half of the ISR task
 */
void UHS_NI MAX3421E_HOST::ISRbottom() {
  uint8_t x;
  //        Serial.print("Enter ");
  //        Serial.print((uint32_t)this,HEX);
  //        Serial.print(" ");
  //        Serial.println(usb_task_state, HEX);

  DDSB();
  if (condet) {
    VBUS_changed();
    #if USB_HOST_SHIELD_USE_ISR
      noInterrupts();
    #endif
    condet = false;
    #if USB_HOST_SHIELD_USE_ISR
      interrupts();
    #endif
  }
  switch (usb_task_state) {
    case UHS_USB_HOST_STATE_INITIALIZE:
      // should never happen...
      MAX_HOST_DEBUG(PSTR("UHS_USB_HOST_STATE_INITIALIZE\r\n"));
      busprobe();
      VBUS_changed();
      break;
    case UHS_USB_HOST_STATE_DEBOUNCE:
      MAX_HOST_DEBUG(PSTR("UHS_USB_HOST_STATE_DEBOUNCE\r\n"));
      // This seems to not be needed. The host controller has debounce built in.
      sof_countdown = UHS_HOST_DEBOUNCE_DELAY_MS;
      usb_task_state = UHS_USB_HOST_STATE_DEBOUNCE_NOT_COMPLETE;
      break;
    case UHS_USB_HOST_STATE_DEBOUNCE_NOT_COMPLETE:
      MAX_HOST_DEBUG(PSTR("UHS_USB_HOST_STATE_DEBOUNCE_NOT_COMPLETE\r\n"));
      if (!sof_countdown) usb_task_state = UHS_USB_HOST_STATE_RESET_DEVICE;
      break;
    case UHS_USB_HOST_STATE_RESET_DEVICE:
      MAX_HOST_DEBUG(PSTR("UHS_USB_HOST_STATE_RESET_DEVICE\r\n"));
      busevent = true;
      usb_task_state = UHS_USB_HOST_STATE_RESET_NOT_COMPLETE;
      regWr(rHIRQ, bmBUSEVENTIRQ); // see data sheet.
      regWr(rHCTL, bmBUSRST); // issue bus reset
      break;
    case UHS_USB_HOST_STATE_RESET_NOT_COMPLETE:
      MAX_HOST_DEBUG(PSTR("UHS_USB_HOST_STATE_RESET_NOT_COMPLETE\r\n"));
      if (!busevent) usb_task_state = UHS_USB_HOST_STATE_WAIT_BUS_READY;
      break;
    case UHS_USB_HOST_STATE_WAIT_BUS_READY:
      MAX_HOST_DEBUG(PSTR("UHS_USB_HOST_STATE_WAIT_BUS_READY\r\n"));
      usb_task_state = UHS_USB_HOST_STATE_CONFIGURING;
      break; // don't fall through

    case UHS_USB_HOST_STATE_CONFIGURING:
      usb_task_state = UHS_USB_HOST_STATE_CHECK;
      x = Configuring(0, 1, usb_host_speed);
      usb_error = x;
      if (usb_task_state == UHS_USB_HOST_STATE_CHECK) {
        if (x) {
          MAX_HOST_DEBUG(PSTR("Error 0x%2.2x"), x);
          if (x == UHS_HOST_ERROR_JERR) {
            usb_task_state = UHS_USB_HOST_STATE_IDLE;
          } else if (x != UHS_HOST_ERROR_DEVICE_INIT_INCOMPLETE) {
            usb_error = x;
            usb_task_state = UHS_USB_HOST_STATE_ERROR;
          }
        } else
          usb_task_state = UHS_USB_HOST_STATE_CONFIGURING_DONE;
      }
      break;

    case UHS_USB_HOST_STATE_CHECK:
      // Serial.println((uint32_t)__builtin_return_address(0), HEX);
      break;
    case UHS_USB_HOST_STATE_CONFIGURING_DONE:
      usb_task_state = UHS_USB_HOST_STATE_RUNNING;
      break;

    #ifdef USB_HOST_MANUAL_POLL
      case UHS_USB_HOST_STATE_RUNNING:
      case UHS_USB_HOST_STATE_ERROR:
      case UHS_USB_HOST_STATE_IDLE:
      case UHS_USB_HOST_STATE_ILLEGAL:
        enable_frame_irq(false);
        break;
    #else
      case UHS_USB_HOST_STATE_RUNNING:
        Poll_Others();
        for (x = 0; (usb_task_state == UHS_USB_HOST_STATE_RUNNING) && (x < UHS_HOST_MAX_INTERFACE_DRIVERS); x++) {
          if (devConfig[x]) {
            if (devConfig[x]->bPollEnable) devConfig[x]->Poll();
          }
        }
        // fall thru
    #endif
    default:
      // Do nothing
      break;
  } // switch ( usb_task_state )
  DDSB();
  #if USB_HOST_SHIELD_USE_ISR
    if (condet) {
      VBUS_changed();
      noInterrupts();
      condet = false;
      interrupts();
    }
  #endif
  #ifdef USB_HOST_SHIELD_TIMING_PIN
    // My counter/timer can't work on an inverted gate signal
    // so we gate using a high pulse -- AJK
    UHS_PIN_WRITE(USB_HOST_SHIELD_TIMING_PIN, LOW);
  #endif
  //usb_task_polling_disabled--;
  EnablePoll();
  DDSB();
}


/* USB main task. Services the MAX3421e */
#if !USB_HOST_SHIELD_USE_ISR
  void UHS_NI MAX3421E_HOST::ISRTask() {}
  void UHS_NI MAX3421E_HOST::Task()
#else
  void UHS_NI MAX3421E_HOST::Task() {
    #ifdef USB_HOST_MANUAL_POLL
      if (usb_task_state == UHS_USB_HOST_STATE_RUNNING) {
        noInterrupts();
        for (uint8_t x = 0; x < UHS_HOST_MAX_INTERFACE_DRIVERS; x++)
          if (devConfig[x] && devConfig[x]->bPollEnable)
            devConfig[x]->Poll();
        interrupts();
      }
    #endif
  }

  void UHS_NI MAX3421E_HOST::ISRTask()
#endif
{
  DDSB();

  #ifndef SWI_IRQ_NUM
    suspend_host();
    #if USB_HOST_SHIELD_USE_ISR
      // Enable interrupts
      interrupts();
    #endif
  #endif

  counted = false;
  if (!MARLIN_UHS_READ_IRQ()) {
    uint8_t HIRQALL = regRd(rHIRQ); // determine interrupt source
    uint8_t HIRQ = HIRQALL & IRQ_CHECK_MASK;
    uint8_t HIRQ_sendback = 0x00;

    if ((HIRQ & bmCONDETIRQ) || (HIRQ & bmBUSEVENTIRQ)) {
      MAX_HOST_DEBUG
      (PSTR("\r\nBEFORE CDIRQ %s BEIRQ %s resetting %s state 0x%2.2x\r\n"),
        (HIRQ & bmCONDETIRQ) ? "T" : "F",
        (HIRQ & bmBUSEVENTIRQ) ? "T" : "F",
        doingreset ? "T" : "F",
        usb_task_state
      );
    }
    // ALWAYS happens BEFORE or WITH CONDETIRQ
    if (HIRQ & bmBUSEVENTIRQ) {
      HIRQ_sendback |= bmBUSEVENTIRQ;
      if (!doingreset) condet = true;
      busprobe();
      busevent = false;
    }

    if (HIRQ & bmCONDETIRQ) {
      HIRQ_sendback |= bmCONDETIRQ;
      if (!doingreset) condet = true;
      busprobe();
    }

    #if 1
      if ((HIRQ & bmCONDETIRQ) || (HIRQ & bmBUSEVENTIRQ)) {
        MAX_HOST_DEBUG
        (PSTR("\r\nAFTER CDIRQ %s BEIRQ %s resetting %s state 0x%2.2x\r\n"),
          (HIRQ & bmCONDETIRQ) ? "T" : "F",
          (HIRQ & bmBUSEVENTIRQ) ? "T" : "F",
          doingreset ? "T" : "F",
          usb_task_state
        );
      }
    #endif

    if (HIRQ & bmFRAMEIRQ) {
      HIRQ_sendback |= bmFRAMEIRQ;
      if (sof_countdown) {
        sof_countdown--;
        counted = true;
      }
      sofevent = false;
    }

    //MAX_HOST_DEBUG(PSTR("\r\n%s%s%s\r\n"),
    //        sof_countdown ? "T" : "F",
    //        counted ? "T" : "F",
    //        usb_task_polling_disabled? "T" : "F");
    DDSB();
    regWr(rHIRQ, HIRQ_sendback);
    #ifndef SWI_IRQ_NUM
      resume_host();
      #if USB_HOST_SHIELD_USE_ISR
        // Disable interrupts
        noInterrupts();
      #endif
    #endif
    if (!sof_countdown && !counted && !usb_task_polling_disabled) {
      DisablePoll();
      //usb_task_polling_disabled++;
      #ifdef USB_HOST_SHIELD_TIMING_PIN
        // My counter/timer can't work on an inverted gate signal
        // so we gate using a high pulse -- AJK
        UHS_PIN_WRITE(USB_HOST_SHIELD_TIMING_PIN, HIGH);
      #endif

      #ifdef SWI_IRQ_NUM
        //MAX_HOST_DEBUG(PSTR("--------------- Doing SWI ----------------"));
        exec_SWI(this);
      #else
        #if USB_HOST_SHIELD_USE_ISR
          // Enable interrupts
          interrupts();
        #endif
        ISRbottom();
      #endif /* SWI_IRQ_NUM */
    }
  }
}

#if 0
  DDSB();
#endif

#else
  #error "Never include USB_HOST_SHIELD_INLINE.h, include UHS_host.h instead"
#endif