/* ----------------------------------------------------------------------------
 *         SAM Software Package License 
 * ----------------------------------------------------------------------------
 * Copyright (c) 2014, Atmel Corporation
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * - Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the disclaimer below.
 *
 * Atmel's name may not be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * DISCLAIMER: THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE
 * DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * ----------------------------------------------------------------------------
 */

/**

 \file

    Implementation of USB device functions on a UDP controller.

    See \ref usbd_api_method USBD API Methods.
*/

/** \addtogroup usbd_hal
 *@{*/

/*---------------------------------------------------------------------------
 *      Headers
 *---------------------------------------------------------------------------*/

#include "chip.h"
#include "USBD_HAL.h"

#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>

/*---------------------------------------------------------------------------
 *      Definitions
 *---------------------------------------------------------------------------*/

#define DMA

/** Maximum number of endpoints interrupts. */
#define NUM_IT_MAX       16
/** Maximum number of endpoint DMA interrupts */
#define NUM_IT_MAX_DMA   \
    ((UDPHS->UDPHS_IPFEATURES \
        & UDPHS_IPFEATURES_DMA_CHANNEL_NBR_Msk) \
      >>UDPHS_IPFEATURES_DMA_CHANNEL_NBR_Pos)
/** Bits that should be shifted to access DMA control bits. */
#define SHIFT_DMA        24
/** Bits that should be shifted to access interrupt bits. */
#define SHIFT_INTERUPT    8

/** Max size of the FMA FIFO */
#define DMA_MAX_FIFO_SIZE     (65536/1)
/** fifo space size in DW */
#define EPT_VIRTUAL_SIZE      16384

/**
 * \section endpoint_states_sec "UDP Endpoint states"
 *
 *  This page lists the endpoint states.
 *
 *  \subsection States
 *  - UDPHS_ENDPOINT_DISABLED
 *  - UDPHS_ENDPOINT_HALTED
 *  - UDPHS_ENDPOINT_IDLE
 *  - UDPHS_ENDPOINT_SENDING
 *  - UDPHS_ENDPOINT_RECEIVING
 *  - UDPHS_ENDPOINT_SENDINGM
 *  - UDPHS_ENDPOINT_RECEIVINGM
 */

/**  Endpoint states: Endpoint is disabled */
#define UDPHS_ENDPOINT_DISABLED       0
/**  Endpoint states: Endpoint is halted (i.e. STALLs every request) */
#define UDPHS_ENDPOINT_HALTED         1
/**  Endpoint states: Endpoint is idle (i.e. ready for transmission) */
#define UDPHS_ENDPOINT_IDLE           2
/**  Endpoint states: Endpoint is sending data */
#define UDPHS_ENDPOINT_SENDING        3
/**  Endpoint states: Endpoint is receiving data */
#define UDPHS_ENDPOINT_RECEIVING      4
/**  Endpoint states: Endpoint is sending MBL */
#define UDPHS_ENDPOINT_SENDINGM       5
/**  Endpoint states: Endpoint is receiving MBL */
#define UDPHS_ENDPOINT_RECEIVINGM     6

/** Get Number of buffer in Multi-Buffer-List
 *  \param i    input index
 *  \param o    output index
 *  \param size list size
 */
#define MBL_NbBuffer(i, o, size) (((i)>(o))?((i)-(o)):((i)+(size)-(o)))

/** Buffer list is full */
#define MBL_FULL        1
/** Buffer list is null */
#define MBL_NULL        2

/*---------------------------------------------------------------------------
 *      Types
 *---------------------------------------------------------------------------*/

/**  Describes header for UDP endpoint transfer. */
typedef struct {
    /**  Optional callback to invoke when the transfer completes. */
    void*   fCallback;
    /**  Optional argument to the callback function. */
    void*   pArgument;
    /**  Transfer type */
    uint8_t transType;
    /* Reserved to 32-b aligned */
    uint8_t reserved[3];
} TransferHeader;

/**  Describes a transfer on a UDP endpoint. */
typedef struct {

    /**  Optional callback to invoke when the transfer completes. */
    TransferCallback fCallback;
    /**  Optional argument to the callback function. */
    void             *pArgument;
    /**  Transfer type */
    uint8_t          transType;
    uint8_t          reserved[3];
    /**  Number of bytes which have been written into the UDP internal FIFO
     *   buffers. */
    int32_t          buffered;
    /**  Pointer to a data buffer used for emission/reception. */
    uint8_t          *pData;
    /**  Number of bytes which have been sent/received. */
    int32_t          transferred;
    /**  Number of bytes which have not been buffered/transferred yet. */
    int32_t          remaining;
} Transfer;

/**  Describes Multi Buffer List transfer on a UDP endpoint. */
typedef struct {
    /**  Optional callback to invoke when the transfer completes. */
    MblTransferCallback fCallback;
    /**  Optional argument to the callback function. */
    void                *pArgument;
    /** Transfer type */
    uint8_t             transType;
    /** List state (OK, FULL, NULL) (run time) */
    uint8_t             listState;
    /**  Multi-Buffer List size */
    uint16_t            listSize;
    /**  Pointer to multi-buffer list */
    USBDTransferBuffer *pMbl;
    /**  Offset number of buffers to start transfer */
    uint16_t            offsetSize;
    /**  Current processing buffer index (run time) */
    uint16_t            outCurr;
    /**  Loast loaded buffer index (run time) */
    uint16_t            outLast;
    /**  Current buffer for input (run time) */
    uint16_t            inCurr;
} MblTransfer;

/**
 *  Describes the state of an endpoint of the UDP controller.
 */
typedef struct {

    /* CSR */
    /**  Current endpoint state. */
    volatile uint8_t  state;
    /**  Current reception bank (0 or 1). */
    volatile uint8_t  bank;
    /**  Maximum packet size for the endpoint. */
    volatile uint16_t size;
    /**  Describes an ongoing transfer (if current state is either
     *   UDPHS_ENDPOINT_SENDING or UDPHS_ENDPOINT_RECEIVING) */
    union {
        TransferHeader transHdr;
        Transfer       singleTransfer;
        MblTransfer    mblTransfer;
    } transfer;
    /** Special case for send a ZLP */
    uint32_t sendZLP;
} Endpoint;

/**
 * DMA Descriptor.
 */
typedef struct {
    void    *pNxtDesc;
    void    *pAddr;
    uint32_t dwCtrl;
    uint32_t dw;
} UdphsDmaDescriptor;

/*---------------------------------------------------------------------------
 *      Internal variables
 *---------------------------------------------------------------------------*/

/** Holds the internal state for each endpoint of the UDP. */
static Endpoint endpoints[CHIP_USB_NUMENDPOINTS];

/** 7.1.20 Test Mode Support
 * Test codes for the USB HS test mode. */
static const char test_packet_buffer[] = {
    0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,                // JKJKJKJK * 9
    0xAA,0xAA,0xAA,0xAA,0xAA,0xAA,0xAA,0xAA,                     // JJKKJJKK * 8
    0xEE,0xEE,0xEE,0xEE,0xEE,0xEE,0xEE,0xEE,                     // JJJJKKKK * 8
    0xFE,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, // JJJJJJJKKKKKKK * 8
    0x7F,0xBF,0xDF,0xEF,0xF7,0xFB,0xFD,                          // JJJJJJJK * 8
    0xFC,0x7E,0xBF,0xDF,0xEF,0xF7,0xFB,0xFD,0x7E                 // {JKKKKKKK * 10}, JK
};

/** Force FS mode */
static uint8_t forceUsbFS = 0;

/** DMA link list */
static UdphsDmaDescriptor  dmaLL[5];
static UdphsDmaDescriptor *pDmaLL;

/*---------------------------------------------------------------------------
 *      Internal Functions
 *---------------------------------------------------------------------------*/

/**
 * Enables the clock of the UDP peripheral.
 * \return 1 if peripheral status changed.
 */
static uint8_t UDPHS_EnablePeripheralClock(void)
{
    if (!PMC_IsPeriphEnabled(ID_UDPHS)) {
        PMC_EnablePeripheral(ID_UDPHS);
        return 1;
    }
    return 0;
}

/**
 * Disables the UDP peripheral clock.
 */
static inline void UDPHS_DisablePeripheralClock(void)
{
    PMC_DisablePeripheral(ID_UDPHS);
}

/**
 * Enables the 480MHz USB clock.
 */
static inline void UDPHS_EnableUsbClock(void)
{
    Pmc *pPmc = PMC;
    /* Enable 480Mhz UPLL */
    pPmc->CKGR_UCKR |= CKGR_UCKR_UPLLEN
                       | CKGR_UCKR_UPLLCOUNT(0x3)
                       | CKGR_UCKR_BIASCOUNT(0x1);
    /* Wait until UPLL is locked */
    while((pPmc->PMC_SR & PMC_SR_LOCKU) == 0);
}

/**
 *  Disables the 480MHz USB clock.
 */
static inline void UDPHS_DisableUsbClock(void)
{
    Pmc *pPmc = PMC;
    /* Disable System Clock */
    //pPmc->PMC_SCDR = PMC_SCDR_UDP;
    pPmc->CKGR_UCKR &= ~(uint32_t)CKGR_UCKR_UPLLEN;
}

/**
 * Enables the BIAS.
 */
static inline void UDPHS_EnableBIAS(void)
{
    Pmc *pPmc = PMC;
    pPmc->CKGR_UCKR |= CKGR_UCKR_BIASEN;
}

/**
 * Disables the BIAS.
 */
static inline void UDPHS_DisableBIAS(void)
{
    Pmc *pPmc = PMC;
    pPmc->CKGR_UCKR &= ~(uint32_t)CKGR_UCKR_BIASEN;
}

/**
 * Handles a completed transfer on the given endpoint, invoking the
 * configured callback if any.
 * \param bEndpoint Number of the endpoint for which the transfer has completed.
 * \param bStatus   Status code returned by the transfer operation
 */
static void UDPHS_EndOfTransfer(uint8_t bEndpoint, uint8_t bStatus)
{
    Endpoint *pEp = &(endpoints[bEndpoint]);

    /* Check that endpoint was sending or receiving data */
    if ( (pEp->state == UDPHS_ENDPOINT_RECEIVING)
            || (pEp->state == UDPHS_ENDPOINT_SENDING) )
    {
        Transfer *pXfr = (Transfer*)&(pEp->transfer);
        uint32_t transferred = pXfr->transferred;
        uint32_t remaining   = pXfr->remaining + pXfr->buffered;
        
        TRACE_DEBUG_WP("EoT ");
        if (pEp->state == UDPHS_ENDPOINT_SENDING)
            pEp->sendZLP = 0;
        pEp->state = UDPHS_ENDPOINT_IDLE;
        pXfr->pData = 0;
        pXfr->transferred = -1;
        pXfr->buffered    = -1;
        pXfr->remaining   = -1;

        /* Invoke callback */
        if (pXfr->fCallback)
        {
            pXfr->fCallback(pXfr->pArgument, bStatus, transferred, remaining);
        }
        else
        {
            TRACE_DEBUG_WP("NoCB ");
        }
    }
    else if ( (pEp->state == UDPHS_ENDPOINT_RECEIVINGM)
                || (pEp->state == UDPHS_ENDPOINT_SENDINGM) )
    {
        MblTransfer *pXfr = (MblTransfer*)&(pEp->transfer);
        TRACE_DEBUG_WP("EoMT ");

        pEp->state = UDPHS_ENDPOINT_IDLE;
        pXfr->listState = 0;
        pXfr->outCurr = pXfr->inCurr = pXfr->outLast = 0;
        /* Invoke callback */
        if (pXfr->fCallback)
        {
            pXfr->fCallback(pXfr->pArgument, bStatus);
        }
        else
        {
            TRACE_DEBUG_WP("NoCB ");
        }
    }
}

