USBD_HAL.c

Go to the documentation of this file.

/* ---------------------------------------------------------------------------- */
/*                  Atmel Microcontroller Software Support                      */
/*                       SAM Software Package License                           */
/* ---------------------------------------------------------------------------- */
/* Copyright (c) 2015, Atmel Corporation                                        */
/*                                                                              */
/* All rights reserved.                                                         */
/*                                                                              */
/* Redistribution and use in source and binary forms, with or without           */
/* modification, are permitted provided that the following condition is 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.                           */
/* ---------------------------------------------------------------------------- */

/*---------------------------------------------------------------------------
 *      Headers
 *---------------------------------------------------------------------------*/

#include "../../libchip/libchip_compiler.h"
#include "../../libchip/include/usbhs.h"
#include "USBD_HAL.h"
#include "USBLib_Trace.h"

#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>

/*---------------------------------------------------------------------------
 *      Definitions
 *---------------------------------------------------------------------------*/

#define DMA


#define SHIFT_INTERUPT   12

#define UDPHS_ENDPOINT_DISABLED       0

#define UDPHS_ENDPOINT_HALTED         1

#define UDPHS_ENDPOINT_IDLE           2

#define UDPHS_ENDPOINT_SENDING        3

#define UDPHS_ENDPOINT_RECEIVING      4

#define UDPHS_ENDPOINT_SENDINGM       5

#define UDPHS_ENDPOINT_RECEIVINGM     6


#define MBL_NbBuffer(i, o, size) (((i)>(o))?((i)-(o)):((i)+(size)-(o)))

#define MBL_FULL        1

#define MBL_NULL        2

/*---------------------------------------------------------------------------
 *      Types
 *---------------------------------------------------------------------------*/

typedef struct {
        void *fCallback;
        void *pArgument;
        uint8_t transType;
        /* Reserved to 32-b aligned */
        uint8_t reserved[3];
} TransferHeader;

typedef struct {

        TransferCallback fCallback;
        void *pArgument;
        uint8_t transType;
        uint8_t reserved[3];
        int32_t buffered;
        uint8_t *pData;
        int32_t transferred;
        int32_t remaining;
} Transfer;

typedef struct {
        MblTransferCallback fCallback;
        void *pArgument;
        uint8_t transType;
        uint8_t listState;
        uint16_t listSize;
        USBDTransferBuffer *pMbl;
        uint16_t offsetSize;
        uint16_t outCurr;
        uint16_t outLast;
        uint16_t inCurr;
} MblTransfer;

typedef struct {
        /* CSR */
        volatile uint8_t state;
        volatile uint8_t bank;
        volatile uint16_t size;
        union {
                TransferHeader transHdr;
                Transfer singleTransfer;
                MblTransfer mblTransfer;
        } transfer;
        uint32_t sendZLP;
} Endpoint;

typedef struct {
        void *pNxtDesc;
        void *pAddr;
        uint32_t dwCtrl;
        uint32_t dw;
} UdphsDmaDescriptor;

/*---------------------------------------------------------------------------
 *      Internal variables
 *---------------------------------------------------------------------------*/

static Endpoint endpoints[CHIP_USB_NUMENDPOINTS];

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
};

COMPILER_ALIGNED(16) static UdphsDmaDescriptor  dmaLL[5];
COMPILER_ALIGNED(16) static UdphsDmaDescriptor *pDmaLL;

/*---------------------------------------------------------------------------
 *      Internal Functions
 *---------------------------------------------------------------------------*/

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 ");
                }
        }
}

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;
}

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 *)USBHS_RAM_ADDR + (EPT_VIRTUAL_SIZE *
                                                bEndpoint));
        memory_sync();

        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;
}

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 *)USBHS_RAM_ADDR + (EPT_VIRTUAL_SIZE *
                                                bEndpoint));
        memory_sync();

        for (; size; size --)
                *(pFifo ++) = *(pTransfer->pData ++);

        memory_sync();

}

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 *)USBHS_RAM_ADDR
                                                + (EPT_VIRTUAL_SIZE * bEndpoint));

        while (wPacketSize > 0) {
                *(pTransfer->pData++) = *(pFifo++);
                memory_sync();
                wPacketSize--;
        }
}

static void UDPHS_ReadRequest(USBGenericRequest *pRequest)
{
        uint32_t *pData = (uint32_t *)(void *)pRequest;
        volatile uint32_t *pFifo;

        pFifo = (volatile uint32_t *)USBHS_RAM_ADDR;
        *pData++ = *pFifo;
        memory_sync();
        pFifo = (volatile uint32_t *)USBHS_RAM_ADDR;
        *pData = *pFifo;
        memory_sync();
}


