diff options
Diffstat (limited to 'os/hal')
| -rw-r--r-- | os/hal/ports/STM32/LLD/OTGv1/hal_usb_lld.c | 171 | ||||
| -rw-r--r-- | os/hal/ports/STM32/LLD/OTGv1/hal_usb_lld.h | 34 | ||||
| -rw-r--r-- | os/hal/ports/STM32/LLD/OTGv1/hal_usb_lld_alt.c | 1422 | ||||
| -rw-r--r-- | os/hal/ports/STM32/LLD/OTGv1/hal_usb_lld_alt.h | 628 | 
4 files changed, 2 insertions, 2253 deletions
diff --git a/os/hal/ports/STM32/LLD/OTGv1/hal_usb_lld.c b/os/hal/ports/STM32/LLD/OTGv1/hal_usb_lld.c index 48581c068..3ff3bc9f0 100644 --- a/os/hal/ports/STM32/LLD/OTGv1/hal_usb_lld.c +++ b/os/hal/ports/STM32/LLD/OTGv1/hal_usb_lld.c @@ -394,17 +394,8 @@ static void otg_epin_handler(USBDriver *usbp, usbep_t ep) {    }
    if ((epint & DIEPINT_TXFE) &&
        (otgp->DIEPEMPMSK & DIEPEMPMSK_INEPTXFEM(ep))) {
 -#if 0
 -    /* The thread is made ready, it will be scheduled on ISR exit.*/
 -    osalSysLockFromISR();
 -    usbp->txpending |= (1 << ep);
 -    otgp->DIEPEMPMSK &= ~(1 << ep);
 -    osalThreadResumeI(&usbp->wait, MSG_OK);
 -    osalSysUnlockFromISR();
 -#else
      /* TX FIFO empty or emptying.*/
      otg_txfifo_handler(usbp, ep);
 -#endif
    }
  }
 @@ -492,17 +483,8 @@ static void otg_isoc_in_failed_handler(USBDriver *usbp) {        /* Prepare data for next frame */
        _usb_isr_invoke_in_cb(usbp, ep);
 -#if 0
 -      /* Pump out data for next frame */
 -      osalSysLockFromISR();
 -      otgp->DIEPEMPMSK &= ~(1 << ep);
 -      usbp->txpending |= (1 << ep);
 -      osalThreadResumeI(&usbp->wait, MSG_OK);
 -      osalSysUnlockFromISR();
 -#else
 -    /* TX FIFO empty or emptying.*/
 -    otg_txfifo_handler(usbp, ep);
 -#endif
 +      /* TX FIFO empty or emptying.*/
 +      otg_txfifo_handler(usbp, ep);
      }
    }
  }
 @@ -550,10 +532,6 @@ static void usb_lld_serve_interrupt(USBDriver *usbp) {    /* Reset interrupt handling.*/
    if (sts & GINTSTS_USBRST) {
 -#if 0
 -    /* Resetting pending operations.*/
 -    usbp->txpending = 0;
 -#endif
      /* Default reset action.*/
      _usb_reset(usbp);
 @@ -578,10 +556,6 @@ static void usb_lld_serve_interrupt(USBDriver *usbp) {    /* Suspend handling.*/
    if (sts & GINTSTS_USBSUSP) {
 -#if 0
 -    /* Resetting pending operations.*/
 -    usbp->txpending = 0;
 -#endif
      /* Default suspend action.*/
      _usb_suspend(usbp);
    }
 @@ -614,23 +588,11 @@ static void usb_lld_serve_interrupt(USBDriver *usbp) {      otg_isoc_out_failed_handler(usbp);
    }
 -  /* RX FIFO not empty handling.*/
 -#if 0
 -  if (sts & GINTSTS_RXFLVL) {
 -    /* The interrupt is masked while the thread has control or it would
 -       be triggered again.*/
 -    osalSysLockFromISR();
 -    otgp->GINTMSK &= ~GINTMSK_RXFLVLM;
 -    osalThreadResumeI(&usbp->wait, MSG_OK);
 -    osalSysUnlockFromISR();
 -  }
 -#else
    /* Performing the whole FIFO emptying in the ISR, it is advised to keep
       this IRQ at a very low priority level.*/
    if ((sts & GINTSTS_RXFLVL) != 0U) {
      otg_rxfifo_handler(usbp);
    }
 -#endif
    /* IN/OUT endpoints event handling.*/
    src = otgp->DAINT;
 @@ -746,52 +708,15 @@ void usb_lld_init(void) {    /* Driver initialization.*/
  #if STM32_USB_USE_OTG1
    usbObjectInit(&USBD1);
 -#if 0
 -  USBD1.wait      = NULL;
 -#endif
    USBD1.otg       = OTG_FS;
    USBD1.otgparams = &fsparams;
 -#if 0
 -#if defined(_CHIBIOS_RT_)
 -  USBD1.tr = NULL;
 -  /* Filling the thread working area here because the function
 -     @p chThdCreateI() does not do it.*/
 -#if CH_DBG_FILL_THREADS
 -  {
 -    void *wsp = USBD1.wa_pump;
 -    _thread_memfill((uint8_t *)wsp,
 -                    (uint8_t *)wsp + sizeof (USBD1.wa_pump),
 -                    CH_DBG_STACK_FILL_VALUE);
 -  }
 -#endif /* CH_DBG_FILL_THREADS */
 -#endif /* defined(_CHIBIOS_RT_) */
 -#endif
  #endif
  #if STM32_USB_USE_OTG2
    usbObjectInit(&USBD2);
 -#if 0
 -  USBD2.wait      = NULL;
 -#endif
    USBD2.otg       = OTG_HS;
    USBD2.otgparams = &hsparams;
 -
 -#if 0
 -#if defined(_CHIBIOS_RT_)
 -  USBD2.tr = NULL;
 -  /* Filling the thread working area here because the function
 -     @p chThdCreateI() does not do it.*/
 -#if CH_DBG_FILL_THREADS
 -  {
 -    void *wsp = USBD2.wa_pump;
 -    _thread_memfill((uint8_t *)wsp,
 -                    (uint8_t *)wsp + sizeof (USBD2.wa_pump),
 -                    CH_DBG_STACK_FILL_VALUE);
 -  }
 -#endif /* CH_DBG_FILL_THREADS */
 -#endif /* defined(_CHIBIOS_RT_) */
 -#endif
  #endif
  }
 @@ -876,10 +801,6 @@ void usb_lld_start(USBDriver *usbp) {      }
  #endif
 -#if 0
 -    /* Clearing mask of TXFIFOs to be filled.*/
 -    usbp->txpending = 0;
 -#endif
      /* PHY enabled.*/
      otgp->PCGCCTL = 0;
 @@ -929,24 +850,6 @@ void usb_lld_start(USBDriver *usbp) {      /* Clears all pending IRQs, if any. */
      otgp->GINTSTS  = 0xFFFFFFFF;
 -#if 0
 -#if defined(_CHIBIOS_RT_)
 -    /* Creates the data pump thread. Note, it is created only once.*/
 -    if (usbp->tr == NULL) {
 -      thread_descriptor_t usbpump_descriptor = {
 -        "usb_pump",
 -        THD_WORKING_AREA_BASE(usbp->wa_pump),
 -        THD_WORKING_AREA_END(usbp->wa_pump),
 -        STM32_USB_OTG_THREAD_PRIO,
 -        usb_lld_pump,
 -        (void *)usbp
 -      };
 -
 -      usbp->tr = chThdCreateI(&usbpump_descriptor);
 -      chSchRescheduleS();
 -  }
 -#endif
 -#endif
      /* Global interrupts enable.*/
      otgp->GAHBCFG |= GAHBCFG_GINTMSK;
    }
 @@ -969,9 +872,6 @@ void usb_lld_stop(USBDriver *usbp) {         active.*/
      otg_disable_ep(usbp);
 -#if 0
 -    usbp->txpending = 0;
 -#endif
      otgp->DAINTMSK   = 0;
      otgp->GAHBCFG    = 0;
      otgp->GCCFG      = 0;
 @@ -1350,73 +1250,6 @@ void usb_lld_clear_in(USBDriver *usbp, usbep_t ep) {    usbp->otg->ie[ep].DIEPCTL &= ~DIEPCTL_STALL;
  }
 -#if 0
 -/**
 - * @brief   USB data transfer loop.
 - * @details This function must be executed by a system thread in order to
 - *          make the USB driver work.
 - * @note    The data copy part of the driver is implemented in this thread
 - *          in order to not perform heavy tasks within interrupt handlers.
 - *
 - * @param[in] p         pointer to the @p USBDriver object
 - *
 - * @special
 - */
 -void usb_lld_pump(void *p) {
 -  USBDriver *usbp = (USBDriver *)p;
 -  stm32_otg_t *otgp = usbp->otg;
 -
 -  osalSysLock();
 -  while (true) {
 -    usbep_t ep;
 -    uint32_t epmask;
 -
 -    /* Nothing to do, going to sleep.*/
 -    if ((usbp->state == USB_STOP) ||
 -        ((usbp->txpending == 0) && !(otgp->GINTSTS & GINTSTS_RXFLVL))) {
 -      otgp->GINTMSK |= GINTMSK_RXFLVLM;
 -      osalThreadSuspendS(&usbp->wait);
 -    }
 -    osalSysUnlock();
 -
 -    /* Checks if there are TXFIFOs to be filled.*/
 -    for (ep = 0; ep <= usbp->otgparams->num_endpoints; ep++) {
 -
 -      /* Empties the RX FIFO.*/
 -      while (otgp->GINTSTS & GINTSTS_RXFLVL) {
 -        otg_rxfifo_handler(usbp);
 -      }
 -
 -      epmask = (1 << ep);
 -      if (usbp->txpending & epmask) {
 -        bool done;
 -
 -        osalSysLock();
 -        /* USB interrupts are globally *suspended* because the peripheral
 -           does not allow any interference during the TX FIFO filling
 -           operation.
 -           Synopsys document: DesignWare Cores USB 2.0 Hi-Speed On-The-Go (OTG)
 -             "The application has to finish writing one complete packet before
 -              switching to a different channel/endpoint FIFO. Violating this
 -              rule results in an error.".*/
 -        otgp->GAHBCFG &= ~GAHBCFG_GINTMSK;
 -        usbp->txpending &= ~epmask;
 -        osalSysUnlock();
 -
 -        done = otg_txfifo_handler(usbp, ep);
 -
 -        osalSysLock();
 -        otgp->GAHBCFG |= GAHBCFG_GINTMSK;
 -        if (!done)
 -          otgp->DIEPEMPMSK |= epmask;
 -        osalSysUnlock();
 -      }
 -    }
 -    osalSysLock();
 -  }
 -}
 -#endif
 -
  #endif /* HAL_USE_USB */
  /** @} */
 diff --git a/os/hal/ports/STM32/LLD/OTGv1/hal_usb_lld.h b/os/hal/ports/STM32/LLD/OTGv1/hal_usb_lld.h index 31cb87a58..19269617e 100644 --- a/os/hal/ports/STM32/LLD/OTGv1/hal_usb_lld.h +++ b/os/hal/ports/STM32/LLD/OTGv1/hal_usb_lld.h @@ -110,20 +110,6 @@  #endif
  /**
 - * @brief   Dedicated data pump threads priority.
 - */
 -#if !defined(STM32_USB_OTG_THREAD_PRIO) || defined(__DOXYGEN__)
 -#define STM32_USB_OTG_THREAD_PRIO           LOWPRIO
 -#endif
 -
 -/**
 - * @brief   Dedicated data pump threads stack size.
 - */
 -#if !defined(STM32_USB_OTG_THREAD_STACK_SIZE) || defined(__DOXYGEN__)
 -#define STM32_USB_OTG_THREAD_STACK_SIZE     128
 -#endif
 -
 -/**
   * @brief   Exception priority level during TXFIFOs operations.
   * @note    Because an undocumented silicon behavior the operation of
   *          copying a packet into a TXFIFO must not be interrupted by
 @@ -518,26 +504,6 @@ struct USBDriver {     * @brief   Pointer to the next address in the packet memory.
     */
    uint32_t                      pmnext;
 -#if 0
 -  /**
 -   * @brief   Mask of TXFIFOs to be filled by the pump thread.
 -   */
 -  uint32_t                      txpending;
 -  /**
 -   * @brief   Pointer to the thread when it is sleeping or @p NULL.
 -   */
 -  thread_reference_t            wait;
 -#if defined(_CHIBIOS_RT_)
 -  /**
 -   * @brief   Pointer to the thread.
 -   */
 -  thread_reference_t            tr;
 -  /**
 -   * @brief   Working area for the dedicated data pump thread;
 -   */
 -  THD_WORKING_AREA(wa_pump, STM32_USB_OTG_THREAD_STACK_SIZE);
 -#endif
 -#endif
  };
  /*===========================================================================*/
 diff --git a/os/hal/ports/STM32/LLD/OTGv1/hal_usb_lld_alt.c b/os/hal/ports/STM32/LLD/OTGv1/hal_usb_lld_alt.c deleted file mode 100644 index 9b94e478d..000000000 --- a/os/hal/ports/STM32/LLD/OTGv1/hal_usb_lld_alt.c +++ /dev/null @@ -1,1422 +0,0 @@ -/*
 -    ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio
 -
 -    Licensed under the Apache License, Version 2.0 (the "License");
 -    you may not use this file except in compliance with the License.
 -    You may obtain a copy of the License at
 -
 -        http://www.apache.org/licenses/LICENSE-2.0
 -
 -    Unless required by applicable law or agreed to in writing, software
 -    distributed under the License is distributed on an "AS IS" BASIS,
 -    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 -    See the License for the specific language governing permissions and
 -    limitations under the License.
 -*/
 -
 -/**
 - * @file    OTGv1/hal_usb_lld.c
 - * @brief   STM32 USB subsystem low level driver source.
 - *
 - * @addtogroup USB
 - * @{
 - */
 -
 -#include <string.h>
 -
 -#include "hal.h"
 -
 -#if HAL_USE_USB || defined(__DOXYGEN__)
 -
 -/*===========================================================================*/
 -/* Driver local definitions.                                                 */
 -/*===========================================================================*/
 -
 -#define TRDT_VALUE_FS           5
 -#define TRDT_VALUE_HS           9
 -
 -#define EP0_MAX_INSIZE          64
 -#define EP0_MAX_OUTSIZE         64
 -
 -#if STM32_OTG_STEPPING == 1
 -#if defined(BOARD_OTG_NOVBUSSENS)
 -#define GCCFG_INIT_VALUE        (GCCFG_NOVBUSSENS | GCCFG_VBUSASEN |        \
 -                                 GCCFG_VBUSBSEN | GCCFG_PWRDWN)
 -#else
 -#define GCCFG_INIT_VALUE        (GCCFG_VBUSASEN | GCCFG_VBUSBSEN |          \
 -                                 GCCFG_PWRDWN)
 -#endif
 -
 -#elif STM32_OTG_STEPPING == 2
 -#if defined(BOARD_OTG_NOVBUSSENS)
 -#define GCCFG_INIT_VALUE        GCCFG_PWRDWN
 -#else
 -#define GCCFG_INIT_VALUE        (GCCFG_VBDEN | GCCFG_PWRDWN)
 -#endif
 -
 -#endif
 -
 -/*===========================================================================*/
 -/* Driver exported variables.                                                */
 -/*===========================================================================*/
 -
 -/** @brief OTG_FS driver identifier.*/
 -#if STM32_USB_USE_OTG1 || defined(__DOXYGEN__)
 -USBDriver USBD1;
 -#endif
 -
 -/** @brief OTG_HS driver identifier.*/
 -#if STM32_USB_USE_OTG2 || defined(__DOXYGEN__)
 -USBDriver USBD2;
 -#endif
 -
 -/*===========================================================================*/
 -/* Driver local variables and types.                                         */
 -/*===========================================================================*/
 -
 -/**
 - * @brief   EP0 state.
 - * @note    It is an union because IN and OUT endpoints are never used at the
 - *          same time for EP0.
 - */
 -static union {
 -  /**
 -   * @brief   IN EP0 state.
 -   */
 -  USBInEndpointState in;
 -  /**
 -   * @brief   OUT EP0 state.
 -   */
 -  USBOutEndpointState out;
 -} ep0_state;
 -
 -/**
 - * @brief   Buffer for the EP0 setup packets.
 - */
 -static uint8_t ep0setup_buffer[8];
 -
 -/**
 - * @brief   EP0 initialization structure.
 - */
 -static const USBEndpointConfig ep0config = {
 -  USB_EP_MODE_TYPE_CTRL,
 -  _usb_ep0setup,
 -  _usb_ep0in,
 -  _usb_ep0out,
 -  0x40,
 -  0x40,
 -  &ep0_state.in,
 -  &ep0_state.out,
 -  1,
 -  ep0setup_buffer
 -};
 -
 -#if STM32_USB_USE_OTG1
 -static const stm32_otg_params_t fsparams = {
 -  STM32_USB_OTG1_RX_FIFO_SIZE / 4,
 -  STM32_OTG1_FIFO_MEM_SIZE,
 -  STM32_OTG1_ENDPOINTS
 -};
 -#endif
 -
 -#if STM32_USB_USE_OTG2
 -static const stm32_otg_params_t hsparams = {
 -  STM32_USB_OTG2_RX_FIFO_SIZE / 4,
 -  STM32_OTG2_FIFO_MEM_SIZE,
 -  STM32_OTG2_ENDPOINTS
 -};
 -#endif
 -
 -/*===========================================================================*/
 -/* Driver local functions.                                                   */
 -/*===========================================================================*/
 -
 -static void otg_core_reset(USBDriver *usbp) {
 -  stm32_otg_t *otgp = usbp->otg;
 -
 -  osalSysPolledDelayX(32);
 -
 -  /* Core reset and delay of at least 3 PHY cycles.*/
 -  otgp->GRSTCTL = GRSTCTL_CSRST;
 -  while ((otgp->GRSTCTL & GRSTCTL_CSRST) != 0)
 -    ;
 -
 -  osalSysPolledDelayX(18);
 -
 -  /* Wait AHB idle condition.*/
 -  while ((otgp->GRSTCTL & GRSTCTL_AHBIDL) == 0)
 -    ;
 -}
 -
 -static void otg_disable_ep(USBDriver *usbp) {
 -  stm32_otg_t *otgp = usbp->otg;
 -  unsigned i;
 -
 -  for (i = 0; i <= usbp->otgparams->num_endpoints; i++) {
 -    otgp->ie[i].DIEPCTL = 0;
 -    otgp->ie[i].DIEPTSIZ = 0;
 -    otgp->ie[i].DIEPINT = 0xFFFFFFFF;
 -
 -    otgp->oe[i].DOEPCTL = 0;
 -    otgp->oe[i].DOEPTSIZ = 0;
 -    otgp->oe[i].DOEPINT = 0xFFFFFFFF;
 -  }
 -  otgp->DAINTMSK = DAINTMSK_OEPM(0) | DAINTMSK_IEPM(0);
 -}
 -
 -static void otg_rxfifo_flush(USBDriver *usbp) {
 -  stm32_otg_t *otgp = usbp->otg;
 -
 -  otgp->GRSTCTL = GRSTCTL_RXFFLSH;
 -  while ((otgp->GRSTCTL & GRSTCTL_RXFFLSH) != 0)
 -    ;
 -  /* Wait for 3 PHY Clocks.*/
 -  osalSysPolledDelayX(18);
 -}
 -
 -static void otg_txfifo_flush(USBDriver *usbp, uint32_t fifo) {
 -  stm32_otg_t *otgp = usbp->otg;
 -
 -  otgp->GRSTCTL = GRSTCTL_TXFNUM(fifo) | GRSTCTL_TXFFLSH;
 -  while ((otgp->GRSTCTL & GRSTCTL_TXFFLSH) != 0)
 -    ;
 -  /* Wait for 3 PHY Clocks.*/
 -  osalSysPolledDelayX(18);
 -}
 -
 -/**
 - * @brief   Resets the FIFO RAM memory allocator.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - *
 - * @notapi
 - */
 -static void otg_ram_reset(USBDriver *usbp) {
 -
 -  usbp->pmnext = usbp->otgparams->rx_fifo_size;
 -}
 -
 -/**
 - * @brief   Allocates a block from the FIFO RAM memory.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - * @param[in] size      size of the packet buffer to allocate in words
 - *
 - * @notapi
 - */
 -static uint32_t otg_ram_alloc(USBDriver *usbp, size_t size) {
 -  uint32_t next;
 -
 -  next = usbp->pmnext;
 -  usbp->pmnext += size;
 -  osalDbgAssert(usbp->pmnext <= usbp->otgparams->otg_ram_size,
 -                "OTG FIFO memory overflow");
 -  return next;
 -}
 -
 -/**
 - * @brief   Writes to a TX FIFO.
 - *
 - * @param[in] fifop     pointer to the FIFO register
 - * @param[in] buf       buffer where to copy the endpoint data
 - * @param[in] n         maximum number of bytes to copy
 - *
 - * @notapi
 - */
 -static void otg_fifo_write_from_buffer(volatile uint32_t *fifop,
 -                                       const uint8_t *buf,
 -                                       size_t n) {
 -
 -  osalDbgAssert(n > 0, "is zero");
 -
 -  while (true) {
 -    *fifop = *((uint32_t *)buf);
 -    if (n <= 4) {
 -      break;
 -    }
 -    n -= 4;
 -    buf += 4;
 -  }
 -}
 -
 -/**
 - * @brief   Reads a packet from the RXFIFO.
 - *
 - * @param[in] fifop     pointer to the FIFO register
 - * @param[out] buf      buffer where to copy the endpoint data
 - * @param[in] n         number of bytes to pull from the FIFO
 - * @param[in] max       number of bytes to copy into the buffer
 - *
 - * @notapi
 - */
 -static void otg_fifo_read_to_buffer(volatile uint32_t *fifop,
 -                                    uint8_t *buf,
 -                                    size_t n,
 -                                    size_t max) {
 -  uint32_t w = 0;
 -  size_t i = 0;
 -
 -  while (i < n) {
 -    if ((i & 3) == 0){
 -      w = *fifop;
 -    }
 -    if (i < max) {
 -      *buf++ = (uint8_t)w;
 -      w >>= 8;
 -    }
 -    i++;
 -  }
 -}
 -
 -/**
 - * @brief   Incoming packets handler.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - *
 - * @notapi
 - */
 -static void otg_rxfifo_handler(USBDriver *usbp) {
 -  uint32_t sts, cnt, ep;
 -
 -  /* Popping the event word out of the RX FIFO.*/
 -  sts = usbp->otg->GRXSTSP;
 -
 -  /* Event details.*/
 -  cnt = (sts & GRXSTSP_BCNT_MASK) >> GRXSTSP_BCNT_OFF;
 -  ep  = (sts & GRXSTSP_EPNUM_MASK) >> GRXSTSP_EPNUM_OFF;
 -
 -  switch (sts & GRXSTSP_PKTSTS_MASK) {
 -  case GRXSTSP_SETUP_DATA:
 -    otg_fifo_read_to_buffer(usbp->otg->FIFO[0], usbp->epc[ep]->setup_buf,
 -                            cnt, 8);
 -    break;
 -  case GRXSTSP_SETUP_COMP:
 -    break;
 -  case GRXSTSP_OUT_DATA:
 -    otg_fifo_read_to_buffer(usbp->otg->FIFO[0],
 -                            usbp->epc[ep]->out_state->rxbuf,
 -                            cnt,
 -                            usbp->epc[ep]->out_state->rxsize -
 -                            usbp->epc[ep]->out_state->rxcnt);
 -    usbp->epc[ep]->out_state->rxbuf += cnt;
 -    usbp->epc[ep]->out_state->rxcnt += cnt;
 -    break;
 -  case GRXSTSP_OUT_COMP:
 -    break;
 -  case GRXSTSP_OUT_GLOBAL_NAK:
 -    break;
 -  default:
 -    break;
 -  }
 -}
 -
 -/**
 - * @brief   Outgoing packets handler.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - * @param[in] ep        endpoint number
 - *
 - * @notapi
 - */
 -static bool otg_txfifo_handler(USBDriver *usbp, usbep_t ep) {
 -
 -  /* The TXFIFO is filled until there is space and data to be transmitted.*/
 -  while (true) {
 -    uint32_t n;
 -
 -    /* Transaction end condition.*/
 -    if (usbp->epc[ep]->in_state->txcnt >= usbp->epc[ep]->in_state->txsize) {
 -      usbp->otg->DIEPEMPMSK &= ~DIEPEMPMSK_INEPTXFEM(ep);
 -      return true;
 -    }
 -
 -    /* Number of bytes remaining in current transaction.*/
 -    n = usbp->epc[ep]->in_state->txsize - usbp->epc[ep]->in_state->txcnt;
 -    if (n > usbp->epc[ep]->in_maxsize)
 -      n = usbp->epc[ep]->in_maxsize;
 -
 -    /* Checks if in the TXFIFO there is enough space to accommodate the
 -       next packet.*/
 -    if (((usbp->otg->ie[ep].DTXFSTS & DTXFSTS_INEPTFSAV_MASK) * 4) < n)
 -      return false;
 -
 -#if STM32_USB_OTGFIFO_FILL_BASEPRI
 -    __set_BASEPRI(CORTEX_PRIO_MASK(STM32_USB_OTGFIFO_FILL_BASEPRI));
 -#endif
 -    otg_fifo_write_from_buffer(usbp->otg->FIFO[ep],
 -                               usbp->epc[ep]->in_state->txbuf,
 -                               n);
 -    usbp->epc[ep]->in_state->txbuf += n;
 -    usbp->epc[ep]->in_state->txcnt += n;
 -#if STM32_USB_OTGFIFO_FILL_BASEPRI
 -  __set_BASEPRI(0);
 -#endif
 -  }
 -}
 -
 -/**
 - * @brief   Generic endpoint IN handler.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - * @param[in] ep        endpoint number
 - *
 - * @notapi
 - */
 -static void otg_epin_handler(USBDriver *usbp, usbep_t ep) {
 -  stm32_otg_t *otgp = usbp->otg;
 -  uint32_t epint = otgp->ie[ep].DIEPINT;
 -
 -  otgp->ie[ep].DIEPINT = epint;
 -
 -  if (epint & DIEPINT_TOC) {
 -    /* Timeouts not handled yet, not sure how to handle.*/
 -  }
 -  if ((epint & DIEPINT_XFRC) && (otgp->DIEPMSK & DIEPMSK_XFRCM)) {
 -    /* Transmit transfer complete.*/
 -    USBInEndpointState *isp = usbp->epc[ep]->in_state;
 -
 -    if (isp->txsize < isp->totsize) {
 -      /* In case the transaction covered only part of the total transfer
 -         then another transaction is immediately started in order to
 -         cover the remaining.*/
 -      isp->txsize = isp->totsize - isp->txsize;
 -      isp->txcnt  = 0;
 -      osalSysLockFromISR();
 -      usb_lld_start_in(usbp, ep);
 -      osalSysUnlockFromISR();
 -    }
 -    else {
 -      /* End on IN transfer.*/
 -      _usb_isr_invoke_in_cb(usbp, ep);
 -    }
 -  }
 -  if ((epint & DIEPINT_TXFE) &&
 -      (otgp->DIEPEMPMSK & DIEPEMPMSK_INEPTXFEM(ep))) {
 -#if 0
 -    /* The thread is made ready, it will be scheduled on ISR exit.*/
 -    osalSysLockFromISR();
 -    usbp->txpending |= (1 << ep);
 -    otgp->DIEPEMPMSK &= ~(1 << ep);
 -    osalThreadResumeI(&usbp->wait, MSG_OK);
 -    osalSysUnlockFromISR();
 -#else
 -    /* TX FIFO empty or emptying.*/
 -    otg_txfifo_handler(usbp, ep);
 -#endif
 -  }
 -}
 -
 -/**
 - * @brief   Generic endpoint OUT handler.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - * @param[in] ep        endpoint number
 - *
 - * @notapi
 - */
 -static void otg_epout_handler(USBDriver *usbp, usbep_t ep) {
 -  stm32_otg_t *otgp = usbp->otg;
 -  uint32_t epint = otgp->oe[ep].DOEPINT;
 -
 -  /* Resets all EP IRQ sources.*/
 -  otgp->oe[ep].DOEPINT = epint;
 -
 -  if ((epint & DOEPINT_STUP) && (otgp->DOEPMSK & DOEPMSK_STUPM)) {
 -    /* Setup packets handling, setup packets are handled using a
 -       specific callback.*/
 -    _usb_isr_invoke_setup_cb(usbp, ep);
 -  }
 -
 -  if ((epint & DOEPINT_XFRC) && (otgp->DOEPMSK & DOEPMSK_XFRCM)) {
 -    USBOutEndpointState *osp;
 -
 -    /* OUT state structure pointer for this endpoint.*/
 -    osp = usbp->epc[ep]->out_state;
 -
 -    /* EP0 requires special handling.*/
 -    if (ep == 0) {
 -
 -#if defined(STM32_OTG_SEQUENCE_WORKAROUND)
 -      /* If an OUT transaction end interrupt is processed while the state
 -         machine is not in an OUT state then it is ignored, this is caused
 -         on some devices (L4) apparently injecting spurious data complete
 -         words in the RX FIFO.*/
 -      if ((usbp->ep0state & USB_OUT_STATE) == 0)
 -        return;
 -#endif
 -
 -      /* In case the transaction covered only part of the total transfer
 -         then another transaction is immediately started in order to
 -         cover the remaining.*/
 -      if (((osp->rxcnt % usbp->epc[ep]->out_maxsize) == 0) &&
 -          (osp->rxsize < osp->totsize)) {
 -        osp->rxsize = osp->totsize - osp->rxsize;
 -        osp->rxcnt  = 0;
 -        osalSysLockFromISR();
 -        usb_lld_start_out(usbp, ep);
 -        osalSysUnlockFromISR();
 -        return;
 -      }
 -    }
 -
 -    /* End on OUT transfer.*/
 -    _usb_isr_invoke_out_cb(usbp, ep);
 -  }
 -}
 -
 -/**
 - * @brief   Isochronous IN transfer failed handler.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - *
 - * @notapi
 - */
 -static void otg_isoc_in_failed_handler(USBDriver *usbp) {
 -  usbep_t ep;
 -  stm32_otg_t *otgp = usbp->otg;
 -
 -  for (ep = 0; ep <= usbp->otgparams->num_endpoints; ep++) {
 -    if (((otgp->ie[ep].DIEPCTL & DIEPCTL_EPTYP_MASK) == DIEPCTL_EPTYP_ISO) &&
 -        ((otgp->ie[ep].DIEPCTL & DIEPCTL_EPENA) != 0)) {
 -      /* Endpoint enabled -> ISOC IN transfer failed */
 -      /* Disable endpoint */
 -      otgp->ie[ep].DIEPCTL |= (DIEPCTL_EPDIS | DIEPCTL_SNAK);
 -      while (otgp->ie[ep].DIEPCTL & DIEPCTL_EPENA)
 -        ;
 -
 -      /* Flush FIFO */
 -      otg_txfifo_flush(usbp, ep);
 -
 -      /* Prepare data for next frame */
 -      _usb_isr_invoke_in_cb(usbp, ep);
 -
 -#if 0
 -      /* Pump out data for next frame */
 -      osalSysLockFromISR();
 -      otgp->DIEPEMPMSK &= ~(1 << ep);
 -      usbp->txpending |= (1 << ep);
 -      osalThreadResumeI(&usbp->wait, MSG_OK);
 -      osalSysUnlockFromISR();
 -#else
 -    /* TX FIFO empty or emptying.*/
 -    otg_txfifo_handler(usbp, ep);
 -#endif
 -    }
 -  }
 -}
 -
 -/**
 - * @brief   Isochronous OUT transfer failed handler.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - *
 - * @notapi
 - */
 -static void otg_isoc_out_failed_handler(USBDriver *usbp) {
 -  usbep_t ep;
 -  stm32_otg_t *otgp = usbp->otg;
 -
 -  for (ep = 0; ep <= usbp->otgparams->num_endpoints; ep++) {
 -    if (((otgp->oe[ep].DOEPCTL & DOEPCTL_EPTYP_MASK) == DOEPCTL_EPTYP_ISO) &&
 -        ((otgp->oe[ep].DOEPCTL & DOEPCTL_EPENA) != 0)) {
 -      /* Endpoint enabled -> ISOC OUT transfer failed */
 -      /* Disable endpoint */
 -      /* FIXME: Core stucks here */
 -      /*otgp->oe[ep].DOEPCTL |= (DOEPCTL_EPDIS | DOEPCTL_SNAK);
 -      while (otgp->oe[ep].DOEPCTL & DOEPCTL_EPENA)
 -        ;*/
 -      /* Prepare transfer for next frame */
 -      _usb_isr_invoke_out_cb(usbp, ep);
 -    }
 -  }
 -}
 -
 -/**
 - * @brief   OTG shared ISR.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - *
 - * @notapi
 - */
 -static void usb_lld_serve_interrupt(USBDriver *usbp) {
 -  stm32_otg_t *otgp = usbp->otg;
 -  uint32_t sts, src;
 -
 -  sts  = otgp->GINTSTS;
 -  sts &= otgp->GINTMSK;
 -  otgp->GINTSTS = sts;
 -
 -  /* Reset interrupt handling.*/
 -  if (sts & GINTSTS_USBRST) {
 -#if 0
 -    /* Resetting pending operations.*/
 -    usbp->txpending = 0;
 -#endif
 -    /* Default reset action.*/
 -    _usb_reset(usbp);
 -
 -    /* Preventing execution of more handlers, the core has been reset.*/
 -    return;
 -  }
 -
 -  /* Wake-up handling.*/
 -  if (sts & GINTSTS_WKUPINT) {
 -    /* If clocks are gated off, turn them back on (may be the case if
 -       coming out of suspend mode).*/
 -    if (otgp->PCGCCTL & (PCGCCTL_STPPCLK | PCGCCTL_GATEHCLK)) {
 -      /* Set to zero to un-gate the USB core clocks.*/
 -      otgp->PCGCCTL &= ~(PCGCCTL_STPPCLK | PCGCCTL_GATEHCLK);
 -    }
 -
 -    /* Clear the Remote Wake-up Signaling.*/
 -    otgp->DCTL &= ~DCTL_RWUSIG;
 -
 -    _usb_wakeup(usbp);
 -  }
 -
 -  /* Suspend handling.*/
 -  if (sts & GINTSTS_USBSUSP) {
 -#if 0
 -    /* Resetting pending operations.*/
 -    usbp->txpending = 0;
 -#endif
 -    /* Default suspend action.*/
 -    _usb_suspend(usbp);
 -  }
 -
 -  /* Enumeration done.*/
 -  if (sts & GINTSTS_ENUMDNE) {
 -    /* Full or High speed timing selection.*/
 -    if ((otgp->DSTS & DSTS_ENUMSPD_MASK) == DSTS_ENUMSPD_HS_480) {
 -      otgp->GUSBCFG = (otgp->GUSBCFG & ~(GUSBCFG_TRDT_MASK)) |
 -                      GUSBCFG_TRDT(TRDT_VALUE_HS);
 -    }
 -    else {
 -      otgp->GUSBCFG = (otgp->GUSBCFG & ~(GUSBCFG_TRDT_MASK)) |
 -                      GUSBCFG_TRDT(TRDT_VALUE_FS);
 -    }
 -  }
 -
 -  /* SOF interrupt handling.*/
 -  if (sts & GINTSTS_SOF) {
 -    _usb_isr_invoke_sof_cb(usbp);
 -  }
 -
 -  /* Isochronous IN failed handling */
 -  if (sts & GINTSTS_IISOIXFR) {
 -    otg_isoc_in_failed_handler(usbp);
 -  }
 -
 -  /* Isochronous OUT failed handling */
 -  if (sts & GINTSTS_IISOOXFR) {
 -    otg_isoc_out_failed_handler(usbp);
 -  }
 -
 -  /* RX FIFO not empty handling.*/
 -#if 0
 -  if (sts & GINTSTS_RXFLVL) {
 -    /* The interrupt is masked while the thread has control or it would
 -       be triggered again.*/
 -    osalSysLockFromISR();
 -    otgp->GINTMSK &= ~GINTMSK_RXFLVLM;
 -    osalThreadResumeI(&usbp->wait, MSG_OK);
 -    osalSysUnlockFromISR();
 -  }
 -#else
 -  /* Performing the whole FIFO emptying in the ISR, it is advised to keep
 -     this IRQ at a very low priority level.*/
 -  if ((sts & GINTSTS_RXFLVL) != 0U) {
 -    otg_rxfifo_handler(usbp);
 -  }
 -#endif
 -
 -  /* IN/OUT endpoints event handling.*/
 -  src = otgp->DAINT;
 -  if (sts & GINTSTS_OEPINT) {
 -    if (src & (1 << 16))
 -      otg_epout_handler(usbp, 0);
 -    if (src & (1 << 17))
 -      otg_epout_handler(usbp, 1);
 -    if (src & (1 << 18))
 -      otg_epout_handler(usbp, 2);
 -    if (src & (1 << 19))
 -      otg_epout_handler(usbp, 3);
 -#if USB_MAX_ENDPOINTS >= 4
 -    if (src & (1 << 20))
 -      otg_epout_handler(usbp, 4);
 -#endif
 -#if USB_MAX_ENDPOINTS >= 5
 -    if (src & (1 << 21))
 -      otg_epout_handler(usbp, 5);
 -#endif
 -#if USB_MAX_ENDPOINTS >= 6
 -    if (src & (1 << 22))
 -      otg_epout_handler(usbp, 6);
 -#endif
 -#if USB_MAX_ENDPOINTS >= 7
 -    if (src & (1 << 23))
 -      otg_epout_handler(usbp, 7);
 -#endif
 -#if USB_MAX_ENDPOINTS >= 8
 -    if (src & (1 << 24))
 -      otg_epout_handler(usbp, 8);
 -#endif
 -  }
 -  if (sts & GINTSTS_IEPINT) {
 -    if (src & (1 << 0))
 -      otg_epin_handler(usbp, 0);
 -    if (src & (1 << 1))
 -      otg_epin_handler(usbp, 1);
 -    if (src & (1 << 2))
 -      otg_epin_handler(usbp, 2);
 -    if (src & (1 << 3))
 -      otg_epin_handler(usbp, 3);
 -#if USB_MAX_ENDPOINTS >= 4
 -    if (src & (1 << 4))
 -      otg_epin_handler(usbp, 4);
 -#endif
 -#if USB_MAX_ENDPOINTS >= 5
 -    if (src & (1 << 5))
 -      otg_epin_handler(usbp, 5);
 -#endif
 -#if USB_MAX_ENDPOINTS >= 6
 -    if (src & (1 << 6))
 -      otg_epin_handler(usbp, 6);
 -#endif
 -#if USB_MAX_ENDPOINTS >= 7
 -    if (src & (1 << 7))
 -      otg_epin_handler(usbp, 7);
 -#endif
 -#if USB_MAX_ENDPOINTS >= 8
 -    if (src & (1 << 8))
 -      otg_epin_handler(usbp, 8);
 -#endif
 -  }
 -}
 -
 -/*===========================================================================*/
 -/* Driver interrupt handlers.                                                */
 -/*===========================================================================*/
 -
 -#if STM32_USB_USE_OTG1 || defined(__DOXYGEN__)
 -/**
 - * @brief   OTG1 interrupt handler.
 - *
 - * @isr
 - */
 -OSAL_IRQ_HANDLER(STM32_OTG1_HANDLER) {
 -
 -  OSAL_IRQ_PROLOGUE();
 -
 -  usb_lld_serve_interrupt(&USBD1);
 -
 -  OSAL_IRQ_EPILOGUE();
 -}
 -#endif
 -
 -#if STM32_USB_USE_OTG2 || defined(__DOXYGEN__)
 -/**
 - * @brief   OTG2 interrupt handler.
 - *
 - * @isr
 - */
 -OSAL_IRQ_HANDLER(STM32_OTG2_HANDLER) {
 -
 -  OSAL_IRQ_PROLOGUE();
 -
 -  usb_lld_serve_interrupt(&USBD2);
 -
 -  OSAL_IRQ_EPILOGUE();
 -}
 -#endif
 -
 -/*===========================================================================*/
 -/* Driver exported functions.                                                */
 -/*===========================================================================*/
 -
 -/**
 - * @brief   Low level USB driver initialization.
 - *
 - * @notapi
 - */
 -void usb_lld_init(void) {
 -
 -  /* Driver initialization.*/
 -#if STM32_USB_USE_OTG1
 -  usbObjectInit(&USBD1);
 -#if 0
 -  USBD1.wait      = NULL;
 -#endif
 -  USBD1.otg       = OTG_FS;
 -  USBD1.otgparams = &fsparams;
 -
 -#if 0
 -#if defined(_CHIBIOS_RT_)
 -  USBD1.tr = NULL;
 -  /* Filling the thread working area here because the function
 -     @p chThdCreateI() does not do it.*/
 -#if CH_DBG_FILL_THREADS
 -  {
 -    void *wsp = USBD1.wa_pump;
 -    _thread_memfill((uint8_t *)wsp,
 -                    (uint8_t *)wsp + sizeof (USBD1.wa_pump),
 -                    CH_DBG_STACK_FILL_VALUE);
 -  }
 -#endif /* CH_DBG_FILL_THREADS */
 -#endif /* defined(_CHIBIOS_RT_) */
 -#endif
 -#endif
 -
 -#if STM32_USB_USE_OTG2
 -  usbObjectInit(&USBD2);
 -#if 0
 -  USBD2.wait      = NULL;
 -#endif
 -  USBD2.otg       = OTG_HS;
 -  USBD2.otgparams = &hsparams;
 -
 -#if 0
 -#if defined(_CHIBIOS_RT_)
 -  USBD2.tr = NULL;
 -  /* Filling the thread working area here because the function
 -     @p chThdCreateI() does not do it.*/
 -#if CH_DBG_FILL_THREADS
 -  {
 -    void *wsp = USBD2.wa_pump;
 -    _thread_memfill((uint8_t *)wsp,
 -                    (uint8_t *)wsp + sizeof (USBD2.wa_pump),
 -                    CH_DBG_STACK_FILL_VALUE);
 -  }
 -#endif /* CH_DBG_FILL_THREADS */
 -#endif /* defined(_CHIBIOS_RT_) */
 -#endif
 -#endif
 -}
 -
 -/**
 - * @brief   Configures and activates the USB peripheral.
 - * @note    Starting the OTG cell can be a slow operation carried out with
 - *          interrupts disabled, perform it before starting time-critical
 - *          operations.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - *
 - * @notapi
 - */
 -void usb_lld_start(USBDriver *usbp) {
 -  stm32_otg_t *otgp = usbp->otg;
 -
 -  if (usbp->state == USB_STOP) {
 -    /* Clock activation.*/
 -
 -#if STM32_USB_USE_OTG1
 -    if (&USBD1 == usbp) {
 -      /* OTG FS clock enable and reset.*/
 -      rccEnableOTG_FS(true);
 -      rccResetOTG_FS();
 -
 -      /* Enables IRQ vector.*/
 -      nvicEnableVector(STM32_OTG1_NUMBER, STM32_USB_OTG1_IRQ_PRIORITY);
 -
 -      /* - Forced device mode.
 -         - USB turn-around time = TRDT_VALUE_FS.
 -         - Full Speed 1.1 PHY.*/
 -      otgp->GUSBCFG = GUSBCFG_FDMOD | GUSBCFG_TRDT(TRDT_VALUE_FS) |
 -                      GUSBCFG_PHYSEL;
 -
 -      /* 48MHz 1.1 PHY.*/
 -      otgp->DCFG = 0x02200000 | DCFG_DSPD_FS11;
 -    }
 -#endif
 -
 -#if STM32_USB_USE_OTG2
 -    if (&USBD2 == usbp) {
 -      /* OTG HS clock enable and reset.*/
 -      rccEnableOTG_HS(true);
 -      rccResetOTG_HS();
 -
 -      /* ULPI clock is managed depending on the presence of an external
 -         PHY.*/
 -#if defined(BOARD_OTG2_USES_ULPI)
 -      rccEnableOTG_HSULPI(true);
 -#else
 -      /* Workaround for the problem described here:
 -         http://forum.chibios.org/phpbb/viewtopic.php?f=16&t=1798.*/
 -      rccDisableOTG_HSULPI();
 -#endif
 -
 -      /* Enables IRQ vector.*/
 -      nvicEnableVector(STM32_OTG2_NUMBER, STM32_USB_OTG2_IRQ_PRIORITY);
 -
 -      /* - Forced device mode.
 -         - USB turn-around time = TRDT_VALUE_HS or TRDT_VALUE_FS.*/
 -#if defined(BOARD_OTG2_USES_ULPI)
 -      /* High speed ULPI PHY.*/
 -      otgp->GUSBCFG = GUSBCFG_FDMOD | GUSBCFG_TRDT(TRDT_VALUE_HS) |
 -                      GUSBCFG_SRPCAP | GUSBCFG_HNPCAP;
 -#else
 -      otgp->GUSBCFG = GUSBCFG_FDMOD | GUSBCFG_TRDT(TRDT_VALUE_FS) |
 -                      GUSBCFG_PHYSEL;
 -#endif
 -
 -#if defined(BOARD_OTG2_USES_ULPI)
 -#if STM32_USE_USB_OTG2_HS
 -      /* USB 2.0 High Speed PHY in HS mode.*/
 -      otgp->DCFG = 0x02200000 | DCFG_DSPD_HS;
 -#else
 -      /* USB 2.0 High Speed PHY in FS mode.*/
 -      otgp->DCFG = 0x02200000 | DCFG_DSPD_HS_FS;
 -#endif
 -#else
 -      /* 48MHz 1.1 PHY.*/
 -      otgp->DCFG = 0x02200000 | DCFG_DSPD_FS11;
 -#endif
 -    }
 -#endif
 -
 -#if 0
 -    /* Clearing mask of TXFIFOs to be filled.*/
 -    usbp->txpending = 0;
 -#endif
 -    /* PHY enabled.*/
 -    otgp->PCGCCTL = 0;
 -
 -    /* VBUS sensing and transceiver enabled.*/
 -    otgp->GOTGCTL = GOTGCTL_BVALOEN | GOTGCTL_BVALOVAL;
 -
 -#if defined(BOARD_OTG2_USES_ULPI)
 -#if STM32_USB_USE_OTG1
 -    if (&USBD1 == usbp) {
 -      otgp->GCCFG = GCCFG_INIT_VALUE;
 -    }
 -#endif
 -
 -#if STM32_USB_USE_OTG2
 -    if (&USBD2 == usbp) {
 -      otgp->GCCFG = 0;
 -    }
 -#endif
 -#else
 -    otgp->GCCFG = GCCFG_INIT_VALUE;
 -#endif
 -
 -    /* Soft core reset.*/
 -    otg_core_reset(usbp);
 -
 -    /* Interrupts on TXFIFOs half empty.*/
 -    otgp->GAHBCFG = 0;
 -
 -    /* Endpoints re-initialization.*/
 -    otg_disable_ep(usbp);
 -
 -    /* Clear all pending Device Interrupts, only the USB Reset interrupt
 -       is required initially.*/
 -    otgp->DIEPMSK  = 0;
 -    otgp->DOEPMSK  = 0;
 -    otgp->DAINTMSK = 0;
 -    if (usbp->config->sof_cb == NULL)
 -      otgp->GINTMSK  = GINTMSK_ENUMDNEM | GINTMSK_USBRSTM | GINTMSK_USBSUSPM |
 -                       GINTMSK_ESUSPM | GINTMSK_SRQM | GINTMSK_WKUM |
 -                       GINTMSK_IISOIXFRM | GINTMSK_IISOOXFRM;
 -    else
 -      otgp->GINTMSK  = GINTMSK_ENUMDNEM | GINTMSK_USBRSTM | GINTMSK_USBSUSPM |
 -                       GINTMSK_ESUSPM | GINTMSK_SRQM | GINTMSK_WKUM |
 -                       GINTMSK_IISOIXFRM | GINTMSK_IISOOXFRM |
 -                       GINTMSK_SOFM;
 -
 -    /* Clears all pending IRQs, if any. */
 -    otgp->GINTSTS  = 0xFFFFFFFF;
 -
 -#if 0
 -#if defined(_CHIBIOS_RT_)
 -    /* Creates the data pump thread. Note, it is created only once.*/
 -    if (usbp->tr == NULL) {
 -      thread_descriptor_t usbpump_descriptor = {
 -        "usb_pump",
 -        THD_WORKING_AREA_BASE(usbp->wa_pump),
 -        THD_WORKING_AREA_END(usbp->wa_pump),
 -        STM32_USB_OTG_THREAD_PRIO,
 -        usb_lld_pump,
 -        (void *)usbp
 -      };
 -
 -      usbp->tr = chThdCreateI(&usbpump_descriptor);
 -      chSchRescheduleS();
 -  }
 -#endif
 -#endif
 -    /* Global interrupts enable.*/
 -    otgp->GAHBCFG |= GAHBCFG_GINTMSK;
 -  }
 -}
 -
 -/**
 - * @brief   Deactivates the USB peripheral.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - *
 - * @notapi
 - */
 -void usb_lld_stop(USBDriver *usbp) {
 -  stm32_otg_t *otgp = usbp->otg;
 -
 -  /* If in ready state then disables the USB clock.*/
 -  if (usbp->state != USB_STOP) {
 -
 -    /* Disabling all endpoints in case the driver has been stopped while
 -       active.*/
 -    otg_disable_ep(usbp);
 -
 -#if 0
 -    usbp->txpending = 0;
 -#endif
 -    otgp->DAINTMSK   = 0;
 -    otgp->GAHBCFG    = 0;
 -    otgp->GCCFG      = 0;
 -
 -#if STM32_USB_USE_OTG1
 -    if (&USBD1 == usbp) {
 -      nvicDisableVector(STM32_OTG1_NUMBER);
 -      rccDisableOTG_FS();
 -    }
 -#endif
 -
 -#if STM32_USB_USE_OTG2
 -    if (&USBD2 == usbp) {
 -      nvicDisableVector(STM32_OTG2_NUMBER);
 -      rccDisableOTG_HS();
 -#if defined(BOARD_OTG2_USES_ULPI)
 -      rccDisableOTG_HSULPI()
 -#endif
 -    }
 -#endif
 -  }
 -}
 -
 -/**
 - * @brief   USB low level reset routine.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - *
 - * @notapi
 - */
 -void usb_lld_reset(USBDriver *usbp) {
 -  unsigned i;
 -  stm32_otg_t *otgp = usbp->otg;
 -
 -  /* Flush the Tx FIFO.*/
 -  otg_txfifo_flush(usbp, 0);
 -
 -  /* Endpoint interrupts all disabled and cleared.*/
 -  otgp->DIEPEMPMSK = 0;
 -  otgp->DAINTMSK   = DAINTMSK_OEPM(0) | DAINTMSK_IEPM(0);
 -
 -  /* All endpoints in NAK mode, interrupts cleared.*/
 -  for (i = 0; i <= usbp->otgparams->num_endpoints; i++) {
 -    otgp->ie[i].DIEPCTL = DIEPCTL_SNAK;
 -    otgp->oe[i].DOEPCTL = DOEPCTL_SNAK;
 -    otgp->ie[i].DIEPINT = 0xFFFFFFFF;
 -    otgp->oe[i].DOEPINT = 0xFFFFFFFF;
 -  }
 -
 -  /* Resets the FIFO memory allocator.*/
 -  otg_ram_reset(usbp);
 -
 -  /* Receive FIFO size initialization, the address is always zero.*/
 -  otgp->GRXFSIZ = usbp->otgparams->rx_fifo_size;
 -  otg_rxfifo_flush(usbp);
 -
 -  /* Resets the device address to zero.*/
 -  otgp->DCFG = (otgp->DCFG & ~DCFG_DAD_MASK) | DCFG_DAD(0);
 -
 -  /* Enables also EP-related interrupt sources.*/
 -  otgp->GINTMSK  |= GINTMSK_RXFLVLM | GINTMSK_OEPM  | GINTMSK_IEPM;
 -  otgp->DIEPMSK   = DIEPMSK_TOCM    | DIEPMSK_XFRCM;
 -  otgp->DOEPMSK   = DOEPMSK_STUPM   | DOEPMSK_XFRCM;
 -
 -  /* EP0 initialization, it is a special case.*/
 -  usbp->epc[0] = &ep0config;
 -  otgp->oe[0].DOEPTSIZ = DOEPTSIZ_STUPCNT(3);
 -  otgp->oe[0].DOEPCTL = DOEPCTL_SD0PID | DOEPCTL_USBAEP | DOEPCTL_EPTYP_CTRL |
 -                        DOEPCTL_MPSIZ(ep0config.out_maxsize);
 -  otgp->ie[0].DIEPTSIZ = 0;
 -  otgp->ie[0].DIEPCTL = DIEPCTL_SD0PID | DIEPCTL_USBAEP | DIEPCTL_EPTYP_CTRL |
 -                        DIEPCTL_TXFNUM(0) | DIEPCTL_MPSIZ(ep0config.in_maxsize);
 -  otgp->DIEPTXF0 = DIEPTXF_INEPTXFD(ep0config.in_maxsize / 4) |
 -                   DIEPTXF_INEPTXSA(otg_ram_alloc(usbp,
 -                                                  ep0config.in_maxsize / 4));
 -}
 -
 -/**
 - * @brief   Sets the USB address.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - *
 - * @notapi
 - */
 -void usb_lld_set_address(USBDriver *usbp) {
 -  stm32_otg_t *otgp = usbp->otg;
 -
 -  otgp->DCFG = (otgp->DCFG & ~DCFG_DAD_MASK) | DCFG_DAD(usbp->address);
 -}
 -
 -/**
 - * @brief   Enables an endpoint.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - * @param[in] ep        endpoint number
 - *
 - * @notapi
 - */
 -void usb_lld_init_endpoint(USBDriver *usbp, usbep_t ep) {
 -  uint32_t ctl, fsize;
 -  stm32_otg_t *otgp = usbp->otg;
 -
 -  /* IN and OUT common parameters.*/
 -  switch (usbp->epc[ep]->ep_mode & USB_EP_MODE_TYPE) {
 -  case USB_EP_MODE_TYPE_CTRL:
 -    ctl = DIEPCTL_SD0PID | DIEPCTL_USBAEP | DIEPCTL_EPTYP_CTRL;
 -    break;
 -  case USB_EP_MODE_TYPE_ISOC:
 -    ctl = DIEPCTL_SD0PID | DIEPCTL_USBAEP | DIEPCTL_EPTYP_ISO;
 -    break;
 -  case USB_EP_MODE_TYPE_BULK:
 -    ctl = DIEPCTL_SD0PID | DIEPCTL_USBAEP | DIEPCTL_EPTYP_BULK;
 -    break;
 -  case USB_EP_MODE_TYPE_INTR:
 -    ctl = DIEPCTL_SD0PID | DIEPCTL_USBAEP | DIEPCTL_EPTYP_INTR;
 -    break;
 -  default:
 -    return;
 -  }
 -
 -  /* OUT endpoint activation or deactivation.*/
 -  otgp->oe[ep].DOEPTSIZ = 0;
 -  if (usbp->epc[ep]->out_state != NULL) {
 -    otgp->oe[ep].DOEPCTL = ctl | DOEPCTL_MPSIZ(usbp->epc[ep]->out_maxsize);
 -    otgp->DAINTMSK |= DAINTMSK_OEPM(ep);
 -  }
 -  else {
 -    otgp->oe[ep].DOEPCTL &= ~DOEPCTL_USBAEP;
 -    otgp->DAINTMSK &= ~DAINTMSK_OEPM(ep);
 -  }
 -
 -  /* IN endpoint activation or deactivation.*/
 -  otgp->ie[ep].DIEPTSIZ = 0;
 -  if (usbp->epc[ep]->in_state != NULL) {
 -    /* FIFO allocation for the IN endpoint.*/
 -    fsize = usbp->epc[ep]->in_maxsize / 4;
 -    if (usbp->epc[ep]->in_multiplier > 1)
 -      fsize *= usbp->epc[ep]->in_multiplier;
 -    otgp->DIEPTXF[ep - 1] = DIEPTXF_INEPTXFD(fsize) |
 -                            DIEPTXF_INEPTXSA(otg_ram_alloc(usbp, fsize));
 -    otg_txfifo_flush(usbp, ep);
 -
 -    otgp->ie[ep].DIEPCTL = ctl |
 -                           DIEPCTL_TXFNUM(ep) |
 -                           DIEPCTL_MPSIZ(usbp->epc[ep]->in_maxsize);
 -    otgp->DAINTMSK |= DAINTMSK_IEPM(ep);
 -  }
 -  else {
 -    otgp->DIEPTXF[ep - 1] = 0x02000400; /* Reset value.*/
 -    otg_txfifo_flush(usbp, ep);
 -    otgp->ie[ep].DIEPCTL &= ~DIEPCTL_USBAEP;
 -    otgp->DAINTMSK &= ~DAINTMSK_IEPM(ep);
 -  }
 -}
 -
 -/**
 - * @brief   Disables all the active endpoints except the endpoint zero.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - *
 - * @notapi
 - */
 -void usb_lld_disable_endpoints(USBDriver *usbp) {
 -
 -  /* Resets the FIFO memory allocator.*/
 -  otg_ram_reset(usbp);
 -
 -  /* Disabling all endpoints.*/
 -  otg_disable_ep(usbp);
 -}
 -
 -/**
 - * @brief   Returns the status of an OUT endpoint.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - * @param[in] ep        endpoint number
 - * @return              The endpoint status.
 - * @retval EP_STATUS_DISABLED The endpoint is not active.
 - * @retval EP_STATUS_STALLED  The endpoint is stalled.
 - * @retval EP_STATUS_ACTIVE   The endpoint is active.
 - *
 - * @notapi
 - */
 -usbepstatus_t usb_lld_get_status_out(USBDriver *usbp, usbep_t ep) {
 -  uint32_t ctl;
 -
 -  (void)usbp;
 -
 -  ctl = usbp->otg->oe[ep].DOEPCTL;
 -  if (!(ctl & DOEPCTL_USBAEP))
 -    return EP_STATUS_DISABLED;
 -  if (ctl & DOEPCTL_STALL)
 -    return EP_STATUS_STALLED;
 -  return EP_STATUS_ACTIVE;
 -}
 -
 -/**
 - * @brief   Returns the status of an IN endpoint.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - * @param[in] ep        endpoint number
 - * @return              The endpoint status.
 - * @retval EP_STATUS_DISABLED The endpoint is not active.
 - * @retval EP_STATUS_STALLED  The endpoint is stalled.
 - * @retval EP_STATUS_ACTIVE   The endpoint is active.
 - *
 - * @notapi
 - */
 -usbepstatus_t usb_lld_get_status_in(USBDriver *usbp, usbep_t ep) {
 -  uint32_t ctl;
 -
 -  (void)usbp;
 -
 -  ctl = usbp->otg->ie[ep].DIEPCTL;
 -  if (!(ctl & DIEPCTL_USBAEP))
 -    return EP_STATUS_DISABLED;
 -  if (ctl & DIEPCTL_STALL)
 -    return EP_STATUS_STALLED;
 -  return EP_STATUS_ACTIVE;
 -}
 -
 -/**
 - * @brief   Reads a setup packet from the dedicated packet buffer.
 - * @details This function must be invoked in the context of the @p setup_cb
 - *          callback in order to read the received setup packet.
 - * @pre     In order to use this function the endpoint must have been
 - *          initialized as a control endpoint.
 - * @post    The endpoint is ready to accept another packet.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - * @param[in] ep        endpoint number
 - * @param[out] buf      buffer where to copy the packet data
 - *
 - * @notapi
 - */
 -void usb_lld_read_setup(USBDriver *usbp, usbep_t ep, uint8_t *buf) {
 -
 -  memcpy(buf, usbp->epc[ep]->setup_buf, 8);
 -}
 -
 -/**
 - * @brief   Starts a receive operation on an OUT endpoint.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - * @param[in] ep        endpoint number
 - *
 - * @notapi
 - */
 -void usb_lld_start_out(USBDriver *usbp, usbep_t ep) {
 -  uint32_t pcnt, rxsize;
 -  USBOutEndpointState *osp = usbp->epc[ep]->out_state;
 -
 -  /* Transfer initialization.*/
 -  osp->totsize = osp->rxsize;
 -  if ((ep == 0) && (osp->rxsize > EP0_MAX_OUTSIZE))
 -      osp->rxsize = EP0_MAX_OUTSIZE;
 -
 -  /* Transaction size is rounded to a multiple of packet size because the
 -     following requirement in the RM:
 -     "For OUT transfers, the transfer size field in the endpoint's transfer
 -     size register must be a multiple of the maximum packet size of the
 -     endpoint, adjusted to the Word boundary".*/
 -  pcnt   = (osp->rxsize + usbp->epc[ep]->out_maxsize - 1U) /
 -           usbp->epc[ep]->out_maxsize;
 -  rxsize = (pcnt * usbp->epc[ep]->out_maxsize + 3U) & 0xFFFFFFFCU;
 -
 -  /*Setting up transaction parameters in DOEPTSIZ.*/
 -  usbp->otg->oe[ep].DOEPTSIZ = DOEPTSIZ_STUPCNT(3) | DOEPTSIZ_PKTCNT(pcnt) |
 -                               DOEPTSIZ_XFRSIZ(rxsize);
 -
 -  /* Special case of isochronous endpoint.*/
 -  if ((usbp->epc[ep]->ep_mode & USB_EP_MODE_TYPE) == USB_EP_MODE_TYPE_ISOC) {
 -    /* Odd/even bit toggling for isochronous endpoint.*/
 -    if (usbp->otg->DSTS & DSTS_FNSOF_ODD)
 -      usbp->otg->oe[ep].DOEPCTL |= DOEPCTL_SEVNFRM;
 -    else
 -      usbp->otg->oe[ep].DOEPCTL |= DOEPCTL_SODDFRM;
 -  }
 -
 -  /* Starting operation.*/
 -  usbp->otg->oe[ep].DOEPCTL |= DOEPCTL_EPENA | DOEPCTL_CNAK;
 -}
 -
 -/**
 - * @brief   Starts a transmit operation on an IN endpoint.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - * @param[in] ep        endpoint number
 - *
 - * @notapi
 - */
 -void usb_lld_start_in(USBDriver *usbp, usbep_t ep) {
 -  USBInEndpointState *isp = usbp->epc[ep]->in_state;
 -
 -  /* Transfer initialization.*/
 -  isp->totsize = isp->txsize;
 -  if (isp->txsize == 0) {
 -    /* Special case, sending zero size packet.*/
 -    usbp->otg->ie[ep].DIEPTSIZ = DIEPTSIZ_PKTCNT(1) | DIEPTSIZ_XFRSIZ(0);
 -  }
 -  else {
 -    if ((ep == 0) && (isp->txsize > EP0_MAX_INSIZE))
 -      isp->txsize = EP0_MAX_INSIZE;
 -
 -    /* Normal case.*/
 -    uint32_t pcnt = (isp->txsize + usbp->epc[ep]->in_maxsize - 1) /
 -                    usbp->epc[ep]->in_maxsize;
 -    /* TODO: Support more than one packet per frame for isochronous transfers.*/
 -    usbp->otg->ie[ep].DIEPTSIZ = DIEPTSIZ_MCNT(1) | DIEPTSIZ_PKTCNT(pcnt) |
 -                                 DIEPTSIZ_XFRSIZ(isp->txsize);
 -  }
 -
 -  /* Special case of isochronous endpoint.*/
 -  if ((usbp->epc[ep]->ep_mode & USB_EP_MODE_TYPE) == USB_EP_MODE_TYPE_ISOC) {
 -    /* Odd/even bit toggling.*/
 -    if (usbp->otg->DSTS & DSTS_FNSOF_ODD)
 -      usbp->otg->ie[ep].DIEPCTL |= DIEPCTL_SEVNFRM;
 -    else
 -      usbp->otg->ie[ep].DIEPCTL |= DIEPCTL_SODDFRM;
 -  }
 -
 -  /* Starting operation.*/
 -  usbp->otg->ie[ep].DIEPCTL |= DIEPCTL_EPENA | DIEPCTL_CNAK;
 -  usbp->otg->DIEPEMPMSK |= DIEPEMPMSK_INEPTXFEM(ep);
 -}
 -
 -/**
 - * @brief   Brings an OUT endpoint in the stalled state.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - * @param[in] ep        endpoint number
 - *
 - * @notapi
 - */
 -void usb_lld_stall_out(USBDriver *usbp, usbep_t ep) {
 -
 -  usbp->otg->oe[ep].DOEPCTL |= DOEPCTL_STALL;
 -}
 -
 -/**
 - * @brief   Brings an IN endpoint in the stalled state.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - * @param[in] ep        endpoint number
 - *
 - * @notapi
 - */
 -void usb_lld_stall_in(USBDriver *usbp, usbep_t ep) {
 -
 -  usbp->otg->ie[ep].DIEPCTL |= DIEPCTL_STALL;
 -}
 -
 -/**
 - * @brief   Brings an OUT endpoint in the active state.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - * @param[in] ep        endpoint number
 - *
 - * @notapi
 - */
 -void usb_lld_clear_out(USBDriver *usbp, usbep_t ep) {
 -
 -  usbp->otg->oe[ep].DOEPCTL &= ~DOEPCTL_STALL;
 -}
 -
 -/**
 - * @brief   Brings an IN endpoint in the active state.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - * @param[in] ep        endpoint number
 - *
 - * @notapi
 - */
 -void usb_lld_clear_in(USBDriver *usbp, usbep_t ep) {
 -
 -  usbp->otg->ie[ep].DIEPCTL &= ~DIEPCTL_STALL;
 -}
 -
 -#if 0
 -/**
 - * @brief   USB data transfer loop.
 - * @details This function must be executed by a system thread in order to
 - *          make the USB driver work.
 - * @note    The data copy part of the driver is implemented in this thread
 - *          in order to not perform heavy tasks within interrupt handlers.
 - *
 - * @param[in] p         pointer to the @p USBDriver object
 - *
 - * @special
 - */
 -void usb_lld_pump(void *p) {
 -  USBDriver *usbp = (USBDriver *)p;
 -  stm32_otg_t *otgp = usbp->otg;
 -
 -  osalSysLock();
 -  while (true) {
 -    usbep_t ep;
 -    uint32_t epmask;
 -
 -    /* Nothing to do, going to sleep.*/
 -    if ((usbp->state == USB_STOP) ||
 -        ((usbp->txpending == 0)/* && !(otgp->GINTSTS & GINTSTS_RXFLVL)*/)) {
 -/*      otgp->GINTMSK |= GINTMSK_RXFLVLM;*/
 -      osalThreadSuspendS(&usbp->wait);
 -    }
 -    osalSysUnlock();
 -
 -    /* Checks if there are TXFIFOs to be filled.*/
 -    for (ep = 0; ep <= usbp->otgparams->num_endpoints; ep++) {
 -
 -#if 0
 -      /* Empties the RX FIFO.*/
 -      while (otgp->GINTSTS & GINTSTS_RXFLVL) {
 -        otg_rxfifo_handler(usbp);
 -      }
 -#endif
 -
 -      epmask = (1 << ep);
 -      if (usbp->txpending & epmask) {
 -        bool done;
 -
 -        osalSysLock();
 -        /* USB interrupts are globally *suspended* because the peripheral
 -           does not allow any interference during the TX FIFO filling
 -           operation.
 -           Synopsys document: DesignWare Cores USB 2.0 Hi-Speed On-The-Go (OTG)
 -             "The application has to finish writing one complete packet before
 -              switching to a different channel/endpoint FIFO. Violating this
 -              rule results in an error.".*/
 -        otgp->GAHBCFG &= ~GAHBCFG_GINTMSK;
 -        usbp->txpending &= ~epmask;
 -        osalSysUnlock();
 -
 -        done = otg_txfifo_handler(usbp, ep);
 -
 -        osalSysLock();
 -        otgp->GAHBCFG |= GAHBCFG_GINTMSK;
 -        if (!done)
 -          otgp->DIEPEMPMSK |= epmask;
 -        osalSysUnlock();
 -      }
 -    }
 -    osalSysLock();
 -  }
 -}
 -#endif
 -
 -#endif /* HAL_USE_USB */
 -
 -/** @} */
 diff --git a/os/hal/ports/STM32/LLD/OTGv1/hal_usb_lld_alt.h b/os/hal/ports/STM32/LLD/OTGv1/hal_usb_lld_alt.h deleted file mode 100644 index 56ef3792c..000000000 --- a/os/hal/ports/STM32/LLD/OTGv1/hal_usb_lld_alt.h +++ /dev/null @@ -1,628 +0,0 @@ -/*
 -    ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio
 -
 -    Licensed under the Apache License, Version 2.0 (the "License");
 -    you may not use this file except in compliance with the License.
 -    You may obtain a copy of the License at
 -
 -        http://www.apache.org/licenses/LICENSE-2.0
 -
 -    Unless required by applicable law or agreed to in writing, software
 -    distributed under the License is distributed on an "AS IS" BASIS,
 -    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 -    See the License for the specific language governing permissions and
 -    limitations under the License.
 -*/
 -
 -/**
 - * @file    OTGv1/hal_usb_lld.h
 - * @brief   STM32 USB subsystem low level driver header.
 - *
 - * @addtogroup USB
 - * @{
 - */
 -
 -#ifndef HAL_USB_LLD_H
 -#define HAL_USB_LLD_H
 -
 -#if HAL_USE_USB || defined(__DOXYGEN__)
 -
 -#include "stm32_otg.h"
 -
 -/*===========================================================================*/
 -/* Driver constants.                                                         */
 -/*===========================================================================*/
 -
 -/**
 - * @brief   Status stage handling method.
 - */
 -#define USB_EP0_STATUS_STAGE                USB_EP0_STATUS_STAGE_SW
 -
 -/**
 - * @brief   The address can be changed immediately upon packet reception.
 - */
 -#define USB_SET_ADDRESS_MODE                USB_EARLY_SET_ADDRESS
 -
 -/**
 - * @brief   Method for set address acknowledge.
 - */
 -#define USB_SET_ADDRESS_ACK_HANDLING        USB_SET_ADDRESS_ACK_SW
 -
 -/*===========================================================================*/
 -/* Driver pre-compile time settings.                                         */
 -/*===========================================================================*/
 -
 -/**
 - * @brief   OTG1 driver enable switch.
 - * @details If set to @p TRUE the support for OTG_FS is included.
 - * @note    The default is @p FALSE
 - */
 -#if !defined(STM32_USB_USE_OTG1) || defined(__DOXYGEN__)
 -#define STM32_USB_USE_OTG1                  FALSE
 -#endif
 -
 -/**
 - * @brief   OTG2 driver enable switch.
 - * @details If set to @p TRUE the support for OTG_HS is included.
 - * @note    The default is @p FALSE.
 - */
 -#if !defined(STM32_USB_USE_OTG2) || defined(__DOXYGEN__)
 -#define STM32_USB_USE_OTG2                  FALSE
 -#endif
 -
 -/**
 - * @brief   OTG1 interrupt priority level setting.
 - */
 -#if !defined(STM32_USB_OTG1_IRQ_PRIORITY) || defined(__DOXYGEN__)
 -#define STM32_USB_OTG1_IRQ_PRIORITY         14
 -#endif
 -
 -/**
 - * @brief   OTG2 interrupt priority level setting.
 - */
 -#if !defined(STM32_USB_OTG2_IRQ_PRIORITY) || defined(__DOXYGEN__)
 -#define STM32_USB_OTG2_IRQ_PRIORITY         14
 -#endif
 -
 -/**
 - * @brief   OTG1 RX shared FIFO size.
 - * @note    Must be a multiple of 4.
 - */
 -#if !defined(STM32_USB_OTG1_RX_FIFO_SIZE) || defined(__DOXYGEN__)
 -#define STM32_USB_OTG1_RX_FIFO_SIZE         512
 -#endif
 -
 -/**
 - * @brief   OTG2 RX shared FIFO size.
 - * @note    Must be a multiple of 4.
 - */
 -#if !defined(STM32_USB_OTG2_RX_FIFO_SIZE) || defined(__DOXYGEN__)
 -#define STM32_USB_OTG2_RX_FIFO_SIZE         1024
 -#endif
 -
 -/**
 - * @brief   Enables HS mode on OTG2 else FS mode.
 - * @note    The default is @p TRUE.
 - * @note    Has effect only if @p BOARD_OTG2_USES_ULPI is defined.
 - */
 -#if !defined(STM32_USE_USB_OTG2_HS) || defined(__DOXYGEN__)
 -#define STM32_USE_USB_OTG2_HS               TRUE
 -#endif
 -
 -/**
 - * @brief   Dedicated data pump threads priority.
 - */
 -#if !defined(STM32_USB_OTG_THREAD_PRIO) || defined(__DOXYGEN__)
 -#define STM32_USB_OTG_THREAD_PRIO           LOWPRIO
 -#endif
 -
 -/**
 - * @brief   Dedicated data pump threads stack size.
 - */
 -#if !defined(STM32_USB_OTG_THREAD_STACK_SIZE) || defined(__DOXYGEN__)
 -#define STM32_USB_OTG_THREAD_STACK_SIZE     128
 -#endif
 -
 -/**
 - * @brief   Exception priority level during TXFIFOs operations.
 - * @note    Because an undocumented silicon behavior the operation of
 - *          copying a packet into a TXFIFO must not be interrupted by
 - *          any other operation on the OTG peripheral.
 - *          This parameter represents the priority mask during copy
 - *          operations. The default value only allows to call USB
 - *          functions from callbacks invoked from USB ISR handlers.
 - *          If you need to invoke USB functions from other handlers
 - *          then raise this priority mast to the same level of the
 - *          handler you need to use.
 - * @note    The value zero means disabled, when disabled calling USB
 - *          functions is only safe from thread level or from USB
 - *          callbacks.
 - */
 -#if !defined(STM32_USB_OTGFIFO_FILL_BASEPRI) || defined(__DOXYGEN__)
 -#define STM32_USB_OTGFIFO_FILL_BASEPRI      0
 -#endif
 -
 -/**
 - * @brief   Host wake-up procedure duration.
 - */
 -#if !defined(USB_HOST_WAKEUP_DURATION) || defined(__DOXYGEN__)
 -#define USB_HOST_WAKEUP_DURATION            2
 -#endif
 -
 -/*===========================================================================*/
 -/* Derived constants and error checks.                                       */
 -/*===========================================================================*/
 -
 -/* Registry checks.*/
 -#if !defined(STM32_OTG_STEPPING)
 -#error "STM32_OTG_STEPPING not defined in registry"
 -#endif
 -
 -#if (STM32_OTG_STEPPING < 1) || (STM32_OTG_STEPPING > 2)
 -#error "unsupported STM32_OTG_STEPPING"
 -#endif
 -
 -#if !defined(STM32_HAS_OTG1) || !defined(STM32_HAS_OTG2)
 -#error "STM32_HAS_OTGx not defined in registry"
 -#endif
 -
 -#if STM32_HAS_OTG1 && !defined(STM32_OTG1_ENDPOINTS)
 -#error "STM32_OTG1_ENDPOINTS not defined in registry"
 -#endif
 -
 -#if STM32_HAS_OTG2 && !defined(STM32_OTG2_ENDPOINTS)
 -#error "STM32_OTG2_ENDPOINTS not defined in registry"
 -#endif
 -
 -#if STM32_HAS_OTG1 && !defined(STM32_OTG1_FIFO_MEM_SIZE)
 -#error "STM32_OTG1_FIFO_MEM_SIZE not defined in registry"
 -#endif
 -
 -#if STM32_HAS_OTG2 && !defined(STM32_OTG2_FIFO_MEM_SIZE)
 -#error "STM32_OTG2_FIFO_MEM_SIZE not defined in registry"
 -#endif
 -
 -#if (STM32_USB_USE_OTG1 && !defined(STM32_OTG1_HANDLER)) ||                 \
 -    (STM32_USB_USE_OTG2 && !defined(STM32_OTG2_HANDLER))
 -#error "STM32_OTGx_HANDLER not defined in registry"
 -#endif
 -
 -#if (STM32_USB_USE_OTG1 && !defined(STM32_OTG1_NUMBER)) ||                  \
 -    (STM32_USB_USE_OTG2 && !defined(STM32_OTG2_NUMBER))
 -#error "STM32_OTGx_NUMBER not defined in registry"
 -#endif
 -
 -/**
 - * @brief   Maximum endpoint address.
 - */
 -#if (STM32_HAS_OTG2 && STM32_USB_USE_OTG2) || defined(__DOXYGEN__)
 -#if (STM32_OTG1_ENDPOINTS < STM32_OTG2_ENDPOINTS) || defined(__DOXYGEN__)
 -#define USB_MAX_ENDPOINTS                   STM32_OTG2_ENDPOINTS
 -#else
 -#define USB_MAX_ENDPOINTS                   STM32_OTG1_ENDPOINTS
 -#endif
 -#else
 -#define USB_MAX_ENDPOINTS                   STM32_OTG1_ENDPOINTS
 -#endif
 -
 -#if STM32_USB_USE_OTG1 && !STM32_HAS_OTG1
 -#error "OTG1 not present in the selected device"
 -#endif
 -
 -#if STM32_USB_USE_OTG2 && !STM32_HAS_OTG2
 -#error "OTG2 not present in the selected device"
 -#endif
 -
 -#if !STM32_USB_USE_OTG1 && !STM32_USB_USE_OTG2
 -#error "USB driver activated but no USB peripheral assigned"
 -#endif
 -
 -#if STM32_USB_USE_OTG1 &&                                                \
 -    !OSAL_IRQ_IS_VALID_PRIORITY(STM32_USB_OTG1_IRQ_PRIORITY)
 -#error "Invalid IRQ priority assigned to OTG1"
 -#endif
 -
 -#if STM32_USB_USE_OTG2 &&                                                \
 -    !OSAL_IRQ_IS_VALID_PRIORITY(STM32_USB_OTG2_IRQ_PRIORITY)
 -#error "Invalid IRQ priority assigned to OTG2"
 -#endif
 -
 -#if (STM32_USB_OTG1_RX_FIFO_SIZE & 3) != 0
 -#error "OTG1 RX FIFO size must be a multiple of 4"
 -#endif
 -
 -#if (STM32_USB_OTG2_RX_FIFO_SIZE & 3) != 0
 -#error "OTG2 RX FIFO size must be a multiple of 4"
 -#endif
 -
 -#if defined(STM32F2XX) || defined(STM32F4XX) || defined(STM32F7XX)
 -#define STM32_USBCLK                        STM32_PLL48CLK
 -#elif defined(STM32F10X_CL)
 -#define STM32_USBCLK                        STM32_OTGFSCLK
 -#elif defined(STM32L4XX)
 -#define STM32_USBCLK                        STM32_48CLK
 -#else
 -#error "unsupported STM32 platform for OTG functionality"
 -#endif
 -
 -#if STM32_USBCLK != 48000000
 -#error "the USB OTG driver requires a 48MHz clock"
 -#endif
 -
 -#if (USB_HOST_WAKEUP_DURATION < 2) || (USB_HOST_WAKEUP_DURATION > 15)
 -#error "invalid USB_HOST_WAKEUP_DURATION setting, it must be between 2 and 15"
 -#endif
 -
 -/*===========================================================================*/
 -/* Driver data structures and types.                                         */
 -/*===========================================================================*/
 -
 -/**
 - * @brief   Peripheral-specific parameters block.
 - */
 -typedef struct {
 -  uint32_t                      rx_fifo_size;
 -  uint32_t                      otg_ram_size;
 -  uint32_t                      num_endpoints;
 -} stm32_otg_params_t;
 -
 -/**
 - * @brief   Type of an IN endpoint state structure.
 - */
 -typedef struct {
 -  /**
 -   * @brief   Requested transmit transfer size.
 -   */
 -  size_t                        txsize;
 -  /**
 -   * @brief   Transmitted bytes so far.
 -   */
 -  size_t                        txcnt;
 -  /**
 -   * @brief   Pointer to the transmission linear buffer.
 -   */
 -  const uint8_t                 *txbuf;
 -#if (USB_USE_WAIT == TRUE) || defined(__DOXYGEN__)
 -  /**
 -   * @brief   Waiting thread.
 -   */
 -  thread_reference_t            thread;
 -#endif
 -  /* End of the mandatory fields.*/
 -  /**
 -   * @brief   Total transmit transfer size.
 -   */
 -  size_t                        totsize;
 -} USBInEndpointState;
 -
 -/**
 - * @brief   Type of an OUT endpoint state structure.
 - */
 -typedef struct {
 -  /**
 -   * @brief   Requested receive transfer size.
 -   */
 -  size_t                        rxsize;
 -  /**
 -   * @brief   Received bytes so far.
 -   */
 -  size_t                        rxcnt;
 -  /**
 -   * @brief   Pointer to the receive linear buffer.
 -   */
 -  uint8_t                       *rxbuf;
 -#if (USB_USE_WAIT == TRUE) || defined(__DOXYGEN__)
 -  /**
 -   * @brief   Waiting thread.
 -   */
 -  thread_reference_t            thread;
 -#endif
 -  /* End of the mandatory fields.*/
 -  /**
 -   * @brief   Total receive transfer size.
 -   */
 -  size_t                        totsize;
 -} USBOutEndpointState;
 -
 -/**
 - * @brief   Type of an USB endpoint configuration structure.
 - * @note    Platform specific restrictions may apply to endpoints.
 - */
 -typedef struct {
 -  /**
 -   * @brief   Type and mode of the endpoint.
 -   */
 -  uint32_t                      ep_mode;
 -  /**
 -   * @brief   Setup packet notification callback.
 -   * @details This callback is invoked when a setup packet has been
 -   *          received.
 -   * @post    The application must immediately call @p usbReadPacket() in
 -   *          order to access the received packet.
 -   * @note    This field is only valid for @p USB_EP_MODE_TYPE_CTRL
 -   *          endpoints, it should be set to @p NULL for other endpoint
 -   *          types.
 -   */
 -  usbepcallback_t               setup_cb;
 -  /**
 -   * @brief   IN endpoint notification callback.
 -   * @details This field must be set to @p NULL if callback is not required.
 -   */
 -  usbepcallback_t               in_cb;
 -  /**
 -   * @brief   OUT endpoint notification callback.
 -   * @details This field must be set to @p NULL if callback is not required.
 -   */
 -  usbepcallback_t               out_cb;
 -  /**
 -   * @brief   IN endpoint maximum packet size.
 -   * @details This field must be set to zero if the IN endpoint is not used.
 -   */
 -  uint16_t                      in_maxsize;
 -  /**
 -   * @brief   OUT endpoint maximum packet size.
 -   * @details This field must be set to zero if the OUT endpoint is not used.
 -   */
 -  uint16_t                      out_maxsize;
 -  /**
 -   * @brief   @p USBEndpointState associated to the IN endpoint.
 -   * @details This field must be set to @p NULL if the IN endpoint is not
 -   *          used.
 -   */
 -  USBInEndpointState            *in_state;
 -  /**
 -   * @brief   @p USBEndpointState associated to the OUT endpoint.
 -   * @details This field must be set to @p NULL if the OUT endpoint is not
 -   *          used.
 -   */
 -  USBOutEndpointState           *out_state;
 -  /* End of the mandatory fields.*/
 -  /**
 -   * @brief   Determines the space allocated for the TXFIFO as multiples of
 -   *          the packet size (@p in_maxsize). Note that zero is interpreted
 -   *          as one for simplicity and robustness.
 -   */
 -  uint16_t                      in_multiplier;
 -  /**
 -   * @brief   Pointer to a buffer for setup packets.
 -   * @details Setup packets require a dedicated 8-bytes buffer, set this
 -   *          field to @p NULL for non-control endpoints.
 -   */
 -  uint8_t                       *setup_buf;
 -} USBEndpointConfig;
 -
 -/**
 - * @brief   Type of an USB driver configuration structure.
 - */
 -typedef struct {
 -  /**
 -   * @brief   USB events callback.
 -   * @details This callback is invoked when an USB driver event is registered.
 -   */
 -  usbeventcb_t                  event_cb;
 -  /**
 -   * @brief   Device GET_DESCRIPTOR request callback.
 -   * @note    This callback is mandatory and cannot be set to @p NULL.
 -   */
 -  usbgetdescriptor_t            get_descriptor_cb;
 -  /**
 -   * @brief   Requests hook callback.
 -   * @details This hook allows to be notified of standard requests or to
 -   *          handle non standard requests.
 -   */
 -  usbreqhandler_t               requests_hook_cb;
 -  /**
 -   * @brief   Start Of Frame callback.
 -   */
 -  usbcallback_t                 sof_cb;
 -  /* End of the mandatory fields.*/
 -} USBConfig;
 -
 -/**
 - * @brief   Structure representing an USB driver.
 - */
 -struct USBDriver {
 -  /**
 -   * @brief   Driver state.
 -   */
 -  usbstate_t                    state;
 -  /**
 -   * @brief   Current configuration data.
 -   */
 -  const USBConfig               *config;
 -  /**
 -   * @brief   Bit map of the transmitting IN endpoints.
 -   */
 -  uint16_t                      transmitting;
 -  /**
 -   * @brief   Bit map of the receiving OUT endpoints.
 -   */
 -  uint16_t                      receiving;
 -  /**
 -   * @brief   Active endpoints configurations.
 -   */
 -  const USBEndpointConfig       *epc[USB_MAX_ENDPOINTS + 1];
 -  /**
 -   * @brief   Fields available to user, it can be used to associate an
 -   *          application-defined handler to an IN endpoint.
 -   * @note    The base index is one, the endpoint zero does not have a
 -   *          reserved element in this array.
 -   */
 -  void                          *in_params[USB_MAX_ENDPOINTS];
 -  /**
 -   * @brief   Fields available to user, it can be used to associate an
 -   *          application-defined handler to an OUT endpoint.
 -   * @note    The base index is one, the endpoint zero does not have a
 -   *          reserved element in this array.
 -   */
 -  void                          *out_params[USB_MAX_ENDPOINTS];
 -  /**
 -   * @brief   Endpoint 0 state.
 -   */
 -  usbep0state_t                 ep0state;
 -  /**
 -   * @brief   Next position in the buffer to be transferred through endpoint 0.
 -   */
 -  uint8_t                       *ep0next;
 -  /**
 -   * @brief   Number of bytes yet to be transferred through endpoint 0.
 -   */
 -  size_t                        ep0n;
 -  /**
 -   * @brief   Endpoint 0 end transaction callback.
 -   */
 -  usbcallback_t                 ep0endcb;
 -  /**
 -   * @brief   Setup packet buffer.
 -   */
 -  uint8_t                       setup[8];
 -  /**
 -   * @brief   Current USB device status.
 -   */
 -  uint16_t                      status;
 -  /**
 -   * @brief   Assigned USB address.
 -   */
 -  uint8_t                       address;
 -  /**
 -   * @brief   Current USB device configuration.
 -   */
 -  uint8_t                       configuration;
 -  /**
 -   * @brief   State of the driver when a suspend happened.
 -   */
 -  usbstate_t                    saved_state;
 -#if defined(USB_DRIVER_EXT_FIELDS)
 -  USB_DRIVER_EXT_FIELDS
 -#endif
 -  /* End of the mandatory fields.*/
 -  /**
 -   * @brief   Pointer to the OTG peripheral associated to this driver.
 -   */
 -  stm32_otg_t                   *otg;
 -  /**
 -   * @brief   Peripheral-specific parameters.
 -   */
 -  const stm32_otg_params_t      *otgparams;
 -  /**
 -   * @brief   Pointer to the next address in the packet memory.
 -   */
 -  uint32_t                      pmnext;
 -#if 0
 -  /**
 -   * @brief   Mask of TXFIFOs to be filled by the pump thread.
 -   */
 -  uint32_t                      txpending;
 -  /**
 -   * @brief   Pointer to the thread when it is sleeping or @p NULL.
 -   */
 -  thread_reference_t            wait;
 -#if defined(_CHIBIOS_RT_)
 -  /**
 -   * @brief   Pointer to the thread.
 -   */
 -  thread_reference_t            tr;
 -  /**
 -   * @brief   Working area for the dedicated data pump thread;
 -   */
 -  THD_WORKING_AREA(wa_pump, STM32_USB_OTG_THREAD_STACK_SIZE);
 -#endif
 -#endif
 -};
 -
 -/*===========================================================================*/
 -/* Driver macros.                                                            */
 -/*===========================================================================*/
 -
 -/**
 - * @brief   Returns the exact size of a receive transaction.
 - * @details The received size can be different from the size specified in
 - *          @p usbStartReceiveI() because the last packet could have a size
 - *          different from the expected one.
 - * @pre     The OUT endpoint must have been configured in transaction mode
 - *          in order to use this function.
 - *
 - * @param[in] usbp      pointer to the @p USBDriver object
 - * @param[in] ep        endpoint number
 - * @return              Received data size.
 - *
 - * @notapi
 - */
 -#define usb_lld_get_transaction_size(usbp, ep)                              \
 -  ((usbp)->epc[ep]->out_state->rxcnt)
 -
 -/**
 - * @brief   Connects the USB device.
 - *
 - * @notapi
 - */
 -#if (STM32_OTG_STEPPING == 1) || defined(__DOXYGEN__)
 -#define usb_lld_connect_bus(usbp) ((usbp)->otg->GCCFG |= GCCFG_VBUSBSEN)
 -#else
 -#define usb_lld_connect_bus(usbp) ((usbp)->otg->DCTL &= ~DCTL_SDIS)
 -#endif
 -
 -/**
 - * @brief   Disconnect the USB device.
 - *
 - * @notapi
 - */
 -#if (STM32_OTG_STEPPING == 1) || defined(__DOXYGEN__)
 -#define usb_lld_disconnect_bus(usbp) ((usbp)->otg->GCCFG &= ~GCCFG_VBUSBSEN)
 -#else
 -#define usb_lld_disconnect_bus(usbp) ((usbp)->otg->DCTL |= DCTL_SDIS)
 -#endif
 -
 -/**
 - * @brief   Start of host wake-up procedure.
 - *
 - * @notapi
 - */
 -#define usb_lld_wakeup_host(usbp)                                           \
 -  do{                                                                       \
 -    (usbp)->otg->DCTL |= DCTL_RWUSIG;                                       \
 -    osalThreadSleepMilliseconds(USB_HOST_WAKEUP_DURATION);                  \
 -    (usbp)->otg->DCTL &= ~DCTL_RWUSIG;                                      \
 -  } while (false)
 -
 -/*===========================================================================*/
 -/* External declarations.                                                    */
 -/*===========================================================================*/
 -
 -#if STM32_USB_USE_OTG1 && !defined(__DOXYGEN__)
 -extern USBDriver USBD1;
 -#endif
 -
 -#if STM32_USB_USE_OTG2 && !defined(__DOXYGEN__)
 -extern USBDriver USBD2;
 -#endif
 -
 -#ifdef __cplusplus
 -extern "C" {
 -#endif
 -  void usb_lld_init(void);
 -  void usb_lld_start(USBDriver *usbp);
 -  void usb_lld_stop(USBDriver *usbp);
 -  void usb_lld_reset(USBDriver *usbp);
 -  void usb_lld_set_address(USBDriver *usbp);
 -  void usb_lld_init_endpoint(USBDriver *usbp, usbep_t ep);
 -  void usb_lld_disable_endpoints(USBDriver *usbp);
 -  usbepstatus_t usb_lld_get_status_in(USBDriver *usbp, usbep_t ep);
 -  usbepstatus_t usb_lld_get_status_out(USBDriver *usbp, usbep_t ep);
 -  void usb_lld_read_setup(USBDriver *usbp, usbep_t ep, uint8_t *buf);
 -  void usb_lld_start_out(USBDriver *usbp, usbep_t ep);
 -  void usb_lld_start_in(USBDriver *usbp, usbep_t ep);
 -  void usb_lld_stall_out(USBDriver *usbp, usbep_t ep);
 -  void usb_lld_stall_in(USBDriver *usbp, usbep_t ep);
 -  void usb_lld_clear_out(USBDriver *usbp, usbep_t ep);
 -  void usb_lld_clear_in(USBDriver *usbp, usbep_t ep);
 -  void usb_lld_pump(void *p);
 -#ifdef __cplusplus
 -}
 -#endif
 -
 -#endif /* HAL_USE_USB */
 -
 -#endif /* HAL_USB_LLD_H */
 -
 -/** @} */
  | 