/**
 * Update multi-buffer-transfer descriptors.
 * \param pTransfer Pointer to instance MblTransfer.
 * \param size      Size of bytes that processed.
 * \param forceEnd  Force the buffer END.
 * \return 1 if current buffer ended.
 */
static uint8_t UDPHS_MblUpdate(MblTransfer *pTransfer,
                          USBDTransferBuffer * pBi,
                          uint16_t size,
                          uint8_t forceEnd)
{
    /* Update transfer descriptor */
    pBi->remaining -= size;
    /* Check if list NULL */
    if (pTransfer->listState == MBL_NULL) {
        return 1;
    }
    /* Check if current buffer ended */
    if (pBi->remaining == 0 || forceEnd || size == 0) {

        /* Process to next buffer */
        if ((++ pTransfer->outCurr) == pTransfer->listSize)
            pTransfer->outCurr = 0;
        /* Check buffer NULL case */
        if (pTransfer->outCurr == pTransfer->inCurr)
            pTransfer->listState = MBL_NULL;
        else {
            pTransfer->listState = 0;
            /* Continue transfer, prepare for next operation */
            pBi = &pTransfer->pMbl[pTransfer->outCurr];
            pBi->buffered    = 0;
            pBi->transferred = 0;
            pBi->remaining   = pBi->size;
        }
        return 1;
    }
    return 0;
}

/**
 * Transfers a data payload from the current tranfer buffer to the endpoint
 * FIFO
 * \param bEndpoint Number of the endpoint which is sending data.
 */
static uint8_t UDPHS_MblWriteFifo(uint8_t bEndpoint)
{
    Endpoint    *pEndpoint   = &(endpoints[bEndpoint]);
    MblTransfer *pTransfer   = (MblTransfer*)&(pEndpoint->transfer);
    USBDTransferBuffer *pBi = &(pTransfer->pMbl[pTransfer->outCurr]);
    uint8_t *pFifo;
    int32_t size;

    volatile uint8_t * pBytes;
    volatile uint8_t bufferEnd = 1;

    /* Get the number of bytes to send */
    size = pEndpoint->size;
    if (size > pBi->remaining) size = pBi->remaining;

    TRACE_DEBUG_WP("w%d.%d ", pTransfer->outCurr, size);

    /* Record last accessed buffer */
    pTransfer->outLast = pTransfer->outCurr;

    pBytes = &(pBi->pBuffer[pBi->transferred + pBi->buffered]);
    pBi->buffered += size;
    bufferEnd = UDPHS_MblUpdate(pTransfer, pBi, size, 0);

    /* Write packet in the FIFO buffer */
    pFifo = (uint8_t*)((uint32_t*)UDPHS_RAM_ADDR
                                    + (EPT_VIRTUAL_SIZE * bEndpoint));
    if (size) {
        int32_t c8 = size >> 3;
        int32_t c1 = size & 0x7;
        for (; c8; c8 --) {
            *(pFifo++) = *(pBytes ++);
            *(pFifo++) = *(pBytes ++);
            *(pFifo++) = *(pBytes ++);
            *(pFifo++) = *(pBytes ++);

            *(pFifo++) = *(pBytes ++);
            *(pFifo++) = *(pBytes ++);
            *(pFifo++) = *(pBytes ++);
            *(pFifo++) = *(pBytes ++);
        }
        for (; c1; c1 --) {
            *(pFifo++) = *(pBytes ++);
        }
    }
    return bufferEnd;
}

#if 0
/**
 *  Transfers a data payload from an endpoint FIFO to the current transfer
 *  buffer, if NULL packet received, the current buffer is ENDed.
 *  \param bEndpoint Endpoint number.
 *  \param wPacketSize Size of received data packet */
 *  \return 1 if the buffer ENDed. */
 */
static uint8_t UDPHS_MblReadFifo(uint8_t bEndpoint, uint16_t wPacketSize)
{
   
    return 0;
}
*/
#endif
/**
 * Transfers a data payload from the current tranfer buffer to the endpoint
 * FIFO
 * \param bEndpoint Number of the endpoint which is sending data.
 */
static void UDPHS_WritePayload(uint8_t bEndpoint, int32_t size)
{
    Endpoint *pEndpoint = &(endpoints[bEndpoint]);
    Transfer *pTransfer = (Transfer*)&(pEndpoint->transfer);
    uint8_t  *pFifo;

    /* Get the number of bytes to send */
    if (size > pTransfer->remaining)
    {
        size = pTransfer->remaining;
    }

    /* Update transfer descriptor information */
    pTransfer->buffered += size;
    pTransfer->remaining -= size;

    /* Write packet in the FIFO buffer */
    pFifo = (uint8_t*)((uint32_t*)UDPHS_RAM_ADDR
                                    + (EPT_VIRTUAL_SIZE * bEndpoint));
    for (; size; size --)
    {
        *(pFifo ++) = *(pTransfer->pData ++);
    }
}

/**
 * Transfers a data payload from an endpoint FIFO to the current transfer buffer
 * \param bEndpoint Endpoint number.
 * \param wPacketSize Size of received data packet
 */
static void UDPHS_ReadPayload(uint8_t bEndpoint, int32_t wPacketSize)
{
    Endpoint *pEndpoint = &(endpoints[bEndpoint]);
    Transfer *pTransfer = (Transfer*)&(pEndpoint->transfer);
    uint8_t  *pFifo;
    /* Check that the requested size is not bigger than the remaining transfer */
    if (wPacketSize > pTransfer->remaining) {

        pTransfer->buffered += wPacketSize - pTransfer->remaining;
        wPacketSize = pTransfer->remaining;
    }

    /* Update transfer descriptor information */
    pTransfer->remaining -= wPacketSize;
    pTransfer->transferred += wPacketSize;

    /* Retrieve packet */
    pFifo = (uint8_t*)((uint32_t*)UDPHS_RAM_ADDR
                                    + (EPT_VIRTUAL_SIZE * bEndpoint));
    while (wPacketSize > 0)
    {
        *(pTransfer->pData ++) = *(pFifo ++);
        wPacketSize--;
    }
}

/**
 * Received SETUP packet from endpoint 0 FIFO
 * \param pRequest Generic USB SETUP request sent over Control endpoints
 */
static void UDPHS_ReadRequest(USBGenericRequest *pRequest)
{
    uint32_t *pData = (uint32_t *)(void*)pRequest;
    volatile uint32_t *pFifo;
    pFifo = (volatile uint32_t*)UDPHS_RAM_ADDR;
    *pData ++ = *pFifo;
    pFifo = (volatile uint32_t*)UDPHS_RAM_ADDR;
    *pData    = *pFifo;
}

/**
 * Endpoint interrupt handler.
 * Handle IN/OUT transfers, received SETUP packets and STALLing
 * \param bEndpoint Index of endpoint
 */