static void UDPHS_EndpointHandler(uint8_t bEndpoint)
{
        Usbhs *pUdp = USBHS;
        Endpoint *pEp = &(endpoints[bEndpoint]);
        Transfer *pXfr = (Transfer *) & (pEp->transfer);

        uint32_t status = USBHS_ReadEPStatus(pUdp, bEndpoint, 0xFFFFFFFF);
        uint32_t type = USBHS_GetEpType(pUdp, bEndpoint);
        uint32_t reqBuf[2];
        USBGenericRequest *pReq = (USBGenericRequest *)reqBuf;
        uint16_t wPktSize;

        TRACE_DEBUG_WP("Ep%d ", bEndpoint);

        /* IN packet sent */
        /* When the bank is empty, USBHS_DEVEPTISRx.TXINI and USBHS_DEVEPTIMRx.FIFOCON
        are set, which triggers a PEP_x interrupt if USBHS_DEVEPTIMRx.TXINE = 1.*/
        if ((status & USBHS_DEVEPTISR_TXINI)
                && USBHS_IsEpIntEnable(pUdp, bEndpoint, USBHS_DEVEPTIMR_TXINE)) {
                TRACE_DEBUG_WP("Wr ");

                /* Multi-buffer-list transfer state */
                if (pEp->state == UDPHS_ENDPOINT_SENDINGM) {
                } else if (pEp->state == UDPHS_ENDPOINT_SENDING) {
                        /* Sending state */
                        if (pXfr->buffered) {
                                pXfr->transferred += pXfr->buffered;
                                pXfr->buffered = 0;
                        }

                        /* Force ZLP transmission in total length is a
                            multiple of endpoint size */
                        if (((pXfr->transferred % pEp->size) == 0)
                                && (pXfr->remaining == 0)
                                && (pXfr->transferred > 0)
                                && (pEp->sendZLP == 0))
                                pEp->sendZLP = 1;


                        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);

                                /* The user writes the data into the current bank by using
                                the USB Pipe/Endpoint nFIFO Data */
                                UDPHS_WritePayload(bEndpoint, pEp->size);

                                /*Acknowledges the interrupt by clearing TXINIC.*/
                                USBHS_AckEpInterrupt(USBHS, bEndpoint, USBHS_DEVEPTICR_TXINIC);

                                if (type != USBHS_DEVEPTCFG_EPTYPE_CTRL >> USBHS_DEVEPTCFG_EPTYPE_Pos)
                                        /* Send the bank and switch to the next bank (if any) by clearing FIFOCON. */
                                        USBHS_DisableEPIntType(pUdp, bEndpoint, USBHS_DEVEPTIDR_FIFOCONC);
                        } else {
                                /* Transfer done */
                                TRACE_DEBUG_WP("l%d ", pXfr->transferred);

                                /* Disable interrupt on non-control EP */
                                if (type != USBHS_DEVEPTCFG_EPTYPE_CTRL >> USBHS_DEVEPTCFG_EPTYPE_Pos)
                                        USBHS_DisableIntEP(pUdp, bEndpoint);

                                USBHS_DisableEPIntType(pUdp, bEndpoint, USBHS_DEVEPTIDR_TXINEC);
                                UDPHS_EndOfTransfer(bEndpoint, USBD_STATUS_SUCCESS);
                                pEp->sendZLP = 0;
                        }
                } else {
                        /* Clear interrupt when the state of endpoint0 is not SENDING. */
                        if (bEndpoint == 0)
                                USBHS_AckEpInterrupt(USBHS, bEndpoint, USBHS_DEVEPTICR_TXINIC);

                        TRACE_DEBUG("Err Wr %d\n\r", pEp->sendZLP);
                }
        }

        /* OUT packet received */
        if (USBHS_DEVEPTISR_RXOUTI & status) {
                TRACE_DEBUG_WP("Rd ");

                /* NOT in receiving state */
                if (pEp->state != UDPHS_ENDPOINT_RECEIVING) {
                        /* Check if ACK received on a Control EP */
                        if (((USBHS_DEVEPTCFG_EPTYPE_CTRL >> USBHS_DEVEPTCFG_EPTYPE_Pos) == type)
                                && (0 == (status & USBHS_DEVEPTISR_BYCT_Msk))) {
                                TRACE_INFO_WP("Ack ");
                                USBHS_AckEpInterrupt(pUdp, bEndpoint, USBHS_DEVEPTICR_RXOUTIC);
                                UDPHS_EndOfTransfer(bEndpoint, USBD_STATUS_SUCCESS);
                        }
                        /* data has been STALLed */
                        else if (USBHS_DEVEPTISR_STALLEDI & status) {
                                TRACE_INFO_WP("Discard ");
                                USBHS_AckEpInterrupt(pUdp, bEndpoint, USBHS_DEVEPTICR_RXOUTIC);
                        }
                        /* NAK the data */
                        else {
                                TRACE_INFO_WP("Nak ");
                                USBHS_AckEpInterrupt(pUdp, bEndpoint, USBHS_DEVEPTICR_RXOUTIC);
                        }
                }
                /* In read state */
                else {
                        TRACE_DEBUG_WP("%d ", wPktSize);

                        /*Acknowledge Received OUT Data Interrupt*/
                        USBHS_AckEpInterrupt(pUdp, bEndpoint, USBHS_DEVEPTICR_RXOUTIC);

                        wPktSize = USBHS_ByteCount(pUdp, bEndpoint);
                        UDPHS_ReadPayload(bEndpoint, wPktSize);

                        /*Free the current bank and to switch to the next bank (If any).*/
                        if (type != USBHS_DEVEPTCFG_EPTYPE_CTRL >> USBHS_DEVEPTCFG_EPTYPE_Pos)
                                USBHS_DisableEPIntType(pUdp, bEndpoint, USBHS_DEVEPTIDR_FIFOCONC);

                        /* Check if transfer is finished */
                        if (pXfr->remaining == 0 || wPktSize < pEp->size) {
                                USBHS_DisableEPIntType(pUdp, bEndpoint, USBHS_DEVEPTIDR_RXOUTEC);

                                /* Disable interrupt if not control EP */
                                if ((USBHS_DEVEPTCFG_EPTYPE_CTRL >> USBHS_DEVEPTCFG_EPTYPE_Pos) != type)
                                        USBHS_DisableIntEP(pUdp, bEndpoint);

                                UDPHS_EndOfTransfer(bEndpoint, USBD_STATUS_SUCCESS);
                                USBHS_AckEpInterrupt(pUdp, bEndpoint, USBHS_DEVEPTICR_NAKINIC);
                                USBHS_AckEpInterrupt(pUdp, bEndpoint, USBHS_DEVEPTICR_TXINIC);
                        }
                }
        }

        /* STALL sent */
        if (USBHS_DEVEPTISR_STALLEDI & status) {
                /* Acknowledge */
                USBHS_AckEpInterrupt(pUdp, bEndpoint, USBHS_DEVEPTICR_STALLEDIC);

                /* ISO error */
                if (type == (USBHS_DEVEPTCFG_EPTYPE_ISO >> USBHS_DEVEPTCFG_EPTYPE_Pos)) {
                        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)
                                USBHS_DisableEPIntType(pUdp, bEndpoint, USBHS_DEVEPTIDR_STALLRQC);
                }
        }

        /* SETUP packet received */
        if (USBHS_DEVEPTISR_RXSTPI & 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 == (USBHS_DEVEPTCFG_EPTYPE_ISO >> USBHS_DEVEPTCFG_EPTYPE_Pos)) {
                        TRACE_WARNING("IsoFE[%d]\n\r", bEndpoint);
                        /* Acknowledge setup packet */
                        USBHS_AckEpInterrupt(pUdp, bEndpoint, USBHS_DEVEPTICR_RXSTPIC);
                } else {
                        TRACE_DEBUG_WP("Stup ");
                        /* Copy setup */
                        UDPHS_ReadRequest(pReq);
                        /* Acknowledge setup packet */
                        USBHS_AckEpInterrupt(pUdp, bEndpoint, USBHS_DEVEPTICR_RXSTPIC);

                        /* Handler */
                        USBD_RequestHandler(bEndpoint, pReq);
                }
        }
}
#ifdef DMA

