//------------------------------------------------------------------------------
void USBD_SetAddress( unsigned char address )
{
    volatile unsigned int i;

    trace_LOG(trace_DEBUG, "SetAddr(%d) ", address);

    if( USBD_IsHighSpeed() ) {
        // Timeout after 6 HUBs
        AT91C_BASE_UDPHS->UDPHS_EPT[0].UDPHS_EPTSETSTA = AT91C_UDPHS_TX_PK_RDY;
        i=0;
        while( (0 != (AT91C_BASE_UDPHS->UDPHS_EPT[0].UDPHS_EPTSTA & AT91C_UDPHS_TX_PK_RDY ))
            && (i< BOARD_MCK/10000) ) {  
            i++; // around 5 ms
        }
    }

    // Set address
    AT91C_BASE_UDPHS->UDPHS_CTRL &= ~AT91C_UDPHS_DEV_ADDR; // RAZ Address
    AT91C_BASE_UDPHS->UDPHS_CTRL |= address | AT91C_UDPHS_FADDR_EN;

    // If the address is 0, the device returns to the Default state
    if (address == 0) {
        deviceState = USBD_STATE_DEFAULT;
    }
    // If the address is non-zero, the device enters the Address state
    else {
        deviceState = USBD_STATE_ADDRESS;
    }
}

//------------------------------------------------------------------------------
// Changes the device state from Address to Configured, or from
//         Configured to Address.
//         This method directly access the last received SETUP packet to
//         decide on what to do.
//------------------------------------------------------------------------------
void USBD_SetConfiguration( unsigned char cfgnum )
{
    trace_LOG(trace_DEBUG, "SetCfg(%d) ", cfgnum);

    // Check the request
    if( cfgnum != 0 ) {

        // Enter Configured state
        deviceState = USBD_STATE_CONFIGURED;
    }
    // If the configuration number is zero, the device goes back to the Address
    // state
    else  {

        // Go back to Address state
        deviceState = USBD_STATE_ADDRESS;

        // Abort all transfers
        UDPHS_DisableEndpoints();
    }
}

//------------------------------------------------------------------------------
// Enables the pull-up on the D+ line to connect the device to the USB.
//------------------------------------------------------------------------------
void USBD_Connect( void )
{
    trace_LOG(trace_DEBUG, "Conn ");
#if defined(BOARD_USB_PULLUP_INTERNAL)
    AT91C_BASE_UDPHS->UDPHS_CTRL &= ~AT91C_UDPHS_DETACH;   // Pull Up on DP
    AT91C_BASE_UDPHS->UDPHS_CTRL |= AT91C_UDPHS_PULLD_DIS; // Disable Pull Down

#elif defined(BOARD_USB_PULLUP_INTERNAL_BY_MATRIX)
    trace_LOG(trace_DEBUG, "PUON 1\n\r");
    AT91C_BASE_MATRIX->MATRIX_USBPCR |= AT91C_MATRIX_USBPCR_PUON;

#elif defined(BOARD_USB_PULLUP_EXTERNAL)

#ifdef PIN_USB_PULLUP
    const Pin pinPullUp = PIN_USB_PULLUP;
    if( pinPullUp.attribute == PIO_OUTPUT_0 ) {

        PIO_Set(&pinPullUp);
    }
    else {

        PIO_Clear(&pinPullUp);
    }
#else
    #error unsupported now
#endif

#elif !defined(BOARD_USB_PULLUP_ALWAYSON)
    #error Unsupported pull-up type.

#endif
}

//------------------------------------------------------------------------------
// Disables the pull-up on the D+ line to disconnect the device from the bus.
//------------------------------------------------------------------------------
void USBD_Disconnect( void )
{
    trace_LOG(trace_DEBUG, "Disc ");

#if defined(BOARD_USB_PULLUP_INTERNAL)
    AT91C_BASE_UDPHS->UDPHS_CTRL |= AT91C_UDPHS_DETACH; // detach
    AT91C_BASE_UDPHS->UDPHS_CTRL &= ~AT91C_UDPHS_PULLD_DIS; // Enable Pull Down

#elif defined(BOARD_USB_PULLUP_INTERNAL_BY_MATRIX)
    AT91C_BASE_MATRIX->MATRIX_USBPCR &= ~AT91C_MATRIX_USBPCR_PUON;

#elif defined(BOARD_USB_PULLUP_EXTERNAL)

#ifdef PIN_USB_PULLUP
    const Pin pinPullUp = PIN_USB_PULLUP;
    if (pinPullUp.attribute == PIO_OUTPUT_0) {

        PIO_Clear(&pinPullUp);
    }
    else {

        PIO_Set(&pinPullUp);
    }
#else
    #error unsupported now
#endif

#elif !defined(BOARD_USB_PULLUP_ALWAYSON)
    #error Unsupported pull-up type.

#endif

    // Device returns to the Powered state
    if (deviceState > USBD_STATE_POWERED) {    

        deviceState = USBD_STATE_POWERED;
    }
}