static void UDPHS_EndpointHandler(uint8_t bEndpoint)
{
    Udphs    *pUdp = UDPHS;
    UdphsEpt *pEpt = &pUdp->UDPHS_EPT[bEndpoint];
    //UdphsDma *pDma = &pUdp->UDPHS_DMA[bEndpoint];

    Endpoint *pEp      = &(endpoints[bEndpoint]);
    Transfer *pXfr     = (Transfer*)&(pEp->transfer);
    //MblTransfer *pMblt = (MblTransfer*)&(pEp->transfer);
    uint32_t status = pEpt->UDPHS_EPTSTA;
    uint32_t type   = pEpt->UDPHS_EPTCFG & UDPHS_EPTCFG_EPT_TYPE_Msk;
    uint32_t reqBuf[2];
    USBGenericRequest *pReq = (USBGenericRequest *)reqBuf;
    uint16_t wPktSize;

    TRACE_DEBUG_WP("Ep%d ", bEndpoint);
    //TRACE_DEBUG_WP("St:%x ", status);
    /* IN packet sent */
    if (   (pEpt->UDPHS_EPTCTL & UDPHS_EPTCTL_TXRDY)
        && (0 == (status & UDPHS_EPTSTA_TXRDY)) )
    {
        TRACE_DEBUG_WP("Wr ");

        /* Multi-buffer-list transfer state */
        if ( pEp->state == UDPHS_ENDPOINT_SENDINGM )
        {
        }
        /* Sending state */
        else if ( pEp->state == UDPHS_ENDPOINT_SENDING )
        {
            if (pXfr->buffered)
            {
                pXfr->transferred += pXfr->buffered;
                pXfr->buffered = 0;
            }
            if((pXfr->transferred % pEp->size == 0) && ( pXfr->remaining == 0) && ( pXfr->transferred > 0)&&(pEp->sendZLP == 0)) 
            { 
              pEp->sendZLP = 1; // Force ZLP transmission in total length is  a multiple of endpoint size 
  
            } 
            if (   pXfr->buffered == 0
                && pXfr->transferred == 0
                && pXfr->remaining == 0
                && pEp->sendZLP == 0 )
            {
                pEp->sendZLP = 1;
            }

            /* End of Xfr ? */
            if (   pXfr->remaining
                || pEp->sendZLP == 1)
            {
          if(pEp->sendZLP == 1)
            { 
                // A null packet will be send, keep trace of it : Change this value only if ZLP will be send!!! 
                pEp->sendZLP = 2; 
            } 

                /* Transfer remaining */
                TRACE_DEBUG_WP("%d ", pEp->size);
                /* Send next packet */
                UDPHS_WritePayload(bEndpoint, pEp->size);
                pEpt->UDPHS_EPTSETSTA = UDPHS_EPTSETSTA_TXRDY;
            }
            else
            {
                TRACE_DEBUG_WP("l%d ", pXfr->transferred);
                /* Disable interrupt on none-control EP */
                if (type != UDPHS_EPTCFG_EPT_TYPE_CTRL8)
                {
                    pUdp->UDPHS_IEN &= ~(UDPHS_IEN_EPT_0 << bEndpoint);
                }
                pEpt->UDPHS_EPTCTLDIS = UDPHS_EPTCTLDIS_TXRDY;

                UDPHS_EndOfTransfer(bEndpoint, USBD_STATUS_SUCCESS);
                pEp->sendZLP = 0;
            }
        }
        else
        {
            TRACE_DEBUG("Err Wr %d\n\r", pEp->sendZLP);
        }
    }
    /* OUT packet received */
    if ( UDPHS_EPTSTA_RXRDY_TXKL & status )
    {
        TRACE_DEBUG_WP("Rd ");

        /* NOT in receiving state */
        if (pEp->state != UDPHS_ENDPOINT_RECEIVING)
        {
            /* Check if ACK received on a Control EP */
            if (   (UDPHS_EPTCFG_EPT_TYPE_CTRL8 == type)
                && (0 == (status & UDPHS_EPTSTA_BYTE_COUNT_Msk)) )
            {
                TRACE_DEBUG_WP("Ack ");
                pEpt->UDPHS_EPTCLRSTA = UDPHS_EPTCLRSTA_RXRDY_TXKL;
                UDPHS_EndOfTransfer(bEndpoint, USBD_STATUS_SUCCESS);
            }
            /* data has been STALLed */
            else if (UDPHS_EPTSTA_FRCESTALL & status)
            {
                TRACE_DEBUG_WP("Discard ");
                pEpt->UDPHS_EPTCLRSTA = UDPHS_EPTCLRSTA_RXRDY_TXKL;
            }
            /* NAK the data */
            else
            {
                TRACE_DEBUG_WP("Nak ");
                pUdp->UDPHS_IEN &= ~(UDPHS_IEN_EPT_0 << bEndpoint);
            }
        }
        /* In read state */
        else
        {
            wPktSize = (uint16_t)((status & UDPHS_EPTSTA_BYTE_COUNT_Msk) >> UDPHS_EPTSTA_BYTE_COUNT_Pos);

            TRACE_DEBUG_WP("%d ", wPktSize);
            UDPHS_ReadPayload(bEndpoint, wPktSize);
            pEpt->UDPHS_EPTCLRSTA = UDPHS_EPTCLRSTA_RXRDY_TXKL;
            /* Check if transfer is finished */
            if (pXfr->remaining == 0 || wPktSize < pEp->size)
            {
                pEpt->UDPHS_EPTCTLDIS = UDPHS_EPTCTLDIS_RXRDY_TXKL;

                /* Disable interrupt if not control EP */
                if (UDPHS_EPTCFG_EPT_TYPE_CTRL8 != type)
                {
                    pUdp->UDPHS_IEN &= ~(UDPHS_IEN_EPT_0 << bEndpoint);
                }
                UDPHS_EndOfTransfer(bEndpoint, USBD_STATUS_SUCCESS);
            }
        }
    }
    /* STALL sent */
    if ( UDPHS_EPTSTA_STALL_SNT & status )
    {
        /* Acknowledge */
        pEpt->UDPHS_EPTCLRSTA = UDPHS_EPTCLRSTA_STALL_SNT;

        /* ISO error */
        if (type == UDPHS_EPTCFG_EPT_TYPE_ISO)
        {
            TRACE_WARNING("IsoE[%d]\n\r", bEndpoint);

            UDPHS_EndOfTransfer(bEndpoint, USBD_STATUS_ABORTED);
        }
        /* If EP is not halted, clear STALL */
        else
        {
            TRACE_WARNING("Stall[%d]\n\r", bEndpoint);

            if (pEp->state != UDPHS_ENDPOINT_HALTED)
            {
                pEpt->UDPHS_EPTCLRSTA = UDPHS_EPTCLRSTA_FRCESTALL;
            }
        }
    }
    /* SETUP packet received */
    if ( UDPHS_EPTSTA_RX_SETUP & status )
    {
        /* If a transfer was pending, complete it
           Handles the case where during the status phase of a control write
           transfer, the host receives the device ZLP and ack it, but the ack
           is not received by the device */
        if (pEp->state == UDPHS_ENDPOINT_RECEIVING
            || pEp->state == UDPHS_ENDPOINT_RECEIVINGM
            || pEp->state == UDPHS_ENDPOINT_SENDING
            || pEp->state == UDPHS_ENDPOINT_SENDINGM)
        {
            UDPHS_EndOfTransfer(bEndpoint, USBD_STATUS_SUCCESS);
        }

        /* ISO Err Flow */
        if (type == UDPHS_EPTCFG_EPT_TYPE_ISO)
        {
            TRACE_WARNING("IsoFE[%d]\n\r", bEndpoint);
            /* Acknowledge setup packet */
            pEpt->UDPHS_EPTCLRSTA = UDPHS_EPTCLRSTA_RX_SETUP;
        }
        else
        {
            TRACE_DEBUG_WP("Stup ");
            
            /* Copy setup */
            UDPHS_ReadRequest(pReq);
            /* Acknowledge setup packet */
            pEpt->UDPHS_EPTCLRSTA = UDPHS_EPTCLRSTA_RX_SETUP;
            /* Handler */
            USBD_RequestHandler(bEndpoint, pReq);
        }
    }
}
#ifdef DMA
/**
 * DMA Single transfer
 * \param bEndpoint EP number.
 * \pXfr  Pointer to transfer instance.
 * \dwCfg DMA Control configuration (excluding length).
 */
static inline void UDPHS_DmaSingle(uint8_t bEndpoint, Transfer *pXfr, uint32_t dwCfg)
{
    Udphs *pUdp = UDPHS;

    /* Single transfer */
    CP15_coherent_dcache_for_dma ((uint32_t)&pXfr->pData[pXfr->transferred], ((uint32_t)&pXfr->pData[pXfr->transferred]) + pXfr->buffered);
    pUdp->UDPHS_DMA[bEndpoint].UDPHS_DMAADDRESS =
                (uint32_t)&pXfr->pData[pXfr->transferred];
    pUdp->UDPHS_DMA[bEndpoint].UDPHS_DMASTATUS;
    /* Interrupt enable */
    pUdp->UDPHS_IEN |= (1 << SHIFT_DMA << bEndpoint);
    
    TRACE_DEBUG_WP("Dma[B%d:T%d] ", pXfr->buffered, pXfr->transferred);
    /* DMA Configure */
    pUdp->UDPHS_DMA[bEndpoint].UDPHS_DMACONTROL = 0;
    pUdp->UDPHS_DMA[bEndpoint].UDPHS_DMACONTROL = 0
                            | UDPHS_DMACONTROL_BUFF_LENGTH(pXfr->buffered)
                            | dwCfg;
}
/**
 * Endpoint DMA interrupt handler.
 * This function handles DMA interrupts.
 * \param bEndpoint Index of endpoint
 */
static void UDPHS_DmaHandler(uint8_t bEndpoint)
{
    Udphs    *pUdp  = UDPHS;
    //UdphsEpt *pHwEp = &pUdp->UDPHS_EPT[bEndpoint];

    Endpoint *pEp  = &(endpoints[bEndpoint]);
    Transfer *pXfr = (Transfer*)&(pEp->transfer);

    uint32_t dwDmaSr;
    int32_t iRemain, iXfred;
    uint8_t bRc = USBD_STATUS_SUCCESS;

    CP15_flush_dcache_for_dma ((uint32_t)&pXfr->pData[pXfr->transferred], ((uint32_t)&pXfr->pData[pXfr->transferred]) + pXfr->buffered);
    dwDmaSr = pUdp->UDPHS_DMA[bEndpoint].UDPHS_DMASTATUS;
    TRACE_DEBUG_WP("iDma%d,%x ", bEndpoint, dwDmaSr);
    /* Mbl transfer */
    if (pEp->state == UDPHS_ENDPOINT_SENDINGM)
    {
        /* Not implemented */
        return;
    }
    else if (pEp->state == UDPHS_ENDPOINT_RECEIVINGM)
    {
        /* Not implemented */
        return;
    }

    /* Disable DMA interrupt to avoid receiving 2 (B_EN and TR_EN) */
    pUdp->UDPHS_DMA[bEndpoint].UDPHS_DMACONTROL &= ~(UDPHS_DMACONTROL_END_TR_EN
                                                    |UDPHS_DMACONTROL_END_B_EN);
    if (UDPHS_DMASTATUS_END_BF_ST & dwDmaSr)
    {
        TRACE_DEBUG_WP("EoDmaB ");
        /* BUFF_COUNT holds the number of untransmitted bytes.
           BUFF_COUNT is equal to zero in case of good transfer */
        iRemain = (dwDmaSr & UDPHS_DMASTATUS_BUFF_COUNT_Msk)
                        >> UDPHS_DMASTATUS_BUFF_COUNT_Pos;
        TRACE_DEBUG_WP("C%d ", iRemain);
        iXfred  = pXfr->buffered - iRemain;

        pXfr->transferred += iXfred;
        pXfr->buffered     = iRemain;
        pXfr->remaining   -= iXfred;
        TRACE_DEBUG_WP("[B%d:T%d:R%d] ", pXfr->buffered, pXfr->transferred, pXfr->remaining);
        /* There is still data */
        if (pXfr->remaining + pXfr->buffered > 0)
        {   
            if (pXfr->remaining > DMA_MAX_FIFO_SIZE)
            {
                pXfr->buffered = DMA_MAX_FIFO_SIZE;
            }
            else
            {
                pXfr->buffered = pXfr->remaining;
            }
            /* Single transfer again */
            UDPHS_DmaSingle(bEndpoint, pXfr, UDPHS_DMACONTROL_END_TR_EN
                                             | UDPHS_DMACONTROL_END_TR_IT
                                             | UDPHS_DMACONTROL_END_B_EN
                                             | UDPHS_DMACONTROL_END_BUFFIT
                                             | UDPHS_DMACONTROL_CHANN_ENB);
        }
    }
    else if (UDPHS_DMASTATUS_END_TR_ST & dwDmaSr)
    {
        TRACE_DEBUG_WP("EoDmaT ");
        pXfr->transferred = pXfr->buffered -
            ((dwDmaSr & UDPHS_DMASTATUS_BUFF_COUNT_Msk)
                    >> UDPHS_DMASTATUS_BUFF_COUNT_Pos);
        pXfr->remaining = 0;

        TRACE_DEBUG_WP("[B%d:T%d] ", pXfr->buffered, pXfr->transferred);
    }
    else
    {
        TRACE_ERROR("UDPHS_DmaHandler: ST 0x%X\n\r", (unsigned int)dwDmaSr);
        bRc = USBD_STATUS_ABORTED;
    }
    /* Callback */
    if (pXfr->remaining == 0)
    {
        UDPHS_EndOfTransfer(bEndpoint, bRc);
        
    }
    
}
#endif
/**
 * Sends data through a USB endpoint. Sets up the transfer descriptor,
 * writes one or two data payloads (depending on the number of FIFO bank
 * for the endpoint) and then starts the actual transfer. The operation is
 * complete when all the data has been sent.
 *
 * *If the size of the buffer is greater than the size of the endpoint
 *  (or twice the size if the endpoint has two FIFO banks), then the buffer
 *  must be kept allocated until the transfer is finished*. This means that
 *  it is not possible to declare it on the stack (i.e. as a local variable
 *  of a function which returns after starting a transfer).
 *
 * \param pEndpoint Pointer to Endpoint struct.
 * \param pData Pointer to a buffer with the data to send.
 * \param dLength Size of the data buffer.
 * \return USBD_STATUS_SUCCESS if the transfer has been started;
 *         otherwise, the corresponding error status code.
 */