static inline void UDPHS_DmaSingle(uint8_t bEndpoint, Transfer *pXfr,
                                                                   uint32_t dwCfg)
{
        Usbhs *pUdp = USBHS;
        UsbhsDevdma *pUsbDma = &pUdp->USBHS_DEVDMA[bEndpoint];

        /* Single transfer */
        USBHS_SetDmaBuffAdd(pUsbDma, (uint32_t)&pXfr->pData[pXfr->transferred]);
        USBHS_GetDmaStatus(pUsbDma);

        TRACE_DEBUG_WP("Dma[B%d:T%d] ", pXfr->buffered, pXfr->transferred);
        /* DMA Configure */
        USBHS_ConfigureDma(pUsbDma, 0);
        USBHS_ConfigureDma(pUsbDma,
                                           (USBHS_DEVDMACONTROL_BUFF_LENGTH(pXfr->buffered) | dwCfg));

        /* Interrupt enable */
        USBHS_EnableDMAIntEP(pUdp, bEndpoint);
}
static void UDPHS_DmaHandler(uint8_t bEndpoint)
{
        Usbhs *pUdp = USBHS;
        uint8_t bDmaEndpoint = bEndpoint - 1;

        Endpoint *pEp = &(endpoints[bEndpoint]);
        Transfer *pXfr = (Transfer *)&(pEp->transfer);

        uint32_t dwDmaSr;
        int32_t iRemain, iXfred;
        uint8_t iRead = 0;
        uint8_t bRc = USBD_STATUS_SUCCESS;
        UsbhsDevdma *pUsbDma = &pUdp->USBHS_DEVDMA[bDmaEndpoint];

        dwDmaSr = USBHS_GetDmaStatus(pUsbDma);
        TRACE_DEBUG_WP("iDma%d,%x ", bDmaEndpoint, dwDmaSr);

        /* Mbl transfer */
        if (pEp->state == UDPHS_ENDPOINT_SENDINGM)
                /* Not implemented */
                return;
        else if (pEp->state == UDPHS_ENDPOINT_RECEIVINGM)
                /* Not implemented */
                return;

        if ((pUdp->USBHS_DEVDMA[bDmaEndpoint].USBHS_DEVDMACONTROL
                 & USBHS_DEVDMACONTROL_END_TR_EN) == USBHS_DEVDMACONTROL_END_TR_EN)
                iRead = 1;

        /* Disable DMA interrupt to avoid receiving 2 (B_EN and TR_EN) */
        pUdp->USBHS_DEVDMA[bDmaEndpoint].USBHS_DEVDMACONTROL &= ~
                        (USBHS_DEVDMACONTROL_END_TR_EN
                         | USBHS_DEVDMACONTROL_END_B_EN);

        if (USBHS_DEVDMASTATUS_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 & USBHS_DEVDMASTATUS_BUFF_COUNT_Msk)
                                  >> USBHS_DEVDMASTATUS_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(bDmaEndpoint, pXfr, USBHS_DEVDMACONTROL_END_TR_EN
                                                        | USBHS_DEVDMACONTROL_END_TR_IT
                                                        | USBHS_DEVDMACONTROL_END_B_EN
                                                        | USBHS_DEVDMACONTROL_END_BUFFIT
                                                        | USBHS_DEVDMACONTROL_CHANN_ENB);
                }
        } else if (USBHS_DEVDMASTATUS_END_TR_ST & dwDmaSr) {
                TRACE_DEBUG_WP("EoDmaT ");
                pXfr->transferred = pXfr->buffered -
                                                        ((dwDmaSr & USBHS_DEVDMASTATUS_BUFF_COUNT_Msk)
                                                         >> USBHS_DEVDMASTATUS_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) {
                if (iRead)
                        SCB_InvalidateDCache_by_Addr((uint32_t *)pXfr->pData, pXfr->transferred);

                UDPHS_EndOfTransfer(bEndpoint, bRc);
        }
}
#endif