//------------------------------------------------------------------------------
// Certification test for High Speed device.
// bIndex char for the test choice
//------------------------------------------------------------------------------
void USBD_Test( unsigned char bIndex )
{
    char          *pFifo;
    unsigned char i;

    AT91C_BASE_UDPHS->UDPHS_IEN &= ~AT91C_UDPHS_DET_SUSPD; // remove suspend for TEST
    AT91C_BASE_UDPHS->UDPHS_TST |= AT91C_UDPHS_SPEED_CFG_HS; // force High Speed (remove suspend)

    switch( bIndex ) {

        case USBFeatureRequest_TESTPACKET:
            trace_LOG(trace_DEBUG, "TEST_PACKET ");

            AT91C_BASE_UDPHS->UDPHS_DMA[1].UDPHS_DMACONTROL = 0;
            AT91C_BASE_UDPHS->UDPHS_DMA[2].UDPHS_DMACONTROL = 0;

            // Configure endpoint 2, 64 bytes, direction IN, type BULK, 1 bank
            AT91C_BASE_UDPHS->UDPHS_EPT[2].UDPHS_EPTCFG = AT91C_UDPHS_EPT_SIZE_64 | AT91C_UDPHS_EPT_DIR_IN | AT91C_UDPHS_EPT_TYPE_BUL_EPT | AT91C_UDPHS_BK_NUMBER_1;
            while( (signed int)(AT91C_BASE_UDPHS->UDPHS_EPT[2].UDPHS_EPTCFG & AT91C_UDPHS_EPT_MAPD) != (signed int)AT91C_UDPHS_EPT_MAPD ) {}

            AT91C_BASE_UDPHS->UDPHS_EPT[2].UDPHS_EPTCTLENB =  AT91C_UDPHS_EPT_ENABL;

            // Write FIFO
            pFifo = (char*)((unsigned int *)(AT91C_BASE_UDPHS_EPTFIFO->UDPHS_READEPT0) + (16384 * 2));
            //pFifo = (char*)&(AT91C_BASE_UDPHS_EPTFIFO->UDPHS_READEPT0[bEndpoint*16384]);
            for( i=0; i<sizeof(test_packet_buffer); i++) {
                pFifo[i] = test_packet_buffer[i];
            }
            // Tst PACKET
            AT91C_BASE_UDPHS->UDPHS_TST |= AT91C_UDPHS_TST_PKT;
            // Send packet
            AT91C_BASE_UDPHS->UDPHS_EPT[2].UDPHS_EPTSETSTA = AT91C_UDPHS_TX_PK_RDY;
            break;

        case USBFeatureRequest_TESTJ:
            trace_LOG(trace_DEBUG, "TEST_J ");
            AT91C_BASE_UDPHS->UDPHS_TST = AT91C_UDPHS_TST_J;
            break;

        case USBFeatureRequest_TESTK:
            trace_LOG(trace_DEBUG, "TEST_K ");
            AT91C_BASE_UDPHS->UDPHS_TST = AT91C_UDPHS_TST_K;
            break;

        case USBFeatureRequest_TESTSE0NAK:
            trace_LOG(trace_DEBUG, "TEST_SEO_NAK ");
            AT91C_BASE_UDPHS->UDPHS_IEN = 0;  // for test
            break;

        case USBFeatureRequest_TESTSENDZLP:
            //while( 0 != (AT91C_BASE_UDPHS->UDPHS_EPT[0].UDPHS_EPTSTA & AT91C_UDPHS_TX_PK_RDY ) ) {}
            AT91C_BASE_UDPHS->UDPHS_EPT[0].UDPHS_EPTSETSTA = AT91C_UDPHS_TX_PK_RDY;
            //while( 0 != (AT91C_BASE_UDPHS->UDPHS_EPT[0].UDPHS_EPTSTA & AT91C_UDPHS_TX_PK_RDY ) ) {}
            trace_LOG(trace_DEBUG, "SEND_ZLP ");
            break;
    }
    trace_LOG(trace_DEBUG, "\n\r");
}