static inline uint8_t UDPHS_Write(uint8_t    bEndpoint,
                                const void *pData,
                                uint32_t   dLength)
{
    Udphs    *pUdp  = UDPHS;
    UdphsEpt *pHwEp = &pUdp->UDPHS_EPT[bEndpoint];
  
    Endpoint *pEp  = &(endpoints[bEndpoint]);
    Transfer *pXfr = (Transfer*)&(pEp->transfer);
    /* Return if busy */
    if (pEp->state != UDPHS_ENDPOINT_IDLE)
    {
        return USBD_STATUS_LOCKED;
    }
    /* Sending state */
    pEp->state = UDPHS_ENDPOINT_SENDING;
    TRACE_DEBUG_WP("Wr%d(%d) ", bEndpoint, dLength);
    pEp->sendZLP = 0;
    /* Setup transfer descriptor */
    pXfr->pData       = (void*) pData;
    pXfr->remaining   = dLength;
    pXfr->buffered    = 0;
    pXfr->transferred = 0;
  #ifdef DMA
    /* 1. DMA supported, 2. Not ZLP */
    if (CHIP_USB_ENDPOINTS_DMA(bEndpoint)
        && pXfr->remaining > 0)
    {
        if (pXfr->remaining > DMA_MAX_FIFO_SIZE)
        {
            /* Transfer the max */
            pXfr->buffered = DMA_MAX_FIFO_SIZE;
        }
        else
        {
            /* Good size */
            pXfr->buffered = pXfr->remaining;
        }
        /* Single transfer */
        UDPHS_DmaSingle(bEndpoint, pXfr, UDPHS_DMACONTROL_END_B_EN
                                         | UDPHS_DMACONTROL_END_BUFFIT
                                         | UDPHS_DMACONTROL_CHANN_ENB);
        return USBD_STATUS_SUCCESS;
    }
  #endif

    /* Enable IT */
    pUdp->UDPHS_IEN |= ( UDPHS_IEN_EPT_0 << bEndpoint );
    pHwEp->UDPHS_EPTCTLENB = UDPHS_EPTCTLENB_TXRDY;
    return USBD_STATUS_SUCCESS;
}

/**
 * Sends data through a USB endpoint. Sets up the transfer descriptor list,
 * writes one or two data payloads (depending on the number of FIFO bank
 * for the endpoint) and then starts the actual transfer. The operation is
 * complete when all the transfer buffer in the list has been sent.
 *
 * *If the size of the buffer is greater than the size of the endpoint
 *  (or twice the size if the endpoint has two FIFO banks), then the buffer
 *  must be kept allocated until the transfer is finished*. This means that
 *  it is not possible to declare it on the stack (i.e. as a local variable
 *  of a function which returns after starting a transfer).
 *
 * \param pEndpoint Pointer to Endpoint struct.
 * \param pData Pointer to a buffer with the data to send.
 * \param dLength Size of the data buffer.
 * \return USBD_STATUS_SUCCESS if the transfer has been started;
 *         otherwise, the corresponding error status code.
 */
static inline uint8_t UDPHS_AddWr(uint8_t   bEndpoint,
                                const void *pData,
                                uint32_t    dLength)
{
    Udphs    *pUdp  = UDPHS;
    UdphsEpt *pHwEp = &pUdp->UDPHS_EPT[bEndpoint];

    Endpoint *pEp  = &(endpoints[bEndpoint]);
    MblTransfer *pMbl = (MblTransfer*)&(pEp->transfer);
    USBDTransferBuffer *pTx;
    /* Check parameter */
    if (dLength >= 0x10000)
    {
        return USBD_STATUS_INVALID_PARAMETER;
    }
    /* Data in process */
    if (pEp->state > UDPHS_ENDPOINT_IDLE)
    {   /* MBL transfer */
        if (pMbl->transType)
        {
            if (pMbl->listState == MBL_FULL)
            {
                return USBD_STATUS_LOCKED;
            }
        }
        else
        {
            return USBD_STATUS_LOCKED;
        }
    }

    TRACE_DEBUG_WP("AddW%d(%d) ", bEndpoint, dLength);
    /* Add buffer to buffer list and update index */
    pTx = &(pMbl->pMbl[pMbl->inCurr]);
    pTx->pBuffer = (uint8_t*)pData;
    pTx->size = pTx->remaining = dLength;
    pTx->transferred = pTx->buffered = 0;
    /* Update input index */
    if (pMbl->inCurr >= (pMbl->listSize-1)) pMbl->inCurr = 0;
    else                                    pMbl->inCurr ++;
    if (pMbl->inCurr == pMbl->outCurr)      pMbl->listState = MBL_FULL;
    else                                    pMbl->listState = 0;
    /* Start sending when offset achieved */
    if (MBL_NbBuffer(pMbl->inCurr, pMbl->outCurr, pMbl->listSize)
            >= pMbl->offsetSize
        && pEp->state == UDPHS_ENDPOINT_IDLE)
    {
        uint8_t nbBanks = CHIP_USB_ENDPOINTS_BANKS(bEndpoint);

        /* Change state */
        pEp->state = UDPHS_ENDPOINT_SENDINGM;

        TRACE_DEBUG_WP("StartM ");

        /* Fill data into FIFO */
        for (;
             nbBanks && pMbl->pMbl[pMbl->inCurr].remaining;
             nbBanks --)
        {
            UDPHS_MblWriteFifo(bEndpoint);
            pHwEp->UDPHS_EPTSETSTA = UDPHS_EPTSETSTA_TXRDY;
        }

        /* Enable interrupt */
        pUdp->UDPHS_IEN |= (UDPHS_IEN_EPT_0 << bEndpoint);
        pHwEp->UDPHS_EPTCTLENB = UDPHS_EPTCTLENB_TXRDY;

    }

    return USBD_STATUS_SUCCESS;
}

/**
 * Reads incoming data on an USB endpoint This methods sets the transfer
 * descriptor and activate the endpoint interrupt. The actual transfer is
 * then carried out by the endpoint interrupt handler. The Read operation
 * finishes either when the buffer is full, or a short packet (inferior to
 * endpoint maximum  size) is received.
 *
 * *The buffer must be kept allocated until the transfer is finished*.
 * \param bEndpoint Endpoint number.
 * \param pData Pointer to a data buffer.
 * \param dLength Size of the data buffer in bytes.
 * \return USBD_STATUS_SUCCESS if the read operation has been started;
 *         otherwise, the corresponding error code.
 */
static inline uint8_t UDPHS_Read(uint8_t  bEndpoint,
                                 void     *pData,
                                 uint32_t dLength)
{
    Udphs    *pUdp  = UDPHS;
    UdphsEpt *pHwEp = &pUdp->UDPHS_EPT[bEndpoint];

    Endpoint *pEp  = &(endpoints[bEndpoint]);
    Transfer *pXfr = (Transfer*)&(pEp->transfer);
    /* Return if busy */
    if (pEp->state != UDPHS_ENDPOINT_IDLE)
    {
        return USBD_STATUS_LOCKED;
    }
    /* Receiving state */
    pEp->state = UDPHS_ENDPOINT_RECEIVING;

    TRACE_DEBUG_WP("Rd%d(%d) ", bEndpoint, dLength);
    /* Setup transfer descriptor */
    pXfr->pData       = (void*) pData;
    pXfr->remaining   = dLength;
    pXfr->buffered    = 0;
    pXfr->transferred = 0;

  #ifdef DMA
    /* If: 1. DMA supported, 2. Has data */
    if (CHIP_USB_ENDPOINTS_DMA(bEndpoint)
        && pXfr->remaining > 0)
    {
        /* DMA XFR size adjust */
        if (pXfr->remaining > DMA_MAX_FIFO_SIZE)
            pXfr->buffered = DMA_MAX_FIFO_SIZE;
        else
            pXfr->buffered = pXfr->remaining;
        /* Single transfer */
        UDPHS_DmaSingle(bEndpoint, pXfr, UDPHS_DMACONTROL_END_TR_EN
                                         | UDPHS_DMACONTROL_END_TR_IT
                                         | UDPHS_DMACONTROL_END_B_EN
                                         | UDPHS_DMACONTROL_END_BUFFIT
                                         | UDPHS_DMACONTROL_CHANN_ENB);
        return USBD_STATUS_SUCCESS;
    }
  #endif

    /* Enable IT */
    pUdp->UDPHS_IEN |= ( UDPHS_IEN_EPT_0 << bEndpoint );
    pHwEp->UDPHS_EPTCTLENB = UDPHS_EPTCTLENB_RXRDY_TXKL;
    
    return USBD_STATUS_SUCCESS;
}
#if 0
/**
 * Reads incoming data on an USB endpoint This methods sets the transfer
 * descriptor and activate the endpoint interrupt. The actual transfer is
 * then carried out by the endpoint interrupt handler. The Read operation
 * finishes either when the buffer is full, or a short packet (inferior to
 * endpoint maximum  size) is received.
 *
 * *The buffer must be kept allocated until the transfer is finished*.
 * \param bEndpoint Endpoint number.
 * \param pData Pointer to a data buffer.
 * \param dLength Size of the data buffer in bytes.
 * \return USBD_STATUS_SUCCESS if the read operation has been started;
 *         otherwise, the corresponding error code.
 */
static inline uint8_t UDPHS_AddRd(uint8_t  bEndpoint,
                                void     *pData,
                                uint32_t dLength)
{
    return USBD_STATUS_SW_NOT_SUPPORTED;
}
#endif
/*---------------------------------------------------------------------------
 *      Exported functions
 *---------------------------------------------------------------------------*/
extern void UDPHS_IrqHandler(void);
extern void USBD_HAL_ForceFs(void);
/**
 * USBD (UDP) interrupt handler
 * Manages device resume, suspend, end of bus reset.
 * Forwards endpoint events to the appropriate handler.
 */