static inline uint8_t UDPHS_Write(uint8_t bEndpoint,
                                                                  const void *pData,
                                                                  uint32_t dLength)
{
        Usbhs *pUdp = USBHS;
        uint8_t bDmaEndpoint = bEndpoint - 1;

        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) {
                /* Enable automatic bank switch for DMA*/
                USBHS_AutoSwitchBankEnable(pUdp, bEndpoint, true);

                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 */
                SCB_CleanDCache_by_Addr((uint32_t *) pData, dLength);
                UDPHS_DmaSingle(bDmaEndpoint, pXfr, USBHS_DEVDMACONTROL_END_B_EN
                                                | USBHS_DEVDMACONTROL_END_BUFFIT
                                                | USBHS_DEVDMACONTROL_CHANN_ENB);
                return USBD_STATUS_SUCCESS;
        }

        /* Wait for the bank to be free in order to transfer data without DMA correctly
         * when the last is done with DMA */
        if (bEndpoint)
                while (USBHS_IsBankFree(pUdp, bEndpoint) == false);

#endif
        /* Disable automatic bank switch*/
        USBHS_AutoSwitchBankEnable(pUdp, bEndpoint, false);
        /* Enable IT */
        USBHS_EnableIntEP(pUdp, bEndpoint);
        USBHS_EnableEPIntType(pUdp, bEndpoint, USBHS_DEVEPTIER_TXINES);
        return USBD_STATUS_SUCCESS;
}

static inline uint8_t UDPHS_AddWr(uint8_t bEndpoint,
                                                                  const void *pData,
                                                                  uint32_t dLength)
{
        Usbhs *pUdp = USBHS;

        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);
                        USBHS_RaiseEPInt(pUdp, bEndpoint, USBHS_DEVEPTIFR_TXINIS);
                }

                /* Enable interrupt */
                USBHS_EnableIntEP(pUdp, bEndpoint);
                USBHS_EnableEPIntType(pUdp, bEndpoint, USBHS_DEVEPTIER_TXINES);
        }

        return USBD_STATUS_SUCCESS;
}

static inline uint8_t UDPHS_Read(uint8_t bEndpoint,
                                                                 void *pData,
                                                                 uint32_t dLength)
{
        Usbhs *pUdp = USBHS;
        uint8_t bDmaEndpoint = (bEndpoint - 1);
        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(bDmaEndpoint, pXfr, USBHS_DEVDMACONTROL_END_TR_EN
                                                | USBHS_DEVDMACONTROL_END_TR_IT
                                                | USBHS_DEVDMACONTROL_END_B_EN
                                                | USBHS_DEVDMACONTROL_END_BUFFIT
                                                | USBHS_DEVDMACONTROL_CHANN_ENB);
                return USBD_STATUS_SUCCESS;
        }

#endif

        /* Enable IT */
        USBHS_EnableIntEP(pUdp, bEndpoint);
        USBHS_EnableEPIntType(pUdp, bEndpoint, USBHS_DEVEPTIER_RXOUTES);

        return USBD_STATUS_SUCCESS;
}

/*---------------------------------------------------------------------------
 *      Exported functions
 *---------------------------------------------------------------------------*/
void USBHS_Handler(void)
{
        Usbhs *pUdp = USBHS;

        uint32_t status;
        uint8_t numIt;

        status = USBHS_ReadIntStatus(pUdp, 0xFFFFFFFF);
        status &= USBHS_IsIntEnable(pUdp, 0xFFFFFFFF);

        /* Handle all USBHS interrupts */
        TRACE_DEBUG_WP("\n\r%c ", USBD_HAL_IsHighSpeed() ? 'H' : 'F');

        while (status) {
                /* SOF */
                if (status & USBHS_DEVISR_SOF) {
                        TRACE_DEBUG_WP("SOF ");
                        /* SOF handler */
                        /* Acknowledge interrupt */
                        USBHS_AckInt(pUdp, USBHS_DEVICR_SOFC);
                } else if (status & USBHS_DEVISR_MSOF) {
                        /* MOSF */
                        TRACE_DEBUG_WP("Mosf ");
                        /* Acknowledge interrupt */
                        USBHS_AckInt(pUdp, USBHS_DEVICR_MSOFC);
                } else if (status & USBHS_DEVISR_SUSP) {
                        /* Suspend, treated last */
                        TRACE_WARNING_WP("Susp ");
                        USBHS_UnFreezeClock(USBHS);
                        USBHS_AckInt(pUdp, USBHS_DEVICR_SUSPC);
                        USBHS_DisableInt(pUdp, USBHS_DEVIDR_SUSPEC);

                        /* Enable wakeup */
                        USBHS_EnableInt(pUdp, USBHS_DEVIER_WAKEUPES);
                        USBHS_FreezeClock(pUdp);
                        USBD_SuspendHandler();
                        USBHS_EnableInt(pUdp, USBHS_DEVIER_UPRSMES);
                } else if (status & USBHS_DEVISR_WAKEUP) {
                        /* Resume */
                        TRACE_INFO_WP("Rsm ");
                        USBHS_UnFreezeClock(USBHS);
                        USBHS_AckInt(pUdp, USBHS_DEVICR_WAKEUPC);
                        USBHS_DisableInt(pUdp, USBHS_DEVIDR_WAKEUPEC);
                        /* Acknowledge interrupt */

                        USBHS_EnableInt(pUdp, (USBHS_DEVIER_SUSPES | USBHS_DEVIER_EORSMES));
                        USBD_ResumeHandler();
                } else if (status & USBHS_DEVISR_EORSM) {
                        /* Bus reset */
                        USBHS_AckInt(pUdp, (USBHS_DEVICR_EORSMC));
                        USBHS_DisableInt(pUdp, (USBHS_DEVIDR_EORSMEC));
                } else if (status & USBHS_DEVISR_EORST) {
                        TRACE_DEBUG_WP("EoB ");
                        /* Acknowledge interrupt */
                        USBHS_AckInt(pUdp, USBHS_DEVICR_EORSTC);
                        /* Flush and enable the suspend interrupt */
                        USBHS_AckInt(pUdp, (USBHS_DEVICR_SUSPC | USBHS_DEVICR_WAKEUPC));
                        USBHS_EnableInt(pUdp, USBHS_DEVIER_SUSPES);

                        /* Reset handler */
                        USBD_ResetHandler();
                        USBHS_EnableInt(pUdp, USBHS_DEVIER_SUSPES |
                                                        USBHS_DEVIER_SOFES | USBHS_DEVIER_MSOFES);
                } else if (status & USBHS_DEVISR_UPRSM) {
                        /* Upstream resume */
                        TRACE_DEBUG_WP("ExtRes ");
                        /* Acknowledge interrupt */
                        USBHS_AckInt(pUdp, USBHS_DEVICR_UPRSMC);
                        USBHS_EnableInt(pUdp, (USBHS_DEVIER_EORSMES));
                } else {
                        /* Endpoints */
                        for (numIt = 0; numIt < CHIP_USB_NUMENDPOINTS; numIt++) {
#ifdef DMA

                                if ((CHIP_USB_ENDPOINTS_DMA(numIt))
                                        && USBHS_ReadDmaIntStatus(pUdp, numIt - 1))
                                        UDPHS_DmaHandler(numIt);
                                else
#endif
                                        if (USBHS_ReadEpIntStatus(pUdp, numIt))
                                                UDPHS_EndpointHandler(numIt);
                        }
                }

                /* Update interrupt status */
                status = USBHS_ReadIntStatus(pUdp, 0xFFFFFFFF);
                status &= USBHS_IsIntEnable(pUdp, 0xFFFFFFFF);

                TRACE_DEBUG_WP("\n\r");

                if (status) {
                        TRACE_DEBUG_WP(" - ");
                }
        }

        NVIC_ClearPendingIRQ(USBHS_IRQn); // clear l'IRQ
        memory_sync();

}

