Merge branch 'master' into rng

Added haltest

Conflicts:
	os/hal/hal.mk
	os/hal/include/hal_community.h
	os/hal/src/hal_community.c

45 files changed, 11923 insertions, 24 deletions

diff --git a/os/hal/boards/NRF51-DK/board.c b/os/hal/boards/NRF51-DK/board.c
index e976e56..c5237d7 100644
--- a/os/hal/boards/NRF51-DK/board.c
+++ b/os/hal/boards/NRF51-DK/board.c
@@ -1,5 +1,6 @@
 /*
     Copyright (C) 2015 Fabio Utzig
+                  2016 Stéphane D'Alu / Bruno Remond
 
     Licensed under the Apache License, Version 2.0 (the "License");
     you may not use this file except in compliance with the License.
@@ -17,6 +18,14 @@
 #include "hal.h"
 
 #if HAL_USE_PAL || defined(__DOXYGEN__)
+
+/* RAM Banks
+ *  (Values are defined in Nordic gcc_startup_nrf51.s)
+ */
+#define NRF_POWER_RAMON_ADDRESS 0x40000524
+#define NRF_POWER_RAMONB_ADDRESS 0x40000554
+#define NRF_POWER_RAMONx_RAMxON_ONMODE_Msk 0x3
+
 /**
  * @brief   PAL setup.
  * @details Digital I/O ports static configuration as defined in @p board.h.
@@ -68,6 +77,9 @@ const PALConfig pal_default_config =
  */
 void __early_init(void)
 {
+  /* Make sure ALL RAM banks are powered on */
+  *(uint32_t *)NRF_POWER_RAMON_ADDRESS  |= NRF_POWER_RAMONx_RAMxON_ONMODE_Msk;
+  *(uint32_t *)NRF_POWER_RAMONB_ADDRESS |= NRF_POWER_RAMONx_RAMxON_ONMODE_Msk;
 }
 
 /**
diff --git a/os/hal/boards/NRF51-DK/board.h b/os/hal/boards/NRF51-DK/board.h
index 5e0c738..04c8daf 100644
--- a/os/hal/boards/NRF51-DK/board.h
+++ b/os/hal/boards/NRF51-DK/board.h
@@ -22,7 +22,8 @@
 #define BOARD_NAME              "nRF51 DK"
 
 /* Board oscillators-related settings. */
-#define XTAL_VALUE              16000000
+#define NRF51_XTAL_VALUE        16000000
+#define NRF51_LFCLK_SOURCE      1
 
 /* GPIO pins. */
 #define BTN1           17
diff --git a/os/hal/boards/NRF51-DK/board.mk b/os/hal/boards/NRF51-DK/board.mk
index 4d20da3..9619bd4 100644
--- a/os/hal/boards/NRF51-DK/board.mk
+++ b/os/hal/boards/NRF51-DK/board.mk
@@ -3,3 +3,9 @@ BOARDSRC = ${CHIBIOS_CONTRIB}/os/hal/boards/NRF51-DK/board.c
 
 # Required include directories
 BOARDINC = ${CHIBIOS_CONTRIB}/os/hal/boards/NRF51-DK
+
+# Flash
+JLINK_DEVICE    = nrf51422
+JLINK_PRE_FLASH = w4 4001e504 1
+JLINK_ERASE_ALL = w4 4001e504 2\nw4 4001e50c 1\nsleep 100
+
diff --git a/os/hal/boards/WVSHARE_BLE400/board.h b/os/hal/boards/WVSHARE_BLE400/board.h
index 57578b5..536b7f6 100644
--- a/os/hal/boards/WVSHARE_BLE400/board.h
+++ b/os/hal/boards/WVSHARE_BLE400/board.h
@@ -22,7 +22,7 @@
 #define BOARD_NAME              "WvShare BLE400"
 
 /* Board oscillators-related settings. */
-#define XTAL_VALUE              16000000
+#define NRF51_XTAL_VALUE        16000000
 
 /* GPIO pins. */
 #define KEY1           16
diff --git a/os/hal/hal.mk b/os/hal/hal.mk
index 8ac84dd..8dc09f7 100644
--- a/os/hal/hal.mk
+++ b/os/hal/hal.mk
@@ -5,6 +5,17 @@ HALSRC += ${CHIBIOS_CONTRIB}/os/hal/src/hal_community.c \
           ${CHIBIOS_CONTRIB}/os/hal/src/onewire.c \

           ${CHIBIOS_CONTRIB}/os/hal/src/eicu.c \

           ${CHIBIOS_CONTRIB}/os/hal/src/crc.c \

-          ${CHIBIOS_CONTRIB}/os/hal/src/rng.c

+          ${CHIBIOS_CONTRIB}/os/hal/src/rng.c \

+          ${CHIBIOS_CONTRIB}/os/hal/src/usbh.c \

+          ${CHIBIOS_CONTRIB}/os/hal/src/usbh/usbh_debug.c \

+          ${CHIBIOS_CONTRIB}/os/hal/src/usbh/usbh_desciter.c \

+          ${CHIBIOS_CONTRIB}/os/hal/src/usbh/usbh_hub.c \

+          ${CHIBIOS_CONTRIB}/os/hal/src/usbh/usbh_msd.c \

+          ${CHIBIOS_CONTRIB}/os/hal/src/usbh/usbh_ftdi.c \

+          ${CHIBIOS_CONTRIB}/os/hal/src/usbh/usbh_uvc.c \

+	      ${CHIBIOS_CONTRIB}/os/hal/src/ee24xx.c \

+          ${CHIBIOS_CONTRIB}/os/hal/src/ee25xx.c \

+          ${CHIBIOS_CONTRIB}/os/hal/src/eeprom.c \

+          ${CHIBIOS_CONTRIB}/os/hal/src/timcap.c \

 

 HALINC += ${CHIBIOS_CONTRIB}/os/hal/include

diff --git a/os/hal/include/ee24xx.h b/os/hal/include/ee24xx.h
new file mode 100644
index 0000000..ab12fd1
--- /dev/null
+++ b/os/hal/include/ee24xx.h
@@ -0,0 +1,64 @@
+/*
+  Copyright 2012 Uladzimir Pylinski aka barthess.
+  You may use this work without restrictions, as long as this notice is included.
+  The work is provided "as is" without warranty of any kind, neither express nor implied.
+*/
+
+#ifndef EE24XX_H
+#define EE24XX_H
+
+#include "hal.h"
+
+#if defined(HAL_USE_EEPROM) && HAL_USE_EEPROM && EEPROM_USE_EE24XX
+
+#define EEPROM_DEV_24XX 24
+
+/**
+ * @extends EepromFileConfig
+ */
+typedef struct {
+  _eeprom_file_config_data
+  /**
+   * Driver connected to IC.
+   */
+  I2CDriver     *i2cp;
+  /**
+   * Address of IC on I2C bus.
+   */
+  i2caddr_t     addr;
+  /**
+   * Pointer to write buffer. The safest size is (pagesize + 2)
+   */
+  uint8_t       *write_buf;
+} I2CEepromFileConfig;
+
+/**
+ * @brief   @p I2CEepromFileStream specific data.
+ */
+#define _eeprom_file_stream_data_i2c                                       \
+  _eeprom_file_stream_data
+
+/**
+ * @extends EepromFileStream
+ *
+ * @brief   EEPROM file stream driver class for I2C device.
+ */
+typedef struct {
+  const struct EepromFileStreamVMT *vmt;
+  _eeprom_file_stream_data_i2c
+  /* Overwritten parent data member. */
+  const I2CEepromFileConfig *cfg;
+} I2CEepromFileStream;
+
+
+/**
+ * Open I2C EEPROM IC as file and return pointer to the file stream object
+ * @note      Fucntion allways successfully open file. All checking makes
+ *            in read/write functions.
+ */
+#define I2CEepromFileOpen(efs, eepcfg, eepdev) \
+  EepromFileOpen((EepromFileStream *)efs, (EepromFileConfig *)eepcfg, eepdev);
+
+#endif /* #if defined(EEPROM_USE_EE24XX) && EEPROM_USE_EE24XX */
+
+#endif // EE24XX_H
diff --git a/os/hal/include/ee25xx.h b/os/hal/include/ee25xx.h
new file mode 100644
index 0000000..fc2ad6f
--- /dev/null
+++ b/os/hal/include/ee25xx.h
@@ -0,0 +1,63 @@
+/*
+  Copyright 2012 Uladzimir Pylinski aka barthess.
+  You may use this work without restrictions, as long as this notice is included.
+  The work is provided "as is" without warranty of any kind, neither express nor implied.
+*/
+
+#ifndef EE25XX_H
+#define EE25XX_H
+
+#include "hal.h"
+
+#if defined(HAL_USE_EEPROM) && HAL_USE_EEPROM && EEPROM_USE_EE25XX
+
+#define EEPROM_DEV_25XX 25
+
+/**
+ * @extends EepromFileConfig
+ */
+typedef struct {
+  _eeprom_file_config_data
+  /**
+   * Driver connected to IC.
+   */
+  SPIDriver       *spip;
+  /**
+   * Config associated with SPI driver.
+   */
+  const SPIConfig *spicfg;
+} SPIEepromFileConfig;
+
+/**
+ * @brief   @p SPIEepromFileStream specific data.
+ */
+#define _eeprom_file_stream_data_spi                                       \
+  _eeprom_file_stream_data
+
+/**
+ * @extends EepromFileStream
+ *
+ * @brief   EEPROM file stream driver class for SPI device.
+ */
+typedef struct {
+  const struct EepromFileStreamVMT *vmt;
+  _eeprom_file_stream_data_spi
+  /* Overwritten parent data member. */
+  const SPIEepromFileConfig *cfg;
+} SPIEepromFileStream;
+
+/**
+ * Open SPI EEPROM IC as file and return pointer to the file stream object
+ * @note      Fucntion allways successfully open file. All checking makes
+ *            in read/write functions.
+ */
+EepromFileStream *SPIEepromFileOpen(SPIEepromFileStream *efs,
+                                    const SPIEepromFileConfig *eepcfg,
+                                    const EepromDevice *eepdev);
+
+#define SPIEepromFileOpen(efs, eepcfg, eepdev) \
+  EepromFileOpen((EepromFileStream *)efs, (EepromFileConfig *)eepcfg, eepdev);
+
+#endif /* #if defined(EEPROM_USE_EE25XX) && EEPROM_USE_EE25XX */
+
+#endif // EE25XX_H
diff --git a/os/hal/include/eeprom.h b/os/hal/include/eeprom.h
new file mode 100644
index 0000000..b0fd360
--- /dev/null
+++ b/os/hal/include/eeprom.h
@@ -0,0 +1,147 @@
+/*
+  Copyright (c) 2013 Timon Wong
+
+  Permission is hereby granted, free of charge, to any person obtaining a copy
+  of this software and associated documentation files (the "Software"), to deal
+  in the Software without restriction, including without limitation the rights
+  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+  copies of the Software, and to permit persons to whom the Software is
+  furnished to do so, subject to the following conditions:
+
+  The above copyright notice and this permission notice shall be included in all
+  copies or substantial portions of the Software.
+
+  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+  THE SOFTWARE.
+*/
+
+/*
+  Copyright 2012 Uladzimir Pylinski aka barthess.
+  You may use this work without restrictions, as long as this notice is included.
+  The work is provided "as is" without warranty of any kind, neither express nor implied.
+*/
+
+#ifndef __EEPROM_H__
+#define __EEPROM_H__
+
+#include "ch.h"
+#include "hal.h"
+
+#ifndef HAL_USE_EEPROM
+#define HAL_USE_EEPROM FALSE
+#endif
+
+#ifndef EEPROM_USE_EE25XX
+#define EEPROM_USE_EE25XX FALSE
+#endif
+
+#ifndef EEPROM_USE_EE24XX
+#define EEPROM_USE_EE24XX FALSE
+#endif
+
+#if HAL_USE_EEPROM
+
+#if EEPROM_USE_EE25XX && EEPROM_USE_EE24XX
+#define EEPROM_TABLE_SIZE 2
+#elif EEPROM_USE_EE25XX || EEPROM_USE_EE24XX
+#define EEPROM_TABLE_SIZE 1
+#else
+#error "No EEPROM device selected!"
+#endif
+
+#if EEPROM_USE_EE25XX && !HAL_USE_SPI
+#error "25xx enabled but SPI driver is disabled!"
+#endif
+
+#if EEPROM_USE_EE24XX && !HAL_USE_I2C
+#error "24xx enabled but I2C driver is disabled!"
+#endif
+
+#define _eeprom_file_config_data                                            \
+  /* Lower barrier of file in EEPROM memory array. */                       \
+  uint32_t        barrier_low;                                              \
+  /* Higher barrier of file in EEPROM memory array. */                      \
+  uint32_t        barrier_hi;                                               \
+  /* Size of memory array in bytes. */                                      \
+  uint32_t        size;                                                     \
+  /* Size of single page in bytes. */                                       \
+  uint16_t        pagesize;                                                 \
+  /* Time needed by IC for single byte/page writing. */                     \
+  systime_t       write_time;
+  
+typedef uint32_t fileoffset_t;
+
+typedef struct {
+  _eeprom_file_config_data
+} EepromFileConfig;
+
+/**
+ * @brief   @p EepromFileStream specific data.
+ */
+#define _eeprom_file_stream_data                                            \
+  _base_sequential_stream_data                                                    \
+  uint32_t                    errors;                                       \
+  uint32_t                    position;                                     \
+
+/**
+ * @extends BaseFileStreamVMT
+ *
+ * @brief   @p EepromFileStream virtual methods table.
+ */
+struct EepromFileStreamVMT {
+  _file_stream_methods
+};
+
+/**
+ * @extends BaseFileStream
+ *
+ * @brief   EEPROM file stream driver class.
+ * @details This class extends @p BaseFileStream by adding some fields.
+ */
+typedef struct {
+  /** @brief Virtual Methods Table.*/
+  const struct EepromFileStreamVMT *vmt;
+  _eeprom_file_stream_data
+  /** pointer to config object, must be overwritten by all derived classes.*/
+  const EepromFileConfig *cfg;
+} EepromFileStream;
+
+/**
+ * @brief   Low level device descriptor.
+ */
+typedef struct {
+  const uint8_t                       id;
+  const struct EepromFileStreamVMT   *efsvmt;
+} EepromDevice;
+
+const EepromDevice *EepromFindDevice(uint8_t id);
+
+EepromFileStream *EepromFileOpen(EepromFileStream *efs,
+                                 const EepromFileConfig *eepcfg,
+                                 const EepromDevice *eepdev);
+
+uint8_t  EepromReadByte(EepromFileStream *efs);
+uint16_t EepromReadHalfword(EepromFileStream *efs);
+uint32_t EepromReadWord(EepromFileStream *efs);
+size_t EepromWriteByte(EepromFileStream *efs, uint8_t data);
+size_t EepromWriteHalfword(EepromFileStream *efs, uint16_t data);
+size_t EepromWriteWord(EepromFileStream *efs, uint32_t data);
+
+msg_t eepfs_getsize(void *ip);
+msg_t eepfs_getposition(void *ip);
+msg_t eepfs_lseek(void *ip, fileoffset_t offset);
+msg_t eepfs_close(void *ip);
+msg_t eepfs_geterror(void *ip);
+msg_t eepfs_put(void *ip, uint8_t b);
+msg_t eepfs_get(void *ip);
+
+#include "ee24xx.h"
+#include "ee25xx.h"
+
+#endif /* #if defined(HAL_USE_EEPROM) && HAL_USE_EEPROM */
+#endif /* __EEPROM_H__ */
diff --git a/os/hal/include/hal_community.h b/os/hal/include/hal_community.h
index d11e4c9..144e495 100644
--- a/os/hal/include/hal_community.h
+++ b/os/hal/include/hal_community.h
@@ -33,10 +33,13 @@
 #include "nand.h"

 #include "eicu.h"

 #include "rng.h"

+#include "usbh.h"

+#include "timcap.h"

 

 /* Complex drivers.*/

 #include "onewire.h"

 #include "crc.h"

+#include "eeprom.h"

 

 /*===========================================================================*/

 /* Driver constants.                                                         */

diff --git a/os/hal/include/timcap.h b/os/hal/include/timcap.h
new file mode 100644
index 0000000..a848783
--- /dev/null
+++ b/os/hal/include/timcap.h
@@ -0,0 +1,210 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010,
+                 2011,2012,2013 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS/RT is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    timcap.h
+ * @brief   TIMCAP Driver macros and structures.
+ *
+ * @addtogroup TIMCAP
+ * @{
+ */
+
+#ifndef _TIMCAP_H_
+#define _TIMCAP_H_
+
+#include "ch.h"
+#include "hal.h"
+
+#ifndef HAL_USE_TIMCAP
+#define HAL_USE_TIMCAP FALSE
+#endif
+
+#if HAL_USE_TIMCAP || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Driver constants.                                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver data structures and types.                                         */
+/*===========================================================================*/
+
+/**
+ * @brief   Driver state machine possible states.
+ */
+typedef enum {
+  TIMCAP_UNINIT = 0,                   /**< Not initialized.                   */
+  TIMCAP_STOP = 1,                     /**< Stopped.                           */
+  TIMCAP_READY = 2,                    /**< Ready.                             */
+  TIMCAP_WAITING = 3,                  /**< Waiting first edge.                */
+  TIMCAP_ACTIVE = 4,                   /**< Active cycle phase.                */
+  TIMCAP_IDLE = 5,                     /**< Idle cycle phase.                  */
+} timcapstate_t;
+
+/**
+ * @brief   Type of a structure representing an TIMCAP driver.
+ */
+typedef struct TIMCAPDriver TIMCAPDriver;
+
+
+/**
+ * @brief   TIMCAP notification callback type.
+ *
+ * @param[in] timcapp      pointer to a @p TIMCAPDriver object
+ */
+typedef void (*timcapcallback_t)(TIMCAPDriver *timcapp);
+
+#include "timcap_lld.h"
+
+/*===========================================================================*/
+/* Driver macros.                                                            */
+/*===========================================================================*/
+
+/**
+ * @name    Macro Functions
+ * @{
+ */
+/**
+ * @brief   Enables the input capture.
+ *
+ * @param[in] timcapp      pointer to the @p TIMCAPDriver object
+ *
+ * @iclass
+ */
+#define timcapEnableI(timcapp) timcap_lld_enable(timcapp)
+
+/**
+ * @brief   Disables the input capture.
+ *
+ * @param[in] timcapp      pointer to the @p TIMCAPDriver object
+ *
+ * @iclass
+ */
+#define timcapDisableI(timcapp) timcap_lld_disable(timcapp)
+
+
+
+
+/** @} */
+
+/**
+ * @name    Low Level driver helper macros
+ * @{
+ */
+
+
+/**
+ * @brief   Common ISR code, TIMCAP channel 1 event.
+ *
+ * @param[in] timcapp      pointer to the @p TIMCAPDriver object
+ *
+ * @notapi
+ */
+#define _timcap_isr_invoke_channel1_cb(timcapp) {                                   \
+  timcapstate_t previous_state = (timcapp)->state;                                \
+  (timcapp)->state = TIMCAP_ACTIVE;                                               \
+  if (previous_state != TIMCAP_WAITING)                                        \
+    (timcapp)->config->capture_cb_array[0](timcapp);                                        \
+}
+
+/**
+ * @brief   Common ISR code, TIMCAP channel 2 event.
+ *
+ * @param[in] timcapp      pointer to the @p TIMCAPDriver object
+ *
+ * @notapi
+ */
+#define _timcap_isr_invoke_channel2_cb(timcapp) {                                   \
+  timcapstate_t previous_state = (timcapp)->state;                                \
+  (timcapp)->state = TIMCAP_ACTIVE;                                               \
+  if (previous_state != TIMCAP_WAITING)                                        \
+    (timcapp)->config->capture_cb_array[1](timcapp);                                        \
+}
+
+/**
+ * @brief   Common ISR code, TIMCAP channel 3 event.
+ *
+ * @param[in] timcapp      pointer to the @p TIMCAPDriver object
+ *
+ * @notapi
+ */
+#define _timcap_isr_invoke_channel3_cb(timcapp) {                                   \
+  timcapstate_t previous_state = (timcapp)->state;                                \
+  (timcapp)->state = TIMCAP_ACTIVE;                                               \
+  if (previous_state != TIMCAP_WAITING)                                        \
+    (timcapp)->config->capture_cb_array[2](timcapp);                                        \
+}
+
+/**
+ * @brief   Common ISR code, TIMCAP channel 4 event.
+ *
+ * @param[in] timcapp      pointer to the @p TIMCAPDriver object
+ *
+ * @notapi
+ */
+#define _timcap_isr_invoke_channel4_cb(timcapp) {                                   \
+  timcapstate_t previous_state = (timcapp)->state;                                \
+  (timcapp)->state = TIMCAP_ACTIVE;                                               \
+  if (previous_state != TIMCAP_WAITING)                                        \
+    (timcapp)->config->capture_cb_array[3](timcapp);                                        \
+}
+
+/**
+ * @brief   Common ISR code, TIMCAP timer overflow event.
+ *
+ * @param[in] timcapp      pointer to the @p TIMCAPDriver object
+ *
+ * @notapi
+ */
+#define _timcap_isr_invoke_overflow_cb(timcapp) {                                 \
+  (timcapp)->config->overflow_cb(timcapp);                                        \
+}
+/** @} */
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+  void timcapInit(void);
+  void timcapObjectInit(TIMCAPDriver *timcapp);
+  void timcapStart(TIMCAPDriver *timcapp, const TIMCAPConfig *config);
+  void timcapStop(TIMCAPDriver *timcapp);
+  void timcapEnable(TIMCAPDriver *timcapp);
+  void timcapDisable(TIMCAPDriver *timcapp);
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* HAL_USE_TIMCAP */
+
+#endif /* _TIMCAP_H_ */
+
+/** @} */
diff --git a/os/hal/include/usbh.h b/os/hal/include/usbh.h
new file mode 100644
index 0000000..2f8f3dd
--- /dev/null
+++ b/os/hal/include/usbh.h
@@ -0,0 +1,439 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

+

+    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.

+*/

+

+#ifndef USBH_H_

+#define USBH_H_

+

+#include "hal.h"

+

+#ifndef HAL_USE_USBH

+#define HAL_USE_USBH FALSE

+#endif

+

+#ifndef HAL_USBH_USE_FTDI

+#define HAL_USBH_USE_FTDI FALSE

+#endif

+

+#ifndef HAL_USBH_USE_HUB

+#define HAL_USBH_USE_HUB FALSE

+#endif

+

+#ifndef HAL_USBH_USE_MSD

+#define HAL_USBH_USE_MSD FALSE

+#endif

+

+#ifndef HAL_USBH_USE_UVC

+#define HAL_USBH_USE_UVC FALSE

+#endif

+

+#if HAL_USE_USBH

+

+#include "osal.h"

+#include "usbh/list.h"

+#include "usbh/defs.h"

+

+/* TODO:

+ *

+ * - Integrate VBUS power switching functionality to the API.

+ *

+ */

+

+

+/*===========================================================================*/

+/* Derived constants and error checks.                                       */

+/*===========================================================================*/

+

+#if !HAL_USBH_USE_HUB

+#define USBH_MAX_ADDRESSES				1

+#else

+#define USBH_MAX_ADDRESSES				(HAL_USBHHUB_MAX_PORTS + 1)

+#endif

+

+enum usbh_status {

+	USBH_STATUS_STOPPED = 0,

+	USBH_STATUS_STARTED,

+	USBH_STATUS_SUSPENDED,

+};

+

+enum usbh_devstatus {

+	USBH_DEVSTATUS_DISCONNECTED = 0,

+	USBH_DEVSTATUS_ATTACHED,

+	USBH_DEVSTATUS_CONNECTED,

+	USBH_DEVSTATUS_DEFAULT,

+	USBH_DEVSTATUS_ADDRESS,

+	USBH_DEVSTATUS_CONFIGURED,

+};

+

+enum usbh_devspeed {

+	USBH_DEVSPEED_LOW = 0,

+	USBH_DEVSPEED_FULL,

+	USBH_DEVSPEED_HIGH,

+};

+

+enum usbh_epdir {

+	USBH_EPDIR_IN		= 0x80,

+	USBH_EPDIR_OUT		= 0

+};

+

+enum usbh_eptype {

+	USBH_EPTYPE_CTRL	= 0,

+	USBH_EPTYPE_ISO		= 1,

+	USBH_EPTYPE_BULK	= 2,

+	USBH_EPTYPE_INT		= 3,

+};

+

+enum usbh_epstatus {

+	USBH_EPSTATUS_UNINITIALIZED = 0,

+	USBH_EPSTATUS_CLOSED,

+	USBH_EPSTATUS_OPEN,

+	USBH_EPSTATUS_HALTED,

+};

+

+enum usbh_urbstatus {

+	USBH_URBSTATUS_UNINITIALIZED = 0,

+	USBH_URBSTATUS_INITIALIZED,

+	USBH_URBSTATUS_PENDING,

+//	USBH_URBSTATUS_QUEUED,

+	USBH_URBSTATUS_ERROR,

+	USBH_URBSTATUS_TIMEOUT,

+	USBH_URBSTATUS_CANCELLED,

+	USBH_URBSTATUS_STALL,

+	USBH_URBSTATUS_DISCONNECTED,

+//	USBH_URBSTATUS_EPCLOSED,

+	USBH_URBSTATUS_OK,

+};

+

+/*===========================================================================*/

+/* Driver data structures and types.                                         */

+/*===========================================================================*/

+

+/* forward declarations */

+typedef struct USBHDriver USBHDriver;

+typedef struct usbh_port usbh_port_t;

+typedef	struct usbh_device usbh_device_t;

+typedef struct usbh_ep usbh_ep_t;

+typedef struct usbh_urb usbh_urb_t;

+typedef struct usbh_baseclassdriver usbh_baseclassdriver_t;

+typedef struct usbh_classdriverinfo usbh_classdriverinfo_t;

+#if HAL_USBH_USE_HUB

+typedef struct USBHHubDriver USBHHubDriver;

+#endif

+

+/* typedefs */

+typedef enum usbh_status usbh_status_t;

+typedef enum usbh_devspeed usbh_devspeed_t;

+typedef enum usbh_devstatus usbh_devstatus_t;

+typedef enum usbh_epdir usbh_epdir_t;

+typedef enum usbh_eptype usbh_eptype_t;

+typedef enum usbh_epstatus usbh_epstatus_t;

+typedef enum usbh_urbstatus usbh_urbstatus_t;

+typedef uint16_t usbh_portstatus_t;

+typedef uint16_t usbh_portcstatus_t;

+typedef void (*usbh_completion_cb)(usbh_urb_t *);

+

+/* include the low level driver; the required definitions are above */

+#include "usbh_lld.h"

+

+#define USBH_DEFINE_BUFFER(type, name)	USBH_LLD_DEFINE_BUFFER(type, name)

+

+struct usbh_urb {

+	usbh_ep_t *ep;

+

+	void *userData;

+	usbh_completion_cb callback;

+

+	const void *setup_buff;

+	void *buff;

+	uint32_t requestedLength;

+	uint32_t actualLength;

+

+	usbh_urbstatus_t status;

+

+	thread_reference_t waitingThread;

+	thread_reference_t abortingThread;

+

+	/* Low level part */

+	_usbh_urb_ll_data

+};

+

+struct usbh_ep {

+	usbh_device_t		*device;

+	usbh_ep_t	 		*next;

+

+	usbh_epstatus_t		status;

+	uint8_t 			address;

+	bool				in;

+	usbh_eptype_t		type;

+	uint16_t			wMaxPacketSize;

+	uint8_t				bInterval;

+

+	/* debug */

+	const char			*name;

+

+	/* Low-level part */

+	_usbh_ep_ll_data

+};

+

+struct usbh_device {

+	USBHDriver *host;	/* shortcut to host */

+

+	usbh_ep_t ctrl;

+	usbh_ep_t *endpoints;

+

+	usbh_baseclassdriver_t *drivers;

+

+	uint16_t langID0;

+

+	usbh_devstatus_t status;

+	usbh_devspeed_t speed;

+

+	USBH_DEFINE_BUFFER(usbh_device_descriptor_t, devDesc);

+	unsigned char align_bytes[2];

+	USBH_DEFINE_BUFFER(usbh_config_descriptor_t, basicConfigDesc);

+

+	uint8_t *fullConfigurationDescriptor;

+	uint8_t keepFullCfgDesc;

+

+	uint8_t address;

+	uint8_t bConfiguration;

+

+	/* Low level part */

+	_usbh_device_ll_data

+};

+

+

+struct usbh_port {

+#if HAL_USBH_USE_HUB

+	USBHHubDriver *hub;

+#endif

+

+	usbh_portstatus_t status;

+	usbh_portcstatus_t c_status;

+

+	usbh_port_t *next;

+

+	uint8_t number;

+

+	usbh_device_t device;

+

+	/* Low level part */

+	_usbh_port_ll_data

+};

+

+struct USBHDriver {

+	usbh_status_t status;

+	uint8_t address_bitmap[(USBH_MAX_ADDRESSES + 7) / 8];

+

+	usbh_port_t rootport;

+

+#if HAL_USBH_USE_HUB

+	struct list_head hubs;

+#endif

+

+	/* Low level part */

+	_usbhdriver_ll_data

+

+#if USBH_DEBUG_ENABLE

+	/* debug */

+	uint8_t dbg_buff[USBH_DEBUG_BUFFER];

+	THD_WORKING_AREA(waDebug, 512);

+	input_queue_t iq;

+#endif

+};

+

+

+

+/*===========================================================================*/

+/* External declarations.                                                    */

+/*===========================================================================*/

+

+#if STM32_USBH_USE_OTG1

+extern USBHDriver USBHD1;

+#endif

+

+#if STM32_USBH_USE_OTG2

+extern USBHDriver USBHD2;

+#endif

+

+

+/*===========================================================================*/

+/* Main driver API.		                                                     */

+/*===========================================================================*/

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+

+	/* Main functions */

+	void usbhObjectInit(USBHDriver *usbh);

+	void usbhInit(void);

+	void usbhStart(USBHDriver *usbh);

+	void usbhStop(USBHDriver *usbh);

+	void usbhSuspend(USBHDriver *usbh);

+	void usbhResume(USBHDriver *usbh);

+

+	/* Device-related */

+#if	USBH_DEBUG_ENABLE && USBH_DEBUG_ENABLE_INFO

+	void usbhDevicePrintInfo(usbh_device_t *dev);

+	void usbhDevicePrintConfiguration(const uint8_t *descriptor, uint16_t rem);

+#else

+#	define usbhDevicePrintInfo(dev) do {} while(0)

+#	define usbhDevicePrintConfiguration(descriptor, rem) do {} while(0)

+#endif

+	bool usbhDeviceReadString(usbh_device_t *dev, char *dest, uint8_t size,

+			uint8_t index, uint16_t langID);

+	static inline usbh_port_t *usbhDeviceGetPort(usbh_device_t *dev) {

+		return container_of(dev, usbh_port_t, device);

+	}

+

+	/* Synchronous API */

+	usbh_urbstatus_t usbhBulkTransfer(usbh_ep_t *ep,

+			void *data,

+			uint32_t len,

+			uint32_t *actual_len,

+			systime_t timeout);

+	usbh_urbstatus_t usbhControlRequest(usbh_device_t *dev,

+			uint8_t bmRequestType,

+			uint8_t bRequest,

+			uint16_t wValue,

+			uint16_t wIndex,

+			uint16_t wLength,

+			uint8_t *buff);

+	usbh_urbstatus_t usbhControlRequestExtended(usbh_device_t *dev,

+			const usbh_control_request_t *req,

+			uint8_t *buff,

+			uint32_t *actual_len,

+			systime_t timeout);

+

+	/* Standard request helpers */

+	bool usbhStdReqGetDeviceDescriptor(usbh_device_t *dev,

+			uint16_t wLength,

+			uint8_t *buf);

+	bool usbhStdReqGetConfigurationDescriptor(usbh_device_t *dev,

+			uint8_t index,

+			uint16_t wLength,

+			uint8_t *buf);

+	bool usbhStdReqGetStringDescriptor(usbh_device_t *dev,

+			uint8_t index,

+			uint16_t langID,

+			uint16_t wLength,

+			uint8_t *buf);

+	bool usbhStdReqSetInterface(usbh_device_t *dev,

+			uint8_t bInterfaceNumber,

+			uint8_t bAlternateSetting);

+	bool usbhStdReqGetInterface(usbh_device_t *dev,

+			uint8_t bInterfaceNumber,

+			uint8_t *bAlternateSetting);

+

+	/* Endpoint/pipe management */

+	void usbhEPObjectInit(usbh_ep_t *ep, usbh_device_t *dev, const usbh_endpoint_descriptor_t *desc);

+	static inline void usbhEPOpen(usbh_ep_t *ep) {

+		osalDbgCheck(ep != 0);

+		osalSysLock();

+		osalDbgAssert(ep->status == USBH_EPSTATUS_CLOSED, "invalid state");

+		usbh_lld_ep_open(ep);

+		ep->next = ep->device->endpoints;

+		ep->device->endpoints = ep;

+		osalSysUnlock();

+	}

+	static inline void usbhEPCloseS(usbh_ep_t *ep) {

+		osalDbgCheck(ep != 0);

+		osalDbgCheckClassS();

+		osalDbgAssert(ep->status != USBH_EPSTATUS_UNINITIALIZED, "invalid state");

+		if (ep->status == USBH_EPSTATUS_CLOSED) {

+			osalOsRescheduleS();

+			return;

+		}

+		usbh_lld_ep_close(ep);

+	}

+	static inline void usbhEPClose(usbh_ep_t *ep) {

+		osalSysLock();

+		usbhEPCloseS(ep);

+		osalSysUnlock();

+	}

+	static inline void usbhEPResetI(usbh_ep_t *ep) {

+		osalDbgCheckClassI();

+		osalDbgCheck(ep != NULL);

+		usbh_lld_epreset(ep);

+	}

+	static inline bool usbhEPIsPeriodic(usbh_ep_t *ep) {

+		osalDbgCheck(ep != NULL);

+		return (ep->type & 1) != 0;

+	}

+	static inline bool usbhURBIsBusy(usbh_urb_t *urb) {

+		osalDbgCheck(urb != NULL);

+		return (urb->status == USBH_URBSTATUS_PENDING);

+	}

+	static inline void usbhEPSetName(usbh_ep_t *ep, const char *name) {

+		ep->name = name;

+	}

+

+	/* URB management */

+	void usbhURBObjectInit(usbh_urb_t *urb, usbh_ep_t *ep, usbh_completion_cb callback,

+			void *user, void *buff, uint32_t len);

+	void usbhURBObjectResetI(usbh_urb_t *urb);

+	void usbhURBSubmitI(usbh_urb_t *urb);

+	bool usbhURBCancelI(usbh_urb_t *urb);

+	msg_t usbhURBSubmitAndWaitS(usbh_urb_t *urb, systime_t timeout);

+	void usbhURBCancelAndWaitS(usbh_urb_t *urb);

+	msg_t usbhURBWaitTimeoutS(usbh_urb_t *urb, systime_t timeout);

+

+	/* Main loop */

+	void usbhMainLoop(USBHDriver *usbh);

+

+#ifdef __cplusplus

+}

+#endif

+

+

+/*===========================================================================*/

+/* Class driver definitions and API.                                         */

+/*===========================================================================*/

+

+typedef struct usbh_classdriver_vmt usbh_classdriver_vmt_t;

+struct usbh_classdriver_vmt {

+	usbh_baseclassdriver_t *(*load)(usbh_device_t *dev,	const uint8_t *descriptor, uint16_t rem);

+	void (*unload)(usbh_baseclassdriver_t *drv);

+};

+

+struct usbh_classdriverinfo {

+	int16_t class;

+	int16_t subclass;

+	int16_t protocol;

+	const char *name;

+	const usbh_classdriver_vmt_t *vmt;

+};

+

+#define _usbh_base_classdriver_data		\

+	const usbh_classdriverinfo_t *info;	\

+	usbh_device_t *dev;					\

+	usbh_baseclassdriver_t *next;

+

+struct usbh_baseclassdriver {

+	_usbh_base_classdriver_data

+};

+

+

+/*===========================================================================*/

+/* Helper functions.				                                         */

+/*===========================================================================*/

+#include <usbh/desciter.h>	/* descriptor iterators */

+#include <usbh/debug.h>		/* debug */

+

+#endif

+

+#endif /* USBH_H_ */

diff --git a/os/hal/include/usbh/debug.h b/os/hal/include/usbh/debug.h
new file mode 100644
index 0000000..219eeb1
--- /dev/null
+++ b/os/hal/include/usbh/debug.h
@@ -0,0 +1,44 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

+

+    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.

+*/

+

+

+#ifndef USBH_DEBUG_H_

+#define USBH_DEBUG_H_

+

+#include "usbh.h"

+

+#if HAL_USE_USBH

+

+//TODO: Debug is only for USBHD1, make it generic.

+

+#if USBH_DEBUG_ENABLE

+	void usbDbgPrintf(const char *fmt, ...);

+	void usbDbgPuts(const char *s);

+	void usbDbgInit(USBHDriver *host);

+	void usbDbgReset(void);

+	void usbDbgSystemHalted(void);

+#else

+#define usbDbgPrintf(fmt, ...) do {} while(0)

+#define usbDbgPuts(s) do {} while(0)

+#define usbDbgInit(host) do {} while(0)

+#define usbDbgReset() do {} while(0)

+#define usbDbgSystemHalted() do {} while(0)

+#endif

+

+#endif

+

+#endif /* USBH_DEBUG_H_ */

diff --git a/os/hal/include/usbh/defs.h b/os/hal/include/usbh/defs.h
new file mode 100644
index 0000000..c3d8a9a
--- /dev/null
+++ b/os/hal/include/usbh/defs.h
@@ -0,0 +1,160 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

+

+    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.

+*/

+

+#ifndef USBH_DEFS_H_

+#define USBH_DEFS_H_

+

+#include "hal.h"

+

+#if HAL_USE_USBH

+

+#include "osal.h"

+

+#ifdef __IAR_SYSTEMS_ICC__

+#define PACKED_STRUCT typedef PACKED_VAR struct

+#else

+#define PACKED_STRUCT typedef struct PACKED_VAR

+#endif

+

+PACKED_STRUCT {

+	uint8_t  bLength;

+	uint8_t  bDescriptorType;

+	uint16_t bcdUSB;

+	uint8_t  bDeviceClass;

+	uint8_t  bDeviceSubClass;

+	uint8_t  bDeviceProtocol;

+	uint8_t  bMaxPacketSize0;

+	uint16_t idVendor;

+	uint16_t idProduct;

+	uint16_t bcdDevice;

+	uint8_t  iManufacturer;

+	uint8_t  iProduct;

+	uint8_t  iSerialNumber;

+	uint8_t  bNumConfigurations;

+} usbh_device_descriptor_t;

+#define USBH_DT_DEVICE                   0x01

+#define USBH_DT_DEVICE_SIZE              18

+

+PACKED_STRUCT {

+	uint8_t  bLength;

+	uint8_t  bDescriptorType;

+	uint16_t wTotalLength;

+	uint8_t  bNumInterfaces;

+	uint8_t  bConfigurationValue;

+	uint8_t  iConfiguration;

+	uint8_t  bmAttributes;

+	uint8_t  bMaxPower;

+} usbh_config_descriptor_t;

+#define USBH_DT_CONFIG                   0x02

+#define USBH_DT_CONFIG_SIZE              9

+

+PACKED_STRUCT {

+	uint8_t  bLength;

+	uint8_t  bDescriptorType;

+	uint16_t wData[1];

+} usbh_string_descriptor_t;

+#define USBH_DT_STRING                   0x03

+#define USBH_DT_STRING_SIZE              2

+

+PACKED_STRUCT {

+	uint8_t  bLength;

+	uint8_t  bDescriptorType;

+	uint8_t  bInterfaceNumber;

+	uint8_t  bAlternateSetting;

+	uint8_t  bNumEndpoints;

+	uint8_t  bInterfaceClass;

+	uint8_t  bInterfaceSubClass;

+	uint8_t  bInterfaceProtocol;

+	uint8_t  iInterface;

+} usbh_interface_descriptor_t;

+#define USBH_DT_INTERFACE                0x04

+#define USBH_DT_INTERFACE_SIZE           9

+

+PACKED_STRUCT {

+	uint8_t	bLength;

+	uint8_t	bDescriptorType;

+	uint8_t	bEndpointAddress;

+	uint8_t	bmAttributes;

+	uint16_t wMaxPacketSize;

+	uint8_t	bInterval;

+} usbh_endpoint_descriptor_t;

+#define USBH_DT_ENDPOINT                 0x05

+#define USBH_DT_ENDPOINT_SIZE            7

+

+PACKED_STRUCT {

+	uint8_t  bLength;

+	uint8_t  bDescriptorType;

+	uint8_t  bFirstInterface;

+	uint8_t  bInterfaceCount;

+	uint8_t  bFunctionClass;

+	uint8_t  bFunctionSubClass;

+	uint8_t  bFunctionProtocol;

+	uint8_t  iFunction;

+} usbh_ia_descriptor_t;

+#define USBH_DT_INTERFACE_ASSOCIATION    	0x0b

+#define USBH_DT_INTERFACE_ASSOCIATION_SIZE	8

+

+PACKED_STRUCT {

+	uint8_t  bDescLength;

+	uint8_t  bDescriptorType;

+	uint8_t  bNbrPorts;

+	uint16_t wHubCharacteristics;

+	uint8_t  bPwrOn2PwrGood;

+	uint8_t  bHubContrCurrent;

+	uint32_t DeviceRemovable;

+} usbh_hub_descriptor_t;

+#define USBH_DT_HUB				    	0x29

+#define USBH_DT_HUB_SIZE		    	(7 + 4)

+

+PACKED_STRUCT {

+	uint8_t  bmRequestType;

+	uint8_t  bRequest;

+	uint16_t wValue;

+	uint16_t wIndex;

+	uint16_t wLength;

+} usbh_control_request_t;

+

+

+#define USBH_REQ_GET_STATUS              0x00

+#define USBH_REQ_CLEAR_FEATURE           0x01

+#define USBH_REQ_SET_FEATURE             0x03

+#define USBH_REQ_SET_ADDRESS             0x05

+#define USBH_REQ_GET_DESCRIPTOR          0x06

+#define USBH_REQ_SET_DESCRIPTOR          0x07

+#define USBH_REQ_GET_CONFIGURATION       0x08

+#define USBH_REQ_SET_CONFIGURATION       0x09

+#define USBH_REQ_GET_INTERFACE           0x0A

+#define USBH_REQ_SET_INTERFACE           0x0B

+#define USBH_REQ_SYNCH_FRAME             0x0C

+

+

+#define USBH_REQTYPE_IN					0x80

+#define USBH_REQTYPE_OUT				0x00

+

+#define USBH_REQTYPE_STANDARD			0x00

+#define USBH_REQTYPE_CLASS				0x20

+#define USBH_REQTYPE_VENDOR				0x40

+

+#define USBH_REQTYPE_DEVICE				0x00

+#define USBH_REQTYPE_INTERFACE			0x01

+#define USBH_REQTYPE_ENDPOINT			0x02

+#define USBH_REQTYPE_OTHER				0x03

+

+#endif

+

+

+#endif /* USBH_DEFS_H_ */

diff --git a/os/hal/include/usbh/desciter.h b/os/hal/include/usbh/desciter.h
new file mode 100644
index 0000000..52b0c98
--- /dev/null
+++ b/os/hal/include/usbh/desciter.h
@@ -0,0 +1,63 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

+

+    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.

+*/

+

+

+#ifndef USBH_DESCITER_H_

+#define USBH_DESCITER_H_

+

+#include "hal.h"

+

+#if HAL_USE_USBH

+

+#include "usbh/defs.h"

+

+

+/* DESCRIPTOR PARSING */

+#define _generic_iterator_fields \

+	const uint8_t *curr;	\

+	uint16_t rem;	\

+	bool valid;

+

+typedef struct {

+	_generic_iterator_fields

+} generic_iterator_t;

+

+typedef struct {

+	_generic_iterator_fields

+	const usbh_ia_descriptor_t *iad;

+} if_iterator_t;

+

+void cfg_iter_init(generic_iterator_t *icfg, const uint8_t *buff, uint16_t rem);

+void if_iter_init(if_iterator_t *iif, const generic_iterator_t *icfg);

+void ep_iter_init(generic_iterator_t *iep, const if_iterator_t *iif);

+void cs_iter_init(generic_iterator_t *ics, const generic_iterator_t *iter);

+void if_iter_next(if_iterator_t *iif);

+void ep_iter_next(generic_iterator_t *iep);

+void cs_iter_next(generic_iterator_t *ics);

+static inline const usbh_config_descriptor_t *cfg_get(generic_iterator_t *icfg) {

+	return (const usbh_config_descriptor_t *)icfg->curr;

+}

+static inline const usbh_interface_descriptor_t *if_get(if_iterator_t *iif) {

+	return (const usbh_interface_descriptor_t *)iif->curr;

+}

+static inline const usbh_endpoint_descriptor_t *ep_get(generic_iterator_t *iep) {

+	return (const usbh_endpoint_descriptor_t *)iep->curr;

+}

+

+#endif

+

+#endif /* USBH_DESCITER_H_ */

diff --git a/os/hal/include/usbh/dev/ftdi.h b/os/hal/include/usbh/dev/ftdi.h
new file mode 100644
index 0000000..678a521
--- /dev/null
+++ b/os/hal/include/usbh/dev/ftdi.h
@@ -0,0 +1,154 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

+

+    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.

+*/

+

+#ifndef USBH_FTDI_H_

+#define USBH_FTDI_H_

+

+#include "usbh.h"

+

+#if HAL_USE_USBH && HAL_USBH_USE_FTDI

+

+/*===========================================================================*/

+/* Driver pre-compile time settings.                                         */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Derived constants and error checks.                                       */

+/*===========================================================================*/

+#define USBHFTDI_FRAMING_DATABITS_7    (0x7 << 0)

+#define USBHFTDI_FRAMING_DATABITS_8    (0x8 << 0)

+#define USBHFTDI_FRAMING_PARITY_NONE   (0x0 << 8)

+#define USBHFTDI_FRAMING_PARITY_NONE   (0x0 << 8)

+#define USBHFTDI_FRAMING_PARITY_ODD    (0x1 << 8)

+#define USBHFTDI_FRAMING_PARITY_EVEN   (0x2 << 8)

+#define USBHFTDI_FRAMING_PARITY_MARK   (0x3 << 8)

+#define USBHFTDI_FRAMING_PARITY_SPACE  (0x4 << 8)

+#define USBHFTDI_FRAMING_STOP_BITS_1   (0x0 << 11)

+#define USBHFTDI_FRAMING_STOP_BITS_15  (0x1 << 11)

+#define USBHFTDI_FRAMING_STOP_BITS_2   (0x2 << 11)

+

+#define USBHFTDI_HANDSHAKE_NONE 		(0x0)

+#define USBHFTDI_HANDSHAKE_RTS_CTS 		(0x1)

+#define USBHFTDI_HANDSHAKE_DTR_DSR 		(0x2)

+#define USBHFTDI_HANDSHAKE_XON_XOFF		(0x4)

+

+

+

+/*===========================================================================*/

+/* Driver data structures and types.                                         */

+/*===========================================================================*/

+typedef struct {

+  uint32_t  speed;

+  uint16_t  framing;

+  uint8_t   handshake;

+  uint8_t   xon_character;

+  uint8_t	xoff_character;

+} USBHFTDIPortConfig;

+

+typedef enum {

+	USBHFTDI_TYPE_A,

+	USBHFTDI_TYPE_B,

+	USBHFTDI_TYPE_H,

+} usbhftdi_type_t;

+

+typedef enum {

+	USBHFTDIP_STATE_UNINIT = 0,

+	USBHFTDIP_STATE_STOP = 1,

+	USBHFTDIP_STATE_ACTIVE = 2,

+	USBHFTDIP_STATE_READY = 3

+} usbhftdip_state_t;

+

+

+#define _ftdi_port_driver_methods                                          \

+  _base_asynchronous_channel_methods

+

+struct FTDIPortDriverVMT {

+	_ftdi_port_driver_methods

+};

+

+typedef struct USBHFTDIPortDriver USBHFTDIPortDriver;

+typedef struct USBHFTDIDriver USBHFTDIDriver;

+

+struct USBHFTDIPortDriver {

+	/* inherited from abstract asyncrhonous channel driver */

+	const struct FTDIPortDriverVMT *vmt;

+	_base_asynchronous_channel_data

+

+	USBHFTDIDriver *ftdip;

+

+	usbhftdip_state_t state;

+

+	usbh_ep_t epin;

+	usbh_urb_t iq_urb;

+	threads_queue_t	iq_waiting;

+	uint32_t iq_counter;

+	USBH_DEFINE_BUFFER(uint8_t, iq_buff[64]);

+	uint8_t *iq_ptr;

+

+

+	usbh_ep_t epout;

+	usbh_urb_t oq_urb;

+	threads_queue_t	oq_waiting;

+	uint32_t oq_counter;

+	USBH_DEFINE_BUFFER(uint8_t, oq_buff[64]);

+	uint8_t *oq_ptr;

+

+	virtual_timer_t vt;

+	uint8_t ifnum;

+

+	USBHFTDIPortDriver *next;

+};

+

+typedef struct USBHFTDIDriver {

+	/* inherited from abstract class driver */

+	_usbh_base_classdriver_data

+

+	usbhftdi_type_t type;

+	USBHFTDIPortDriver *ports;

+

+	mutex_t mtx;

+} USBHFTDIDriver;

+

+/*===========================================================================*/

+/* Driver macros.                                                            */

+/*===========================================================================*/

+

+

+/*===========================================================================*/

+/* External declarations.                                                    */

+/*===========================================================================*/

+extern USBHFTDIDriver USBHFTDID[HAL_USBHFTDI_MAX_INSTANCES];

+extern USBHFTDIPortDriver FTDIPD[HAL_USBHFTDI_MAX_PORTS];

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+	/* FTDI device driver */

+	void usbhftdiObjectInit(USBHFTDIDriver *ftdip);

+

+	/* FTDI port driver */

+	void usbhftdipObjectInit(USBHFTDIPortDriver *ftdipp);

+	void usbhftdipStart(USBHFTDIPortDriver *ftdipp, const USBHFTDIPortConfig *config);

+	void usbhftdipStop(USBHFTDIPortDriver *ftdipp);

+#ifdef __cplusplus

+}

+#endif

+

+

+#endif

+

+#endif /* USBH_FTDI_H_ */

diff --git a/os/hal/include/usbh/dev/hub.h b/os/hal/include/usbh/dev/hub.h
new file mode 100644
index 0000000..28a0681
--- /dev/null
+++ b/os/hal/include/usbh/dev/hub.h
@@ -0,0 +1,138 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

+

+    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.

+*/

+

+#ifndef USBH_HUB_H_

+#define USBH_HUB_H_

+

+#include "usbh.h"

+

+#if HAL_USE_USBH

+#if HAL_USBH_USE_HUB

+

+typedef struct USBHHubDriver {

+	/* inherited from abstract class driver */

+	_usbh_base_classdriver_data

+

+	struct list_head node;

+

+	usbh_ep_t epint;

+	usbh_urb_t urb;

+

+	USBH_DEFINE_BUFFER(uint8_t, scbuff[4]);

+	volatile uint32_t statuschange;

+	uint16_t status;

+	uint16_t c_status;

+

+	usbh_port_t *ports;

+

+	USBH_DEFINE_BUFFER(usbh_hub_descriptor_t, hubDesc);

+

+	/* Low level part */

+	_usbh_hub_ll_data

+

+} USBHHubDriver;

+

+extern USBHHubDriver USBHHUBD[HAL_USBHHUB_MAX_INSTANCES];

+

+

+usbh_urbstatus_t usbhhubControlRequest(USBHDriver *host, USBHHubDriver *hub,

+											uint8_t bmRequestType,

+											uint8_t bRequest,

+											uint16_t wValue,

+											uint16_t wIndex,

+											uint16_t wLength,

+											uint8_t *buf);

+

+

+static inline usbh_urbstatus_t usbhhubClearFeaturePort(usbh_port_t *port, uint8_t feature) {

+	return usbhhubControlRequest(port->device.host, port->hub,

+				USBH_REQTYPE_OUT | USBH_REQTYPE_CLASS | USBH_REQTYPE_OTHER,

+				USBH_REQ_CLEAR_FEATURE,

+				feature,

+				port->number,

+				0,

+				0);

+}

+

+static inline usbh_urbstatus_t usbhhubClearFeatureHub(USBHDriver *host, USBHHubDriver *hub, uint8_t feature) {

+	return usbhhubControlRequest(host, hub,

+				USBH_REQTYPE_OUT | USBH_REQTYPE_CLASS | USBH_REQTYPE_DEVICE,

+				USBH_REQ_CLEAR_FEATURE,

+				feature,

+				0,

+				0,

+				0);

+}

+

+static inline usbh_urbstatus_t usbhhubSetFeaturePort(usbh_port_t *port, uint8_t feature) {

+	return usbhhubControlRequest(port->device.host, port->hub,

+				USBH_REQTYPE_OUT | USBH_REQTYPE_CLASS | USBH_REQTYPE_OTHER,

+				USBH_REQ_SET_FEATURE,

+				feature,

+				port->number,

+				0,

+				0);

+}

+

+void usbhhubObjectInit(USBHHubDriver *hubdp);

+#else

+

+static inline usbh_urbstatus_t usbhhubControlRequest(USBHDriver *host,

+											uint8_t bmRequestType,

+											uint8_t bRequest,

+											uint16_t wValue,

+											uint16_t wIndex,

+											uint16_t wLength,

+											uint8_t *buf) {

+		return usbh_lld_root_hub_request(host, bmRequestType, bRequest, wValue, wIndex, wLength, buf);

+}

+

+static inline usbh_urbstatus_t usbhhubClearFeaturePort(usbh_port_t *port, uint8_t feature) {

+	return usbhhubControlRequest(port->device.host,

+				USBH_REQTYPE_OUT | USBH_REQTYPE_CLASS | USBH_REQTYPE_OTHER,

+				USBH_REQ_CLEAR_FEATURE,

+				feature,

+				port->number,

+				0,

+				0);

+}

+

+static inline usbh_urbstatus_t usbhhubClearFeatureHub(USBHDriver *host, uint8_t feature) {

+	return usbhhubControlRequest(host,

+				USBH_REQTYPE_OUT | USBH_REQTYPE_CLASS | USBH_REQTYPE_DEVICE,

+				USBH_REQ_CLEAR_FEATURE,

+				feature,

+				0,

+				0,

+				0);

+}

+

+static inline usbh_urbstatus_t usbhhubSetFeaturePort(usbh_port_t *port, uint8_t feature) {

+	return usbhhubControlRequest(port->device.host,

+				USBH_REQTYPE_OUT | USBH_REQTYPE_CLASS | USBH_REQTYPE_OTHER,

+				USBH_REQ_SET_FEATURE,

+				feature,

+				port->number,

+				0,

+				0);

+}

+

+#endif

+

+#endif

+

+#endif /* USBH_HUB_H_ */

diff --git a/os/hal/include/usbh/dev/msd.h b/os/hal/include/usbh/dev/msd.h
new file mode 100644
index 0000000..2ca8817
--- /dev/null
+++ b/os/hal/include/usbh/dev/msd.h
@@ -0,0 +1,125 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

+

+    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.

+*/

+

+#ifndef USBH_MSD_H_

+#define USBH_MSD_H_

+

+#include "usbh.h"

+

+#if HAL_USE_USBH && HAL_USBH_USE_MSD

+

+/* TODO:

+ *

+ * - Implement of conditional compilation of multiple-luns per instance.

+ * - Implement error checking and recovery when commands fail.

+ *

+ */

+

+

+/*===========================================================================*/

+/* Driver pre-compile time settings.                                         */

+/*===========================================================================*/

+

+

+/*===========================================================================*/

+/* Derived constants and error checks.                                       */

+/*===========================================================================*/

+

+

+/*===========================================================================*/

+/* Driver data structures and types.                                         */

+/*===========================================================================*/

+

+#define _usbhmsd_driver_methods                                                 \

+	_base_block_device_methods

+

+struct USBHMassStorageDriverVMT {

+	_usbhmsd_driver_methods

+};

+

+typedef struct USBHMassStorageLUNDriver USBHMassStorageLUNDriver;

+typedef struct USBHMassStorageDriver USBHMassStorageDriver;

+

+struct USBHMassStorageLUNDriver {

+	/* inherited from abstract block driver */

+	const struct USBHMassStorageDriverVMT *vmt;

+	_base_block_device_data

+

+	BlockDeviceInfo info;

+	USBHMassStorageDriver *msdp;

+

+	USBHMassStorageLUNDriver *next;

+};

+

+typedef struct USBHMassStorageDriver {

+	/* inherited from abstract class driver */

+	_usbh_base_classdriver_data

+

+	/* for LUN request serialization, can be removed

+	 * if the driver is configured to support only one LUN

+	 * per USBHMassStorageDriver instance */

+	mutex_t mtx;

+

+	usbh_ep_t epin;

+	usbh_ep_t epout;

+	uint8_t ifnum;

+	uint8_t max_lun;

+	uint32_t tag;

+

+	USBHMassStorageLUNDriver *luns;

+} USBHMassStorageDriver;

+

+

+/*===========================================================================*/

+/* Driver macros.                                                            */

+/*===========================================================================*/

+

+

+/*===========================================================================*/

+/* External declarations.                                                    */

+/*===========================================================================*/

+

+extern USBHMassStorageLUNDriver MSBLKD[HAL_USBHMSD_MAX_LUNS];

+extern USBHMassStorageDriver USBHMSD[HAL_USBHMSD_MAX_INSTANCES];

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+	/* Mass Storage Driver */

+	void usbhmsdObjectInit(USBHMassStorageDriver *msdp);

+

+	/* Mass Storage LUN Driver (block driver) */

+	void usbhmsdLUNObjectInit(USBHMassStorageLUNDriver *lunp);

+	void usbhmsdLUNStart(USBHMassStorageLUNDriver *lunp);

+	void usbhmsdLUNStop(USBHMassStorageLUNDriver *lunp);

+	bool usbhmsdLUNConnect(USBHMassStorageLUNDriver *lunp);

+	bool usbhmsdLUNDisconnect(USBHMassStorageLUNDriver *lunp);

+	bool usbhmsdLUNRead(USBHMassStorageLUNDriver *lunp, uint32_t startblk,

+					uint8_t *buffer, uint32_t n);

+	bool usbhmsdLUNWrite(USBHMassStorageLUNDriver *lunp, uint32_t startblk,

+					const uint8_t *buffer, uint32_t n);

+	bool usbhmsdLUNSync(USBHMassStorageLUNDriver *lunp);

+	bool usbhmsdLUNGetInfo(USBHMassStorageLUNDriver *lunp, BlockDeviceInfo *bdip);

+	bool usbhmsdLUNIsInserted(USBHMassStorageLUNDriver *lunp);

+	bool usbhmsdLUNIsProtected(USBHMassStorageLUNDriver *lunp);

+#ifdef __cplusplus

+}

+#endif

+

+#endif

+

+#endif /* USBH_MSD_H_ */

diff --git a/os/hal/include/usbh/internal.h b/os/hal/include/usbh/internal.h
new file mode 100644
index 0000000..6811e20
--- /dev/null
+++ b/os/hal/include/usbh/internal.h
@@ -0,0 +1,148 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

+

+    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.

+*/

+

+#ifndef USBH_INTERNAL_H_

+#define USBH_INTERNAL_H_

+

+#include "usbh.h"

+

+#if HAL_USE_USBH

+

+/*===========================================================================*/

+/* These declarations are not part of the public API.                        */

+/*===========================================================================*/

+

+#if HAL_USBH_USE_FTDI

+extern const usbh_classdriverinfo_t usbhftdiClassDriverInfo;

+#endif

+#if HAL_USBH_USE_MSD

+extern const usbh_classdriverinfo_t usbhmsdClassDriverInfo;

+#endif

+#if HAL_USBH_USE_UVC

+extern const usbh_classdriverinfo_t usbhuvcClassDriverInfo;

+#endif

+#if HAL_USBH_USE_HUB

+extern const usbh_classdriverinfo_t usbhhubClassDriverInfo;

+void _usbhub_port_object_init(usbh_port_t *port, USBHDriver *usbh,

+		USBHHubDriver *hub, uint8_t number);

+#else

+void _usbhub_port_object_init(usbh_port_t *port, USBHDriver *usbh, uint8_t number);

+#endif

+

+void _usbh_port_disconnected(usbh_port_t *port);

+void _usbh_urb_completeI(usbh_urb_t *urb, usbh_urbstatus_t status);

+bool _usbh_urb_abortI(usbh_urb_t *urb, usbh_urbstatus_t status);

+void _usbh_urb_abort_and_waitS(usbh_urb_t *urb, usbh_urbstatus_t status);

+

+

+#define USBH_CLASSIN(type, req, value, index)	\

+	(USBH_REQTYPE_IN | type | USBH_REQTYPE_CLASS), \

+	req, \

+	value, \

+	index

+

+#define USBH_CLASSOUT(type, req, value, index)	\

+	(USBH_REQTYPE_OUT | type | USBH_REQTYPE_CLASS), \

+	req, \

+	value, \

+	index

+

+#define USBH_STANDARDIN(type, req, value, index)	\

+	(USBH_REQTYPE_IN | type | USBH_REQTYPE_STANDARD), \

+	req, \

+	value, \

+	index

+

+#define USBH_STANDARDOUT(type, req, value, index)	\

+	(USBH_REQTYPE_OUT | type | USBH_REQTYPE_STANDARD), \

+	req, \

+	value, \

+	index

+

+

+#define USBH_PID_DATA0            0

+#define USBH_PID_DATA2            1

+#define USBH_PID_DATA1            2

+#define USBH_PID_MDATA            3

+#define USBH_PID_SETUP            3

+

+

+/* GetBusState and SetHubDescriptor are optional, omitted */

+#define ClearHubFeature   (((USBH_REQTYPE_OUT | USBH_REQTYPE_CLASS | USBH_REQTYPE_DEVICE) << 8) \

+							| USBH_REQ_CLEAR_FEATURE)

+#define SetHubFeature     (((USBH_REQTYPE_OUT | USBH_REQTYPE_CLASS | USBH_REQTYPE_DEVICE) << 8) \

+							| USBH_REQ_SET_FEATURE)

+#define ClearPortFeature   (((USBH_REQTYPE_OUT | USBH_REQTYPE_CLASS | USBH_REQTYPE_OTHER) << 8) \

+							| USBH_REQ_CLEAR_FEATURE)

+#define SetPortFeature     (((USBH_REQTYPE_OUT | USBH_REQTYPE_CLASS | USBH_REQTYPE_OTHER) << 8) \

+							| USBH_REQ_SET_FEATURE)

+#define GetHubDescriptor  (((USBH_REQTYPE_IN | USBH_REQTYPE_CLASS | USBH_REQTYPE_DEVICE) << 8) \

+							| USBH_REQ_GET_DESCRIPTOR)

+#define GetHubStatus      (((USBH_REQTYPE_IN | USBH_REQTYPE_CLASS | USBH_REQTYPE_DEVICE) << 8) \

+							| USBH_REQ_GET_STATUS)

+#define GetPortStatus     (((USBH_REQTYPE_IN | USBH_REQTYPE_CLASS | USBH_REQTYPE_OTHER) << 8) \

+							| USBH_REQ_GET_STATUS)

+

+

+#define USBH_PORTSTATUS_CONNECTION        0x0001

+#define USBH_PORTSTATUS_ENABLE            0x0002

+#define USBH_PORTSTATUS_SUSPEND           0x0004

+#define USBH_PORTSTATUS_OVERCURRENT       0x0008

+#define USBH_PORTSTATUS_RESET             0x0010

+/* bits 5 to 7 are reserved */

+#define USBH_PORTSTATUS_POWER             0x0100

+#define USBH_PORTSTATUS_LOW_SPEED         0x0200

+#define USBH_PORTSTATUS_HIGH_SPEED        0x0400

+#define USBH_PORTSTATUS_TEST              0x0800

+#define USBH_PORTSTATUS_INDICATOR         0x1000

+/* bits 13 to 15 are reserved */

+

+#define USBH_PORTSTATUS_C_CONNECTION      0x0001

+#define USBH_PORTSTATUS_C_ENABLE          0x0002

+#define USBH_PORTSTATUS_C_SUSPEND         0x0004

+#define USBH_PORTSTATUS_C_OVERCURRENT     0x0008

+#define USBH_PORTSTATUS_C_RESET           0x0010

+

+#define USBH_HUBSTATUS_C_HUB_LOCAL_POWER       0x0001

+#define USBH_HUBSTATUS_C_HUB_OVER_CURRENT      0x0002

+

+/*

+ * Port feature numbers

+ * See USB 2.0 spec Table 11-17

+ */

+#define USBH_HUB_FEAT_C_HUB_LOCAL_POWER  0

+#define USBH_HUB_FEAT_C_HUB_OVER_CURRENT 1

+#define USBH_PORT_FEAT_CONNECTION        0

+#define USBH_PORT_FEAT_ENABLE            1

+#define USBH_PORT_FEAT_SUSPEND           2

+#define USBH_PORT_FEAT_OVERCURRENT     	 3

+#define USBH_PORT_FEAT_RESET             4

+#define USBH_PORT_FEAT_POWER             8

+#define USBH_PORT_FEAT_LOWSPEED          9

+#define USBH_PORT_FEAT_C_CONNECTION      16

+#define USBH_PORT_FEAT_C_ENABLE          17

+#define USBH_PORT_FEAT_C_SUSPEND         18

+#define USBH_PORT_FEAT_C_OVERCURRENT     19

+#define USBH_PORT_FEAT_C_RESET           20

+#define USBH_PORT_FEAT_TEST              21

+#define USBH_PORT_FEAT_INDICATOR         22

+

+#define sizeof_array(x) 	(sizeof(x)/sizeof(*(x)))

+

+#endif

+

+#endif /* USBH_INTERNAL_H_ */

diff --git a/os/hal/include/usbh/list.h b/os/hal/include/usbh/list.h
new file mode 100644
index 0000000..4eceacd
--- /dev/null
+++ b/os/hal/include/usbh/list.h
@@ -0,0 +1,598 @@
+#ifndef USBH_LIST_H_

+#define USBH_LIST_H_

+

+/* TODO: re-write this file; stolen from linux */

+

+#ifndef offsetof

+#define offsetof(TYPE, MEMBER) ((size_t) &((TYPE *)0)->MEMBER)

+#endif

+

+#define container_of(ptr, type, member) ((type *)(void *)((char *)(ptr) - offsetof(type, member)))

+#ifndef container_of

+#define container_of(ptr, type, member) ({                  \

+	const typeof(((type *)0)->member) * __mptr = (ptr);     \

+	(type *)((char *)__mptr - offsetof(type, member)); })

+#endif

+

+/*

+ * Simple doubly linked list implementation.

+ *

+ * Some of the internal functions ("__xxx") are useful when

+ * manipulating whole lists rather than single entries, as

+ * sometimes we already know the next/prev entries and we can

+ * generate better code by using them directly rather than

+ * using the generic single-entry routines.

+ */

+struct list_head {

+        struct list_head *next, *prev;

+};

+

+#define LIST_HEAD_INIT(name) { &(name), &(name) }

+

+#define LIST_HEAD(name) \

+        struct list_head name = LIST_HEAD_INIT(name)

+

+static inline void INIT_LIST_HEAD(struct list_head *list)

+{

+        list->next = list;

+        list->prev = list;

+}

+

+/*

+ * Insert a new entry between two known consecutive entries.

+ *

+ * This is only for internal list manipulation where we know

+ * the prev/next entries already!

+ */

+#ifndef CONFIG_DEBUG_LIST

+static inline void __list_add(struct list_head *new,

+                              struct list_head *prev,

+                              struct list_head *next)

+{

+        next->prev = new;

+        new->next = next;

+        new->prev = prev;

+        prev->next = new;

+}

+#else

+extern void __list_add(struct list_head *new,

+                              struct list_head *prev,

+                              struct list_head *next);

+#endif

+

+/**

+ * list_add - add a new entry

+ * @new: new entry to be added

+ * @head: list head to add it after

+ *

+ * Insert a new entry after the specified head.

+ * This is good for implementing stacks.

+ */

+static inline void list_add(struct list_head *new, struct list_head *head)

+{

+        __list_add(new, head, head->next);

+}

+

+

+/**

+ * list_add_tail - add a new entry

+ * @new: new entry to be added

+ * @head: list head to add it before

+ *

+ * Insert a new entry before the specified head.

+ * This is useful for implementing queues.

+ */

+static inline void list_add_tail(struct list_head *new, struct list_head *head)

+{

+        __list_add(new, head->prev, head);

+}

+

+/*

+ * Delete a list entry by making the prev/next entries

+ * point to each other.

+ *

+ * This is only for internal list manipulation where we know

+ * the prev/next entries already!

+ */

+static inline void __list_del(struct list_head * prev, struct list_head * next)

+{

+        next->prev = prev;

+        prev->next = next;

+}

+

+/**

+ * list_del - deletes entry from list.

+ * @entry: the element to delete from the list.

+ * Note: list_empty() on entry does not return true after this, the entry is

+ * in an undefined state.

+ */

+#ifndef CONFIG_DEBUG_LIST

+static inline void __list_del_entry(struct list_head *entry)

+{

+        __list_del(entry->prev, entry->next);

+}

+

+static inline void list_del(struct list_head *entry)

+{

+        __list_del(entry->prev, entry->next);

+       // entry->next = LIST_POISON1;

+       // entry->prev = LIST_POISON2;

+}

+#else

+extern void __list_del_entry(struct list_head *entry);

+extern void list_del(struct list_head *entry);

+#endif

+

+/**

+ * list_replace - replace old entry by new one

+ * @old : the element to be replaced

+ * @new : the new element to insert

+ *

+ * If @old was empty, it will be overwritten.

+ */

+static inline void list_replace(struct list_head *old,

+                                struct list_head *new)

+{

+        new->next = old->next;

+        new->next->prev = new;

+        new->prev = old->prev;

+        new->prev->next = new;

+}

+

+static inline void list_replace_init(struct list_head *old,

+                                        struct list_head *new)

+{

+        list_replace(old, new);

+        INIT_LIST_HEAD(old);

+}

+

+/**

+ * list_del_init - deletes entry from list and reinitialize it.

+ * @entry: the element to delete from the list.

+ */

+static inline void list_del_init(struct list_head *entry)

+{

+        __list_del_entry(entry);

+        INIT_LIST_HEAD(entry);

+}

+

+/**

+ * list_move - delete from one list and add as another's head

+ * @list: the entry to move

+ * @head: the head that will precede our entry

+ */

+static inline void list_move(struct list_head *list, struct list_head *head)

+{

+        __list_del_entry(list);

+        list_add(list, head);

+}

+

+/**

+ * list_move_tail - delete from one list and add as another's tail

+ * @list: the entry to move

+ * @head: the head that will follow our entry

+ */

+static inline void list_move_tail(struct list_head *list,

+                                  struct list_head *head)

+{

+        __list_del_entry(list);

+        list_add_tail(list, head);

+}

+

+/**

+ * list_is_last - tests whether @list is the last entry in list @head

+ * @list: the entry to test

+ * @head: the head of the list

+ */

+static inline int list_is_last(const struct list_head *list,

+                                const struct list_head *head)

+{

+        return list->next == head;

+}

+

+/**

+ * list_empty - tests whether a list is empty

+ * @head: the list to test.

+ */

+static inline int list_empty(const struct list_head *head)

+{

+        return head->next == head;

+}

+

+/**

+ * list_empty_careful - tests whether a list is empty and not being modified

+ * @head: the list to test

+ *

+ * Description:

+ * tests whether a list is empty _and_ checks that no other CPU might be

+ * in the process of modifying either member (next or prev)

+ *

+ * NOTE: using list_empty_careful() without synchronization

+ * can only be safe if the only activity that can happen

+ * to the list entry is list_del_init(). Eg. it cannot be used

+ * if another CPU could re-list_add() it.

+ */

+static inline int list_empty_careful(const struct list_head *head)

+{

+        struct list_head *next = head->next;

+        return (next == head) && (next == head->prev);

+}

+

+/**

+ * list_rotate_left - rotate the list to the left

+ * @head: the head of the list

+ */

+static inline void list_rotate_left(struct list_head *head)

+{

+        struct list_head *first;

+

+        if (!list_empty(head)) {

+                first = head->next;

+                list_move_tail(first, head);

+        }

+}

+

+/**

+ * list_is_singular - tests whether a list has just one entry.

+ * @head: the list to test.

+ */

+static inline int list_is_singular(const struct list_head *head)

+{

+        return !list_empty(head) && (head->next == head->prev);

+}

+

+static inline void __list_cut_position(struct list_head *list,

+                struct list_head *head, struct list_head *entry)

+{

+        struct list_head *new_first = entry->next;

+        list->next = head->next;

+        list->next->prev = list;

+        list->prev = entry;

+        entry->next = list;

+        head->next = new_first;

+        new_first->prev = head;

+}

+

+/**

+ * list_cut_position - cut a list into two

+ * @list: a new list to add all removed entries

+ * @head: a list with entries

+ * @entry: an entry within head, could be the head itself

+ *      and if so we won't cut the list

+ *

+ * This helper moves the initial part of @head, up to and

+ * including @entry, from @head to @list. You should

+ * pass on @entry an element you know is on @head. @list

+ * should be an empty list or a list you do not care about

+ * losing its data.

+ *

+ */

+static inline void list_cut_position(struct list_head *list,

+                struct list_head *head, struct list_head *entry)

+{

+        if (list_empty(head))

+                return;

+        if (list_is_singular(head) &&

+                (head->next != entry && head != entry))

+                return;

+        if (entry == head)

+                INIT_LIST_HEAD(list);

+        else

+                __list_cut_position(list, head, entry);

+}

+

+static inline void __list_splice(const struct list_head *list,

+                                 struct list_head *prev,

+                                 struct list_head *next)

+{

+        struct list_head *first = list->next;

+        struct list_head *last = list->prev;

+

+        first->prev = prev;

+        prev->next = first;

+

+        last->next = next;

+        next->prev = last;

+}

+

+/**

+ * list_splice - join two lists, this is designed for stacks

+ * @list: the new list to add.

+ * @head: the place to add it in the first list.

+ */

+static inline void list_splice(const struct list_head *list,

+                                struct list_head *head)

+{

+        if (!list_empty(list))

+                __list_splice(list, head, head->next);

+}

+

+/**

+ * list_splice_tail - join two lists, each list being a queue

+ * @list: the new list to add.

+ * @head: the place to add it in the first list.

+ */

+static inline void list_splice_tail(struct list_head *list,

+                                struct list_head *head)

+{

+        if (!list_empty(list))

+                __list_splice(list, head->prev, head);

+}

+

+/**

+ * list_splice_init - join two lists and reinitialise the emptied list.

+ * @list: the new list to add.

+ * @head: the place to add it in the first list.

+ *

+ * The list at @list is reinitialised

+ */

+static inline void list_splice_init(struct list_head *list,

+                                    struct list_head *head)

+{

+        if (!list_empty(list)) {

+                __list_splice(list, head, head->next);

+                INIT_LIST_HEAD(list);

+        }

+}

+

+/**

+ * list_splice_tail_init - join two lists and reinitialise the emptied list

+ * @list: the new list to add.

+ * @head: the place to add it in the first list.

+ *

+ * Each of the lists is a queue.

+ * The list at @list is reinitialised

+ */

+static inline void list_splice_tail_init(struct list_head *list,

+                                         struct list_head *head)

+{

+        if (!list_empty(list)) {

+                __list_splice(list, head->prev, head);

+                INIT_LIST_HEAD(list);

+        }

+}

+

+/**

+ * list_entry - get the struct for this entry

+ * @ptr:        the &struct list_head pointer.

+ * @type:       the type of the struct this is embedded in.

+ * @member:     the name of the list_head within the struct.

+ */

+#define list_entry(ptr, type, member) \

+        container_of(ptr, type, member)

+

+/**

+ * list_first_entry - get the first element from a list

+ * @ptr:        the list head to take the element from.

+ * @type:       the type of the struct this is embedded in.

+ * @member:     the name of the list_head within the struct.

+ *

+ * Note, that list is expected to be not empty.

+ */

+#define list_first_entry(ptr, type, member) \

+        list_entry((ptr)->next, type, member)

+

+/**

+ * list_last_entry - get the last element from a list

+ * @ptr:        the list head to take the element from.

+ * @type:       the type of the struct this is embedded in.

+ * @member:     the name of the list_head within the struct.

+ *

+ * Note, that list is expected to be not empty.

+ */

+#define list_last_entry(ptr, type, member) \

+        list_entry((ptr)->prev, type, member)

+

+/**

+ * list_first_entry_or_null - get the first element from a list

+ * @ptr:        the list head to take the element from.

+ * @type:       the type of the struct this is embedded in.

+ * @member:     the name of the list_head within the struct.

+ *

+ * Note that if the list is empty, it returns NULL.

+ */

+#define list_first_entry_or_null(ptr, type, member) \

+        (!list_empty(ptr) ? list_first_entry(ptr, type, member) : NULL)

+

+/**

+ * list_next_entry - get the next element in list

+ * @pos:        the type * to cursor

+ * @member:     the name of the list_head within the struct.

+ */

+#define list_next_entry(pos, type, member) \

+        list_entry((pos)->member.next, type, member)

+

+/**

+ * list_prev_entry - get the prev element in list

+ * @pos:        the type * to cursor

+ * @member:     the name of the list_head within the struct.

+ */

+#define list_prev_entry(pos, type, member) \

+        list_entry((pos)->member.prev, type, member)

+

+/**

+ * list_for_each        -       iterate over a list

+ * @pos:        the &struct list_head to use as a loop cursor.

+ * @head:       the head for your list.

+ */

+#define list_for_each(pos, head) \

+        for (pos = (head)->next; pos != (head); pos = pos->next)

+

+/**

+ * list_for_each_prev   -       iterate over a list backwards

+ * @pos:        the &struct list_head to use as a loop cursor.

+ * @head:       the head for your list.

+ */

+#define list_for_each_prev(pos, head) \

+        for (pos = (head)->prev; pos != (head); pos = pos->prev)

+

+/**

+ * list_for_each_safe - iterate over a list safe against removal of list entry

+ * @pos:        the &struct list_head to use as a loop cursor.

+ * @n:          another &struct list_head to use as temporary storage

+ * @head:       the head for your list.

+ */

+#define list_for_each_safe(pos, n, head) \

+        for (pos = (head)->next, n = pos->next; pos != (head); \

+                pos = n, n = pos->next)

+

+/**

+ * list_for_each_prev_safe - iterate over a list backwards safe against removal of list entry

+ * @pos:        the &struct list_head to use as a loop cursor.

+ * @n:          another &struct list_head to use as temporary storage

+ * @head:       the head for your list.

+ */

+#define list_for_each_prev_safe(pos, n, head) \

+        for (pos = (head)->prev, n = pos->prev; \

+             pos != (head); \

+             pos = n, n = pos->prev)

+

+/**

+ * list_for_each_entry  -       iterate over list of given type

+ * @pos:        the type * to use as a loop cursor.

+ * @head:       the head for your list.

+ * @member:     the name of the list_head within the struct.

+ */

+#define list_for_each_entry(pos, type, head, member)                          \

+        for (pos = list_first_entry(head, type, member);        \

+             &pos->member != (head);                                    \

+             pos = list_next_entry(pos, type, member))

+

+/**

+ * list_for_each_entry_reverse - iterate backwards over list of given type.

+ * @pos:        the type * to use as a loop cursor.

+ * @head:       the head for your list.

+ * @member:     the name of the list_head within the struct.

+ */

+#define list_for_each_entry_reverse(pos, type, head, member)                  \

+        for (pos = list_last_entry(head, type, member);         \

+             &pos->member != (head);                                    \

+             pos = list_prev_entry(pos, type, member))

+

+/**

+ * list_prepare_entry - prepare a pos entry for use in list_for_each_entry_continue()

+ * @pos:        the type * to use as a start point

+ * @head:       the head of the list

+ * @member:     the name of the list_head within the struct.

+ *

+ * Prepares a pos entry for use as a start point in list_for_each_entry_continue().

+ */

+#define list_prepare_entry(pos, type, head, member) \

+        ((pos) ? : list_entry(head, type, member))

+

+/**

+ * list_for_each_entry_continue - continue iteration over list of given type

+ * @pos:        the type * to use as a loop cursor.

+ * @head:       the head for your list.

+ * @member:     the name of the list_head within the struct.

+ *

+ * Continue to iterate over list of given type, continuing after

+ * the current position.

+ */

+#define list_for_each_entry_continue(pos, type, head, member)                 \

+        for (pos = list_next_entry(pos, type, member);                        \

+             &pos->member != (head);                                    \

+             pos = list_next_entry(pos, type, member))

+

+/**

+ * list_for_each_entry_continue_reverse - iterate backwards from the given point

+ * @pos:        the type * to use as a loop cursor.

+ * @head:       the head for your list.

+ * @member:     the name of the list_head within the struct.

+ *

+ * Start to iterate over list of given type backwards, continuing after

+ * the current position.

+ */

+#define list_for_each_entry_continue_reverse(pos, type, head, member)         \

+        for (pos = list_prev_entry(pos, type, member);                        \

+             &pos->member != (head);                                    \

+             pos = list_prev_entry(pos, type, member))

+

+/**

+ * list_for_each_entry_from - iterate over list of given type from the current point

+ * @pos:        the type * to use as a loop cursor.

+ * @head:       the head for your list.

+ * @member:     the name of the list_head within the struct.

+ *

+ * Iterate over list of given type, continuing from current position.

+ */

+#define list_for_each_entry_from(pos, type, head, member)                     \

+        for (; &pos->member != (head);                                  \

+             pos = list_next_entry(pos, type, member))

+

+/**

+ * list_for_each_entry_safe - iterate over list of given type safe against removal of list entry

+ * @pos:        the type * to use as a loop cursor.

+ * @n:          another type * to use as temporary storage

+ * @head:       the head for your list.

+ * @member:     the name of the list_head within the struct.

+ */

+#define list_for_each_entry_safe(pos, type, n, head, member)                  \

+        for (pos = list_first_entry(head, type, member),        \

+                n = list_next_entry(pos, type, member);                       \

+             &pos->member != (head);                                    \

+             pos = n, n = list_next_entry(n, type, member))

+

+/**

+ * list_for_each_entry_safe_continue - continue list iteration safe against removal

+ * @pos:        the type * to use as a loop cursor.

+ * @n:          another type * to use as temporary storage

+ * @head:       the head for your list.

+ * @member:     the name of the list_head within the struct.

+ *

+ * Iterate over list of given type, continuing after current point,

+ * safe against removal of list entry.

+ */

+#define list_for_each_entry_safe_continue(pos, type, n, head, member)                 \

+        for (pos = list_next_entry(pos, type, member),                                \

+                n = list_next_entry(pos, type, member);                               \

+             &pos->member != (head);                                            \

+             pos = n, n = list_next_entry(n, type, member))

+

+/**

+ * list_for_each_entry_safe_from - iterate over list from current point safe against removal

+ * @pos:        the type * to use as a loop cursor.

+ * @n:          another type * to use as temporary storage

+ * @head:       the head for your list.

+ * @member:     the name of the list_head within the struct.

+ *

+ * Iterate over list of given type from current point, safe against

+ * removal of list entry.

+ */

+#define list_for_each_entry_safe_from(pos, type, n, head, member)                     \

+        for (n = list_next_entry(pos, type, member);                                  \

+             &pos->member != (head);                                            \

+             pos = n, n = list_next_entry(n, type, member))

+

+/**

+ * list_for_each_entry_safe_reverse - iterate backwards over list safe against removal

+ * @pos:        the type * to use as a loop cursor.

+ * @n:          another type * to use as temporary storage

+ * @head:       the head for your list.

+ * @member:     the name of the list_head within the struct.

+ *

+ * Iterate backwards over list of given type, safe against removal

+ * of list entry.

+ */

+#define list_for_each_entry_safe_reverse(pos, type, n, head, member)          \

+        for (pos = list_last_entry(head, type, member),         \

+                n = list_prev_entry(pos, type, member);                       \

+             &pos->member != (head);                                    \

+             pos = n, n = list_prev_entry(n, type, member))

+

+/**

+ * list_safe_reset_next - reset a stale list_for_each_entry_safe loop

+ * @pos:        the loop cursor used in the list_for_each_entry_safe loop

+ * @n:          temporary storage used in list_for_each_entry_safe

+ * @member:     the name of the list_head within the struct.

+ *

+ * list_safe_reset_next is not safe to use in general if the list may be

+ * modified concurrently (eg. the lock is dropped in the loop body). An

+ * exception to this is if the cursor element (pos) is pinned in the list,

+ * and list_safe_reset_next is called after re-taking the lock and before

+ * completing the current iteration of the loop body.

+ */

+#define list_safe_reset_next(pos, type, n, member)                            \

+        n = list_next_entry(pos, type, member)

+

+#endif /* USBH_LIST_H_ */

diff --git a/os/hal/ports/NRF51/NRF51822/gpt_lld.h b/os/hal/ports/NRF51/NRF51822/gpt_lld.h
index d707cda..2c84d6c 100644
--- a/os/hal/ports/NRF51/NRF51822/gpt_lld.h
+++ b/os/hal/ports/NRF51/NRF51822/gpt_lld.h
@@ -94,7 +94,7 @@
 

 #if !NRF51_GPT_USE_TIMER0 && !NRF51_GPT_USE_TIMER1 &&                         \

     !NRF51_GPT_USE_TIMER2

-#error "GPT driver activated but no TIM peripheral assigned"

+#error "GPT driver activated but no TIMER peripheral assigned"

 #endif

 

 #if 0

diff --git a/os/hal/ports/NRF51/NRF51822/hal_lld.c b/os/hal/ports/NRF51/NRF51822/hal_lld.c
index e1d2ed5..af5e377 100644
--- a/os/hal/ports/NRF51/NRF51822/hal_lld.c
+++ b/os/hal/ports/NRF51/NRF51822/hal_lld.c
@@ -55,6 +55,29 @@
  */
 void hal_lld_init(void)
 {
+  /* High frequency clock initialisation
+   *  (If NRF51_XTAL_VALUE is not defined assume its an RC oscillator)
+   */
+  NRF_CLOCK->TASKS_HFCLKSTOP = 1;
+#if defined(NRF51_XTAL_VALUE)
+#if   NRF51_XTAL_VALUE == 16000000
+  NRF_CLOCK->XTALFREQ = 0xFF;
+#elif NRF51_XTAL_VALUE == 32000000
+  NRF_CLOCK->XTALFREQ = 0x00;
+#endif
+#endif
+
+  
+  /* Low frequency clock initialisation
+   * Clock is only started if st driver requires it
+   */
+  NRF_CLOCK->TASKS_LFCLKSTOP = 1;
+  NRF_CLOCK->LFCLKSRC = NRF51_LFCLK_SOURCE;
+  
+#if (OSAL_ST_MODE != OSAL_ST_MODE_NONE) &&			\
+    (NRF51_SYSTEM_TICKS == NRF51_SYSTEM_TICKS_AS_RTC)
+  NRF_CLOCK->TASKS_LFCLKSTART = 1;
+#endif
 }
 
 /**
diff --git a/os/hal/ports/NRF51/NRF51822/hal_lld.h b/os/hal/ports/NRF51/NRF51822/hal_lld.h
index 436789c..283c207 100644
--- a/os/hal/ports/NRF51/NRF51822/hal_lld.h
+++ b/os/hal/ports/NRF51/NRF51822/hal_lld.h
@@ -39,14 +39,36 @@
  * @}
  */
 
+/**
+ * @brief  Frequency valuefor the Low Frequency Clock
+ */
+#define NRF51_LFCLK_FREQUENCY  32768
+
 /*===========================================================================*/
 /* Driver pre-compile time settings.                                         */
 /*===========================================================================*/
 
+/**
+ * @brief   Select source of Low Frequency Clock (LFCLK)
+ * @details Possible values for source are:
+ *            0 : RC oscillator
+ *            1 : External cristal
+ *            2 : Synthetized clock from High Frequency Clock (HFCLK)
+ *          When cristal is not available it's preferable to use the
+ *          internal RC oscillator that synthezing the clock.
+ */
+#if !defined(NRF51_LFCLK_SOURCE) || defined(__DOXYGEN__)
+#define NRF51_LFCLK_SOURCE             0
+#endif
+
 /*===========================================================================*/
 /* Derived constants and error checks.                                       */
 /*===========================================================================*/
 
+#if (NRF51_LFCLK_SOURCE < 0) || (NRF51_LFCLK_SOURCE > 2)
+#error "Possible value for NRF51_LFCLK_SOURCE are 0=RC, 1=XTAL, 2=Synth"
+#endif
+
 /*===========================================================================*/
 /* Driver data structures and types.                                         */
 /*===========================================================================*/
diff --git a/os/hal/ports/NRF51/NRF51822/platform.mk b/os/hal/ports/NRF51/NRF51822/platform.mk
index 663ca1f..11259e5 100644
--- a/os/hal/ports/NRF51/NRF51822/platform.mk
+++ b/os/hal/ports/NRF51/NRF51822/platform.mk
@@ -1,5 +1,5 @@
 ifeq ($(USE_SMART_BUILD),yes)
-HALCONF := $(strip $(shell cat halconf.h | egrep -e "define"))
+HALCONF := $(strip $(shell cat halconf.h halconf_community.h | egrep -e "define"))
 
 # List of all the NRF51x platform files.
 PLATFORMSRC  = ${CHIBIOS}/os/hal/ports/common/ARMCMx/nvic.c \
diff --git a/os/hal/ports/NRF51/NRF51822/st_lld.c b/os/hal/ports/NRF51/NRF51822/st_lld.c
index 526db35..f65117b 100644
--- a/os/hal/ports/NRF51/NRF51822/st_lld.c
+++ b/os/hal/ports/NRF51/NRF51822/st_lld.c
@@ -1,5 +1,6 @@
 /*

     ChibiOS - Copyright (C) 2015 Fabio Utzig

+                            2016 Stephane D'Alu

 

     Licensed under the Apache License, Version 2.0 (the "License");

     you may not use this file except in compliance with the License.

@@ -51,9 +52,55 @@
 /*===========================================================================*/

 

 #if (OSAL_ST_MODE == OSAL_ST_MODE_PERIODIC) || defined(__DOXYGEN__)

+#if NRF51_ST_USE_RTC0 == TRUE

 /**

- * @brief   System Timer vector.

- * @details This interrupt is used for system tick in periodic mode.

+ * @brief   System Timer vector (RTC0)

+ * @details This interrupt is used for system tick in periodic mode

+ *          if selected with NRF51_ST_USE_RTC0

+ *

+ * @isr

+ */

+OSAL_IRQ_HANDLER(Vector6C) {

+

+  OSAL_IRQ_PROLOGUE();

+

+  NRF_RTC0->EVENTS_TICK = 0;

+      

+  osalSysLockFromISR();

+  osalOsTimerHandlerI();

+  osalSysUnlockFromISR();

+

+  OSAL_IRQ_EPILOGUE();

+}

+#endif

+

+#if NRF51_ST_USE_RTC1 == TRUE

+/**

+ * @brief   System Timer vector (RTC1)

+ * @details This interrupt is used for system tick in periodic mode

+ *          if selected with NRF51_ST_USE_RTC1

+ *

+ * @isr

+ */

+OSAL_IRQ_HANDLER(Vector84) {

+

+  OSAL_IRQ_PROLOGUE();

+

+  NRF_RTC1->EVENTS_TICK = 0;

+      

+  osalSysLockFromISR();

+  osalOsTimerHandlerI();

+  osalSysUnlockFromISR();

+

+  OSAL_IRQ_EPILOGUE();

+}

+#endif

+

+#if NRF51_ST_USE_TIMER0 == TRUE

+/**

+ * @brief   System Timer vector. (TIMER0)

+ * @details This interrupt is used for system tick in periodic mode

+ *          if selected with NRF51_ST_USE_TIMER0

  *

  * @isr

  */

@@ -71,8 +118,73 @@ OSAL_IRQ_HANDLER(Vector60) {
 

   OSAL_IRQ_EPILOGUE();

 }

+#endif

 #endif /* OSAL_ST_MODE == OSAL_ST_MODE_PERIODIC */

 

+#if (OSAL_ST_MODE == OSAL_ST_MODE_FREERUNNING) || defined(__DOXYGEN__)

+#if NRF51_ST_USE_RTC0 == TRUE

+/**

+ * @brief   System Timer vector (RTC0)

+ * @details This interrupt is used for freerunning mode (tick-less)

+ *          if selected with NRF51_ST_USE_RTC0 

+ *

+ * @isr

+ */

+OSAL_IRQ_HANDLER(Vector6C) {

+

+  OSAL_IRQ_PROLOGUE();

+

+  if (NRF_RTC0->EVENTS_COMPARE[0]) {

+      NRF_RTC0->EVENTS_COMPARE[0] = 0;

+      

+      osalSysLockFromISR();

+      osalOsTimerHandlerI();

+      osalSysUnlockFromISR();

+  }

+

+#if OSAL_ST_RESOLUTION == 16

+  if (NRF_RTC0->EVENTS_COMPARE[1]) {

+      NRF_RTC0->EVENTS_COMPARE[1] = 0;

+      NRF_RTC0->TASKS_CLEAR = 1;

+  }

+#endif

+  

+  OSAL_IRQ_EPILOGUE();

+}

+#endif

+

+#if NRF51_ST_USE_RTC1 == TRUE

+/**

+ * @brief   System Timer vector (RTC1)

+ * @details This interrupt is used for freerunning mode (tick-less)

+ *          if selected with NRF51_ST_USE_RTC1

+ *

+ * @isr

+ */

+OSAL_IRQ_HANDLER(Vector84) {

+

+  OSAL_IRQ_PROLOGUE();

+

+  if (NRF_RTC1->EVENTS_COMPARE[0]) {

+      NRF_RTC1->EVENTS_COMPARE[0] = 0;

+      

+      osalSysLockFromISR();

+      osalOsTimerHandlerI();

+      osalSysUnlockFromISR();

+  }

+

+#if OSAL_ST_RESOLUTION == 16

+  if (NRF_RTC1->EVENTS_COMPARE[1]) {

+      NRF_RTC1->EVENTS_COMPARE[1] = 0;

+      NRF_RTC1->TASKS_CLEAR = 1;

+  }

+#endif

+  

+  OSAL_IRQ_EPILOGUE();

+}

+#endif

+#endif /* OSAL_ST_MODE == OSAL_ST_MODE_FREERUNNING */

+

 /*===========================================================================*/

 /* Driver exported functions.                                                */

 /*===========================================================================*/

@@ -83,8 +195,75 @@ OSAL_IRQ_HANDLER(Vector60) {
  * @notapi

  */

 void st_lld_init(void) {

+#if OSAL_ST_MODE == OSAL_ST_MODE_FREERUNNING

+

+#if NRF51_ST_USE_RTC0 == TRUE

+  /* Using RTC with prescaler */

+  NRF_RTC0->TASKS_STOP  = 1;

+  NRF_RTC0->PRESCALER   = (NRF51_LFCLK_FREQUENCY / OSAL_ST_FREQUENCY) - 1; 

+  NRF_RTC0->EVTENCLR    = RTC_EVTENSET_COMPARE0_Msk;

+  NRF_RTC0->EVENTS_COMPARE[0] = 0;

+  NRF_RTC0->INTENSET    = RTC_INTENSET_COMPARE0_Msk;

+#if OSAL_ST_RESOLUTION == 16

+  NRF_RTC0->CC[1]       = 0x10000; /* 2^16 */

+  NRF_RTC0->EVENTS_COMPARE[1] = 0;

+  NRF_RTC0->EVTENSET    = RTC_EVTENSET_COMPARE0_Msk;

+  NRF_RTC0->INTENSET    = RTC_INTENSET_COMPARE1_Msk;

+#endif

+  NRF_RTC0->TASKS_CLEAR  = 1;

+  

+    /* Start timer */

+  nvicEnableVector(RTC0_IRQn, NRF51_ST_PRIORITY);

+  NRF_RTC0->TASKS_START = 1;

+#endif /* NRF51_ST_USE_RTC0 == TRUE */

+

+#if NRF51_ST_USE_RTC1 == TRUE

+  /* Using RTC with prescaler */

+  NRF_RTC1->TASKS_STOP  = 1;

+  NRF_RTC1->PRESCALER   = (NRF51_LFCLK_FREQUENCY / OSAL_ST_FREQUENCY) - 1; 

+  NRF_RTC1->EVTENCLR    = RTC_EVTENSET_COMPARE0_Msk;

+  NRF_RTC1->EVENTS_COMPARE[0] = 0;

+  NRF_RTC1->INTENSET    = RTC_INTENSET_COMPARE0_Msk;

+#if OSAL_ST_RESOLUTION == 16

+  NRF_RTC1->CC[1]       = 0x10000; /* 2^16 */

+  NRF_RTC1->EVENTS_COMPARE[1] = 0;

+  NRF_RTC1->EVTENSET    = RTC_EVTENSET_COMPARE0_Msk;

+  NRF_RTC1->INTENSET    = RTC_INTENSET_COMPARE1_Msk;

+#endif

+  NRF_RTC1->TASKS_CLEAR  = 1;

+  

+    /* Start timer */

+  nvicEnableVector(RTC1_IRQn, NRF51_ST_PRIORITY);

+  NRF_RTC1->TASKS_START = 1;

+#endif /* NRF51_ST_USE_RTC1 == TRUE */

+

+#endif /* OSAL_ST_MODE == OSAL_ST_MODE_FREERUNNING */

 

 #if OSAL_ST_MODE == OSAL_ST_MODE_PERIODIC

+

+#if NRF51_ST_USE_RTC0 == TRUE

+  /* Using RTC with prescaler */

+  NRF_RTC0->TASKS_STOP  = 1;

+  NRF_RTC0->PRESCALER   = (NRF51_LFCLK_FREQUENCY / OSAL_ST_FREQUENCY) - 1; 

+  NRF_RTC0->INTENSET    = RTC_INTENSET_TICK_Msk;

+

+  /* Start timer */

+  nvicEnableVector(RTC0_IRQn, NRF51_ST_PRIORITY);

+  NRF_RTC0->TASKS_START = 1;

+#endif /* NRF51_ST_USE_RTC0 == TRUE */

+

+#if NRF51_ST_USE_RTC1 == TRUE

+  /* Using RTC with prescaler */

+  NRF_RTC1->TASKS_STOP  = 1;

+  NRF_RTC1->PRESCALER   = (NRF51_LFCLK_FREQUENCY / OSAL_ST_FREQUENCY) - 1; 

+  NRF_RTC1->INTENSET    = RTC_INTENSET_TICK_Msk;

+

+  /* Start timer */

+  nvicEnableVector(RTC1_IRQn, NRF51_ST_PRIORITY);

+  NRF_RTC1->TASKS_START = 1;

+#endif /* NRF51_ST_USE_RTC1 == TRUE */

+

+#if NRF51_ST_USE_TIMER0 == TRUE

   NRF_TIMER0->TASKS_CLEAR = 1;

 

   /*

@@ -102,12 +281,12 @@ void st_lld_init(void) {
   NRF_TIMER0->SHORTS = 1;

   NRF_TIMER0->INTENSET = 0x10000;

 

-  nvicEnableVector(TIMER0_IRQn, 8);

-

   /* Start timer */

+  nvicEnableVector(TIMER0_IRQn, NRF51_ST_PRIORITY);

   NRF_TIMER0->TASKS_START = 1;

-#endif

-

+#endif /* NRF51_ST_USE_TIMER0 == TRUE */

+  

+#endif /* OSAL_ST_MODE == OSAL_ST_MODE_PERIODIC */

 }

 

 #endif /* OSAL_ST_MODE != OSAL_ST_MODE_NONE */

diff --git a/os/hal/ports/NRF51/NRF51822/st_lld.h b/os/hal/ports/NRF51/NRF51822/st_lld.h
index 2e0672e..ab25dbb 100644
--- a/os/hal/ports/NRF51/NRF51822/st_lld.h
+++ b/os/hal/ports/NRF51/NRF51822/st_lld.h
@@ -27,6 +27,8 @@
 #ifndef _ST_LLD_H_

 #define _ST_LLD_H_

 

+#include "halconf.h"

+

 /*===========================================================================*/

 /* Driver constants.                                                         */

 /*===========================================================================*/

@@ -35,10 +37,92 @@
 /* Driver pre-compile time settings.                                         */

 /*===========================================================================*/

 

+/**

+ * @brief   Use RTC0 to generates system ticks

+ */

+#if !defined(NRF51_ST_USE_RTC0) || defined(__DOXYGEN__)

+#if !defined(SOFTDEVICE_PRESENT)

+#define NRF51_ST_USE_RTC0 		TRUE

+#else

+#define NRF51_ST_USE_RTC0 		FALSE

+#endif

+#endif

+

+/**

+ * @brief   Use RTC1 to generates system ticks

+ */

+#if !defined(NRF51_ST_USE_RTC1) || defined(__DOXYGEN__)

+#if !defined(SOFTDEVICE_PRESENT)

+#define NRF51_ST_USE_RTC1		FALSE

+#else

+#define NRF51_ST_USE_RTC1		TRUE

+#endif

+#endif

+

+/**

+ * @brief   Use TIMER0 to generates system ticks

+ */

+#if !defined(NRF51_ST_USE_TIMER0) || defined(__DOXYGEN__)

+#define NRF51_ST_USE_TIMER0		FALSE

+#endif

+

+/**

+ * @brief   ST interrupt priority level setting.

+ */

+#if !defined(NRF51_ST_PRIORITY) || defined(__DOXYGEN__)

+#if !defined(SOFTDEVICE_PRESENT)

+#define NRF51_ST_PRIORITY        8

+#else

+#define NRF51_ST_PRIORITY        1

+#endif

+#endif

+

 /*===========================================================================*/

 /* Derived constants and error checks.                                       */

 /*===========================================================================*/

 

+#if OSAL_ST_MODE != OSAL_ST_MODE_NONE

+#if (NRF51_ST_USE_TIMER0 == TRUE) && (NRF51_GPT_USE_TIMER0 == TRUE)

+#error "TIMER0 already used by GPT driver"

+#endif

+

+#if (NRF51_ST_USE_RTC0   == FALSE) && \

+    (NRF51_ST_USE_RTC1   == FALSE) && \

+    (NRF51_ST_USE_TIMER0 == FALSE)

+#error "One clock source is needed, enable one (RTC0, RTC1, or TIMER0)"

+#endif

+

+#if ((NRF51_ST_USE_RTC0   == TRUE ? 1 : 0) +	\

+     (NRF51_ST_USE_RTC1   == TRUE ? 1 : 0) +	\

+     (NRF51_ST_USE_TIMER0 == TRUE ? 1 : 0)) > 1

+#error "Only one clock source can be used (RTC0, RTC1, or TIMER0)"

+#endif

+

+#if defined(SOFTDEVICE_PRESENT)

+#if NRF51_ST_USE_RTC0 == TRUE

+#error "RTC0 cannot be used for system ticks when SOFTDEVICE present"

+#endif

+

+#if NRF51_ST_USE_TIMER0 == TRUE

+#error "TIMER0 cannot be used for system ticks when SOFTDEVICE present"

+#endif

+

+#if NRF51_ST_PRIORITY != 1

+#error "ST priority must be 1 when SOFTDEVICE present"

+#endif

+

+#endif /* defined(SOFTDEVICE_PRESENT) */

+#endif /* OSAL_ST_MODE != OSAL_ST_MODE_NONE */

+

+#if OSAL_ST_MODE == OSAL_ST_MODE_FREERUNNING

+#if defined(CH_CFG_ST_TIMEDELTA) && (CH_CFG_ST_TIMEDELTA < 5)

+#error "CH_CFG_ST_TIMEDELTA is too low"

+#endif

+#if NRF51_ST_USE_TIMER0 == TRUE

+#error "Freeruning (tick-less) mode not supported with TIMER, use RTC"

+#endif

+#endif /* OSAL_ST_MODE == OSAL_ST_MODE_FREERUNNING */

+

 /*===========================================================================*/

 /* Driver data structures and types.                                         */

 /*===========================================================================*/

@@ -71,8 +155,12 @@ extern "C" {
  * @notapi

  */

 static inline systime_t st_lld_get_counter(void) {

-

-  return (systime_t)0;

+#if NRF51_ST_USE_RTC0 == TRUE

+    return (systime_t)NRF_RTC0->COUNTER;

+#endif

+#if NRF51_ST_USE_RTC1 == TRUE

+    return (systime_t)NRF_RTC1->COUNTER;

+#endif

 }

 

 /**

@@ -85,8 +173,16 @@ static inline systime_t st_lld_get_counter(void) {
  * @notapi

  */

 static inline void st_lld_start_alarm(systime_t abstime) {

-

-  (void)abstime;

+#if NRF51_ST_USE_RTC0 == TRUE

+  NRF_RTC0->CC[0]               = abstime;

+  NRF_RTC0->EVENTS_COMPARE[0]   = 0;

+  NRF_RTC0->EVTENSET            = RTC_EVTENSET_COMPARE0_Msk;

+#endif

+#if NRF51_ST_USE_RTC1 == TRUE

+  NRF_RTC1->CC[0]               = abstime;

+  NRF_RTC1->EVENTS_COMPARE[0]   = 0;

+  NRF_RTC1->EVTENSET            = RTC_EVTENSET_COMPARE0_Msk;

+#endif

 }

 

 /**

@@ -95,7 +191,14 @@ static inline void st_lld_start_alarm(systime_t abstime) {
  * @notapi

  */

 static inline void st_lld_stop_alarm(void) {

-

+#if NRF51_ST_USE_RTC0 == TRUE

+  NRF_RTC0->EVTENCLR            = RTC_EVTENCLR_COMPARE0_Msk;

+  NRF_RTC0->EVENTS_COMPARE[0]   = 0;

+#endif

+#if NRF51_ST_USE_RTC1 == TRUE

+  NRF_RTC1->EVTENCLR            = RTC_EVTENCLR_COMPARE0_Msk;

+  NRF_RTC1->EVENTS_COMPARE[0]   = 0;

+#endif

 }

 

 /**

@@ -106,8 +209,12 @@ static inline void st_lld_stop_alarm(void) {
  * @notapi

  */

 static inline void st_lld_set_alarm(systime_t abstime) {

-

-  (void)abstime;

+#if NRF51_ST_USE_RTC0 == TRUE

+    NRF_RTC0->CC[0]             = abstime;

+#endif

+#if NRF51_ST_USE_RTC1 == TRUE

+    NRF_RTC1->CC[0]             = abstime;

+#endif

 }

 

 /**

@@ -118,8 +225,12 @@ static inline void st_lld_set_alarm(systime_t abstime) {
  * @notapi

  */

 static inline systime_t st_lld_get_alarm(void) {

-

-  return (systime_t)0;

+#if NRF51_ST_USE_RTC0 == TRUE

+  return (systime_t)NRF_RTC0->CC[0];

+#endif

+#if NRF51_ST_USE_RTC1 == TRUE

+  return (systime_t)NRF_RTC1->CC[0];

+#endif

 }

 

 /**

@@ -132,8 +243,12 @@ static inline systime_t st_lld_get_alarm(void) {
  * @notapi

  */

 static inline bool st_lld_is_alarm_active(void) {

-

-  return false;

+#if NRF51_ST_USE_RTC0 == TRUE

+  return NRF_RTC0->EVTEN & RTC_EVTEN_COMPARE0_Msk;

+#endif

+#if NRF51_ST_USE_RTC1 == TRUE

+  return NRF_RTC1->EVTEN & RTC_EVTEN_COMPARE0_Msk;

+#endif

 }

 

 #endif /* _ST_LLD_H_ */

diff --git a/os/hal/ports/STM32/LLD/TIMv1/timcap_lld.c b/os/hal/ports/STM32/LLD/TIMv1/timcap_lld.c
new file mode 100644
index 0000000..635e9b2
--- /dev/null
+++ b/os/hal/ports/STM32/LLD/TIMv1/timcap_lld.c
@@ -0,0 +1,818 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006-2013 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.
+*/
+/*
+   This file was derived from the ICU subsystem code, modified to achieve
+   timing measurements on 2 and/or 4 channel STM32 timers by Dave Camarillo.
+ */
+/*
+   Concepts and parts of this file have been contributed by Fabio Utzig and
+   Xo Wang.
+ */
+
+
+/**
+ * @file    STM32/timcap_lld.c
+ * @brief   STM32 TIMCAP subsystem low level driver header.
+ *
+ * @addtogroup TIMCAP
+ * @{
+ */
+
+#include "ch.h"
+#include "hal.h"
+
+#if HAL_USE_TIMCAP || defined(__DOXYGEN__)
+
+#include "stm32_tim.h"
+#include "timcap.h"
+
+/*===========================================================================*/
+/* Driver local definitions.                                                 */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported variables.                                                */
+/*===========================================================================*/
+
+/**
+ * @brief   TIMCAPD1 driver identifier.
+ * @note    The driver TIMCAPD1 allocates the complex timer TIM1 when enabled.
+ */
+#if STM32_TIMCAP_USE_TIM1 || defined(__DOXYGEN__)
+TIMCAPDriver TIMCAPD1;
+#endif
+
+/**
+ * @brief   TIMCAPD2 driver identifier.
+ * @note    The driver TIMCAPD1 allocates the timer TIM2 when enabled.
+ */
+#if STM32_TIMCAP_USE_TIM2 || defined(__DOXYGEN__)
+TIMCAPDriver TIMCAPD2;
+#endif
+
+/**
+ * @brief   TIMCAPD3 driver identifier.
+ * @note    The driver TIMCAPD1 allocates the timer TIM3 when enabled.
+ */
+#if STM32_TIMCAP_USE_TIM3 || defined(__DOXYGEN__)
+TIMCAPDriver TIMCAPD3;
+#endif
+
+/**
+ * @brief   TIMCAPD4 driver identifier.
+ * @note    The driver TIMCAPD4 allocates the timer TIM4 when enabled.
+ */
+#if STM32_TIMCAP_USE_TIM4 || defined(__DOXYGEN__)
+TIMCAPDriver TIMCAPD4;
+#endif
+
+/**
+ * @brief   TIMCAPD5 driver identifier.
+ * @note    The driver TIMCAPD5 allocates the timer TIM5 when enabled.
+ */
+#if STM32_TIMCAP_USE_TIM5 || defined(__DOXYGEN__)
+TIMCAPDriver TIMCAPD5;
+#endif
+
+/**
+ * @brief   TIMCAPD8 driver identifier.
+ * @note    The driver TIMCAPD8 allocates the timer TIM8 when enabled.
+ */
+#if STM32_TIMCAP_USE_TIM8 || defined(__DOXYGEN__)
+TIMCAPDriver TIMCAPD8;
+#endif
+
+/**
+ * @brief   TIMCAPD9 driver identifier.
+ * @note    The driver TIMCAPD9 allocates the timer TIM9 when enabled.
+ */
+#if STM32_TIMCAP_USE_TIM9 || defined(__DOXYGEN__)
+TIMCAPDriver TIMCAPD9;
+#endif
+
+/*===========================================================================*/
+/* Driver local variables and types.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local functions.                                                   */
+/*===========================================================================*/
+
+
+/**
+ * @brief   Returns the maximum channel number for the respective TIMCAP driver.
+ *          Note: different timer perepherials on the STM32 have between 1 and 4
+ *          CCR registers.
+ *
+ * @param[in] timcapp      pointer to the @p TIMCAPDriver object
+ */
+static timcapchannel_t timcap_get_max_timer_channel(const TIMCAPDriver *timcapp) {
+  //Choose a sane default value
+#if STM32_TIMCAP_USE_TIM1 || defined(__DOXYGEN__)
+  if( timcapp == &TIMCAPD1 ) {
+    return(TIMCAP_CHANNEL_4);
+  }
+#endif
+
+#if STM32_TIMCAP_USE_TIM2 || defined(__DOXYGEN__)
+  if( timcapp == &TIMCAPD2 ) {
+    return(TIMCAP_CHANNEL_4);
+  }
+#endif
+
+#if STM32_TIMCAP_USE_TIM3 || defined(__DOXYGEN__)
+  if( timcapp == &TIMCAPD3 ) {
+    return(TIMCAP_CHANNEL_4);
+  }
+#endif
+
+#if STM32_TIMCAP_USE_TIM4 || defined(__DOXYGEN__)
+  if( timcapp == &TIMCAPD4 ) {
+    return(TIMCAP_CHANNEL_4);
+  }
+#endif
+
+#if STM32_TIMCAP_USE_TIM5 || defined(__DOXYGEN__)
+  if( timcapp == &TIMCAPD5 ) {
+    return(TIMCAP_CHANNEL_4);
+  }
+#endif
+
+#if STM32_TIMCAP_USE_TIM8 || defined(__DOXYGEN__)
+  if( timcapp == &TIMCAPD8 ) {
+    return(TIMCAP_CHANNEL_4);
+  }
+#endif
+
+#if STM32_TIMCAP_USE_TIM9 || defined(__DOXYGEN__)
+  if( timcapp == &TIMCAPD9 ) {
+    return(TIMCAP_CHANNEL_2);
+  }
+#endif
+
+  /*Return a conservative default value.*/
+  return(TIMCAP_CHANNEL_1);
+}
+
+
+/**
+ * @brief   Returns the maximum value for the ARR register of a given timer.
+ *
+ * @param[in] timcapp      pointer to the @p TIMCAPDriver object
+ */
+static uint32_t timcap_get_max_arr(const TIMCAPDriver *timcapp) {
+  //Choose a sane default value
+#if STM32_TIMCAP_USE_TIM1 || defined(__DOXYGEN__)
+  if( timcapp == &TIMCAPD1 ) {
+    return(UINT16_MAX);
+  }
+#endif
+
+#if STM32_TIMCAP_USE_TIM2 || defined(__DOXYGEN__)
+  if( timcapp == &TIMCAPD2 ) {
+    return(UINT32_MAX);
+  }
+#endif
+
+#if STM32_TIMCAP_USE_TIM3 || defined(__DOXYGEN__)
+  if( timcapp == &TIMCAPD3 ) {
+    return(UINT16_MAX);
+  }
+#endif
+
+#if STM32_TIMCAP_USE_TIM4 || defined(__DOXYGEN__)
+  if( timcapp == &TIMCAPD4 ) {
+    return(UINT16_MAX);
+  }
+#endif
+
+#if STM32_TIMCAP_USE_TIM5 || defined(__DOXYGEN__)
+  if( timcapp == &TIMCAPD5 ) {
+    return(UINT32_MAX);
+  }
+#endif
+
+#if STM32_TIMCAP_USE_TIM8 || defined(__DOXYGEN__)
+  if( timcapp == &TIMCAPD8 ) {
+    return(UINT16_MAX);
+  }
+#endif
+
+#if STM32_TIMCAP_USE_TIM9 || defined(__DOXYGEN__)
+  if( timcapp == &TIMCAPD9 ) {
+    return(UINT16_MAX);
+  }
+#endif
+
+  /*Return a conservative default value.*/
+  return(UINT16_MAX);
+}
+
+/**
+ * @brief   Shared IRQ handler.
+ *
+ * @param[in] timcapp      pointer to the @p TIMCAPDriver object
+ */
+static void timcap_lld_serve_interrupt(TIMCAPDriver *timcapp) {
+  uint16_t sr;
+
+  sr  = timcapp->tim->SR;
+  sr &= timcapp->tim->DIER & STM32_TIM_DIER_IRQ_MASK;
+  timcapp->tim->SR = ~sr;
+
+  if ((sr & STM32_TIM_SR_CC1IF) != 0 && timcapp->config->capture_cb_array[TIMCAP_CHANNEL_1] != NULL )
+    _timcap_isr_invoke_channel1_cb(timcapp);
+
+  if ((sr & STM32_TIM_SR_CC2IF) != 0 && timcapp->config->capture_cb_array[TIMCAP_CHANNEL_2] != NULL )
+    _timcap_isr_invoke_channel2_cb(timcapp);
+
+  if ((sr & STM32_TIM_SR_CC3IF) != 0 && timcapp->config->capture_cb_array[TIMCAP_CHANNEL_3] != NULL )
+    _timcap_isr_invoke_channel3_cb(timcapp);
+
+  if ((sr & STM32_TIM_SR_CC4IF) != 0 && timcapp->config->capture_cb_array[TIMCAP_CHANNEL_4] != NULL )
+    _timcap_isr_invoke_channel4_cb(timcapp);
+
+  if ((sr & STM32_TIM_SR_UIF) != 0 && timcapp->config->overflow_cb != NULL)
+    _timcap_isr_invoke_overflow_cb(timcapp);
+}
+
+/*===========================================================================*/
+/* Driver interrupt handlers.                                                */
+/*===========================================================================*/
+
+#if STM32_TIMCAP_USE_TIM1
+#if !defined(STM32_TIM1_UP_HANDLER)
+#error "STM32_TIM1_UP_HANDLER not defined"
+#endif
+/**
+ * @brief   TIM1 compare interrupt handler.
+ * @note    It is assumed that the various sources are only activated if the
+ *          associated callback pointer is not equal to @p NULL in order to not
+ *          perform an extra check in a potentially critical interrupt handler.
+ *
+ * @isr
+ */
+CH_IRQ_HANDLER(STM32_TIM1_UP_HANDLER) {
+
+  CH_IRQ_PROLOGUE();
+
+  timcap_lld_serve_interrupt(&TIMCAPD1);
+
+  CH_IRQ_EPILOGUE();
+}
+
+#if !defined(STM32_TIM1_CC_HANDLER)
+#error "STM32_TIM1_CC_HANDLER not defined"
+#endif
+/**
+ * @brief   TIM1 compare interrupt handler.
+ * @note    It is assumed that the various sources are only activated if the
+ *          associated callback pointer is not equal to @p NULL in order to not
+ *          perform an extra check in a potentially critical interrupt handler.
+ *
+ * @isr
+ */
+CH_IRQ_HANDLER(STM32_TIM1_CC_HANDLER) {
+
+  CH_IRQ_PROLOGUE();
+
+  timcap_lld_serve_interrupt(&TIMCAPD1);
+
+  CH_IRQ_EPILOGUE();
+}
+#endif /* STM32_TIMCAP_USE_TIM1 */
+
+#if STM32_TIMCAP_USE_TIM2
+#if !defined(STM32_TIM2_HANDLER)
+#error "STM32_TIM2_HANDLER not defined"
+#endif
+/**
+ * @brief   TIM2 interrupt handler.
+ * @note    It is assumed that the various sources are only activated if the
+ *          associated callback pointer is not equal to @p NULL in order to not
+ *          perform an extra check in a potentially critical interrupt handler.
+ *
+ * @isr
+ */
+CH_IRQ_HANDLER(STM32_TIM2_HANDLER) {
+
+  CH_IRQ_PROLOGUE();
+
+  timcap_lld_serve_interrupt(&TIMCAPD2);
+
+  CH_IRQ_EPILOGUE();
+}
+#endif /* STM32_TIMCAP_USE_TIM2 */
+
+#if STM32_TIMCAP_USE_TIM3
+#if !defined(STM32_TIM3_HANDLER)
+#error "STM32_TIM3_HANDLER not defined"
+#endif
+/**
+ * @brief   TIM3 interrupt handler.
+ * @note    It is assumed that the various sources are only activated if the
+ *          associated callback pointer is not equal to @p NULL in order to not
+ *          perform an extra check in a potentially critical interrupt handler.
+ *
+ * @isr
+ */
+CH_IRQ_HANDLER(STM32_TIM3_HANDLER) {
+
+  CH_IRQ_PROLOGUE();
+
+  timcap_lld_serve_interrupt(&TIMCAPD3);
+
+  CH_IRQ_EPILOGUE();
+}
+#endif /* STM32_TIMCAP_USE_TIM3 */
+
+#if STM32_TIMCAP_USE_TIM4
+#if !defined(STM32_TIM4_HANDLER)
+#error "STM32_TIM4_HANDLER not defined"
+#endif
+/**
+ * @brief   TIM4 interrupt handler.
+ * @note    It is assumed that the various sources are only activated if the
+ *          associated callback pointer is not equal to @p NULL in order to not
+ *          perform an extra check in a potentially critical interrupt handler.
+ *
+ * @isr
+ */
+CH_IRQ_HANDLER(STM32_TIM4_HANDLER) {
+
+  CH_IRQ_PROLOGUE();
+
+  timcap_lld_serve_interrupt(&TIMCAPD4);
+
+  CH_IRQ_EPILOGUE();
+}
+#endif /* STM32_TIMCAP_USE_TIM4 */
+
+#if STM32_TIMCAP_USE_TIM5
+#if !defined(STM32_TIM5_HANDLER)
+#error "STM32_TIM5_HANDLER not defined"
+#endif
+/**
+ * @brief   TIM5 interrupt handler.
+ * @note    It is assumed that the various sources are only activated if the
+ *          associated callback pointer is not equal to @p NULL in order to not
+ *          perform an extra check in a potentially critical interrupt handler.
+ *
+ * @isr
+ */
+CH_IRQ_HANDLER(STM32_TIM5_HANDLER) {
+
+  CH_IRQ_PROLOGUE();
+
+  timcap_lld_serve_interrupt(&TIMCAPD5);
+
+  CH_IRQ_EPILOGUE();
+}
+#endif /* STM32_TIMCAP_USE_TIM5 */
+
+#if STM32_TIMCAP_USE_TIM8
+#if !defined(STM32_TIM8_UP_HANDLER)
+#error "STM32_TIM8_UP_HANDLER not defined"
+#endif
+/**
+ * @brief   TIM8 compare interrupt handler.
+ * @note    It is assumed that the various sources are only activated if the
+ *          associated callback pointer is not equal to @p NULL in order to not
+ *          perform an extra check in a potentially critical interrupt handler.
+ *
+ * @isr
+ */
+CH_IRQ_HANDLER(STM32_TIM8_UP_HANDLER) {
+
+  CH_IRQ_PROLOGUE();
+
+  timcap_lld_serve_interrupt(&TIMCAPD8);
+
+  CH_IRQ_EPILOGUE();
+}
+
+#if !defined(STM32_TIM8_CC_HANDLER)
+#error "STM32_TIM8_CC_HANDLER not defined"
+#endif
+/**
+ * @brief   TIM8 compare interrupt handler.
+ * @note    It is assumed that the various sources are only activated if the
+ *          associated callback pointer is not equal to @p NULL in order to not
+ *          perform an extra check in a potentially critical interrupt handler.
+ *
+ * @isr
+ */
+CH_IRQ_HANDLER(STM32_TIM8_CC_HANDLER) {
+
+  CH_IRQ_PROLOGUE();
+
+  timcap_lld_serve_interrupt(&TIMCAPD8);
+
+  CH_IRQ_EPILOGUE();
+}
+#endif /* STM32_TIMCAP_USE_TIM8 */
+
+#if STM32_TIMCAP_USE_TIM9
+#if !defined(STM32_TIM9_HANDLER)
+#error "STM32_TIM9_HANDLER not defined"
+#endif
+/**
+ * @brief   TIM9 interrupt handler.
+ * @note    It is assumed that the various sources are only activated if the
+ *          associated callback pointer is not equal to @p NULL in order to not
+ *          perform an extra check in a potentially critical interrupt handler.
+ *
+ * @isr
+ */
+CH_IRQ_HANDLER(STM32_TIM9_HANDLER) {
+
+  CH_IRQ_PROLOGUE();
+
+  timcap_lld_serve_interrupt(&TIMCAPD9);
+
+  CH_IRQ_EPILOGUE();
+}
+#endif /* STM32_TIMCAP_USE_TIM9 */
+
+/*===========================================================================*/
+/* Driver exported functions.                                                */
+/*===========================================================================*/
+
+/**
+ * @brief   Low level TIMCAP driver initialization.
+ *
+ * @notapi
+ */
+void timcap_lld_init(void) {
+
+#if STM32_TIMCAP_USE_TIM1
+  /* Driver initialization.*/
+  timcapObjectInit(&TIMCAPD1);
+  TIMCAPD1.tim = STM32_TIM1;
+#endif
+
+#if STM32_TIMCAP_USE_TIM2
+  /* Driver initialization.*/
+  timcapObjectInit(&TIMCAPD2);
+  TIMCAPD2.tim = STM32_TIM2;
+#endif
+
+#if STM32_TIMCAP_USE_TIM3
+  /* Driver initialization.*/
+  timcapObjectInit(&TIMCAPD3);
+  TIMCAPD3.tim = STM32_TIM3;
+#endif
+
+#if STM32_TIMCAP_USE_TIM4
+  /* Driver initialization.*/
+  timcapObjectInit(&TIMCAPD4);
+  TIMCAPD4.tim = STM32_TIM4;
+#endif
+
+#if STM32_TIMCAP_USE_TIM5
+  /* Driver initialization.*/
+  timcapObjectInit(&TIMCAPD5);
+  TIMCAPD5.tim = STM32_TIM5;
+#endif
+
+#if STM32_TIMCAP_USE_TIM8
+  /* Driver initialization.*/
+  timcapObjectInit(&TIMCAPD8);
+  TIMCAPD8.tim = STM32_TIM8;
+#endif
+
+#if STM32_TIMCAP_USE_TIM9
+  /* Driver initialization.*/
+  timcapObjectInit(&TIMCAPD9);
+  TIMCAPD9.tim = STM32_TIM9;
+#endif
+}
+
+/**
+ * @brief   Configures and activates the TIMCAP peripheral.
+ *
+ * @param[in] timcapp      pointer to the @p TIMCAPDriver object
+ *
+ * @notapi
+ */
+void timcap_lld_start(TIMCAPDriver *timcapp) {
+  uint32_t psc;
+
+  const timcapchannel_t tim_max_channel = timcap_get_max_timer_channel(timcapp);
+
+  if (timcapp->state == TIMCAP_STOP) {
+    /* Clock activation and timer reset.*/
+#if STM32_TIMCAP_USE_TIM1
+    if (&TIMCAPD1 == timcapp) {
+      rccEnableTIM1(FALSE);
+      rccResetTIM1();
+      nvicEnableVector(STM32_TIM1_UP_NUMBER, STM32_TIMCAP_TIM1_IRQ_PRIORITY);
+      nvicEnableVector(STM32_TIM1_CC_NUMBER, STM32_TIMCAP_TIM1_IRQ_PRIORITY);
+#if defined(STM32_TIM1CLK)
+      timcapp->clock = STM32_TIM1CLK;
+#else
+      timcapp->clock = STM32_TIMCLK2;
+#endif
+    }
+#endif
+#if STM32_TIMCAP_USE_TIM2
+    if (&TIMCAPD2 == timcapp) {
+      rccEnableTIM2(FALSE);
+      rccResetTIM2();
+      nvicEnableVector(STM32_TIM2_NUMBER, STM32_TIMCAP_TIM2_IRQ_PRIORITY);
+      timcapp->clock = STM32_TIMCLK1;
+    }
+#endif
+#if STM32_TIMCAP_USE_TIM3
+    if (&TIMCAPD3 == timcapp) {
+      rccEnableTIM3(FALSE);
+      rccResetTIM3();
+      nvicEnableVector(STM32_TIM3_NUMBER, STM32_TIMCAP_TIM3_IRQ_PRIORITY);
+      timcapp->clock = STM32_TIMCLK1;
+    }
+#endif
+#if STM32_TIMCAP_USE_TIM4
+    if (&TIMCAPD4 == timcapp) {
+      rccEnableTIM4(FALSE);
+      rccResetTIM4();
+      nvicEnableVector(STM32_TIM4_NUMBER, STM32_TIMCAP_TIM4_IRQ_PRIORITY);
+      timcapp->clock = STM32_TIMCLK1;
+    }
+#endif
+#if STM32_TIMCAP_USE_TIM5
+    if (&TIMCAPD5 == timcapp) {
+      rccEnableTIM5(FALSE);
+      rccResetTIM5();
+      nvicEnableVector(STM32_TIM5_NUMBER, STM32_TIMCAP_TIM5_IRQ_PRIORITY);
+      timcapp->clock = STM32_TIMCLK1;
+    }
+#endif
+#if STM32_TIMCAP_USE_TIM8
+    if (&TIMCAPD8 == timcapp) {
+      rccEnableTIM8(FALSE);
+      rccResetTIM8();
+      nvicEnableVector(STM32_TIM8_UP_NUMBER, STM32_TIMCAP_TIM8_IRQ_PRIORITY);
+      nvicEnableVector(STM32_TIM8_CC_NUMBER, STM32_TIMCAP_TIM8_IRQ_PRIORITY);
+#if defined(STM32_TIM8CLK)
+      timcapp->clock = STM32_TIM8CLK;
+#else
+      timcapp->clock = STM32_TIMCLK2;
+#endif
+    }
+#endif
+#if STM32_TIMCAP_USE_TIM9
+    if (&TIMCAPD9 == timcapp) {
+      rccEnableTIM9(FALSE);
+      rccResetTIM9();
+      nvicEnableVector(STM32_TIM9_NUMBER, STM32_TIMCAP_TIM9_IRQ_PRIORITY);
+      timcapp->clock = STM32_TIMCLK1;
+    }
+#endif
+  }
+  else {
+    /* Driver re-configuration scenario, it must be stopped first.*/
+    timcapp->tim->CR1    = 0;                  /* Timer disabled.              */
+    timcapp->tim->DIER   = timcapp->config->dier &/* DMA-related DIER settings.   */
+                        ~STM32_TIM_DIER_IRQ_MASK;
+    timcapp->tim->SR     = 0;                  /* Clear eventual pending IRQs. */
+    timcapp->tim->CCR[0] = 0;                  /* Comparator 1 disabled.       */
+    timcapp->tim->CCR[1] = 0;                  /* Comparator 2 disabled.       */
+    if( tim_max_channel >= TIMCAP_CHANNEL_3 )
+      timcapp->tim->CCR[2] = 0;                /* Comparator 3 disabled.       */
+    if( tim_max_channel >= TIMCAP_CHANNEL_4 )
+      timcapp->tim->CCR[3] = 0;                /* Comparator 4 disabled.       */
+    timcapp->tim->CNT    = 0;                  /* Counter reset to zero.       */
+  }
+
+  /* Timer configuration.*/
+  psc = (timcapp->clock / timcapp->config->frequency) - 1;
+  osalDbgAssert((psc <= 0xFFFF) &&
+              ((psc + 1) * timcapp->config->frequency) == timcapp->clock,
+              "invalid frequency");
+  timcapp->tim->PSC  = (uint16_t)psc;
+  timcapp->tim->ARR = timcap_get_max_arr(timcapp);
+
+  timcapp->tim->CCMR1 = 0;
+  timcapp->tim->CCMR2 = 0;
+  timcapp->tim->CCER = 0;
+
+  timcapchannel_t chan = TIMCAP_CHANNEL_1;
+
+  /*go through each non-NULL callback channel and enable the capture register on rising/falling edge*/
+  for( chan = TIMCAP_CHANNEL_1; chan <= tim_max_channel; chan++ ) {
+    if( timcapp->config->capture_cb_array[chan] == NULL ) {
+      continue;
+    }
+
+    switch (chan) {
+      case TIMCAP_CHANNEL_1:
+        /*CCMR1_CC1S = 01 = CH1 Input on TI1.*/
+        timcapp->tim->CCMR1 |= STM32_TIM_CCMR1_CC1S(1);
+        break;
+      case TIMCAP_CHANNEL_2:
+        /*CCMR1_CC2S = 10 = CH2 Input on TI1.*/
+        timcapp->tim->CCMR1 |= STM32_TIM_CCMR1_CC2S(1);
+        break;
+      case TIMCAP_CHANNEL_3:
+        timcapp->tim->CCMR2 |= STM32_TIM_CCMR2_CC3S(1);
+        break;
+      case TIMCAP_CHANNEL_4:
+        timcapp->tim->CCMR2 |= STM32_TIM_CCMR2_CC4S(1);
+        break;
+    }
+
+    /* The CCER settings depend on the selected trigger mode.
+       TIMCAP_INPUT_DISABLED: Input not used.
+       TIMCAP_INPUT_ACTIVE_HIGH: Active on rising edge, idle on falling edge.
+       TIMCAP_INPUT_ACTIVE_LOW:  Active on falling edge, idle on rising edge.*/
+    if (timcapp->config->modes[chan] == TIMCAP_INPUT_ACTIVE_HIGH) {
+      switch (chan) {
+        case TIMCAP_CHANNEL_1:
+          timcapp->tim->CCER |= STM32_TIM_CCER_CC1E;
+          break;
+        case TIMCAP_CHANNEL_2:
+          timcapp->tim->CCER |= STM32_TIM_CCER_CC2E;
+          break;
+        case TIMCAP_CHANNEL_3:
+          timcapp->tim->CCER |= STM32_TIM_CCER_CC3E;
+          break;
+        case TIMCAP_CHANNEL_4:
+          timcapp->tim->CCER |= STM32_TIM_CCER_CC4E;
+          break;
+      }
+    }
+    else if (timcapp->config->modes[chan] == TIMCAP_INPUT_ACTIVE_LOW) {
+      switch (chan) {
+        case TIMCAP_CHANNEL_1:
+          timcapp->tim->CCER |= STM32_TIM_CCER_CC1E | STM32_TIM_CCER_CC1P;
+          break;
+        case TIMCAP_CHANNEL_2:
+          timcapp->tim->CCER |= STM32_TIM_CCER_CC2E | STM32_TIM_CCER_CC2P;
+          break;
+        case TIMCAP_CHANNEL_3:
+          timcapp->tim->CCER |= STM32_TIM_CCER_CC3E | STM32_TIM_CCER_CC3P;
+          break;
+        case TIMCAP_CHANNEL_4:
+          timcapp->tim->CCER |= STM32_TIM_CCER_CC4E | STM32_TIM_CCER_CC4P;
+          break;
+      }
+    }
+    else {
+      switch (chan) {
+        case TIMCAP_CHANNEL_1:
+          timcapp->tim->CCER &= ~STM32_TIM_CCER_CC1E;
+          break;
+        case TIMCAP_CHANNEL_2:
+          timcapp->tim->CCER &= ~STM32_TIM_CCER_CC2E;
+          break;
+        case TIMCAP_CHANNEL_3:
+          timcapp->tim->CCER &= ~STM32_TIM_CCER_CC3E;
+          break;
+        case TIMCAP_CHANNEL_4:
+          timcapp->tim->CCER &= ~STM32_TIM_CCER_CC4E;
+          break;
+      }
+    }
+    /* Direct pointers to the capture registers in order to make reading
+         data faster from within callbacks.*/
+    timcapp->ccr_p[chan] = &timcapp->tim->CCR[chan];
+  }
+
+  /* SMCR_TS  = 101, input is TI1FP1.*/
+  timcapp->tim->SMCR  = STM32_TIM_SMCR_TS(5);
+}
+
+/**
+ * @brief   Deactivates the TIMCAP peripheral.
+ *
+ * @param[in] timcapp      pointer to the @p TIMCAPDriver object
+ *
+ * @notapi
+ */
+void timcap_lld_stop(TIMCAPDriver *timcapp) {
+
+  if (timcapp->state == TIMCAP_READY) {
+    /* Clock deactivation.*/
+    timcapp->tim->CR1  = 0;                    /* Timer disabled.              */
+    timcapp->tim->DIER = 0;                    /* All IRQs disabled.           */
+    timcapp->tim->SR   = 0;                    /* Clear eventual pending IRQs. */
+
+#if STM32_TIMCAP_USE_TIM1
+    if (&TIMCAPD1 == timcapp) {
+      nvicDisableVector(STM32_TIM1_UP_NUMBER);
+      nvicDisableVector(STM32_TIM1_CC_NUMBER);
+      rccDisableTIM1(FALSE);
+    }
+#endif
+#if STM32_TIMCAP_USE_TIM2
+    if (&TIMCAPD2 == timcapp) {
+      nvicDisableVector(STM32_TIM2_NUMBER);
+      rccDisableTIM2(FALSE);
+    }
+#endif
+#if STM32_TIMCAP_USE_TIM3
+    if (&TIMCAPD3 == timcapp) {
+      nvicDisableVector(STM32_TIM3_NUMBER);
+      rccDisableTIM3(FALSE);
+    }
+#endif
+#if STM32_TIMCAP_USE_TIM4
+    if (&TIMCAPD4 == timcapp) {
+      nvicDisableVector(STM32_TIM4_NUMBER);
+      rccDisableTIM4(FALSE);
+    }
+#endif
+#if STM32_TIMCAP_USE_TIM5
+    if (&TIMCAPD5 == timcapp) {
+      nvicDisableVector(STM32_TIM5_NUMBER);
+      rccDisableTIM5(FALSE);
+    }
+#endif
+#if STM32_TIMCAP_USE_TIM8
+    if (&TIMCAPD8 == timcapp) {
+      nvicDisableVector(STM32_TIM8_UP_NUMBER);
+      nvicDisableVector(STM32_TIM8_CC_NUMBER);
+      rccDisableTIM8(FALSE);
+    }
+#endif
+#if STM32_TIMCAP_USE_TIM9
+    if (&TIMCAPD9 == timcapp) {
+      nvicDisableVector(STM32_TIM9_NUMBER);
+      rccDisableTIM9(FALSE);
+    }
+#endif
+  }
+}
+
+/**
+ * @brief   Enables the input capture.
+ *
+ * @param[in] timcapp      pointer to the @p TIMCAPDriver object
+ *
+ * @notapi
+ */
+void timcap_lld_enable(TIMCAPDriver *timcapp) {
+
+  timcapp->tim->EGR |= STM32_TIM_EGR_UG;
+  timcapp->tim->SR = 0;                        /* Clear pending IRQs (if any). */
+
+  timcapchannel_t chan = TIMCAP_CHANNEL_1;
+  const timcapchannel_t tim_max_channel = timcap_get_max_timer_channel(timcapp);
+  for( chan = TIMCAP_CHANNEL_1; chan <= tim_max_channel; chan++ ) {
+    if( timcapp->config->capture_cb_array[chan] != NULL 
+      && timcapp->config->modes[chan] != TIMCAP_INPUT_DISABLED ) {
+      switch (chan) {
+        case TIMCAP_CHANNEL_1:
+          timcapp->tim->DIER |= STM32_TIM_DIER_CC1IE;
+          break;
+        case TIMCAP_CHANNEL_2:
+          timcapp->tim->DIER |= STM32_TIM_DIER_CC2IE;
+          break;
+        case TIMCAP_CHANNEL_3:
+          timcapp->tim->DIER |= STM32_TIM_DIER_CC3IE;
+          break;
+        case TIMCAP_CHANNEL_4:
+          timcapp->tim->DIER |= STM32_TIM_DIER_CC4IE;
+          break;
+      }
+    }
+  }
+
+  if (timcapp->config->overflow_cb != NULL)
+    timcapp->tim->DIER |= STM32_TIM_DIER_UIE;
+  
+  timcapp->tim->CR1 = STM32_TIM_CR1_URS | STM32_TIM_CR1_CEN | timcapp->config->cr1;
+}
+
+/**
+ * @brief   Disables the input capture.
+ *
+ * @param[in] timcapp      pointer to the @p TIMCAPDriver object
+ *
+ * @notapi
+ */
+void timcap_lld_disable(TIMCAPDriver *timcapp) {
+
+  timcapp->tim->CR1   = 0;                     /* Initially stopped.           */
+  timcapp->tim->SR    = 0;                     /* Clear pending IRQs (if any). */
+
+  /* All interrupts disabled.*/
+  timcapp->tim->DIER &= ~STM32_TIM_DIER_IRQ_MASK;
+}
+
+#endif /* HAL_USE_TIMCAP */
+
+/** @} */
diff --git a/os/hal/ports/STM32/LLD/TIMv1/timcap_lld.h b/os/hal/ports/STM32/LLD/TIMv1/timcap_lld.h
new file mode 100644
index 0000000..d39c438
--- /dev/null
+++ b/os/hal/ports/STM32/LLD/TIMv1/timcap_lld.h
@@ -0,0 +1,390 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006-2013 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    STM32/timcap_lld.h
+ * @brief   STM32 TIMCAP subsystem low level driver header.
+ *
+ * @addtogroup TIMCAP
+ * @{
+ */
+
+#ifndef _TIMCAP_LLD_H_
+#define _TIMCAP_LLD_H_
+
+#include "ch.h"
+#include "hal.h"
+#include "stm32_tim.h"
+
+
+#if HAL_USE_TIMCAP || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Driver constants.                                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/**
+ * @name    Configuration options
+ * @{
+ */
+/**
+ * @brief   TIMCAPD1 driver enable switch.
+ * @details If set to @p TRUE the support for TIMCAPD1 is included.
+ * @note    The default is @p TRUE.
+ */
+#if !defined(STM32_TIMCAP_USE_TIM1) || defined(__DOXYGEN__)
+#define STM32_TIMCAP_USE_TIM1                  FALSE
+#endif
+
+/**
+ * @brief   TIMCAPD2 driver enable switch.
+ * @details If set to @p TRUE the support for TIMCAPD2 is included.
+ * @note    The default is @p TRUE.
+ */
+#if !defined(STM32_TIMCAP_USE_TIM2) || defined(__DOXYGEN__)
+#define STM32_TIMCAP_USE_TIM2                  FALSE
+#endif
+
+/**
+ * @brief   TIMCAPD3 driver enable switch.
+ * @details If set to @p TRUE the support for TIMCAPD3 is included.
+ * @note    The default is @p TRUE.
+ */
+#if !defined(STM32_TIMCAP_USE_TIM3) || defined(__DOXYGEN__)
+#define STM32_TIMCAP_USE_TIM3                  FALSE
+#endif
+
+/**
+ * @brief   TIMCAPD4 driver enable switch.
+ * @details If set to @p TRUE the support for TIMCAPD4 is included.
+ * @note    The default is @p TRUE.
+ */
+#if !defined(STM32_TIMCAP_USE_TIM4) || defined(__DOXYGEN__)
+#define STM32_TIMCAP_USE_TIM4                  FALSE
+#endif
+
+/**
+ * @brief   TIMCAPD5 driver enable switch.
+ * @details If set to @p TRUE the support for TIMCAPD5 is included.
+ * @note    The default is @p TRUE.
+ */
+#if !defined(STM32_TIMCAP_USE_TIM5) || defined(__DOXYGEN__)
+#define STM32_TIMCAP_USE_TIM5                  FALSE
+#endif
+
+/**
+ * @brief   TIMCAPD8 driver enable switch.
+ * @details If set to @p TRUE the support for TIMCAPD8 is included.
+ * @note    The default is @p TRUE.
+ */
+#if !defined(STM32_TIMCAP_USE_TIM8) || defined(__DOXYGEN__)
+#define STM32_TIMCAP_USE_TIM8                  FALSE
+#endif
+
+/**
+ * @brief   TIMCAPD9 driver enable switch.
+ * @details If set to @p TRUE the support for TIMCAPD9 is included.
+ * @note    The default is @p TRUE.
+ */
+#if !defined(STM32_TIMCAP_USE_TIM9) || defined(__DOXYGEN__)
+#define STM32_TIMCAP_USE_TIM9                  FALSE
+#endif
+
+/**
+ * @brief   TIMCAPD1 interrupt priority level setting.
+ */
+#if !defined(STM32_TIMCAP_TIM1_IRQ_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_TIMCAP_TIM1_IRQ_PRIORITY         7
+#endif
+
+/**
+ * @brief   TIMCAPD2 interrupt priority level setting.
+ */
+#if !defined(STM32_TIMCAP_TIM2_IRQ_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_TIMCAP_TIM2_IRQ_PRIORITY         7
+#endif
+
+/**
+ * @brief   TIMCAPD3 interrupt priority level setting.
+ */
+#if !defined(STM32_TIMCAP_TIM3_IRQ_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_TIMCAP_TIM3_IRQ_PRIORITY         7
+#endif
+
+/**
+ * @brief   TIMCAPD4 interrupt priority level setting.
+ */
+#if !defined(STM32_TIMCAP_TIM4_IRQ_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_TIMCAP_TIM4_IRQ_PRIORITY         7
+#endif
+
+/**
+ * @brief   TIMCAPD5 interrupt priority level setting.
+ */
+#if !defined(STM32_TIMCAP_TIM5_IRQ_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_TIMCAP_TIM5_IRQ_PRIORITY         7
+#endif
+
+/**
+ * @brief   TIMCAPD8 interrupt priority level setting.
+ */
+#if !defined(STM32_TIMCAP_TIM8_IRQ_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_TIMCAP_TIM8_IRQ_PRIORITY         7
+#endif
+
+/**
+ * @brief   TIMCAPD9 interrupt priority level setting.
+ */
+#if !defined(STM32_TIMCAP_TIM9_IRQ_PRIORITY) || defined(__DOXYGEN__)
+#define STM32_TIMCAP_TIM9_IRQ_PRIORITY         7
+#endif
+/** @} */
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+#if STM32_TIMCAP_USE_TIM1 && !STM32_HAS_TIM1
+#error "TIM1 not present in the selected device"
+#endif
+
+#if STM32_TIMCAP_USE_TIM2 && !STM32_HAS_TIM2
+#error "TIM2 not present in the selected device"
+#endif
+
+#if STM32_TIMCAP_USE_TIM3 && !STM32_HAS_TIM3
+#error "TIM3 not present in the selected device"
+#endif
+
+#if STM32_TIMCAP_USE_TIM4 && !STM32_HAS_TIM4
+#error "TIM4 not present in the selected device"
+#endif
+
+#if STM32_TIMCAP_USE_TIM5 && !STM32_HAS_TIM5
+#error "TIM5 not present in the selected device"
+#endif
+
+#if STM32_TIMCAP_USE_TIM8 && !STM32_HAS_TIM8
+#error "TIM8 not present in the selected device"
+#endif
+
+#if STM32_TIMCAP_USE_TIM9 && !STM32_HAS_TIM9
+#error "TIM9 not present in the selected device"
+#endif
+
+#if !STM32_TIMCAP_USE_TIM1 && !STM32_TIMCAP_USE_TIM2 &&                           \
+    !STM32_TIMCAP_USE_TIM3 && !STM32_TIMCAP_USE_TIM4 &&                           \
+    !STM32_TIMCAP_USE_TIM5 && !STM32_TIMCAP_USE_TIM8 &&                           \
+    !STM32_TIMCAP_USE_TIM9
+#error "TIMCAP driver activated but no TIM peripheral assigned"
+#endif
+
+#if STM32_TIMCAP_USE_TIM1 &&                                                   \
+    !OSAL_IRQ_IS_VALID_PRIORITY(STM32_TIMCAP_TIM1_IRQ_PRIORITY)
+#error "Invalid IRQ priority assigned to TIM1"
+#endif
+
+#if STM32_TIMCAP_USE_TIM2 &&                                                   \
+    !OSAL_IRQ_IS_VALID_PRIORITY(STM32_TIMCAP_TIM2_IRQ_PRIORITY)
+#error "Invalid IRQ priority assigned to TIM2"
+#endif
+
+#if STM32_TIMCAP_USE_TIM3 &&                                                   \
+    !OSAL_IRQ_IS_VALID_PRIORITY(STM32_TIMCAP_TIM3_IRQ_PRIORITY)
+#error "Invalid IRQ priority assigned to TIM3"
+#endif
+
+#if STM32_TIMCAP_USE_TIM4 &&                                                   \
+    !OSAL_IRQ_IS_VALID_PRIORITY(STM32_TIMCAP_TIM4_IRQ_PRIORITY)
+#error "Invalid IRQ priority assigned to TIM4"
+#endif
+
+#if STM32_TIMCAP_USE_TIM5 &&                                                   \
+    !OSAL_IRQ_IS_VALID_PRIORITY(STM32_TIMCAP_TIM5_IRQ_PRIORITY)
+#error "Invalid IRQ priority assigned to TIM5"
+#endif
+
+#if STM32_TIMCAP_USE_TIM8 &&                                                   \
+    !OSAL_IRQ_IS_VALID_PRIORITY(STM32_TIMCAP_TIM8_IRQ_PRIORITY)
+#error "Invalid IRQ priority assigned to TIM8"
+#endif
+
+#if STM32_TIMCAP_USE_TIM9 &&                                                   \
+    !OSAL_IRQ_IS_VALID_PRIORITY(STM32_TIMCAP_TIM9_IRQ_PRIORITY)
+#error "Invalid IRQ priority assigned to TIM9"
+#endif
+
+/*===========================================================================*/
+/* Driver data structures and types.                                         */
+/*===========================================================================*/
+
+/**
+ * @brief   TIMCAP driver mode.
+ */
+typedef enum {
+  TIMCAP_INPUT_DISABLED = 0,
+  TIMCAP_INPUT_ACTIVE_HIGH = 1,        /**< Trigger on rising edge.            */
+  TIMCAP_INPUT_ACTIVE_LOW = 2,         /**< Trigger on falling edge.           */
+} timcapmode_t;
+
+/**
+ * @brief   TIMCAP frequency type.
+ */
+typedef uint32_t timcapfreq_t;
+
+/**
+ * @brief   TIMCAP channel type.
+ */
+typedef enum {
+  TIMCAP_CHANNEL_1 = 0,              /**< Use TIMxCH1.      */
+  TIMCAP_CHANNEL_2 = 1,              /**< Use TIMxCH2.      */
+  TIMCAP_CHANNEL_3 = 2,              /**< Use TIMxCH3.      */
+  TIMCAP_CHANNEL_4 = 3,              /**< Use TIMxCH4.      */
+} timcapchannel_t;
+
+
+/**
+ * @brief   Driver configuration structure.
+ * @note    It could be empty on some architectures.
+ */
+typedef struct {
+  /**
+   * @brief   Driver mode.
+   */
+  timcapmode_t                 modes[4];
+  /**
+   * @brief   Timer clock in Hz.
+   * @note    The low level can use assertions in order to catch invalid
+   *          frequency specifications.
+   */
+  timcapfreq_t                 frequency;
+
+  /**
+   * @brief   Callback when a capture occurs
+   */
+  timcapcallback_t             capture_cb_array[4];
+
+  /**
+   * @brief   Callback for timer overflow.
+   */
+  timcapcallback_t             overflow_cb;
+
+  /* End of the mandatory fields.*/
+
+  /**
+   * @brief TIM DIER register initialization data.
+   * @note  The value of this field should normally be equal to zero.
+   * @note  Only the DMA-related bits can be specified in this field.
+   */
+  uint32_t                  dier;
+  
+  /**
+   * @brief TIM CR1 register initialization data.
+   * @note  The value of this field should normally be equal to zero.
+   */
+  uint32_t                  cr1;
+} TIMCAPConfig;
+
+/**
+ * @brief   Structure representing an TIMCAP driver.
+ */
+struct TIMCAPDriver {
+  /**
+   * @brief Driver state.
+   */
+  timcapstate_t                state;
+  /**
+   * @brief Current configuration data.
+   */
+  const TIMCAPConfig           *config;
+#if defined(TIMCAP_DRIVER_EXT_FIELDS)
+  TIMCAP_DRIVER_EXT_FIELDS
+#endif
+  /* End of the mandatory fields.*/
+  /**
+   * @brief Timer base clock.
+   */
+  uint32_t                  clock;
+  /**
+   * @brief Pointer to the TIMx registers block.
+   */
+  stm32_tim_t               *tim;
+  /**
+   * @brief CCR register used for capture.
+   */
+  volatile uint32_t         *ccr_p[4];
+};
+
+/*===========================================================================*/
+/* Driver macros.                                                            */
+/*===========================================================================*/
+
+//FIXME document this
+#define timcap_lld_get_ccr(timcapp, channel) (*((timcapp)->ccr_p[channel]) + 1)
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+#if STM32_TIMCAP_USE_TIM1 && !defined(__DOXYGEN__)
+extern TIMCAPDriver TIMCAPD1;
+#endif
+
+#if STM32_TIMCAP_USE_TIM2 && !defined(__DOXYGEN__)
+extern TIMCAPDriver TIMCAPD2;
+#endif
+
+#if STM32_TIMCAP_USE_TIM3 && !defined(__DOXYGEN__)
+extern TIMCAPDriver TIMCAPD3;
+#endif
+
+#if STM32_TIMCAP_USE_TIM4 && !defined(__DOXYGEN__)
+extern TIMCAPDriver TIMCAPD4;
+#endif
+
+#if STM32_TIMCAP_USE_TIM5 && !defined(__DOXYGEN__)
+extern TIMCAPDriver TIMCAPD5;
+#endif
+
+#if STM32_TIMCAP_USE_TIM8 && !defined(__DOXYGEN__)
+extern TIMCAPDriver TIMCAPD8;
+#endif
+
+#if STM32_TIMCAP_USE_TIM9 && !defined(__DOXYGEN__)
+extern TIMCAPDriver TIMCAPD9;
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+  void timcap_lld_init(void);
+  void timcap_lld_start(TIMCAPDriver *timcapp);
+  void timcap_lld_stop(TIMCAPDriver *timcapp);
+  void timcap_lld_enable(TIMCAPDriver *timcapp);
+  void timcap_lld_disable(TIMCAPDriver *timcapp);
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* HAL_USE_TIMCAP */
+
+#endif /* _TIMCAP_LLD_H_ */
+
+/** @} */
diff --git a/os/hal/ports/STM32/LLD/USBHv1/stm32_otg.h b/os/hal/ports/STM32/LLD/USBHv1/stm32_otg.h
new file mode 100644
index 0000000..268c9bf
--- /dev/null
+++ b/os/hal/ports/STM32/LLD/USBHv1/stm32_otg.h
@@ -0,0 +1,929 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 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    stm32_otg.h

+ * @brief   STM32 OTG registers layout header.

+ *

+ * @addtogroup USB

+ * @{

+ */

+

+

+#ifndef _STM32_OTG_H_

+#define _STM32_OTG_H_

+

+/**

+ * @brief   Number of the implemented endpoints in OTG_FS.

+ * @details This value does not include the endpoint 0 that is always present.

+ */

+#define STM32_OTG1_ENDOPOINTS_NUMBER    3

+

+/**

+ * @brief   Number of the implemented endpoints in OTG_HS.

+ * @details This value does not include the endpoint 0 that is always present.

+ */

+#define STM32_OTG2_ENDOPOINTS_NUMBER    5

+

+/**

+ * @brief   OTG_FS FIFO memory size in words.

+ */

+#define STM32_OTG1_FIFO_MEM_SIZE        320

+

+/**

+ * @brief   OTG_HS FIFO memory size in words.

+ */

+#define STM32_OTG2_FIFO_MEM_SIZE        1024

+

+/**

+ * @brief   Host channel registers group.

+ */

+typedef struct {

+  volatile uint32_t HCCHAR;     /**< @brief Host channel characteristics

+                                            register.                       */

+  volatile uint32_t resvd8;

+  volatile uint32_t HCINT;      /**< @brief Host channel interrupt register.*/

+  volatile uint32_t HCINTMSK;   /**< @brief Host channel interrupt mask

+                                            register.                       */

+  volatile uint32_t HCTSIZ;     /**< @brief Host channel transfer size

+                                            register.                       */

+  volatile uint32_t resvd14;

+  volatile uint32_t resvd18;

+  volatile uint32_t resvd1c;

+} stm32_otg_host_chn_t;

+

+/**

+ * @brief   Device input endpoint registers group.

+ */

+typedef struct {

+  volatile uint32_t DIEPCTL;    /**< @brief Device control IN endpoint

+                                            control register.               */

+  volatile uint32_t resvd4;

+  volatile uint32_t DIEPINT;    /**< @brief Device IN endpoint interrupt

+                                            register.                       */

+  volatile uint32_t resvdC;

+  volatile uint32_t DIEPTSIZ;   /**< @brief Device IN endpoint transfer size

+                                            register.                       */

+  volatile uint32_t resvd14;

+  volatile uint32_t DTXFSTS;    /**< @brief Device IN endpoint transmit FIFO

+                                            status register.                */

+  volatile uint32_t resvd1C;

+} stm32_otg_in_ep_t;

+

+/**

+ * @brief   Device output endpoint registers group.

+ */

+typedef struct {

+  volatile uint32_t DOEPCTL;    /**< @brief Device control OUT endpoint

+                                            control register.               */

+  volatile uint32_t resvd4;

+  volatile uint32_t DOEPINT;    /**< @brief Device OUT endpoint interrupt

+                                            register.                       */

+  volatile uint32_t resvdC;

+  volatile uint32_t DOEPTSIZ;   /**< @brief Device OUT endpoint transfer

+                                            size register.                  */

+  volatile uint32_t resvd14;

+  volatile uint32_t resvd18;

+  volatile uint32_t resvd1C;

+} stm32_otg_out_ep_t;

+

+/**

+ * @brief   USB registers memory map.

+ */

+typedef struct {

+  volatile uint32_t GOTGCTL;    /**< @brief OTG control and status register.*/

+  volatile uint32_t GOTGINT;    /**< @brief OTG interrupt register.         */

+  volatile uint32_t GAHBCFG;    /**< @brief AHB configuration register.     */

+  volatile uint32_t GUSBCFG;    /**< @brief USB configuration register.     */

+  volatile uint32_t GRSTCTL;    /**< @brief Reset register size.            */

+  volatile uint32_t GINTSTS;    /**< @brief Interrupt register.             */

+  volatile uint32_t GINTMSK;    /**< @brief Interrupt mask register.        */

+  volatile uint32_t GRXSTSR;    /**< @brief Receive status debug read

+                                            register.                       */

+  volatile uint32_t GRXSTSP;    /**< @brief Receive status read/pop

+                                            register.                       */

+  volatile uint32_t GRXFSIZ;    /**< @brief Receive FIFO size register.     */

+  volatile uint32_t DIEPTXF0;   /**< @brief Endpoint 0 transmit FIFO size

+                                            register.                       */

+  volatile uint32_t HNPTXSTS;   /**< @brief Non-periodic transmit FIFO/queue

+                                            status register.                */

+  volatile uint32_t resvd30;

+  volatile uint32_t resvd34;

+  volatile uint32_t GCCFG;      /**< @brief General core configuration.     */

+  volatile uint32_t CID;        /**< @brief Core ID register.               */

+  volatile uint32_t resvd58[48];

+  volatile uint32_t HPTXFSIZ;   /**< @brief Host periodic transmit FIFO size

+                                            register.                       */

+  volatile uint32_t DIEPTXF[15];/**< @brief Device IN endpoint transmit FIFO

+                                            size registers.                 */

+  volatile uint32_t resvd140[176];

+  volatile uint32_t HCFG;       /**< @brief Host configuration register.    */

+  volatile uint32_t HFIR;       /**< @brief Host frame interval register.   */

+  volatile uint32_t HFNUM;      /**< @brief Host frame number/frame time

+                                            Remaining register.             */

+  volatile uint32_t resvd40C;

+  volatile uint32_t HPTXSTS;    /**< @brief Host periodic transmit FIFO/queue

+                                            status register.                */

+  volatile uint32_t HAINT;      /**< @brief Host all channels interrupt

+                                            register.                       */

+  volatile uint32_t HAINTMSK;   /**< @brief Host all channels interrupt mask

+                                            register.                       */

+  volatile uint32_t resvd41C[9];

+  volatile uint32_t HPRT;       /**< @brief Host port control and status

+                                            register.                       */

+  volatile uint32_t resvd444[47];

+  stm32_otg_host_chn_t hc[16];  /**< @brief Host channels array.            */

+  volatile uint32_t resvd700[64];

+  volatile uint32_t DCFG;       /**< @brief Device configuration register.  */

+  volatile uint32_t DCTL;       /**< @brief Device control register.        */

+  volatile uint32_t DSTS;       /**< @brief Device status register.         */

+  volatile uint32_t resvd80C;

+  volatile uint32_t DIEPMSK;    /**< @brief Device IN endpoint common

+                                            interrupt mask register.        */

+  volatile uint32_t DOEPMSK;    /**< @brief Device OUT endpoint common

+                                            interrupt mask register.        */

+  volatile uint32_t DAINT;      /**< @brief Device all endpoints interrupt

+                                            register.                       */

+  volatile uint32_t DAINTMSK;   /**< @brief Device all endpoints interrupt

+                                            mask register.                  */

+  volatile uint32_t resvd820;

+  volatile uint32_t resvd824;

+  volatile uint32_t DVBUSDIS;   /**< @brief Device VBUS discharge time

+                                            register.                       */

+  volatile uint32_t DVBUSPULSE; /**< @brief Device VBUS pulsing time

+                                            register.                       */

+  volatile uint32_t resvd830;

+  volatile uint32_t DIEPEMPMSK; /**< @brief Device IN endpoint FIFO empty

+                                            interrupt mask register.        */

+  volatile uint32_t resvd838;

+  volatile uint32_t resvd83C;

+  volatile uint32_t resvd840[16];

+  volatile uint32_t resvd880[16];

+  volatile uint32_t resvd8C0[16];

+  stm32_otg_in_ep_t ie[16];     /**< @brief Input endpoints.                */

+  stm32_otg_out_ep_t oe[16];    /**< @brief Output endpoints.               */

+  volatile uint32_t resvdD00[64];

+  volatile uint32_t PCGCCTL;    /**< @brief Power and clock gating control

+                                            register.                       */

+  volatile uint32_t resvdE04[127];

+  volatile uint32_t FIFO[16][1024];

+} stm32_otg_t;

+

+/**

+ * @name GOTGCTL register bit definitions

+ * @{

+ */

+#define GOTGCTL_BSVLD           (1U<<19)    /**< B-Session Valid.           */

+#define GOTGCTL_ASVLD           (1U<<18)    /**< A-Session Valid.           */

+#define GOTGCTL_DBCT            (1U<<17)    /**< Long/Short debounce time.  */

+#define GOTGCTL_CIDSTS          (1U<<16)    /**< Connector ID status.       */

+#define GOTGCTL_EHEN            (1U<<12)

+#define GOTGCTL_DHNPEN          (1U<<11)    /**< Device HNP enabled.        */

+#define GOTGCTL_HSHNPEN         (1U<<10)    /**< Host Set HNP enable.       */

+#define GOTGCTL_HNPRQ           (1U<<9)     /**< HNP request.               */

+#define GOTGCTL_HNGSCS          (1U<<8)     /**< Host negotiation success.  */

+#define GOTGCTL_BVALOVAL        (1U<<7)

+#define GOTGCTL_BVALOEN         (1U<<6)

+#define GOTGCTL_AVALOVAL        (1U<<5)

+#define GOTGCTL_AVALOEN         (1U<<4)

+#define GOTGCTL_VBVALOVAL       (1U<<3)

+#define GOTGCTL_VBVALOEN        (1U<<2)

+#define GOTGCTL_SRQ             (1U<<1)     /**< Session request.           */

+#define GOTGCTL_SRQSCS          (1U<<0)     /**< Session request success.   */

+/** @} */

+

+/**

+ * @name GOTGINT register bit definitions

+ * @{

+ */

+#define GOTGINT_DBCDNE          (1U<<19)    /**< Debounce done.             */

+#define GOTGINT_ADTOCHG         (1U<<18)    /**< A-Device timeout change.   */

+#define GOTGINT_HNGDET          (1U<<17)    /**< Host negotiation detected. */

+#define GOTGINT_HNSSCHG         (1U<<9)     /**< Host negotiation success

+                                                 status change.             */

+#define GOTGINT_SRSSCHG         (1U<<8)     /**< Session request success

+                                                 status change.             */

+#define GOTGINT_SEDET           (1U<<2)     /**< Session end detected.      */

+/** @} */

+

+/**

+ * @name GAHBCFG register bit definitions

+ * @{

+ */

+#define GAHBCFG_PTXFELVL        (1U<<8)     /**< Periodic TxFIFO empty

+                                                 level.                     */

+#define GAHBCFG_TXFELVL         (1U<<7)     /**< Non-periodic TxFIFO empty

+                                                 level.                     */

+#define GAHBCFG_DMAEN           (1U<<5)     /**< DMA enable (HS only).      */

+#define GAHBCFG_HBSTLEN_MASK    (15U<<1)    /**< Burst length/type mask (HS

+                                                 only).                     */

+#define GAHBCFG_HBSTLEN(n)      ((n)<<1)    /**< Burst length/type (HS

+                                                 only).                     */

+#define GAHBCFG_GINTMSK         (1U<<0)     /**< Global interrupt mask.     */

+/** @} */

+

+/**

+ * @name GUSBCFG register bit definitions

+ * @{

+ */

+#define GUSBCFG_CTXPKT          (1U<<31)    /**< Corrupt Tx packet.         */

+#define GUSBCFG_FDMOD           (1U<<30)    /**< Force Device Mode.         */

+#define GUSBCFG_FHMOD           (1U<<29)    /**< Force Host Mode.           */

+#define GUSBCFG_TRDT_MASK       (15U<<10)   /**< USB Turnaround time field

+                                                 mask.                      */

+#define GUSBCFG_TRDT(n)         ((n)<<10)   /**< USB Turnaround time field

+                                                 value.                     */

+#define GUSBCFG_HNPCAP          (1U<<9)     /**< HNP-Capable.               */

+#define GUSBCFG_SRPCAP          (1U<<8)     /**< SRP-Capable.               */

+#define GUSBCFG_PHYSEL          (1U<<6)     /**< USB 2.0 High-Speed PHY or

+                                                 USB 1.1 Full-Speed serial

+                                                 transceiver Select.        */

+#define GUSBCFG_TOCAL_MASK      (7U<<0)     /**< HS/FS timeout calibration

+                                                 field mask.                */

+#define GUSBCFG_TOCAL(n)        ((n)<<0)    /**< HS/FS timeout calibration

+                                                 field value.               */

+/** @} */

+

+/**

+ * @name GRSTCTL register bit definitions

+ * @{

+ */

+#define GRSTCTL_AHBIDL          (1U<<31)    /**< AHB Master Idle.           */

+#define GRSTCTL_TXFNUM_MASK     (31U<<6)    /**< TxFIFO number field mask.  */

+#define GRSTCTL_TXFNUM(n)       ((n)<<6)    /**< TxFIFO number field value. */

+#define GRSTCTL_TXFFLSH         (1U<<5)     /**< TxFIFO flush.              */

+#define GRSTCTL_RXFFLSH         (1U<<4)     /**< RxFIFO flush.              */

+#define GRSTCTL_FCRST           (1U<<2)     /**< Host frame counter reset.  */

+#define GRSTCTL_HSRST           (1U<<1)     /**< HClk soft reset.           */

+#define GRSTCTL_CSRST           (1U<<0)     /**< Core soft reset.           */

+/** @} */

+

+/**

+ * @name GINTSTS register bit definitions

+ * @{

+ */

+#define GINTSTS_WKUPINT         (1U<<31)    /**< Resume/Remote wakeup

+                                                 detected interrupt.        */

+#define GINTSTS_SRQINT          (1U<<30)    /**< Session request/New session

+                                                 detected interrupt.        */

+#define GINTSTS_DISCINT         (1U<<29)    /**< Disconnect detected

+                                                 interrupt.                 */

+#define GINTSTS_CIDSCHG         (1U<<28)    /**< Connector ID status change.*/

+#define GINTSTS_PTXFE           (1U<<26)    /**< Periodic TxFIFO empty.     */

+#define GINTSTS_HCINT           (1U<<25)    /**< Host channels interrupt.   */

+#define GINTSTS_HPRTINT         (1U<<24)    /**< Host port interrupt.       */

+#define GINTSTS_IPXFR           (1U<<21)    /**< Incomplete periodic

+                                                 transfer.                  */

+#define GINTSTS_IISOOXFR        (1U<<21)    /**< Incomplete isochronous OUT

+                                                 transfer.                  */

+#define GINTSTS_IISOIXFR        (1U<<20)    /**< Incomplete isochronous IN

+                                                 transfer.                  */

+#define GINTSTS_OEPINT          (1U<<19)    /**< OUT endpoints interrupt.   */

+#define GINTSTS_IEPINT          (1U<<18)    /**< IN endpoints interrupt.    */

+#define GINTSTS_EOPF            (1U<<15)    /**< End of periodic frame

+                                                 interrupt.                 */

+#define GINTSTS_ISOODRP         (1U<<14)    /**< Isochronous OUT packet

+                                                 dropped interrupt.         */

+#define GINTSTS_ENUMDNE         (1U<<13)    /**< Enumeration done.          */

+#define GINTSTS_USBRST          (1U<<12)    /**< USB reset.                 */

+#define GINTSTS_USBSUSP         (1U<<11)    /**< USB suspend.               */

+#define GINTSTS_ESUSP           (1U<<10)    /**< Early suspend.             */

+#define GINTSTS_GONAKEFF        (1U<<7)     /**< Global OUT NAK effective.  */

+#define GINTSTS_GINAKEFF        (1U<<6)     /**< Global IN non-periodic NAK

+                                                 effective.                 */

+#define GINTSTS_NPTXFE          (1U<<5)     /**< Non-periodic TxFIFO empty. */

+#define GINTSTS_RXFLVL          (1U<<4)     /**< RxFIFO non-empty.          */

+#define GINTSTS_SOF             (1U<<3)     /**< Start of frame.            */

+#define GINTSTS_OTGINT          (1U<<2)     /**< OTG interrupt.             */

+#define GINTSTS_MMIS            (1U<<1)     /**< Mode Mismatch interrupt.   */

+#define GINTSTS_CMOD            (1U<<0)     /**< Current mode of operation. */

+/** @} */

+

+/**

+ * @name GINTMSK register bit definitions

+ * @{

+ */

+#define GINTMSK_WKUM            (1U<<31)    /**< Resume/remote wakeup

+                                                 detected interrupt mask.   */

+#define GINTMSK_SRQM            (1U<<30)    /**< Session request/New session

+                                                 detected interrupt mask.   */

+#define GINTMSK_DISCM           (1U<<29)    /**< Disconnect detected

+                                                 interrupt mask.            */

+#define GINTMSK_CIDSCHGM        (1U<<28)    /**< Connector ID status change

+                                                 mask.                      */

+#define GINTMSK_PTXFEM          (1U<<26)    /**< Periodic TxFIFO empty mask.*/

+#define GINTMSK_HCM             (1U<<25)    /**< Host channels interrupt

+                                                 mask.                      */

+#define GINTMSK_HPRTM           (1U<<24)    /**< Host port interrupt mask.  */

+#define GINTMSK_IPXFRM          (1U<<21)    /**< Incomplete periodic

+                                                 transfer mask.             */

+#define GINTMSK_IISOOXFRM       (1U<<21)    /**< Incomplete isochronous OUT

+                                                 transfer mask.             */

+#define GINTMSK_IISOIXFRM       (1U<<20)    /**< Incomplete isochronous IN

+                                                 transfer mask.             */

+#define GINTMSK_OEPM            (1U<<19)    /**< OUT endpoints interrupt

+                                                 mask.                      */

+#define GINTMSK_IEPM            (1U<<18)    /**< IN endpoints interrupt

+                                                 mask.                      */

+#define GINTMSK_EOPFM           (1U<<15)    /**< End of periodic frame

+                                                 interrupt mask.            */

+#define GINTMSK_ISOODRPM        (1U<<14)    /**< Isochronous OUT packet

+                                                 dropped interrupt mask.    */

+#define GINTMSK_ENUMDNEM        (1U<<13)    /**< Enumeration done mask.     */

+#define GINTMSK_USBRSTM         (1U<<12)    /**< USB reset mask.            */

+#define GINTMSK_USBSUSPM        (1U<<11)    /**< USB suspend mask.          */

+#define GINTMSK_ESUSPM          (1U<<10)    /**< Early suspend mask.        */

+#define GINTMSK_GONAKEFFM       (1U<<7)     /**< Global OUT NAK effective

+                                                 mask.                      */

+#define GINTMSK_GINAKEFFM       (1U<<6)     /**< Global non-periodic IN NAK

+                                                 effective mask.            */

+#define GINTMSK_NPTXFEM         (1U<<5)     /**< Non-periodic TxFIFO empty

+                                                 mask.                      */

+#define GINTMSK_RXFLVLM         (1U<<4)     /**< Receive FIFO non-empty

+                                                 mask.                      */

+#define GINTMSK_SOFM            (1U<<3)     /**< Start of (micro)frame mask.*/

+#define GINTMSK_OTGM            (1U<<2)     /**< OTG interrupt mask.        */

+#define GINTMSK_MMISM           (1U<<1)     /**< Mode Mismatch interrupt

+                                                 mask.                      */

+/** @} */

+

+/**

+ * @name GRXSTSR register bit definitions

+ * @{

+ */

+#define GRXSTSR_PKTSTS_MASK     (15U<<17)   /**< Packet status mask.        */

+#define GRXSTSR_PKTSTS(n)       ((n)<<17)   /**< Packet status value.       */

+#define GRXSTSR_OUT_GLOBAL_NAK  GRXSTSR_PKTSTS(1)

+#define GRXSTSR_OUT_DATA        GRXSTSR_PKTSTS(2)

+#define GRXSTSR_OUT_COMP        GRXSTSR_PKTSTS(3)

+#define GRXSTSR_SETUP_COMP      GRXSTSR_PKTSTS(4)

+#define GRXSTSR_SETUP_DATA      GRXSTSR_PKTSTS(6)

+#define GRXSTSR_DPID_MASK       (3U<<15)    /**< Data PID mask.             */

+#define GRXSTSR_DPID(n)         ((n)<<15)   /**< Data PID value.            */

+#define GRXSTSR_BCNT_MASK       (0x7FF<<4)  /**< Byte count mask.           */

+#define GRXSTSR_BCNT(n)         ((n)<<4)    /**< Byte count value.          */

+#define GRXSTSR_CHNUM_MASK      (15U<<0)    /**< Channel number mask.       */

+#define GRXSTSR_CHNUM(n)        ((n)<<0)    /**< Channel number value.      */

+#define GRXSTSR_EPNUM_MASK      (15U<<0)    /**< Endpoint number mask.      */

+#define GRXSTSR_EPNUM(n)        ((n)<<0)    /**< Endpoint number value.     */

+/** @} */

+

+/**

+ * @name GRXSTSP register bit definitions

+ * @{

+ */

+#define GRXSTSP_PKTSTS_MASK     (15<<17)    /**< Packet status mask.        */

+#define GRXSTSP_PKTSTS(n)       ((n)<<17)   /**< Packet status value.       */

+#define GRXSTSP_OUT_GLOBAL_NAK  GRXSTSP_PKTSTS(1)

+#define GRXSTSP_OUT_DATA        GRXSTSP_PKTSTS(2)

+#define GRXSTSP_OUT_COMP        GRXSTSP_PKTSTS(3)

+#define GRXSTSP_SETUP_COMP      GRXSTSP_PKTSTS(4)

+#define GRXSTSP_SETUP_DATA      GRXSTSP_PKTSTS(6)

+#define GRXSTSP_DPID_MASK       (3U<<15)    /**< Data PID mask.             */

+#define GRXSTSP_DPID(n)         ((n)<<15)   /**< Data PID value.            */

+#define GRXSTSP_BCNT_MASK       (0x7FF<<4)  /**< Byte count mask.           */

+#define GRXSTSP_BCNT_OFF        4           /**< Byte count offset.         */

+#define GRXSTSP_BCNT(n)         ((n)<<4)    /**< Byte count value.          */

+#define GRXSTSP_CHNUM_MASK      (15U<<0)    /**< Channel number mask.       */

+#define GRXSTSP_CHNUM(n)        ((n)<<0)    /**< Channel number value.      */

+#define GRXSTSP_EPNUM_MASK      (15U<<0)    /**< Endpoint number mask.      */

+#define GRXSTSP_EPNUM_OFF       0           /**< Endpoint number offset.    */

+#define GRXSTSP_EPNUM(n)        ((n)<<0)    /**< Endpoint number value.     */

+/** @} */

+

+/**

+ * @name GRXFSIZ register bit definitions

+ * @{

+ */

+#define GRXFSIZ_RXFD_MASK       (0xFFFF<<0) /**< RxFIFO depth mask.         */

+#define GRXFSIZ_RXFD(n)         ((n)<<0)    /**< RxFIFO depth value.        */

+/** @} */

+

+/**

+ * @name DIEPTXFx register bit definitions

+ * @{

+ */

+#define DIEPTXF_INEPTXFD_MASK   (0xFFFFU<<16)/**< IN endpoint TxFIFO depth

+                                                 mask.                      */

+#define DIEPTXF_INEPTXFD(n)     ((n)<<16)   /**< IN endpoint TxFIFO depth

+                                                 value.                     */

+#define DIEPTXF_INEPTXSA_MASK   (0xFFFF<<0) /**< IN endpoint FIFOx transmit

+                                                 RAM start address mask.    */

+#define DIEPTXF_INEPTXSA(n)     ((n)<<0)    /**< IN endpoint FIFOx transmit

+                                                 RAM start address value.   */

+/** @} */

+

+/**

+ * @name GCCFG register bit definitions

+ * @{

+ */

+#define GCCFG_NOVBUSSENS        (1U<<21)    /**< VBUS sensing disable.      */

+#define GCCFG_SOFOUTEN          (1U<<20)    /**< SOF output enable.         */

+#define GCCFG_VBUSBSEN          (1U<<19)    /**< Enable the VBUS sensing "B"

+                                                 device.                    */

+#define GCCFG_VBUSASEN          (1U<<18)    /**< Enable the VBUS sensing "A"

+                                                 device.                    */

+#define GCCFG_PWRDWN            (1U<<16)    /**< Power down.                */

+/** @} */

+

+/**

+ * @name HPTXFSIZ register bit definitions

+ * @{

+ */

+#define HPTXFSIZ_PTXFD_MASK     (0xFFFFU<<16)/**< Host periodic TxFIFO

+                                                 depth mask.                */

+#define HPTXFSIZ_PTXFD(n)       ((n)<<16)   /**< Host periodic TxFIFO

+                                                 depth value.               */

+#define HPTXFSIZ_PTXSA_MASK     (0xFFFFU<<0)/**< Host periodic TxFIFO

+                                                 Start address mask.        */

+#define HPTXFSIZ_PTXSA(n)       ((n)<<0)    /**< Host periodic TxFIFO

+                                                 start address value.       */

+/** @} */

+

+/**

+ * @name HCFG register bit definitions

+ * @{

+ */

+#define HCFG_FSLSS              (1U<<2)     /**< FS- and LS-only support.   */

+#define HCFG_FSLSPCS_MASK       (3U<<0)     /**< FS/LS PHY clock select

+                                                 mask.                      */

+#define HCFG_FSLSPCS_48         (1U<<0)     /**< PHY clock is running at

+                                                 48 MHz.                    */

+#define HCFG_FSLSPCS_6          (2U<<0)     /**< PHY clock is running at

+                                                 6 MHz.                     */

+/** @} */

+

+/**

+ * @name HFIR register bit definitions

+ * @{

+ */

+#define HFIR_FRIVL_MASK         (0xFFFFU<<0)/**< Frame interval mask.       */

+#define HFIR_FRIVL(n)           ((n)<<0)    /**< Frame interval value.      */

+/** @} */

+

+/**

+ * @name HFNUM register bit definitions

+ * @{

+ */

+#define HFNUM_FTREM_MASK        (0xFFFFU<<16)/**< Frame time Remaining mask.*/

+#define HFNUM_FTREM(n)          ((n)<<16)   /**< Frame time Remaining value.*/

+#define HFNUM_FRNUM_MASK        (0xFFFFU<<0)/**< Frame number mask.         */

+#define HFNUM_FRNUM(n)          ((n)<<0)    /**< Frame number value.        */

+/** @} */

+

+/**

+ * @name HPTXSTS register bit definitions

+ * @{

+ */

+#define HPTXSTS_PTXQTOP_MASK    (0xFFU<<24) /**< Top of the periodic

+                                                 transmit request queue

+                                                 mask.                      */

+#define HPTXSTS_PTXQTOP(n)      ((n)<<24)   /**< Top of the periodic

+                                                 transmit request queue

+                                                 value.                     */

+#define HPTXSTS_PTXQSAV_MASK    (0xFF<<16)  /**< Periodic transmit request

+                                                 queue Space Available

+                                                 mask.                      */

+#define HPTXSTS_PTXQSAV(n)      ((n)<<16)   /**< Periodic transmit request

+                                                 queue Space Available

+                                                 value.                     */

+#define HPTXSTS_PTXFSAVL_MASK   (0xFFFF<<0) /**< Periodic transmit Data

+                                                 FIFO Space Available

+                                                 mask.                      */

+#define HPTXSTS_PTXFSAVL(n)     ((n)<<0)    /**< Periodic transmit Data

+                                                 FIFO Space Available

+                                                 value.                     */

+/** @} */

+

+/**

+ * @name HAINT register bit definitions

+ * @{

+ */

+#define HAINT_HAINT_MASK        (0xFFFFU<<0)/**< Channel interrupts mask.   */

+#define HAINT_HAINT(n)          ((n)<<0)    /**< Channel interrupts value.  */

+/** @} */

+

+/**

+ * @name HAINTMSK register bit definitions

+ * @{

+ */

+#define HAINTMSK_HAINTM_MASK    (0xFFFFU<<0)/**< Channel interrupt mask

+                                                 mask.                      */

+#define HAINTMSK_HAINTM(n)      ((n)<<0)    /**< Channel interrupt mask

+                                                 value.                     */

+/** @} */

+

+/**

+ * @name HPRT register bit definitions

+ * @{

+ */

+#define HPRT_PSPD_MASK          (3U<<17)    /**< Port speed mask.           */

+#define HPRT_PSPD_FS            (1U<<17)    /**< Full speed value.          */

+#define HPRT_PSPD_LS            (2U<<17)    /**< Low speed value.           */

+#define HPRT_PTCTL_MASK         (15<<13)    /**< Port Test control mask.    */

+#define HPRT_PTCTL(n)           ((n)<<13)   /**< Port Test control value.   */

+#define HPRT_PPWR               (1U<<12)    /**< Port power.                */

+#define HPRT_PLSTS_MASK         (3U<<11)    /**< Port Line status mask.     */

+#define HPRT_PLSTS_DM           (1U<<11)    /**< Logic level of D-.         */

+#define HPRT_PLSTS_DP           (1U<<10)    /**< Logic level of D+.         */

+#define HPRT_PRST               (1U<<8)     /**< Port reset.                */

+#define HPRT_PSUSP              (1U<<7)     /**< Port suspend.              */

+#define HPRT_PRES               (1U<<6)     /**< Port Resume.               */

+#define HPRT_POCCHNG            (1U<<5)     /**< Port overcurrent change.   */

+#define HPRT_POCA               (1U<<4)     /**< Port overcurrent active.   */

+#define HPRT_PENCHNG            (1U<<3)     /**< Port enable/disable change.*/

+#define HPRT_PENA               (1U<<2)     /**< Port enable.               */

+#define HPRT_PCDET              (1U<<1)     /**< Port Connect detected.     */

+#define HPRT_PCSTS              (1U<<0)     /**< Port connect status.       */

+/** @} */

+

+/**

+ * @name HCCHAR register bit definitions

+ * @{

+ */

+#define HCCHAR_CHENA            (1U<<31)    /**< Channel enable.            */

+#define HCCHAR_CHDIS            (1U<<30)    /**< Channel Disable.           */

+#define HCCHAR_ODDFRM           (1U<<29)    /**< Odd frame.                 */

+#define HCCHAR_DAD_MASK         (0x7FU<<22) /**< Device Address mask.       */

+#define HCCHAR_DAD(n)           ((n)<<22)   /**< Device Address value.      */

+#define HCCHAR_MCNT_MASK        (3U<<20)    /**< Multicount mask.           */

+#define HCCHAR_MCNT(n)          ((n)<<20)   /**< Multicount value.          */

+#define HCCHAR_EPTYP_MASK       (3U<<18)    /**< Endpoint type mask.        */

+#define HCCHAR_EPTYP(n)         ((n)<<18)   /**< Endpoint type value.       */

+#define HCCHAR_EPTYP_CTL        (0U<<18)    /**< Control endpoint value.    */

+#define HCCHAR_EPTYP_ISO        (1U<<18)    /**< Isochronous endpoint value.*/

+#define HCCHAR_EPTYP_BULK       (2U<<18)    /**< Bulk endpoint value.       */

+#define HCCHAR_EPTYP_INTR       (3U<<18)    /**< Interrupt endpoint value.  */

+#define HCCHAR_LSDEV            (1U<<17)    /**< Low-Speed device.          */

+#define HCCHAR_EPDIR            (1U<<15)    /**< Endpoint direction.        */

+#define HCCHAR_EPNUM_MASK       (15U<<11)   /**< Endpoint number mask.      */

+#define HCCHAR_EPNUM(n)         ((n)<<11)   /**< Endpoint number value.     */

+#define HCCHAR_MPS_MASK         (0x7FFU<<0)    /**< Maximum packet size mask.  */

+#define HCCHAR_MPS(n)           ((n)<<0)    /**< Maximum packet size value. */

+/** @} */

+

+/**

+ * @name HCINT register bit definitions

+ * @{

+ */

+#define HCINT_DTERR             (1U<<10)    /**< Data toggle error.         */

+#define HCINT_FRMOR             (1U<<9)     /**< Frame overrun.             */

+#define HCINT_BBERR             (1U<<8)     /**< Babble error.              */

+#define HCINT_TRERR             (1U<<7)     /**< Transaction Error.         */

+#define HCINT_ACK               (1U<<5)     /**< ACK response

+                                                 received/transmitted

+                                                 interrupt.                 */

+#define HCINT_NAK               (1U<<4)     /**< NAK response received

+                                                 interrupt.                 */

+#define HCINT_STALL             (1U<<3)     /**< STALL response received

+                                                 interrupt.                 */

+#define HCINT_CHH               (1U<<1)     /**< Channel halted.            */

+#define HCINT_XFRC              (1U<<0)     /**< Transfer completed.        */

+/** @} */

+

+/**

+ * @name HCINTMSK register bit definitions

+ * @{

+ */

+#define HCINTMSK_DTERRM         (1U<<10)    /**< Data toggle error mask.    */

+#define HCINTMSK_FRMORM         (1U<<9)     /**< Frame overrun mask.        */

+#define HCINTMSK_BBERRM         (1U<<8)     /**< Babble error mask.         */

+#define HCINTMSK_TRERRM         (1U<<7)     /**< Transaction error mask.    */

+#define HCINTMSK_NYET           (1U<<6)     /**< NYET response received

+                                                 interrupt mask.            */

+#define HCINTMSK_ACKM           (1U<<5)     /**< ACK Response

+                                                 received/transmitted

+                                                 interrupt mask.            */

+#define HCINTMSK_NAKM           (1U<<4)     /**< NAK response received

+                                                 interrupt mask.            */

+#define HCINTMSK_STALLM         (1U<<3)     /**< STALL response received

+                                                 interrupt mask.            */

+#define HCINTMSK_AHBERRM        (1U<<2)

+#define HCINTMSK_CHHM           (1U<<1)     /**< Channel halted mask.       */

+#define HCINTMSK_XFRCM          (1U<<0)     /**< Transfer completed mask.   */

+/** @} */

+

+/**

+ * @name HCTSIZ register bit definitions

+ * @{

+ */

+#define HCTSIZ_DPID_MASK        (3U<<29)    /**< PID mask.                  */

+#define HCTSIZ_DPID_DATA0       (0U<<29)    /**< DATA0.                     */

+#define HCTSIZ_DPID_DATA2       (1U<<29)    /**< DATA2.                     */

+#define HCTSIZ_DPID_DATA1       (2U<<29)    /**< DATA1.                     */

+#define HCTSIZ_DPID_MDATA       (3U<<29)    /**< MDATA.                     */

+#define HCTSIZ_DPID_SETUP       (3U<<29)    /**< SETUP.                     */

+#define HCTSIZ_PKTCNT_MASK      (0x3FFU<<19)/**< Packet count mask.         */

+#define HCTSIZ_PKTCNT(n)        ((n)<<19)   /**< Packet count value.        */

+#define HCTSIZ_XFRSIZ_MASK      (0x7FFFF<<0)/**< Transfer size mask.        */

+#define HCTSIZ_XFRSIZ(n)        ((n)<<0)    /**< Transfer size value.       */

+/** @} */

+

+/**

+ * @name DCFG register bit definitions

+ * @{

+ */

+#define DCFG_PFIVL_MASK         (3U<<11)    /**< Periodic frame interval

+                                                 mask.                      */

+#define DCFG_PFIVL(n)           ((n)<<11)   /**< Periodic frame interval

+                                                 value.                     */

+#define DCFG_DAD_MASK           (0x7FU<<4)  /**< Device address mask.       */

+#define DCFG_DAD(n)             ((n)<<4)    /**< Device address value.      */

+#define DCFG_NZLSOHSK           (1U<<2)     /**< Non-Zero-Length status

+                                                 OUT handshake.             */

+#define DCFG_DSPD_MASK          (3U<<0)     /**< Device speed mask.         */

+#define DCFG_DSPD_HS            (0U<<0)     /**< High speed (USB 2.0).      */

+#define DCFG_DSPD_HS_FS         (1U<<0)     /**< High speed (USB 2.0) in FS

+                                                 mode.                      */

+#define DCFG_DSPD_FS11          (3U<<0)     /**< Full speed (USB 1.1

+                                                 transceiver clock is 48

+                                                 MHz).                      */

+/** @} */

+

+/**

+ * @name DCTL register bit definitions

+ * @{

+ */

+#define DCTL_POPRGDNE           (1U<<11)    /**< Power-on programming done. */

+#define DCTL_CGONAK             (1U<<10)    /**< Clear global OUT NAK.      */

+#define DCTL_SGONAK             (1U<<9)     /**< Set global OUT NAK.        */

+#define DCTL_CGINAK             (1U<<8)     /**< Clear global non-periodic

+                                                 IN NAK.                    */

+#define DCTL_SGINAK             (1U<<7)     /**< Set global non-periodic

+                                                 IN NAK.                    */

+#define DCTL_TCTL_MASK          (7U<<4)     /**< Test control mask.         */

+#define DCTL_TCTL(n)            ((n)<<4     /**< Test control value.        */

+#define DCTL_GONSTS             (1U<<3)     /**< Global OUT NAK status.     */

+#define DCTL_GINSTS             (1U<<2)     /**< Global non-periodic IN

+                                                 NAK status.                */

+#define DCTL_SDIS               (1U<<1)     /**< Soft disconnect.           */

+#define DCTL_RWUSIG             (1U<<0)     /**< Remote wakeup signaling.   */

+/** @} */

+

+/**

+ * @name DSTS register bit definitions

+ * @{

+ */

+#define DSTS_FNSOF_MASK         (0x3FFU<<8) /**< Frame number of the received

+                                                 SOF mask.                  */

+#define DSTS_FNSOF(n)           ((n)<<8)    /**< Frame number of the received

+                                                 SOF value.                 */

+#define DSTS_FNSOF_ODD          (1U<<8)     /**< Frame parity of the received

+                                                 SOF value.                 */

+#define DSTS_EERR               (1U<<3)     /**< Erratic error.             */

+#define DSTS_ENUMSPD_MASK       (3U<<1)     /**< Enumerated speed mask.     */

+#define DSTS_ENUMSPD_FS_48      (3U<<1)     /**< Full speed (PHY clock is

+                                                 running at 48 MHz).        */

+#define DSTS_ENUMSPD_HS_480     (0U<<1)     /**< High speed.                */

+#define DSTS_SUSPSTS            (1U<<0)     /**< Suspend status.            */

+/** @} */

+

+/**

+ * @name DIEPMSK register bit definitions

+ * @{

+ */

+#define DIEPMSK_TXFEM           (1U<<6)     /**< Transmit FIFO empty mask.  */

+#define DIEPMSK_INEPNEM         (1U<<6)     /**< IN endpoint NAK effective

+                                                 mask.                      */

+#define DIEPMSK_ITTXFEMSK       (1U<<4)     /**< IN token received when

+                                                 TxFIFO empty mask.         */

+#define DIEPMSK_TOCM            (1U<<3)     /**< Timeout condition mask.    */

+#define DIEPMSK_EPDM            (1U<<1)     /**< Endpoint disabled

+                                                 interrupt mask.            */

+#define DIEPMSK_XFRCM           (1U<<0)     /**< Transfer completed

+                                                 interrupt mask.            */

+/** @} */

+

+/**

+ * @name DOEPMSK register bit definitions

+ * @{

+ */

+#define DOEPMSK_OTEPDM          (1U<<4)     /**< OUT token received when

+                                                 endpoint disabled mask.    */

+#define DOEPMSK_STUPM           (1U<<3)     /**< SETUP phase done mask.     */

+#define DOEPMSK_EPDM            (1U<<1)     /**< Endpoint disabled

+                                                 interrupt mask.            */

+#define DOEPMSK_XFRCM           (1U<<0)     /**< Transfer completed

+                                                 interrupt mask.            */

+/** @} */

+

+/**

+ * @name DAINT register bit definitions

+ * @{

+ */

+#define DAINT_OEPINT_MASK       (0xFFFFU<<16)/**< OUT endpoint interrupt

+                                                 bits mask.                 */

+#define DAINT_OEPINT(n)         ((n)<<16)   /**< OUT endpoint interrupt

+                                                 bits value.                */

+#define DAINT_IEPINT_MASK       (0xFFFFU<<0)/**< IN endpoint interrupt

+                                                 bits mask.                 */

+#define DAINT_IEPINT(n)         ((n)<<0)    /**< IN endpoint interrupt

+                                                 bits value.                */

+/** @} */

+

+/**

+ * @name DAINTMSK register bit definitions

+ * @{

+ */

+#define DAINTMSK_OEPM_MASK      (0xFFFFU<<16)/**< OUT EP interrupt mask

+                                                 bits mask.                 */

+#define DAINTMSK_OEPM(n)        (1U<<(16+(n)))/**< OUT EP interrupt mask

+                                                 bits value.                */

+#define DAINTMSK_IEPM_MASK      (0xFFFFU<<0)/**< IN EP interrupt mask

+                                                 bits mask.                 */

+#define DAINTMSK_IEPM(n)        (1U<<(n))   /**< IN EP interrupt mask

+                                                 bits value.                */

+/** @} */

+

+/**

+ * @name DVBUSDIS register bit definitions

+ * @{

+ */

+#define DVBUSDIS_VBUSDT_MASK    (0xFFFFU<<0)/**< Device VBUS discharge

+                                                 time mask.                 */

+#define DVBUSDIS_VBUSDT(n)      ((n)<<0)    /**< Device VBUS discharge

+                                                 time value.                */

+/** @} */

+

+/**

+ * @name DVBUSPULSE register bit definitions

+ * @{

+ */

+#define DVBUSPULSE_DVBUSP_MASK  (0xFFFU<<0) /**< Device VBUSpulsing time

+                                                 mask.                      */

+#define DVBUSPULSE_DVBUSP(n)    ((n)<<0)    /**< Device VBUS pulsing time

+                                                 value.                     */

+/** @} */

+

+/**

+ * @name DIEPEMPMSK register bit definitions

+ * @{

+ */

+#define DIEPEMPMSK_INEPTXFEM(n) (1U<<(n))   /**< IN EP Tx FIFO empty

+                                                 interrupt mask bit.        */

+/** @} */

+

+/**

+ * @name DIEPCTL register bit definitions

+ * @{

+ */

+#define DIEPCTL_EPENA           (1U<<31)    /**< Endpoint enable.           */

+#define DIEPCTL_EPDIS           (1U<<30)    /**< Endpoint disable.          */

+#define DIEPCTL_SD1PID          (1U<<29)    /**< Set DATA1 PID.             */

+#define DIEPCTL_SODDFRM         (1U<<29)    /**< Set odd frame.             */

+#define DIEPCTL_SD0PID          (1U<<28)    /**< Set DATA0 PID.             */

+#define DIEPCTL_SEVNFRM         (1U<<28)    /**< Set even frame.            */

+#define DIEPCTL_SNAK            (1U<<27)    /**< Set NAK.                   */

+#define DIEPCTL_CNAK            (1U<<26)    /**< Clear NAK.                 */

+#define DIEPCTL_TXFNUM_MASK     (15U<<22)   /**< TxFIFO number mask.        */

+#define DIEPCTL_TXFNUM(n)       ((n)<<22)   /**< TxFIFO number value.       */

+#define DIEPCTL_STALL           (1U<<21)    /**< STALL handshake.           */

+#define DIEPCTL_SNPM            (1U<<20)    /**< Snoop mode.                */

+#define DIEPCTL_EPTYP_MASK      (3<<18)     /**< Endpoint type mask.        */

+#define DIEPCTL_EPTYP_CTRL      (0U<<18)    /**< Control.                   */

+#define DIEPCTL_EPTYP_ISO       (1U<<18)    /**< Isochronous.               */

+#define DIEPCTL_EPTYP_BULK      (2U<<18)    /**< Bulk.                      */

+#define DIEPCTL_EPTYP_INTR      (3U<<18)    /**< Interrupt.                 */

+#define DIEPCTL_NAKSTS          (1U<<17)    /**< NAK status.                */

+#define DIEPCTL_EONUM           (1U<<16)    /**< Even/odd frame.            */

+#define DIEPCTL_DPID            (1U<<16)    /**< Endpoint data PID.         */

+#define DIEPCTL_USBAEP          (1U<<15)    /**< USB active endpoint.       */

+#define DIEPCTL_MPSIZ_MASK      (0x3FFU<<0) /**< Maximum Packet size mask.  */

+#define DIEPCTL_MPSIZ(n)        ((n)<<0)    /**< Maximum Packet size value. */

+/** @} */

+

+/**

+ * @name DIEPINT register bit definitions

+ * @{

+ */

+#define DIEPINT_TXFE            (1U<<7)     /**< Transmit FIFO empty.       */

+#define DIEPINT_INEPNE          (1U<<6)     /**< IN endpoint NAK effective. */

+#define DIEPINT_ITTXFE          (1U<<4)     /**< IN Token received when

+                                                 TxFIFO is empty.           */

+#define DIEPINT_TOC             (1U<<3)     /**< Timeout condition.         */

+#define DIEPINT_EPDISD          (1U<<1)     /**< Endpoint disabled

+                                                 interrupt.                 */

+#define DIEPINT_XFRC            (1U<<0)     /**< Transfer completed.        */

+/** @} */

+

+/**

+ * @name DIEPTSIZ register bit definitions

+ * @{

+ */

+#define DIEPTSIZ_MCNT_MASK      (3U<<29)    /**< Multi count mask.          */

+#define DIEPTSIZ_MCNT(n)        ((n)<<29)   /**< Multi count value.         */

+#define DIEPTSIZ_PKTCNT_MASK    (0x3FF<<19) /**< Packet count mask.         */

+#define DIEPTSIZ_PKTCNT(n)      ((n)<<19)   /**< Packet count value.        */

+#define DIEPTSIZ_XFRSIZ_MASK    (0x7FFFFU<<0)/**< Transfer size mask.       */

+#define DIEPTSIZ_XFRSIZ(n)      ((n)<<0)    /**< Transfer size value.       */

+/** @} */

+

+/**

+ * @name DTXFSTS register bit definitions.

+ * @{

+ */

+#define DTXFSTS_INEPTFSAV_MASK  (0xFFFF<<0) /**< IN endpoint TxFIFO space

+                                                 available.                 */

+/** @} */

+

+/**

+ * @name DOEPCTL register bit definitions.

+ * @{

+ */

+#define DOEPCTL_EPENA           (1U<<31)    /**< Endpoint enable.           */

+#define DOEPCTL_EPDIS           (1U<<30)    /**< Endpoint disable.          */

+#define DOEPCTL_SD1PID          (1U<<29)    /**< Set DATA1 PID.             */

+#define DOEPCTL_SODDFRM         (1U<<29)    /**< Set odd frame.             */

+#define DOEPCTL_SD0PID          (1U<<28)    /**< Set DATA0 PID.             */

+#define DOEPCTL_SEVNFRM         (1U<<28)    /**< Set even frame.            */

+#define DOEPCTL_SNAK            (1U<<27)    /**< Set NAK.                   */

+#define DOEPCTL_CNAK            (1U<<26)    /**< Clear NAK.                 */

+#define DOEPCTL_STALL           (1U<<21)    /**< STALL handshake.           */

+#define DOEPCTL_SNPM            (1U<<20)    /**< Snoop mode.                */

+#define DOEPCTL_EPTYP_MASK      (3U<<18)    /**< Endpoint type mask.        */

+#define DOEPCTL_EPTYP_CTRL      (0U<<18)    /**< Control.                   */

+#define DOEPCTL_EPTYP_ISO       (1U<<18)    /**< Isochronous.               */

+#define DOEPCTL_EPTYP_BULK      (2U<<18)    /**< Bulk.                      */

+#define DOEPCTL_EPTYP_INTR      (3U<<18)    /**< Interrupt.                 */

+#define DOEPCTL_NAKSTS          (1U<<17)    /**< NAK status.                */

+#define DOEPCTL_EONUM           (1U<<16)    /**< Even/odd frame.            */

+#define DOEPCTL_DPID            (1U<<16)    /**< Endpoint data PID.         */

+#define DOEPCTL_USBAEP          (1U<<15)    /**< USB active endpoint.       */

+#define DOEPCTL_MPSIZ_MASK      (0x3FFU<<0) /**< Maximum Packet size mask.  */

+#define DOEPCTL_MPSIZ(n)        ((n)<<0)    /**< Maximum Packet size value. */

+/** @} */

+

+/**

+ * @name DOEPINT register bit definitions

+ * @{

+ */

+#define DOEPINT_B2BSTUP         (1U<<6)     /**< Back-to-back SETUP packets

+                                                 received.                  */

+#define DOEPINT_OTEPDIS         (1U<<4)     /**< OUT token received when

+                                                 endpoint disabled.         */

+#define DOEPINT_STUP            (1U<<3)     /**< SETUP phase done.          */

+#define DOEPINT_EPDISD          (1U<<1)     /**< Endpoint disabled

+                                                 interrupt.                 */

+#define DOEPINT_XFRC            (1U<<0)     /**< Transfer completed

+                                                 interrupt.                 */

+/** @} */

+

+/**

+ * @name DOEPTSIZ register bit definitions

+ * @{

+ */

+#define DOEPTSIZ_RXDPID_MASK    (3U<<29)    /**< Received data PID mask.    */

+#define DOEPTSIZ_RXDPID(n)      ((n)<<29)   /**< Received data PID value.   */

+#define DOEPTSIZ_STUPCNT_MASK   (3U<<29)    /**< SETUP packet count mask.   */

+#define DOEPTSIZ_STUPCNT(n)     ((n)<<29)   /**< SETUP packet count value.  */

+#define DOEPTSIZ_PKTCNT_MASK    (0x3FFU<<19)/**< Packet count mask.         */

+#define DOEPTSIZ_PKTCNT(n)      ((n)<<19)   /**< Packet count value.        */

+#define DOEPTSIZ_XFRSIZ_MASK    (0x7FFFFU<<0)/**< Transfer size mask.       */

+#define DOEPTSIZ_XFRSIZ(n)      ((n)<<0)    /**< Transfer size value.       */

+/** @} */

+

+/**

+ * @name PCGCCTL register bit definitions

+ * @{

+ */

+#define PCGCCTL_PHYSUSP         (1U<<4)     /**< PHY Suspended.             */

+#define PCGCCTL_GATEHCLK        (1U<<1)     /**< Gate HCLK.                 */

+#define PCGCCTL_STPPCLK         (1U<<0)     /**< Stop PCLK.                 */

+/** @} */

+

+/**

+ * @brief   OTG_FS registers block memory address.

+ */

+#define OTG_FS_ADDR                 0x50000000

+

+/**

+ * @brief   OTG_HS registers block memory address.

+ */

+#define OTG_HS_ADDR                 0x40040000

+

+/**

+ * @brief   Accesses to the OTG_FS registers block.

+ */

+#define OTG_FS                      ((stm32_otg_t *)OTG_FS_ADDR)

+

+/**

+ * @brief   Accesses to the OTG_HS registers block.

+ */

+#define OTG_HS                      ((stm32_otg_t *)OTG_HS_ADDR)

+

+#endif /* _STM32_OTG_H_ */

+

+/** @} */

diff --git a/os/hal/ports/STM32/LLD/USBHv1/usbh_lld.c b/os/hal/ports/STM32/LLD/USBHv1/usbh_lld.c
new file mode 100644
index 0000000..5455f52
--- /dev/null
+++ b/os/hal/ports/STM32/LLD/USBHv1/usbh_lld.c
@@ -0,0 +1,1604 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

+

+    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.

+*/

+

+#include "hal.h"

+

+#if HAL_USE_USBH

+#include "usbh/internal.h"

+#include <string.h>

+

+#if USBH_LLD_DEBUG_ENABLE_TRACE

+#define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define udbgf(f, ...)  do {} while(0)

+#define udbg(f, ...)   do {} while(0)

+#endif

+

+#if USBH_LLD_DEBUG_ENABLE_INFO

+#define uinfof(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define uinfo(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define uinfof(f, ...)  do {} while(0)

+#define uinfo(f, ...)   do {} while(0)

+#endif

+

+#if USBH_LLD_DEBUG_ENABLE_WARNINGS

+#define uwarnf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define uwarn(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define uwarnf(f, ...)  do {} while(0)

+#define uwarn(f, ...)   do {} while(0)

+#endif

+

+#if USBH_LLD_DEBUG_ENABLE_ERRORS

+#define uerrf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define uerr(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define uerrf(f, ...)  do {} while(0)

+#define uerr(f, ...)   do {} while(0)

+#endif

+

+static void _transfer_completedI(usbh_ep_t *ep, usbh_urb_t *urb, usbh_urbstatus_t status);

+static void _try_commit_np(USBHDriver *host);

+static void otg_rxfifo_flush(USBHDriver *usbp);

+static void otg_txfifo_flush(USBHDriver *usbp, uint32_t fifo);

+

+/*===========================================================================*/

+/* Little helper functions.	    	                                 		 */

+/*===========================================================================*/

+static inline void _move_to_pending_queue(usbh_ep_t *ep) {

+	list_move_tail(&ep->node, ep->pending_list);

+}

+

+static inline usbh_urb_t *_active_urb(usbh_ep_t *ep) {

+	return list_first_entry(&ep->urb_list, usbh_urb_t, node);

+}

+

+static inline void _save_dt_mask(usbh_ep_t *ep, uint32_t hctsiz) {

+	ep->dt_mask = hctsiz & HCTSIZ_DPID_MASK;

+}

+

+#if 1

+#define _transfer_completed _transfer_completedI

+#else

+static inline void _transfer_completed(usbh_ep_t *ep, usbh_urb_t *urb, usbh_urbstatus_t status) {

+	osalSysLockFromISR();

+	_transfer_completedI(ep, urb, status);

+	osalSysUnlockFromISR();

+}

+#endif

+

+/*===========================================================================*/

+/* Functions called from many places.                                 		 */

+/*===========================================================================*/

+static void _transfer_completedI(usbh_ep_t *ep, usbh_urb_t *urb, usbh_urbstatus_t status) {

+	osalDbgCheckClassI();

+

+	urb->queued = FALSE;

+

+	/* remove URB from EP's queue */

+	list_del_init(&urb->node);

+

+	/* Call the callback function now, so that if it calls usbhURBSubmitI,

+	 * the list_empty check below will be false. Also, note that the

+	 *   if (list_empty(&ep->node)) {

+	 *     ...

+	 *   }

+	 * in usbh_lld_urb_submit will be false, since the endpoint is

+	 * still in the active queue.

+	 */

+	_usbh_urb_completeI(urb, status);

+

+	if (list_empty(&ep->urb_list)) {

+		/* no more URBs to process in this EP, remove EP from the host's queue */

+		list_del_init(&ep->node);

+	} else {

+		/* more URBs to process */

+		_move_to_pending_queue(ep);

+	}

+}

+

+static void _halt_channel(USBHDriver *host, stm32_hc_management_t *hcm, usbh_lld_halt_reason_t reason) {

+	(void)host;

+

+	if (hcm->halt_reason != USBH_LLD_HALTREASON_NONE) {

+		uwarnf("\t%s: Repeated halt (original=%d, new=%d)", hcm->ep->name, hcm->halt_reason, reason);

+		return;

+	}

+

+#if CH_DBG_ENABLE_CHECKS

+	if (usbhEPIsPeriodic(hcm->ep)) {

+		osalDbgCheck(host->otg->HPTXSTS & HPTXSTS_PTXQSAV_MASK);

+	} else {

+		osalDbgCheck(host->otg->HNPTXSTS & HPTXSTS_PTXQSAV_MASK);

+	}

+#endif

+

+	hcm->halt_reason = reason;

+	hcm->hc->HCCHAR |= HCCHAR_CHENA | HCCHAR_CHDIS;

+}

+

+static void _release_channel(USBHDriver *host, stm32_hc_management_t *hcm) {

+//	static const char *reason[] =  {"XFRC",	"XFRC",	"NAK", "STALL",	"ERROR", "ABORT"};

+//	udbgf("\t%s: release (%s)", hcm->ep->name, reason[hcm->halt_reason]);

+	hcm->hc->HCINTMSK = 0;

+	host->otg->HAINTMSK &= ~hcm->haintmsk;

+	hcm->halt_reason = USBH_LLD_HALTREASON_NONE;

+	if (usbhEPIsPeriodic(hcm->ep)) {

+		list_add(&hcm->node, &host->ch_free[0]);

+	} else {

+		list_add(&hcm->node, &host->ch_free[1]);

+	}

+	hcm->ep->xfer.hcm = 0;

+	hcm->ep = 0;

+}

+

+static bool _activate_ep(USBHDriver *host, usbh_ep_t *ep) {

+	struct list_head *list;

+	uint16_t spc;

+

+	osalDbgCheck(ep->xfer.hcm == NULL);

+

+	if (usbhEPIsPeriodic(ep)) {

+		list = &host->ch_free[0];

+		spc = (host->otg->HPTXSTS >> 16) & 0xff;

+	} else {

+		list = &host->ch_free[1];

+		spc = (host->otg->HNPTXSTS >> 16) & 0xff;

+	}

+

+	if (list_empty(list)) {

+		uwarnf("\t%s: No free %s channels", ep->name, usbhEPIsPeriodic(ep) ? "P" : "NP");

+		return FALSE;

+	}

+

+	if (spc <= STM32_USBH_MIN_QSPACE) {

+		uwarnf("\t%s: No space in %s Queue (spc=%d)", ep->name, usbhEPIsPeriodic(ep) ? "P" : "NP", spc);

+		return FALSE;

+	}

+

+	/* get the first channel */

+	stm32_hc_management_t *hcm = list_first_entry(list, stm32_hc_management_t, node);

+	osalDbgCheck((hcm->halt_reason == USBH_LLD_HALTREASON_NONE) && (hcm->ep == NULL));

+

+	usbh_urb_t *const urb = _active_urb(ep);

+	uint32_t hcintmsk = ep->hcintmsk;

+	uint32_t hcchar = ep->hcchar;

+	uint16_t mps = ep->wMaxPacketSize;

+

+	uint32_t xfer_packets;

+	uint32_t xfer_len = 0;	//Initialize just to shut up a compiler warning

+

+	osalDbgCheck(urb->status == USBH_URBSTATUS_PENDING);

+

+	/* check if the URB is a new one, or we must continue a previously started URB */

+	if (urb->queued == FALSE) {

+		/* prepare EP for a new URB */

+		if (ep->type == USBH_EPTYPE_CTRL) {

+			xfer_len = 8;

+			ep->xfer.buf = (uint8_t *)urb->setup_buff;

+			ep->dt_mask = HCTSIZ_DPID_SETUP;

+			ep->in = FALSE;

+			ep->xfer.u.ctrl_phase = USBH_LLD_CTRLPHASE_SETUP;

+		} else {

+			xfer_len = urb->requestedLength;

+			ep->xfer.buf = urb->buff;

+		}

+		ep->xfer.error_count = 0;

+		//urb->status = USBH_URBSTATUS_QUEUED;

+	} else {

+		osalDbgCheck(urb->requestedLength >= urb->actualLength);

+

+		if (ep->type == USBH_EPTYPE_CTRL) {

+			switch (ep->xfer.u.ctrl_phase) {

+			case USBH_LLD_CTRLPHASE_SETUP:

+				xfer_len = 8;

+				ep->xfer.buf = (uint8_t *)urb->setup_buff;

+				ep->dt_mask = HCTSIZ_DPID_SETUP;

+				break;

+			case USBH_LLD_CTRLPHASE_DATA:

+				xfer_len = urb->requestedLength - urb->actualLength;

+				ep->xfer.buf = (uint8_t *) urb->buff + urb->actualLength;

+				break;

+			case USBH_LLD_CTRLPHASE_STATUS:

+				xfer_len = 0;

+				ep->dt_mask = HCTSIZ_DPID_DATA1;

+				ep->xfer.error_count = 0;

+				break;

+			default:

+				osalDbgCheck(0);

+			}

+			if (ep->in) {

+				hcintmsk |= HCINTMSK_DTERRM | HCINTMSK_BBERRM;

+				hcchar |= HCCHAR_EPDIR;

+			}

+		} else {

+			xfer_len = urb->requestedLength - urb->actualLength;

+			ep->xfer.buf = (uint8_t *) urb->buff + urb->actualLength;

+		}

+

+		if (ep->xfer.error_count)

+			hcintmsk |= HCINTMSK_ACKM;

+

+	}

+	ep->xfer.partial = 0;

+

+	if (ep->type == USBH_EPTYPE_ISO) {

+		ep->dt_mask = HCTSIZ_DPID_DATA0;

+

+		/* [USB 2.0 spec, 5.6.4]: A host must not issue more than 1

+		 * transaction in a (micro)frame for an isochronous endpoint

+		 * unless the endpoint is high-speed, high-bandwidth.

+		 */

+		if (xfer_len > mps)

+			xfer_len = mps;

+	} else if (xfer_len > 0x7FFFF) {

+		xfer_len = 0x7FFFF - mps + 1;

+	}

+

+	/* calculate required packets */

+	if (xfer_len) {

+		xfer_packets = (xfer_len + mps - 1) / mps;

+

+		if (xfer_packets > 0x3FF) {

+			xfer_packets = 0x3FF;

+			xfer_len = xfer_packets * mps;

+		}

+	} else {

+		xfer_packets = 1;	/* Need 1 packet for transfer length of 0 */

+	}

+

+	if (ep->in)

+		xfer_len = xfer_packets * mps;

+

+	/* Clear old interrupt conditions,

+	 * configure transfer size,

+	 * enable required interrupts */

+	stm32_otg_host_chn_t *const hc = hcm->hc;

+	hc->HCINT = 0xffffffff;

+	hc->HCTSIZ = ep->dt_mask

+					| HCTSIZ_PKTCNT(xfer_packets)

+					| HCTSIZ_XFRSIZ(xfer_len);

+	hc->HCINTMSK = hcintmsk;

+

+	/* Queue the transfer for the next frame (no effect for non-periodic transfers) */

+	if (!(host->otg->HFNUM & 1))

+		hcchar |= HCCHAR_ODDFRM;

+

+	/* configure channel characteristics and queue a request */

+	hc->HCCHAR = hcchar;

+	if (ep->in && (xfer_packets > 1)) {

+		/* For IN transfers, try to queue two back-to-back packets.

+		 * This results in a 1% performance gain for Full Speed transfers

+		 */

+		if (--spc > STM32_USBH_MIN_QSPACE) {

+			hc->HCCHAR |= HCCHAR_CHENA;

+		} else {

+			uwarnf("\t%s: Could not queue back-to-back packets", ep->name);

+		}

+	}

+

+	if (urb->queued == FALSE) {

+		urb->queued = TRUE;

+		udbgf("\t%s: Start (%dB)", ep->name, xfer_len);

+	} else {

+		udbgf("\t%s: Restart (%dB)", ep->name, xfer_len);

+	}

+

+	ep->xfer.len = xfer_len;

+	ep->xfer.packets = (uint16_t)xfer_packets;

+

+	/* remove the channel from the free list, link endpoint <-> channel and move to the active queue*/

+	list_del(&hcm->node);

+	ep->xfer.hcm = hcm;

+	hcm->ep = ep;

+	list_move_tail(&ep->node, ep->active_list);

+

+

+	stm32_otg_t *const otg = host->otg;

+

+	/* enable this channel's interrupt and global channel interrupt */

+	otg->HAINTMSK |= hcm->haintmsk;

+	if (ep->in) {

+		otg->GINTMSK |= GINTMSK_HCM;

+	} else if (usbhEPIsPeriodic(ep)) {

+		otg->GINTMSK |= GINTMSK_HCM | GINTMSK_PTXFEM;

+	} else {

+		//TODO: write to the FIFO now

+		otg->GINTMSK |= GINTMSK_HCM | GINTMSK_NPTXFEM;

+	}

+

+	return TRUE;

+}

+

+static bool _update_urb(usbh_ep_t *ep, uint32_t hctsiz, usbh_urb_t *urb, bool completed) {

+	uint32_t len;

+

+	if (!completed) {

+		len = ep->wMaxPacketSize * (ep->xfer.packets - ((hctsiz & HCTSIZ_PKTCNT_MASK) >> 19));

+	} else {

+		if (ep->in) {

+			len = ep->xfer.len - ((hctsiz & HCTSIZ_XFRSIZ_MASK) >> 0);

+		} else {

+			len = ep->xfer.len;

+		}

+		osalDbgCheck(len == ep->xfer.partial);	//TODO: if len == ep->xfer.partial, use this instead of the above code

+	}

+

+#if 1

+	osalDbgAssert(urb->actualLength + len <= urb->requestedLength, "what happened?");

+#else

+	if (urb->actualLength + len > urb->requestedLength) {

+		uerrf("\t%s: Trimming actualLength %u -> %u", ep->name, urb->actualLength + len, urb->requestedLength);

+		urb->actualLength = urb->requestedLength;

+		return TRUE;

+	}

+#endif

+

+	urb->actualLength += len;

+	if ((urb->actualLength == urb->requestedLength)

+			|| (ep->in && completed && (hctsiz & HCTSIZ_XFRSIZ_MASK)))

+		return TRUE;

+

+	return FALSE;

+}

+

+static void _try_commit_np(USBHDriver *host) {

+	usbh_ep_t *item, *tmp;

+

+	list_for_each_entry_safe(item, usbh_ep_t, tmp, &host->ep_pending_lists[USBH_EPTYPE_CTRL], node) {

+		if (!_activate_ep(host, item))

+			return;

+	}

+

+	list_for_each_entry_safe(item, usbh_ep_t, tmp, &host->ep_pending_lists[USBH_EPTYPE_BULK], node) {

+		if (!_activate_ep(host, item))

+			return;

+	}

+}

+

+static void _try_commit_p(USBHDriver *host, bool sof) {

+	usbh_ep_t *item, *tmp;

+

+	list_for_each_entry_safe(item, usbh_ep_t, tmp, &host->ep_pending_lists[USBH_EPTYPE_ISO], node) {

+		if (!_activate_ep(host, item))

+			return;

+	}

+

+	list_for_each_entry_safe(item, usbh_ep_t, tmp, &host->ep_pending_lists[USBH_EPTYPE_INT], node) {

+		osalDbgCheck(item);

+		/* TODO: improve this */

+		if (sof && item->xfer.u.frame_counter)

+			--item->xfer.u.frame_counter;

+

+		if (item->xfer.u.frame_counter == 0) {

+			if (!_activate_ep(host, item))

+				return;

+			item->xfer.u.frame_counter = item->bInterval;

+		}

+	}

+

+	if (list_empty(&host->ep_pending_lists[USBH_EPTYPE_ISO])

+		&& list_empty(&host->ep_pending_lists[USBH_EPTYPE_INT])) {

+		host->otg->GINTMSK &= ~GINTMSK_SOFM;

+	} else {

+		host->otg->GINTMSK |= GINTMSK_SOFM;

+	}

+}

+

+static void _purge_queue(USBHDriver *host, struct list_head *list) {

+	usbh_ep_t *ep, *tmp;

+	list_for_each_entry_safe(ep, usbh_ep_t, tmp, list, node) {

+		usbh_urb_t *const urb = _active_urb(ep);

+		stm32_hc_management_t *const hcm = ep->xfer.hcm;

+		uwarnf("\t%s: Abort URB, USBH_URBSTATUS_DISCONNECTED", ep->name);

+		if (hcm) {

+			uwarnf("\t%s: URB had channel %d assigned, halt_reason = %d", ep->name, hcm - host->channels, hcm->halt_reason);

+			_release_channel(host, hcm);

+			_update_urb(ep, hcm->hc->HCTSIZ, urb, FALSE);

+		}

+		_transfer_completed(ep, urb, USBH_URBSTATUS_DISCONNECTED);

+	}

+}

+

+static void _purge_active(USBHDriver *host) {

+	_purge_queue(host, &host->ep_active_lists[0]);

+	_purge_queue(host, &host->ep_active_lists[1]);

+	_purge_queue(host, &host->ep_active_lists[2]);

+	_purge_queue(host, &host->ep_active_lists[3]);

+}

+

+static void _purge_pending(USBHDriver *host) {

+	_purge_queue(host, &host->ep_pending_lists[0]);

+	_purge_queue(host, &host->ep_pending_lists[1]);

+	_purge_queue(host, &host->ep_pending_lists[2]);

+	_purge_queue(host, &host->ep_pending_lists[3]);

+}

+

+static uint32_t _write_packet(struct list_head *list, uint32_t space_available) {

+	usbh_ep_t *ep;

+

+	uint32_t remaining = 0;

+

+	list_for_each_entry(ep, usbh_ep_t, list, node) {

+		if (ep->in || (ep->xfer.hcm->halt_reason != USBH_LLD_HALTREASON_NONE))

+			continue;

+

+		int32_t rem = ep->xfer.len - ep->xfer.partial;

+		osalDbgCheck(rem >= 0);

+		if (rem <= 0)

+			continue;

+

+		remaining += rem;

+

+		if (!space_available) {

+			if (remaining)

+				break;

+

+			continue;

+		}

+

+		/* write one packet only */

+		if (rem > ep->wMaxPacketSize)

+			rem = ep->wMaxPacketSize;

+

+		/* round up to dwords */

+		uint32_t words = (rem + 3) / 4;

+

+		if (words > space_available)

+			words = space_available;

+

+		space_available -= words;

+

+		uint32_t written = words * 4;

+		if ((int32_t)written > rem)

+			written = rem;

+

+		volatile uint32_t *dest = ep->xfer.hcm->fifo;

+		uint32_t *src = (uint32_t *)ep->xfer.buf;

+		udbgf("\t%s: write %d words (%dB), partial=%d", ep->name, words, written, ep->xfer.partial);

+		while (words--) {

+			*dest = *src++;

+		}

+

+		ep->xfer.buf += written;

+		ep->xfer.partial += written;

+

+		remaining -= written;

+	}

+

+	return remaining;

+}

+

+

+/*===========================================================================*/

+/* API.					   	    	                                 		 */

+/*===========================================================================*/

+

+void usbh_lld_ep_object_init(usbh_ep_t *ep) {

+/*			CTRL(IN)	CTRL(OUT)	INT(IN)		INT(OUT)	BULK(IN)	BULK(OUT)	ISO(IN)		ISO(OUT)

+ * STALL		si		sólo DAT/STAT	si			si			si			si			no			no		ep->type != ISO && (ep->type != CTRL || ctrlphase != SETUP)

+ * ACK			si			si			si			si			si			si			no			no		ep->type != ISO

+ * NAK			si			si			si			si			si			si			no			no		ep->type != ISO

+ * BBERR		si			no			si			no			si			no			si			no		ep->in

+ * TRERR		si			si			si			si			si			si			si			no		ep->type != ISO || ep->in

+ * DTERR		si			no			si			no			si			no			no			no		ep->type != ISO && ep->in

+ * FRMOR		no			no			si			si			no			no			si			si		ep->type = PERIODIC

+ */

+	USBHDriver *host = ep->device->host;

+	uint32_t hcintmsk = HCINTMSK_CHHM | HCINTMSK_XFRCM | HCINTMSK_AHBERRM;

+

+	switch (ep->type) {

+	case USBH_EPTYPE_ISO:

+		hcintmsk |= HCINTMSK_FRMORM;

+		if (ep->in) {

+			hcintmsk |= HCINTMSK_TRERRM | HCINTMSK_BBERRM;

+		}

+		break;

+	case USBH_EPTYPE_INT:

+		hcintmsk |= HCINTMSK_TRERRM | HCINTMSK_FRMORM | HCINTMSK_STALLM | HCINTMSK_NAKM;

+		if (ep->in) {

+			hcintmsk |= HCINTMSK_DTERRM | HCINTMSK_BBERRM;

+		}

+		ep->xfer.u.frame_counter = 1;

+		break;

+	case USBH_EPTYPE_CTRL:

+		hcintmsk |= HCINTMSK_TRERRM | HCINTMSK_STALLM | HCINTMSK_NAKM;

+		break;

+	case USBH_EPTYPE_BULK:

+		hcintmsk |= HCINTMSK_TRERRM | HCINTMSK_STALLM | HCINTMSK_NAKM;

+		if (ep->in) {

+			hcintmsk |= HCINTMSK_DTERRM | HCINTMSK_BBERRM;

+		}

+		break;

+	default:

+		chDbgCheck(0);

+	}

+	ep->active_list = &host->ep_active_lists[ep->type];

+	ep->pending_list = &host->ep_pending_lists[ep->type];

+	INIT_LIST_HEAD(&ep->urb_list);

+	INIT_LIST_HEAD(&ep->node);

+

+	ep->hcintmsk = hcintmsk;

+	ep->hcchar = HCCHAR_CHENA

+			| HCCHAR_DAD(ep->device->address)

+			| HCCHAR_MCNT(1)

+			| HCCHAR_EPTYP(ep->type)

+			| ((ep->device->speed == USBH_DEVSPEED_LOW) ? HCCHAR_LSDEV : 0)

+			| (ep->in ? HCCHAR_EPDIR : 0)

+			| HCCHAR_EPNUM(ep->address)

+			| HCCHAR_MPS(ep->wMaxPacketSize);

+}

+

+void usbh_lld_ep_open(usbh_ep_t *ep) {

+	uinfof("\t%s: Open EP", ep->name);

+	ep->status = USBH_EPSTATUS_OPEN;

+	osalOsRescheduleS();

+}

+

+void usbh_lld_ep_close(usbh_ep_t *ep) {

+	usbh_urb_t *urb, *tmp;

+	uinfof("\t%s: Closing EP...", ep->name);

+	list_for_each_entry_safe(urb, usbh_urb_t, tmp, &ep->urb_list, node) {

+		uinfof("\t%s: Abort URB, USBH_URBSTATUS_DISCONNECTED", ep->name);

+		_usbh_urb_abort_and_waitS(urb, USBH_URBSTATUS_DISCONNECTED);

+	}

+	uinfof("\t%s: Closed", ep->name);

+	ep->status = USBH_EPSTATUS_CLOSED;

+	osalOsRescheduleS();

+}

+

+void usbh_lld_urb_submit(usbh_urb_t *urb) {

+	usbh_ep_t *const ep = urb->ep;

+

+	/* add the URB to the EP's queue */

+	list_add_tail(&urb->node, &ep->urb_list);

+

+	/* check if the EP wasn't in any queue (pending nor active) */

+	if (list_empty(&ep->node)) {

+

+		/* add the EP to the pending queue */

+		_move_to_pending_queue(ep);

+

+		if (usbhEPIsPeriodic(ep)) {

+			ep->device->host->otg->GINTMSK |= GINTMSK_SOFM;

+		} else {

+			/* try to queue non-periodic transfers */

+			_try_commit_np(ep->device->host);

+		}

+	}

+}

+

+bool usbh_lld_urb_abort(usbh_urb_t *urb, usbh_urbstatus_t status) {

+	osalDbgCheck(usbhURBIsBusy(urb));

+

+	usbh_ep_t *const ep = urb->ep;

+	osalDbgCheck(ep);

+	stm32_hc_management_t *const hcm = ep->xfer.hcm;

+

+	if ((hcm != NULL) && (urb == _active_urb(ep))) {

+		/* This URB is active (channel assigned, top of the EP's URB list) */

+

+		if (hcm->halt_reason == USBH_LLD_HALTREASON_NONE) {

+			/* The channel is not being halted */

+			urb->status = status;

+			_halt_channel(ep->device->host, hcm, USBH_LLD_HALTREASON_ABORT);

+		} else {

+			/* The channel is being halted, so we can't re-halt it. The CHH interrupt will

+			 * be in charge of completing the transfer, but the URB will not have the specified status.

+			 */

+		}

+		return FALSE;

+	}

+

+	/* This URB is active, we can cancel it now */

+	_transfer_completedI(ep, urb, status);

+

+	return TRUE;

+}

+

+

+/*===========================================================================*/

+/* Channel Interrupts.   	    	                                 		 */

+/*===========================================================================*/

+

+//CTRL(IN)	CTRL(OUT)	INT(IN)		INT(OUT)	BULK(IN)	BULK(OUT)	ISO(IN)		ISO(OUT)

+//	si			si			si			si			si			si			no			no		ep->type != ISO && !ep->in

+static inline void _ack_int(USBHDriver *host, stm32_hc_management_t *hcm, stm32_otg_host_chn_t *hc) {

+	(void)host;

+	osalDbgAssert(hcm->ep->type != USBH_EPTYPE_ISO, "ACK should not happen in ISO endpoints");

+	hcm->ep->xfer.error_count = 0;

+	hc->HCINTMSK &= ~HCINTMSK_ACKM;

+	udbgf("\t%s: ACK", hcm->ep->name);

+}

+

+//CTRL(IN)	CTRL(OUT)	INT(IN)		INT(OUT)	BULK(IN)	BULK(OUT)	ISO(IN)		ISO(OUT)

+//	si			no			si			no			si			no			no			no		ep->type != ISO && ep->in

+static inline void _dterr_int(USBHDriver *host, stm32_hc_management_t *hcm, stm32_otg_host_chn_t *hc) {

+	(void)host;

+	osalDbgAssert(hcm->ep->in && (hcm->ep->type != USBH_EPTYPE_ISO), "DTERR should not happen in OUT or ISO endpoints");

+#if 0

+	hc->HCINTMSK &= ~(HCINTMSK_DTERRM | HCINTMSK_ACKM);

+	hcm->ep->xfer.error_count = 0;

+	_halt_channel(host, hcm, USBH_LLD_HALTREASON_ERROR);

+#else

+	/* restart directly, no need to halt it in this case */

+	hcm->ep->xfer.error_count = 0;

+	hc->HCINTMSK &= ~HCINTMSK_ACKM;

+	hc->HCCHAR |= HCCHAR_CHENA;

+#endif

+	uerrf("\t%s: DTERR", hcm->ep->name);

+}

+

+//CTRL(IN)	CTRL(OUT)	INT(IN)		INT(OUT)	BULK(IN)	BULK(OUT)	ISO(IN)		ISO(OUT)

+//	si			no			si			no			si			no			si			no		ep->in

+static inline void _bberr_int(USBHDriver *host, stm32_hc_management_t *hcm, stm32_otg_host_chn_t *hc) {

+	osalDbgAssert(hcm->ep->in, "BBERR should not happen in OUT endpoints");

+	hc->HCINTMSK &= ~HCINTMSK_BBERRM;

+	hcm->ep->xfer.error_count = 3;

+	_halt_channel(host, hcm, USBH_LLD_HALTREASON_ERROR);

+	uerrf("\t%s: BBERR", hcm->ep->name);

+}

+

+///CTRL(IN)	CTRL(OUT)	INT(IN)		INT(OUT)	BULK(IN)	BULK(OUT)	ISO(IN)		ISO(OUT)

+//	si			si			si			si			si			si			si			no		ep->type != ISO || ep->in

+static inline void _trerr_int(USBHDriver *host, stm32_hc_management_t *hcm, stm32_otg_host_chn_t *hc) {

+	osalDbgAssert(hcm->ep->in || (hcm->ep->type != USBH_EPTYPE_ISO), "TRERR should not happen in ISO OUT endpoints");

+	hc->HCINTMSK &= ~HCINTMSK_TRERRM;

+	++hcm->ep->xfer.error_count;

+	_halt_channel(host, hcm, USBH_LLD_HALTREASON_ERROR);

+	uerrf("\t%s: TRERR", hcm->ep->name);

+}

+

+//CTRL(IN)	CTRL(OUT)	INT(IN)		INT(OUT)	BULK(IN)	BULK(OUT)	ISO(IN)		ISO(OUT)

+//	no			no			si			si			no			no			si			si		ep->type = PERIODIC

+static inline void _frmor_int(USBHDriver *host, stm32_hc_management_t *hcm, stm32_otg_host_chn_t *hc) {

+	osalDbgAssert(usbhEPIsPeriodic(hcm->ep), "FRMOR should not happen in non-periodic endpoints");

+	hc->HCINTMSK &= ~HCINTMSK_FRMORM;

+	hcm->ep->xfer.error_count = 3;

+	_halt_channel(host, hcm, USBH_LLD_HALTREASON_ERROR);

+	uerrf("\t%s: FRMOR", hcm->ep->name);

+}

+

+//CTRL(IN)	CTRL(OUT)	INT(IN)		INT(OUT)	BULK(IN)	BULK(OUT)	ISO(IN)		ISO(OUT)

+//	si			si			si			si			si			si			no			no		ep->type != ISO

+static inline void _nak_int(USBHDriver *host, stm32_hc_management_t *hcm, stm32_otg_host_chn_t *hc) {

+	osalDbgAssert(hcm->ep->type != USBH_EPTYPE_ISO, "NAK should not happen in ISO endpoints");

+	if (!hcm->ep->in || (hcm->ep->type == USBH_EPTYPE_INT)) {

+		hc->HCINTMSK &= ~HCINTMSK_NAKM;

+		_halt_channel(host, hcm, USBH_LLD_HALTREASON_NAK);

+	} else {

+		/* restart directly, no need to halt it in this case */

+		hcm->ep->xfer.error_count = 0;

+		hc->HCINTMSK &= ~HCINTMSK_ACKM;

+		hc->HCCHAR |= HCCHAR_CHENA;

+	}

+	udbgf("\t%s: NAK", hcm->ep->name);

+}

+

+//CTRL(IN)	CTRL(OUT)	INT(IN)		INT(OUT)	BULK(IN)	BULK(OUT)	ISO(IN)		ISO(OUT)

+//	si		sólo DAT/STAT	si			si			si			si			no			no		ep->type != ISO && (ep->type != CTRL || ctrlphase != SETUP)

+static inline void _stall_int(USBHDriver *host, stm32_hc_management_t *hcm, stm32_otg_host_chn_t *hc) {

+	osalDbgAssert(hcm->ep->type != USBH_EPTYPE_ISO, "STALL should not happen in ISO endpoints");

+	hc->HCINTMSK &= ~HCINTMSK_STALLM;

+	_halt_channel(host, hcm, USBH_LLD_HALTREASON_STALL);

+	uwarnf("\t%s: STALL", hcm->ep->name);

+}

+

+static void _complete_bulk_int(USBHDriver *host, stm32_hc_management_t *hcm, usbh_ep_t *ep, usbh_urb_t *urb, uint32_t hctsiz) {

+	_release_channel(host, hcm);

+	_save_dt_mask(ep, hctsiz);

+	if (_update_urb(ep, hctsiz, urb, TRUE)) {

+		udbgf("\t%s: done", ep->name);

+		_transfer_completed(ep, urb, USBH_URBSTATUS_OK);

+	} else {

+		osalDbgCheck(urb->requestedLength > 0x7FFFF);

+		uwarnf("\t%s: incomplete", ep->name);

+		_move_to_pending_queue(ep);

+	}

+	if (usbhEPIsPeriodic(ep)) {

+		_try_commit_p(host, FALSE);

+	} else {

+		_try_commit_np(host);

+	}

+}

+

+static void _complete_control(USBHDriver *host, stm32_hc_management_t *hcm, usbh_ep_t *ep, usbh_urb_t *urb, uint32_t hctsiz) {

+	osalDbgCheck(ep->xfer.u.ctrl_phase != USBH_LLD_CTRLPHASE_SETUP);

+

+	_release_channel(host, hcm);

+	if (ep->xfer.u.ctrl_phase == USBH_LLD_CTRLPHASE_DATA) {

+		if (_update_urb(ep, hctsiz, urb, TRUE)) {

+			udbgf("\t%s: DATA done", ep->name);

+			ep->xfer.u.ctrl_phase = USBH_LLD_CTRLPHASE_STATUS;

+			ep->in = !ep->in;

+		} else {

+			osalDbgCheck(urb->requestedLength > 0x7FFFF);

+			uwarnf("\t%s: DATA incomplete", ep->name);

+			_save_dt_mask(ep, hctsiz);

+		}

+		_move_to_pending_queue(ep);

+	} else {

+		osalDbgCheck(ep->xfer.u.ctrl_phase == USBH_LLD_CTRLPHASE_STATUS);

+		udbgf("\t%s: STATUS done", ep->name);

+		_transfer_completed(ep, urb, USBH_URBSTATUS_OK);

+	}

+	_try_commit_np(host);

+}

+

+static void _complete_control_setup(USBHDriver *host, stm32_hc_management_t *hcm, usbh_ep_t *ep, usbh_urb_t *urb) {

+	_release_channel(host, hcm);

+	if (urb->requestedLength) {

+		udbgf("\t%s: SETUP done -> DATA", ep->name);

+		ep->xfer.u.ctrl_phase = USBH_LLD_CTRLPHASE_DATA;

+		ep->in = *((uint8_t *)urb->setup_buff) & 0x80 ? TRUE : FALSE;

+		ep->dt_mask = HCTSIZ_DPID_DATA1;

+		ep->xfer.error_count = 0;

+	} else {

+		udbgf("\t%s: SETUP done -> STATUS", ep->name);

+		ep->in = TRUE;

+		ep->xfer.u.ctrl_phase = USBH_LLD_CTRLPHASE_STATUS;

+	}

+	_move_to_pending_queue(ep);

+	_try_commit_np(host);

+}

+

+static void _complete_iso(USBHDriver *host, stm32_hc_management_t *hcm, usbh_ep_t *ep, usbh_urb_t *urb, uint32_t hctsiz) {

+	udbgf("\t%s: done", hcm->ep->name);

+	_release_channel(host, hcm);

+	_update_urb(ep, hctsiz, urb, TRUE);

+	_transfer_completed(ep, urb, USBH_URBSTATUS_OK);

+	_try_commit_p(host, FALSE);

+}

+

+static inline void _xfrc_int(USBHDriver *host, stm32_hc_management_t *hcm, stm32_otg_host_chn_t *hc) {

+	usbh_ep_t *const ep = hcm->ep;

+	usbh_urb_t *const urb = _active_urb(ep);

+	osalDbgCheck(urb);

+	uint32_t hctsiz = hc->HCTSIZ;

+

+	hc->HCINTMSK &= ~HCINTMSK_XFRCM;

+

+	switch (ep->type) {

+	case USBH_EPTYPE_CTRL:

+		if (ep->xfer.u.ctrl_phase == USBH_LLD_CTRLPHASE_SETUP) {

+			_complete_control_setup(host, hcm, ep, urb);

+		} else if (ep->in) {

+			_halt_channel(host, hcm, USBH_LLD_HALTREASON_XFRC);

+		} else {

+			_complete_control(host, hcm, ep, urb, hctsiz);

+		}

+		break;

+

+	case USBH_EPTYPE_BULK:

+		if (ep->in) {

+			_halt_channel(host, hcm, USBH_LLD_HALTREASON_XFRC);

+		} else {

+			_complete_bulk_int(host, hcm, ep, urb, hctsiz);

+		}

+		break;

+

+	case USBH_EPTYPE_INT:

+		if (ep->in && (hctsiz & HCTSIZ_PKTCNT_MASK)) {

+			_halt_channel(host, hcm, USBH_LLD_HALTREASON_XFRC);

+		} else {

+			_complete_bulk_int(host, hcm, ep, urb, hctsiz);

+		}

+		break;

+

+	case USBH_EPTYPE_ISO:

+		if (ep->in && (hctsiz & HCTSIZ_PKTCNT_MASK)) {

+			_halt_channel(host, hcm, USBH_LLD_HALTREASON_XFRC);

+		} else {

+			_complete_iso(host, hcm, ep, urb, hctsiz);

+		}

+		break;

+	}

+}

+

+static inline void _chh_int(USBHDriver *host, stm32_hc_management_t *hcm, stm32_otg_host_chn_t *hc) {

+

+	usbh_ep_t *const ep = hcm->ep;

+	usbh_urb_t *const urb = _active_urb(ep);

+	osalDbgCheck(urb);

+	uint32_t hctsiz = hc->HCTSIZ;

+	usbh_lld_halt_reason_t reason = hcm->halt_reason;

+

+	//osalDbgCheck(reason != USBH_LLD_HALTREASON_NONE);

+	if (reason == USBH_LLD_HALTREASON_NONE) {

+		uwarnf("\tCHH: ch=%d, USBH_LLD_HALTREASON_NONE", hcm - host->channels);

+		return;

+	}

+

+	if (reason == USBH_LLD_HALTREASON_XFRC) {

+		osalDbgCheck(ep->in);

+		switch (ep->type) {

+		case USBH_EPTYPE_CTRL:

+			_complete_control(host, hcm, ep, urb, hctsiz);

+			break;

+		case USBH_EPTYPE_BULK:

+		case USBH_EPTYPE_INT:

+			_complete_bulk_int(host, hcm, ep, urb, hctsiz);

+			break;

+		case USBH_EPTYPE_ISO:

+			_complete_iso(host, hcm, ep, urb, hctsiz);

+			break;

+		}

+	} else {

+		_release_channel(host, hcm);

+		_save_dt_mask(ep, hctsiz);

+		bool done = _update_urb(ep, hctsiz, urb, FALSE);

+

+		switch (reason) {

+		case USBH_LLD_HALTREASON_NAK:

+			if ((ep->type == USBH_EPTYPE_INT) && ep->in) {

+				_transfer_completed(ep, urb, USBH_URBSTATUS_TIMEOUT);

+			} else {

+				ep->xfer.error_count = 0;

+				_move_to_pending_queue(ep);

+			}

+			break;

+

+		case USBH_LLD_HALTREASON_STALL:

+			if ((ep->type == USBH_EPTYPE_CTRL) && (ep->xfer.u.ctrl_phase == USBH_LLD_CTRLPHASE_SETUP)) {

+				uerrf("\t%s: Faulty device: STALLed SETUP phase", ep->name);

+			}

+			_transfer_completed(ep, urb, USBH_URBSTATUS_STALL);

+			break;

+

+		case USBH_LLD_HALTREASON_ERROR:

+			if ((ep->type == USBH_EPTYPE_ISO) || done || (ep->xfer.error_count >= 3)) {

+				_transfer_completed(ep, urb, USBH_URBSTATUS_ERROR);

+			} else {

+				uerrf("\t%s: err=%d, done=%d, retry", ep->name, ep->xfer.error_count, done);

+				_move_to_pending_queue(ep);

+			}

+			break;

+

+		case USBH_LLD_HALTREASON_ABORT:

+			uwarnf("\t%s: Abort", ep->name);

+			_transfer_completed(ep, urb, urb->status);

+			break;

+

+		default:

+			osalDbgCheck(0);

+			break;

+		}

+

+		if (usbhEPIsPeriodic(ep)) {

+			_try_commit_p(host, FALSE);

+		} else {

+			_try_commit_np(host);

+		}

+	}

+}

+

+static void _hcint_n_int(USBHDriver *host, uint8_t chn) {

+

+	stm32_hc_management_t *const hcm = &host->channels[chn];

+	stm32_otg_host_chn_t *const hc = hcm->hc;

+

+	uint32_t hcint = hc->HCINT;

+	hcint &= hc->HCINTMSK;

+	hc->HCINT = hcint;

+

+	osalDbgCheck((hcint & HCINTMSK_AHBERRM) == 0);

+	osalDbgCheck(hcm->ep);

+

+	if (hcint & HCINTMSK_STALLM)

+		_stall_int(host, hcm, hc);

+	if (hcint & HCINTMSK_NAKM)

+		_nak_int(host, hcm, hc);

+	if (hcint & HCINTMSK_ACKM)

+		_ack_int(host, hcm, hc);

+	if (hcint & HCINTMSK_TRERRM)

+		_trerr_int(host, hcm, hc);

+	if (hcint & HCINTMSK_BBERRM)

+		_bberr_int(host, hcm, hc);

+	if (hcint & HCINTMSK_FRMORM)

+		_frmor_int(host, hcm, hc);

+	if (hcint & HCINTMSK_DTERRM)

+		_dterr_int(host, hcm, hc);

+	if (hcint & HCINTMSK_XFRCM)

+		_xfrc_int(host, hcm, hc);

+	if (hcint & HCINTMSK_CHHM)

+		_chh_int(host, hcm, hc);

+}

+

+static inline void _hcint_int(USBHDriver *host) {

+	uint32_t haint;

+

+	haint = host->otg->HAINT;

+	haint &= host->otg->HAINTMSK;

+

+	if (!haint) {

+		uerrf("HAINT=%08x, HAINTMSK=%08x", host->otg->HAINT, host->otg->HAINTMSK);

+		return;

+	}

+

+#if 1	//channel lookup loop

+	uint8_t i;

+	for (i = 0; haint && (i < host->channels_number); i++) {

+		if (haint & (1 << i)) {

+			_hcint_n_int(host, i);

+			haint &= ~(1 << i);

+		}

+	}

+#else	//faster calculation, with __CLZ (count leading zeroes)

+	while (haint) {

+		uint8_t chn = (uint8_t)(31 - __CLZ(haint));

+		osalDbgAssert(chn < host->channels_number, "what?");

+		haint &= ~host->channels[chn].haintmsk;

+		_hcint_n_int(host, chn);

+	}

+#endif

+}

+

+

+/*===========================================================================*/

+/* Host interrupts.			   	                                     		 */

+/*===========================================================================*/

+static inline void _sof_int(USBHDriver *host) {

+	udbg("SOF");

+	_try_commit_p(host, TRUE);

+}

+

+static inline void _rxflvl_int(USBHDriver *host) {

+

+	stm32_otg_t *const otg = host->otg;

+

+	otg->GINTMSK &= ~GINTMSK_RXFLVLM;

+	while (otg->GINTSTS & GINTSTS_RXFLVL) {

+		uint32_t grxstsp = otg->GRXSTSP;

+		osalDbgCheck((grxstsp & GRXSTSP_CHNUM_MASK) < host->channels_number);

+		stm32_hc_management_t *const hcm = &host->channels[grxstsp & GRXSTSP_CHNUM_MASK];

+		uint32_t hctsiz = hcm->hc->HCTSIZ;

+

+		if ((grxstsp & GRXSTSP_PKTSTS_MASK) == GRXSTSP_PKTSTS(2)) {

+			/* 0010: IN data packet received */

+			usbh_ep_t *const ep = hcm->ep;

+			osalDbgCheck(ep);

+

+			/* restart the channel ASAP */

+			if (hctsiz & HCTSIZ_PKTCNT_MASK) {

+#if CH_DBG_ENABLE_CHECKS

+				if (usbhEPIsPeriodic(ep)) {

+					osalDbgCheck(host->otg->HPTXSTS & HPTXSTS_PTXQSAV_MASK);

+				} else {

+					osalDbgCheck(host->otg->HNPTXSTS & HPTXSTS_PTXQSAV_MASK);

+				}

+#endif

+				hcm->hc->HCCHAR |= HCCHAR_CHENA;

+			}

+

+			udbgf("\t%s: RXFLVL rx=%dB, rem=%dB (%dpkts)",

+					ep->name,

+					(grxstsp & GRXSTSP_BCNT_MASK) >> 4,

+					(hctsiz & HCTSIZ_XFRSIZ_MASK),

+					(hctsiz & HCTSIZ_PKTCNT_MASK) >> 19);

+

+			/* Read */

+			uint32_t *dest = (uint32_t *)ep->xfer.buf;

+			volatile uint32_t *const src = hcm->fifo;

+

+			uint32_t bcnt = (grxstsp & GRXSTSP_BCNT_MASK) >> GRXSTSP_BCNT_OFF;

+			osalDbgCheck(bcnt + ep->xfer.partial <= ep->xfer.len);

+

+			//TODO: optimize this

+			uint32_t words = bcnt / 4;

+			uint8_t bytes = bcnt & 3;

+			while (words--) {

+				*dest++ = *src;

+			}

+			if (bytes) {

+				uint32_t r = *src;

+				uint8_t *bsrc = (uint8_t *)&r;

+				uint8_t *bdest = (uint8_t *)dest;

+				do {

+					*bdest++ = *bsrc++;

+				} while (--bytes);

+			}

+

+			ep->xfer.buf += bcnt;

+			ep->xfer.partial += bcnt;

+

+#if 0 //STM32_USBH_CHANNELS_NP > 1

+			/* check bug */

+			if (hctsiz & HCTSIZ_PKTCNT_MASK) {

+				uint32_t pkt = (hctsiz & HCTSIZ_PKTCNT_MASK) >> 19;

+				uint32_t siz = (hctsiz & HCTSIZ_XFRSIZ_MASK);

+				if (pkt * ep->wMaxPacketSize != siz) {

+					uerrf("\t%s: whatttt???", ep->name);

+				}

+			}

+#endif

+

+#if USBH_DEBUG_ENABLE && USBH_LLD_DEBUG_ENABLE_ERRORS

+		} else {

+			/* 0011: IN transfer completed (triggers an interrupt)

+			 * 0101: Data toggle error (triggers an interrupt)

+			 * 0111: Channel halted (triggers an interrupt)

+			 */

+			switch (grxstsp & GRXSTSP_PKTSTS_MASK) {

+			case GRXSTSP_PKTSTS(3):

+			case GRXSTSP_PKTSTS(5):

+			case GRXSTSP_PKTSTS(7):

+				break;

+			default:

+				uerrf("\tRXFLVL: ch=%d, UNK=%d", grxstsp & GRXSTSP_CHNUM_MASK, (grxstsp & GRXSTSP_PKTSTS_MASK) >> 17);

+				break;

+			}

+#endif

+		}

+	}

+	otg->GINTMSK |= GINTMSK_RXFLVLM;

+}

+

+static inline void _nptxfe_int(USBHDriver *host) {

+	uint32_t rem;

+	stm32_otg_t *const otg = host->otg;

+

+	rem = _write_packet(&host->ep_active_lists[USBH_EPTYPE_CTRL],

+			otg->HNPTXSTS & HPTXSTS_PTXFSAVL_MASK);

+

+	rem += _write_packet(&host->ep_active_lists[USBH_EPTYPE_BULK],

+			otg->HNPTXSTS & HPTXSTS_PTXFSAVL_MASK);

+

+//	if (rem)

+//		otg->GINTMSK |= GINTMSK_NPTXFEM;

+

+	if (!rem)

+		otg->GINTMSK &= ~GINTMSK_NPTXFEM;

+

+}

+

+static inline void _ptxfe_int(USBHDriver *host) {

+	//TODO: implement

+	(void)host;

+	uinfo("PTXFE");

+}

+

+static inline void _discint_int(USBHDriver *host) {

+	uint32_t hprt = host->otg->HPRT;

+

+	uwarn("\tDISCINT");

+

+	if (!(hprt & HPRT_PCSTS)) {

+		host->rootport.lld_status &= ~(USBH_PORTSTATUS_CONNECTION | USBH_PORTSTATUS_ENABLE);

+		host->rootport.lld_c_status |= USBH_PORTSTATUS_C_CONNECTION | USBH_PORTSTATUS_C_ENABLE;

+	}

+	_purge_active(host);

+	_purge_pending(host);

+}

+

+static inline void _hprtint_int(USBHDriver *host) {

+	stm32_otg_t *const otg = host->otg;

+	uint32_t hprt = otg->HPRT;

+

+	/* note: writing PENA = 1 actually disables the port */

+	uint32_t hprt_clr = hprt & ~(HPRT_PENA | HPRT_PCDET | HPRT_PENCHNG | HPRT_POCCHNG);

+

+	if (hprt & HPRT_PCDET) {

+		hprt_clr |= HPRT_PCDET;

+		if (hprt & HPRT_PCSTS) {

+			uinfo("\tHPRT: Port connection detected");

+			host->rootport.lld_status |= USBH_PORTSTATUS_CONNECTION;

+			host->rootport.lld_c_status |= USBH_PORTSTATUS_C_CONNECTION;

+		} else {

+			uinfo("\tHPRT: Port disconnection detected");

+		}

+	}

+

+	if (hprt & HPRT_PENCHNG) {

+		hprt_clr |= HPRT_PENCHNG;

+		if (hprt & HPRT_PENA) {

+			uinfo("\tHPRT: Port enabled");

+			host->rootport.lld_status |= USBH_PORTSTATUS_ENABLE;

+			host->rootport.lld_status &= ~(USBH_PORTSTATUS_HIGH_SPEED | USBH_PORTSTATUS_LOW_SPEED);

+

+			/* Make sure the FIFOs are flushed. */

+		    otg_txfifo_flush(host, 0x10);

+		    otg_rxfifo_flush(host);

+

+		    /* Clear all pending HC Interrupts */

+			uint8_t i;

+			for (i = 0; i < host->channels_number; i++) {

+				otg->hc[i].HCINTMSK = 0;

+				otg->hc[i].HCINT = 0xFFFFFFFF;

+			}

+

+			/* configure speed */

+			if ((hprt & HPRT_PSPD_MASK) == HPRT_PSPD_LS) {

+				host->rootport.lld_status |= USBH_PORTSTATUS_LOW_SPEED;

+				otg->HFIR = 6000;

+				otg->HCFG = (otg->HCFG & ~HCFG_FSLSPCS_MASK) | HCFG_FSLSPCS_6;

+			} else {

+				otg->HFIR = 48000;

+				otg->HCFG = (otg->HCFG & ~HCFG_FSLSPCS_MASK) | HCFG_FSLSPCS_48;

+			}

+		} else {

+			if (hprt & HPRT_PCSTS) {

+				if (hprt & HPRT_POCA) {

+					uerr("\tHPRT: Port disabled due to overcurrent");

+				} else {

+					uerr("\tHPRT: Port disabled due to port babble");

+				}

+			} else {

+				uerr("\tHPRT: Port disabled due to disconnect");

+			}

+

+			_purge_active(host);

+			_purge_pending(host);

+

+			host->rootport.lld_status &= ~USBH_PORTSTATUS_ENABLE;

+		}

+		host->rootport.lld_c_status |= USBH_PORTSTATUS_C_ENABLE;

+	}

+

+	if (hprt & HPRT_POCCHNG) {

+		hprt_clr |= HPRT_POCCHNG;

+		if (hprt & HPRT_POCA) {

+			uerr("\tHPRT: Overcurrent");

+			host->rootport.lld_status |= USBH_PORTSTATUS_OVERCURRENT;

+		} else {

+			udbg("\tHPRT: Clear overcurrent");

+			host->rootport.lld_status &= ~USBH_PORTSTATUS_OVERCURRENT;

+		}

+		host->rootport.lld_c_status |= USBH_PORTSTATUS_C_OVERCURRENT;

+	}

+

+	otg->HPRT = hprt_clr;

+}

+

+static void usb_lld_serve_interrupt(USBHDriver *host) {

+	osalDbgCheck(host && (host->status != USBH_STATUS_STOPPED));

+

+	stm32_otg_t *const otg = host->otg;

+	uint32_t gintsts = otg->GINTSTS;

+

+	/* check host mode */

+	if (!(gintsts & GINTSTS_CMOD)) {

+		uerr("Device mode");

+		otg->GINTSTS = gintsts;

+		return;

+	}

+

+	/* check mismatch */

+	if (gintsts & GINTSTS_MMIS) {

+		uerr("Mode Mismatch");

+		otg->GINTSTS = gintsts;

+		return;

+	}

+

+	gintsts &= otg->GINTMSK;

+	if (!gintsts) {

+		uwarnf("GINTSTS=%08x, GINTMSK=%08x", otg->GINTSTS, otg->GINTMSK);

+		return;

+	}

+//	otg->GINTMSK &= ~(GINTMSK_NPTXFEM | GINTMSK_PTXFEM);

+	otg->GINTSTS = gintsts;

+

+	if (gintsts & GINTSTS_SOF)

+		_sof_int(host);

+	if (gintsts & GINTSTS_RXFLVL)

+		_rxflvl_int(host);

+	if (gintsts & GINTSTS_HPRTINT)

+		_hprtint_int(host);

+	if (gintsts & GINTSTS_DISCINT)

+		_discint_int(host);

+	if (gintsts & GINTSTS_HCINT)

+		_hcint_int(host);

+	if (gintsts & GINTSTS_NPTXFE)

+		_nptxfe_int(host);

+	if (gintsts & GINTSTS_PTXFE)

+		_ptxfe_int(host);

+	if (gintsts & GINTSTS_IPXFR) {

+		uerr("IPXFRM");

+	}

+}

+

+

+/*===========================================================================*/

+/* Interrupt handlers.   	    	                                 		 */

+/*===========================================================================*/

+

+#if STM32_USBH_USE_OTG1

+OSAL_IRQ_HANDLER(STM32_OTG1_HANDLER) {

+	OSAL_IRQ_PROLOGUE();

+	osalSysLockFromISR();

+	usb_lld_serve_interrupt(&USBHD1);

+	osalSysUnlockFromISR();

+	OSAL_IRQ_EPILOGUE();

+}

+#endif

+

+#if STM32_USBH_USE_OTG2

+OSAL_IRQ_HANDLER(STM32_OTG2_HANDLER) {

+	OSAL_IRQ_PROLOGUE();

+	osalSysLockFromISR();

+	usb_lld_serve_interrupt(&USBHD2);

+	osalSysUnlockFromISR();

+	OSAL_IRQ_EPILOGUE();

+}

+#endif

+

+

+/*===========================================================================*/

+/* Initialization functions.   	                                     		 */

+/*===========================================================================*/

+static void otg_core_reset(USBHDriver *usbp) {

+  stm32_otg_t *const otgp = usbp->otg;

+

+  /* Wait AHB idle condition.*/

+  while ((otgp->GRSTCTL & GRSTCTL_AHBIDL) == 0)

+	;

+

+  osalSysPolledDelayX(64);

+

+  /* Core reset and delay of at least 3 PHY cycles.*/

+  otgp->GRSTCTL = GRSTCTL_CSRST;

+  while ((otgp->GRSTCTL & GRSTCTL_CSRST) != 0)

+	;

+

+  osalSysPolledDelayX(24);

+

+  /* Wait AHB idle condition.*/

+  while ((otgp->GRSTCTL & GRSTCTL_AHBIDL) == 0)

+	;

+}

+

+static void otg_rxfifo_flush(USBHDriver *usbp) {

+  stm32_otg_t *const otgp = usbp->otg;

+

+  otgp->GRSTCTL = GRSTCTL_RXFFLSH;

+  while ((otgp->GRSTCTL & GRSTCTL_RXFFLSH) != 0)

+	;

+  /* Wait for 3 PHY Clocks.*/

+  osalSysPolledDelayX(24);

+}

+

+static void otg_txfifo_flush(USBHDriver *usbp, uint32_t fifo) {

+  stm32_otg_t *const otgp = usbp->otg;

+

+  otgp->GRSTCTL = GRSTCTL_TXFNUM(fifo) | GRSTCTL_TXFFLSH;

+  while ((otgp->GRSTCTL & GRSTCTL_TXFFLSH) != 0)

+	;

+  /* Wait for 3 PHY Clocks.*/

+  osalSysPolledDelayX(24);

+}

+

+static void _init(USBHDriver *host) {

+	int i;

+

+	usbhObjectInit(host);

+

+#if STM32_USBH_USE_OTG1

+#if STM32_USBH_USE_OTG2

+	if (&USBHD1 == host) {

+#endif

+		host->otg = OTG_FS;

+		host->channels_number = STM32_OTG1_CHANNELS_NUMBER;

+#if STM32_USBH_USE_OTG2

+	}

+#endif

+#endif

+

+#if STM32_USBH_USE_OTG2

+#if STM32_USBH_USE_OTG1

+	if (&USBHD2 == host) {

+#endif

+		host->otg = OTG_HS;

+		host->channels_number = STM32_OTG2_CHANNELS_NUMBER;

+#if STM32_USBH_USE_OTG1

+	}

+#endif

+#endif

+	INIT_LIST_HEAD(&host->ch_free[0]);

+	INIT_LIST_HEAD(&host->ch_free[1]);

+	for (i = 0; i < host->channels_number; i++) {

+		host->channels[i].haintmsk = 1 << i;

+		host->channels[i].hc = &host->otg->hc[i];

+		host->channels[i].fifo = host->otg->FIFO[i];

+		if (i < STM32_USBH_CHANNELS_NP) {

+			list_add_tail(&host->channels[i].node, &host->ch_free[1]);

+		} else {

+			list_add_tail(&host->channels[i].node, &host->ch_free[0]);

+		}

+	}

+	for (i = 0; i < 4; i++) {

+		INIT_LIST_HEAD(&host->ep_active_lists[i]);

+		INIT_LIST_HEAD(&host->ep_pending_lists[i]);

+	}

+}

+

+void usbh_lld_init(void) {

+#if STM32_USBH_USE_OTG1

+	_init(&USBHD1);

+#endif

+#if STM32_USBH_USE_OTG2

+	_init(&USBHD2);

+#endif

+}

+

+static void _usbh_start(USBHDriver *usbh) {

+	stm32_otg_t *const otgp = usbh->otg;

+

+	/* Clock activation.*/

+#if STM32_USBH_USE_OTG1

+#if STM32_USBH_USE_OTG2

+	if (&USBHD1 == usbh) {

+#endif

+		/* OTG FS clock enable and reset.*/

+		rccEnableOTG_FS(FALSE);

+		rccResetOTG_FS();

+

+		otgp->GINTMSK = 0;

+

+		/* Enables IRQ vector.*/

+		nvicEnableVector(STM32_OTG1_NUMBER, STM32_USB_OTG1_IRQ_PRIORITY);

+#if STM32_USBH_USE_OTG2

+	}

+#endif

+#endif

+

+#if STM32_USBH_USE_OTG2

+#if STM32_USBH_USE_OTG1

+	if (&USBHD2 == usbh) {

+#endif

+		/* OTG HS clock enable and reset.*/

+		rccEnableOTG_HS(FALSE);

+		rccResetOTG_HS();

+

+		otgp->GINTMSK = 0;

+

+		/* Enables IRQ vector.*/

+		nvicEnableVector(STM32_OTG2_NUMBER, STM32_USB_OTG2_IRQ_PRIORITY);

+#if STM32_USBH_USE_OTG1

+	}

+#endif

+#endif

+

+	otgp->GUSBCFG = GUSBCFG_PHYSEL | GUSBCFG_TRDT(5);

+

+	otg_core_reset(usbh);

+

+	otgp->GCCFG = GCCFG_PWRDWN;

+

+	/* Forced host mode. */

+	otgp->GUSBCFG = GUSBCFG_FHMOD | GUSBCFG_PHYSEL | GUSBCFG_TRDT(5);

+

+	/* PHY enabled.*/

+	otgp->PCGCCTL = 0;

+

+	/* Internal FS PHY activation.*/

+#if defined(BOARD_OTG_NOVBUSSENS)

+	otgp->GCCFG = GCCFG_NOVBUSSENS | GCCFG_PWRDWN;

+#else

+	otgp->GCCFG = GCCFG_PWRDWN;

+#endif

+

+	/* 48MHz 1.1 PHY.*/

+	otgp->HCFG = HCFG_FSLSS | HCFG_FSLSPCS_48;

+

+	/* Interrupts on FIFOs half empty.*/

+	otgp->GAHBCFG = 0;

+

+	otgp->GOTGINT = 0xFFFFFFFF;

+

+	otgp->HPRT |= HPRT_PPWR;

+

+	/* without this delay, the FIFO sizes are set INcorrectly */

+	osalThreadSleepS(MS2ST(200));

+

+#define HNPTXFSIZ						DIEPTXF0

+#if STM32_USBH_USE_OTG1

+#if STM32_USBH_USE_OTG2

+	if (&USBHD1 == usbh) {

+#endif

+		otgp->GRXFSIZ = GRXFSIZ_RXFD(STM32_OTG1_RXFIFO_SIZE / 4);

+		otgp->HNPTXFSIZ = HPTXFSIZ_PTXSA((STM32_OTG1_RXFIFO_SIZE / 4)) | HPTXFSIZ_PTXFD(STM32_OTG1_NPTXFIFO_SIZE / 4);

+		otgp->HPTXFSIZ = HPTXFSIZ_PTXSA((STM32_OTG1_RXFIFO_SIZE / 4) + (STM32_OTG1_NPTXFIFO_SIZE / 4)) | HPTXFSIZ_PTXFD(STM32_OTG1_PTXFIFO_SIZE / 4);

+#if STM32_USBH_USE_OTG2

+	}

+#endif

+#endif

+#if STM32_USBH_USE_OTG2

+#if STM32_USBH_USE_OTG1

+	if (&USBHD2 == usbh) {

+#endif

+		otgp->GRXFSIZ = GRXFSIZ_RXFD(STM32_OTG2_RXFIFO_SIZE / 4);

+		otgp->HNPTXFSIZ = HPTXFSIZ_PTXSA((STM32_OTG2_RXFIFO_SIZE / 4)) | HPTXFSIZ_PTXFD(STM32_OTG2_NPTXFIFO_SIZE / 4);

+		otgp->HPTXFSIZ = HPTXFSIZ_PTXSA((STM32_OTG2_RXFIFO_SIZE / 4) + (STM32_OTG2_NPTXFIFO_SIZE / 4)) | HPTXFSIZ_PTXFD(STM32_OTG2_PTXFIFO_SIZE / 4);

+#if STM32_USBH_USE_OTG1

+	}

+#endif

+#endif

+

+	otg_txfifo_flush(usbh, 0x10);

+	otg_rxfifo_flush(usbh);

+

+	otgp->GINTSTS = 0xffffffff;

+	otgp->GINTMSK = GINTMSK_DISCM /*| GINTMSK_PTXFEM*/ | GINTMSK_HCM | GINTMSK_HPRTM

+					/*| GINTMSK_IPXFRM | GINTMSK_NPTXFEM*/ | GINTMSK_RXFLVLM

+					/*| GINTMSK_SOFM */ | GINTMSK_MMISM;

+

+	usbh->rootport.lld_status = USBH_PORTSTATUS_POWER;

+	usbh->rootport.lld_c_status = 0;

+

+	/* Global interrupts enable.*/

+	otgp->GAHBCFG |= GAHBCFG_GINTMSK;

+}

+

+void usbh_lld_start(USBHDriver *usbh) {

+	if (usbh->status != USBH_STATUS_STOPPED) return;

+	_usbh_start(usbh);

+}

+

+/*===========================================================================*/

+/* Root Hub request handler.   	                                     		 */

+/*===========================================================================*/

+usbh_urbstatus_t usbh_lld_root_hub_request(USBHDriver *usbh, uint8_t bmRequestType, uint8_t bRequest,

+		uint16_t wvalue, uint16_t windex, uint16_t wlength, uint8_t *buf) {

+

+	uint16_t typereq = (bmRequestType << 8) | bRequest;

+

+	switch (typereq) {

+	case ClearHubFeature:

+		switch (wvalue) {

+		case USBH_HUB_FEAT_C_HUB_LOCAL_POWER:

+		case USBH_HUB_FEAT_C_HUB_OVER_CURRENT:

+			break;

+		default:

+			osalDbgAssert(0, "invalid wvalue");

+		}

+		break;

+

+	case ClearPortFeature:

+		chDbgAssert(windex == 1, "invalid windex");

+

+		osalSysLock();

+		switch (wvalue) {

+		case USBH_PORT_FEAT_ENABLE:

+		case USBH_PORT_FEAT_SUSPEND:

+		case USBH_PORT_FEAT_POWER:

+			chDbgAssert(0, "unimplemented");	/* TODO */

+			break;

+

+		case USBH_PORT_FEAT_INDICATOR:

+			chDbgAssert(0, "unsupported");

+			break;

+

+		case USBH_PORT_FEAT_C_CONNECTION:

+			usbh->rootport.lld_c_status &= ~USBH_PORTSTATUS_C_CONNECTION;

+			break;

+

+		case USBH_PORT_FEAT_C_RESET:

+			usbh->rootport.lld_c_status &= ~USBH_PORTSTATUS_C_RESET;

+			break;

+

+		case USBH_PORT_FEAT_C_ENABLE:

+			usbh->rootport.lld_c_status &= ~USBH_PORTSTATUS_C_ENABLE;

+			break;

+

+		case USBH_PORT_FEAT_C_SUSPEND:

+			usbh->rootport.lld_c_status &= ~USBH_PORTSTATUS_C_SUSPEND;

+			break;

+

+		case USBH_PORT_FEAT_C_OVERCURRENT:

+			usbh->rootport.lld_c_status &= USBH_PORTSTATUS_C_OVERCURRENT;

+			break;

+

+		default:

+			osalDbgAssert(0, "invalid wvalue");

+			break;

+		}

+		osalOsRescheduleS();

+		osalSysUnlock();

+		break;

+

+	case GetHubDescriptor:

+		/*dev_dbg(hsotg->dev, "GetHubDescriptor\n");

+		hub_desc = (struct usb_hub_descriptor *)buf;

+		hub_desc->bDescLength = 9;

+		hub_desc->bDescriptorType = USB_DT_HUB;

+		hub_desc->bNbrPorts = 1;

+		hub_desc->wHubCharacteristics =

+			cpu_to_le16(HUB_CHAR_COMMON_LPSM |

+					HUB_CHAR_INDV_PORT_OCPM);

+		hub_desc->bPwrOn2PwrGood = 1;

+		hub_desc->bHubContrCurrent = 0;

+		hub_desc->u.hs.DeviceRemovable[0] = 0;

+		hub_desc->u.hs.DeviceRemovable[1] = 0xff;*/

+		break;

+

+	case GetHubStatus:

+		osalDbgCheck(wlength >= 4);

+		*(uint32_t *)buf = 0;

+		break;

+

+	case GetPortStatus:

+		chDbgAssert(windex == 1, "invalid windex");

+		osalDbgCheck(wlength >= 4);

+		osalSysLock();

+		*(uint32_t *)buf = usbh->rootport.lld_status | (usbh->rootport.lld_c_status << 16);

+		osalOsRescheduleS();

+		osalSysUnlock();

+		break;

+

+	case SetHubFeature:

+		chDbgAssert(0, "unsupported");

+		break;

+

+	case SetPortFeature:

+		chDbgAssert(windex == 1, "invalid windex");

+

+		switch (wvalue) {

+		case USBH_PORT_FEAT_TEST:

+		case USBH_PORT_FEAT_SUSPEND:

+		case USBH_PORT_FEAT_POWER:

+			chDbgAssert(0, "unimplemented");	/* TODO */

+			break;

+

+		case USBH_PORT_FEAT_RESET: {

+			osalSysLock();

+			stm32_otg_t *const otg = usbh->otg;

+			uint32_t hprt;

+			otg->PCGCCTL = 0;

+			hprt = otg->HPRT;

+			/* note: writing PENA = 1 actually disables the port */

+			hprt &= ~(HPRT_PSUSP | HPRT_PENA | HPRT_PCDET | HPRT_PENCHNG | HPRT_POCCHNG );

+			otg->HPRT = hprt | HPRT_PRST;

+			osalThreadSleepS(MS2ST(60));

+			otg->HPRT = hprt;

+			usbh->rootport.lld_c_status |= USBH_PORTSTATUS_C_RESET;

+			osalOsRescheduleS();

+			osalSysUnlock();

+		} 	break;

+

+		case USBH_PORT_FEAT_INDICATOR:

+			chDbgAssert(0, "unsupported");

+			break;

+

+		default:

+			osalDbgAssert(0, "invalid wvalue");

+			break;

+		}

+		break;

+

+	default:

+		osalDbgAssert(0, "invalid typereq");

+		break;

+	}

+

+	return USBH_URBSTATUS_OK;

+}

+

+uint8_t usbh_lld_roothub_get_statuschange_bitmap(USBHDriver *usbh) {

+	osalSysLock();

+	if (usbh->rootport.lld_c_status) {

+		osalOsRescheduleS();

+		osalSysUnlock();

+		return 1 << 1;

+	}

+	osalOsRescheduleS();

+	osalSysUnlock();

+	return 0;

+}

+

+

+#endif

diff --git a/os/hal/ports/STM32/LLD/USBHv1/usbh_lld.h b/os/hal/ports/STM32/LLD/USBHv1/usbh_lld.h
new file mode 100644
index 0000000..e8da2ac
--- /dev/null
+++ b/os/hal/ports/STM32/LLD/USBHv1/usbh_lld.h
@@ -0,0 +1,153 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

+

+    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.

+*/

+

+#ifndef USBH_LLD_H_

+#define USBH_LLD_H_

+

+#include "hal.h"

+

+#if HAL_USE_USBH

+

+#include "osal.h"

+#include "stm32_otg.h"

+

+/* TODO:

+ *

+ * - Implement ISO/INT OUT and test

+ * - Consider DMA mode for OTG_HS, consider external PHY for HS.

+ * - Implement a data pump thread, so we don't have to copy data from the ISR

+ * 		This might be a bad idea for small endpoint packet sizes (the context switch

+ * 		could be longer than the copy)

+ */

+

+typedef enum {

+	USBH_LLD_CTRLPHASE_SETUP,

+	USBH_LLD_CTRLPHASE_DATA,

+	USBH_LLD_CTRLPHASE_STATUS

+} usbh_lld_ctrlphase_t;

+

+typedef enum {

+	USBH_LLD_HALTREASON_NONE,

+	USBH_LLD_HALTREASON_XFRC,

+	USBH_LLD_HALTREASON_NAK,

+	USBH_LLD_HALTREASON_STALL,

+	USBH_LLD_HALTREASON_ERROR,

+	USBH_LLD_HALTREASON_ABORT

+} usbh_lld_halt_reason_t;

+

+

+typedef struct stm32_hc_management {

+	struct list_head node;

+

+	stm32_otg_host_chn_t *hc;

+	volatile uint32_t	*fifo;

+	usbh_ep_t 			*ep;

+	uint16_t			haintmsk;

+	usbh_lld_halt_reason_t halt_reason;

+} stm32_hc_management_t;

+

+

+#define _usbhdriver_ll_data											\

+	stm32_otg_t *otg;												\

+	/* channels */													\

+	uint8_t channels_number;										\

+	stm32_hc_management_t channels[STM32_OTG2_CHANNELS_NUMBER];		\

+	struct list_head ch_free[2];									\

+	/* Enpoints being processed */									\

+	struct list_head ep_active_lists[4];							\

+	/* Pending endpoints */											\

+	struct list_head ep_pending_lists[4];

+

+

+#define _usbh_ep_ll_data																\

+		struct list_head	*active_list;		/* shortcut to ep list */				\

+		struct list_head	*pending_list;		/* shortcut to ep list */				\

+		struct list_head	urb_list;			/* list of URBs queued in this EP */	\

+		struct list_head	node;				/* this EP */							\

+		uint32_t 			hcintmsk;													\

+		uint32_t			hcchar;														\

+		uint32_t 			dt_mask;			/* data-toggle mask */					\

+		/* current transfer */															\

+		struct {																		\

+			stm32_hc_management_t *hcm;				/* assigned channel */				\

+			uint32_t			len;				/* this transfer's total length */	\

+			uint8_t				*buf;				/* this transfer's buffer */		\

+			uint32_t			partial;			/* this transfer's partial length */\

+			uint16_t			packets;			/* packets allocated */				\

+			union {																		\

+				uint32_t			frame_counter;		/* frame counter (for INT) */	\

+				usbh_lld_ctrlphase_t	ctrl_phase;		/* control phase (for CTRL) */	\

+			} u;																		\

+			uint8_t				error_count;		/* error count */					\

+		} xfer;

+

+

+

+

+

+#define _usbh_port_ll_data		\

+	uint16_t lld_c_status;		\

+	uint16_t lld_status;

+

+#define _usbh_device_ll_data

+

+#define _usbh_hub_ll_data

+

+#define _usbh_urb_ll_data		\

+	struct list_head node;		\

+	bool queued;

+

+

+#define usbh_lld_urb_object_init(urb) 									\

+		do {															\

+			osalDbgAssert(((uint32_t)urb->buff & 3) == 0, 				\

+				"use USBH_DEFINE_BUFFER() to declare the IO buffers"); 	\

+				urb->queued = FALSE;									\

+		} while (0)

+

+

+#define usbh_lld_urb_object_reset(urb) 									\

+		do {															\

+			osalDbgAssert(urb->queued == FALSE, "wrong state");			\

+			osalDbgAssert(((uint32_t)urb->buff & 3) == 0, 				\

+				"use USBH_DEFINE_BUFFER() to declare the IO buffers"); 	\

+		} while (0)

+

+

+

+void usbh_lld_init(void);

+void usbh_lld_start(USBHDriver *usbh);

+void usbh_lld_ep_object_init(usbh_ep_t *ep);

+void usbh_lld_ep_open(usbh_ep_t *ep);

+void usbh_lld_ep_close(usbh_ep_t *ep);

+void usbh_lld_urb_submit(usbh_urb_t *urb);

+bool usbh_lld_urb_abort(usbh_urb_t *urb, usbh_urbstatus_t status);

+usbh_urbstatus_t usbh_lld_root_hub_request(USBHDriver *usbh, uint8_t bmRequestType, uint8_t bRequest,

+		uint16_t wvalue, uint16_t windex, uint16_t wlength, uint8_t *buf);

+uint8_t usbh_lld_roothub_get_statuschange_bitmap(USBHDriver *usbh);

+

+#define usbh_lld_epreset(ep) do {(ep)->dt_mask = HCTSIZ_DPID_DATA0;} while (0);

+

+#ifdef __IAR_SYSTEMS_ICC__

+#define USBH_LLD_DEFINE_BUFFER(type, name) type name

+#else

+#define USBH_LLD_DEFINE_BUFFER(type, name) type name __attribute__((aligned(4)))

+#endif

+

+#endif

+

+#endif /* USBH_LLD_H_ */

diff --git a/os/hal/ports/STM32/STM32F0xx/platform.mk b/os/hal/ports/STM32/STM32F0xx/platform.mk
index f7464ff..1dff83a 100644
--- a/os/hal/ports/STM32/STM32F0xx/platform.mk
+++ b/os/hal/ports/STM32/STM32F0xx/platform.mk
@@ -1,6 +1,8 @@
 include ${CHIBIOS}/os/hal/ports/STM32/STM32F0xx/platform.mk
 
-PLATFORMSRC += ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD/CRCv1/crc_lld.c
+PLATFORMSRC += ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD/CRCv1/crc_lld.c \
+               ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD/TIMv1/timcap_lld.c \
 
 PLATFORMINC += ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD/CRCv1 \
+               ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD/TIMv1 \
                ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD
diff --git a/os/hal/ports/STM32/STM32F3xx/platform.mk b/os/hal/ports/STM32/STM32F3xx/platform.mk
new file mode 100644
index 0000000..e98de86
--- /dev/null
+++ b/os/hal/ports/STM32/STM32F3xx/platform.mk
@@ -0,0 +1,9 @@
+include ${CHIBIOS}/os/hal/ports/STM32/STM32F3xx/platform.mk
+
+PLATFORMSRC += ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD/CRCv1/crc_lld.c \
+               ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD/TIMv1/eicu_lld.c \
+               ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD/TIMv1/timcap_lld.c
+
+PLATFORMINC += ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD/CRCv1 \
+               ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD/TIMv1 \
+               ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD
diff --git a/os/hal/ports/STM32/STM32F4xx/platform.mk b/os/hal/ports/STM32/STM32F4xx/platform.mk
index eed262e..5ffc9dd 100644
--- a/os/hal/ports/STM32/STM32F4xx/platform.mk
+++ b/os/hal/ports/STM32/STM32F4xx/platform.mk
@@ -6,10 +6,15 @@ PLATFORMSRC += ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD/DMA2Dv1/stm32_dma2d.c \
                ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD/FSMCv1/fsmc_sram.c \

                ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD/LTDCv1/stm32_ltdc.c \

                ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD/TIMv1/eicu_lld.c \

-               ${CHIBIOS_CONTRIB}/os/hal/src/fsmc_sdram.c

+               ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD/TIMv1/timcap_lld.c \

+	           ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD/USBHv1/usbh_lld.c \

+               ${CHIBIOS_CONTRIB}/os/hal/src/fsmc_sdram.c \

+               ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD/CRCv1/crc_lld.c

 

 PLATFORMINC += ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD/DMA2Dv1 \

                ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD/FSMCv1 \

                ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD/LTDCv1 \

                ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD/TIMv1 \

+	           ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD/USBHv1 \

+	           ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD/CRCv1 \

                ${CHIBIOS_CONTRIB}/os/hal/ports/STM32/LLD

diff --git a/os/hal/src/ee24xx.c b/os/hal/src/ee24xx.c
new file mode 100644
index 0000000..c89635f
--- /dev/null
+++ b/os/hal/src/ee24xx.c
@@ -0,0 +1,353 @@
+/*
+  Copyright (c) 2013 Timon Wong
+
+  Permission is hereby granted, free of charge, to any person obtaining a copy
+  of this software and associated documentation files (the "Software"), to deal
+  in the Software without restriction, including without limitation the rights
+  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+  copies of the Software, and to permit persons to whom the Software is
+  furnished to do so, subject to the following conditions:
+
+  The above copyright notice and this permission notice shall be included in all
+  copies or substantial portions of the Software.
+
+  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+  THE SOFTWARE.
+*/
+
+/*
+  Copyright 2012 Uladzimir Pylinski aka barthess.
+  You may use this work without restrictions, as long as this notice is included.
+  The work is provided "as is" without warranty of any kind, neither express nor implied.
+*/
+
+/*****************************************************************************
+ * DATASHEET NOTES
+ *****************************************************************************
+Write cycle time (byte or page) - 5 ms
+
+Note:
+  Page write operations are limited to writing bytes within a single physical
+  page, regardless of the number of bytes actually being written. Physical page
+  boundaries start at addresses that are integer multiples of the page buffer
+  size (or page size and end at addresses that are integer multiples of
+  [page size]. If a Page Write command attempts to write across a physical
+  page boundary, the result is that the data wraps around to the beginning of
+  the current page (overwriting data previously stored there), instead of
+  being written to the next page as might be expected.
+*********************************************************************/
+
+#include "ee24xx.h"
+#include <string.h>
+
+#if (defined(HAL_USE_EEPROM) && HAL_USE_EEPROM && EEPROM_USE_EE24XX) || defined(__DOXYGEN__)
+
+/*
+ ******************************************************************************
+ * DEFINES
+ ******************************************************************************
+ */
+/*
+#if defined(SAM7_PLATFORM)
+#define EEPROM_I2C_CLOCK (MCK / (((i2cp->config->cwgr & 0xFF) + ((i2cp->config->cwgr >> 8) & 0xFF)) * (1 << ((i2cp->config->cwgr >> 16) & 7)) + 6))
+#else
+#define EEPROM_I2C_CLOCK (i2cp->config->clock_speed)
+#endif
+*/
+#define EEPROM_I2C_CLOCK 400000
+
+/*
+ ******************************************************************************
+ * EXTERNS
+ ******************************************************************************
+ */
+
+/*
+ ******************************************************************************
+ * GLOBAL VARIABLES
+ ******************************************************************************
+ */
+
+/*
+ *******************************************************************************
+ * LOCAL FUNCTIONS
+ *******************************************************************************
+ */
+/**
+ * @brief   Split one uint16_t address to two uint8_t.
+ *
+ * @param[in] txbuf pointer to driver transmit buffer
+ * @param[in] addr  uint16_t address
+ */
+#define eeprom_split_addr(txbuf, addr){                                       \
+    (txbuf)[0] = ((uint8_t)((addr >> 8) & 0xFF));                              \
+    (txbuf)[1] = ((uint8_t)(addr & 0xFF));                                     \
+  }
+
+/*
+ *******************************************************************************
+ * EXPORTED FUNCTIONS
+ *******************************************************************************
+ */
+
+/**
+ * @brief     Calculates requred timeout.
+ */
+static systime_t calc_timeout(I2CDriver *i2cp, size_t txbytes, size_t rxbytes) {
+  (void)i2cp;
+  const uint32_t bitsinbyte = 10;
+  uint32_t tmo;
+  tmo = ((txbytes + rxbytes + 1) * bitsinbyte * 1000);
+  tmo /= EEPROM_I2C_CLOCK;
+  tmo += 10; /* some additional milliseconds to be safer */
+  return MS2ST(tmo);
+}
+
+/**
+ * @brief   EEPROM read routine.
+ *
+ * @param[in] eepcfg    pointer to configuration structure of eeprom file
+ * @param[in] offset    addres of 1-st byte to be read
+ * @param[in] data      pointer to buffer with data to be written
+ * @param[in] len       number of bytes to be red
+ */
+static msg_t eeprom_read(const I2CEepromFileConfig *eepcfg,
+                         uint32_t offset, uint8_t *data, size_t len) {
+
+  msg_t status = MSG_RESET;
+  systime_t tmo = calc_timeout(eepcfg->i2cp, 2, len);
+
+  osalDbgAssert(((len <= eepcfg->size) && ((offset + len) <= eepcfg->size)),
+             "out of device bounds");
+
+  eeprom_split_addr(eepcfg->write_buf, (offset + eepcfg->barrier_low));
+
+#if I2C_USE_MUTUAL_EXCLUSION
+  i2cAcquireBus(eepcfg->i2cp);
+#endif
+
+  status = i2cMasterTransmitTimeout(eepcfg->i2cp, eepcfg->addr,
+                                    eepcfg->write_buf, 2, data, len, tmo);
+
+#if I2C_USE_MUTUAL_EXCLUSION
+  i2cReleaseBus(eepcfg->i2cp);
+#endif
+
+  return status;
+}
+
+/**
+ * @brief   EEPROM write routine.
+ * @details Function writes data to EEPROM.
+ * @pre     Data must be fit to single EEPROM page.
+ *
+ * @param[in] eepcfg  pointer to configuration structure of eeprom file
+ * @param[in] offset  addres of 1-st byte to be write
+ * @param[in] data    pointer to buffer with data to be written
+ * @param[in] len     number of bytes to be written
+ */
+static msg_t eeprom_write(const I2CEepromFileConfig *eepcfg, uint32_t offset,
+                          const uint8_t *data, size_t len) {
+  msg_t status = MSG_RESET;
+  systime_t tmo = calc_timeout(eepcfg->i2cp, (len + 2), 0);
+
+  osalDbgAssert(((len <= eepcfg->size) && ((offset + len) <= eepcfg->size)),
+             "out of device bounds");
+  osalDbgAssert((((offset + eepcfg->barrier_low) / eepcfg->pagesize) ==
+              (((offset + eepcfg->barrier_low) + len - 1) / eepcfg->pagesize)),
+             "data can not be fitted in single page");
+
+  /* write address bytes */
+  eeprom_split_addr(eepcfg->write_buf, (offset + eepcfg->barrier_low));
+  /* write data bytes */
+  memcpy(&(eepcfg->write_buf[2]), data, len);
+
+#if I2C_USE_MUTUAL_EXCLUSION
+  i2cAcquireBus(eepcfg->i2cp);
+#endif
+
+  status = i2cMasterTransmitTimeout(eepcfg->i2cp, eepcfg->addr,
+                                    eepcfg->write_buf, (len + 2), NULL, 0, tmo);
+
+#if I2C_USE_MUTUAL_EXCLUSION
+  i2cReleaseBus(eepcfg->i2cp);
+#endif
+
+  /* wait until EEPROM process data */
+  chThdSleep(eepcfg->write_time);
+
+  return status;
+}
+
+/**
+ * @brief   Determines and returns size of data that can be processed
+ */
+static size_t __clamp_size(void *ip, size_t n) {
+
+  if (((size_t)eepfs_getposition(ip) + n) > (size_t)eepfs_getsize(ip))
+    return eepfs_getsize(ip) - eepfs_getposition(ip);
+  else
+    return n;
+}
+
+/**
+ * @brief   Write data that can be fitted in one page boundary
+ */
+static void __fitted_write(void *ip, const uint8_t *data, size_t len, uint32_t *written) {
+
+  msg_t status = MSG_RESET;
+
+  osalDbgAssert(len != 0, "something broken in hi level part");
+
+  status = eeprom_write(((I2CEepromFileStream *)ip)->cfg,
+                        eepfs_getposition(ip), data, len);
+  if (status == MSG_OK) {
+    *written += len;
+    eepfs_lseek(ip, eepfs_getposition(ip) + len);
+  }
+}
+
+/**
+ * @brief     Write data to EEPROM.
+ * @details   Only one EEPROM page can be written at once. So fucntion
+ *            splits large data chunks in small EEPROM transactions if needed.
+ * @note      To achieve the maximum effectivity use write operations
+ *            aligned to EEPROM page boundaries.
+ */
+static size_t write(void *ip, const uint8_t *bp, size_t n) {
+
+  size_t   len = 0;     /* bytes to be written at one trasaction */
+  uint32_t written; /* total bytes successfully written */
+  uint16_t pagesize;
+  uint32_t firstpage;
+  uint32_t lastpage;
+
+  osalDbgCheck((ip != NULL) && (((EepromFileStream *)ip)->vmt != NULL));
+
+  if (n == 0)
+    return 0;
+
+  n = __clamp_size(ip, n);
+  if (n == 0)
+    return 0;
+
+  pagesize  =  ((EepromFileStream *)ip)->cfg->pagesize;
+  firstpage = (((EepromFileStream *)ip)->cfg->barrier_low +
+               eepfs_getposition(ip)) / pagesize;
+  lastpage  = (((EepromFileStream *)ip)->cfg->barrier_low +
+               eepfs_getposition(ip) + n - 1) / pagesize;
+
+  written = 0;
+  /* data fitted in single page */
+  if (firstpage == lastpage) {
+    len = n;
+    __fitted_write(ip, bp, len, &written);
+    bp += len;
+    return written;
+  }
+
+  else {
+    /* write first piece of data to first page boundary */
+    len =  ((firstpage + 1) * pagesize) - eepfs_getposition(ip);
+    len -= ((EepromFileStream *)ip)->cfg->barrier_low;
+    __fitted_write(ip, bp, len, &written);
+    bp += len;
+
+    /* now writes blocks at a size of pages (may be no one) */
+    while ((n - written) > pagesize) {
+      len = pagesize;
+      __fitted_write(ip, bp, len, &written);
+      bp += len;
+    }
+
+    /* wrtie tail */
+    len = n - written;
+    if (len == 0)
+      return written;
+    else {
+      __fitted_write(ip, bp, len, &written);
+    }
+  }
+
+  return written;
+}
+
+/**
+ * Read some bytes from current position in file. After successful
+ * read operation the position pointer will be increased by the number
+ * of read bytes.
+ */
+static size_t read(void *ip, uint8_t *bp, size_t n) {
+  msg_t status = MSG_OK;
+
+  osalDbgCheck((ip != NULL) && (((EepromFileStream *)ip)->vmt != NULL));
+
+  if (n == 0)
+    return 0;
+
+  n = __clamp_size(ip, n);
+  if (n == 0)
+    return 0;
+
+  /* Stupid I2C cell in STM32F1x does not allow to read single byte.
+     So we must read 2 bytes and return needed one. */
+#if defined(STM32F1XX_I2C)
+  if (n == 1) {
+    uint8_t __buf[2];
+    /* if NOT last byte of file requested */
+    if ((eepfs_getposition(ip) + 1) < eepfs_getsize(ip)) {
+      if (read(ip, __buf, 2) == 2) {
+        eepfs_lseek(ip, (eepfs_getposition(ip) + 1));
+        bp[0] = __buf[0];
+        return 1;
+      }
+      else
+        return 0;
+    }
+    else {
+      eepfs_lseek(ip, (eepfs_getposition(ip) - 1));
+      if (read(ip, __buf, 2) == 2) {
+        eepfs_lseek(ip, (eepfs_getposition(ip) + 2));
+        bp[0] = __buf[1];
+        return 1;
+      }
+      else
+        return 0;
+    }
+  }
+#endif /* defined(STM32F1XX_I2C) */
+
+  /* call low level function */
+  status  = eeprom_read(((I2CEepromFileStream *)ip)->cfg,
+                        eepfs_getposition(ip), bp, n);
+  if (status != MSG_OK)
+    return 0;
+  else {
+    eepfs_lseek(ip, (eepfs_getposition(ip) + n));
+    return n;
+  }
+}
+
+static const struct EepromFileStreamVMT vmt = {
+  write,
+  read,
+  eepfs_put,
+  eepfs_get,
+  eepfs_close,
+  eepfs_geterror,
+  eepfs_getsize,
+  eepfs_getposition,
+  eepfs_lseek,
+};
+
+EepromDevice eepdev_24xx = {
+  EEPROM_DEV_24XX,
+  &vmt
+};
+
+#endif /* EEPROM_USE_EE24XX */
diff --git a/os/hal/src/ee25xx.c b/os/hal/src/ee25xx.c
new file mode 100644
index 0000000..07f4373
--- /dev/null
+++ b/os/hal/src/ee25xx.c
@@ -0,0 +1,404 @@
+/*
+  Copyright (c) 2013 Timon Wong
+
+  Permission is hereby granted, free of charge, to any person obtaining a copy
+  of this software and associated documentation files (the "Software"), to deal
+  in the Software without restriction, including without limitation the rights
+  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+  copies of the Software, and to permit persons to whom the Software is
+  furnished to do so, subject to the following conditions:
+
+  The above copyright notice and this permission notice shall be included in all
+  copies or substantial portions of the Software.
+
+  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+  THE SOFTWARE.
+*/
+
+/*
+  Copyright 2012 Uladzimir Pylinski aka barthess.
+  You may use this work without restrictions, as long as this notice is included.
+  The work is provided "as is" without warranty of any kind, neither express nor implied.
+*/
+
+/*****************************************************************************
+ * DATASHEET NOTES
+ *****************************************************************************
+Write cycle time (byte or page) - 5 ms
+
+Note:
+  Page write operations are limited to writing bytes within a single physical
+  page, regardless of the number of bytes actually being written. Physical page
+  boundaries start at addresses that are integer multiples of the page buffer
+  size (or page size and end at addresses that are integer multiples of
+  [page size]. If a Page Write command attempts to write across a physical
+  page boundary, the result is that the data wraps around to the beginning of
+  the current page (overwriting data previously stored there), instead of
+  being written to the next page as might be expected.
+*********************************************************************/
+
+#include "ee25xx.h"
+#include <string.h>
+
+#if (defined(HAL_USE_EEPROM) && HAL_USE_EEPROM && EEPROM_USE_EE25XX) || defined(__DOXYGEN__)
+
+/**
+ * @name Commands of 25XX chip.
+ * @{
+ */
+#define CMD_READ    0x03  /**< @brief Read data from memory array beginning at
+                               selected address. */
+#define CMD_WRITE   0x02  /**< @brief Write data to memory array beginning at
+                               selected address. */
+#define CMD_WRDI    0x04  /**< Reset the write enable latch (disable write
+                               operations). */
+#define CMD_WREN    0x06  /**< Set the write enable latch (enable write
+                               operations). */
+#define CMD_RDSR    0x05  /**< Read STATUS register. */
+#define CMD_WRSR    0x01  /**< Write STATUS register. */
+
+/** @} */
+
+/**
+ * @name Status of 25XX chip.
+ * @{}
+ */
+#define STAT_BP1    0x08  /**< @brief Block protection (high). */
+#define STAT_BP0    0x04  /**< @brief Block protection (low). */
+#define STAT_WEL    0x02  /**< @brief Write enable latch. */
+#define STAT_WIP    0x01  /**< @brief Write-In-Progress. */
+
+/** @} */
+
+/**
+ * @brief 25XX low level write then read rountine.
+ *
+ * @param[in]  eepcfg pointer to configuration structure of eeprom file.
+ * @param[in]  txbuf  pointer to buffer to be transfered.
+ * @param[in]  txlen  number of bytes to be transfered.
+ * @param[out] rxbuf  pointer to buffer to be received.
+ * @param[in]  rxlen  number of bytes to be received.
+ */
+static void ll_25xx_transmit_receive(const SPIEepromFileConfig *eepcfg,
+                                     const uint8_t *txbuf, size_t txlen,
+                                     uint8_t *rxbuf, size_t rxlen) {
+
+#if SPI_USE_MUTUAL_EXCLUSION
+  spiAcquireBus(eepcfg->spip);
+#endif
+  spiSelect(eepcfg->spip);
+  spiSend(eepcfg->spip, txlen, txbuf);
+  if (rxlen) /* Check if receive is needed. */
+      spiReceive(eepcfg->spip, rxlen, rxbuf);
+  spiUnselect(eepcfg->spip);
+
+#if SPI_USE_MUTUAL_EXCLUSION
+  spiReleaseBus(eepcfg->spip);
+#endif
+}
+
+/**
+ * @brief Check whether the device is busy (writing in progress).
+ *
+ * @param[in] eepcfg   pointer to configuration structure of eeprom file.
+ * @return @p true on busy.
+ */
+static bool ll_eeprom_is_busy(const SPIEepromFileConfig *eepcfg) {
+
+  uint8_t cmd = CMD_RDSR;
+  uint8_t stat;
+  ll_25xx_transmit_receive(eepcfg, &cmd, 1, &stat, 1);
+  if (stat & STAT_WIP)
+    return TRUE;
+  return FALSE;
+}
+
+/**
+ * @brief Lock device.
+ *
+ * @param[in] eepcfg  pointer to configuration structure of eeprom file.
+ */
+static void ll_eeprom_lock(const SPIEepromFileConfig *eepcfg) {
+
+  uint8_t cmd = CMD_WRDI;
+  ll_25xx_transmit_receive(eepcfg, &cmd, 1, NULL, 0);
+}
+
+/**
+ * @brief Unlock device.
+ *
+ * @param[in] eepcfg  pointer to configuration structure of eeprom file.
+ */
+static void ll_eeprom_unlock(const SPIEepromFileConfig *eepcfg) {
+
+  uint8_t cmd = CMD_WREN;
+  ll_25xx_transmit_receive(eepcfg, &cmd, 1, NULL, 0);
+}
+
+/**
+ * @brief   Prepare byte sequence for command and address
+ *
+ * @param[in] seq   pointer to first 3byte sequence
+ * @param[in] size  size of the eeprom device
+ * @param[in] cmd   command
+ * @param[in] addr  address
+ * @return number of bytes of this sequence
+ */
+static uint8_t ll_eeprom_prepare_seq(uint8_t *seq, uint32_t size, uint8_t cmd,
+                                     uint32_t addr) {
+
+  seq[0] = ((uint8_t)cmd & 0xff);
+
+  if (size > 0xffffUL) {
+    /* High density, 24bit address. */
+    seq[1] = (uint8_t)((addr >> 16) & 0xff);
+    seq[2] = (uint8_t)((addr >> 8) & 0xff);
+    seq[3] = (uint8_t)(addr & 0xff);
+    return 4;
+  }
+  else if (size > 0x00ffUL) {
+    /* Medium density, 16bit address. */
+    seq[1] = (uint8_t)((addr >> 8) & 0xff);
+    seq[2] = (uint8_t)(addr & 0xff);
+    return 3;
+  }
+
+  /* Low density, 8bit address. */
+  seq[1] = (uint8_t)(addr & 0xff);
+  return 2;
+}
+
+/**
+ * @brief   EEPROM read routine.
+ *
+ * @param[in]  eepcfg   pointer to configuration structure of eeprom file.
+ * @param[in]  offset   addres of 1-st byte to be read.
+ * @param[out] data     pointer to buffer with data to be written.
+ * @param[in]  len      number of bytes to be red.
+ */
+static msg_t ll_eeprom_read(const SPIEepromFileConfig *eepcfg, uint32_t offset,
+                            uint8_t *data, size_t len) {
+
+  uint8_t txbuff[4];
+  uint8_t txlen;
+
+  osalDbgAssert(((len <= eepcfg->size) && ((offset + len) <= eepcfg->size)),
+             "out of device bounds");
+
+  if (eepcfg->spip->state != SPI_READY)
+      return MSG_RESET;
+
+  txlen = ll_eeprom_prepare_seq(txbuff, eepcfg->size, CMD_READ,
+                                (offset + eepcfg->barrier_low));
+  ll_25xx_transmit_receive(eepcfg, txbuff, txlen, data, len);
+
+  return MSG_OK;
+}
+
+/**
+ * @brief   EEPROM write routine.
+ * @details Function writes data to EEPROM.
+ * @pre     Data must be fit to single EEPROM page.
+ *
+ * @param[in] eepcfg  pointer to configuration structure of eeprom file.
+ * @param[in] offset  addres of 1-st byte to be writen.
+ * @param[in] data    pointer to buffer with data to be written.
+ * @param[in] len     number of bytes to be written.
+ */
+static msg_t ll_eeprom_write(const SPIEepromFileConfig *eepcfg, uint32_t offset,
+                             const uint8_t *data, size_t len) {
+
+  uint8_t txbuff[4];
+  uint8_t txlen;
+  systime_t now;
+
+  osalDbgAssert(((len <= eepcfg->size) && ((offset + len) <= eepcfg->size)),
+             "out of device bounds");
+  osalDbgAssert((((offset + eepcfg->barrier_low) / eepcfg->pagesize) ==
+              (((offset + eepcfg->barrier_low) + len - 1) / eepcfg->pagesize)),
+             "data can not be fitted in single page");
+
+  if (eepcfg->spip->state != SPI_READY)
+      return MSG_RESET;
+
+  /* Unlock array for writting. */
+  ll_eeprom_unlock(eepcfg);
+
+#if SPI_USE_MUTUAL_EXCLUSION
+  spiAcquireBus(eepcfg->spip);
+#endif
+
+  spiSelect(eepcfg->spip);
+  txlen = ll_eeprom_prepare_seq(txbuff, eepcfg->size, CMD_WRITE,
+                                (offset + eepcfg->barrier_low));
+  spiSend(eepcfg->spip, txlen, txbuff);
+  spiSend(eepcfg->spip, len, data);
+  spiUnselect(eepcfg->spip);
+
+#if SPI_USE_MUTUAL_EXCLUSION
+  spiReleaseBus(eepcfg->spip);
+#endif
+
+  /* Wait until EEPROM process data. */
+  now = chVTGetSystemTimeX();
+  while (ll_eeprom_is_busy(eepcfg)) {
+    if ((chVTGetSystemTimeX() - now) > eepcfg->write_time) {
+      return MSG_TIMEOUT;
+    }
+
+    chThdYield();
+  }
+
+  /* Lock array preventing unexpected access */
+  ll_eeprom_lock(eepcfg);
+  return MSG_OK;
+}
+
+/**
+ * @brief   Determines and returns size of data that can be processed
+ */
+static size_t __clamp_size(void *ip, size_t n) {
+
+  if (((size_t)eepfs_getposition(ip) + n) > (size_t)eepfs_getsize(ip))
+    return eepfs_getsize(ip) - eepfs_getposition(ip);
+  else
+    return n;
+}
+
+/**
+ * @brief   Write data that can be fitted in one page boundary
+ */
+static msg_t __fitted_write(void *ip, const uint8_t *data, size_t len, uint32_t *written) {
+
+  msg_t status = MSG_RESET;
+
+  osalDbgAssert(len != 0, "something broken in hi level part");
+
+  status = ll_eeprom_write(((SPIEepromFileStream *)ip)->cfg,
+                           eepfs_getposition(ip), data, len);
+  if (status == MSG_OK) {
+    *written += len;
+    eepfs_lseek(ip, eepfs_getposition(ip) + len);
+  }
+  return status;
+}
+
+/**
+ * @brief     Write data to EEPROM.
+ * @details   Only one EEPROM page can be written at once. So fucntion
+ *            splits large data chunks in small EEPROM transactions if needed.
+ * @note      To achieve the maximum effectivity use write operations
+ *            aligned to EEPROM page boundaries.
+ */
+static size_t write(void *ip, const uint8_t *bp, size_t n) {
+
+  size_t   len = 0;     /* bytes to be written at one trasaction */
+  uint32_t written; /* total bytes successfully written */
+  uint16_t pagesize;
+  uint32_t firstpage;
+  uint32_t lastpage;
+
+  volatile const SPIEepromFileConfig *cfg = ((SPIEepromFileStream *)ip)->cfg;
+
+  osalDbgCheck((ip != NULL) && (((SPIEepromFileStream *)ip)->vmt != NULL));
+
+  if (n == 0)
+    return 0;
+
+  n = __clamp_size(ip, n);
+  if (n == 0)
+    return 0;
+
+  pagesize  = cfg->pagesize;
+  firstpage = (cfg->barrier_low + eepfs_getposition(ip)) / pagesize;
+  lastpage  = ((cfg->barrier_low + eepfs_getposition(ip) + n) - 1) / pagesize;
+
+  written = 0;
+  /* data fitted in single page */
+  if (firstpage == lastpage) {
+    len = n;
+    __fitted_write(ip, bp, len, &written);
+    bp += len;
+    return written;
+  }
+  else {
+    /* write first piece of data to first page boundary */
+    len =  ((firstpage + 1) * pagesize) - eepfs_getposition(ip);
+    len -= cfg->barrier_low;
+    __fitted_write(ip, bp, len, &written);
+    bp += len;
+
+    /* now writes blocks at a size of pages (may be no one) */
+    while ((n - written) > pagesize) {
+      len = pagesize;
+      if (__fitted_write(ip, bp, len, &written) != MSG_OK) // Fixed: Would increase bp forever and crash in case of timeouts...
+        return written;
+
+      bp += len;
+    }
+
+
+    /* wrtie tail */
+    len = n - written;
+    if (len == 0)
+      return written;
+    else {
+      __fitted_write(ip, bp, len, &written);
+    }
+  }
+
+  return written;
+}
+
+/**
+ * Read some bytes from current position in file. After successful
+ * read operation the position pointer will be increased by the number
+ * of read bytes.
+ */
+static size_t read(void *ip, uint8_t *bp, size_t n) {
+
+  msg_t status = MSG_OK;
+
+  osalDbgCheck((ip != NULL) && (((EepromFileStream *)ip)->vmt != NULL));
+
+  if (n == 0)
+    return 0;
+
+  n = __clamp_size(ip, n);
+  if (n == 0)
+    return 0;
+
+  /* call low level function */
+  status = ll_eeprom_read(((SPIEepromFileStream *)ip)->cfg,
+                          eepfs_getposition(ip), bp, n);
+  if (status != MSG_OK)
+    return 0;
+  else {
+    eepfs_lseek(ip, (eepfs_getposition(ip) + n));
+    return n;
+  }
+}
+
+static const struct EepromFileStreamVMT vmt = {
+  write,
+  read,
+  eepfs_put,
+  eepfs_get,
+  eepfs_close,
+  eepfs_geterror,
+  eepfs_getsize,
+  eepfs_getposition,
+  eepfs_lseek,
+};
+
+EepromDevice eepdev_25xx = {
+  EEPROM_DEV_25XX,
+  &vmt
+};
+
+#endif /* EEPROM_USE_EE25XX */
diff --git a/os/hal/src/eeprom.c b/os/hal/src/eeprom.c
new file mode 100644
index 0000000..60d90ed
--- /dev/null
+++ b/os/hal/src/eeprom.c
@@ -0,0 +1,197 @@
+/*
+  Copyright (c) 2013 Timon Wong
+
+  Permission is hereby granted, free of charge, to any person obtaining a copy
+  of this software and associated documentation files (the "Software"), to deal
+  in the Software without restriction, including without limitation the rights
+  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+  copies of the Software, and to permit persons to whom the Software is
+  furnished to do so, subject to the following conditions:
+
+  The above copyright notice and this permission notice shall be included in all
+  copies or substantial portions of the Software.
+
+  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+  THE SOFTWARE.
+*/
+
+/*
+  Copyright 2012 Uladzimir Pylinski aka barthess.
+  You may use this work without restrictions, as long as this notice is included.
+  The work is provided "as is" without warranty of any kind, neither express nor implied.
+*/
+
+#include "eeprom.h"
+#include <string.h>
+
+#if defined(HAL_USE_EEPROM) && HAL_USE_EEPROM
+
+extern EepromDevice eepdev_24xx;
+extern EepromDevice eepdev_25xx;
+
+EepromDevice *__eeprom_drv_table[] = {
+  /* I2C related. */
+#if HAL_USE_I2C
+
+# if EEPROM_USE_EE24XX
+  &eepdev_24xx,
+# endif
+
+#endif /* HAL_USE_I2C */
+
+  /* SPI related. */
+#if HAL_USE_SPI
+
+# if EEPROM_USE_EE25XX
+  &eepdev_25xx,
+# endif
+
+#endif /* HAL_USE_SPI */
+};
+
+
+/**
+ * @breif  Find low level EEPROM device by id.
+ */
+const EepromDevice *EepromFindDevice(uint8_t id) {
+
+  uint8_t i;
+  const EepromDevice *drv;
+
+  for (i = 0; i < EEPROM_TABLE_SIZE; i++) {
+    drv = __eeprom_drv_table[i];
+    if (drv->id == id) {
+      return drv;
+    }
+  }
+
+  return NULL;
+}
+
+/**
+ * Open EEPROM IC as file and return pointer to the file stream object
+ * @note      Fucntion allways successfully open file. All checking makes
+ *            in read/write functions.
+ */
+EepromFileStream *EepromFileOpen(EepromFileStream *efs,
+                                 const EepromFileConfig *eepcfg,
+                                 const EepromDevice *eepdev) {
+
+  osalDbgAssert((efs != NULL) && (eepcfg != NULL) && (eepdev != NULL) &&
+             (eepdev->efsvmt != NULL), "EepromFileOpen");
+  osalDbgAssert(efs->vmt != eepdev->efsvmt, "File allready opened");
+  osalDbgAssert(eepcfg->barrier_hi > eepcfg->barrier_low, "Low barrier exceeds High barrier");
+  osalDbgAssert(eepcfg->pagesize < eepcfg->size, "Pagesize cannot be lager than EEPROM size");
+  osalDbgAssert(eepcfg->barrier_hi <= eepcfg->size, "Barrier exceeds EEPROM size");
+
+  efs->vmt      = eepdev->efsvmt;
+  efs->cfg      = eepcfg;
+  efs->errors   = FILE_OK;
+  efs->position = 0;
+  return (EepromFileStream *)efs;
+}
+
+uint8_t EepromReadByte(EepromFileStream *efs) {
+
+  uint8_t buf;
+  fileStreamRead(efs, &buf, sizeof(buf));
+  return buf;
+}
+
+uint16_t EepromReadHalfword(EepromFileStream *efs) {
+
+  uint16_t buf;
+  fileStreamRead(efs, (uint8_t *)&buf, sizeof(buf));
+  return buf;
+}
+
+uint32_t EepromReadWord(EepromFileStream *efs) {
+
+  uint32_t buf;
+  fileStreamRead(efs, (uint8_t *)&buf, sizeof(buf));
+  return buf;
+}
+
+size_t EepromWriteByte(EepromFileStream *efs, uint8_t data) {
+
+  return fileStreamWrite(efs, &data, sizeof(data));
+}
+
+size_t EepromWriteHalfword(EepromFileStream *efs, uint16_t data) {
+
+  return fileStreamWrite(efs, (uint8_t *)&data, sizeof(data));
+}
+
+size_t EepromWriteWord(EepromFileStream *efs, uint32_t data) {
+
+  return fileStreamWrite(efs, (uint8_t *)&data, sizeof(data));
+}
+
+msg_t eepfs_getsize(void *ip) {
+
+  uint32_t h, l;
+
+  osalDbgCheck((ip != NULL) && (((EepromFileStream *)ip)->vmt != NULL) &&
+             (((EepromFileStream *)ip)->cfg != NULL));
+
+  h = ((EepromFileStream *)ip)->cfg->barrier_hi;
+  l = ((EepromFileStream *)ip)->cfg->barrier_low;
+  return  h - l;
+}
+
+msg_t eepfs_getposition(void *ip) {
+
+  osalDbgCheck((ip != NULL) && (((EepromFileStream *)ip)->vmt != NULL));
+
+  return ((EepromFileStream *)ip)->position;
+}
+
+msg_t eepfs_lseek(void *ip, fileoffset_t offset) {
+
+  uint32_t size;
+
+  osalDbgCheck((ip != NULL) && (((EepromFileStream *)ip)->vmt != NULL));
+
+  size = eepfs_getsize(ip);
+  if (offset > size)
+    offset = size;
+  ((EepromFileStream *)ip)->position = offset;
+  return offset;
+}
+
+msg_t eepfs_close(void *ip) {
+
+  osalDbgCheck((ip != NULL) && (((EepromFileStream *)ip)->vmt != NULL));
+
+  ((EepromFileStream *)ip)->errors   = FILE_OK;
+  ((EepromFileStream *)ip)->position = 0;
+  ((EepromFileStream *)ip)->vmt      = NULL;
+  ((EepromFileStream *)ip)->cfg      = NULL;
+  return FILE_OK;
+}
+
+msg_t eepfs_geterror(void *ip) {
+
+  osalDbgCheck((ip != NULL) && (((EepromFileStream *)ip)->vmt != NULL));
+  return ((EepromFileStream *)ip)->errors;
+}
+
+msg_t eepfs_put(void *ip, uint8_t b) {
+
+  (void)ip;
+  (void)b;
+  return 0;
+}
+
+msg_t eepfs_get(void *ip) {
+
+  (void)ip;
+  return 0;
+}
+
+#endif /* #if defined(HAL_USE_EEPROM) && HAL_USE_EEPROM */
diff --git a/os/hal/src/hal_community.c b/os/hal/src/hal_community.c
index 210df69..4c48113 100644
--- a/os/hal/src/hal_community.c
+++ b/os/hal/src/hal_community.c
@@ -68,6 +68,14 @@ void halCommunityInit(void) {
 #if HAL_USE_RNG || defined(__DOXYGEN__)

   rngInit();

 #endif

+

+#if HAL_USE_USBH || defined(__DOXYGEN__)

+  usbhInit();

+#endif

+

+#if HAL_USE_TIMCAP || defined(__DOXYGEN__)

+  timcapInit();

+#endif

 }

 

 #endif /* HAL_USE_COMMUNITY */

diff --git a/os/hal/src/timcap.c b/os/hal/src/timcap.c
new file mode 100644
index 0000000..6ee97ad
--- /dev/null
+++ b/os/hal/src/timcap.c
@@ -0,0 +1,159 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010,
+                 2011,2012,2013 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS/RT is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @file    timcap.c
+ * @brief   TIMCAP Driver code.
+ *
+ * @addtogroup TIMCAP
+ * @{
+ */
+
+#include "timcap.h"
+
+#if HAL_USE_TIMCAP || defined(__DOXYGEN__)
+
+
+/*===========================================================================*/
+/* Driver local definitions.                                                 */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported variables.                                                */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local variables and types.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local functions.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported functions.                                                */
+/*===========================================================================*/
+
+/**
+ * @brief   TIMCAP Driver initialization.
+ * @note    This function is implicitly invoked by @p halInit(), there is
+ *          no need to explicitly initialize the driver.
+ *
+ * @init
+ */
+void timcapInit(void) {
+
+  timcap_lld_init();
+}
+
+/**
+ * @brief   Initializes the standard part of a @p TIMCAPDriver structure.
+ *
+ * @param[out] timcapp     pointer to the @p TIMCAPDriver object
+ *
+ * @init
+ */
+void timcapObjectInit(TIMCAPDriver *timcapp) {
+
+  timcapp->state  = TIMCAP_STOP;
+  timcapp->config = NULL;
+}
+
+/**
+ * @brief   Configures and activates the TIMCAP peripheral.
+ *
+ * @param[in] timcapp      pointer to the @p TIMCAPDriver object
+ * @param[in] config    pointer to the @p TIMCAPConfig object
+ *
+ * @api
+ */
+void timcapStart(TIMCAPDriver *timcapp, const TIMCAPConfig *config) {
+
+  osalDbgCheck((timcapp != NULL) && (config != NULL));
+
+  osalSysLock();
+  osalDbgAssert((timcapp->state == TIMCAP_STOP) || (timcapp->state == TIMCAP_READY),
+              "invalid state");
+  timcapp->config = config;
+  timcap_lld_start(timcapp);
+  timcapp->state = TIMCAP_READY;
+  osalSysUnlock();
+}
+
+/**
+ * @brief   Deactivates the TIMCAP peripheral.
+ *
+ * @param[in] timcapp      pointer to the @p TIMCAPDriver object
+ *
+ * @api
+ */
+void timcapStop(TIMCAPDriver *timcapp) {
+
+  osalDbgCheck(timcapp != NULL);
+
+  osalSysLock();
+  osalDbgAssert((timcapp->state == TIMCAP_STOP) || (timcapp->state == TIMCAP_READY),
+              "invalid state");
+  timcap_lld_stop(timcapp);
+  timcapp->state = TIMCAP_STOP;
+  osalSysUnlock();
+}
+
+/**
+ * @brief   Enables the input capture.
+ *
+ * @param[in] timcapp      pointer to the @p TIMCAPDriver object
+ *
+ * @api
+ */
+void timcapEnable(TIMCAPDriver *timcapp) {
+
+  osalDbgCheck(timcapp != NULL);
+
+  osalSysLock();
+  osalDbgAssert(timcapp->state == TIMCAP_READY, "invalid state");
+  timcap_lld_enable(timcapp);
+  timcapp->state = TIMCAP_WAITING;
+  osalSysUnlock();
+}
+
+/**
+ * @brief   Disables the input capture.
+ *
+ * @param[in] timcapp      pointer to the @p TIMCAPDriver object
+ *
+ * @api
+ */
+void timcapDisable(TIMCAPDriver *timcapp) {
+
+  osalDbgCheck(timcapp != NULL);
+
+  osalSysLock();
+  osalDbgAssert((timcapp->state == TIMCAP_READY) || (timcapp->state == TIMCAP_WAITING) ||
+              (timcapp->state == TIMCAP_ACTIVE) || (timcapp->state == TIMCAP_IDLE),
+             "invalid state");
+  timcap_lld_disable(timcapp);
+  timcapp->state = TIMCAP_READY;
+  osalSysUnlock();
+}
+
+#endif /* HAL_USE_TIMCAP */
+
+/** @} */
diff --git a/os/hal/src/usbh.c b/os/hal/src/usbh.c
new file mode 100644
index 0000000..3bdbac3
--- /dev/null
+++ b/os/hal/src/usbh.c
@@ -0,0 +1,1395 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

+

+    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.

+*/

+

+#include "hal.h"

+

+#if HAL_USE_USBH

+

+#include "usbh/dev/hub.h"

+#include "usbh/internal.h"

+#include <string.h>

+

+#if USBH_DEBUG_ENABLE_TRACE

+#define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define udbgf(f, ...)  do {} while(0)

+#define udbg(f, ...)   do {} while(0)

+#endif

+

+#if USBH_DEBUG_ENABLE_INFO

+#define uinfof(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define uinfo(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define uinfof(f, ...)  do {} while(0)

+#define uinfo(f, ...)   do {} while(0)

+#endif

+

+#if USBH_DEBUG_ENABLE_WARNINGS

+#define uwarnf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define uwarn(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define uwarnf(f, ...)  do {} while(0)

+#define uwarn(f, ...)   do {} while(0)

+#endif

+

+#if USBH_DEBUG_ENABLE_ERRORS

+#define uerrf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define uerr(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define uerrf(f, ...)  do {} while(0)

+#define uerr(f, ...)   do {} while(0)

+#endif

+

+#if STM32_USBH_USE_OTG1

+USBHDriver USBHD1;

+#endif

+#if STM32_USBH_USE_OTG2

+USBHDriver USBHD2;

+#endif

+

+

+static void _classdriver_process_device(usbh_device_t *dev);

+static bool _classdriver_load(usbh_device_t *dev, uint8_t class,

+		uint8_t subclass, uint8_t protocol, uint8_t *descbuff, uint16_t rem);

+

+

+/*===========================================================================*/

+/* Checks.								                                     */

+/*===========================================================================*/

+

+static inline void _check_dev(usbh_device_t *dev) {

+	osalDbgCheck(dev);

+	//TODO: add more checks.

+}

+

+static inline void _check_ep(usbh_ep_t *ep) {

+	osalDbgCheck(ep != 0);

+	_check_dev(ep->device);

+	osalDbgCheck(ep->type <= 3);

+	//TODO: add more checks.

+}

+

+static inline void _check_urb(usbh_urb_t *urb) {

+	osalDbgCheck(urb != 0);

+	_check_ep(urb->ep);

+	osalDbgCheck((urb->buff != NULL) || (urb->requestedLength == 0));

+	//TODO: add more checks.

+}

+

+/*===========================================================================*/

+/* Main driver API.						                                     */

+/*===========================================================================*/

+

+void usbhObjectInit(USBHDriver *usbh) {

+	memset(usbh, 0, sizeof(*usbh));

+	usbh->status = USBH_STATUS_STOPPED;

+#if HAL_USBH_USE_HUB

+	INIT_LIST_HEAD(&usbh->hubs);

+	_usbhub_port_object_init(&usbh->rootport, usbh, 0, 1);

+#else

+	_usbhub_port_object_init(&usbh->rootport, usbh, 1);

+#endif

+}

+

+void usbhInit(void) {

+#if HAL_USBH_USE_HUB

+	uint8_t i;

+	for (i = 0; i < HAL_USBHHUB_MAX_INSTANCES; i++) {

+		usbhhubObjectInit(&USBHHUBD[i]);

+	}

+#endif

+	usbh_lld_init();

+}

+

+void usbhStart(USBHDriver *usbh) {

+	usbDbgInit(usbh);

+

+	osalSysLock();

+	osalDbgAssert((usbh->status == USBH_STATUS_STOPPED) || (usbh->status == USBH_STATUS_STARTED),

+				"invalid state");

+	usbh_lld_start(usbh);

+	usbh->status = USBH_STATUS_STARTED;

+	osalOsRescheduleS();

+	osalSysUnlock();

+}

+

+

+void usbhStop(USBHDriver *usbh) {

+	//TODO: implement

+	(void)usbh;

+}

+void usbhSuspend(USBHDriver *usbh) {

+	//TODO: implement

+	(void)usbh;

+}

+void usbhResume(USBHDriver *usbh) {

+	//TODO: implement

+	(void)usbh;

+}

+

+/*===========================================================================*/

+/* Endpoint API.						                                     */

+/*===========================================================================*/

+

+void usbhEPObjectInit(usbh_ep_t *ep, usbh_device_t *dev, const usbh_endpoint_descriptor_t *desc) {

+	osalDbgCheck(ep);

+	_check_dev(dev);

+	osalDbgCheck(desc);

+

+	memset(ep, 0, sizeof(*ep));

+	ep->device = dev;

+	ep->wMaxPacketSize = desc->wMaxPacketSize;

+	ep->address = desc->bEndpointAddress & 0x0F;

+	ep->type = (usbh_eptype_t) (desc->bmAttributes & 0x03);

+	if (ep->type != USBH_EPTYPE_CTRL) {

+		ep->in = (desc->bEndpointAddress & 0x80) ? TRUE : FALSE;

+	}

+	ep->bInterval = desc->bInterval;

+

+	/* low-level part */

+	usbh_lld_ep_object_init(ep);

+

+	ep->status = USBH_EPSTATUS_CLOSED;

+}

+

+

+static void _ep0_object_init(usbh_device_t *dev, uint16_t wMaxPacketSize) {

+	const usbh_endpoint_descriptor_t ep0_descriptor = {

+		7,	//bLength

+		5,	//bDescriptorType

+		0,	//bEndpointAddress

+		0,	//bmAttributes

+		wMaxPacketSize,

+		0,	//bInterval

+	};

+	usbhEPObjectInit(&dev->ctrl, dev, &ep0_descriptor);

+	usbhEPSetName(&dev->ctrl, "DEV[CTRL]");

+}

+

+

+/*===========================================================================*/

+/* URB API.				    	                                     		 */

+/*===========================================================================*/

+

+void usbhURBObjectInit(usbh_urb_t *urb, usbh_ep_t *ep, usbh_completion_cb callback,

+		void *user, void *buff, uint32_t len) {

+

+	osalDbgCheck(urb != 0);

+	_check_ep(ep);

+

+	/* initialize the common part: */

+	urb->ep = ep;

+	urb->callback = callback;

+	urb->userData = user;

+	urb->buff = buff;

+	urb->requestedLength = len;

+	urb->actualLength = 0;

+	urb->status = USBH_URBSTATUS_INITIALIZED;

+	urb->waitingThread = 0;

+	urb->abortingThread = 0;

+

+	/* initialize the ll part: */

+	usbh_lld_urb_object_init(urb);

+}

+

+void usbhURBObjectResetI(usbh_urb_t *urb) {

+	osalDbgAssert(!usbhURBIsBusy(urb), "invalid status");

+

+	osalDbgCheck((urb->waitingThread == 0) && (urb->abortingThread == 0));

+

+	urb->actualLength = 0;

+	urb->status = USBH_URBSTATUS_INITIALIZED;

+

+	/* reset the ll part: */

+	usbh_lld_urb_object_reset(urb);

+}

+

+void usbhURBSubmitI(usbh_urb_t *urb) {

+	osalDbgCheckClassI();

+	_check_urb(urb);

+	osalDbgAssert(urb->status == USBH_URBSTATUS_INITIALIZED, "invalid status");

+	usbh_ep_t *const ep = urb->ep;

+	if (ep->status == USBH_EPSTATUS_HALTED) {

+		_usbh_urb_completeI(urb, USBH_URBSTATUS_STALL);

+		return;

+	}

+	if (ep->status != USBH_EPSTATUS_OPEN) {

+		_usbh_urb_completeI(urb, USBH_URBSTATUS_DISCONNECTED);

+		return;

+	}

+	if (!(usbhDeviceGetPort(ep->device)->status & USBH_PORTSTATUS_ENABLE)) {

+		_usbh_urb_completeI(urb, USBH_URBSTATUS_DISCONNECTED);

+		return;

+	}

+	urb->status = USBH_URBSTATUS_PENDING;

+	usbh_lld_urb_submit(urb);

+}

+

+bool _usbh_urb_abortI(usbh_urb_t *urb, usbh_urbstatus_t status) {

+	osalDbgCheckClassI();

+	_check_urb(urb);

+

+	switch (urb->status) {

+/*	case USBH_URBSTATUS_UNINITIALIZED:

+ * 	case USBH_URBSTATUS_INITIALIZED:

+ *	case USBH_URBSTATUS_ERROR:

+ *	case USBH_URBSTATUS_TIMEOUT:

+ *	case USBH_URBSTATUS_CANCELLED:

+ *	case USBH_URBSTATUS_STALL:

+ *	case USBH_URBSTATUS_DISCONNECTED:

+ *	case USBH_URBSTATUS_OK: */

+	default:

+		/* already finished */

+		_usbh_urb_completeI(urb, status);

+		return TRUE;

+

+//	case USBH_URBSTATUS_QUEUED:

+	case USBH_URBSTATUS_PENDING:

+		return usbh_lld_urb_abort(urb, status);

+	}

+}

+

+void _usbh_urb_abort_and_waitS(usbh_urb_t *urb, usbh_urbstatus_t status) {

+	osalDbgCheckClassS();

+	_check_urb(urb);

+

+	if (_usbh_urb_abortI(urb, status) == FALSE) {

+		uwarn("URB wasn't aborted immediately, suspend");

+		osalThreadSuspendS(&urb->abortingThread);

+		urb->abortingThread = 0;

+	} else {

+		osalOsRescheduleS();

+	}

+	uwarn("URB aborted");

+}

+

+bool usbhURBCancelI(usbh_urb_t *urb) {

+	return _usbh_urb_abortI(urb, USBH_URBSTATUS_CANCELLED);

+}

+

+void usbhURBCancelAndWaitS(usbh_urb_t *urb) {

+	_usbh_urb_abort_and_waitS(urb, USBH_URBSTATUS_CANCELLED);

+}

+

+msg_t usbhURBWaitTimeoutS(usbh_urb_t *urb, systime_t timeout) {

+	msg_t ret;

+

+	osalDbgCheckClassS();

+	_check_urb(urb);

+

+	switch (urb->status) {

+	case USBH_URBSTATUS_INITIALIZED:

+	case USBH_URBSTATUS_PENDING:

+//	case USBH_URBSTATUS_QUEUED:

+		ret = osalThreadSuspendTimeoutS(&urb->waitingThread, timeout);

+		urb->waitingThread = 0;

+		break;

+

+	case USBH_URBSTATUS_OK:

+		ret = MSG_OK;

+		osalOsRescheduleS();

+		break;

+

+/*	case USBH_URBSTATUS_UNINITIALIZED:

+ *	case USBH_URBSTATUS_ERROR:

+ *	case USBH_URBSTATUS_TIMEOUT:

+ *	case USBH_URBSTATUS_CANCELLED:

+ *	case USBH_URBSTATUS_STALL:

+ *	case USBH_URBSTATUS_DISCONNECTED: */

+	default:

+		ret = MSG_RESET;

+		osalOsRescheduleS();

+		break;

+	}

+	return ret;

+}

+

+msg_t usbhURBSubmitAndWaitS(usbh_urb_t *urb, systime_t timeout) {

+	msg_t ret;

+

+	osalDbgCheckClassS();

+	_check_urb(urb);

+

+	usbhURBSubmitI(urb);

+	ret = usbhURBWaitTimeoutS(urb, timeout);

+	if (ret == MSG_TIMEOUT)

+		_usbh_urb_abort_and_waitS(urb, USBH_URBSTATUS_TIMEOUT);

+

+	return ret;

+}

+

+static inline msg_t _wakeup_message(usbh_urbstatus_t status) {

+	if (status == USBH_URBSTATUS_OK) return MSG_OK;

+	if (status == USBH_URBSTATUS_TIMEOUT) return MSG_TIMEOUT;

+	return MSG_RESET;

+}

+

+void _usbh_urb_completeI(usbh_urb_t *urb, usbh_urbstatus_t status) {

+	osalDbgCheckClassI();

+	_check_urb(urb);

+	urb->status = status;

+	osalThreadResumeI(&urb->waitingThread, _wakeup_message(status));

+	osalThreadResumeI(&urb->abortingThread, MSG_RESET);

+	if (urb->callback)

+		urb->callback(urb);

+}

+

+/*===========================================================================*/

+/* Synchronous API.		    	                                     		 */

+/*===========================================================================*/

+

+usbh_urbstatus_t usbhBulkTransfer(usbh_ep_t *ep,

+		void *data,

+		uint32_t len,

+		uint32_t *actual_len,

+		systime_t timeout) {

+

+	osalDbgCheck(ep != NULL);

+	osalDbgCheck((data != NULL) || (len == 0));

+	osalDbgAssert(ep->type == USBH_EPTYPE_BULK, "wrong ep");

+

+	usbh_urb_t urb;

+	usbhURBObjectInit(&urb, ep, 0, 0, data, len);

+

+	osalSysLock();

+	usbhURBSubmitAndWaitS(&urb, timeout);

+	osalSysUnlock();

+

+	if (actual_len != NULL)

+		*actual_len = urb.actualLength;

+

+	return urb.status;

+}

+

+usbh_urbstatus_t usbhControlRequestExtended(usbh_device_t *dev,

+		const usbh_control_request_t *req,

+		uint8_t *buff,

+		uint32_t *actual_len,

+		systime_t timeout) {

+

+	_check_dev(dev);

+	osalDbgCheck(req != NULL);

+

+	usbh_urb_t urb;

+

+	usbhURBObjectInit(&urb, &dev->ctrl, 0, 0, buff, req->wLength);

+	urb.setup_buff = req;

+

+	osalSysLock();

+	usbhURBSubmitAndWaitS(&urb, timeout);

+	osalSysUnlock();

+

+	if (actual_len != NULL)

+		*actual_len = urb.actualLength;

+

+	return urb.status;

+}

+

+usbh_urbstatus_t usbhControlRequest(usbh_device_t *dev,

+		uint8_t bmRequestType,

+		uint8_t bRequest,

+		uint16_t wValue,

+		uint16_t wIndex,

+		uint16_t wLength,

+		uint8_t *buff) {

+

+	const USBH_DEFINE_BUFFER(usbh_control_request_t, req) = {

+			bmRequestType,

+			bRequest,

+			wValue,

+			wIndex,

+			wLength

+	};

+	return usbhControlRequestExtended(dev, &req, buff, NULL, MS2ST(1000));

+}

+

+/*===========================================================================*/

+/* Standard request helpers.   	                                     		 */

+/*===========================================================================*/

+

+#define USBH_GET_DESCRIPTOR(type, value, index)	\

+	USBH_STANDARDIN(type, \

+	USBH_REQ_GET_DESCRIPTOR, \

+	value, \

+	index) \

+

+#define USBH_GETDEVICEDESCRIPTOR \

+	USBH_GET_DESCRIPTOR(USBH_REQTYPE_DEVICE, (USBH_DT_DEVICE << 8) | 0, 0)

+

+#define USBH_GETCONFIGURATIONDESCRIPTOR(index) \

+	USBH_GET_DESCRIPTOR(USBH_REQTYPE_DEVICE, (USBH_DT_CONFIG << 8) | index, 0)

+

+#define USBH_GETSTRINGDESCRIPTOR(index, langID) \

+	USBH_GET_DESCRIPTOR(USBH_REQTYPE_DEVICE, (USBH_DT_STRING << 8) | index, langID)

+

+bool usbhStdReqGetDeviceDescriptor(usbh_device_t *dev,

+		uint16_t wLength,

+		uint8_t *buf) {

+	usbh_device_descriptor_t *desc;

+	usbh_urbstatus_t ret = usbhControlRequest(dev, USBH_GETDEVICEDESCRIPTOR, wLength, buf);

+	desc = (usbh_device_descriptor_t *)buf;

+	if ((ret != USBH_URBSTATUS_OK)

+			|| (desc->bLength != USBH_DT_DEVICE_SIZE)

+			|| (desc->bDescriptorType != USBH_DT_DEVICE)) {

+		return HAL_FAILED;

+	}

+	return HAL_SUCCESS;

+}

+

+bool usbhStdReqGetConfigurationDescriptor(usbh_device_t *dev,

+		uint8_t index,

+		uint16_t wLength,

+		uint8_t *buf) {

+	usbh_urbstatus_t ret = usbhControlRequest(dev, USBH_GETCONFIGURATIONDESCRIPTOR(index), wLength, buf);

+	usbh_config_descriptor_t *const desc = (usbh_config_descriptor_t *)buf;

+	if ((ret != USBH_URBSTATUS_OK)

+			|| (desc->bLength < USBH_DT_CONFIG_SIZE)

+			|| (desc->bDescriptorType != USBH_DT_CONFIG)) {

+		return HAL_FAILED;

+	}

+	return HAL_SUCCESS;

+}

+

+bool usbhStdReqGetStringDescriptor(usbh_device_t *dev,

+		uint8_t index,

+		uint16_t langID,

+		uint16_t wLength,

+		uint8_t *buf) {

+

+	osalDbgAssert(wLength >= USBH_DT_STRING_SIZE, "wrong size");

+	usbh_string_descriptor_t *desc = (usbh_string_descriptor_t *)buf;

+	usbh_urbstatus_t ret = usbhControlRequest(dev, USBH_GETSTRINGDESCRIPTOR(index, langID), wLength, buf);

+	if ((ret != USBH_URBSTATUS_OK)

+			|| (desc->bLength < USBH_DT_STRING_SIZE)

+			|| (desc->bDescriptorType != USBH_DT_STRING)) {

+		return HAL_FAILED;

+	}

+	return HAL_SUCCESS;

+}

+

+

+

+#define USBH_SET_INTERFACE(interface, alt)	\

+	USBH_STANDARDOUT(USBH_REQTYPE_INTERFACE, \

+	USBH_REQ_SET_INTERFACE, \

+	alt, \

+	interface) \

+

+#define USBH_GET_INTERFACE(interface)	\

+	USBH_STANDARDIN(USBH_REQTYPE_INTERFACE, \

+	USBH_REQ_GET_INTERFACE, \

+	0, \

+	interface) \

+

+bool usbhStdReqSetInterface(usbh_device_t *dev,

+		uint8_t bInterfaceNumber,

+		uint8_t bAlternateSetting) {

+

+	usbh_urbstatus_t ret = usbhControlRequest(dev, USBH_SET_INTERFACE(bInterfaceNumber, bAlternateSetting), 0, NULL);

+	if (ret != USBH_URBSTATUS_OK)

+		return HAL_FAILED;

+

+	return HAL_SUCCESS;

+}

+

+bool usbhStdReqGetInterface(usbh_device_t *dev,

+		uint8_t bInterfaceNumber,

+		uint8_t *bAlternateSetting) {

+

+	USBH_DEFINE_BUFFER(uint8_t, alt);

+

+	usbh_urbstatus_t ret = usbhControlRequest(dev, USBH_GET_INTERFACE(bInterfaceNumber), 1, &alt);

+	if (ret != USBH_URBSTATUS_OK)

+		return HAL_FAILED;

+

+	*bAlternateSetting = alt;

+	return HAL_SUCCESS;

+}

+

+

+/*===========================================================================*/

+/* Device-related functions.   	                                     		 */

+/*===========================================================================*/

+

+static uint8_t _find_address(USBHDriver *host) {

+	uint8_t addr, i, j;

+	for (i = 0; i < sizeof_array(host->address_bitmap); i++) {

+		addr = host->address_bitmap[i];

+		for (j = 0; j < 8; j++) {

+			if ((addr & (1 << j)) == 0) {

+				//found:

+				addr = i * 8 + j + 1;

+				host->address_bitmap[i] |= (1 << j);

+				return addr;

+			}

+		}

+	}

+	return 0;

+}

+

+static void _free_address(USBHDriver *host, uint8_t addr) {

+	uinfof("Free address %d", addr);

+	host->address_bitmap[addr / 8] &= ~(1 << ((addr - 1) & 7));

+}

+

+static void _device_initialize(usbh_device_t *dev, usbh_devspeed_t speed) {

+	dev->address = 0;

+	dev->speed = speed;

+	dev->status = USBH_DEVSTATUS_DEFAULT;

+	dev->langID0 = 0;

+	dev->keepFullCfgDesc = 0;

+	_ep0_object_init(dev, 64);

+}

+

+static bool _device_setaddress(usbh_device_t *dev, uint8_t address) {

+	usbh_urbstatus_t ret = usbhControlRequest(dev,

+			USBH_STANDARDOUT(USBH_REQTYPE_DEVICE, USBH_REQ_SET_ADDRESS, address, 0),

+			0,

+			0);

+	if (ret != USBH_URBSTATUS_OK)

+		return HAL_FAILED;

+

+	dev->address = address;

+	return HAL_SUCCESS;

+}

+

+static inline bool _device_read_basic_cfgdesc(usbh_device_t *dev, uint8_t bConfiguration) {

+	/* get configuration descriptor */

+	return usbhStdReqGetConfigurationDescriptor(dev, bConfiguration,

+			sizeof(dev->basicConfigDesc), (uint8_t *)&dev->basicConfigDesc);

+}

+

+static void _device_read_full_cfgdesc(usbh_device_t *dev, uint8_t bConfiguration) {

+	_check_dev(dev);

+

+	uint8_t i;

+

+	if (dev->fullConfigurationDescriptor != NULL) {

+		chHeapFree(dev->fullConfigurationDescriptor);

+	}

+

+	dev->fullConfigurationDescriptor =

+			(uint8_t *)chHeapAlloc(0, dev->basicConfigDesc.wTotalLength);

+

+	if (!dev->fullConfigurationDescriptor)

+		return;

+

+	for (i = 0; i < 3; i++) {

+		if (usbhStdReqGetConfigurationDescriptor(dev, bConfiguration,

+				dev->basicConfigDesc.wTotalLength,

+				dev->fullConfigurationDescriptor) == HAL_SUCCESS) {

+			return;

+		}

+		osalThreadSleepMilliseconds(200);

+	}

+

+	/* error */

+	chHeapFree(dev->fullConfigurationDescriptor);

+	dev->fullConfigurationDescriptor = NULL;

+}

+

+static void _device_free_full_cfgdesc(usbh_device_t *dev) {

+	osalDbgCheck(dev);

+	if (dev->fullConfigurationDescriptor != NULL) {

+		chHeapFree(dev->fullConfigurationDescriptor);

+		dev->fullConfigurationDescriptor = NULL;

+	}

+}

+

+

+#define USBH_SET_CONFIGURATION(type, value, index)	\

+	USBH_STANDARDOUT(type, \

+	USBH_REQ_SET_CONFIGURATION, \

+	value, \

+	index) \

+

+#define USBH_SETDEVICECONFIGURATION(index) \

+	USBH_SET_CONFIGURATION(USBH_REQTYPE_DEVICE, index, 0)

+

+

+static bool _device_set_configuration(usbh_device_t *dev, uint8_t configuration) {

+	usbh_urbstatus_t ret = usbhControlRequest(dev,

+			USBH_SETDEVICECONFIGURATION(configuration),

+			0,

+			0);

+	if (ret != USBH_URBSTATUS_OK)

+		return HAL_FAILED;

+	return HAL_SUCCESS;

+}

+

+static bool _device_configure(usbh_device_t *dev, uint8_t bConfiguration) {

+	uint8_t i;

+

+	uinfof("Reading basic configuration descriptor %d", bConfiguration);

+	for (i = 0; i < 3; i++) {

+		if (!_device_read_basic_cfgdesc(dev, bConfiguration))

+			break;

+	}

+

+	if (i == 3) {

+		uerrf("Could not read basic configuration descriptor %d; "

+					"won't configure device", bConfiguration);

+		return HAL_FAILED;

+	}

+

+	uinfof("Selecting configuration %d", bConfiguration);

+	for (i = 0; i < 3; i++) {

+		if (!_device_set_configuration(dev, dev->basicConfigDesc.bConfigurationValue)) {

+			/* TODO: check if correctly configured using GET_CONFIGURATION */

+			dev->status = USBH_DEVSTATUS_CONFIGURED;

+			dev->bConfiguration = bConfiguration;

+

+			uinfo("Device configured.");

+			return HAL_SUCCESS;

+		}

+	}

+

+	return HAL_FAILED;

+}

+

+static bool _device_enumerate(usbh_device_t *dev) {

+

+	uinfo("Enumerate.");

+	uinfo("Get first 8 bytes of device descriptor");

+

+	/* get first 8 bytes of device descriptor */

+	if (usbhStdReqGetDeviceDescriptor(dev, 8, (uint8_t *)&dev->devDesc)) {

+		uerr("Error");

+		return HAL_FAILED;

+	}

+

+	uinfof("Configure bMaxPacketSize0 = %d", dev->devDesc.bMaxPacketSize0);

+	/* configure EP0 wMaxPacketSize */

+	usbhEPClose(&dev->ctrl);

+	_ep0_object_init(dev, dev->devDesc.bMaxPacketSize0);

+	usbhEPOpen(&dev->ctrl);

+

+	uint8_t addr = _find_address(dev->host);

+	if (addr == 0) {

+		uerr("No free addresses found");

+		return HAL_FAILED;

+	}

+

+	/* set device address */

+	uinfof("Set device address: %d", addr);

+	if (_device_setaddress(dev, addr)) {

+		uerr("Error");

+		_free_address(dev->host, addr);

+		return HAL_FAILED;

+	}

+

+	/* update EP because of the address change */

+	usbhEPClose(&dev->ctrl);

+	_ep0_object_init(dev, dev->devDesc.bMaxPacketSize0);

+	usbhEPOpen(&dev->ctrl);

+

+	uinfof("Wait stabilization...");

+	osalThreadSleepMilliseconds(HAL_USBH_DEVICE_ADDRESS_STABILIZATION);

+

+	/* address is set */

+	dev->status = USBH_DEVSTATUS_ADDRESS;

+

+	uinfof("Get full device desc");

+	/* get full device descriptor */

+	if (usbhStdReqGetDeviceDescriptor(dev, sizeof(dev->devDesc),

+			(uint8_t *)&dev->devDesc)) {

+		uerr("Error");

+		_device_setaddress(dev, 0);

+		_free_address(dev->host, addr);

+		return HAL_FAILED;

+	}

+

+	uinfof("Enumeration finished.");

+	return HAL_SUCCESS;

+}

+

+#if USBH_DEBUG_ENABLE && USBH_DEBUG_ENABLE_INFO

+void usbhDevicePrintInfo(usbh_device_t *dev) {

+	USBH_DEFINE_BUFFER(char, str[64]);

+	usbh_device_descriptor_t *const desc = &dev->devDesc;

+

+	uinfo("----- Device info -----");

+	uinfo("Device descriptor:");

+	uinfof("\tUSBSpec=%04x, #configurations=%d, langID0=%04x",

+			desc->bcdUSB,

+			desc->bNumConfigurations,

+			dev->langID0);

+

+	uinfof("\tClass=%02x, Subclass=%02x, Protocol=%02x",

+			desc->bDeviceClass,

+			desc->bDeviceSubClass,

+			desc->bDeviceProtocol);

+

+	uinfof("\tVID=%04x, PID=%04x, Release=%04x",

+			desc->idVendor,

+			desc->idProduct,

+			desc->bcdDevice);

+

+	if (dev->langID0) {

+		usbhDeviceReadString(dev, str, sizeof(str), desc->iManufacturer, dev->langID0);

+		uinfof("\tManufacturer: %s", str);

+		usbhDeviceReadString(dev, str, sizeof(str), desc->iProduct, dev->langID0);

+		uinfof("\tProduct: %s", str);

+		usbhDeviceReadString(dev, str, sizeof(str), desc->iSerialNumber, dev->langID0);

+		uinfof("\tSerial Number: %s", str);

+	}

+

+	if (dev->status == USBH_DEVSTATUS_CONFIGURED) {

+		uinfo("Configuration descriptor (partial):");

+		usbh_config_descriptor_t *const cfg = &dev->basicConfigDesc;

+		uinfof("\tbConfigurationValue=%d, Length=%d, #interfaces=%d",

+				cfg->bConfigurationValue,

+				cfg->wTotalLength,

+				cfg->bNumInterfaces);

+

+		uinfof("\tCurrent=%dmA", cfg->bMaxPower * 2);

+		uinfof("\tSelfPowered=%d, RemoteWakeup=%d",

+				cfg->bmAttributes & 0x40 ? 1 : 0,

+				cfg->bmAttributes & 0x20 ? 1 : 0);

+		if (dev->langID0) {

+			usbhDeviceReadString(dev, str, sizeof(str), cfg->iConfiguration, dev->langID0);

+			uinfof("\tName: %s", str);

+		}

+	}

+

+	uinfo("----- End Device info -----");

+

+}

+

+void usbhDevicePrintConfiguration(const uint8_t *descriptor, uint16_t rem) {

+	generic_iterator_t iep, icfg, ics;

+	if_iterator_t iif;

+

+	uinfo("----- Configuration info -----");

+	uinfo("Configuration descriptor:");

+	cfg_iter_init(&icfg, descriptor, rem);

+	const usbh_config_descriptor_t *const cfgdesc = cfg_get(&icfg);

+	uinfof("Configuration %d, #IFs=%d", cfgdesc->bConfigurationValue, cfgdesc->bNumInterfaces);

+

+	for (if_iter_init(&iif, &icfg); iif.valid; if_iter_next(&iif)) {

+		const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);

+

+		uinfof("  Interface %d, alt=%d, #EPs=%d, "

+				"Class=%02x, Subclass=%02x, Protocol=%02x",

+				ifdesc->bInterfaceNumber, ifdesc->bAlternateSetting, ifdesc->bNumEndpoints,

+				ifdesc->bInterfaceClass, ifdesc->bInterfaceSubClass, ifdesc->bInterfaceProtocol);

+

+		for (cs_iter_init(&ics, (generic_iterator_t *)&iif); ics.valid; cs_iter_next(&ics)) {

+			uinfof("    Class-Specific descriptor, Length=%d, Type=%02x",

+					ics.curr[0], ics.curr[1]);

+		}

+

+		for (ep_iter_init(&iep, &iif); iep.valid; ep_iter_next(&iep)) {

+			const usbh_endpoint_descriptor_t *const epdesc = ep_get(&iep);

+

+			uinfof("    Endpoint descriptor, Address=%02x, Type=%d, MaxPacket=%d, Interval=%d",

+					epdesc->bEndpointAddress,

+					epdesc->bmAttributes & 3,

+					epdesc->wMaxPacketSize,

+					epdesc->bInterval);

+

+			for (cs_iter_init(&ics, &iep); ics.valid; cs_iter_next(&ics)) {

+				uinfof("    Class-Specific descriptor, Length=%d, Type=%02x",

+						ics.curr[0], ics.curr[1]);

+			}

+		}

+	}

+	uinfo("----- End Configuration info -----");

+}

+#endif

+

+bool usbhDeviceReadString(usbh_device_t *dev, char *dest, uint8_t size,

+		uint8_t index, uint16_t langID) {

+

+	usbh_string_descriptor_t *const desc = (usbh_string_descriptor_t *)dest;

+	osalDbgAssert(size >= 2, "wrong size");

+

+	*dest = 0;

+	if (index == 0)

+		return HAL_SUCCESS;

+	if (usbhStdReqGetStringDescriptor(dev, index, langID, size, (uint8_t *)dest))

+		return HAL_FAILED;

+	if (desc->bLength & 1)

+		return HAL_FAILED;

+	if (desc->bLength <= 2)

+		return HAL_SUCCESS;

+

+	uint8_t nchars = desc->bLength / 2;		/* including the trailing 0 */

+	if (size < nchars)

+		nchars = size;

+

+	char *src = (char *)&desc->wData[0];

+	while (--nchars) {

+		*dest++ = *src;

+		src += 2;

+	}

+	*dest = 0;

+	return HAL_SUCCESS;

+}

+

+

+

+

+/*===========================================================================*/

+/* Port processing functions.  	                                     		 */

+/*===========================================================================*/

+

+static void _port_connected(usbh_port_t *port);

+

+static void _port_reset(usbh_port_t *port) {

+	usbhhubControlRequest(port->device.host,

+#if HAL_USBH_USE_HUB

+			port->hub,

+#endif

+			USBH_REQTYPE_OUT | USBH_REQTYPE_CLASS | USBH_REQTYPE_OTHER,

+			USBH_REQ_SET_FEATURE,

+			USBH_PORT_FEAT_RESET,

+			port->number,

+			0,

+			0);

+}

+

+static void _port_update_status(usbh_port_t *port) {

+	uint32_t stat;

+	if (usbhhubControlRequest(port->device.host,

+#if HAL_USBH_USE_HUB

+			port->hub,

+#endif

+			USBH_REQTYPE_IN | USBH_REQTYPE_CLASS | USBH_REQTYPE_OTHER,

+			USBH_REQ_GET_STATUS,

+			0,

+			port->number,

+			4,

+			(uint8_t *)&stat) != USBH_URBSTATUS_OK) {

+		return;

+	}

+	port->status = stat & 0xffff;

+	port->c_status |= stat >> 16;

+}

+

+static void _port_process_status_change(usbh_port_t *port) {

+

+	_port_update_status(port);

+

+	if (port->c_status & USBH_PORTSTATUS_C_CONNECTION) {

+		/* port connected status changed */

+		port->c_status &= ~USBH_PORTSTATUS_C_CONNECTION;

+		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_CONNECTION);

+		if ((port->status & (USBH_PORTSTATUS_CONNECTION | USBH_PORTSTATUS_ENABLE))

+				== USBH_PORTSTATUS_CONNECTION) {

+			if (port->device.status != USBH_DEVSTATUS_DISCONNECTED) {

+				_usbh_port_disconnected(port);

+			}

+

+			/* connected, disabled */

+			_port_connected(port);

+		} else {

+			/* disconnected */

+			_usbh_port_disconnected(port);

+		}

+	}

+

+	if (port->c_status & USBH_PORTSTATUS_C_RESET) {

+		port->c_status &= ~USBH_PORTSTATUS_C_RESET;

+		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_RESET);

+	}

+

+	if (port->c_status & USBH_PORTSTATUS_C_ENABLE) {

+		port->c_status &= ~USBH_PORTSTATUS_C_ENABLE;

+		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_ENABLE);

+	}

+

+	if (port->c_status & USBH_PORTSTATUS_C_OVERCURRENT) {

+		port->c_status &= ~USBH_PORTSTATUS_C_OVERCURRENT;

+		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_OVERCURRENT);

+	}

+

+	if (port->c_status & USBH_PORTSTATUS_C_SUSPEND) {

+		port->c_status &= ~USBH_PORTSTATUS_C_SUSPEND;

+		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_SUSPEND);

+	}

+

+}

+

+

+static void _port_connected(usbh_port_t *port) {

+	/* connected */

+

+	systime_t start;

+	uint8_t i;

+	uint8_t retries;

+	usbh_devspeed_t speed;

+	USBH_DEFINE_BUFFER(usbh_string_descriptor_t, strdesc);

+

+	uinfof("Port %d connected, wait debounce...", port->number);

+

+	port->device.status = USBH_DEVSTATUS_ATTACHED;

+

+	/* wait for attach de-bounce */

+	osalThreadSleepMilliseconds(HAL_USBH_PORT_DEBOUNCE_TIME);

+

+	/* check disconnection */

+	_port_update_status(port);

+	if (port->c_status & USBH_PORTSTATUS_C_CONNECTION) {

+		/* connection state changed; abort */

+		goto abort;

+	}

+

+	port->device.status = USBH_DEVSTATUS_CONNECTED;

+	retries = 3;

+

+reset:

+	for (i = 0; i < 3; i++) {

+		uinfo("Try reset...");

+		port->c_status &= ~(USBH_PORTSTATUS_C_RESET | USBH_PORTSTATUS_C_ENABLE);

+		_port_reset(port);

+		osalThreadSleepMilliseconds(20);	/* give it some time to reset (min. 10ms) */

+		start = osalOsGetSystemTimeX();

+		while (TRUE) {

+			_port_update_status(port);

+

+			/* check for disconnection */

+			if (port->c_status & USBH_PORTSTATUS_C_CONNECTION)

+				goto abort;

+

+			/* check for reset completion */

+			if (port->c_status & USBH_PORTSTATUS_C_RESET) {

+				port->c_status &= ~USBH_PORTSTATUS_C_RESET;

+				usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_RESET);

+

+				if ((port->status & (USBH_PORTSTATUS_ENABLE | USBH_PORTSTATUS_CONNECTION))

+						== (USBH_PORTSTATUS_ENABLE | USBH_PORTSTATUS_CONNECTION)) {

+					goto reset_success;

+				}

+			}

+

+			/* check for timeout */

+			if (osalOsGetSystemTimeX() - start > HAL_USBH_PORT_RESET_TIMEOUT) break;

+		}

+	}

+

+	/* reset procedure failed; abort */

+	goto abort;

+

+reset_success:

+

+	uinfo("Reset OK, recovery...");

+

+	/* reset recovery */

+	osalThreadSleepMilliseconds(100);

+

+	/* initialize object */

+	if (port->status & USBH_PORTSTATUS_LOW_SPEED) {

+		speed = USBH_DEVSPEED_LOW;

+	} else if (port->status & USBH_PORTSTATUS_HIGH_SPEED) {

+		speed = USBH_DEVSPEED_HIGH;

+	} else {

+		speed = USBH_DEVSPEED_FULL;

+	}

+	_device_initialize(&port->device, speed);

+	usbhEPOpen(&port->device.ctrl);

+

+	/* device with default address (0), try enumeration */

+	if (_device_enumerate(&port->device)) {

+		/* enumeration failed */

+		usbhEPClose(&port->device.ctrl);

+

+		if (!--retries)

+			goto abort;

+

+		/* retry reset & enumeration */

+		goto reset;

+	}

+

+	/* load the default language ID */

+	uinfo("Loading langID0...");

+	if (!usbhStdReqGetStringDescriptor(&port->device, 0, 0,

+			USBH_DT_STRING_SIZE, (uint8_t *)&strdesc)

+		&& (strdesc.bLength >= 4)

+		&& !usbhStdReqGetStringDescriptor(&port->device, 0, 0,

+			4, (uint8_t *)&strdesc)) {

+

+		port->device.langID0 = strdesc.wData[0];

+		uinfof("langID0=%04x", port->device.langID0);

+	}

+

+	/* check if the device has only one configuration */

+	if (port->device.devDesc.bNumConfigurations == 1) {

+		uinfo("Device has only one configuration");

+		_device_configure(&port->device, 0);

+	}

+

+	_classdriver_process_device(&port->device);

+	return;

+

+abort:

+	uerr("Abort");

+	port->device.status = USBH_DEVSTATUS_DISCONNECTED;

+}

+

+void _usbh_port_disconnected(usbh_port_t *port) {

+	if (port->device.status == USBH_DEVSTATUS_DISCONNECTED)

+		return;

+

+	uinfo("Port disconnected");

+

+	/* unload drivers */

+	while (port->device.drivers) {

+		usbh_baseclassdriver_t *drv = port->device.drivers;

+

+		/* unload */

+		uinfof("Unload driver %s", drv->info->name);

+		drv->info->vmt->unload(drv);

+

+		/* unlink */

+		drv->dev = 0;

+		port->device.drivers = drv->next;

+	}

+

+	/* close control endpoint */

+	osalSysLock();

+	usbhEPCloseS(&port->device.ctrl);

+	osalSysUnlock();

+

+	/* free address */

+	if (port->device.address)

+		_free_address(port->device.host, port->device.address);

+

+	_device_free_full_cfgdesc(&port->device);

+

+	port->device.status = USBH_DEVSTATUS_DISCONNECTED;

+}

+

+

+

+/*===========================================================================*/

+/* Hub processing functions.  	                                     		 */

+/*===========================================================================*/

+

+#if HAL_USBH_USE_HUB

+static void _hub_update_status(USBHDriver *host, USBHHubDriver *hub) {

+	uint32_t stat;

+	if (usbhhubControlRequest(host,

+			hub,

+			USBH_REQTYPE_IN | USBH_REQTYPE_CLASS | USBH_REQTYPE_DEVICE,

+			USBH_REQ_GET_STATUS,

+			0,

+			0,

+			4,

+			(uint8_t *)&stat) != USBH_URBSTATUS_OK) {

+		return;

+	}

+	if (hub) {

+		hub->status = stat & 0xffff;

+		hub->c_status |= stat >> 16;

+	}

+}

+

+static void _hub_process_status_change(USBHDriver *host, USBHHubDriver *hub) {

+	uinfo("Hub status change. GET_STATUS.");

+	_hub_update_status(host, hub);

+

+	if (hub->c_status & USBH_HUBSTATUS_C_HUB_LOCAL_POWER) {

+		hub->c_status &= ~USBH_HUBSTATUS_C_HUB_LOCAL_POWER;

+		uinfo("Clear USBH_HUB_FEAT_C_HUB_LOCAL_POWER");

+		usbhhubClearFeatureHub(host, hub, USBH_HUB_FEAT_C_HUB_LOCAL_POWER);

+	}

+

+	if (hub->c_status & USBH_HUBSTATUS_C_HUB_OVER_CURRENT) {

+		hub->c_status &= ~USBH_HUBSTATUS_C_HUB_OVER_CURRENT;

+		uinfo("Clear USBH_HUB_FEAT_C_HUB_OVER_CURRENT");

+		usbhhubClearFeatureHub(host, hub, USBH_HUB_FEAT_C_HUB_OVER_CURRENT);

+	}

+}

+

+static uint32_t _hub_get_status_change_bitmap(USBHDriver *host, USBHHubDriver *hub) {

+	if (hub != NULL) {

+		osalSysLock();

+		uint32_t ret = hub->statuschange;

+		hub->statuschange = 0;

+		osalOsRescheduleS();

+		osalSysUnlock();

+		return ret;

+	}

+	return usbh_lld_roothub_get_statuschange_bitmap(host);

+}

+

+#else

+//TODO: replace the functions above

+#endif

+

+#if HAL_USBH_USE_HUB

+static void _hub_process(USBHDriver *host, USBHHubDriver *hub) {

+	uint32_t bitmap = _hub_get_status_change_bitmap(host, hub);

+	if (!bitmap)

+		return;

+

+	if (bitmap & 1) {

+		_hub_process_status_change(host, hub);

+		bitmap &= ~1;

+	}

+

+	usbh_port_t *port = (hub == NULL) ? &host->rootport : hub->ports;

+	uint8_t i;

+	for (i = 1; i < 32; i++) {

+		if (!bitmap || !port)

+			break;

+		if (bitmap & (1 << i)) {

+			bitmap &= ~(1 << i);

+			_port_process_status_change(port);

+		}

+		port = port->next;

+	}

+

+}

+#else

+static void _hub_process(USBHDriver *host) {

+	uint32_t bitmap = usbh_lld_roothub_get_statuschange_bitmap(host);

+

+#if 0	//TODO: complete _hub_process_status_change for root hub

+	if (bitmap & 1) {

+		_hub_process_status_change(host, hub);

+		bitmap &= ~1;

+	}

+#endif

+

+	if (!bitmap)

+		return;

+

+	_port_process_status_change(&host->rootport);

+}

+#endif

+

+/*===========================================================================*/

+/* Main processing loop (enumeration, loading/unloading drivers, etc). 		 */

+/*===========================================================================*/

+void usbhMainLoop(USBHDriver *usbh) {

+

+	if (usbh->status == USBH_STATUS_STOPPED)

+		return;

+

+#if HAL_USBH_USE_HUB

+	/* process root hub */

+	_hub_process(usbh, NULL);

+

+	/* process connected hubs */

+	USBHHubDriver *hub;

+    list_for_each_entry(hub, USBHHubDriver, &usbh->hubs, node) {

+		_hub_process(usbh, hub);

+	}

+#else

+	/* process root hub */

+	_hub_process(usbh);

+#endif

+}

+

+

+/*===========================================================================*/

+/* IAD class driver.											 		 	 */

+/*===========================================================================*/

+#if HAL_USBH_USE_IAD

+static usbh_baseclassdriver_t *iad_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);

+static void iad_unload(usbh_baseclassdriver_t *drv);

+static const usbh_classdriver_vmt_t usbhiadClassDriverVMT = {

+	iad_load,

+	iad_unload

+};

+static const usbh_classdriverinfo_t usbhiadClassDriverInfo = {

+	0xef, 0x02, 0x01, "IAD", &usbhiadClassDriverVMT

+};

+

+static usbh_baseclassdriver_t *iad_load(usbh_device_t *dev,

+		const uint8_t *descriptor, uint16_t rem) {

+	(void)rem;

+

+	if (descriptor[1] != USBH_DT_DEVICE)

+		return 0;

+

+	uinfo("Load a driver for each IF collection.");

+

+	generic_iterator_t icfg;

+	if_iterator_t iif;

+	const usbh_ia_descriptor_t *last_iad = 0;

+

+	cfg_iter_init(&icfg, dev->fullConfigurationDescriptor,

+			dev->basicConfigDesc.wTotalLength);

+	if (!icfg.valid) {

+		uerr("Invalid configuration descriptor.");

+		return 0;

+	}

+

+	for (if_iter_init(&iif, &icfg); iif.valid; if_iter_next(&iif)) {

+		if (iif.iad && (iif.iad != last_iad)) {

+			last_iad = iif.iad;

+			if (_classdriver_load(dev, iif.iad->bFunctionClass,

+					iif.iad->bFunctionSubClass,

+					iif.iad->bFunctionProtocol,

+					(uint8_t *)iif.iad,

+					(uint8_t *)iif.curr - (uint8_t *)iif.iad + iif.rem) != HAL_SUCCESS) {

+				uwarnf("No drivers found for IF collection #%d:%d",

+						iif.iad->bFirstInterface,

+						iif.iad->bFirstInterface + iif.iad->bInterfaceCount - 1);

+			}

+		}

+	}

+

+	return 0;

+}

+

+static void iad_unload(usbh_baseclassdriver_t *drv) {

+	(void)drv;

+}

+#endif

+

+

+/*===========================================================================*/

+/* Class driver loader.											 		 	 */

+/*===========================================================================*/

+

+static const usbh_classdriverinfo_t *usbh_classdrivers_lookup[] = {

+#if HAL_USBH_USE_FTDI

+	&usbhftdiClassDriverInfo,

+#endif

+#if HAL_USBH_USE_IAD

+	&usbhiadClassDriverInfo,

+#endif

+#if HAL_USBH_USE_UVC

+	&usbhuvcClassDriverInfo,

+#endif

+#if HAL_USBH_USE_MSD

+	&usbhmsdClassDriverInfo,

+#endif

+#if HAL_USBH_USE_HUB

+	&usbhhubClassDriverInfo

+#endif

+};

+

+static bool _classdriver_load(usbh_device_t *dev, uint8_t class,

+		uint8_t subclass, uint8_t protocol, uint8_t *descbuff, uint16_t rem) {

+	uint8_t i;

+	usbh_baseclassdriver_t *drv = NULL;

+	for (i = 0; i < sizeof_array(usbh_classdrivers_lookup); i++) {

+		const usbh_classdriverinfo_t *const info = usbh_classdrivers_lookup[i];

+		if (class == 0xff) {

+			/* vendor specific */

+			if (info->class == 0xff) {

+				uinfof("Try load vendor-specific driver %s", info->name);

+				drv = info->vmt->load(dev, descbuff, rem);

+				if (drv != NULL)

+					goto success;

+			}

+		} else if ((info->class < 0) || ((info->class == class)

+			&& ((info->subclass < 0) || ((info->subclass == subclass)

+			&& ((info->protocol < 0) || (info->protocol == protocol)))))) {

+			uinfof("Try load driver %s", info->name);

+			drv = info->vmt->load(dev, descbuff, rem);

+

+#if HAL_USBH_USE_IAD

+			/* special case: */

+			if (info == &usbhiadClassDriverInfo)

+				return HAL_SUCCESS;

+#endif

+

+			if (drv != NULL)

+				goto success;

+		}

+	}

+	return HAL_FAILED;

+

+success:

+	/* Link this driver to the device */

+	drv->next = dev->drivers;

+	dev->drivers = drv;

+	drv->dev = dev;

+	return HAL_SUCCESS;

+}

+

+static void _classdriver_process_device(usbh_device_t *dev) {

+	uinfo("New device found.");

+	const usbh_device_descriptor_t *const devdesc = &dev->devDesc;

+

+	usbhDevicePrintInfo(dev);

+

+	/* TODO: Support multiple configurations

+	 *

+	 * Windows doesn't support them, so it's unlikely that any commercial USB device

+	 * will have multiple configurations.

+	 */

+	if (dev->status != USBH_DEVSTATUS_CONFIGURED) {

+		uwarn("Multiple configurations not supported, selecting configuration #0");

+		if (_device_configure(dev, 0) != HAL_SUCCESS) {

+			uerr("Couldn't configure device; abort.");

+			return;

+		}

+	}

+

+	_device_read_full_cfgdesc(dev, dev->bConfiguration);

+	if (dev->fullConfigurationDescriptor == NULL) {

+		uerr("Couldn't read full configuration descriptor; abort.");

+		return;

+	}

+

+	usbhDevicePrintConfiguration(dev->fullConfigurationDescriptor,

+			dev->basicConfigDesc.wTotalLength);

+

+	if (devdesc->bDeviceClass == 0) {

+		/* each interface defines its own device class/subclass/protocol */

+		uinfo("Load a driver for each IF.");

+

+		generic_iterator_t icfg;

+		if_iterator_t iif;

+		uint8_t last_if = 0xff;

+

+		cfg_iter_init(&icfg, dev->fullConfigurationDescriptor,

+				dev->basicConfigDesc.wTotalLength);

+		if (!icfg.valid) {

+			uerr("Invalid configuration descriptor.");

+			goto exit;

+		}

+

+		for (if_iter_init(&iif, &icfg); iif.valid; if_iter_next(&iif)) {

+			const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);

+			if (ifdesc->bInterfaceNumber != last_if) {

+				last_if = ifdesc->bInterfaceNumber;

+				if (_classdriver_load(dev, ifdesc->bInterfaceClass,

+						ifdesc->bInterfaceSubClass,

+						ifdesc->bInterfaceProtocol,

+						(uint8_t *)ifdesc, iif.rem) != HAL_SUCCESS) {

+					uwarnf("No drivers found for IF #%d", ifdesc->bInterfaceNumber);

+				}

+			}

+		}

+

+	} else {

+		if (_classdriver_load(dev, devdesc->bDeviceClass,

+				devdesc->bDeviceSubClass,

+				devdesc->bDeviceProtocol,

+				(uint8_t *)devdesc, USBH_DT_DEVICE_SIZE) != HAL_SUCCESS) {

+			uwarn("No drivers found.");

+		}

+	}

+

+exit:

+	if (dev->keepFullCfgDesc == 0) {

+		_device_free_full_cfgdesc(dev);

+	}

+}

+

+

+#endif

+

diff --git a/os/hal/src/usbh/usbh_debug.c b/os/hal/src/usbh/usbh_debug.c
new file mode 100644
index 0000000..63505aa
--- /dev/null
+++ b/os/hal/src/usbh/usbh_debug.c
@@ -0,0 +1,536 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

+

+    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.

+*/

+

+#include "hal.h"

+

+#if HAL_USE_USBH

+

+#include "ch.h"

+#include "usbh/debug.h"

+#include <stdarg.h>

+#include "chprintf.h"

+

+#if USBH_DEBUG_ENABLE

+

+#define MAX_FILLER 11

+#define FLOAT_PRECISION 9

+#define MPRINTF_USE_FLOAT 0

+

+static char *long_to_string_with_divisor(char *p, long num, unsigned radix, long divisor) 

+{

+	int i;

+	char *q;

+	long l, ll;

+

+	l = num;

+	if (divisor == 0) {

+		ll = num;

+	} else {

+		ll = divisor;

+	}

+

+	q = p + MAX_FILLER;

+	do {

+		i = (int)(l % radix);

+		i += '0';

+		if (i > '9') {

+			i += 'A' - '0' - 10;

+		}

+		*--q = i;

+		l /= radix;

+	} while ((ll /= radix) != 0);

+

+	i = (int)(p + MAX_FILLER - q);

+	do {

+		*p++ = *q++;

+	} while (--i);

+

+	return p;

+}

+

+static char *ltoa(char *p, long num, unsigned radix) {

+

+	return long_to_string_with_divisor(p, num, radix, 0);

+}

+

+#if MPRINTF_USE_FLOAT

+static const long _pow10[FLOAT_PRECISION] = {10, 100, 1000, 10000, 100000, 1000000,

+	10000000, 100000000, 1000000000};

+static const double m10[FLOAT_PRECISION] = {5.0/100, 5.0/1000, 5.0/10000, 5.0/100000, 5.0/1000000,

+	5.0/10000000, 5.0/100000000, 5.0/1000000000, 5.0/10000000000};

+

+static char *ftoa(char *p, double num, unsigned long precision, bool dot) {

+	long l;

+	char *q;

+	double r;

+

+

+	if (precision == 0) {

+		l = (long)(num + 0.5);

+		return long_to_string_with_divisor(p, l, 10, 0);

+	} else {

+		if (precision > FLOAT_PRECISION) precision = FLOAT_PRECISION;

+		r = m10[precision - 1];

+		precision = _pow10[precision - 1];

+

+		l = (long)num;

+		p = long_to_string_with_divisor(p, l, 10, 0);

+		if (dot) *p++ = '.';

+		l = (long)((num - l + r) * precision);

+		q = long_to_string_with_divisor(p, l, 10, precision / 10) - 1;

+

+		while (q > p) {

+			if (*q != '0') {

+				break;

+			}

+			--q;

+		}

+		return ++q;

+	}

+

+	

+

+

+}

+#endif

+

+static inline void _put(char c) {

+	input_queue_t *iqp = &USBH_DEBUG_USBHD.iq;

+

+	if (chIQIsFullI(iqp))

+		return;

+

+	iqp->q_counter++;

+	*iqp->q_wrptr++ = c;

+	if (iqp->q_wrptr >= iqp->q_top)

+		iqp->q_wrptr = iqp->q_buffer;

+

+}

+

+int _dbg_printf(const char *fmt, va_list ap) {

+	char *p, *s, c, filler;

+	int i, precision, width;

+	int n = 0;

+	bool is_long, left_align, sign;

+	long l;

+#if MPRINTF_USE_FLOAT

+	double f;

+	char tmpbuf[2*MAX_FILLER + 1];

+#else

+	char tmpbuf[MAX_FILLER + 1];

+#endif

+

+	for (;;) {

+

+		//agarrar nuevo caracter de formato

+		c = *fmt++;

+

+		//chequeo eos

+		if (c == 0) return n;

+

+		//copio los caracteres comunes

+		if (c != '%') {

+			_put(c);

+			n++;

+			continue;

+		}

+

+		//encontré un '%'

+		p = tmpbuf;

+		s = tmpbuf;

+

+		//left align

+		left_align = FALSE;

+		if (*fmt == '-') {

+			fmt++;

+			left_align = TRUE;

+		}

+		

+		sign = FALSE;

+		if (*fmt == '+') {

+			fmt++;

+			sign = TRUE;

+		}

+

+		//filler

+		filler = ' ';

+		if (*fmt == '0') {

+			fmt++;

+			filler = '0';

+		}

+

+		//width

+		width = 0;

+		while (TRUE) {

+			c = *fmt++;

+			if (c >= '0' && c <= '9')

+				c -= '0';

+			else if (c == '*')

+				c = va_arg(ap, int);

+			else

+				break;

+			width = width * 10 + c;

+		}

+

+		//precision

+		precision = 0;

+		if (c == '.') {

+

+			if (*fmt == 'n') {

+				fmt++;

+

+			}

+			while (TRUE) {

+				c = *fmt++;

+				if (c >= '0' && c <= '9')

+					c -= '0';

+				else if (c == '*')

+					c = va_arg(ap, int);

+				else

+					break;

+				precision = precision * 10 + c;

+			}

+		}

+

+		//long modifier

+		if (c == 'l' || c == 'L') {

+			is_long = TRUE;

+			if (*fmt)

+				c = *fmt++;

+		}

+		else

+			is_long = (c >= 'A') && (c <= 'Z');

+

+		/* Command decoding.*/

+		switch (c) {

+		//char

+		case 'c':

+			filler = ' ';

+			*p++ = va_arg(ap, int);

+			break;

+

+		//string

+		case 's':

+			filler = ' ';

+			if ((s = va_arg(ap, char *)) == 0)

+				s = (char *)"(null)";

+			if (precision == 0)

+				precision = 32767;

+

+			//strlen con límite hasta precision

+			for (p = s; *p && (--precision >= 0); p++)

+				;

+			break;

+

+

+

+		case 'D':

+		case 'd':

+		case 'I':

+		case 'i':

+			if (is_long)

+				l = va_arg(ap, long);

+			else

+				l = va_arg(ap, int);

+			if (l < 0) {

+				*p++ = '-';

+				l = -l;

+				sign = TRUE;

+			} else if (sign) {

+				*p++ = '+';

+			}

+			p = ltoa(p, l, 10);

+			break;

+

+#if MPRINTF_USE_FLOAT

+		case 'f':

+			f = va_arg(ap, double);

+			if (f < 0) {

+				*p++ = '-';

+				f = -f;

+				sign = TRUE;

+			} else if (sign) {

+				*p++ = '+';

+			}

+			if (prec == FALSE) precision = 6;

+			p = ftoa(p, f, precision, dot);

+			break;

+#endif

+

+

+		case 'X':

+		case 'x':

+			c = 16;

+			goto unsigned_common;

+		case 'U':

+		case 'u':

+			c = 10;

+			goto unsigned_common;

+		case 'O':

+		case 'o':

+			c = 8;

+

+unsigned_common:

+			if (is_long)

+				l = va_arg(ap, unsigned long);

+			else

+				l = va_arg(ap, unsigned int);

+			p = ltoa(p, l, c);

+			break;

+

+		//copiar

+		default:

+			*p++ = c;

+			break;

+		}

+

+		//longitud

+		i = (int)(p - s);

+

+		//calculo cuántos caracteres de filler debo poner

+		if ((width -= i) < 0)

+			width = 0;

+

+		if (left_align == FALSE)

+			width = -width;

+

+		if (width < 0) {

+			//alineado a la derecha

+

+			//poner el signo adelante

+			if (sign && filler == '0') {

+				_put(*s++);

+				n++;

+				i--;

+			}

+

+			//fill a la izquierda

+			do {

+				_put(filler);

+				n++;

+			} while (++width != 0);

+		}

+

+		//copiar los caracteres

+		while (--i >= 0) {

+			_put(*s++);

+			n++;

+		}

+

+		//fill a la derecha

+		while (width) {

+			_put(filler);

+			n++;

+			width--;

+		}

+	}

+

+	//return n; // can raise 'code is unreachable' warning

+

+}

+

+static void _print_hdr(void)

+{

+	uint32_t hfnum = USBH_DEBUG_USBHD.otg->HFNUM;

+	uint16_t hfir = USBH_DEBUG_USBHD.otg->HFIR;

+

+	_put(0xff);

+	_put(0xff);

+	_put(hfir & 0xff);

+	_put(hfir >> 8);

+	_put(hfnum & 0xff);

+	_put((hfnum >> 8) & 0xff);

+	_put((hfnum >> 16) & 0xff);

+	_put((hfnum >> 24) & 0xff);

+}

+

+void usbDbgPrintf(const char *fmt, ...)

+{

+	va_list ap;

+	va_start(ap, fmt);

+	syssts_t sts = chSysGetStatusAndLockX();

+	_print_hdr();

+	_dbg_printf(fmt, ap);

+	_put(0);

+	chThdDequeueNextI(&USBH_DEBUG_USBHD.iq.q_waiting, Q_OK);

+	chSysRestoreStatusX(sts);

+	va_end(ap);

+}

+

+

+void usbDbgPuts(const char *s)

+{

+	uint32_t buff[2] = {

+		0xffff | (USBH_DEBUG_USBHD.otg->HFIR << 16),

+		USBH_DEBUG_USBHD.otg->HFNUM

+	};

+	uint8_t *p = (uint8_t *)buff;

+	uint8_t *top = p + 8;

+

+	syssts_t sts = chSysGetStatusAndLockX();

+	input_queue_t *iqp = &USBH_DEBUG_USBHD.iq;

+	int rem = sizeof(USBH_DEBUG_USBHD.dbg_buff) - iqp->q_counter;

+	while (rem) {

+		*iqp->q_wrptr++ = *p;

+		if (iqp->q_wrptr >= iqp->q_top)

+			iqp->q_wrptr = iqp->q_buffer;

+		rem--;

+		if (++p == top) break;

+	}

+	while (rem) {

+		*iqp->q_wrptr++ = *s;

+		if (iqp->q_wrptr >= iqp->q_top)

+			iqp->q_wrptr = iqp->q_buffer;

+		rem--;

+		if (!*s++) break;

+	}

+	iqp->q_counter = sizeof(USBH_DEBUG_USBHD.dbg_buff) - rem;

+	chThdDequeueNextI(&USBH_DEBUG_USBHD.iq.q_waiting, Q_OK);

+	chSysRestoreStatusX(sts);

+}

+

+void usbDbgReset(void) {

+	const char *msg = "\r\n\r\n==== DEBUG OUTPUT RESET ====\r\n";

+

+	syssts_t sts = chSysGetStatusAndLockX();

+	chIQResetI(&USBH_DEBUG_USBHD.iq);

+	chOQResetI(&USBH_DEBUG_SD.oqueue);

+	while (*msg) {

+		*USBH_DEBUG_SD.oqueue.q_wrptr++ = *msg++;

+		USBH_DEBUG_SD.oqueue.q_counter--;

+	}

+	chSysRestoreStatusX(sts);

+}

+

+static int _get(void) {

+	if (!USBH_DEBUG_USBHD.iq.q_counter) return -1;

+	USBH_DEBUG_USBHD.iq.q_counter--;

+	uint8_t b = *USBH_DEBUG_USBHD.iq.q_rdptr++;

+	if (USBH_DEBUG_USBHD.iq.q_rdptr >= USBH_DEBUG_USBHD.iq.q_top) {

+		USBH_DEBUG_USBHD.iq.q_rdptr = USBH_DEBUG_USBHD.iq.q_buffer;

+	}

+	return b;

+}

+

+void usbDbgSystemHalted(void) {

+	while (true) {

+		if (!((bool)((USBH_DEBUG_SD.oqueue.q_wrptr == USBH_DEBUG_SD.oqueue.q_rdptr) && (USBH_DEBUG_SD.oqueue.q_counter != 0U))))

+			break;

+		USBH_DEBUG_SD.oqueue.q_counter++;

+		while (!(USART1->SR & USART_SR_TXE));

+		USART1->DR = *USBH_DEBUG_SD.oqueue.q_rdptr++;

+		if (USBH_DEBUG_SD.oqueue.q_rdptr >= USBH_DEBUG_SD.oqueue.q_top) {

+			USBH_DEBUG_SD.oqueue.q_rdptr = USBH_DEBUG_SD.oqueue.q_buffer;

+		}

+	}

+

+	int c;

+	int state = 0;

+	for (;;) {

+		c = _get(); if (c < 0) break;

+

+		if (state == 0) {

+			if (c == 0xff) state = 1;

+		} else if (state == 1) {

+			if (c == 0xff) state = 2;

+			else (state = 0);

+		} else {

+			c = _get(); if (c < 0) return;

+			c = _get(); if (c < 0) return;

+			c = _get(); if (c < 0) return;

+			c = _get(); if (c < 0) return;

+			c = _get(); if (c < 0) return;

+

+			while (true) {

+				c = _get(); if (c < 0) return;

+				if (!c) {

+					while (!(USART1->SR & USART_SR_TXE));

+					USART1->DR = '\r';

+					while (!(USART1->SR & USART_SR_TXE));

+					USART1->DR = '\n';

+					state = 0;

+					break;

+				}

+				while (!(USART1->SR & USART_SR_TXE));

+				USART1->DR = c;

+			}

+		}

+	}

+}

+

+static void usb_debug_thread(void *p) {

+	USBHDriver *host = (USBHDriver *)p;

+	uint8_t state = 0;

+

+	chRegSetThreadName("USBH_DBG");

+	while (true) {

+		msg_t c = chIQGet(&host->iq);

+		if (c < 0) goto reset;

+

+		if (state == 0) {

+			if (c == 0xff) state = 1;

+		} else if (state == 1) {

+			if (c == 0xff) state = 2;

+			else (state = 0);

+		} else {

+			uint16_t hfir;

+			uint32_t hfnum;

+

+			hfir = c;

+			c = chIQGet(&host->iq); if (c < 0) goto reset;

+			hfir |= c << 8;

+

+			c = chIQGet(&host->iq); if (c < 0) goto reset;

+			hfnum = c;

+			c = chIQGet(&host->iq); if (c < 0) goto reset;

+			hfnum |= c << 8;

+			c = chIQGet(&host->iq); if (c < 0) goto reset;

+			hfnum |= c << 16;

+			c = chIQGet(&host->iq); if (c < 0) goto reset;

+			hfnum |= c << 24;

+

+			uint32_t f = hfnum & 0xffff;

+			uint32_t p = 1000 - ((hfnum >> 16) / (hfir / 1000));

+			chprintf((BaseSequentialStream *)&USBH_DEBUG_SD, "%05d.%03d  ", f, p);

+

+			while (true) {

+				c = chIQGet(&host->iq); if (c < 0) goto reset;

+				if (!c) {

+					sdPut(&USBH_DEBUG_SD, '\r');

+					sdPut(&USBH_DEBUG_SD, '\n');

+					state = 0;

+					break;

+				}

+				sdPut(&USBH_DEBUG_SD, (uint8_t)c);

+			}

+		}

+

+		continue;

+reset:

+		state = 0;

+	}

+}

+

+void usbDbgInit(USBHDriver *host) {

+	if (host != &USBH_DEBUG_USBHD)

+		return;

+	chIQObjectInit(&USBH_DEBUG_USBHD.iq, USBH_DEBUG_USBHD.dbg_buff, sizeof(USBH_DEBUG_USBHD.dbg_buff), 0, 0);

+	chThdCreateStatic(USBH_DEBUG_USBHD.waDebug, sizeof(USBH_DEBUG_USBHD.waDebug), NORMALPRIO, usb_debug_thread, &USBH_DEBUG_USBHD);

+}

+#endif

+

+#endif

diff --git a/os/hal/src/usbh/usbh_desciter.c b/os/hal/src/usbh/usbh_desciter.c
new file mode 100644
index 0000000..80e4728
--- /dev/null
+++ b/os/hal/src/usbh/usbh_desciter.c
@@ -0,0 +1,165 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

+

+    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.

+*/

+

+#include "hal.h"

+

+#if HAL_USE_USBH

+

+#include "usbh/defs.h"

+#include "usbh/desciter.h"

+

+void cfg_iter_init(generic_iterator_t *icfg, const uint8_t *buff, uint16_t rem) {

+	icfg->valid = 0;

+

+	if ((buff[0] < 2) || (rem < 2) || (rem < buff[0])

+			|| (buff[0] < USBH_DT_CONFIG_SIZE)

+			|| (buff[1] != USBH_DT_CONFIG))

+		return;

+

+	if (rem > ((usbh_config_descriptor_t *)buff)->wTotalLength) {

+		rem = ((usbh_config_descriptor_t *)buff)->wTotalLength;

+	}

+

+	icfg->valid = 1;

+	icfg->rem = rem;

+	icfg->curr = buff;

+}

+

+void if_iter_next(if_iterator_t *iif) {

+	const uint8_t *curr = iif->curr;

+	uint16_t rem = iif->rem;

+

+	iif->valid = 0;

+

+	if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))

+		return;

+

+	for (;;) {

+		rem -= curr[0];

+		curr += curr[0];

+

+		if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))

+			return;

+

+		if (curr[1] == USBH_DT_INTERFACE_ASSOCIATION) {

+			if (curr[0] < USBH_DT_INTERFACE_ASSOCIATION_SIZE)

+				return;

+

+			iif->iad = (usbh_ia_descriptor_t *)curr;

+

+		} else if (curr[1] == USBH_DT_INTERFACE) {

+			if (curr[0] < USBH_DT_INTERFACE_SIZE)

+				return;

+

+			if (iif->iad) {

+				if ((curr[2] < iif->iad->bFirstInterface)

+					|| (curr[2] >= (iif->iad->bFirstInterface + iif->iad->bInterfaceCount)))

+					iif->iad = 0;

+			}

+			break;

+		}

+	}

+

+	iif->valid = 1;

+	iif->rem = rem;

+	iif->curr = curr;

+}

+

+void if_iter_init(if_iterator_t *iif, const generic_iterator_t *icfg) {

+	iif->iad = 0;

+	iif->curr = icfg->curr;

+	iif->rem = icfg->rem;

+	if_iter_next(iif);

+}

+

+void ep_iter_next(generic_iterator_t *iep) {

+	const uint8_t *curr = iep->curr;

+	uint16_t rem = iep->rem;

+

+	iep->valid = 0;

+

+	if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))

+		return;

+

+	for (;;) {

+		rem -= curr[0];

+		curr += curr[0];

+

+		if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))

+			return;

+

+		if ((curr[1] == USBH_DT_INTERFACE_ASSOCIATION)

+				|| (curr[1] == USBH_DT_INTERFACE)

+				|| (curr[1] == USBH_DT_CONFIG)) {

+			return;

+		} else if (curr[1] == USBH_DT_ENDPOINT) {

+			if (curr[0] < USBH_DT_ENDPOINT_SIZE)

+				return;

+

+			break;

+		}

+	}

+

+	iep->valid = 1;

+	iep->rem = rem;

+	iep->curr = curr;

+}

+

+void ep_iter_init(generic_iterator_t *iep, const if_iterator_t *iif) {

+	iep->curr = iif->curr;

+	iep->rem = iif->rem;

+	ep_iter_next(iep);

+}

+

+void cs_iter_next(generic_iterator_t *ics) {

+	const uint8_t *curr = ics->curr;

+	uint16_t rem = ics->rem;

+

+	ics->valid = 0;

+

+	if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))

+		return;

+

+	//for (;;) {

+		rem -= curr[0];

+		curr += curr[0];

+

+		if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))

+			return;

+

+		if ((curr[1] == USBH_DT_INTERFACE_ASSOCIATION)

+				|| (curr[1] == USBH_DT_INTERFACE)

+				|| (curr[1] == USBH_DT_CONFIG)

+				|| (curr[1] == USBH_DT_ENDPOINT)) {

+			return;

+		}

+

+	//	break;

+	//}

+

+	ics->valid = 1;

+	ics->rem = rem;

+	ics->curr = curr;

+}

+

+void cs_iter_init(generic_iterator_t *ics, const generic_iterator_t *iter) {

+	ics->curr = iter->curr;

+	ics->rem = iter->rem;

+	cs_iter_next(ics);

+}

+

+#endif

diff --git a/os/hal/src/usbh/usbh_ftdi.c b/os/hal/src/usbh/usbh_ftdi.c
new file mode 100644
index 0000000..cdf3410
--- /dev/null
+++ b/os/hal/src/usbh/usbh_ftdi.c
@@ -0,0 +1,717 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

+

+    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.

+*/

+

+#include "hal.h"

+#include "usbh.h"

+

+#if HAL_USBH_USE_FTDI

+

+#if !HAL_USE_USBH

+#error "USBHFTDI needs USBH"

+#endif

+

+#include <string.h>

+#include "usbh/dev/ftdi.h"

+#include "usbh/internal.h"

+

+//#pragma GCC optimize("Og")

+

+

+#if USBHFTDI_DEBUG_ENABLE_TRACE

+#define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define udbgf(f, ...)  do {} while(0)

+#define udbg(f, ...)   do {} while(0)

+#endif

+

+#if USBHFTDI_DEBUG_ENABLE_INFO

+#define uinfof(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define uinfo(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define uinfof(f, ...)  do {} while(0)

+#define uinfo(f, ...)   do {} while(0)

+#endif

+

+#if USBHFTDI_DEBUG_ENABLE_WARNINGS

+#define uwarnf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define uwarn(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define uwarnf(f, ...)  do {} while(0)

+#define uwarn(f, ...)   do {} while(0)

+#endif

+

+#if USBHFTDI_DEBUG_ENABLE_ERRORS

+#define uerrf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define uerr(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define uerrf(f, ...)  do {} while(0)

+#define uerr(f, ...)   do {} while(0)

+#endif

+

+

+/*===========================================================================*/

+/* USB Class driver loader for FTDI								 		 	 */

+/*===========================================================================*/

+USBHFTDIDriver USBHFTDID[HAL_USBHFTDI_MAX_INSTANCES];

+

+static usbh_baseclassdriver_t *_ftdi_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);

+static void _ftdi_unload(usbh_baseclassdriver_t *drv);

+

+static const usbh_classdriver_vmt_t class_driver_vmt = {

+	_ftdi_load,

+	_ftdi_unload

+};

+

+const usbh_classdriverinfo_t usbhftdiClassDriverInfo = {

+	0xff, 0xff, 0xff, "FTDI", &class_driver_vmt

+};

+

+static USBHFTDIPortDriver *_find_port(void) {

+	uint8_t i;

+	for (i = 0; i < HAL_USBHFTDI_MAX_PORTS; i++) {

+		if (FTDIPD[i].ftdip == NULL)

+			return &FTDIPD[i];

+	}

+	return NULL;

+}

+

+static usbh_baseclassdriver_t *_ftdi_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {

+	int i;

+	USBHFTDIDriver *ftdip;

+

+	if (dev->devDesc.idVendor != 0x0403) {

+		uerr("FTDI: Unrecognized VID");

+		return NULL;

+	}

+

+	switch (dev->devDesc.idProduct) {

+	case 0x6001:

+	case 0x6010:

+	case 0x6011:

+	case 0x6014:

+	case 0x6015:

+		break;

+	default:

+		uerr("FTDI: Unrecognized PID");

+		return NULL;

+	}

+

+	if ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_INTERFACE))

+		return NULL;

+

+	const usbh_interface_descriptor_t * const ifdesc = (const usbh_interface_descriptor_t * const)descriptor;

+	if (ifdesc->bInterfaceNumber != 0) {

+		uwarn("FTDI: Will allocate driver along with IF #0");

+	}

+

+	/* alloc driver */

+	for (i = 0; i < HAL_USBHFTDI_MAX_INSTANCES; i++) {

+		if (USBHFTDID[i].dev == NULL) {

+			ftdip = &USBHFTDID[i];

+			goto alloc_ok;

+		}

+	}

+

+	uwarn("FTDI: Can't alloc driver");

+

+	/* can't alloc */

+	return NULL;

+

+alloc_ok:

+	/* initialize the driver's variables */

+	ftdip->ports = 0;

+	switch (dev->devDesc.bcdDevice) {

+	case 0x200:		//AM

+		uinfo("FTDI: Type A chip");

+		ftdip->type = USBHFTDI_TYPE_A;

+		break;

+	case 0x400:		//BM

+	case 0x500:		//2232C

+	case 0x600:		//R

+	case 0x1000:	//230X

+		uinfo("FTDI: Type B chip");

+		ftdip->type = USBHFTDI_TYPE_B;

+		break;

+	case 0x700:		//2232H;

+	case 0x800:		//4232H;

+	case 0x900:		//232H;

+		uinfo("FTDI: Type H chip");

+		ftdip->type = USBHFTDI_TYPE_H;

+	default:

+		uerr("FTDI: Unrecognized chip type");

+		return NULL;

+	}

+	usbhEPSetName(&dev->ctrl, "FTD[CTRL]");

+

+	/* parse the configuration descriptor */

+	generic_iterator_t iep, icfg;

+	if_iterator_t iif;

+	cfg_iter_init(&icfg, dev->fullConfigurationDescriptor, dev->basicConfigDesc.wTotalLength);

+	for (if_iter_init(&iif, &icfg); iif.valid; if_iter_next(&iif)) {

+		const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);

+		uinfof("FTDI: Interface #%d", ifdesc->bInterfaceNumber);

+

+		USBHFTDIPortDriver *const prt = _find_port();

+		if (prt == NULL) {

+			uwarn("\tCan't alloc port for this interface");

+			break;

+		}

+

+		prt->ifnum = ifdesc->bInterfaceNumber;

+		prt->epin.status = USBH_EPSTATUS_UNINITIALIZED;

+		prt->epout.status = USBH_EPSTATUS_UNINITIALIZED;

+

+		for (ep_iter_init(&iep, &iif); iep.valid; ep_iter_next(&iep)) {

+			const usbh_endpoint_descriptor_t *const epdesc = ep_get(&iep);

+			if ((epdesc->bEndpointAddress & 0x80) && (epdesc->bmAttributes == USBH_EPTYPE_BULK)) {

+				uinfof("BULK IN endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);

+				usbhEPObjectInit(&prt->epin, dev, epdesc);

+				usbhEPSetName(&prt->epin, "FTD[BIN ]");

+			} else if (((epdesc->bEndpointAddress & 0x80) == 0)

+					&& (epdesc->bmAttributes == USBH_EPTYPE_BULK)) {

+				uinfof("BULK OUT endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);

+				usbhEPObjectInit(&prt->epout, dev, epdesc);

+				usbhEPSetName(&prt->epout, "FTD[BOUT]");

+			} else {

+				uinfof("unsupported endpoint found: bEndpointAddress=%02x, bmAttributes=%02x",

+						epdesc->bEndpointAddress, epdesc->bmAttributes);

+			}

+		}

+

+		if ((prt->epin.status != USBH_EPSTATUS_CLOSED)

+				|| (prt->epout.status != USBH_EPSTATUS_CLOSED)) {

+			uwarn("\tCouldn't find endpoints; can't alloc port for this interface");

+			continue;

+		}

+

+		/* link the new block driver to the list */

+		prt->next = ftdip->ports;

+		ftdip->ports = prt;

+		prt->ftdip = ftdip;

+

+		prt->state = USBHFTDIP_STATE_ACTIVE;

+	}

+

+	return (usbh_baseclassdriver_t *)ftdip;

+

+}

+

+static void _stop(USBHFTDIPortDriver *ftdipp);

+static void _ftdi_unload(usbh_baseclassdriver_t *drv) {

+	osalDbgCheck(drv != NULL);

+	USBHFTDIDriver *const ftdip = (USBHFTDIDriver *)drv;

+	USBHFTDIPortDriver *ftdipp = ftdip->ports;

+

+	osalMutexLock(&ftdip->mtx);

+	while (ftdipp) {

+		_stop(ftdipp);

+		ftdipp = ftdipp->next;

+	}

+

+	ftdipp = ftdip->ports;

+	osalSysLock();

+	while (ftdipp) {

+		USBHFTDIPortDriver *next = ftdipp->next;

+		usbhftdipObjectInit(ftdipp);

+		ftdipp = next;

+	}

+	osalSysUnlock();

+	osalMutexUnlock(&ftdip->mtx);

+}

+

+

+USBHFTDIPortDriver FTDIPD[HAL_USBHFTDI_MAX_PORTS];

+

+

+#define FTDI_COMMAND_RESET		0

+#define FTDI_RESET_ALL 			0

+#define FTDI_RESET_PURGE_RX 	1

+#define FTDI_RESET_PURGE_TX 	2

+

+#define FTDI_COMMAND_SETFLOW    2

+

+#define FTDI_COMMAND_SETBAUD    3

+

+#define FTDI_COMMAND_SETDATA    4

+#define FTDI_SETDATA_BREAK      (0x1 << 14)

+

+#if 0

+#define FTDI_COMMAND_MODEMCTRL  	1

+#define FTDI_COMMAND_GETMODEMSTATUS       5 /* Retrieve current value of modem status register */

+#define FTDI_COMMAND_SETEVENTCHAR         6 /* Set the event character */

+#define FTDI_COMMAND_SETERRORCHAR         7 /* Set the error character */

+#define FTDI_COMMAND_SETLATENCYTIMER      9 /* Set the latency timer */

+#define FTDI_COMMAND_GETLATENCYTIMER      10 /* Get the latency timer */

+#endif

+

+/*

+ * DATA FORMAT

+ *

+ * IN Endpoint

+ *

+ * The device reserves the first two bytes of data on this endpoint to contain

+ * the current values of the modem and line status registers. In the absence of

+ * data, the device generates a message consisting of these two status bytes

+ * every 40 ms

+ *

+ * Byte 0: Modem Status

+ *

+ * Offset       Description

+ * B0   Reserved - must be 1

+ * B1   Reserved - must be 0

+ * B2   Reserved - must be 0

+ * B3   Reserved - must be 0

+ * B4   Clear to Send (CTS)

+ * B5   Data Set Ready (DSR)

+ * B6   Ring Indicator (RI)

+ * B7   Receive Line Signal Detect (RLSD)

+ *

+ * Byte 1: Line Status

+ *

+ * Offset       Description

+ * B0   Data Ready (DR)

+ * B1   Overrun Error (OE)

+ * B2   Parity Error (PE)

+ * B3   Framing Error (FE)

+ * B4   Break Interrupt (BI)

+ * B5   Transmitter Holding Register (THRE)

+ * B6   Transmitter Empty (TEMT)

+ * B7   Error in RCVR FIFO

+ *

+ */

+#define FTDI_RS0_CTS    (1 << 4)

+#define FTDI_RS0_DSR    (1 << 5)

+#define FTDI_RS0_RI     (1 << 6)

+#define FTDI_RS0_RLSD   (1 << 7)

+

+#define FTDI_RS_DR      1

+#define FTDI_RS_OE      (1<<1)

+#define FTDI_RS_PE      (1<<2)

+#define FTDI_RS_FE      (1<<3)

+#define FTDI_RS_BI      (1<<4)

+#define FTDI_RS_THRE    (1<<5)

+#define FTDI_RS_TEMT    (1<<6)

+#define FTDI_RS_FIFO    (1<<7)

+

+

+static usbh_urbstatus_t _ftdi_port_control(USBHFTDIPortDriver *ftdipp,

+		uint8_t bRequest, uint8_t wValue, uint8_t bHIndex, uint16_t wLength,

+		uint8_t *buff) {

+

+	static const uint8_t bmRequestType[] = {

+		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //0 FTDI_COMMAND_RESET

+		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //1 FTDI_COMMAND_MODEMCTRL

+		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //2 FTDI_COMMAND_SETFLOW

+		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //3 FTDI_COMMAND_SETBAUD

+		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //4 FTDI_COMMAND_SETDATA

+	};

+

+	osalDbgCheck(bRequest < sizeof_array(bmRequestType));

+	osalDbgCheck(bRequest != 1);

+

+	const USBH_DEFINE_BUFFER(usbh_control_request_t, req) = {

+			bmRequestType[bRequest],

+			bRequest,

+			wValue,

+			(bHIndex << 8) | (ftdipp->ifnum + 1),

+			wLength

+	};

+

+	return usbhControlRequestExtended(ftdipp->ftdip->dev, &req, buff, NULL, MS2ST(1000));

+}

+

+static uint32_t _get_divisor(uint32_t baud, usbhftdi_type_t type) {

+	static const uint8_t divfrac[8] = {0, 3, 2, 4, 1, 5, 6, 7};

+	uint32_t divisor;

+

+	if (type == USBHFTDI_TYPE_A) {

+		uint32_t divisor3 = ((48000000UL / 2) + baud / 2) / baud;

+		uinfof("FTDI: desired=%dbps, real=%dbps", baud, (48000000UL / 2) / divisor3);

+		if ((divisor3 & 0x7) == 7)

+			divisor3++; /* round x.7/8 up to x+1 */

+

+		divisor = divisor3 >> 3;

+	    divisor3 &= 0x7;

+		if (divisor3 == 1)

+			divisor |= 0xc000;

+		else if (divisor3 >= 4)

+			divisor |= 0x4000;

+		else if (divisor3 != 0)

+			divisor |= 0x8000;

+		else if (divisor == 1)

+			divisor = 0;    /* special case for maximum baud rate */

+	} else {

+		if (type == USBHFTDI_TYPE_B) {

+			divisor = ((48000000UL / 2) + baud / 2) / baud;

+			uinfof("FTDI: desired=%dbps, real=%dbps", baud, (48000000UL / 2) / divisor);

+		} else {

+			/* hi-speed baud rate is 10-bit sampling instead of 16-bit */

+			if (baud < 1200)

+				baud = 1200;

+			divisor = (120000000UL * 8 + baud * 5) / (baud * 10);

+			uinfof("FTDI: desired=%dbps, real=%dbps", baud, (120000000UL * 8) / divisor / 10);

+		}

+		divisor = (divisor >> 3) | (divfrac[divisor & 0x7] << 14);

+

+		/* Deal with special cases for highest baud rates. */

+		if (divisor == 1)

+			divisor = 0;

+		else if (divisor == 0x4001)

+			divisor = 1;

+

+		if (type == USBHFTDI_TYPE_H)

+			divisor |= 0x00020000;

+	}

+	return divisor;

+}

+

+static usbh_urbstatus_t _set_baudrate(USBHFTDIPortDriver *ftdipp, uint32_t baudrate) {

+	uint32_t divisor = _get_divisor(baudrate, ftdipp->ftdip->type);

+	uint16_t wValue = (uint16_t)divisor;

+	uint16_t wIndex = (uint16_t)(divisor >> 16);

+	if (ftdipp->ftdip->dev->basicConfigDesc.bNumInterfaces > 1)

+		wIndex = (wIndex << 8) | (ftdipp->ifnum + 1);

+

+	const USBH_DEFINE_BUFFER(usbh_control_request_t, req) = {

+		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE,

+		FTDI_COMMAND_SETBAUD,

+		wValue,

+		wIndex,

+		0

+	};

+	return usbhControlRequestExtended(ftdipp->ftdip->dev, &req, NULL, NULL, MS2ST(1000));

+}

+

+

+static void _submitOutI(USBHFTDIPortDriver *ftdipp, uint32_t len) {

+	udbgf("FTDI: Submit OUT %d", len);

+	ftdipp->oq_urb.requestedLength = len;

+	usbhURBObjectResetI(&ftdipp->oq_urb);

+	usbhURBSubmitI(&ftdipp->oq_urb);

+}

+

+static void _out_cb(usbh_urb_t *urb) {

+	USBHFTDIPortDriver *const ftdipp = (USBHFTDIPortDriver *)urb->userData;

+	switch (urb->status) {

+	case USBH_URBSTATUS_OK:

+		ftdipp->oq_ptr = ftdipp->oq_buff;

+		ftdipp->oq_counter = 64;

+		chThdDequeueNextI(&ftdipp->oq_waiting, Q_OK);

+		return;

+	case USBH_URBSTATUS_DISCONNECTED:

+		uwarn("FTDI: URB OUT disconnected");

+		chThdDequeueNextI(&ftdipp->oq_waiting, Q_RESET);

+		return;

+	default:

+		uerrf("FTDI: URB OUT status unexpected = %d", urb->status);

+		break;

+	}

+	usbhURBObjectResetI(&ftdipp->oq_urb);

+	usbhURBSubmitI(&ftdipp->oq_urb);

+}

+

+static size_t _write_timeout(USBHFTDIPortDriver *ftdipp, const uint8_t *bp,

+		size_t n, systime_t timeout) {

+	chDbgCheck(n > 0U);

+

+	size_t w = 0;

+	chSysLock();

+	while (true) {

+		if (ftdipp->state != USBHFTDIP_STATE_READY) {

+			chSysUnlock();

+			return w;

+		}

+		while (usbhURBIsBusy(&ftdipp->oq_urb)) {

+			if (chThdEnqueueTimeoutS(&ftdipp->oq_waiting, timeout) != Q_OK) {

+				chSysUnlock();

+				return w;

+			}

+		}

+

+		*ftdipp->oq_ptr++ = *bp++;

+		if (--ftdipp->oq_counter == 0) {

+			_submitOutI(ftdipp, 64);

+			chSchRescheduleS();

+		}

+		chSysUnlock(); /* Gives a preemption chance in a controlled point.*/

+

+		w++;

+		if (--n == 0U)

+			return w;

+

+		chSysLock();

+	}

+}

+

+static msg_t _put_timeout(USBHFTDIPortDriver *ftdipp, uint8_t b, systime_t timeout) {

+

+	chSysLock();

+	if (ftdipp->state != USBHFTDIP_STATE_READY) {

+		chSysUnlock();

+		return Q_RESET;

+	}

+

+	while (usbhURBIsBusy(&ftdipp->oq_urb)) {

+		msg_t msg = chThdEnqueueTimeoutS(&ftdipp->oq_waiting, timeout);

+		if (msg < Q_OK) {

+			chSysUnlock();

+			return msg;

+		}

+	}

+

+	*ftdipp->oq_ptr++ = b;

+	if (--ftdipp->oq_counter == 0) {

+		_submitOutI(ftdipp, 64);

+		chSchRescheduleS();

+	}

+	chSysUnlock();

+	return Q_OK;

+}

+

+static size_t _write(USBHFTDIPortDriver *ftdipp, const uint8_t *bp, size_t n) {

+	return _write_timeout(ftdipp, bp, n, TIME_INFINITE);

+}

+

+static msg_t _put(USBHFTDIPortDriver *ftdipp, uint8_t b) {

+	return _put_timeout(ftdipp, b, TIME_INFINITE);

+}

+

+static void _submitInI(USBHFTDIPortDriver *ftdipp) {

+	udbg("FTDI: Submit IN");

+	usbhURBObjectResetI(&ftdipp->iq_urb);

+	usbhURBSubmitI(&ftdipp->iq_urb);

+}

+

+static void _in_cb(usbh_urb_t *urb) {

+	USBHFTDIPortDriver *const ftdipp = (USBHFTDIPortDriver *)urb->userData;

+	switch (urb->status) {

+	case USBH_URBSTATUS_OK:

+		if (urb->actualLength < 2) {

+			uwarnf("FTDI: URB IN actualLength = %d, < 2", urb->actualLength);

+		} else if (urb->actualLength > 2) {

+			udbgf("FTDI: URB IN data len=%d, status=%02x %02x",

+					urb->actualLength - 2,

+					((uint8_t *)urb->buff)[0],

+					((uint8_t *)urb->buff)[1]);

+			ftdipp->iq_ptr = ftdipp->iq_buff + 2;

+			ftdipp->iq_counter = urb->actualLength - 2;

+			chThdDequeueNextI(&ftdipp->iq_waiting, Q_OK);

+			return;

+		} else {

+			udbgf("FTDI: URB IN no data, status=%02x %02x",

+					((uint8_t *)urb->buff)[0],

+					((uint8_t *)urb->buff)[1]);

+			return;

+		}

+		break;

+	case USBH_URBSTATUS_DISCONNECTED:

+		uwarn("FTDI: URB IN disconnected");

+		chThdDequeueNextI(&ftdipp->iq_waiting, Q_RESET);

+		return;

+	default:

+		uerrf("FTDI: URB IN status unexpected = %d", urb->status);

+		break;

+	}

+	_submitInI(ftdipp);

+}

+

+static size_t _read_timeout(USBHFTDIPortDriver *ftdipp, uint8_t *bp,

+		size_t n, systime_t timeout) {

+	size_t r = 0;

+

+	chDbgCheck(n > 0U);

+

+	chSysLock();

+	while (true) {

+		if (ftdipp->state != USBHFTDIP_STATE_READY) {

+			chSysUnlock();

+			return r;

+		}

+		while (ftdipp->iq_counter == 0) {

+			if (!usbhURBIsBusy(&ftdipp->iq_urb))

+				_submitInI(ftdipp);

+			if (chThdEnqueueTimeoutS(&ftdipp->iq_waiting, timeout) != Q_OK) {

+				chSysUnlock();

+				return r;

+			}

+		}

+		*bp++ = *ftdipp->iq_ptr++;

+		if (--ftdipp->iq_counter == 0) {

+			_submitInI(ftdipp);

+			chSchRescheduleS();

+		}

+		chSysUnlock();

+

+		r++;

+		if (--n == 0U)

+			return r;

+

+		chSysLock();

+	}

+}

+

+static msg_t _get_timeout(USBHFTDIPortDriver *ftdipp, systime_t timeout) {

+	uint8_t b;

+

+	chSysLock();

+	if (ftdipp->state != USBHFTDIP_STATE_READY) {

+		chSysUnlock();

+		return Q_RESET;

+	}

+	while (ftdipp->iq_counter == 0) {

+		if (!usbhURBIsBusy(&ftdipp->iq_urb))

+			_submitInI(ftdipp);

+		msg_t msg = chThdEnqueueTimeoutS(&ftdipp->iq_waiting, timeout);

+		if (msg < Q_OK) {

+			chSysUnlock();

+			return msg;

+		}

+	}

+	b = *ftdipp->iq_ptr++;

+	if (--ftdipp->iq_counter == 0) {

+		_submitInI(ftdipp);

+		chSchRescheduleS();

+	}

+	chSysUnlock();

+

+	return (msg_t)b;

+}

+

+static msg_t _get(USBHFTDIPortDriver *ftdipp) {

+	return _get_timeout(ftdipp, TIME_INFINITE);

+}

+

+static size_t _read(USBHFTDIPortDriver *ftdipp, uint8_t *bp, size_t n) {

+	return _read_timeout(ftdipp, bp, n, TIME_INFINITE);

+}

+

+static void _vt(void *p) {

+	USBHFTDIPortDriver *const ftdipp = (USBHFTDIPortDriver *)p;

+	chSysLockFromISR();

+	uint32_t len = ftdipp->oq_ptr - ftdipp->oq_buff;

+	if (len && !usbhURBIsBusy(&ftdipp->oq_urb)) {

+		_submitOutI(ftdipp, len);

+	}

+	if ((ftdipp->iq_counter == 0) && !usbhURBIsBusy(&ftdipp->iq_urb)) {

+		_submitInI(ftdipp);

+	}

+	chVTSetI(&ftdipp->vt, MS2ST(16), _vt, ftdipp);

+	chSysUnlockFromISR();

+}

+

+static const struct FTDIPortDriverVMT async_channel_vmt = {

+	(size_t (*)(void *, const uint8_t *, size_t))_write,

+	(size_t (*)(void *, uint8_t *, size_t))_read,

+	(msg_t (*)(void *, uint8_t))_put,

+	(msg_t (*)(void *))_get,

+	(msg_t (*)(void *, uint8_t, systime_t))_put_timeout,

+	(msg_t (*)(void *, systime_t))_get_timeout,

+	(size_t (*)(void *, const uint8_t *, size_t, systime_t))_write_timeout,

+	(size_t (*)(void *, uint8_t *, size_t, systime_t))_read_timeout

+};

+

+

+static void _stop(USBHFTDIPortDriver *ftdipp) {

+	osalSysLock();

+	chVTResetI(&ftdipp->vt);

+	usbhEPCloseS(&ftdipp->epin);

+	usbhEPCloseS(&ftdipp->epout);

+	chThdDequeueAllI(&ftdipp->iq_waiting, Q_RESET);

+	chThdDequeueAllI(&ftdipp->oq_waiting, Q_RESET);

+	osalOsRescheduleS();

+	ftdipp->state = USBHFTDIP_STATE_ACTIVE;

+	osalSysUnlock();

+}

+

+void usbhftdipStop(USBHFTDIPortDriver *ftdipp) {

+	osalDbgCheck((ftdipp->state == USBHFTDIP_STATE_ACTIVE)

+			|| (ftdipp->state == USBHFTDIP_STATE_READY));

+

+	if (ftdipp->state == USBHFTDIP_STATE_ACTIVE) {

+		return;

+	}

+

+	osalMutexLock(&ftdipp->ftdip->mtx);

+	_stop(ftdipp);

+	osalMutexUnlock(&ftdipp->ftdip->mtx);

+}

+

+void usbhftdipStart(USBHFTDIPortDriver *ftdipp, const USBHFTDIPortConfig *config) {

+	static const USBHFTDIPortConfig default_config = {

+		HAL_USBHFTDI_DEFAULT_SPEED,

+		HAL_USBHFTDI_DEFAULT_FRAMING,

+		HAL_USBHFTDI_DEFAULT_HANDSHAKE,

+		HAL_USBHFTDI_DEFAULT_XON,

+		HAL_USBHFTDI_DEFAULT_XOFF

+	};

+

+	osalDbgCheck((ftdipp->state == USBHFTDIP_STATE_ACTIVE)

+			|| (ftdipp->state == USBHFTDIP_STATE_READY));

+

+	if (ftdipp->state == USBHFTDIP_STATE_READY)

+		return;

+

+	osalMutexLock(&ftdipp->ftdip->mtx);

+	if (config == NULL)

+		config = &default_config;

+

+	uint16_t wValue = 0;

+	_ftdi_port_control(ftdipp, FTDI_COMMAND_RESET, FTDI_RESET_ALL, 0, 0, NULL);

+	_set_baudrate(ftdipp, config->speed);

+	_ftdi_port_control(ftdipp, FTDI_COMMAND_SETDATA, config->framing, 0, 0, NULL);

+	if (config->handshake & USBHFTDI_HANDSHAKE_XON_XOFF)

+		wValue = (config->xoff_character << 8) | config->xon_character;

+	_ftdi_port_control(ftdipp, FTDI_COMMAND_SETFLOW, wValue, config->handshake, 0, NULL);

+

+	usbhURBObjectInit(&ftdipp->oq_urb, &ftdipp->epout, _out_cb, ftdipp, ftdipp->oq_buff, 0);

+	chThdQueueObjectInit(&ftdipp->oq_waiting);

+	ftdipp->oq_counter = 64;

+	ftdipp->oq_ptr = ftdipp->oq_buff;

+	usbhEPOpen(&ftdipp->epout);

+

+	usbhURBObjectInit(&ftdipp->iq_urb, &ftdipp->epin, _in_cb, ftdipp, ftdipp->iq_buff, 64);

+	chThdQueueObjectInit(&ftdipp->iq_waiting);

+	ftdipp->iq_counter = 0;

+	ftdipp->iq_ptr = ftdipp->iq_buff;

+	usbhEPOpen(&ftdipp->epin);

+	osalSysLock();

+	usbhURBSubmitI(&ftdipp->iq_urb);

+	osalSysUnlock();

+

+	chVTObjectInit(&ftdipp->vt);

+	chVTSet(&ftdipp->vt, MS2ST(16), _vt, ftdipp);

+

+	ftdipp->state = USBHFTDIP_STATE_READY;

+	osalMutexUnlock(&ftdipp->ftdip->mtx);

+}

+

+void usbhftdiObjectInit(USBHFTDIDriver *ftdip) {

+	osalDbgCheck(ftdip != NULL);

+	memset(ftdip, 0, sizeof(*ftdip));

+	ftdip->info = &usbhftdiClassDriverInfo;

+	osalMutexObjectInit(&ftdip->mtx);

+}

+

+void usbhftdipObjectInit(USBHFTDIPortDriver *ftdipp) {

+	osalDbgCheck(ftdipp != NULL);

+	memset(ftdipp, 0, sizeof(*ftdipp));

+	ftdipp->vmt = &async_channel_vmt;

+	ftdipp->state = USBHFTDIP_STATE_STOP;

+}

+

+#endif

diff --git a/os/hal/src/usbh/usbh_hub.c b/os/hal/src/usbh/usbh_hub.c
new file mode 100644
index 0000000..eb585d2
--- /dev/null
+++ b/os/hal/src/usbh/usbh_hub.c
@@ -0,0 +1,302 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

+

+    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.

+*/

+

+#include "hal.h"

+#include "usbh.h"

+#include "usbh/internal.h"

+

+#if HAL_USBH_USE_HUB

+

+#if !HAL_USE_USBH

+#error "USBHHUB needs HAL_USE_USBH"

+#endif

+

+#include <string.h>

+#include "usbh/dev/hub.h"

+

+#if USBHHUB_DEBUG_ENABLE_TRACE

+#define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define udbgf(f, ...)  do {} while(0)

+#define udbg(f, ...)   do {} while(0)

+#endif

+

+#if USBHHUB_DEBUG_ENABLE_INFO

+#define uinfof(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define uinfo(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define uinfof(f, ...)  do {} while(0)

+#define uinfo(f, ...)   do {} while(0)

+#endif

+

+#if USBHHUB_DEBUG_ENABLE_WARNINGS

+#define uwarnf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define uwarn(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define uwarnf(f, ...)  do {} while(0)

+#define uwarn(f, ...)   do {} while(0)

+#endif

+

+#if USBHHUB_DEBUG_ENABLE_ERRORS

+#define uerrf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define uerr(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define uerrf(f, ...)  do {} while(0)

+#define uerr(f, ...)   do {} while(0)

+#endif

+

+

+USBHHubDriver USBHHUBD[HAL_USBHHUB_MAX_INSTANCES];

+usbh_port_t USBHPorts[HAL_USBHHUB_MAX_PORTS];

+

+static usbh_baseclassdriver_t *hub_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);

+static void hub_unload(usbh_baseclassdriver_t *drv);

+static const usbh_classdriver_vmt_t usbhhubClassDriverVMT = {

+	hub_load,

+	hub_unload

+};

+const usbh_classdriverinfo_t usbhhubClassDriverInfo = {

+	0x09, 0x00, -1, "HUB", &usbhhubClassDriverVMT

+};

+

+

+void _usbhub_port_object_init(usbh_port_t *port, USBHDriver *usbh,

+		USBHHubDriver *hub, uint8_t number) {

+	memset(port, 0, sizeof(*port));

+	port->number = number;

+	port->device.host = usbh;

+	port->hub = hub;

+}

+

+usbh_urbstatus_t usbhhubControlRequest(USBHDriver *host, USBHHubDriver *hub,

+											uint8_t bmRequestType,

+											uint8_t bRequest,

+											uint16_t wValue,

+											uint16_t wIndex,

+											uint16_t wLength,

+											uint8_t *buf) {

+	if (hub == NULL)

+		return usbh_lld_root_hub_request(host, bmRequestType, bRequest, wValue, wIndex, wLength, buf);

+

+	return usbhControlRequest(hub->dev,

+			bmRequestType, bRequest, wValue, wIndex, wLength, buf);

+}

+

+

+static void _urb_complete(usbh_urb_t *urb) {

+

+	USBHHubDriver *const hubdp = (USBHHubDriver *)urb->userData;

+	switch (urb->status) {

+	case USBH_URBSTATUS_TIMEOUT:

+		/* the device NAKed */

+		udbg("HUB: no info");

+		hubdp->statuschange = 0;

+		break;

+	case USBH_URBSTATUS_OK: {

+		uint8_t len = hubdp->hubDesc.bNbrPorts / 8 + 1;

+		if (urb->actualLength != len) {

+			uwarnf("Expected %d status change bytes but got %d", len, urb->actualLength);

+		}

+

+		if (urb->actualLength < len)

+			len = urb->actualLength;

+

+		if (len > 4)

+			len = 4;

+

+		uint8_t *sc = (uint8_t *)&hubdp->statuschange;

+		uint8_t *r = hubdp->scbuff;

+		while (len--)

+			*sc++ |= *r++;

+

+		uinfof("HUB: change, %08x", hubdp->statuschange);

+	}	break;

+	case USBH_URBSTATUS_DISCONNECTED:

+		uwarn("HUB: URB disconnected, aborting poll");

+		return;

+	default:

+		uerrf("HUB: URB status unexpected = %d", urb->status);

+		break;

+	}

+

+	usbhURBObjectResetI(urb);

+	usbhURBSubmitI(urb);

+}

+

+static usbh_baseclassdriver_t *hub_load(usbh_device_t *dev,

+		const uint8_t *descriptor, uint16_t rem) {

+	int i;

+

+	USBHHubDriver *hubdp;

+

+	if ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_DEVICE))

+		return NULL;

+

+	if (dev->devDesc.bDeviceProtocol != 0)

+		return NULL;

+

+	generic_iterator_t iep, icfg;

+	if_iterator_t iif;

+

+	cfg_iter_init(&icfg, dev->fullConfigurationDescriptor,

+			dev->basicConfigDesc.wTotalLength);

+

+	if_iter_init(&iif, &icfg);

+	if (!iif.valid)

+		return NULL;

+	const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);

+	if ((ifdesc->bInterfaceClass != 0x09)

+		|| (ifdesc->bInterfaceSubClass != 0x00)

+		|| (ifdesc->bInterfaceProtocol != 0x00)) {

+		return NULL;

+	}

+

+	ep_iter_init(&iep, &iif);

+	if (!iep.valid)

+		return NULL;

+	const usbh_endpoint_descriptor_t *const epdesc = ep_get(&iep);

+	if ((epdesc->bmAttributes & 0x03) != USBH_EPTYPE_INT) {

+		return NULL;

+	}

+

+

+	/* alloc driver */

+	for (i = 0; i < HAL_USBHHUB_MAX_INSTANCES; i++) {

+		if (USBHHUBD[i].dev == NULL) {

+			hubdp = &USBHHUBD[i];

+			goto alloc_ok;

+		}

+	}

+

+	uwarn("Can't alloc HUB driver");

+

+	/* can't alloc */

+	return NULL;

+

+alloc_ok:

+	/* initialize the driver's variables */

+	hubdp->epint.status = USBH_EPSTATUS_UNINITIALIZED;

+	hubdp->dev = dev;

+	hubdp->ports = 0;

+

+	usbhEPSetName(&dev->ctrl, "HUB[CTRL]");

+

+	/* read Hub descriptor */

+	uinfo("Read Hub descriptor");

+	if (usbhhubControlRequest(dev->host, hubdp,

+			USBH_REQTYPE_IN | USBH_REQTYPE_CLASS | USBH_REQTYPE_DEVICE,

+			USBH_REQ_GET_DESCRIPTOR,

+			(USBH_DT_HUB << 8), 0, sizeof(hubdp->hubDesc),

+			(uint8_t *)&hubdp->hubDesc) != USBH_URBSTATUS_OK) {

+		hubdp->dev = NULL;

+		return NULL;

+	}

+

+	const usbh_hub_descriptor_t *const hubdesc = &hubdp->hubDesc;

+

+	uinfof("Hub descriptor loaded; %d ports, wHubCharacteristics=%04x, bPwrOn2PwrGood=%d, bHubContrCurrent=%d",

+			hubdesc->bNbrPorts,

+			hubdesc->wHubCharacteristics,

+			hubdesc->bPwrOn2PwrGood,

+			hubdesc->bHubContrCurrent);

+

+	/* Alloc ports */

+	uint8_t ports = hubdesc->bNbrPorts;

+	for (i = 0; (ports > 0) && (i < HAL_USBHHUB_MAX_PORTS); i++) {

+		if (USBHPorts[i].hub == NULL) {

+			uinfof("Alloc port %d", ports);

+			_usbhub_port_object_init(&USBHPorts[i], dev->host, hubdp, ports);

+			USBHPorts[i].next = hubdp->ports;

+			hubdp->ports = &USBHPorts[i];

+			--ports;

+		}

+	}

+

+	if (ports) {

+		uwarn("Could not alloc all ports");

+	}

+

+	/* link hub to the host's list */

+	list_add_tail(&hubdp->node, &dev->host->hubs);

+

+	/* enable power to ports */

+	usbh_port_t *port = hubdp->ports;

+	while (port) {

+		uinfof("Enable power for port %d", port->number);

+		usbhhubSetFeaturePort(port, USBH_PORT_FEAT_POWER);

+		port = port->next;

+	}

+

+	if (hubdesc->bPwrOn2PwrGood)

+		osalThreadSleepMilliseconds(2 * hubdesc->bPwrOn2PwrGood);

+

+	/* initialize the status change endpoint and trigger the first transfer */

+	usbhEPObjectInit(&hubdp->epint, dev, epdesc);

+	usbhEPSetName(&hubdp->epint, "HUB[INT ]");

+	usbhEPOpen(&hubdp->epint);

+

+	usbhURBObjectInit(&hubdp->urb, &hubdp->epint,

+			_urb_complete, hubdp, hubdp->scbuff,

+			(hubdesc->bNbrPorts + 8) / 8);

+

+	osalSysLock();

+	usbhURBSubmitI(&hubdp->urb);

+	osalOsRescheduleS();

+	osalSysUnlock();

+

+	return (usbh_baseclassdriver_t *)hubdp;

+}

+

+static void hub_unload(usbh_baseclassdriver_t *drv) {

+	osalDbgCheck(drv != NULL);

+	USBHHubDriver *const hubdp = (USBHHubDriver *)drv;

+

+	/* close the status change endpoint (this cancels ongoing URBs) */

+	osalSysLock();

+	usbhEPCloseS(&hubdp->epint);

+	osalSysUnlock();

+

+	/* de-alloc ports and unload drivers */

+	usbh_port_t *port = hubdp->ports;

+	while (port) {

+		_usbh_port_disconnected(port);

+		port->hub = NULL;

+		port = port->next;

+	}

+

+	/* unlink the hub from the host's list */

+	list_del(&hubdp->node);

+

+}

+

+void usbhhubObjectInit(USBHHubDriver *hubdp) {

+	osalDbgCheck(hubdp != NULL);

+	memset(hubdp, 0, sizeof(*hubdp));

+	hubdp->info = &usbhhubClassDriverInfo;

+}

+#else

+

+#if HAL_USE_USBH

+void _usbhub_port_object_init(usbh_port_t *port, USBHDriver *usbh, uint8_t number) {

+	memset(port, 0, sizeof(*port));

+	port->number = number;

+	port->device.host = usbh;

+}

+#endif

+

+#endif

diff --git a/os/hal/src/usbh/usbh_msd.c b/os/hal/src/usbh/usbh_msd.c
new file mode 100644
index 0000000..8db68a4
--- /dev/null
+++ b/os/hal/src/usbh/usbh_msd.c
@@ -0,0 +1,939 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

+

+    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.

+*/

+

+#include "hal.h"

+#include "usbh.h"

+

+#if HAL_USBH_USE_MSD

+

+#if !HAL_USE_USBH

+#error "USBHMSD needs USBH"

+#endif

+

+#include <string.h>

+#include "usbh/dev/msd.h"

+#include "usbh/internal.h"

+

+//#pragma GCC optimize("Og")

+

+

+#if USBHMSD_DEBUG_ENABLE_TRACE

+#define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define udbgf(f, ...)  do {} while(0)

+#define udbg(f, ...)   do {} while(0)

+#endif

+

+#if USBHMSD_DEBUG_ENABLE_INFO

+#define uinfof(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define uinfo(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define uinfof(f, ...)  do {} while(0)

+#define uinfo(f, ...)   do {} while(0)

+#endif

+

+#if USBHMSD_DEBUG_ENABLE_WARNINGS

+#define uwarnf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define uwarn(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define uwarnf(f, ...)  do {} while(0)

+#define uwarn(f, ...)   do {} while(0)

+#endif

+

+#if USBHMSD_DEBUG_ENABLE_ERRORS

+#define uerrf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define uerr(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define uerrf(f, ...)  do {} while(0)

+#define uerr(f, ...)   do {} while(0)

+#endif

+

+

+

+

+

+/*===========================================================================*/

+/* USB Class driver loader for MSD								 		 	 */

+/*===========================================================================*/

+

+USBHMassStorageDriver USBHMSD[HAL_USBHMSD_MAX_INSTANCES];

+

+static usbh_baseclassdriver_t *_msd_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);

+static void _msd_unload(usbh_baseclassdriver_t *drv);

+

+static const usbh_classdriver_vmt_t class_driver_vmt = {

+	_msd_load,

+	_msd_unload

+};

+

+const usbh_classdriverinfo_t usbhmsdClassDriverInfo = {

+	0x08, 0x06, 0x50, "MSD", &class_driver_vmt

+};

+

+#define MSD_REQ_RESET							0xFF

+#define MSD_GET_MAX_LUN							0xFE

+

+static usbh_baseclassdriver_t *_msd_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {

+	int i;

+	USBHMassStorageDriver *msdp;

+	uint8_t luns; // should declare it here to eliminate 'control bypass initialization' warning

+	usbh_urbstatus_t stat;  // should declare it here to eliminate 'control bypass initialization' warning

+

+	if ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_INTERFACE))

+		return NULL;

+

+	const usbh_interface_descriptor_t * const ifdesc = (const usbh_interface_descriptor_t *)descriptor;

+

+	if ((ifdesc->bAlternateSetting != 0)

+			|| (ifdesc->bNumEndpoints < 2)

+			|| (ifdesc->bInterfaceSubClass != 0x06)

+			|| (ifdesc->bInterfaceProtocol != 0x50)) {

+		return NULL;

+	}

+

+	/* alloc driver */

+	for (i = 0; i < HAL_USBHMSD_MAX_INSTANCES; i++) {

+		if (USBHMSD[i].dev == NULL) {

+			msdp = &USBHMSD[i];

+			goto alloc_ok;

+		}

+	}

+

+	uwarn("Can't alloc MSD driver");

+

+	/* can't alloc */

+	return NULL;

+

+alloc_ok:

+	/* initialize the driver's variables */

+	msdp->epin.status = USBH_EPSTATUS_UNINITIALIZED;

+	msdp->epout.status = USBH_EPSTATUS_UNINITIALIZED;

+	msdp->max_lun = 0;

+	msdp->tag = 0;

+	msdp->luns = 0;

+	msdp->ifnum = ifdesc->bInterfaceNumber;

+	usbhEPSetName(&dev->ctrl, "MSD[CTRL]");

+

+	/* parse the configuration descriptor */

+	if_iterator_t iif;

+	generic_iterator_t iep;

+	iif.iad = 0;

+	iif.curr = descriptor;

+	iif.rem = rem;

+	for (ep_iter_init(&iep, &iif); iep.valid; ep_iter_next(&iep)) {

+		const usbh_endpoint_descriptor_t *const epdesc = ep_get(&iep);

+		if ((epdesc->bEndpointAddress & 0x80) && (epdesc->bmAttributes == USBH_EPTYPE_BULK)) {

+			uinfof("BULK IN endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);

+			usbhEPObjectInit(&msdp->epin, dev, epdesc);

+			usbhEPSetName(&msdp->epin, "MSD[BIN ]");

+		} else if (((epdesc->bEndpointAddress & 0x80) == 0)

+				&& (epdesc->bmAttributes == USBH_EPTYPE_BULK)) {

+			uinfof("BULK OUT endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);

+			usbhEPObjectInit(&msdp->epout, dev, epdesc);

+			usbhEPSetName(&msdp->epout, "MSD[BOUT]");

+		} else {

+			uinfof("unsupported endpoint found: bEndpointAddress=%02x, bmAttributes=%02x",

+					epdesc->bEndpointAddress, epdesc->bmAttributes);

+		}

+	}

+	if ((msdp->epin.status != USBH_EPSTATUS_CLOSED) || (msdp->epout.status != USBH_EPSTATUS_CLOSED)) {

+		goto deinit;

+	}

+

+	/* read the number of LUNs */

+	uinfo("Reading Max LUN:");

+	USBH_DEFINE_BUFFER(uint8_t, buff[4]);

+	stat = usbhControlRequest(dev,

+			USBH_CLASSIN(USBH_REQTYPE_INTERFACE, MSD_GET_MAX_LUN, 0, msdp->ifnum),

+			1, buff);

+	if (stat == USBH_URBSTATUS_OK) {

+		msdp->max_lun = buff[0] + 1;

+		uinfof("\tmax_lun = %d", msdp->max_lun);

+		if (msdp->max_lun > HAL_USBHMSD_MAX_LUNS) {

+			msdp->max_lun = HAL_USBHMSD_MAX_LUNS;

+			uwarnf("\tUsing max_lun = %d", msdp->max_lun);

+		}

+	} else if (stat == USBH_URBSTATUS_STALL) {

+		uwarn("\tStall, max_lun = 1");

+		msdp->max_lun = 1;

+	} else {

+		uerr("\tError");

+		goto deinit;

+	}

+

+	/* open the bulk IN/OUT endpoints */

+	usbhEPOpen(&msdp->epin);

+	usbhEPOpen(&msdp->epout);

+

+	/* Alloc one block device per logical unit found */

+	luns = msdp->max_lun;

+	for (i = 0; (luns > 0) && (i < HAL_USBHMSD_MAX_LUNS); i++) {

+		if (MSBLKD[i].msdp == NULL) {

+			/* link the new block driver to the list */

+			MSBLKD[i].next = msdp->luns;

+			msdp->luns = &MSBLKD[i];

+			MSBLKD[i].msdp = msdp;

+

+			osalSysLock();

+			MSBLKD[i].state = BLK_ACTIVE;	/* transition directly to active, instead of BLK_STOP */

+			osalSysUnlock();

+

+			/* connect the LUN (TODO: review if it's best to leave the LUN disconnected) */

+			usbhmsdLUNConnect(&MSBLKD[i]);

+			luns--;

+		}

+	}

+

+	return (usbh_baseclassdriver_t *)msdp;

+

+deinit:

+	/* Here, the enpoints are closed, and the driver is unlinked */

+	return NULL;

+}

+

+static void _msd_unload(usbh_baseclassdriver_t *drv) {

+	osalDbgCheck(drv != NULL);

+	USBHMassStorageDriver *const msdp = (USBHMassStorageDriver *)drv;

+	USBHMassStorageLUNDriver *lunp = msdp->luns;

+

+	osalMutexLock(&msdp->mtx);

+	osalSysLock();

+	usbhEPCloseS(&msdp->epin);

+	usbhEPCloseS(&msdp->epout);

+	while (lunp) {

+		lunp->state = BLK_STOP;

+		lunp = lunp->next;

+	}

+	osalSysUnlock();

+	osalMutexUnlock(&msdp->mtx);

+

+	/* now that the LUNs are idle, deinit them */

+	lunp = msdp->luns;

+	osalSysLock();

+	while (lunp) {

+		usbhmsdLUNObjectInit(lunp);

+		lunp = lunp->next;

+	}

+	osalSysUnlock();

+}

+

+

+/*===========================================================================*/

+/* MSD Class driver operations (Bulk-Only transport)			 		 	 */

+/*===========================================================================*/

+

+

+

+/* USB Bulk Only Transport SCSI Command block wrapper */

+PACKED_STRUCT {

+	uint32_t dCBWSignature;

+	uint32_t dCBWTag;

+	uint32_t dCBWDataTransferLength;

+	uint8_t bmCBWFlags;

+	uint8_t bCBWLUN;

+	uint8_t bCBWCBLength;

+	uint8_t CBWCB[16];

+} msd_cbw_t;

+#define MSD_CBW_SIGNATURE						0x43425355

+#define MSD_CBWFLAGS_D2H						0x80

+#define MSD_CBWFLAGS_H2D						0x00

+

+

+/* USB Bulk Only Transport SCSI Command status wrapper */

+PACKED_STRUCT {

+	uint32_t dCSWSignature;

+	uint32_t dCSWTag;

+	uint32_t dCSWDataResidue;

+	uint8_t bCSWStatus;

+} msd_csw_t;

+#define MSD_CSW_SIGNATURE						0x53425355

+

+

+typedef union {

+	msd_cbw_t cbw;

+	msd_csw_t csw;

+} msd_transaction_t;

+

+typedef enum {

+	MSD_TRANSACTIONRESULT_OK,

+	MSD_TRANSACTIONRESULT_DISCONNECTED,

+	MSD_TRANSACTIONRESULT_STALL,

+	MSD_TRANSACTIONRESULT_BUS_ERROR,

+	MSD_TRANSACTIONRESULT_SYNC_ERROR

+} msd_transaction_result_t;

+

+typedef enum {

+	MSD_COMMANDRESULT_PASSED = 0,

+	MSD_COMMANDRESULT_FAILED = 1,

+	MSD_COMMANDRESULT_PHASE_ERROR = 2

+} msd_command_result_t;

+

+typedef struct {

+	msd_transaction_result_t tres;

+	msd_command_result_t cres;

+} msd_result_t;

+

+

+/* ----------------------------------------------------- */

+/* 			SCSI Commands 								*/

+/* ----------------------------------------------------- */

+

+/* Read 10 and Write 10 */

+#define SCSI_CMD_READ_10 						0x28

+#define SCSI_CMD_WRITE_10						0x2A

+

+/* Request sense */

+#define SCSI_CMD_REQUEST_SENSE 					0x03

+PACKED_STRUCT {

+	uint8_t byte[18];

+} scsi_sense_response_t;

+

+#define SCSI_SENSE_KEY_GOOD                     0x00

+#define SCSI_SENSE_KEY_RECOVERED_ERROR          0x01

+#define SCSI_SENSE_KEY_NOT_READY                0x02

+#define SCSI_SENSE_KEY_MEDIUM_ERROR             0x03

+#define SCSI_SENSE_KEY_HARDWARE_ERROR           0x04

+#define SCSI_SENSE_KEY_ILLEGAL_REQUEST          0x05

+#define SCSI_SENSE_KEY_UNIT_ATTENTION           0x06

+#define SCSI_SENSE_KEY_DATA_PROTECT             0x07

+#define SCSI_SENSE_KEY_BLANK_CHECK              0x08

+#define SCSI_SENSE_KEY_VENDOR_SPECIFIC          0x09

+#define SCSI_SENSE_KEY_COPY_ABORTED             0x0A

+#define SCSI_SENSE_KEY_ABORTED_COMMAND          0x0B

+#define SCSI_SENSE_KEY_VOLUME_OVERFLOW          0x0D

+#define SCSI_SENSE_KEY_MISCOMPARE               0x0E

+#define SCSI_ASENSE_NO_ADDITIONAL_INFORMATION   0x00

+#define SCSI_ASENSE_LOGICAL_UNIT_NOT_READY      0x04

+#define SCSI_ASENSE_INVALID_FIELD_IN_CDB        0x24

+#define SCSI_ASENSE_NOT_READY_TO_READY_CHANGE   0x28

+#define SCSI_ASENSE_WRITE_PROTECTED             0x27

+#define SCSI_ASENSE_FORMAT_ERROR                0x31

+#define SCSI_ASENSE_INVALID_COMMAND             0x20

+#define SCSI_ASENSE_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE 0x21

+#define SCSI_ASENSE_MEDIUM_NOT_PRESENT                 0x3A

+#define SCSI_ASENSEQ_NO_QUALIFIER                      0x00

+#define SCSI_ASENSEQ_FORMAT_COMMAND_FAILED             0x01

+#define SCSI_ASENSEQ_INITIALIZING_COMMAND_REQUIRED     0x02

+#define SCSI_ASENSEQ_OPERATION_IN_PROGRESS             0x07

+

+/* Inquiry */

+#define SCSI_CMD_INQUIRY 						0x12

+PACKED_STRUCT {

+	uint8_t peripheral;

+	uint8_t removable;

+	uint8_t version;

+	uint8_t response_data_format;

+	uint8_t additional_length;

+	uint8_t sccstp;

+	uint8_t bqueetc;

+	uint8_t cmdque;

+	uint8_t vendorID[8];

+	uint8_t productID[16];

+	uint8_t productRev[4];

+} scsi_inquiry_response_t;

+

+/* Read Capacity 10 */

+#define SCSI_CMD_READ_CAPACITY_10				0x25

+PACKED_STRUCT {

+	uint32_t last_block_addr;

+	uint32_t block_size;

+} scsi_readcapacity10_response_t;

+

+/* Start/Stop Unit */

+#define SCSI_CMD_START_STOP_UNIT				0x1B

+PACKED_STRUCT {

+	uint8_t op_code;

+	uint8_t lun_immed;

+	uint8_t res1;

+	uint8_t res2;

+	uint8_t loej_start;

+	uint8_t control;

+} scsi_startstopunit_request_t;

+

+/* test unit ready */

+#define SCSI_CMD_TEST_UNIT_READY				0x00

+

+/* Other commands, TODO: use or remove them

+#define SCSI_CMD_PREVENT_ALLOW_MEDIUM_REMOVAL	0x1E

+#define SCSI_CMD_VERIFY_10						0x2F

+#define SCSI_CMD_SEND_DIAGNOSTIC				0x1D

+#define SCSI_CMD_MODE_SENSE_6                   0x1A

+*/

+

+static inline void _prepare_cbw(msd_transaction_t *tran, USBHMassStorageLUNDriver *lunp) {

+	tran->cbw.bCBWLUN = (uint8_t)(lunp - &lunp->msdp->luns[0]);

+	memset(&tran->cbw.CBWCB, 0, sizeof(tran->cbw.CBWCB));

+}

+

+static msd_transaction_result_t _msd_transaction(msd_transaction_t *tran, USBHMassStorageLUNDriver *lunp, void *data) {

+

+	uint32_t actual_len;

+	usbh_urbstatus_t status;

+

+	tran->cbw.dCBWSignature = MSD_CBW_SIGNATURE;

+	tran->cbw.dCBWTag = ++lunp->msdp->tag;

+

+	/* control phase */

+	status = usbhBulkTransfer(&lunp->msdp->epout, &tran->cbw,

+					sizeof(tran->cbw), &actual_len, MS2ST(1000));

+

+	if (status == USBH_URBSTATUS_CANCELLED) {

+		uerr("\tMSD: Control phase: USBH_URBSTATUS_CANCELLED");

+		return MSD_TRANSACTIONRESULT_DISCONNECTED;

+	} else if (status == USBH_URBSTATUS_STALL) {

+		uerr("\tMSD: Control phase: USBH_URBSTATUS_STALL");

+		return MSD_TRANSACTIONRESULT_STALL;

+	} else if (status != USBH_URBSTATUS_OK) {

+		uerrf("\tMSD: Control phase: status = %d, != OK", status);

+		return MSD_TRANSACTIONRESULT_BUS_ERROR;

+	} else if (actual_len != sizeof(tran->cbw)) {

+		uerrf("\tMSD: Control phase: wrong actual_len = %d", actual_len);

+		return MSD_TRANSACTIONRESULT_BUS_ERROR;

+	}

+

+

+	/* data phase */

+	if (tran->cbw.dCBWDataTransferLength) {

+		status = usbhBulkTransfer(

+				tran->cbw.bmCBWFlags & MSD_CBWFLAGS_D2H ? &lunp->msdp->epin : &lunp->msdp->epout,

+				data,

+				tran->cbw.dCBWDataTransferLength,

+				&actual_len, MS2ST(20000));

+

+		if (status == USBH_URBSTATUS_CANCELLED) {

+			uerr("\tMSD: Data phase: USBH_URBSTATUS_CANCELLED");

+			return MSD_TRANSACTIONRESULT_DISCONNECTED;

+		} else if (status == USBH_URBSTATUS_STALL) {

+			uerr("\tMSD: Data phase: USBH_URBSTATUS_STALL");

+			return MSD_TRANSACTIONRESULT_STALL;

+		} else if (status != USBH_URBSTATUS_OK) {

+			uerrf("\tMSD: Data phase: status = %d, != OK", status);

+			return MSD_TRANSACTIONRESULT_BUS_ERROR;

+		} else if (actual_len != tran->cbw.dCBWDataTransferLength) {

+			uerrf("\tMSD: Data phase: wrong actual_len = %d", actual_len);

+			return MSD_TRANSACTIONRESULT_BUS_ERROR;

+		}

+	}

+

+

+	/* status phase */

+	status = usbhBulkTransfer(&lunp->msdp->epin, &tran->csw,

+			sizeof(tran->csw), &actual_len, MS2ST(1000));

+

+	if (status == USBH_URBSTATUS_CANCELLED) {

+		uerr("\tMSD: Status phase: USBH_URBSTATUS_CANCELLED");

+		return MSD_TRANSACTIONRESULT_DISCONNECTED;

+	} else if (status == USBH_URBSTATUS_STALL) {

+		uerr("\tMSD: Status phase: USBH_URBSTATUS_STALL");

+		return MSD_TRANSACTIONRESULT_STALL;

+	} else if (status != USBH_URBSTATUS_OK) {

+		uerrf("\tMSD: Status phase: status = %d, != OK", status);

+		return MSD_TRANSACTIONRESULT_BUS_ERROR;

+	} else if (actual_len != sizeof(tran->csw)) {

+		uerrf("\tMSD: Status phase: wrong actual_len = %d", actual_len);

+		return MSD_TRANSACTIONRESULT_BUS_ERROR;

+	} else if (tran->csw.dCSWSignature != MSD_CSW_SIGNATURE) {

+		uerr("\tMSD: Status phase: wrong signature");

+		return MSD_TRANSACTIONRESULT_BUS_ERROR;

+	} else if (tran->csw.dCSWTag != lunp->msdp->tag) {

+		uerrf("\tMSD: Status phase: wrong tag (expected %d, got %d)",

+				lunp->msdp->tag, tran->csw.dCSWTag);

+		return MSD_TRANSACTIONRESULT_SYNC_ERROR;

+	}

+

+	if (tran->csw.dCSWDataResidue) {

+		uwarnf("\tMSD: Residue=%d", tran->csw.dCSWDataResidue);

+	}

+

+	return MSD_TRANSACTIONRESULT_OK;

+}

+

+

+static msd_result_t scsi_inquiry(USBHMassStorageLUNDriver *lunp, scsi_inquiry_response_t *resp) {

+	msd_transaction_t transaction;

+	msd_result_t res;

+

+	_prepare_cbw(&transaction, lunp);

+	transaction.cbw.dCBWDataTransferLength = sizeof(scsi_inquiry_response_t);

+	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;

+	transaction.cbw.bCBWCBLength = 6;

+	transaction.cbw.CBWCB[0] = SCSI_CMD_INQUIRY;

+	transaction.cbw.CBWCB[4] = sizeof(scsi_inquiry_response_t);

+

+	res.tres = _msd_transaction(&transaction, lunp, resp);

+	if (res.tres == MSD_TRANSACTIONRESULT_OK) {

+		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;

+	}

+	return res;

+}

+

+static msd_result_t scsi_requestsense(USBHMassStorageLUNDriver *lunp, scsi_sense_response_t *resp) {

+	msd_transaction_t transaction;

+	msd_result_t res;

+

+	_prepare_cbw(&transaction, lunp);

+	transaction.cbw.dCBWDataTransferLength = sizeof(scsi_sense_response_t);

+	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;

+	transaction.cbw.bCBWCBLength = 12;

+	transaction.cbw.CBWCB[0] = SCSI_CMD_REQUEST_SENSE;

+	transaction.cbw.CBWCB[4] = sizeof(scsi_sense_response_t);

+

+	res.tres = _msd_transaction(&transaction, lunp, resp);

+	if (res.tres == MSD_TRANSACTIONRESULT_OK) {

+		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;

+	}

+	return res;

+}

+

+static msd_result_t scsi_testunitready(USBHMassStorageLUNDriver *lunp) {

+	msd_transaction_t transaction;

+	msd_result_t res;

+

+	_prepare_cbw(&transaction, lunp);

+	transaction.cbw.dCBWDataTransferLength = 0;

+	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;

+	transaction.cbw.bCBWCBLength = 6;

+	transaction.cbw.CBWCB[0] = SCSI_CMD_TEST_UNIT_READY;

+

+	res.tres = _msd_transaction(&transaction, lunp, NULL);

+	if (res.tres == MSD_TRANSACTIONRESULT_OK) {

+		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;

+	}

+	return res;

+}

+

+static msd_result_t scsi_readcapacity10(USBHMassStorageLUNDriver *lunp, scsi_readcapacity10_response_t *resp) {

+	msd_transaction_t transaction;

+	msd_result_t res;

+

+	_prepare_cbw(&transaction, lunp);

+	transaction.cbw.dCBWDataTransferLength = sizeof(scsi_readcapacity10_response_t);

+	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;

+	transaction.cbw.bCBWCBLength = 12;

+	transaction.cbw.CBWCB[0] = SCSI_CMD_READ_CAPACITY_10;

+

+	res.tres = _msd_transaction(&transaction, lunp, resp);

+	if (res.tres == MSD_TRANSACTIONRESULT_OK) {

+		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;

+	}

+	return res;

+}

+

+

+static msd_result_t scsi_read10(USBHMassStorageLUNDriver *lunp, uint32_t lba, uint16_t n, uint8_t *data) {

+	msd_transaction_t transaction;

+	msd_result_t res;

+

+	_prepare_cbw(&transaction, lunp);

+	transaction.cbw.dCBWDataTransferLength = n * lunp->info.blk_size;

+	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;

+	transaction.cbw.bCBWCBLength = 10;

+	transaction.cbw.CBWCB[0] = SCSI_CMD_READ_10;

+	transaction.cbw.CBWCB[2] = (uint8_t)(lba >> 24);

+	transaction.cbw.CBWCB[3] = (uint8_t)(lba >> 16);

+	transaction.cbw.CBWCB[4] = (uint8_t)(lba >> 8);

+	transaction.cbw.CBWCB[5] = (uint8_t)(lba);

+	transaction.cbw.CBWCB[7] = (uint8_t)(n >> 8);

+	transaction.cbw.CBWCB[8] = (uint8_t)(n);

+

+	res.tres = _msd_transaction(&transaction, lunp, data);

+	if (res.tres == MSD_TRANSACTIONRESULT_OK) {

+		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;

+	}

+	return res;

+}

+

+static msd_result_t scsi_write10(USBHMassStorageLUNDriver *lunp, uint32_t lba, uint16_t n, const uint8_t *data) {

+	msd_transaction_t transaction;

+	msd_result_t res;

+

+	_prepare_cbw(&transaction, lunp);

+	transaction.cbw.dCBWDataTransferLength = n * lunp->info.blk_size;

+	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_H2D;

+	transaction.cbw.bCBWCBLength = 10;

+	transaction.cbw.CBWCB[0] = SCSI_CMD_WRITE_10;

+	transaction.cbw.CBWCB[2] = (uint8_t)(lba >> 24);

+	transaction.cbw.CBWCB[3] = (uint8_t)(lba >> 16);

+	transaction.cbw.CBWCB[4] = (uint8_t)(lba >> 8);

+	transaction.cbw.CBWCB[5] = (uint8_t)(lba);

+	transaction.cbw.CBWCB[7] = (uint8_t)(n >> 8);

+	transaction.cbw.CBWCB[8] = (uint8_t)(n);

+

+	res.tres = _msd_transaction(&transaction, lunp, (uint8_t *)data);

+	if (res.tres == MSD_TRANSACTIONRESULT_OK) {

+		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;

+	}

+	return res;

+}

+

+

+

+/*===========================================================================*/

+/* Block driver data/functions								 		 	 	 */

+/*===========================================================================*/

+

+USBHMassStorageLUNDriver MSBLKD[HAL_USBHMSD_MAX_LUNS];

+

+static const struct USBHMassStorageDriverVMT blk_vmt = {

+	(bool (*)(void *))usbhmsdLUNIsInserted,

+	(bool (*)(void *))usbhmsdLUNIsProtected,

+	(bool (*)(void *))usbhmsdLUNConnect,

+	(bool (*)(void *))usbhmsdLUNDisconnect,

+	(bool (*)(void *, uint32_t, uint8_t *, uint32_t))usbhmsdLUNRead,

+	(bool (*)(void *, uint32_t,	const uint8_t *, uint32_t))usbhmsdLUNWrite,

+	(bool (*)(void *))usbhmsdLUNSync,

+	(bool (*)(void *, BlockDeviceInfo *))usbhmsdLUNGetInfo

+};

+

+

+

+static uint32_t _requestsense(USBHMassStorageLUNDriver *lunp) {

+	scsi_sense_response_t sense;

+	msd_result_t res;

+

+	res = scsi_requestsense(lunp, &sense);

+	if (res.tres != MSD_TRANSACTIONRESULT_OK) {

+		uerr("\tREQUEST SENSE: Transaction error");

+		goto failed;

+	} else if (res.cres == MSD_COMMANDRESULT_FAILED) {

+		uerr("\tREQUEST SENSE: Command Failed");

+		goto failed;

+	} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {

+		//TODO: Do reset, etc.

+		uerr("\tREQUEST SENSE: Command Phase Error");

+		goto failed;

+	}

+

+	uerrf("\tREQUEST SENSE: Sense key=%x, ASC=%02x, ASCQ=%02x",

+			sense.byte[2] & 0xf, sense.byte[12], sense.byte[13]);

+

+	return (sense.byte[2] & 0xf) | (sense.byte[12] << 8) | (sense.byte[13] << 16);

+

+failed:

+	return 0xffffffff;

+}

+

+void usbhmsdLUNObjectInit(USBHMassStorageLUNDriver *lunp) {

+	osalDbgCheck(lunp != NULL);

+	memset(lunp, 0, sizeof(*lunp));

+	lunp->vmt = &blk_vmt;

+	lunp->state = BLK_STOP;

+	/* Unnecessary because of the memset:

+		lunp->msdp = NULL;

+		lunp->next = NULL;

+		lunp->info.* = 0;

+	*/

+}

+

+void usbhmsdLUNStart(USBHMassStorageLUNDriver *lunp) {

+	osalDbgCheck(lunp != NULL);

+	osalSysLock();

+	osalDbgAssert((lunp->state == BLK_STOP) || (lunp->state == BLK_ACTIVE),

+			"invalid state");

+	//TODO: complete

+	//lunp->state = BLK_ACTIVE;

+	osalSysUnlock();

+}

+

+void usbhmsdLUNStop(USBHMassStorageLUNDriver *lunp) {

+	osalDbgCheck(lunp != NULL);

+	osalSysLock();

+	osalDbgAssert((lunp->state == BLK_STOP) || (lunp->state == BLK_ACTIVE),

+			"invalid state");

+	//TODO: complete

+	//lunp->state = BLK_STOP;

+	osalSysUnlock();

+}

+

+bool usbhmsdLUNConnect(USBHMassStorageLUNDriver *lunp) {

+	USBHMassStorageDriver *const msdp = lunp->msdp;

+	msd_result_t res;

+

+	osalDbgCheck(msdp != NULL);

+	osalSysLock();

+	//osalDbgAssert((lunp->state == BLK_ACTIVE) || (lunp->state == BLK_READY),

+    //            "invalid state");

+	if (lunp->state == BLK_READY) {

+	    osalSysUnlock();

+		return HAL_SUCCESS;

+	} else if (lunp->state != BLK_ACTIVE) {

+		osalSysUnlock();

+		return HAL_FAILED;

+	}

+	lunp->state = BLK_CONNECTING;

+    osalSysUnlock();

+

+    osalMutexLock(&msdp->mtx);

+

+    USBH_DEFINE_BUFFER(union {

+    		scsi_inquiry_response_t inq;

+    		scsi_readcapacity10_response_t cap;	}, u);

+

+	uinfo("INQUIRY...");

+	res = scsi_inquiry(lunp, &u.inq);

+	if (res.tres != MSD_TRANSACTIONRESULT_OK) {

+		uerr("\tINQUIRY: Transaction error");

+		goto failed;

+	} else if (res.cres == MSD_COMMANDRESULT_FAILED) {

+		uerr("\tINQUIRY: Command Failed");

+		_requestsense(lunp);

+		goto failed;

+	} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {

+		//TODO: Do reset, etc.

+		uerr("\tINQUIRY: Command Phase Error");

+		goto failed;

+	}

+

+	uinfof("\tPDT=%02x", u.inq.peripheral & 0x1f);

+	if (u.inq.peripheral != 0) {

+		uerr("\tUnsupported PDT");

+		goto failed;

+	}

+

+	// Test if unit ready

+	uint8_t i;

+	for (i = 0; i < 10; i++) {

+		uinfo("TEST UNIT READY...");

+		res = scsi_testunitready(lunp);

+		if (res.tres != MSD_TRANSACTIONRESULT_OK) {

+			uerr("\tTEST UNIT READY: Transaction error");

+			goto failed;

+		} else if (res.cres == MSD_COMMANDRESULT_FAILED) {

+			uerr("\tTEST UNIT READY: Command Failed");

+			_requestsense(lunp);

+			continue;

+		} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {

+			//TODO: Do reset, etc.

+			uerr("\tTEST UNIT READY: Command Phase Error");

+			goto failed;

+		}

+		uinfo("\tReady.");

+		break;

+		// osalThreadSleepMilliseconds(200); // will raise 'code is unreachable' warning

+	}

+	if (i == 10) goto failed;

+

+	// Read capacity

+	uinfo("READ CAPACITY(10)...");

+	res = scsi_readcapacity10(lunp, &u.cap);

+	if (res.tres != MSD_TRANSACTIONRESULT_OK) {

+		uerr("\tREAD CAPACITY(10): Transaction error");

+		goto failed;

+	} else if (res.cres == MSD_COMMANDRESULT_FAILED) {

+		uerr("\tREAD CAPACITY(10): Command Failed");

+		_requestsense(lunp);

+		goto failed;

+	} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {

+		//TODO: Do reset, etc.

+		uerr("\tREAD CAPACITY(10): Command Phase Error");

+		goto failed;

+	}

+	lunp->info.blk_size = __REV(u.cap.block_size);

+	lunp->info.blk_num = __REV(u.cap.last_block_addr) + 1;

+	uinfof("\tBlock size=%dbytes, blocks=%u (~%u MB)", lunp->info.blk_size, lunp->info.blk_num,

+		(uint32_t)(((uint64_t)lunp->info.blk_size * lunp->info.blk_num) / (1024UL * 1024UL)));

+

+	uinfo("MSD Connected.");

+

+	osalMutexUnlock(&msdp->mtx);

+	osalSysLock();

+	lunp->state = BLK_READY;

+    osalSysUnlock();

+

+	return HAL_SUCCESS;

+

+  /* Connection failed, state reset to BLK_ACTIVE.*/

+failed:

+	osalMutexUnlock(&msdp->mtx);

+	osalSysLock();

+	lunp->state = BLK_ACTIVE;

+    osalSysUnlock();

+	return HAL_FAILED;

+}

+

+

+bool usbhmsdLUNDisconnect(USBHMassStorageLUNDriver *lunp) {

+	osalDbgCheck(lunp != NULL);

+	osalSysLock();

+	osalDbgAssert((lunp->state == BLK_ACTIVE) || (lunp->state == BLK_READY),

+				"invalid state");

+	if (lunp->state == BLK_ACTIVE) {

+		osalSysUnlock();

+		return HAL_SUCCESS;

+	}

+	lunp->state = BLK_DISCONNECTING;

+	osalSysUnlock();

+

+	//TODO: complete

+

+	osalSysLock();

+	lunp->state = BLK_ACTIVE;

+	osalSysUnlock();

+	return HAL_SUCCESS;

+}

+

+bool usbhmsdLUNRead(USBHMassStorageLUNDriver *lunp, uint32_t startblk,

+                uint8_t *buffer, uint32_t n) {

+

+	osalDbgCheck(lunp != NULL);

+	bool ret = HAL_FAILED;

+	uint16_t blocks;

+	msd_result_t res;

+

+	osalSysLock();

+	if (lunp->state != BLK_READY) {

+		osalSysUnlock();

+		return ret;

+	}

+	lunp->state = BLK_READING;

+    osalSysUnlock();

+

+	osalMutexLock(&lunp->msdp->mtx);

+	while (n) {

+		if (n > 0xffff) {

+			blocks = 0xffff;

+		} else {

+			blocks = (uint16_t)n;

+		}

+		res = scsi_read10(lunp, startblk, blocks, buffer);

+		if (res.tres != MSD_TRANSACTIONRESULT_OK) {

+			uerr("\tREAD (10): Transaction error");

+			goto exit;

+		} else if (res.cres == MSD_COMMANDRESULT_FAILED) {

+			//TODO: request sense, and act appropriately

+			uerr("\tREAD (10): Command Failed");

+			_requestsense(lunp);

+			goto exit;

+		} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {

+			//TODO: Do reset, etc.

+			uerr("\tREAD (10): Command Phase Error");

+			goto exit;

+		}

+		n -= blocks;

+		startblk += blocks;

+		buffer += blocks * lunp->info.blk_size;

+	}

+

+	ret = HAL_SUCCESS;

+

+exit:

+	osalMutexUnlock(&lunp->msdp->mtx);

+	osalSysLock();

+	if (lunp->state == BLK_READING) {

+		lunp->state = BLK_READY;

+	} else {

+		osalDbgCheck(lunp->state == BLK_STOP);

+		uwarn("MSD: State = BLK_STOP");

+	}

+    osalSysUnlock();

+	return ret;

+}

+

+bool usbhmsdLUNWrite(USBHMassStorageLUNDriver *lunp, uint32_t startblk,

+                const uint8_t *buffer, uint32_t n) {

+

+	osalDbgCheck(lunp != NULL);

+	bool ret = HAL_FAILED;

+	uint16_t blocks;

+	msd_result_t res;

+

+	osalSysLock();

+	if (lunp->state != BLK_READY) {

+		osalSysUnlock();

+		return ret;

+	}

+	lunp->state = BLK_WRITING;

+    osalSysUnlock();

+

+	osalMutexLock(&lunp->msdp->mtx);

+	while (n) {

+		if (n > 0xffff) {

+			blocks = 0xffff;

+		} else {

+			blocks = (uint16_t)n;

+		}

+		res = scsi_write10(lunp, startblk, blocks, buffer);

+		if (res.tres != MSD_TRANSACTIONRESULT_OK) {

+			uerr("\tWRITE (10): Transaction error");

+			goto exit;

+		} else if (res.cres == MSD_COMMANDRESULT_FAILED) {

+			//TODO: request sense, and act appropriately

+			uerr("\tWRITE (10): Command Failed");

+			_requestsense(lunp);

+			goto exit;

+		} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {

+			//TODO: Do reset, etc.

+			uerr("\tWRITE (10): Command Phase Error");

+			goto exit;

+		}

+		n -= blocks;

+		startblk += blocks;

+		buffer += blocks * lunp->info.blk_size;

+	}

+

+	ret = HAL_SUCCESS;

+

+exit:

+	osalMutexUnlock(&lunp->msdp->mtx);

+	osalSysLock();

+	if (lunp->state == BLK_WRITING) {

+		lunp->state = BLK_READY;

+	} else {

+		osalDbgCheck(lunp->state == BLK_STOP);

+		uwarn("MSD: State = BLK_STOP");

+	}

+	osalSysUnlock();

+	return ret;

+}

+

+bool usbhmsdLUNSync(USBHMassStorageLUNDriver *lunp) {

+	osalDbgCheck(lunp != NULL);

+	(void)lunp;

+	//TODO: Do SCSI Sync

+	return HAL_SUCCESS;

+}

+

+bool usbhmsdLUNGetInfo(USBHMassStorageLUNDriver *lunp, BlockDeviceInfo *bdip) {

+	osalDbgCheck(lunp != NULL);

+	osalDbgCheck(bdip != NULL);

+	*bdip = lunp->info;

+	return HAL_SUCCESS;

+}

+

+bool usbhmsdLUNIsInserted(USBHMassStorageLUNDriver *lunp) {

+	osalDbgCheck(lunp != NULL);

+	blkstate_t state;

+	osalSysLock();

+	state = lunp->state;

+	osalSysUnlock();

+	return (state >= BLK_ACTIVE);

+}

+

+bool usbhmsdLUNIsProtected(USBHMassStorageLUNDriver *lunp) {

+	osalDbgCheck(lunp != NULL);

+	return FALSE;

+}

+

+void usbhmsdObjectInit(USBHMassStorageDriver *msdp) {

+	osalDbgCheck(msdp != NULL);

+	memset(msdp, 0, sizeof(*msdp));

+	msdp->info = &usbhmsdClassDriverInfo;

+	osalMutexObjectInit(&msdp->mtx);

+}

+

+#endif

diff --git a/os/hal/src/usbh/usbh_uvc.c b/os/hal/src/usbh/usbh_uvc.c
new file mode 100644
index 0000000..2fb2563
--- /dev/null
+++ b/os/hal/src/usbh/usbh_uvc.c
@@ -0,0 +1,89 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

+

+    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.

+*/

+

+#include "hal.h"

+#include "usbh.h"

+

+#if HAL_USBH_USE_UVC

+

+#if !HAL_USE_USBH

+#error "USBHUVC needs HAL_USE_USBH"

+#endif

+

+#if !HAL_USBH_USE_IAD

+#error "USBHUVC needs HAL_USBH_USE_IAD"

+#endif

+

+#if USBHUVC_DEBUG_ENABLE_TRACE

+#define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define udbgf(f, ...)  do {} while(0)

+#define udbg(f, ...)   do {} while(0)

+#endif

+

+#if USBHUVC_DEBUG_ENABLE_INFO

+#define uinfof(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define uinfo(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define uinfof(f, ...)  do {} while(0)

+#define uinfo(f, ...)   do {} while(0)

+#endif

+

+#if USBHUVC_DEBUG_ENABLE_WARNINGS

+#define uwarnf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define uwarn(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define uwarnf(f, ...)  do {} while(0)

+#define uwarn(f, ...)   do {} while(0)

+#endif

+

+#if USBHUVC_DEBUG_ENABLE_ERRORS

+#define uerrf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

+#define uerr(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

+#else

+#define uerrf(f, ...)  do {} while(0)

+#define uerr(f, ...)   do {} while(0)

+#endif

+

+

+static usbh_baseclassdriver_t *uvc_load(usbh_device_t *dev,

+		const uint8_t *descriptor, uint16_t rem);

+static void uvc_unload(usbh_baseclassdriver_t *drv);

+

+static const usbh_classdriver_vmt_t class_driver_vmt = {

+	uvc_load,

+	uvc_unload

+};

+const usbh_classdriverinfo_t usbhuvcClassDriverInfo = {

+	0x0e, 0x03, 0x00, "UVC", &class_driver_vmt

+};

+

+

+static usbh_baseclassdriver_t *uvc_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {

+	(void)dev;

+	(void)descriptor;

+	(void)rem;

+	return NULL;

+}

+

+static void uvc_unload(usbh_baseclassdriver_t *drv) {

+	(void)drv;

+}

+

+#endif

+