void UDPHS_IrqHandler(void)
{
    Udphs *pUdp = UDPHS;

    uint32_t status;
    uint8_t  numIt;

    status  = pUdp->UDPHS_INTSTA;
    status &= pUdp->UDPHS_IEN;

    /* Handle all UDPHS interrupts */
    TRACE_DEBUG_WP("\n\r%c ", USBD_HAL_IsHighSpeed() ? 'H' : 'F');
    while( status )
    {
        /* SOF */
        if (status & UDPHS_INTSTA_INT_SOF)
        {
            TRACE_DEBUG_WP("SOF ");
            /* SOF handler */
            //USBD_SofHandler();

            /* Acknowledge interrupt */
            pUdp->UDPHS_CLRINT = UDPHS_CLRINT_INT_SOF;
            status &= ~(uint32_t)UDPHS_INTSTA_INT_SOF;
        }
        /* Suspend, treated last */
        else if (status == UDPHS_INTSTA_DET_SUSPD)
        {
            TRACE_WARNING_WP("Susp ");
            /* Enable wakeup */
            pUdp->UDPHS_IEN |= (UDPHS_IEN_WAKE_UP | UDPHS_IEN_ENDOFRSM);
            pUdp->UDPHS_IEN &= ~(uint32_t)UDPHS_IEN_DET_SUSPD;

            /* Acknowledge interrupt */
            pUdp->UDPHS_CLRINT = UDPHS_CLRINT_DET_SUSPD | UDPHS_CLRINT_WAKE_UP;

            USBD_SuspendHandler();
        }
        /* Resume */
        else if ( (status & UDPHS_INTSTA_WAKE_UP)
               || (status & UDPHS_INTSTA_ENDOFRSM) )
        {
            USBD_ResumeHandler();

            TRACE_INFO_WP("Rsm ");

            /* Acknowledge interrupt */
            pUdp->UDPHS_CLRINT = UDPHS_CLRINT_WAKE_UP
                               | UDPHS_CLRINT_ENDOFRSM
                               | UDPHS_CLRINT_DET_SUSPD;

            pUdp->UDPHS_IEN |= UDPHS_IEN_ENDOFRSM | UDPHS_IEN_DET_SUSPD;
            pUdp->UDPHS_CLRINT = UDPHS_CLRINT_WAKE_UP | UDPHS_CLRINT_ENDOFRSM;
            pUdp->UDPHS_IEN &= ~(uint32_t)UDPHS_IEN_WAKE_UP;
        }
        /* Bus reset */
        else if (status & UDPHS_INTSTA_ENDRESET)
        {
            TRACE_DEBUG_WP("EoB ");
            /* Flush and enable the suspend interrupt */
            pUdp->UDPHS_CLRINT = UDPHS_CLRINT_WAKE_UP | UDPHS_CLRINT_DET_SUSPD;
            pUdp->UDPHS_IEN |= UDPHS_IEN_DET_SUSPD;

            /* Reset handler */
            USBD_ResetHandler();

            /* Acknowledge interrupt */
            pUdp->UDPHS_CLRINT = UDPHS_CLRINT_ENDRESET;
        }
        /* Upstream resume */
        else if (status & UDPHS_INTSTA_UPSTR_RES)
        {
            TRACE_DEBUG_WP("ExtRes ");
            /* Acknowledge interrupt */
            pUdp->UDPHS_CLRINT = UDPHS_CLRINT_UPSTR_RES;
        }
        /* Endpoints */
        else
        {
          #ifdef DMA
            for (numIt = 0; numIt < NUM_IT_MAX; numIt ++)
            {
                if (status & (1 << SHIFT_DMA << numIt))
                {
                    UDPHS_DmaHandler(numIt);
                }
                else if (status & (UDPHS_INTSTA_EPT_0 << numIt))
                {
                    UDPHS_EndpointHandler(numIt);
                }
            }
          #else
            for (numIt = 0; numIt < NUM_IT_MAX; numIt ++)
            {
                if (status & (UDPHS_INTSTA_EPT_0 << numIt))
                {
                    UDPHS_EndpointHandler(numIt);
                }
            }
          #endif
        }

        /* Update interrupt status */
        status  = pUdp->UDPHS_INTSTA;
        status &= pUdp->UDPHS_IEN;

        TRACE_DEBUG_WP("\n\r");
        if (status)
        {
            TRACE_DEBUG_WP(" - ");
        }
    }
}

/**
 * \brief Reset endpoints and disable them.
 * -# Terminate transfer if there is any, with given status;
 * -# Reset the endpoint & disable it.
 * \param bmEPs    Bitmap for endpoints to reset.
 * \param bStatus  Status passed to terminate transfer on endpoint.
 * \param bKeepCfg 1 to keep old endpoint configuration.
 * \note Use USBD_HAL_ConfigureEP() to configure and enable endpoint
         if not keeping old configuration.
 * \sa USBD_HAL_ConfigureEP().
 */
void USBD_HAL_ResetEPs(uint32_t bmEPs, uint8_t bStatus, uint8_t bKeepCfg)
{
    Udphs *pUdp = UDPHS;
    UdphsEpt *pHwEp;

    Endpoint *pEndpoint;
    uint32_t tmp = bmEPs & ((1<<CHIP_USB_NUMENDPOINTS)-1);
    uint8_t  ep;
    uint32_t epBit, epCfg;

    for (ep = 0, epBit = 1; ep < CHIP_USB_NUMENDPOINTS; ep ++)
    {
        if (tmp & epBit)
        {
            pHwEp = &pUdp->UDPHS_EPT[ep];

            /* Disable ISR */
            pUdp->UDPHS_IEN &= ~(epBit << SHIFT_INTERUPT);
            /* Kill pending Banks ?? */
            #if 0
            pHwEp->UDPHS_EPTSETSTA = UDPHS_EPTSETSTA_KILL_BANK;
            pHwEp->UDPHS_EPTSETSTA = UDPHS_EPTSETSTA_KILL_BANK;
            pHwEp->UDPHS_EPTSETSTA = UDPHS_EPTSETSTA_KILL_BANK;
            #endif
            
            /* Reset transfer information */
            pEndpoint = &(endpoints[ep]);
            /* Reset endpoint state */
            pEndpoint->bank = 0;
            /* Endpoint configure */
            epCfg = pHwEp->UDPHS_EPTCFG;
            /* Reset endpoint */
            pUdp->UDPHS_EPTRST = epBit;
            /* Restore configure */
            if (bKeepCfg)
            {
                pHwEp->UDPHS_EPTCFG = epCfg;
            }
            else
            {
                pEndpoint->state = UDPHS_ENDPOINT_DISABLED;
            }

            /* Terminate transfer on this EP */
            UDPHS_EndOfTransfer(ep, bStatus);
        }
        epBit <<= 1;
    }
}

/**
 * Cancel pending READ/WRITE
 * \param bmEPs    Bitmap for endpoints to reset.
 * \note EP callback is invoked with USBD_STATUS_CANCELED.
 */
void USBD_HAL_CancelIo(uint32_t bmEPs)
{
    Udphs *pUdp = UDPHS;
    //UdphsEpt *pHwEp = NULL;

    uint32_t tmp = bmEPs & ((1<<CHIP_USB_NUMENDPOINTS)-1);
    uint8_t  ep;
    uint32_t epBit;
    for (ep = 0, epBit = 1; ep < CHIP_USB_NUMENDPOINTS; ep ++)
    {
        if (tmp & epBit)
        {
            //pHwEp = &pUdp->UDPHS_EPT[ep];

            /* Disable ISR */
            pUdp->UDPHS_IEN &= ~(epBit << SHIFT_INTERUPT);
            /* Kill pending Banks ?? */
            #if 0
            pHwEp->UDPHS_EPTSETSTA = UDPHS_EPTSETSTA_KILL_BANK;
            pHwEp->UDPHS_EPTSETSTA = UDPHS_EPTSETSTA_KILL_BANK;
            pHwEp->UDPHS_EPTSETSTA = UDPHS_EPTSETSTA_KILL_BANK;
            #endif

            /* Terminate transfer on this EP */
            UDPHS_EndOfTransfer(ep, USBD_STATUS_CANCELED);
        }
        epBit <<= 1;
    }
}

/**
 * Configures an endpoint according to its endpoint Descriptor.
 * \param pDescriptor Pointer to an endpoint descriptor.
 * \return The endpoint address.
 */
uint8_t USBD_HAL_ConfigureEP(const USBEndpointDescriptor *pDescriptor)
{
    Udphs    *pUdp = UDPHS;
    UdphsEpt *pEpt;
    //UdphsDma *pDma;

    Endpoint *pEndpoint;
    uint8_t  bEndpoint;
    uint8_t  bType;
    uint8_t  bEndpointDir;
    //uint8_t bInterval = 0;
    uint8_t  bNbTrans  = 1;
    uint8_t  bSizeEpt  = 0;
    uint8_t  bHs = ((pUdp->UDPHS_INTSTA & UDPHS_INTSTA_SPEED) > 0);

    /* NULL descriptor -> Control endpoint 0 */
    if (pDescriptor == 0)
    {

        bEndpoint = 0;
        pEndpoint = &(endpoints[bEndpoint]);
        pEpt      = &(pUdp->UDPHS_EPT[0]);
        bType = USBEndpointDescriptor_CONTROL;
        bEndpointDir = 0;
        pEndpoint->size = CHIP_USB_ENDPOINTS_MAXPACKETSIZE(0);
        pEndpoint->bank = CHIP_USB_ENDPOINTS_BANKS(0);
    }
    /* Device descriptor -> Control endpoint 0 */
    else if (pDescriptor->bDescriptorType == USBGenericDescriptor_DEVICE)
    {
        USBDeviceDescriptor *pDevDesc = (USBDeviceDescriptor*)pDescriptor;
        bEndpoint = 0;
        pEndpoint = &(endpoints[bEndpoint]);
        pEpt      = &(pUdp->UDPHS_EPT[0]);
        bType = USBEndpointDescriptor_CONTROL;
        bEndpointDir = 0;
        pEndpoint->size =pDevDesc->bMaxPacketSize0;
        pEndpoint->bank = CHIP_USB_ENDPOINTS_BANKS(0);
    }
    /* Endpoint descriptor */
    else
    {
        /* The endpoint number */
        bEndpoint = USBEndpointDescriptor_GetNumber(pDescriptor);
        pEndpoint = &(endpoints[bEndpoint]);
        pEpt      = &(pUdp->UDPHS_EPT[bEndpoint]);
        /* Transfer type: Control, Isochronous, Bulk, Interrupt */
        bType = USBEndpointDescriptor_GetType(pDescriptor);
        /* interval */
        //bInterval = USBEndpointDescriptor_GetInterval(pDescriptor);
        /* Direction, ignored for control endpoints */
        bEndpointDir = USBEndpointDescriptor_GetDirection(pDescriptor);
        pEndpoint->size = USBEndpointDescriptor_GetMaxPacketSize(pDescriptor);
        pEndpoint->bank = CHIP_USB_ENDPOINTS_BANKS(bEndpoint);

        /* Convert descriptor value to EP configuration */
        if (bHs) {  /* HS Interval, *125us */

            /* MPS: Bit12,11 specify NB_TRANS, as USB 2.0 Spec. */
            bNbTrans = ((pEndpoint->size >> 11) & 0x3);
            if (bNbTrans == 3)
                bNbTrans = 1;
            else
                bNbTrans ++;

            /* Mask, bit 10..0 is the size */
            pEndpoint->size &= 0x7FF;
        }
    }

    //TRACE_DEBUG_WP("CfgE%d ", bEndpoint);

    /* Abort the current transfer is the endpoint was configured and in
       Write or Read state */
    if( (pEndpoint->state == UDPHS_ENDPOINT_RECEIVING)
     || (pEndpoint->state == UDPHS_ENDPOINT_SENDING)
     || (pEndpoint->state == UDPHS_ENDPOINT_RECEIVINGM)
     || (pEndpoint->state == UDPHS_ENDPOINT_SENDINGM) ) {

        UDPHS_EndOfTransfer(bEndpoint, USBD_STATUS_RESET);
    }
    pEndpoint->state = UDPHS_ENDPOINT_IDLE;

    /* Disable endpoint */
    pEpt->UDPHS_EPTCTLDIS = UDPHS_EPTCTLDIS_SHRT_PCKT
                          | UDPHS_EPTCTLDIS_BUSY_BANK
                          | UDPHS_EPTCTLDIS_NAK_OUT
                          | UDPHS_EPTCTLDIS_NAK_IN
                          | UDPHS_EPTCTLDIS_STALL_SNT
                          | UDPHS_EPTCTLDIS_RX_SETUP
                          | UDPHS_EPTCTLDIS_TXRDY
                          | UDPHS_EPTCTLDIS_RXRDY_TXKL
                          | UDPHS_EPTCTLDIS_ERR_OVFLW
                          | UDPHS_EPTCTLDIS_MDATA_RX
                          | UDPHS_EPTCTLDIS_DATAX_RX
                          | UDPHS_EPTCTLDIS_NYET_DIS
                          | UDPHS_EPTCTLDIS_INTDIS_DMA
                          | UDPHS_EPTCTLDIS_AUTO_VALID
                          | UDPHS_EPTCTLDIS_EPT_DISABL
                          ;
    /* Reset Endpoint Fifos */
    pEpt->UDPHS_EPTCLRSTA = UDPHS_EPTCLRSTA_TOGGLESQ | UDPHS_EPTCLRSTA_FRCESTALL;
    pUdp->UDPHS_EPTRST = 1 << bEndpoint;
    /* Configure endpoint size */
    if( pEndpoint->size <= 8 )
        bSizeEpt = 0;
    else if ( pEndpoint->size <= 16 )
        bSizeEpt = 1;
    else if ( pEndpoint->size <= 32 )
        bSizeEpt = 2;
    else if ( pEndpoint->size <= 64 )
        bSizeEpt = 3;
    else if ( pEndpoint->size <= 128 )
        bSizeEpt = 4;
    else if ( pEndpoint->size <= 256 )
        bSizeEpt = 5;
    else if ( pEndpoint->size <= 512 )
        bSizeEpt = 6;
    else if ( pEndpoint->size <= 1024 )
        bSizeEpt = 7;

    /* Configure endpoint */
    if (bType == USBEndpointDescriptor_CONTROL)
    {
        pUdp->UDPHS_IEN |= (UDPHS_IEN_EPT_0 << bEndpoint);
    }

    pEpt->UDPHS_EPTCFG =    bSizeEpt 
                        | ( bEndpointDir << 3) 
                        | ( bType << 4) 
                        | ((pEndpoint->bank) << 6)
                        | ( bNbTrans << 8)
                        ;
    while( (UDPHS_EPTCFG_EPT_MAPD & pEpt->UDPHS_EPTCFG) == 0 ) {

        /* resolved by clearing the reset IT in good place */
        TRACE_ERROR("PB bEndpoint: 0x%X\n\r", bEndpoint);
        TRACE_ERROR("PB bSizeEpt: 0x%X\n\r", bSizeEpt);
        TRACE_ERROR("PB bEndpointDir: 0x%X\n\r", bEndpointDir);
        TRACE_ERROR("PB bType: 0x%X\n\r", bType);
        TRACE_ERROR("PB pEndpoint->bank: 0x%X\n\r", pEndpoint->bank);
        TRACE_ERROR("PB UDPHS_EPTCFG: 0x%X\n\r", (unsigned int)pEpt->UDPHS_EPTCFG);
        for(;;);
    }

    if (bType == USBEndpointDescriptor_CONTROL)
    {
        pEpt->UDPHS_EPTCTLENB = UDPHS_EPTCTLENB_RXRDY_TXKL 
                              | UDPHS_EPTCTLENB_RX_SETUP
                              | UDPHS_EPTCTLENB_EPT_ENABL;
    }
    else
    {
#ifndef DMA
        pEpt->UDPHS_EPTCTLENB = UDPHS_EPTCTLENB_EPT_ENABL;
#else
        pEpt->UDPHS_EPTCTLENB = UDPHS_EPTCTLENB_AUTO_VALID | UDPHS_EPTCTLENB_EPT_ENABL;
#endif
    }

    //TRACE_DEBUG_WP("<%x,%x,%x> ", pEpt->UDPHS_EPTCFG, pEpt->UDPHS_EPTCTL, pEpt->UDPHS_EPTSTA);
    return bEndpoint;
}