void USBD_HAL_ResetEPs(uint32_t bmEPs, uint8_t bStatus, uint8_t bKeepCfg)
{
        Usbhs *pUdp = USBHS;

        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) {
                        /* Disable ISR */
                        pUdp->USBHS_DEVIDR |= (epBit << SHIFT_INTERUPT);
                        /* Reset transfer information */
                        pEndpoint = &(endpoints[ep]);
                        /* Reset endpoint state */
                        pEndpoint->bank = 0;
                        /* Endpoint configure */
                        epCfg = pUdp->USBHS_DEVEPTCFG[ep];
                        /* Reset endpoint */
                        USBHS_ResetEP(pUdp, ep);

                        /* Restore configure */
                        if (bKeepCfg)
                                pUdp->USBHS_DEVEPTCFG[ep] = epCfg;
                        else
                                pEndpoint->state = UDPHS_ENDPOINT_DISABLED;

                        /*Clear data toggle sequence*/
                        USBHS_EnableEPIntType(pUdp, ep, USBHS_DEVEPTIER_RSTDTS);

                        /* Terminate transfer on this EP */
                        UDPHS_EndOfTransfer(ep, bStatus);
                }

                epBit <<= 1;
        }
}

void USBD_HAL_CancelIo(uint32_t bmEPs)
{
        Usbhs *pUdp = USBHS;

        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) {
                        /* Disable ISR */
                        pUdp->USBHS_DEVIDR |= (epBit << SHIFT_INTERUPT);
                        /* Terminate transfer on this EP */
                        UDPHS_EndOfTransfer(ep, USBD_STATUS_CANCELED);
                }

                epBit <<= 1;
        }
}