//------------------------------------------------------------------------------
// Initializes the specified USB driver
//         This function initializes the current FIFO bank of endpoints,
//         configures the pull-up and VBus lines, disconnects the pull-up and
//         then trigger the Init callback.
//------------------------------------------------------------------------------
void USBD_Init( void )
{
    unsigned char i;

    trace_LOG(trace_DEBUG, "USBD Init()\n\r");

    // Reset endpoint structures
    UDPHS_ResetEndpoints();

    // Enables the USB Clock
    UDPHS_EnableUsbClock();

    // Configure the pull-up on D+ and disconnect it
#if defined(BOARD_USB_PULLUP_INTERNAL)
    AT91C_BASE_UDPHS->UDPHS_CTRL |= AT91C_UDPHS_DETACH; // detach
    AT91C_BASE_UDPHS->UDPHS_CTRL |= AT91C_UDPHS_PULLD_DIS; // Disable Pull Down

#elif defined(BOARD_USB_PULLUP_INTERNAL_BY_MATRIX)
    trace_LOG(trace_DEBUG, "PUON 0\n\r");
    AT91C_BASE_MATRIX->MATRIX_USBPCR &= ~AT91C_MATRIX_USBPCR_PUON;

#elif defined(BOARD_USB_PULLUP_EXTERNAL)
#ifdef PIN_USB_PULLUP
    const Pin pinPullUp = PIN_USB_PULLUP;
    PIO_Configure(&pinPullUp, 1);
    if (pinPullUp.attribute == PIO_OUTPUT_0) {

        PIO_Clear(&pinPullUp);
    }
    else {

        PIO_Set(&pinPullUp);
    }
#else
    #error unsupported now
#endif
#elif !defined(BOARD_USB_PULLUP_ALWAYSON)
    #error Unsupported pull-up type.

#endif

    // Reset and enable IP UDPHS
    AT91C_BASE_UDPHS->UDPHS_CTRL &= ~AT91C_UDPHS_EN_UDPHS;
    AT91C_BASE_UDPHS->UDPHS_CTRL |= AT91C_UDPHS_EN_UDPHS;
    // Enable and disable of the transceiver is automaticaly done by the IP.

    // With OR without DMA !!!
    // Initialization of DMA
    for( i=1; i<=((AT91C_BASE_UDPHS->UDPHS_IPFEATURES & AT91C_UDPHS_DMA_CHANNEL_NBR)>>4); i++ ) {

        // RESET endpoint canal DMA:
        // DMA stop channel command
        AT91C_BASE_UDPHS->UDPHS_DMA[i].UDPHS_DMACONTROL = 0;  // STOP command

        // Disable endpoint
        AT91C_BASE_UDPHS->UDPHS_EPT[i].UDPHS_EPTCTLDIS = AT91C_UDPHS_SHRT_PCKT
                                                       | AT91C_UDPHS_BUSY_BANK
                                                       | AT91C_UDPHS_NAK_OUT
                                                       | AT91C_UDPHS_NAK_IN
                                                       | AT91C_UDPHS_STALL_SNT
                                                       | AT91C_UDPHS_RX_SETUP
                                                       | AT91C_UDPHS_TX_PK_RDY
                                                       | AT91C_UDPHS_TX_COMPLT
                                                       | AT91C_UDPHS_RX_BK_RDY
                                                       | AT91C_UDPHS_ERR_OVFLW
                                                       | AT91C_UDPHS_MDATA_RX
                                                       | AT91C_UDPHS_DATAX_RX
                                                       | AT91C_UDPHS_NYET_DIS
                                                       | AT91C_UDPHS_INTDIS_DMA
                                                       | AT91C_UDPHS_AUTO_VALID
                                                       | AT91C_UDPHS_EPT_DISABL;

        // Reset endpoint config
        AT91C_BASE_UDPHS->UDPHS_EPT[i].UDPHS_EPTCTLENB = 0;

        // Reset DMA channel (Buff count and Control field)
        AT91C_BASE_UDPHS->UDPHS_DMA[i].UDPHS_DMACONTROL = AT91C_UDPHS_LDNXT_DSC;  // NON STOP command

        // Reset DMA channel 0 (STOP)
        AT91C_BASE_UDPHS->UDPHS_DMA[i].UDPHS_DMACONTROL = 0;  // STOP command

        // Clear DMA channel status (read the register for clear it)
        AT91C_BASE_UDPHS->UDPHS_DMA[i].UDPHS_DMASTATUS = AT91C_BASE_UDPHS->UDPHS_DMA[i].UDPHS_DMASTATUS;

    }

    AT91C_BASE_UDPHS->UDPHS_TST = 0;
    AT91C_BASE_UDPHS->UDPHS_IEN = 0;
    AT91C_BASE_UDPHS->UDPHS_CLRINT = AT91C_UDPHS_UPSTR_RES
                                   | AT91C_UDPHS_ENDOFRSM
                                   | AT91C_UDPHS_WAKE_UP
                                   | AT91C_UDPHS_ENDRESET
                                   | AT91C_UDPHS_IEN_SOF
                                   | AT91C_UDPHS_MICRO_SOF
                                   | AT91C_UDPHS_DET_SUSPD;
    
    // Detach
    //USBD_Disconnect();
//jcb to be tested

    // Device is in the Attached state
    deviceState = USBD_STATE_SUSPENDED;
    previousDeviceState = USBD_STATE_POWERED;

    // Disable interrupts
    AT91C_BASE_UDPHS->UDPHS_IEN = AT91C_UDPHS_ENDOFRSM
                                | AT91C_UDPHS_WAKE_UP
                                | AT91C_UDPHS_DET_SUSPD;

    // Disable USB clocks
    UDPHS_DisableUsbClock();
//jcb to be tested

    // Configure interrupts
    USBDCallbacks_Initialized();
}

//------------------------------------------------------------------------------
//    Function: USBD_GetState
//        Returns the current state of the USB device.
//    Returns:
//        Device current state.
//------------------------------------------------------------------------------
unsigned char USBD_GetState( void )
{
    return deviceState;
}

#endif // BOARD_USB_UDPHS