/**
 * Set callback for a USB endpoint for transfer (read/write).
 *
 * \param bEP       Endpoint number.
 * \param fCallback Optional callback function to invoke when the transfer is
 *                  complete.
 * \param pCbData   Optional pointer to data to the callback function.
 * \return USBD_STATUS_SUCCESS or USBD_STATUS_LOCKED if endpoint is busy.
 */
uint8_t USBD_HAL_SetTransferCallback(uint8_t          bEP,
                                  TransferCallback fCallback,
                                  void             *pCbData)
{
    Endpoint *pEndpoint = &(endpoints[bEP]);
    TransferHeader *pTransfer = (TransferHeader*)&(pEndpoint->transfer);
    /* Check that the endpoint is not transferring */
    if (pEndpoint->state > UDPHS_ENDPOINT_IDLE) {
        return USBD_STATUS_LOCKED;
    }
    TRACE_DEBUG_WP("sXfrCb ");
    /* Setup the transfer callback and extension data */
    pTransfer->fCallback = (void*)fCallback;
    pTransfer->pArgument = pCbData;
    return USBD_STATUS_SUCCESS;
}

/**
 * Configure an endpoint to use multi-buffer-list transfer mode.
 * The buffers can be added by _Read/_Write function.
 * \param pMbList  Pointer to a multi-buffer list used, NULL to disable MBL.
 * \param mblSize  Multi-buffer list size (number of buffers can be queued)
 * \param startOffset When number of buffer achieve this offset transfer start
 */
uint8_t USBD_HAL_SetupMblTransfer( uint8_t bEndpoint,
                                   USBDTransferBuffer* pMbList,
                                   uint16_t mblSize,
                                   uint16_t startOffset)
{
    Endpoint *pEndpoint = &(endpoints[bEndpoint]);
    MblTransfer *pXfr = (MblTransfer*)&(pEndpoint->transfer);
    uint16_t i;
    /* Check that the endpoint is not transferring */
    if (pEndpoint->state > UDPHS_ENDPOINT_IDLE) {
        return USBD_STATUS_LOCKED;
    }
    TRACE_DEBUG_WP("sMblXfr ");
    /* Enable Multi-Buffer Transfer List */
    if (pMbList) {
        /* Reset list items */
        for (i = 0; i < mblSize; i --) {
            pMbList[i].pBuffer     = NULL;
            pMbList[i].size        = 0;
            pMbList[i].transferred = 0;
            pMbList[i].buffered    = 0;
            pMbList[i].remaining   = 0;
        }
        /* Setup transfer */
        pXfr->transType  = 1;
        pXfr->listState  = 0; /* OK */
        pXfr->listSize   = mblSize;
        pXfr->pMbl       = pMbList;
        pXfr->outCurr = pXfr->outLast = 0;
        pXfr->inCurr  = 0;
        pXfr->offsetSize = startOffset;
    }
    /* Disable Multi-Buffer Transfer */
    else {
        pXfr->transType  = 0;
        pXfr->pMbl       = NULL;
        pXfr->listSize   = 0;
        pXfr->offsetSize = 1;
    }
    return USBD_STATUS_SUCCESS;
}

/**
 * Sends data through a USB endpoint. Sets up the transfer descriptor,
 * writes one or two data payloads (depending on the number of FIFO bank
 * for the endpoint) and then starts the actual transfer. The operation is
 * complete when all the data has been sent.
 *
 * *If the size of the buffer is greater than the size of the endpoint
 *  (or twice the size if the endpoint has two FIFO banks), then the buffer
 *  must be kept allocated until the transfer is finished*. This means that
 *  it is not possible to declare it on the stack (i.e. as a local variable
 *  of a function which returns after starting a transfer).
 *
 * \param bEndpoint Endpoint number.
 * \param pData Pointer to a buffer with the data to send.
 * \param dLength Size of the data buffer.
 * \return USBD_STATUS_SUCCESS if the transfer has been started;
 *         otherwise, the corresponding error status code.
 */
uint8_t USBD_HAL_Write( uint8_t          bEndpoint,
                        const void       *pData,
                        uint32_t         dLength)
{
    if (endpoints[bEndpoint].transfer.transHdr.transType)
        return UDPHS_AddWr(bEndpoint, pData, dLength);
    else
        return UDPHS_Write(bEndpoint, pData, dLength);
}

/**
 * Special write function.
 * Sends data through a USB endpoint. Sets up the transfer descriptor,
 * writes header and one or two data payloads (depending on the number of
 * FIFO bank for the endpoint) and then starts the actual transfer. The
 * operation is complete when all the data has been sent.
 *
 * *If the size of the buffer is greater than the size of the endpoint
 *  (or twice the size if the endpoint has two FIFO banks), then the buffer
 *  must be kept allocated until the transfer is finished*. This means that
 *  it is not possible to declare it on the stack (i.e. as a local variable
 *  of a function which returns after starting a transfer).
 *
 * \param bEndpoint Endpoint number.
 * \param pData Pointer to a buffer with the data to send.
 * \param dLength Size of the data buffer.
 * \return USBD_STATUS_SUCCESS if the transfer has been started;
 *         otherwise, the corresponding error status code.
 */