uint8_t USBD_HAL_ConfigureEP(const USBEndpointDescriptor *pDescriptor)
{
        Usbhs *pUdp = USBHS;

        Endpoint *pEndpoint;
        uint8_t bEndpoint;
        uint8_t bType;
        uint8_t bEndpointDir;
        uint8_t bNbTrans = 1;
        uint8_t bSizeEpt = 0;
        uint8_t bHs = ((USBHS_GetUsbSpeed(pUdp) == USBHS_SR_SPEED_HIGH_SPEED) ? true :
                                   false);

        /* NULL descriptor -> Control endpoint 0 */
        if (pDescriptor == 0) {

                bEndpoint = 0;
                pEndpoint = &(endpoints[bEndpoint]);
                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]);
                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]);
                /* Transfer type: Control, Isochronous, Bulk, Interrupt */
                bType = USBEndpointDescriptor_GetType(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 */
                /* HS Interval, *125us */
                if (bHs) {
                        /* MPS: Bit12,11 specify NB_TRANS, as USB 2.0 Spec. */
                        bNbTrans = ((pEndpoint->size >> 11) & 0x3);

                        if (CHIP_USB_ENDPOINTS_HBW(bEndpoint)) {
                                if (bNbTrans == 3)
                                        bNbTrans = 1;
                                else
                                        bNbTrans ++;
                        } else
                                bNbTrans = 0;

                        /* 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;


        /* 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)
                USBHS_EnableIntEP(pUdp, bEndpoint);

        USBHS_ConfigureEPs(pUdp, bEndpoint, bType, bEndpointDir, bSizeEpt,
                                           ((pEndpoint->bank) - 1));

        USBHS_AllocateMemory(pUdp, bEndpoint);

        while ((USBHS_DEVEPTISR_CFGOK & pUdp->USBHS_DEVEPTISR[bEndpoint]) == 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)pUdp->USBHS_DEVEPTCFG[bEndpoint]);

                for (;;);
        }

        if (bType == USBEndpointDescriptor_CONTROL) {
                // enable Endpoint
                USBHS_EnableEP(pUdp, bEndpoint, true);
                // Enable Ep interrupt type
                USBHS_EnableEPIntType(pUdp, bEndpoint,
                                                          USBHS_DEVEPTIER_RXOUTES | USBHS_DEVEPTIER_RXSTPES);
                // enable endpoint interrupt
                USBHS_EnableIntEP(pUdp, bEndpoint);
        } else {
#ifndef DMA
                USBHS_EnableEP(pUdp, bEndpoint, true);
#else
                USBHS_EnableEP(pUdp, bEndpoint, true);

                if (bType == USBEndpointDescriptor_ISOCHRONOUS)
                        USBHS_SetIsoTrans(pUdp, bEndpoint, bNbTrans);

                USBHS_AutoSwitchBankEnable(pUdp, bEndpoint, true);
#endif
        }

        //TRACE_DEBUG_WP("<%x,%x,%x> ", pEpt->UDPHS_EPTCFG, pEpt->UDPHS_EPTCTL, pEpt->UDPHS_EPTSTA);
        return bEndpoint;
}

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;
}

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;
}

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);
}

uint8_t USBD_HAL_WrWithHdr(uint8_t bEndpoint,
                                                   const void *pHdr, uint8_t bHdrLen,
                                                   const void *pData, uint32_t dLength)
{
        Usbhs *pUdp = USBHS;

        Endpoint *pEp = &(endpoints[bEndpoint]);
        uint8_t bDmaEndpoint = (bEndpoint - 1);
        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;

        SCB_CleanDCache_by_Addr((uint32_t *)pHdr, bHdrLen);
        SCB_CleanDCache_by_Addr((uint32_t *)pData, dLength);

        /* 1. DMA supported always, 2. Not ZLP */
        if (bHdrLen + dLength > 0) {
                uint8_t bNbTrans = (USBHS_GetConfigureEPs(pUdp, bEndpoint,
                                                        USBHS_DEVEPTCFG_NBTRANS_Msk)
                                                        >> USBHS_DEVEPTCFG_NBTRANS_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 = USBHS_DEVDMACONTROL_CHANN_ENB
                                                   | USBHS_DEVDMACONTROL_BUFF_LENGTH(bHdrLen)
                                                   | USBHS_DEVDMACONTROL_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;

                        uint32_t pktLen, ndxData = 0;
                        /* LD2: data -  bank 0 */
                        pktLen = pEp->size - bHdrLen;

                        /* It's the last DMA LLI */
                        if (pktLen >= dLength) {
                                pDmaLL[1].pNxtDesc = (void *)NULL;
                                pDmaLL[1].pAddr = (void *)pU8;
                                pDmaLL[1].dwCtrl = USBHS_DEVDMACONTROL_CHANN_ENB
                                                                   | USBHS_DEVDMACONTROL_BUFF_LENGTH(dLength)
                                                                   | USBHS_DEVDMACONTROL_END_B_EN
                                                                   | USBHS_DEVDMACONTROL_END_BUFFIT;
                        } else {
                                pDmaLL[1].pNxtDesc = (void *)&pDmaLL[2];
                                pDmaLL[1].pAddr = (void *)pU8;
                                pDmaLL[1].dwCtrl = USBHS_DEVDMACONTROL_CHANN_ENB
                                                                   | USBHS_DEVDMACONTROL_BUFF_LENGTH(pktLen)
                                                                   | USBHS_DEVDMACONTROL_END_B_EN
                                                                   | USBHS_DEVDMACONTROL_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 = USBHS_DEVDMACONTROL_CHANN_ENB
                                                                           | USBHS_DEVDMACONTROL_BUFF_LENGTH(dLength)
                                                                           | USBHS_DEVDMACONTROL_END_B_EN
                                                                           | USBHS_DEVDMACONTROL_END_BUFFIT;
                                } else {
                                        pDmaLL[2].pNxtDesc = (void *)&pDmaLL[3];
                                        pDmaLL[2].pAddr = (void *)&pU8[ndxData];
                                        pDmaLL[2].dwCtrl = USBHS_DEVDMACONTROL_CHANN_ENB
                                                                           | USBHS_DEVDMACONTROL_BUFF_LENGTH(pktLen)
                                                                           | USBHS_DEVDMACONTROL_END_B_EN
                                                                           | USBHS_DEVDMACONTROL_LDNXT_DSC;
                                        dLength -= pktLen; ndxData += pktLen;
                                        /* LD4: data  - bank 2 */
                                        pDmaLL[3].pNxtDesc = (void *) NULL;
                                        pDmaLL[3].pAddr = (void *)&pU8[ndxData];
                                        pDmaLL[3].dwCtrl = USBHS_DEVDMACONTROL_CHANN_ENB
                                                                           | USBHS_DEVDMACONTROL_BUFF_LENGTH(dLength)
                                                                           | USBHS_DEVDMACONTROL_END_B_EN
                                                                           | USBHS_DEVDMACONTROL_END_BUFFIT;
                                }
                        }

                        /* Normal, fill all data */
                } else {
                        /* LD2: data   -  load to fifo with interrupt */
                        dLength = pXfr->buffered - bHdrLen;
                        pDmaLL[1].pNxtDesc = (void *)NULL;
                        pDmaLL[1].pAddr = (void *)pData;
                        pDmaLL[1].dwCtrl = USBHS_DEVDMACONTROL_CHANN_ENB
                                                           | USBHS_DEVDMACONTROL_BUFF_LENGTH(dLength)
                                                           | USBHS_DEVDMACONTROL_END_B_EN
                                                           | USBHS_DEVDMACONTROL_END_BUFFIT;
                }

                SCB_CleanDCache_by_Addr((uint32_t *)dmaLL, sizeof(dmaLL));
                /* Interrupt enable */
                USBHS_EnableDMAIntEP(pUdp, bDmaEndpoint);
                /* Start transfer with LLI */
                pUdp->USBHS_DEVDMA[bDmaEndpoint].USBHS_DEVDMANXTDSC = (uint32_t)pDmaLL;
                pUdp->USBHS_DEVDMA[bDmaEndpoint].USBHS_DEVDMACONTROL = 0;
                pUdp->USBHS_DEVDMA[bDmaEndpoint].USBHS_DEVDMACONTROL =
                        USBHS_DEVDMACONTROL_LDNXT_DSC;
                return USBD_STATUS_SUCCESS;
        }

#endif

        /* Enable IT */
        USBHS_EnableIntEP(pUdp, bEndpoint);
        USBHS_EnableEPIntType(pUdp, bEndpoint, USBHS_DEVEPTIER_TXINES);
        return USBD_STATUS_SUCCESS;
}

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);
}