uint8_t USBD_HAL_WrWithHdr(uint8_t bEndpoint,
                           const void * pHdr, uint8_t bHdrLen,
                           const void * pData,uint32_t dLength)
{
    Udphs    *pUdp  = UDPHS;
    UdphsEpt *pHwEp = &pUdp->UDPHS_EPT[bEndpoint];
    Endpoint *pEp  = &(endpoints[bEndpoint]);
    Transfer *pXfr = (Transfer*)&(pEp->transfer);
    /* Return if DMA is not supported */
    if (!CHIP_USB_ENDPOINTS_DMA(bEndpoint))
    {
       return USBD_STATUS_HW_NOT_SUPPORTED;
    }

#ifdef DMA
    /* Return if busy */
    if (pEp->state != UDPHS_ENDPOINT_IDLE)
    {
        return USBD_STATUS_LOCKED;
    }
    /* Sending state */
    pEp->state = UDPHS_ENDPOINT_SENDING;
    TRACE_DEBUG_WP("Wr%d(%d+%d) ", bEndpoint, bHdrLen, dLength);

    pEp->sendZLP = 0;

    /* Setup transfer descriptor */
    pXfr->pData       = (void*) pData;
    pXfr->remaining   = bHdrLen + dLength;
    pXfr->buffered    = 0;
    pXfr->transferred = 0;

    /* 1. DMA supported always, 2. Not ZLP */
    if (bHdrLen + dLength > 0)
    {
        uint8_t bNbTrans = (pHwEp->UDPHS_EPTCFG & UDPHS_EPTCFG_NB_TRANS_Msk)
                                                  >> UDPHS_EPTCFG_NB_TRANS_Pos;
        if (pXfr->remaining > DMA_MAX_FIFO_SIZE)
        {
            /* Transfer the max */
            pXfr->buffered = DMA_MAX_FIFO_SIZE;
        }
        else
        {
            /* Good size, total size */
            pXfr->buffered = pXfr->remaining;
        }

        /* LD1: header - load to fifo without interrupt */
        /* Header discarded if exceed the DMA FIFO length */
        //if (bHdrLen > DMA_MAX_FIFO_SIZE) bHdrLen = DMA_MAX_FIFO_SIZE;
        pDmaLL[0].pNxtDesc = (void*)&pDmaLL[1];
        pDmaLL[0].pAddr    = (void*)pHdr;
        pDmaLL[0].dwCtrl   = UDPHS_DMACONTROL_CHANN_ENB
                           | UDPHS_DMACONTROL_BUFF_LENGTH(bHdrLen)
                           | UDPHS_DMACONTROL_LDNXT_DSC;
        /* High bandwidth ISO EP, max size n*ep_size */
        if (bNbTrans > 1) {
            uint8_t* pU8 = (uint8_t*)pData;
            uint32_t maxSize = bNbTrans * pEp->size;
            dLength = pXfr->buffered - bHdrLen;
            if (dLength > maxSize) dLength = maxSize;
          #if 0 /* Prepare banks by 1 DMA descriptor -- NK if not standard EP size, works! */
            /* LD2: data   -  load to fifo with interrupt */
            pDmaLL[1].pNxtDesc = (void*)NULL;
            pDmaLL[1].pAddr    = (void*)pU8;
            pDmaLL[1].dwCtrl   = UDPHS_DMACONTROL_CHANN_ENB
                               | UDPHS_DMACONTROL_BUFF_LENGTH(dLength)
                               | UDPHS_DMACONTROL_END_B_EN
                               | UDPHS_DMACONTROL_END_BUFFIT;
          #else
            uint32_t pktLen, ndxData = 0;
            /* LD2: data   -  bank 0 */
            pktLen = pEp->size - bHdrLen;
            if (pktLen >= dLength) { /* It's the last DMA LLI */
                pDmaLL[1].pNxtDesc = (void*)NULL;
                pDmaLL[1].pAddr    = (void*)pU8;
                pDmaLL[1].dwCtrl   = UDPHS_DMACONTROL_CHANN_ENB
                                   | UDPHS_DMACONTROL_BUFF_LENGTH(dLength)
                                   | UDPHS_DMACONTROL_END_B_EN
                                   | UDPHS_DMACONTROL_END_BUFFIT;
            }
            else {
                pDmaLL[1].pNxtDesc = (void*)&pDmaLL[2];
                pDmaLL[1].pAddr    = (void*)pU8;
                pDmaLL[1].dwCtrl   = UDPHS_DMACONTROL_CHANN_ENB
                                   | UDPHS_DMACONTROL_BUFF_LENGTH(pktLen)
                                   | UDPHS_DMACONTROL_END_B_EN
                                   | UDPHS_DMACONTROL_LDNXT_DSC;
                dLength -= pktLen; ndxData += pktLen;
                /* LD3: data  - bank 1 */
                pktLen = pEp->size;
                if (pktLen >= dLength) { /* It's the last */
                    pDmaLL[1].pNxtDesc = (void*) NULL;
                    pDmaLL[1].pAddr    = (void*)&pU8[ndxData];
                    pDmaLL[1].dwCtrl   = UDPHS_DMACONTROL_CHANN_ENB
                                       | UDPHS_DMACONTROL_BUFF_LENGTH(dLength)
                                       | UDPHS_DMACONTROL_END_B_EN
                                       | UDPHS_DMACONTROL_END_BUFFIT;
                }
                else {
                    pDmaLL[2].pNxtDesc = (void*)&pDmaLL[3];
                    pDmaLL[2].pAddr    = (void*)&pU8[ndxData];
                    pDmaLL[2].dwCtrl   = UDPHS_DMACONTROL_CHANN_ENB
                                       | UDPHS_DMACONTROL_BUFF_LENGTH(pktLen)
                                       | UDPHS_DMACONTROL_END_B_EN
                                       | UDPHS_DMACONTROL_LDNXT_DSC;
                    dLength -= pktLen; ndxData += pktLen;
                    /* LD4: data  - bank 2 */
                    pDmaLL[3].pNxtDesc = (void*) NULL;
                    pDmaLL[3].pAddr    = (void*)&pU8[ndxData];
                    pDmaLL[3].dwCtrl   = UDPHS_DMACONTROL_CHANN_ENB
                                       | UDPHS_DMACONTROL_BUFF_LENGTH(dLength)
                                       | UDPHS_DMACONTROL_END_B_EN
                                       | UDPHS_DMACONTROL_END_BUFFIT;
                }
            }
          #endif
        }
        else { /* Normal, fill all data */
            /* LD2: data   -  load to fifo with interrupt */
            dLength = pXfr->buffered - bHdrLen;
            pDmaLL[1].pNxtDesc = (void*)NULL;
            pDmaLL[1].pAddr    = (void*)pData;
            pDmaLL[1].dwCtrl   = UDPHS_DMACONTROL_CHANN_ENB
                               | UDPHS_DMACONTROL_BUFF_LENGTH(dLength)
                               | UDPHS_DMACONTROL_END_B_EN
                               | UDPHS_DMACONTROL_END_BUFFIT;
        }
        /* Interrupt enable */
        pUdp->UDPHS_IEN |= (1 << SHIFT_DMA << bEndpoint);
        /* Start transfer with LLI */
        pUdp->UDPHS_DMA[bEndpoint].UDPHS_DMANXTDSC  = (uint32_t)pDmaLL;
        pUdp->UDPHS_DMA[bEndpoint].UDPHS_DMACONTROL = 0;
        pUdp->UDPHS_DMA[bEndpoint].UDPHS_DMACONTROL = UDPHS_DMACONTROL_LDNXT_DSC;
        return USBD_STATUS_SUCCESS;
    }
#endif
    
    /* Enable IT */
    pUdp->UDPHS_IEN |= ( UDPHS_IEN_EPT_0 << bEndpoint );
    pHwEp->UDPHS_EPTCTLENB = UDPHS_EPTCTLENB_TXRDY;
    return USBD_STATUS_SUCCESS;
}

/**
 * Reads incoming data on an USB endpoint This methods sets the transfer
 * descriptor and activate the endpoint interrupt. The actual transfer is
 * then carried out by the endpoint interrupt handler. The Read operation
 * finishes either when the buffer is full, or a short packet (inferior to
 * endpoint maximum  size) is received.
 *
 * *The buffer must be kept allocated until the transfer is finished*.
 * \param bEndpoint Endpoint number.
 * \param pData Pointer to a data buffer.
 * \param dLength Size of the data buffer in bytes.
 * \return USBD_STATUS_SUCCESS if the read operation has been started;
 *         otherwise, the corresponding error code.
 */
uint8_t USBD_HAL_Read(uint8_t    bEndpoint,
                      void       *pData,
                      uint32_t   dLength)
{
    if (endpoints[bEndpoint].transfer.transHdr.transType)
        return USBD_STATUS_SW_NOT_SUPPORTED;
    else
        return UDPHS_Read(bEndpoint, pData, dLength);
}

/**
 *  \brief Enable Pull-up, connect.
 *
 *  -# Enable HW access if needed
 *  -# Enable Pull-Up
 *  -# Disable HW access if needed
 */
void USBD_HAL_Connect(void)
{
    Udphs *pUdp = UDPHS;

    uint8_t dis = UDPHS_EnablePeripheralClock();
    pUdp->UDPHS_CTRL |= UDPHS_CTRL_PULLD_DIS;
    pUdp->UDPHS_CTRL &= ~(uint32_t)UDPHS_CTRL_DETACH;
    if (dis) UDPHS_DisablePeripheralClock();
}

/**
 *  \brief Disable Pull-up, disconnect.
 *
 *  -# Enable HW access if needed
 *  -# Disable PULL-Up
 *  -# Disable HW access if needed
 */
void USBD_HAL_Disconnect(void)
{
    Udphs *pUdp = UDPHS;

    uint8_t dis = UDPHS_EnablePeripheralClock();
    pUdp->UDPHS_CTRL |= UDPHS_CTRL_DETACH;
    pUdp->UDPHS_CTRL &= ~(uint32_t)UDPHS_CTRL_PULLD_DIS;
    if (dis) UDPHS_DisablePeripheralClock();
}

/**
 * Starts a remote wake-up procedure.
 */
void USBD_HAL_RemoteWakeUp(void)
{
    Udphs *pUdp = UDPHS;

    UDPHS_EnablePeripheralClock();
    UDPHS_EnableUsbClock();

    TRACE_INFO_WP("RWUp ");

    /* Activates a remote wakeup (edge on ESR), then clear ESR */
    pUdp->UDPHS_CTRL |= UDPHS_CTRL_REWAKEUP;
    while(pUdp->UDPHS_CTRL & UDPHS_CTRL_REWAKEUP)
    {
        TRACE_DEBUG_WP("w");
    }
    UDPHS_EnableBIAS();
}

/**
 * Sets the device address to the given value.
 * \param address New device address.
 */
void USBD_HAL_SetAddress(uint8_t address)
{
    Udphs *pUdp = UDPHS;

    if (address)
    {
        pUdp->UDPHS_CTRL &= ~(uint32_t)UDPHS_CTRL_DEV_ADDR_Msk;
        pUdp->UDPHS_CTRL |= address | UDPHS_CTRL_FADDR_EN;
    }
    else
    {
        pUdp->UDPHS_CTRL &= ~(uint32_t)UDPHS_CTRL_FADDR_EN;
    }
}

/**
 * Sets the current device configuration.
 * \param cfgnum - Configuration number to set.
 */
void USBD_HAL_SetConfiguration(uint8_t cfgnum)
{
    /* Nothing to do now */
    cfgnum = cfgnum;
}

/**
 * Initializes the USB HW Access driver.
 */