void USBD_HAL_Connect(void)
{
        uint32_t Interrupt;

        // At startup the USB bus state is unknown,
        // therefore the state is considered IDLE to not miss any USB event

        USBHS_UnFreezeClock(USBHS);

        // Authorize attach
        USBHS_DetachUsb(USBHS, false);

        // (RESET_AND_WAKEUP)
        // After the attach and the first USB suspend, the following USB Reset time can be inferior to CPU restart clock time.
        // Thus, the USB Reset state is not detected and endpoint control is not allocated
        // In this case, a Reset is do automatically after attach.
        USBD_HAL_ConfigureEP(0);

        // Enable USB line events
        Interrupt = (USBHS_DEVIER_EORSTES | USBHS_DEVIER_WAKEUPES |
                                 USBHS_DEVIER_SUSPES);

#ifdef USB_DEVICE_HS_SUPPORT
        Interrupt |= USBHS_DEVIER_MSOFES;
#endif

        USBHS_EnableInt(USBHS, Interrupt);

        // Reset following interrupts flag
        Interrupt = (USBHS_DEVICR_EORSTC | USBHS_DEVICR_SOFC | USBHS_DEVICR_MSOFC |
                                 USBHS_DEVICR_SUSPC);
        USBHS_AckInt(USBHS, Interrupt);


        // The first suspend interrupt is not detected else raise it
        USBHS_RaiseInt(USBHS, USBHS_DEVIFR_SUSPS);

        USBHS_AckInt(USBHS, USBHS_DEVICR_WAKEUPC);

        USBHS_FreezeClock(USBHS);
}

void USBD_HAL_Disconnect(void)
{
        USBHS_FreezeClock(USBHS);
        // Detach device from the bus
        USBHS_DetachUsb(USBHS, true);
}

void USBD_HAL_RemoteWakeUp(void)
{
        Usbhs *pUdp = USBHS;
        TRACE_INFO_WP("RWUp ");

        /* Activates a remote wakeup (edge on ESR), then clear ESR */
        USBHS_SetRemoteWakeUp(pUdp);

        while (pUdp->USBHS_DEVCTRL & USBHS_DEVCTRL_RMWKUP);

        TRACE_DEBUG_WP("w");
}

void USBD_HAL_SetAddress(uint8_t address)
{
        Usbhs *pUdp = USBHS;

        if (address)
                USBHS_SetAddress(pUdp, address);
        else
                USBHS_EnableAddress(pUdp, false);
}

void USBD_HAL_SetConfiguration(uint8_t cfgnum)
{
        /* Nothing to do now */
        cfgnum = cfgnum;
}

void USBD_HAL_Init(void)
{

#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

        USBHS_UsbEnable(USBHS, false);

        USBHS_UsbMode(USBHS, DEVICE_MODE);

        USBHS_UsbEnable(USBHS, true);

        USBHS_UnFreezeClock(USBHS);

        if (ForceFS)
                USBHS_EnableHighSpeed(USBHS, false);
        else
                USBHS_EnableHighSpeed(USBHS, true);

        /*  Check USB clock */
        while (!USBHS_ISUsableClock(USBHS));

        USBHS_FreezeClock(USBHS);

        /* Clear IRQ */
        NVIC_ClearPendingIRQ(USBHS_IRQn);
        /* IRQ */
        NVIC_EnableIRQ(USBHS_IRQn);

}

uint8_t USBD_HAL_Stall(uint8_t bEP)
{
        Usbhs *pUdp = USBHS;

        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 */
        USBHS_EnableEPIntType(pUdp, bEP, USBHS_DEVEPTIER_STALLRQS);

        TRACE_INFO_WP("Stall%d ", bEP);
        return USBD_STATUS_SUCCESS;
}

uint8_t USBD_HAL_Halt(uint8_t bEndpoint, uint8_t ctl)
{
        Usbhs *pUdp = USBHS;

        Endpoint *pEndpoint = &(endpoints[bEndpoint]);
        uint8_t bDmaEndpoint = (bEndpoint - 1);
        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_INFO_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;
                        memory_sync();

                        while (!USBHS_IsBankFree(pUdp, bEndpoint)) {
                                USBHS_KillBank(pUdp, bEndpoint);

                                while (USBHS_IsBankKilled(pUdp, bEndpoint));
                        }

                        if (USBHS_IsBankFree(pUdp, bEndpoint)) {
                                USBHS_AutoSwitchBankEnable(pUdp, bEndpoint, false);
                                USBHS_EnableEPIntType(pUdp, bEndpoint,
                                                                          (USBHS_DEVEPTIER_STALLRQS | USBHS_DEVEPTIER_RSTDTS));
                        } else {
                                USBHS_EnableEPIntType(pUdp, bEndpoint, USBHS_DEVEPTIER_NBUSYBKES);
#ifdef DMA

                                if (CHIP_USB_ENDPOINTS_DMA(bDmaEndpoint)) {
                                        /* Enable the endpoint DMA interrupt */
                                        USBHS_EnableDMAIntEP(pUdp, bDmaEndpoint);
                                } else {
                                        /* Enable the endpoint interrupt */
                                        USBHS_EnableIntEP(pUdp, bEndpoint);
                                }

#else
                                /* Enable the endpoint interrupt */
                                USBHS_EnableIntEP(pUdp, bEndpoint);
#endif
                        }
                }

                /* CLEAR Halt */
        } else if (ctl == 0) {
                /* Check if the endpoint is halted */
                if ((pEndpoint->state == UDPHS_ENDPOINT_HALTED)
                        || (USBHS_IsEpIntEnable(pUdp, bEndpoint, USBHS_DEVEPTIMR_STALLRQ))) {
                        TRACE_INFO_WP("Unhalt%d ", bEndpoint);
                        /* Return endpoint to Idle state */
                        pEndpoint->state = UDPHS_ENDPOINT_IDLE;

                        /* Clear FORCESTALL flag */
                        USBHS_DisableEPIntType(pUdp, bEndpoint, USBHS_DEVEPTIDR_STALLRQC);
                        USBHS_AutoSwitchBankEnable(pUdp, bEndpoint, true);
                }
        }

        /* Return Halt status */
        if (pEndpoint->state == UDPHS_ENDPOINT_HALTED)
                status = 1;

        return (status);
}

void USBD_HAL_WaitReadData(uint8_t bEndpoint)
{
        Usbhs *pUdp = USBHS;

        while (USBHS_ByteCount(pUdp, bEndpoint) == 0);
}

uint8_t USBD_HAL_IsHighSpeed(void)
{
        return USBHS_IsUsbHighSpeed(USBHS);
}

void USBD_HAL_Suspend(void)
{
        /* The device enters the Suspended state */
        USBHS_FreezeClock(USBHS);
}

void USBD_HAL_Activate(void)
{
        USBHS_UnFreezeClock(USBHS);
}

void USBD_HAL_Disable(void)
{
        //** Disable USB hardware
        USBHS_UsbEnable(USBHS, false);

        /* Clear IRQ */
        NVIC_ClearPendingIRQ(USBHS_IRQn);
        /* IRQ */
        NVIC_DisableIRQ(USBHS_IRQn);
}


void USBD_HAL_Test(uint8_t bIndex)
{
        Usbhs *pUdp = USBHS;
        uint8_t *pFifo;
        uint32_t i;

        /* remove suspend for TEST */
        USBHS_DisableInt(pUdp, USBHS_DEVIDR_SUSPEC);
        /* force High Speed (remove suspend) */
        pUdp->USBHS_DEVCTRL |= USBHS_DEVCTRL_SPDCONF_HIGH_SPEED;

        USBHS_EnableTestMode(pUdp, USBHS_DEVCTRL_OPMODE2);

        switch (bIndex) {
        case USBFeatureRequest_TESTPACKET:
                TRACE_DEBUG_WP("TEST_PACKET ");

                pUdp->USBHS_DEVDMA[1].USBHS_DEVDMACONTROL = 0;
                pUdp->USBHS_DEVDMA[2].USBHS_DEVDMACONTROL = 0;

                /* Configure endpoint 2, 64 bytes, direction IN, type BULK, 1 bank */
                pUdp->USBHS_DEVEPTCFG[2] = USBHS_DEVEPTCFG_EPSIZE_64_BYTE
                                                                   | USBHS_DEVEPTCFG_EPDIR
                                                                   | USBHS_DEVEPTCFG_EPTYPE_BLK
                                                                   | USBHS_DEVEPTCFG_EPBK_1_BANK;
                USBHS_AllocateMemory(pUdp, 2);

                while ((USBHS_DEVEPTISR_CFGOK & pUdp->USBHS_DEVEPTISR[2]) !=
                           USBHS_DEVEPTISR_CFGOK);

                USBHS_EnableEP(pUdp, 2, true);

                /* Write FIFO */
                pFifo = (uint8_t *)((uint32_t *)(USBHS_RAM_ADDR) + (EPT_VIRTUAL_SIZE * 2));

                for (i = 0; i < sizeof(test_packet_buffer); i++)
                        pFifo[i] = test_packet_buffer[i];

                /* Tst PACKET */
                USBHS_EnableTestMode(pUdp, USBHS_DEVCTRL_TSTPCKT);
                /* Send packet */
                USBHS_RaiseEPInt(pUdp, 2, USBHS_DEVEPTIFR_TXINIS);
                break;

        case USBFeatureRequest_TESTJ:
                TRACE_DEBUG_WP("TEST_J ");
                USBHS_EnableTestMode(pUdp, USBHS_DEVCTRL_TSTJ);
                break;

        case USBFeatureRequest_TESTK:
                TRACE_DEBUG_WP("TEST_K ");
                USBHS_EnableTestMode(pUdp, USBHS_DEVCTRL_TSTK);
                break;

        case USBFeatureRequest_TESTSE0NAK:
                TRACE_DEBUG_WP("TEST_SEO_NAK ");
                USBHS_DisableInt(pUdp, 0xFFFFFFFF); // for test
                break;

        case USBFeatureRequest_TESTSENDZLP:
                USBHS_RaiseEPInt(pUdp, 0, USBHS_DEVEPTIFR_TXINIS);
                TRACE_DEBUG_WP("SEND_ZLP ");
                break;
        }

        TRACE_DEBUG_WP("\n\r");
}