void USBD_HAL_Init(void)
{
    Udphs *pUdp = UDPHS;
    UdphsEpt *pEpt;
    UdphsDma *pDma;
    uint32_t i;
#ifdef DMA
    /* DMA Link list should be 16-bytes aligned */
    if ((uint32_t)dmaLL & 0xFFFFFFF0)
        pDmaLL = (UdphsDmaDescriptor*)((uint32_t)&dmaLL[1] & 0xFFFFFFF0);
    else
        pDmaLL = (UdphsDmaDescriptor*)((uint32_t)&dmaLL[0]);
#endif
    /* Must before USB & TXVC access! */
    UDPHS_EnablePeripheralClock();

    /* Reset & disable endpoints */
    USBD_HAL_ResetEPs(0xFFFFFFFF, USBD_STATUS_RESET, 0);

    /* Configure the pull-up on D+ and disconnect it */
    pUdp->UDPHS_CTRL |= UDPHS_CTRL_DETACH;
    pUdp->UDPHS_CTRL |= UDPHS_CTRL_PULLD_DIS;

    /* Reset IP */
    pUdp->UDPHS_CTRL &= ~(uint32_t)UDPHS_CTRL_EN_UDPHS;
    pUdp->UDPHS_CTRL |= UDPHS_CTRL_EN_UDPHS;

    /* (XCHQ[2010.1.21], IP recomendation, setup clock after reset IP) */
    UDPHS_EnableUsbClock();

    /* Initialize DMA */
    for (i = 1; i < 7; i ++)
    {
        pEpt = &pUdp->UDPHS_EPT[i];
        pDma = &pUdp->UDPHS_DMA[i];
        /* DMA stop */
        pDma->UDPHS_DMACONTROL = 0;
        /* Disable endpoint */
        pEpt->UDPHS_EPTCTLDIS = (uint32_t)UDPHS_EPTCTLDIS_SHRT_PCKT
                              | UDPHS_EPTCTLDIS_BUSY_BANK
                              | UDPHS_EPTCTLDIS_NAK_OUT
                              | UDPHS_EPTCTLDIS_NAK_IN
                              | UDPHS_EPTCTLDIS_STALL_SNT
                              | UDPHS_EPTCTLDIS_RX_SETUP
                              | UDPHS_EPTCTLDIS_TXRDY
                              | UDPHS_EPTCTLDIS_TX_COMPLT
                              | UDPHS_EPTCTLDIS_RXRDY_TXKL
                              | UDPHS_EPTCTLDIS_ERR_OVFLW
                              | UDPHS_EPTCTLDIS_MDATA_RX
                              | UDPHS_EPTCTLDIS_DATAX_RX
                              | UDPHS_EPTCTLDIS_NYET_DIS
                              | UDPHS_EPTCTLDIS_INTDIS_DMA
                              | UDPHS_EPTCTLDIS_AUTO_VALID
                              | UDPHS_EPTCTLDIS_EPT_DISABL
                              ;
        /* Clear status endpoint */
        pEpt->UDPHS_EPTCLRSTA = UDPHS_EPTCLRSTA_TOGGLESQ
                              | UDPHS_EPTCLRSTA_FRCESTALL
                              | UDPHS_EPTCLRSTA_RXRDY_TXKL
                              | UDPHS_EPTCLRSTA_TX_COMPLT
                              | UDPHS_EPTCLRSTA_RX_SETUP
                              | UDPHS_EPTCLRSTA_STALL_SNT
                              | UDPHS_EPTCLRSTA_NAK_IN
                              | UDPHS_EPTCLRSTA_NAK_OUT
                              ;
        /* Reset endpoint config */
        pEpt->UDPHS_EPTCTLENB = 0;
        /* Reset DMA channel (Buffer count and Control field) */
        pDma->UDPHS_DMACONTROL = UDPHS_DMACONTROL_LDNXT_DSC;
        /* Reset DMA channel */
        pDma->UDPHS_DMACONTROL = 0;
        /* Clear DMA channel status (read to clear) */
        pDma->UDPHS_DMASTATUS = pDma->UDPHS_DMASTATUS;
    }

    /* Force Full-Speed */
    pUdp->UDPHS_TST = forceUsbFS ? UDPHS_TST_SPEED_CFG_FULL_SPEED : 0;

    pUdp->UDPHS_IEN = 0;
    pUdp->UDPHS_CLRINT = UDPHS_CLRINT_UPSTR_RES
                       | UDPHS_CLRINT_ENDOFRSM
                       | UDPHS_CLRINT_WAKE_UP
                       | UDPHS_CLRINT_ENDRESET
                       | UDPHS_CLRINT_INT_SOF
                       | UDPHS_CLRINT_MICRO_SOF
                       | UDPHS_CLRINT_DET_SUSPD
                       ;

    /* Enable interrupts */
    pUdp->UDPHS_IEN = UDPHS_IEN_ENDOFRSM
                    | UDPHS_IEN_WAKE_UP
                    | UDPHS_IEN_DET_SUSPD;

    /* Disable USB clocks */
    UDPHS_DisableUsbClock();
}

/**
 * Causes the given endpoint to acknowledge the next packet it receives
 * with a STALL handshake except setup request.
 * \param bEP Endpoint number.
 * \return USBD_STATUS_SUCCESS or USBD_STATUS_LOCKED.
 */
uint8_t USBD_HAL_Stall(uint8_t bEP)
{
    Udphs    *pUdp = UDPHS;
    UdphsEpt *pEpt = &pUdp->UDPHS_EPT[bEP];

    Endpoint *pEndpoint = &(endpoints[bEP]);

    /* Check that endpoint is in Idle state */
    if (pEndpoint->state != UDPHS_ENDPOINT_IDLE)
    {
        TRACE_WARNING("UDP_Stall: EP%d locked\n\r", bEP);
        return USBD_STATUS_LOCKED;
    }
    /* STALL endpoint */
    pEpt->UDPHS_EPTSETSTA = UDPHS_EPTSETSTA_FRCESTALL;

    TRACE_DEBUG_WP("Stall%d ", bEP);
    return USBD_STATUS_SUCCESS;
}

/**
 * Sets/Clear/Get the HALT state on the endpoint.
 * In HALT state, the endpoint should keep stalling any packet.
 * \param bEndpoint Endpoint number.
 * \param ctl       Control code CLR/HALT/READ.
 *                  0: Clear HALT state;
 *                  1: Set HALT state;
 *                  .: Return HALT status.
 * \return USBD_STATUS_INVALID_PARAMETER if endpoint not exist,
 *         otherwise endpoint halt status.
 */
uint8_t USBD_HAL_Halt(uint8_t bEndpoint, uint8_t ctl)
{
    Udphs    *pUdp = UDPHS;
    UdphsEpt *pEpt = &pUdp->UDPHS_EPT[bEndpoint];

    Endpoint *pEndpoint = &(endpoints[bEndpoint]);
    uint8_t status = 0;

    /* SET Halt */
    if (ctl == 1)
    {
        /* Check that endpoint is enabled and not already in Halt state */
        if ((pEndpoint->state != UDPHS_ENDPOINT_DISABLED)
            && (pEndpoint->state != UDPHS_ENDPOINT_HALTED))
        {

            TRACE_DEBUG_WP("Halt%d ", bEndpoint);

            /* Abort the current transfer if necessary */
            UDPHS_EndOfTransfer(bEndpoint, USBD_STATUS_ABORTED);

            /* Put endpoint into Halt state */
            pEndpoint->state = UDPHS_ENDPOINT_HALTED;
            pEpt->UDPHS_EPTSETSTA = UDPHS_EPTSETSTA_FRCESTALL;

          #ifdef DMA
            if (CHIP_USB_ENDPOINTS_DMA(bEndpoint))
            {
                /* Enable the endpoint DMA interrupt */
                pUdp->UDPHS_IEN |= ( 1 << SHIFT_DMA << bEndpoint );
            }
            else
            {
                /* Enable the endpoint interrupt */
                pUdp->UDPHS_IEN |= ( UDPHS_IEN_EPT_0 << bEndpoint );
            }
          #else
            /* Enable the endpoint interrupt */
            pUdp->UDPHS_IEN |= ( UDPHS_IEN_EPT_0 << bEndpoint );
          #endif
        }
    }
    /* CLEAR Halt */
    else if (ctl == 0)
    {
        /* Check if the endpoint is halted */
        if (pEndpoint->state == UDPHS_ENDPOINT_HALTED)
        {

            TRACE_DEBUG_WP("Unhalt%d ", bEndpoint);

            /* Return endpoint to Idle state */
            pEndpoint->state = UDPHS_ENDPOINT_IDLE;

            /* Clear FORCESTALL flag */
            pEpt->UDPHS_EPTCLRSTA = UDPHS_EPTCLRSTA_TOGGLESQ
                                  | UDPHS_EPTCLRSTA_FRCESTALL;

            /* Reset Endpoint Fifos */
            pUdp->UDPHS_EPTRST = (1 << bEndpoint);
        }
    }

    /* Return Halt status */
    if (pEndpoint->state == UDPHS_ENDPOINT_HALTED)
    {
        status = 1;
    }
    return( status );
}

/**
 * Indicates if the device is running in high or full-speed. Always returns 0
 * since UDP does not support high-speed mode.
 */
uint8_t USBD_HAL_IsHighSpeed(void)
{
    Udphs    *pUdp = UDPHS;
    return (pUdp->UDPHS_INTSTA & UDPHS_INTSTA_SPEED);
}

/**
 * Suspend USB Device HW Interface
 *
 * -# Disable transceiver
 * -# Disable USB Clock
 * -# Disable USB Peripheral
 */
void USBD_HAL_Suspend(void)
{
    /* The device enters the Suspended state */
    UDPHS_DisableBIAS();
    UDPHS_DisableUsbClock();
    UDPHS_DisablePeripheralClock();
}

/**
 * Activate USB Device HW Interface
 * -# Enable USB Peripheral
 * -# Enable USB Clock
 * -# Enable transceiver
 */
void USBD_HAL_Activate(void)
{
    UDPHS_EnablePeripheralClock();
    UDPHS_EnableUsbClock();
    UDPHS_EnableBIAS();
}

/**
 * Certification test for High Speed device.
 * \param bIndex Test to be done
 */
void USBD_HAL_Test( uint8_t bIndex )
{
    Udphs *pUdp = UDPHS;
    uint8_t      *pFifo;
    uint32_t      i;

    /* remove suspend for TEST */
    pUdp->UDPHS_IEN &= ~UDPHS_IEN_DET_SUSPD;
    /* force High Speed (remove suspend) */
    pUdp->UDPHS_TST |= UDPHS_TST_SPEED_CFG_HIGH_SPEED;

    switch( bIndex ) {

        case USBFeatureRequest_TESTPACKET:
            TRACE_DEBUG_WP("TEST_PACKET ");

            pUdp->UDPHS_DMA[1].UDPHS_DMACONTROL = 0;
            pUdp->UDPHS_DMA[2].UDPHS_DMACONTROL = 0;

            /* Configure endpoint 2, 64 bytes, direction IN, type BULK, 1 bank */
            pUdp->UDPHS_EPT[2].UDPHS_EPTCFG = UDPHS_EPTCFG_EPT_SIZE_64
                                            | UDPHS_EPTCFG_EPT_DIR
                                            | UDPHS_EPTCFG_EPT_TYPE_BULK
                                            | UDPHS_EPTCFG_BK_NUMBER_1;
            while( (pUdp->UDPHS_EPT[2].UDPHS_EPTCFG & UDPHS_EPTCFG_EPT_MAPD) != UDPHS_EPTCFG_EPT_MAPD );
            pUdp->UDPHS_EPT[2].UDPHS_EPTCTLENB = UDPHS_EPTCTLENB_EPT_ENABL;

            /* Write FIFO */
            pFifo = (uint8_t*)((uint32_t *)(UDPHS_RAM_ADDR) + (EPT_VIRTUAL_SIZE * 2));
            for( i=0; i<sizeof(test_packet_buffer); i++) {
                pFifo[i] = test_packet_buffer[i];
            }
            /* Tst PACKET */
            pUdp->UDPHS_TST |= UDPHS_TST_TST_PKT;
            /* Send packet */
            pUdp->UDPHS_EPT[2].UDPHS_EPTSETSTA = UDPHS_EPTSETSTA_TXRDY;
            break;

        case USBFeatureRequest_TESTJ:
            TRACE_DEBUG_WP("TEST_J ");
            pUdp->UDPHS_TST = UDPHS_TST_TST_J;
            break;

        case USBFeatureRequest_TESTK:
            TRACE_DEBUG_WP("TEST_K ");
            pUdp->UDPHS_TST = UDPHS_TST_TST_K;
            break;

        case USBFeatureRequest_TESTSE0NAK:
            TRACE_DEBUG_WP("TEST_SEO_NAK ");
            pUdp->UDPHS_IEN = 0;  // for test
            break;

        case USBFeatureRequest_TESTSENDZLP:
            //while( 0 != (pUdp->UDPHS_EPT[0].UDPHS_EPTSTA & UDPHS_EPTSETSTA_TXRDY ) ) {}
            pUdp->UDPHS_EPT[0].UDPHS_EPTSETSTA = UDPHS_EPTSETSTA_TXRDY;
            //while( 0 != (pUdp->UDPHS_EPT[0].UDPHS_EPTSTA & UDPHS_EPTSETSTA_TXRDY ) ) {}
            TRACE_DEBUG_WP("SEND_ZLP ");
            break;
    }
    TRACE_DEBUG_WP("\n\r");
}

extern void USBD_HAL_ForceFs(void)
{
   forceUsbFS = 1;
}

/**@}*/