Merge branch 'master' into fork/subsonicpulse/master

92 files changed, 1704 insertions, 819 deletions

diff --git a/hw/bsp/board.c b/hw/bsp/board.c
index bb339f613..6d9488431 100644
--- a/hw/bsp/board.c
+++ b/hw/bsp/board.c
@@ -134,3 +134,8 @@ int board_getchar(void) {
   char c;
   return (sys_read(0, &c, 1) > 0) ? (int) c : (-1);
 }
+
+
+uint32_t tusb_time_millis_api(void) {
+  return board_millis();
+}
diff --git a/hw/bsp/board_api.h b/hw/bsp/board_api.h
index a458a3fdc..eaee1ee97 100644
--- a/hw/bsp/board_api.h
+++ b/hw/bsp/board_api.h
@@ -39,7 +39,7 @@ extern "C" {
 #include "tusb.h"
 
 #if CFG_TUSB_OS == OPT_OS_FREERTOS
-#if TUP_MCU_ESPRESSIF
+#if TUSB_MCU_VENDOR_ESPRESSIF
   // ESP-IDF need "freertos/" prefix in include path.
   // CFG_TUSB_OS_INC_PATH should be defined accordingly.
   #include "freertos/FreeRTOS.h"
diff --git a/hw/bsp/broadcom_32bit/family.cmake b/hw/bsp/broadcom_32bit/family.cmake
index 6205d4e1b..93e7d3545 100644
--- a/hw/bsp/broadcom_32bit/family.cmake
+++ b/hw/bsp/broadcom_32bit/family.cmake
@@ -97,6 +97,8 @@ function(family_configure_example TARGET RTOS)
   family_add_tinyusb(${TARGET} OPT_MCU_BCM2835 ${RTOS})
   target_sources(${TARGET}-tinyusb PUBLIC
     ${TOP}/src/portable/synopsys/dwc2/dcd_dwc2.c
+    ${TOP}/src/portable/synopsys/dwc2/hcd_dwc2.c
+    ${TOP}/src/portable/synopsys/dwc2/dwc2_common.c
     )
   target_link_libraries(${TARGET}-tinyusb PUBLIC board_${BOARD})
 
diff --git a/hw/bsp/broadcom_32bit/family.mk b/hw/bsp/broadcom_32bit/family.mk
index e15bd93f7..a282e9961 100644
--- a/hw/bsp/broadcom_32bit/family.mk
+++ b/hw/bsp/broadcom_32bit/family.mk
@@ -19,6 +19,8 @@ CFLAGS += -Wno-error=cast-qual -Wno-error=redundant-decls
 
 SRC_C += \
 	src/portable/synopsys/dwc2/dcd_dwc2.c \
+	src/portable/synopsys/dwc2/hcd_dwc2.c \
+	src/portable/synopsys/dwc2/dwc2_common.c \
 	$(MCU_DIR)/broadcom/gen/interrupt_handlers.c \
 	$(MCU_DIR)/broadcom/gpio.c \
 	$(MCU_DIR)/broadcom/interrupts.c \
diff --git a/hw/bsp/broadcom_64bit/family.cmake b/hw/bsp/broadcom_64bit/family.cmake
index f373dc633..d790944bc 100644
--- a/hw/bsp/broadcom_64bit/family.cmake
+++ b/hw/bsp/broadcom_64bit/family.cmake
@@ -104,6 +104,8 @@ function(family_configure_example TARGET RTOS)
   family_add_tinyusb(${TARGET} OPT_MCU_BCM${BCM_VERSION} ${RTOS})
   target_sources(${TARGET}-tinyusb PUBLIC
     ${TOP}/src/portable/synopsys/dwc2/dcd_dwc2.c
+    ${TOP}/src/portable/synopsys/dwc2/hcd_dwc2.c
+    ${TOP}/src/portable/synopsys/dwc2/dwc2_common.c
     )
   target_link_libraries(${TARGET}-tinyusb PUBLIC board_${BOARD})
 
diff --git a/hw/bsp/broadcom_64bit/family.mk b/hw/bsp/broadcom_64bit/family.mk
index 89f798f19..37d381f9f 100644
--- a/hw/bsp/broadcom_64bit/family.mk
+++ b/hw/bsp/broadcom_64bit/family.mk
@@ -18,6 +18,8 @@ CFLAGS += -Wno-error=cast-qual -Wno-error=redundant-decls
 
 SRC_C += \
 	src/portable/synopsys/dwc2/dcd_dwc2.c \
+	src/portable/synopsys/dwc2/hcd_dwc2.c \
+	src/portable/synopsys/dwc2/dwc2_common.c \
 	$(MCU_DIR)/broadcom/gen/interrupt_handlers.c \
 	$(MCU_DIR)/broadcom/gpio.c \
 	$(MCU_DIR)/broadcom/interrupts.c \
diff --git a/hw/bsp/espressif/boards/espressif_p4_function_ev/board.h b/hw/bsp/espressif/boards/espressif_p4_function_ev/board.h
index 9c8aa409f..0022f0d2d 100644
--- a/hw/bsp/espressif/boards/espressif_p4_function_ev/board.h
+++ b/hw/bsp/espressif/boards/espressif_p4_function_ev/board.h
@@ -36,6 +36,10 @@
 #define BUTTON_PIN            0
 #define BUTTON_STATE_ACTIVE   0
 
+// For CI hardware test, to test both device and host on the same HS port with help of
+#define HIL_DEVICE_HOST_MUX_PIN  47
+#define HIL_DEVICE_STATE         1
+
 #ifdef __cplusplus
  }
 #endif
diff --git a/hw/bsp/espressif/boards/family.c b/hw/bsp/espressif/boards/family.c
index a1b4334b2..729b3539d 100644
--- a/hw/bsp/espressif/boards/family.c
+++ b/hw/bsp/espressif/boards/family.c
@@ -105,6 +105,12 @@ void board_init(void) {
   usb_init();
 #endif
 
+#ifdef HIL_DEVICE_HOST_MUX_PIN
+  gpio_reset_pin(HIL_DEVICE_HOST_MUX_PIN);
+  gpio_set_direction(HIL_DEVICE_HOST_MUX_PIN, GPIO_MODE_OUTPUT);
+  gpio_set_level(HIL_DEVICE_HOST_MUX_PIN, CFG_TUD_ENABLED ? HIL_DEVICE_STATE : (1-HIL_DEVICE_STATE));
+#endif
+
 #if CFG_TUH_ENABLED && CFG_TUH_MAX3421
   max3421_init();
 #endif
@@ -175,7 +181,15 @@ bool usb_init(void) {
   usb_phy_config_t phy_conf = {
     .controller = USB_PHY_CTRL_OTG,
     .target = USB_PHY_TARGET_INT,
+
+    // maybe we can use USB_OTG_MODE_DEFAULT and switch using dwc2 driver
+#if CFG_TUD_ENABLED
     .otg_mode = USB_OTG_MODE_DEVICE,
+    .otg_speed = BOARD_TUD_RHPORT ? USB_PHY_SPEED_HIGH : USB_PHY_SPEED_FULL,
+#elif CFG_TUH_ENABLED
+    .otg_mode = USB_OTG_MODE_HOST,
+    .otg_speed= BOARD_TUH_RHPORT ? USB_PHY_SPEED_HIGH : USB_PHY_SPEED_FULL,
+#endif
   };
 
   // OTG IOs config
diff --git a/hw/bsp/espressif/components/tinyusb_src/CMakeLists.txt b/hw/bsp/espressif/components/tinyusb_src/CMakeLists.txt
index 26c7e4030..900f620fd 100644
--- a/hw/bsp/espressif/components/tinyusb_src/CMakeLists.txt
+++ b/hw/bsp/espressif/components/tinyusb_src/CMakeLists.txt
@@ -9,9 +9,10 @@ string(TOUPPER OPT_MCU_${target} tusb_mcu)
 list(APPEND compile_definitions
   CFG_TUSB_MCU=${tusb_mcu}
   CFG_TUSB_OS=OPT_OS_FREERTOS
-  # EXAMPLE port selection: port0 is fullspeed, port1 is highspeed
-  BOARD_TUD_RHPORT=${TUD_PORT}
-  BOARD_TUD_MAX_SPEED=$<IF:${TUD_PORT},OPT_MODE_HIGH_SPEED,OPT_MODE_FULL_SPEED>
+  BOARD_TUD_RHPORT=${RHPORT_DEVICE}
+  BOARD_TUD_MAX_SPEED=${RHPORT_DEVICE_SPEED}
+  BOARD_TUH_RHPORT=${RHPORT_HOST}
+  BOARD_TUH_MAX_SPEED=${RHPORT_HOST_SPEED}
   )
 
 list(APPEND srcs
@@ -34,6 +35,8 @@ list(APPEND srcs
   ${tusb_src}/class/vendor/vendor_device.c
   ${tusb_src}/class/video/video_device.c
   ${tusb_src}/portable/synopsys/dwc2/dcd_dwc2.c
+  ${tusb_src}/portable/synopsys/dwc2/hcd_dwc2.c
+  ${tusb_src}/portable/synopsys/dwc2/dwc2_common.c
   # host
   ${tusb_src}/host/usbh.c
   ${tusb_src}/host/hub.c
@@ -57,6 +60,11 @@ if (DEFINED LOG)
   endif ()
 endif()
 
+if(DEFINED CFLAGS_CLI)
+  list(APPEND compile_definitions ${CFLAGS_CLI})
+endif()
+
+
 idf_component_register(SRCS ${srcs}
   INCLUDE_DIRS ${tusb_src}
   REQUIRES src
diff --git a/hw/bsp/espressif/family.cmake b/hw/bsp/espressif/family.cmake
index ada082c76..daa12cdb4 100644
--- a/hw/bsp/espressif/family.cmake
+++ b/hw/bsp/espressif/family.cmake
@@ -5,14 +5,31 @@ include("${CMAKE_CURRENT_LIST_DIR}/boards/${BOARD}/board.cmake")
 string(TOUPPER ${IDF_TARGET} FAMILY_MCUS)
 
 # Device port default to Port1 for P4 (highspeed), Port0 for others (fullspeed)
-if (NOT DEFINED TUD_PORT)
+set(RHPORT_SPEED OPT_MODE_FULL_SPEED OPT_MODE_HIGH_SPEED)
+
+if (NOT DEFINED RHPORT_DEVICE)
   if (IDF_TARGET STREQUAL "esp32p4")
-    set(TUD_PORT 1)
+    set(RHPORT_DEVICE 1)
   else ()
-    set(TUD_PORT 0)
+    set(RHPORT_DEVICE 0)
   endif ()
 endif()
 
+if (NOT DEFINED RHPORT_HOST)
+  if (IDF_TARGET STREQUAL "esp32p4")
+    set(RHPORT_HOST 1)
+  else ()
+    set(RHPORT_HOST 0)
+  endif ()
+endif()
+
+if (NOT DEFINED RHPORT_DEVICE_SPEED)
+  list(GET RHPORT_SPEED ${RHPORT_DEVICE} RHPORT_DEVICE_SPEED)
+endif ()
+if (NOT DEFINED RHPORT_HOST_SPEED)
+  list(GET RHPORT_SPEED ${RHPORT_HOST} RHPORT_HOST_SPEED)
+endif ()
+
 # Add example src and bsp directories
 set(EXTRA_COMPONENT_DIRS "src" "${CMAKE_CURRENT_LIST_DIR}/boards" "${CMAKE_CURRENT_LIST_DIR}/components")
 
diff --git a/hw/bsp/gd32vf103/family.cmake b/hw/bsp/gd32vf103/family.cmake
index 5f4a3da8d..1441e41de 100644
--- a/hw/bsp/gd32vf103/family.cmake
+++ b/hw/bsp/gd32vf103/family.cmake
@@ -108,6 +108,8 @@ function(family_configure_example TARGET RTOS)
   family_add_tinyusb(${TARGET} OPT_MCU_GD32VF103 ${RTOS})
   target_sources(${TARGET}-tinyusb PUBLIC
     ${TOP}/src/portable/synopsys/dwc2/dcd_dwc2.c
+    ${TOP}/src/portable/synopsys/dwc2/hcd_dwc2.c
+    ${TOP}/src/portable/synopsys/dwc2/dwc2_common.c
     )
   target_link_libraries(${TARGET}-tinyusb PUBLIC board_${BOARD})
 
diff --git a/hw/bsp/gd32vf103/family.mk b/hw/bsp/gd32vf103/family.mk
index 48588886c..75b3efb33 100644
--- a/hw/bsp/gd32vf103/family.mk
+++ b/hw/bsp/gd32vf103/family.mk
@@ -31,6 +31,8 @@ CFLAGS += -Wno-error=unused-parameter
 
 SRC_C += \
 	src/portable/synopsys/dwc2/dcd_dwc2.c \
+	src/portable/synopsys/dwc2/hcd_dwc2.c \
+	src/portable/synopsys/dwc2/dwc2_common.c \
 	$(GD32VF103_SDK_DRIVER)/gd32vf103_gpio.c \
 	$(GD32VF103_SDK_DRIVER)/gd32vf103_rcu.c \
 	$(GD32VF103_SDK_DRIVER)/gd32vf103_usart.c \
diff --git a/hw/bsp/sltb009a/board.mk b/hw/bsp/sltb009a/board.mk
index a04bc19d8..687761364 100644
--- a/hw/bsp/sltb009a/board.mk
+++ b/hw/bsp/sltb009a/board.mk
@@ -26,7 +26,9 @@ LD_FILE = $(SILABS_CMSIS)/Source/GCC/$(SILABS_FAMILY).ld
 
 SRC_C += \
   $(SILABS_CMSIS)/Source/system_$(SILABS_FAMILY).c \
-	src/portable/synopsys/dwc2/dcd_dwc2.c
+	src/portable/synopsys/dwc2/dcd_dwc2.c \
+	src/portable/synopsys/dwc2/hcd_dwc2.c \
+	src/portable/synopsys/dwc2/dwc2_common.c \
 
 SRC_S += \
   $(SILABS_CMSIS)/Source/GCC/startup_$(SILABS_FAMILY).S
diff --git a/hw/bsp/stm32f2/family.cmake b/hw/bsp/stm32f2/family.cmake
index a01ebef5c..538a6cd66 100644
--- a/hw/bsp/stm32f2/family.cmake
+++ b/hw/bsp/stm32f2/family.cmake
@@ -102,6 +102,8 @@ function(family_configure_example TARGET RTOS)
   family_add_tinyusb(${TARGET} OPT_MCU_STM32F2 ${RTOS})
   target_sources(${TARGET}-tinyusb PUBLIC
     ${TOP}/src/portable/synopsys/dwc2/dcd_dwc2.c
+    ${TOP}/src/portable/synopsys/dwc2/hcd_dwc2.c
+    ${TOP}/src/portable/synopsys/dwc2/dwc2_common.c
     )
   target_link_libraries(${TARGET}-tinyusb PUBLIC board_${BOARD})
 
diff --git a/hw/bsp/stm32f2/family.mk b/hw/bsp/stm32f2/family.mk
index 7e71f6eed..7af9a76a0 100644
--- a/hw/bsp/stm32f2/family.mk
+++ b/hw/bsp/stm32f2/family.mk
@@ -26,6 +26,8 @@ LDFLAGS_GCC += \
 
 SRC_C += \
   src/portable/synopsys/dwc2/dcd_dwc2.c \
+  src/portable/synopsys/dwc2/hcd_dwc2.c \
+  src/portable/synopsys/dwc2/dwc2_common.c \
   $(ST_CMSIS)/Source/Templates/system_stm32$(ST_FAMILY)xx.c \
   $(ST_HAL_DRIVER)/Src/stm32$(ST_FAMILY)xx_hal.c \
   $(ST_HAL_DRIVER)/Src/stm32$(ST_FAMILY)xx_hal_cortex.c \
diff --git a/hw/bsp/stm32f4/boards/feather_stm32f405/board.cmake b/hw/bsp/stm32f4/boards/feather_stm32f405/board.cmake
index fff6c502d..4910d3a88 100644
--- a/hw/bsp/stm32f4/boards/feather_stm32f405/board.cmake
+++ b/hw/bsp/stm32f4/boards/feather_stm32f405/board.cmake
@@ -6,6 +6,5 @@ set(LD_FILE_GNU ${CMAKE_CURRENT_LIST_DIR}/STM32F405RGTx_FLASH.ld)
 function(update_board TARGET)
   target_compile_definitions(${TARGET} PUBLIC
     STM32F405xx
-    BOARD_TUD_RHPORT=0
     )
 endfunction()
diff --git a/hw/bsp/stm32f4/boards/feather_stm32f405/board.h b/hw/bsp/stm32f4/boards/feather_stm32f405/board.h
index d1ad2a6ce..670ce8012 100644
--- a/hw/bsp/stm32f4/boards/feather_stm32f405/board.h
+++ b/hw/bsp/stm32f4/boards/feather_stm32f405/board.h
@@ -31,22 +31,35 @@
  extern "C" {
 #endif
 
-// LED
-#define LED_PORT              GPIOC
-#define LED_PIN               GPIO_PIN_1
-#define LED_STATE_ON          1
+#define UART_DEV              USART3
 
-// Button: Pin D5
-#define BUTTON_PORT           GPIOC
-#define BUTTON_PIN            GPIO_PIN_7
-#define BUTTON_STATE_ACTIVE   0
+#define PINID_LED      0
+#define PINID_BUTTON   1
+#define PINID_UART_TX  2
+#define PINID_UART_RX  3
 
-// UART
-#define UART_DEV              USART3
-#define UART_GPIO_PORT        GPIOB
-#define UART_GPIO_AF          GPIO_AF7_USART3
-#define UART_TX_PIN           GPIO_PIN_10
-#define UART_RX_PIN           GPIO_PIN_11
+static board_pindef_t board_pindef[] = {
+  { // LED
+    .port = GPIOC,
+    .pin_init = { .Pin = GPIO_PIN_1, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // BUTTON
+    .port = GPIOC,
+    .pin_init = { .Pin = GPIO_PIN_7, .Mode = GPIO_MODE_INPUT, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 0
+  },
+  { // UART TX
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_10, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  },
+  { // UART RX
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_11, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  },
+};
 
 //--------------------------------------------------------------------+
 // RCC Clock
@@ -84,17 +97,20 @@ static inline void board_clock_init(void)
   RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
   HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5);
 
-  // Enable clocks for LED, Button, Uart
-  __HAL_RCC_GPIOB_CLK_ENABLE();
-  __HAL_RCC_GPIOC_CLK_ENABLE();
+  // Enable clocks for Uart
   __HAL_RCC_USART3_CLK_ENABLE();
 }
 
-static inline void board_vbus_sense_init(void)
-{
-  // Enable VBUS sense (B device) via pin PA9
-  USB_OTG_FS->GCCFG &= ~USB_OTG_GCCFG_NOVBUSSENS;
-  USB_OTG_FS->GCCFG |= USB_OTG_GCCFG_VBUSBSEN;
+static inline void board_vbus_sense_init(uint8_t rhport) {
+  if  (rhport == 0) {
+    // Enable VBUS sense (B device) via pin PA9
+    USB_OTG_FS->GCCFG &= ~USB_OTG_GCCFG_NOVBUSSENS;
+    USB_OTG_FS->GCCFG |= USB_OTG_GCCFG_VBUSBSEN;
+  }
+}
+
+static inline void board_vbus_set(uint8_t rhport, bool state) {
+  (void) rhport; (void) state;
 }
 
 #ifdef __cplusplus
diff --git a/hw/bsp/stm32f4/boards/pyboardv11/board.cmake b/hw/bsp/stm32f4/boards/pyboardv11/board.cmake
index fff6c502d..4910d3a88 100644
--- a/hw/bsp/stm32f4/boards/pyboardv11/board.cmake
+++ b/hw/bsp/stm32f4/boards/pyboardv11/board.cmake
@@ -6,6 +6,5 @@ set(LD_FILE_GNU ${CMAKE_CURRENT_LIST_DIR}/STM32F405RGTx_FLASH.ld)
 function(update_board TARGET)
   target_compile_definitions(${TARGET} PUBLIC
     STM32F405xx
-    BOARD_TUD_RHPORT=0
     )
 endfunction()
diff --git a/hw/bsp/stm32f4/boards/pyboardv11/board.h b/hw/bsp/stm32f4/boards/pyboardv11/board.h
index c126f0666..077313518 100644
--- a/hw/bsp/stm32f4/boards/pyboardv11/board.h
+++ b/hw/bsp/stm32f4/boards/pyboardv11/board.h
@@ -31,22 +31,35 @@
  extern "C" {
 #endif
 
-// Blue LED is chosen because the other LEDs are connected to ST-LINK lines.
-#define LED_PORT              GPIOB
-#define LED_PIN               GPIO_PIN_4
-#define LED_STATE_ON          1
+#define UART_DEV              USART2
 
-#define BUTTON_PORT           GPIOB
-#define BUTTON_PIN            GPIO_PIN_3
-#define BUTTON_STATE_ACTIVE   1
+#define PINID_LED      0
+#define PINID_BUTTON   1
+#define PINID_UART_TX  2
+#define PINID_UART_RX  3
 
-// Enable PA2 as the debug log UART
-// It is not routed to the ST/Link on the Discovery board.
-//#define UART_DEV              USART2
-//#define UART_GPIO_PORT        GPIOA
-//#define UART_GPIO_AF          GPIO_AF7_USART2
-//#define UART_TX_PIN           GPIO_PIN_2
-//#define UART_RX_PIN           GPIO_PIN_3
+static board_pindef_t board_pindef[] = {
+  { // LED
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_4, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // BUTTON
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_3, .Mode = GPIO_MODE_INPUT, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // UART TX
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_2, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART2 },
+    .active_state = 0
+  },
+  { // UART RX
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_3, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART2 },
+    .active_state = 0
+  },
+};
 
 //--------------------------------------------------------------------+
 // RCC Clock
@@ -84,15 +97,20 @@ static inline void board_clock_init(void)
   RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
   HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5);
 
-  // Enable clocks for LED, Button, Uart
-  __HAL_RCC_GPIOB_CLK_ENABLE();
+  // Enable clocks for Uart
+  __HAL_RCC_USART2_CLK_ENABLE();
 }
 
-static inline void board_vbus_sense_init(void)
-{
-  // Enable VBUS sense (B device) via pin PA9
-  USB_OTG_FS->GCCFG &= ~USB_OTG_GCCFG_NOVBUSSENS;
-  USB_OTG_FS->GCCFG |= USB_OTG_GCCFG_VBUSBSEN;
+static inline void board_vbus_sense_init(uint8_t rhport) {
+  if  (rhport == 0) {
+    // Enable VBUS sense (B device) via pin PA9
+    USB_OTG_FS->GCCFG &= ~USB_OTG_GCCFG_NOVBUSSENS;
+    USB_OTG_FS->GCCFG |= USB_OTG_GCCFG_VBUSBSEN;
+  }
+}
+
+static inline void board_vbus_set(uint8_t rhport, bool state) {
+  (void) rhport; (void) state;
 }
 
 #ifdef __cplusplus
diff --git a/hw/bsp/stm32f4/boards/stm32f401blackpill/board.cmake b/hw/bsp/stm32f4/boards/stm32f401blackpill/board.cmake
index bf2bef38b..fab6a42d2 100644
--- a/hw/bsp/stm32f4/boards/stm32f401blackpill/board.cmake
+++ b/hw/bsp/stm32f4/boards/stm32f401blackpill/board.cmake
@@ -6,6 +6,5 @@ set(LD_FILE_GNU ${CMAKE_CURRENT_LIST_DIR}/STM32F401VCTx_FLASH.ld)
 function(update_board TARGET)
   target_compile_definitions(${TARGET} PUBLIC
     STM32F405xx
-    BOARD_TUD_RHPORT=0
     )
 endfunction()
diff --git a/hw/bsp/stm32f4/boards/stm32f401blackpill/board.h b/hw/bsp/stm32f4/boards/stm32f401blackpill/board.h
index e6c99a462..ef40089c9 100644
--- a/hw/bsp/stm32f4/boards/stm32f401blackpill/board.h
+++ b/hw/bsp/stm32f4/boards/stm32f401blackpill/board.h
@@ -31,23 +31,36 @@
  extern "C" {
 #endif
 
-// LED
-#define LED_PORT              GPIOC
-#define LED_PIN               GPIO_PIN_13
-#define LED_STATE_ON          0
-
-// Button
-#define BUTTON_PORT           GPIOA
-#define BUTTON_PIN            GPIO_PIN_0
-#define BUTTON_STATE_ACTIVE   0
-
 // Enable PA2 as the debug log UART
-//#define UART_DEV              USART2
-//#define UART_GPIO_PORT        GPIOA
-//#define UART_GPIO_AF          GPIO_AF7_USART2
-//#define UART_TX_PIN           GPIO_PIN_2
-//#define UART_RX_PIN           GPIO_PIN_3
+#define UART_DEV              USART2
+
+#define PINID_LED      0
+#define PINID_BUTTON   1
+#define PINID_UART_TX  2
+#define PINID_UART_RX  3
 
+static board_pindef_t board_pindef[] = {
+  { // LED
+    .port = GPIOC,
+    .pin_init = { .Pin = GPIO_PIN_13, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 0
+  },
+  { // BUTTON
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_0, .Mode = GPIO_MODE_INPUT, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 0
+  },
+  { // UART TX
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_2, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART2 },
+    .active_state = 0
+  },
+  { // UART RX
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_3, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART2 },
+    .active_state = 0
+  },
+};
 
 //--------------------------------------------------------------------+
 // RCC Clock
@@ -85,18 +98,21 @@ static inline void board_clock_init(void)
   RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
   HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2);
 
-  // Enable clocks for LED, Button, Uart
-  __HAL_RCC_GPIOA_CLK_ENABLE();
-  __HAL_RCC_GPIOC_CLK_ENABLE();
-  //__HAL_RCC_USART2_CLK_ENABLE();
+  // Enable clocks for Uart
+  __HAL_RCC_USART2_CLK_ENABLE();
 }
 
-static inline void board_vbus_sense_init(void)
-{
+static inline void board_vbus_sense_init(uint8_t rhport) {
   // Blackpill doesn't use VBUS sense (B device) explicitly disable it
-  USB_OTG_FS->GCCFG |= USB_OTG_GCCFG_NOVBUSSENS;
-  USB_OTG_FS->GCCFG &= ~USB_OTG_GCCFG_VBUSBSEN;
-  USB_OTG_FS->GCCFG &= ~USB_OTG_GCCFG_VBUSASEN;
+  if  (rhport == 0) {
+    USB_OTG_FS->GCCFG |= USB_OTG_GCCFG_NOVBUSSENS;
+    USB_OTG_FS->GCCFG &= ~USB_OTG_GCCFG_VBUSBSEN;
+    USB_OTG_FS->GCCFG &= ~USB_OTG_GCCFG_VBUSASEN;
+  }
+}
+
+static inline void board_vbus_set(uint8_t rhport, bool state) {
+  (void) rhport; (void) state;
 }
 
 #ifdef __cplusplus
diff --git a/hw/bsp/stm32f4/boards/stm32f407blackvet/board.cmake b/hw/bsp/stm32f4/boards/stm32f407blackvet/board.cmake
index 64626d7bd..50e6b1592 100644
--- a/hw/bsp/stm32f4/boards/stm32f407blackvet/board.cmake
+++ b/hw/bsp/stm32f4/boards/stm32f407blackvet/board.cmake
@@ -7,7 +7,5 @@ function(update_board TARGET)
   target_compile_definitions(${TARGET} PUBLIC
     STM32F407xx
     HSE_VALUE=8000000
-    BOARD_TUD_RHPORT=0
-    BOARD_TUD_MAX_SPEED=OPT_MODE_FULL_SPEED
     )
 endfunction()
diff --git a/hw/bsp/stm32f4/boards/stm32f407blackvet/board.h b/hw/bsp/stm32f4/boards/stm32f407blackvet/board.h
index b7a6d96c2..6879d066b 100644
--- a/hw/bsp/stm32f4/boards/stm32f407blackvet/board.h
+++ b/hw/bsp/stm32f4/boards/stm32f407blackvet/board.h
@@ -31,24 +31,36 @@
  extern "C" {
 #endif
 
-// LED
-#define LED_PORT              GPIOA
-#define LED_PIN               GPIO_PIN_6
-#define LED_STATE_ON          1
-
-// Button
-#define BUTTON_PORT           GPIOE
-#define BUTTON_PIN            GPIO_PIN_4
-#define BUTTON_STATE_ACTIVE   0
-
 // Enable PA2 as the debug log UART
-// It is not routed to the ST/Link on the Discovery board.
 #define UART_DEV              USART2
-#define UART_GPIO_PORT        GPIOA
-#define UART_GPIO_AF          GPIO_AF7_USART2
-#define UART_TX_PIN           GPIO_PIN_2
-#define UART_RX_PIN           GPIO_PIN_3
 
+#define PINID_LED      0
+#define PINID_BUTTON   1
+#define PINID_UART_TX  2
+#define PINID_UART_RX  3
+
+static board_pindef_t board_pindef[] = {
+  { // LED
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_6, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // BUTTON
+    .port = GPIOE,
+    .pin_init = { .Pin = GPIO_PIN_4, .Mode = GPIO_MODE_INPUT, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 0
+  },
+  { // UART TX
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_2, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART2 },
+    .active_state = 0
+  },
+  { // UART RX
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_3, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART2 },
+    .active_state = 0
+  },
+};
 //--------------------------------------------------------------------+
 // RCC Clock
 //--------------------------------------------------------------------+
@@ -86,18 +98,20 @@ static inline void board_clock_init(void)
   HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5);
 
   // Enable clocks for LED, Button, Uart
-  __HAL_RCC_GPIOA_CLK_ENABLE();
-  __HAL_RCC_GPIOE_CLK_ENABLE();
-  __HAL_RCC_GPIOH_CLK_ENABLE();
   __HAL_RCC_USART2_CLK_ENABLE();
 }
 
-static inline void board_vbus_sense_init(void)
-{
+static inline void board_vbus_sense_init(uint8_t rhport) {
+  if (rhport == 0) {
     // Black F407VET6 doesn't use VBUS sense (B device) explicitly disable it
     USB_OTG_FS->GCCFG |= USB_OTG_GCCFG_NOVBUSSENS;
     USB_OTG_FS->GCCFG &= ~USB_OTG_GCCFG_VBUSBSEN;
     USB_OTG_FS->GCCFG &= ~USB_OTG_GCCFG_VBUSASEN;
+  }
+}
+
+static inline void board_vbus_set(uint8_t rhport, bool state) {
+  (void) rhport; (void) state;
 }
 
 #ifdef __cplusplus
diff --git a/hw/bsp/stm32f4/boards/stm32f407disco/board.cmake b/hw/bsp/stm32f4/boards/stm32f407disco/board.cmake
index b2514dc5e..c8f0330ed 100644
--- a/hw/bsp/stm32f4/boards/stm32f407disco/board.cmake
+++ b/hw/bsp/stm32f4/boards/stm32f407disco/board.cmake
@@ -6,6 +6,5 @@ set(LD_FILE_GNU ${CMAKE_CURRENT_LIST_DIR}/STM32F407VGTx_FLASH.ld)
 function(update_board TARGET)
   target_compile_definitions(${TARGET} PUBLIC
     STM32F407xx
-    BOARD_TUD_RHPORT=0
     )
 endfunction()
diff --git a/hw/bsp/stm32f4/boards/stm32f407disco/board.h b/hw/bsp/stm32f4/boards/stm32f407disco/board.h
index c38f354ce..380f8e391 100644
--- a/hw/bsp/stm32f4/boards/stm32f407disco/board.h
+++ b/hw/bsp/stm32f4/boards/stm32f407disco/board.h
@@ -31,23 +31,43 @@
  extern "C" {
 #endif
 
-// LED
-#define LED_PORT              GPIOD
-#define LED_PIN               GPIO_PIN_14
-#define LED_STATE_ON          1
-
-// Button
-#define BUTTON_PORT           GPIOA
-#define BUTTON_PIN            GPIO_PIN_0
-#define BUTTON_STATE_ACTIVE   1
-
 // Enable PA2 as the debug log UART
 // It is not routed to the ST/Link on the Discovery board.
 #define UART_DEV              USART2
-#define UART_GPIO_PORT        GPIOA
-#define UART_GPIO_AF          GPIO_AF7_USART2
-#define UART_TX_PIN           GPIO_PIN_2
-#define UART_RX_PIN           GPIO_PIN_3
+
+#define PINID_LED      0
+#define PINID_BUTTON   1
+#define PINID_UART_TX  2
+#define PINID_UART_RX  3
+#define PINID_VBUS0_EN 4
+
+static board_pindef_t board_pindef[] = {
+  { // LED
+    .port = GPIOD,
+    .pin_init = { .Pin = GPIO_PIN_14, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // BUTTON
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_0, .Mode = GPIO_MODE_INPUT, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // UART TX
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_2, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART2 },
+    .active_state = 0
+  },
+  { // UART RX
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_3, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART2 },
+    .active_state = 0
+  },
+  { // VBUS0 EN
+    .port = GPIOC,
+    .pin_init = { .Pin = GPIO_PIN_0, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_NOPULL, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 0
+  }
+};
 
 //--------------------------------------------------------------------+
 // RCC Clock
@@ -85,17 +105,23 @@ static inline void board_clock_init(void)
   RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
   HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5);
 
-  // Enable clocks for LED, Button, Uart
-  __HAL_RCC_GPIOA_CLK_ENABLE();
-  __HAL_RCC_GPIOD_CLK_ENABLE();
+  // Enable clocks Uart
   __HAL_RCC_USART2_CLK_ENABLE();
 }
 
-static inline void board_vbus_sense_init(void)
-{
-  // Enable VBUS sense (B device) via pin PA9
-  USB_OTG_FS->GCCFG &= ~USB_OTG_GCCFG_NOVBUSSENS;
-  USB_OTG_FS->GCCFG |= USB_OTG_GCCFG_VBUSBSEN;
+static inline void board_vbus_sense_init(uint8_t rhport) {
+  if (rhport == 0) {
+    // Enable VBUS sense (B device) via pin PA9
+    USB_OTG_FS->GCCFG &= ~USB_OTG_GCCFG_NOVBUSSENS;
+    USB_OTG_FS->GCCFG |= USB_OTG_GCCFG_VBUSBSEN;
+  }
+}
+
+static inline void board_vbus_set(uint8_t rhport, bool state) {
+  if (rhport == 0) {
+    board_pindef_t* pindef = &board_pindef[PINID_VBUS0_EN];
+    HAL_GPIO_WritePin(pindef->port, pindef->pin_init.Pin, state == pindef->active_state ? GPIO_PIN_SET : GPIO_PIN_RESET);
+  }
 }
 
 #ifdef __cplusplus
diff --git a/hw/bsp/stm32f4/boards/stm32f411blackpill/board.cmake b/hw/bsp/stm32f4/boards/stm32f411blackpill/board.cmake
index 185507d7f..d16db508f 100644
--- a/hw/bsp/stm32f4/boards/stm32f411blackpill/board.cmake
+++ b/hw/bsp/stm32f4/boards/stm32f411blackpill/board.cmake
@@ -6,6 +6,5 @@ set(LD_FILE_GNU ${CMAKE_CURRENT_LIST_DIR}/STM32F411CEUx_FLASH.ld)
 function(update_board TARGET)
   target_compile_definitions(${TARGET} PUBLIC
     STM32F411xE
-    BOARD_TUD_RHPORT=0
     )
 endfunction()
diff --git a/hw/bsp/stm32f4/boards/stm32f411blackpill/board.h b/hw/bsp/stm32f4/boards/stm32f411blackpill/board.h
index e6c99a462..efa618b72 100644
--- a/hw/bsp/stm32f4/boards/stm32f411blackpill/board.h
+++ b/hw/bsp/stm32f4/boards/stm32f411blackpill/board.h
@@ -31,23 +31,35 @@
  extern "C" {
 #endif
 
-// LED
-#define LED_PORT              GPIOC
-#define LED_PIN               GPIO_PIN_13
-#define LED_STATE_ON          0
+#define UART_DEV              USART2
 
-// Button
-#define BUTTON_PORT           GPIOA
-#define BUTTON_PIN            GPIO_PIN_0
-#define BUTTON_STATE_ACTIVE   0
-
-// Enable PA2 as the debug log UART
-//#define UART_DEV              USART2
-//#define UART_GPIO_PORT        GPIOA
-//#define UART_GPIO_AF          GPIO_AF7_USART2
-//#define UART_TX_PIN           GPIO_PIN_2
-//#define UART_RX_PIN           GPIO_PIN_3
+#define PINID_LED      0
+#define PINID_BUTTON   1
+#define PINID_UART_TX  2
+#define PINID_UART_RX  3
 
+static board_pindef_t board_pindef[] = {
+  { // LED
+    .port = GPIOC,
+    .pin_init = { .Pin = GPIO_PIN_13, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 0
+  },
+  { // BUTTON
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_0, .Mode = GPIO_MODE_INPUT, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 0
+  },
+  { // UART TX
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_2, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART2 },
+    .active_state = 0
+  },
+  { // UART RX
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_3, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART2 },
+    .active_state = 0
+  },
+};
 
 //--------------------------------------------------------------------+
 // RCC Clock
@@ -85,18 +97,21 @@ static inline void board_clock_init(void)
   RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
   HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2);
 
-  // Enable clocks for LED, Button, Uart
-  __HAL_RCC_GPIOA_CLK_ENABLE();
-  __HAL_RCC_GPIOC_CLK_ENABLE();
-  //__HAL_RCC_USART2_CLK_ENABLE();
+  // Enable clocks for Uart
+  __HAL_RCC_USART2_CLK_ENABLE();
 }
 
-static inline void board_vbus_sense_init(void)
-{
+static inline void board_vbus_sense_init(uint8_t rhport) {
   // Blackpill doesn't use VBUS sense (B device) explicitly disable it
-  USB_OTG_FS->GCCFG |= USB_OTG_GCCFG_NOVBUSSENS;
-  USB_OTG_FS->GCCFG &= ~USB_OTG_GCCFG_VBUSBSEN;
-  USB_OTG_FS->GCCFG &= ~USB_OTG_GCCFG_VBUSASEN;
+  if  (rhport == 0) {
+    USB_OTG_FS->GCCFG |= USB_OTG_GCCFG_NOVBUSSENS;
+    USB_OTG_FS->GCCFG &= ~USB_OTG_GCCFG_VBUSBSEN;
+    USB_OTG_FS->GCCFG &= ~USB_OTG_GCCFG_VBUSASEN;
+  }
+}
+
+static inline void board_vbus_set(uint8_t rhport, bool state) {
+  (void) rhport; (void) state;
 }
 
 #ifdef __cplusplus
diff --git a/hw/bsp/stm32f4/boards/stm32f411disco/board.cmake b/hw/bsp/stm32f4/boards/stm32f411disco/board.cmake
index 80cf94160..d7c32c27d 100644
--- a/hw/bsp/stm32f4/boards/stm32f411disco/board.cmake
+++ b/hw/bsp/stm32f4/boards/stm32f411disco/board.cmake
@@ -6,6 +6,5 @@ set(LD_FILE_GNU ${CMAKE_CURRENT_LIST_DIR}/STM32F411VETx_FLASH.ld)
 function(update_board TARGET)
   target_compile_definitions(${TARGET} PUBLIC
     STM32F411xE
-    BOARD_TUD_RHPORT=0
     )
 endfunction()
diff --git a/hw/bsp/stm32f4/boards/stm32f411disco/board.h b/hw/bsp/stm32f4/boards/stm32f411disco/board.h
index 57d1e061e..d4bad8e40 100644
--- a/hw/bsp/stm32f4/boards/stm32f411disco/board.h
+++ b/hw/bsp/stm32f4/boards/stm32f411disco/board.h
@@ -31,28 +31,46 @@
  extern "C" {
 #endif
 
-// Orange LED
-#define LED_PORT              GPIOD
-#define LED_PIN               GPIO_PIN_13
-#define LED_STATE_ON          1
-
-#define BUTTON_PORT           GPIOA
-#define BUTTON_PIN            GPIO_PIN_0
-#define BUTTON_STATE_ACTIVE   1
-
-// Enable PA2 as the debug log UART
 #define UART_DEV              USART2
-#define UART_GPIO_PORT        GPIOA
-#define UART_GPIO_AF          GPIO_AF7_USART2
-#define UART_TX_PIN           GPIO_PIN_2
-#define UART_RX_PIN           GPIO_PIN_3
 
+#define PINID_LED      0
+#define PINID_BUTTON   1
+#define PINID_UART_TX  2
+#define PINID_UART_RX  3
+#define PINID_VBUS0_EN 4
+
+static board_pindef_t board_pindef[] = {
+  { // LED
+    .port = GPIOD,
+    .pin_init = { .Pin = GPIO_PIN_13, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // BUTTON
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_0, .Mode = GPIO_MODE_INPUT, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // UART TX
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_2, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART2 },
+    .active_state = 0
+  },
+  { // UART RX
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_3, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART2 },
+    .active_state = 0
+  },
+  { // VBUS0 EN
+    .port = GPIOC,
+    .pin_init = { .Pin = GPIO_PIN_0, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_NOPULL, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 0
+  }
+};
 
 //--------------------------------------------------------------------+
 // RCC Clock
 //--------------------------------------------------------------------+
-static inline void board_clock_init(void)
-{
+static inline void board_clock_init(void) {
   RCC_ClkInitTypeDef RCC_ClkInitStruct;
   RCC_OscInitTypeDef RCC_OscInitStruct;
 
@@ -84,17 +102,23 @@ static inline void board_clock_init(void)
   RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
   HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2);
 
-  // Enable clocks for LED, Button, Uart
-  __HAL_RCC_GPIOA_CLK_ENABLE();
-  __HAL_RCC_GPIOD_CLK_ENABLE();
+  // Enable clocks for UART
   __HAL_RCC_USART2_CLK_ENABLE();
 }
 
-static inline void board_vbus_sense_init(void)
-{
+static inline void board_vbus_sense_init(uint8_t rhport) {
   // Enable VBUS sense (B device) via pin PA9
-  USB_OTG_FS->GCCFG &= ~USB_OTG_GCCFG_NOVBUSSENS;
-  USB_OTG_FS->GCCFG |= USB_OTG_GCCFG_VBUSBSEN;
+  if (rhport == 0) {
+    USB_OTG_FS->GCCFG &= ~USB_OTG_GCCFG_NOVBUSSENS;
+    USB_OTG_FS->GCCFG |= USB_OTG_GCCFG_VBUSBSEN;
+  }
+}
+
+static inline void board_vbus_set(uint8_t rhport, bool state) {
+  if (rhport == 0) {
+    board_pindef_t* pindef = &board_pindef[PINID_VBUS0_EN];
+    HAL_GPIO_WritePin(pindef->port, pindef->pin_init.Pin, state == pindef->active_state ? GPIO_PIN_SET : GPIO_PIN_RESET);
+  }
 }
 
 #ifdef __cplusplus
diff --git a/hw/bsp/stm32f4/boards/stm32f412disco/board.cmake b/hw/bsp/stm32f4/boards/stm32f412disco/board.cmake
index c282af98e..59c1c3a7b 100644
--- a/hw/bsp/stm32f4/boards/stm32f412disco/board.cmake
+++ b/hw/bsp/stm32f4/boards/stm32f412disco/board.cmake
@@ -7,6 +7,5 @@ set(LD_FILE_GNU ${CMAKE_CURRENT_LIST_DIR}/STM32F412ZGTx_FLASH.ld)
 function(update_board TARGET)
   target_compile_definitions(${TARGET} PUBLIC
     STM32F412Zx
-    BOARD_TUD_RHPORT=0
     )
 endfunction()
diff --git a/hw/bsp/stm32f4/boards/stm32f412disco/board.h b/hw/bsp/stm32f4/boards/stm32f412disco/board.h
index d61b70eb9..74e664418 100644
--- a/hw/bsp/stm32f4/boards/stm32f412disco/board.h
+++ b/hw/bsp/stm32f4/boards/stm32f412disco/board.h
@@ -31,28 +31,47 @@
  extern "C" {
 #endif
 
-// LED
-#define LED_PORT              GPIOE
-#define LED_PIN               GPIO_PIN_2
-#define LED_STATE_ON          0
-
-// Button
-#define BUTTON_PORT           GPIOA
-#define BUTTON_PIN            GPIO_PIN_0
-#define BUTTON_STATE_ACTIVE   1
-
 // UART Enable PA2 as the debug log UART
 #define UART_DEV              USART2
-#define UART_GPIO_PORT        GPIOA
-#define UART_GPIO_AF          GPIO_AF7_USART2
-#define UART_TX_PIN           GPIO_PIN_2
-#define UART_RX_PIN           GPIO_PIN_3
+
+#define PINID_LED      0
+#define PINID_BUTTON   1
+#define PINID_UART_TX  2
+#define PINID_UART_RX  3
+#define PINID_VBUS0_EN 4
+
+static board_pindef_t board_pindef[] = {
+  { // LED
+    .port = GPIOE,
+    .pin_init = { .Pin = GPIO_PIN_2, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 0
+  },
+  { // BUTTON
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_0, .Mode = GPIO_MODE_INPUT, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // UART TX
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_2, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART2 },
+    .active_state = 0
+  },
+  { // UART RX
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_3, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART2 },
+    .active_state = 0
+  },
+  { // VBUS0 EN
+    .port = GPIOG,
+    .pin_init = { .Pin = GPIO_PIN_8, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_NOPULL, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 0
+  }
+};
 
 //--------------------------------------------------------------------+
 // RCC Clock
 //--------------------------------------------------------------------+
-static inline void board_clock_init(void)
-{
+static inline void board_clock_init(void) {
   RCC_ClkInitTypeDef RCC_ClkInitStruct;
   RCC_OscInitTypeDef RCC_OscInitStruct;
   RCC_PeriphCLKInitTypeDef PeriphClkInitStruct;
@@ -99,18 +118,25 @@ static inline void board_clock_init(void)
   RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
   HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_3);
 
-  // Enable clocks for LED, Button, Uart
-  __HAL_RCC_GPIOA_CLK_ENABLE();
-  __HAL_RCC_GPIOE_CLK_ENABLE();
+  // Enable clocks for Uart
   __HAL_RCC_USART2_CLK_ENABLE();
 }
 
-static inline void board_vbus_sense_init(void)
-{
-  // Enable VBUS sense (B device) via pin PA9
-  USB_OTG_FS->GCCFG |= USB_OTG_GCCFG_VBDEN;
+static inline void board_vbus_sense_init(uint8_t rhport) {
+  if (rhport == 0) {
+    // Enable VBUS sense (B device) via pin PA9
+    USB_OTG_FS->GCCFG |= USB_OTG_GCCFG_VBDEN;
+  }
 }
 
+static inline void board_vbus_set(uint8_t rhport, bool state) {
+  if (rhport == 0) {
+    board_pindef_t* pindef = &board_pindef[PINID_VBUS0_EN];
+    HAL_GPIO_WritePin(pindef->port, pindef->pin_init.Pin, state == pindef->active_state ? GPIO_PIN_SET : GPIO_PIN_RESET);
+  }
+}
+
+
 #ifdef __cplusplus
  }
 #endif
diff --git a/hw/bsp/stm32f4/boards/stm32f412nucleo/board.h b/hw/bsp/stm32f4/boards/stm32f412nucleo/board.h
index 73c5f83b9..8900a1e6b 100644
--- a/hw/bsp/stm32f4/boards/stm32f412nucleo/board.h
+++ b/hw/bsp/stm32f4/boards/stm32f412nucleo/board.h
@@ -31,22 +31,42 @@
  extern "C" {
 #endif
 
-// LED
-#define LED_PORT              GPIOB
-#define LED_PIN               GPIO_PIN_14
-#define LED_STATE_ON          0
-
-// Button
-#define BUTTON_PORT           GPIOC
-#define BUTTON_PIN            GPIO_PIN_13
-#define BUTTON_STATE_ACTIVE   1
-
 // UART Enable for STLink VCOM
 #define UART_DEV              USART3
-#define UART_GPIO_PORT        GPIOD
-#define UART_GPIO_AF          GPIO_AF7_USART3
-#define UART_TX_PIN           GPIO_PIN_8
-#define UART_RX_PIN           GPIO_PIN_9
+
+#define PINID_LED      0
+#define PINID_BUTTON   1
+#define PINID_UART_TX  2
+#define PINID_UART_RX  3
+#define PINID_VBUS0_EN 4
+
+static board_pindef_t board_pindef[] = {
+  { // LED
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_14, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 0
+  },
+  { // BUTTON
+    .port = GPIOC,
+    .pin_init = { .Pin = GPIO_PIN_13, .Mode = GPIO_MODE_INPUT, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // UART TX
+    .port = GPIOD,
+    .pin_init = { .Pin = GPIO_PIN_8, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  },
+  { // UART RX
+    .port = GPIOD,
+    .pin_init = { .Pin = GPIO_PIN_9, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  },
+  { // VBUS0 EN
+    .port = GPIOG,
+    .pin_init = { .Pin = GPIO_PIN_6, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  }
+};
 
 //--------------------------------------------------------------------+
 // RCC Clock
@@ -99,17 +119,22 @@ static inline void board_clock_init(void)
   RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
   HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_3);
 
-  // Enable clocks for LED, Button, Uart
-  __HAL_RCC_GPIOB_CLK_ENABLE();
-  __HAL_RCC_GPIOC_CLK_ENABLE();
-  __HAL_RCC_GPIOD_CLK_ENABLE();
+  // Enable clocks for Uart
   __HAL_RCC_USART3_CLK_ENABLE();
 }
 
-static inline void board_vbus_sense_init(void)
-{
-  // Enable VBUS sense (B device) via pin PA9
-  USB_OTG_FS->GCCFG |= USB_OTG_GCCFG_VBDEN;
+static inline void board_vbus_sense_init(uint8_t rhport) {
+  if (rhport == 0) {
+    // Enable VBUS sense (B device) via pin PA9
+    USB_OTG_FS->GCCFG |= USB_OTG_GCCFG_VBDEN;
+  }
+}
+
+static inline void board_vbus_set(uint8_t rhport, bool state) {
+  if (rhport == 0) {
+    board_pindef_t* pindef = &board_pindef[PINID_VBUS0_EN];
+    HAL_GPIO_WritePin(pindef->port, pindef->pin_init.Pin, state == pindef->active_state ? GPIO_PIN_SET : GPIO_PIN_RESET);
+  }
 }
 
 #ifdef __cplusplus
diff --git a/hw/bsp/stm32f4/boards/stm32f439nucleo/board.h b/hw/bsp/stm32f4/boards/stm32f439nucleo/board.h
index e5a822426..aa9de4073 100644
--- a/hw/bsp/stm32f4/boards/stm32f439nucleo/board.h
+++ b/hw/bsp/stm32f4/boards/stm32f439nucleo/board.h
@@ -31,22 +31,43 @@
  extern "C" {
 #endif
 
-// LED
-#define LED_PORT              GPIOB
-#define LED_PIN               GPIO_PIN_14
-#define LED_STATE_ON          0
-
-// Button
-#define BUTTON_PORT           GPIOC
-#define BUTTON_PIN            GPIO_PIN_13
-#define BUTTON_STATE_ACTIVE   1
 
 // UART Enable for STLink VCOM
 #define UART_DEV              USART3
-#define UART_GPIO_PORT        GPIOD
-#define UART_GPIO_AF          GPIO_AF7_USART3
-#define UART_TX_PIN           GPIO_PIN_8
-#define UART_RX_PIN           GPIO_PIN_9
+
+#define PINID_LED      0
+#define PINID_BUTTON   1
+#define PINID_UART_TX  2
+#define PINID_UART_RX  3
+#define PINID_VBUS0_EN 4
+
+static board_pindef_t board_pindef[] = {
+  { // LED
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_14, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 0
+  },
+  { // BUTTON
+    .port = GPIOC,
+    .pin_init = { .Pin = GPIO_PIN_13, .Mode = GPIO_MODE_INPUT, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // UART TX
+    .port = GPIOD,
+    .pin_init = { .Pin = GPIO_PIN_8, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  },
+  { // UART RX
+    .port = GPIOD,
+    .pin_init = { .Pin = GPIO_PIN_9, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  },
+  { // VBUS0 EN
+    .port = GPIOG,
+    .pin_init = { .Pin = GPIO_PIN_6, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  }
+};
 
 //--------------------------------------------------------------------+
 // RCC Clock
@@ -87,18 +108,23 @@ static inline void board_clock_init(void)
   RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
   HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5);
 
-  // Enable clocks for LED, Button, Uart
-  __HAL_RCC_GPIOB_CLK_ENABLE();
-  __HAL_RCC_GPIOC_CLK_ENABLE();
-  __HAL_RCC_GPIOD_CLK_ENABLE();
+  // Enable clocks Uart
   __HAL_RCC_USART3_CLK_ENABLE();
 }
 
-static inline void board_vbus_sense_init(void)
-{
-  // Enable VBUS sense (B device) via pin PA9
-  USB_OTG_FS->GCCFG |= USB_OTG_GCCFG_NOVBUSSENS;
-  USB_OTG_FS->GCCFG |= USB_OTG_GCCFG_VBUSBSEN;
+static inline void board_vbus_sense_init(uint8_t rhport) {
+  if (rhport == 0) {
+    // Enable VBUS sense (B device) via pin PA9
+    USB_OTG_FS->GCCFG |= USB_OTG_GCCFG_NOVBUSSENS;
+    USB_OTG_FS->GCCFG |= USB_OTG_GCCFG_VBUSBSEN;
+  }
+}
+
+static inline void board_vbus_set(uint8_t rhport, bool state) {
+  if (rhport == 0) {
+    board_pindef_t* pindef = &board_pindef[PINID_VBUS0_EN];
+    HAL_GPIO_WritePin(pindef->port, pindef->pin_init.Pin, state == pindef->active_state ? GPIO_PIN_SET : GPIO_PIN_RESET);
+  }
 }
 
 #ifdef __cplusplus
diff --git a/hw/bsp/stm32f4/family.c b/hw/bsp/stm32f4/family.c
index b4a661421..866a09d6f 100644
--- a/hw/bsp/stm32f4/family.c
+++ b/hw/bsp/stm32f4/family.c
@@ -26,6 +26,13 @@
 
 #include "stm32f4xx_hal.h"
 #include "bsp/board_api.h"
+
+typedef struct {
+  GPIO_TypeDef* port;
+  GPIO_InitTypeDef pin_init;
+  uint8_t active_state;
+} board_pindef_t;
+
 #include "board.h"
 
 //--------------------------------------------------------------------+
@@ -42,12 +49,49 @@ void OTG_HS_IRQHandler(void) {
 //--------------------------------------------------------------------+
 // MACRO TYPEDEF CONSTANT ENUM
 //--------------------------------------------------------------------+
-UART_HandleTypeDef UartHandle;
+UART_HandleTypeDef UartHandle = {
+    .Instance = UART_DEV,
+    .Init = {
+      .BaudRate   = CFG_BOARD_UART_BAUDRATE,
+      .WordLength = UART_WORDLENGTH_8B,
+      .StopBits   = UART_STOPBITS_1,
+      .Parity     = UART_PARITY_NONE,
+      .HwFlowCtl  = UART_HWCONTROL_NONE,
+      .Mode       = UART_MODE_TX_RX,
+      .OverSampling = UART_OVERSAMPLING_16
+    }
+};
 
 void board_init(void) {
   board_clock_init();
   //SystemCoreClockUpdate();
 
+  // Enable All GPIOs clocks
+  __HAL_RCC_GPIOA_CLK_ENABLE();
+  __HAL_RCC_GPIOB_CLK_ENABLE();
+  __HAL_RCC_GPIOC_CLK_ENABLE();
+  __HAL_RCC_GPIOD_CLK_ENABLE();
+#ifdef __HAL_RCC_GPIOE_CLK_ENABLE
+  __HAL_RCC_GPIOE_CLK_ENABLE();
+#endif
+#ifdef __HAL_RCC_GPIOF_CLK_ENABLE
+  __HAL_RCC_GPIOF_CLK_ENABLE();
+#endif
+#ifdef __HAL_RCC_GPIOG_CLK_ENABLE
+  __HAL_RCC_GPIOG_CLK_ENABLE();
+#endif
+  __HAL_RCC_GPIOH_CLK_ENABLE();
+#ifdef __HAL_RCC_GPIOI_CLK_ENABLE
+  __HAL_RCC_GPIOI_CLK_ENABLE();
+#endif
+#ifdef __HAL_RCC_GPIOJ_CLK_ENABLE
+  __HAL_RCC_GPIOJ_CLK_ENABLE();
+#endif
+
+  for (uint8_t i = 0; i < TU_ARRAY_SIZE(board_pindef); i++) {
+    HAL_GPIO_Init(board_pindef[i].port, &board_pindef[i].pin_init);
+  }
+
 #if CFG_TUSB_OS == OPT_OS_NONE
   // 1ms tick timer
   SysTick_Config(SystemCoreClock / 1000);
@@ -59,49 +103,14 @@ void board_init(void) {
   NVIC_SetPriority(OTG_FS_IRQn, configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY );
 #endif
 
-  GPIO_InitTypeDef GPIO_InitStruct;
-
-  // LED
-  GPIO_InitStruct.Pin = LED_PIN;
-  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
-  GPIO_InitStruct.Pull = GPIO_PULLUP;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
-  HAL_GPIO_Init(LED_PORT, &GPIO_InitStruct);
-
   board_led_write(false);
 
-  // Button
-  GPIO_InitStruct.Pin = BUTTON_PIN;
-  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
-  GPIO_InitStruct.Pull = BUTTON_STATE_ACTIVE ? GPIO_PULLDOWN : GPIO_PULLUP;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
-  HAL_GPIO_Init(BUTTON_PORT, &GPIO_InitStruct);
-
 #ifdef UART_DEV
-  // UART
-  GPIO_InitStruct.Pin = UART_TX_PIN | UART_RX_PIN;
-  GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
-  GPIO_InitStruct.Pull = GPIO_PULLUP;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
-  GPIO_InitStruct.Alternate = UART_GPIO_AF;
-  HAL_GPIO_Init(UART_GPIO_PORT, &GPIO_InitStruct);
-
-  UartHandle = (UART_HandleTypeDef) {
-      .Instance        = UART_DEV,
-      .Init.BaudRate   = CFG_BOARD_UART_BAUDRATE,
-      .Init.WordLength = UART_WORDLENGTH_8B,
-      .Init.StopBits   = UART_STOPBITS_1,
-      .Init.Parity     = UART_PARITY_NONE,
-      .Init.HwFlowCtl  = UART_HWCONTROL_NONE,
-      .Init.Mode       = UART_MODE_TX_RX,
-      .Init.OverSampling = UART_OVERSAMPLING_16
-  };
   HAL_UART_Init(&UartHandle);
 #endif
 
-#if BOARD_TUD_RHPORT == 0
-  /* Configure USB FS GPIOs */
-  __HAL_RCC_GPIOA_CLK_ENABLE();
+  //------------- USB FS -------------//
+  GPIO_InitTypeDef GPIO_InitStruct;
 
   /* Configure USB D+ D- Pins */
   GPIO_InitStruct.Pin = GPIO_PIN_11 | GPIO_PIN_12;
@@ -127,11 +136,9 @@ void board_init(void) {
 
   // Enable USB OTG clock
   __HAL_RCC_USB_OTG_FS_CLK_ENABLE();
-#else
-  /* Configure USB HS GPIOs */
-  __HAL_RCC_GPIOB_CLK_ENABLE();
 
-  /* Configure USB D+ D- Pins */
+  //------------- USB HS -------------//
+#ifdef __HAL_RCC_USB_OTG_HS_CLK_ENABLE
   GPIO_InitStruct.Pin = GPIO_PIN_14 | GPIO_PIN_15;
   GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
   GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
@@ -166,7 +173,13 @@ void board_init(void) {
   HAL_GPIO_Init(GPIOG, &GPIO_InitStruct);
 #endif
 
-  board_vbus_sense_init();
+#if CFG_TUD_ENABLED
+  board_vbus_sense_init(BOARD_TUD_RHPORT);
+#endif
+
+#if CFG_TUH_ENABLED
+  board_vbus_set(BOARD_TUD_RHPORT, true);
+#endif
 }
 
 //--------------------------------------------------------------------+
@@ -174,12 +187,22 @@ void board_init(void) {
 //--------------------------------------------------------------------+
 
 void board_led_write(bool state) {
-  GPIO_PinState pin_state = (GPIO_PinState) (state ? LED_STATE_ON : (1 - LED_STATE_ON));
-  HAL_GPIO_WritePin(LED_PORT, LED_PIN, pin_state);
+#ifdef PINID_LED
+  board_pindef_t* pindef = &board_pindef[PINID_LED];
+  GPIO_PinState pin_state = state == pindef->active_state ? GPIO_PIN_SET : GPIO_PIN_RESET;
+  HAL_GPIO_WritePin(pindef->port, pindef->pin_init.Pin, pin_state);
+#else
+  (void) state;
+#endif
 }
 
 uint32_t board_button_read(void) {
-  return BUTTON_STATE_ACTIVE == HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN);
+#ifdef PINID_BUTTON
+  board_pindef_t* pindef = &board_pindef[PINID_BUTTON];
+  return pindef->active_state == HAL_GPIO_ReadPin(pindef->port, pindef->pin_init.Pin);
+#else
+  return 0;
+#endif
 }
 
 size_t board_get_unique_id(uint8_t id[], size_t max_len) {
diff --git a/hw/bsp/stm32f4/family.cmake b/hw/bsp/stm32f4/family.cmake
index f24ef366e..c0c9fe902 100644
--- a/hw/bsp/stm32f4/family.cmake
+++ b/hw/bsp/stm32f4/family.cmake
@@ -16,6 +16,28 @@ set(CMAKE_TOOLCHAIN_FILE ${TOP}/examples/build_system/cmake/toolchain/arm_${TOOL
 
 set(FAMILY_MCUS STM32F4 CACHE INTERNAL "")
 
+# ----------------------
+# Port & Speed Selection
+# ----------------------
+if (NOT DEFINED RHPORT_DEVICE)
+  set(RHPORT_DEVICE 0)
+endif ()
+if (NOT DEFINED RHPORT_HOST)
+  set(RHPORT_HOST 0)
+endif ()
+
+if (NOT DEFINED RHPORT_SPEED)
+  # Most F7 does not has built-in HS PHY
+  set(RHPORT_SPEED OPT_MODE_FULL_SPEED OPT_MODE_FULL_SPEED)
+endif ()
+if (NOT DEFINED RHPORT_DEVICE_SPEED)
+  list(GET RHPORT_SPEED ${RHPORT_DEVICE} RHPORT_DEVICE_SPEED)
+endif ()
+if (NOT DEFINED RHPORT_HOST_SPEED)
+  list(GET RHPORT_SPEED ${RHPORT_HOST} RHPORT_HOST_SPEED)
+endif ()
+
+cmake_print_variables(RHPORT_DEVICE RHPORT_DEVICE_SPEED RHPORT_HOST RHPORT_HOST_SPEED)
 
 #------------------------------------
 # BOARD_TARGET
@@ -52,8 +74,12 @@ function(add_board_target BOARD_TARGET)
     ${ST_CMSIS}/Include
     ${ST_HAL_DRIVER}/Inc
     )
-  # target_compile_options(${BOARD_TARGET} PUBLIC)
-  # target_compile_definitions(${BOARD_TARGET} PUBLIC)
+  target_compile_definitions(${BOARD_TARGET} PUBLIC
+    BOARD_TUD_RHPORT=${RHPORT_DEVICE}
+    BOARD_TUD_MAX_SPEED=${RHPORT_DEVICE_SPEED}
+    BOARD_TUH_RHPORT=${RHPORT_HOST}
+    BOARD_TUH_MAX_SPEED=${RHPORT_HOST_SPEED}
+    )
 
   update_board(${BOARD_TARGET})
 
@@ -102,6 +128,8 @@ function(family_configure_example TARGET RTOS)
   family_add_tinyusb(${TARGET} OPT_MCU_STM32F4 ${RTOS})
   target_sources(${TARGET}-tinyusb PUBLIC
     ${TOP}/src/portable/synopsys/dwc2/dcd_dwc2.c
+    ${TOP}/src/portable/synopsys/dwc2/hcd_dwc2.c
+    ${TOP}/src/portable/synopsys/dwc2/dwc2_common.c
     )
   target_link_libraries(${TARGET}-tinyusb PUBLIC board_${BOARD})
 
diff --git a/hw/bsp/stm32f4/family.mk b/hw/bsp/stm32f4/family.mk
index 523a1cb04..51ff43a60 100644
--- a/hw/bsp/stm32f4/family.mk
+++ b/hw/bsp/stm32f4/family.mk
@@ -8,14 +8,40 @@ ST_HAL_DRIVER = hw/mcu/st/stm32$(ST_FAMILY)xx_hal_driver
 include $(TOP)/$(BOARD_PATH)/board.mk
 CPU_CORE ?= cortex-m4
 
-PORT ?= 0
+# ----------------------
+# Port & Speed Selection
+# ----------------------
+RHPORT_SPEED ?= OPT_MODE_FULL_SPEED OPT_MODE_FULL_SPEED
+RHPORT_DEVICE ?= 0
+RHPORT_HOST ?= 0
+
+# Determine RHPORT_DEVICE_SPEED if not defined
+ifndef RHPORT_DEVICE_SPEED
+ifeq ($(RHPORT_DEVICE), 0)
+  RHPORT_DEVICE_SPEED = $(firstword $(RHPORT_SPEED))
+else
+  RHPORT_DEVICE_SPEED = $(lastword $(RHPORT_SPEED))
+endif
+endif
+
+# Determine RHPORT_HOST_SPEED if not defined
+ifndef RHPORT_HOST_SPEED
+ifeq ($(RHPORT_HOST), 0)
+  RHPORT_HOST_SPEED = $(firstword $(RHPORT_SPEED))
+else
+  RHPORT_HOST_SPEED = $(lastword $(RHPORT_SPEED))
+endif
+endif
 
 # --------------
 # Compiler Flags
 # --------------
 CFLAGS += \
   -DCFG_TUSB_MCU=OPT_MCU_STM32F4 \
-  -DBOARD_TUD_RHPORT=$(PORT)
+	-DBOARD_TUD_RHPORT=${RHPORT_DEVICE} \
+	-DBOARD_TUD_MAX_SPEED=${RHPORT_DEVICE_SPEED} \
+	-DBOARD_TUH_RHPORT=${RHPORT_HOST} \
+	-DBOARD_TUH_MAX_SPEED=${RHPORT_HOST_SPEED} \
 
 # GCC Flags
 CFLAGS_GCC += \
@@ -34,6 +60,8 @@ LDFLAGS_GCC += \
 
 SRC_C += \
 	src/portable/synopsys/dwc2/dcd_dwc2.c \
+	src/portable/synopsys/dwc2/hcd_dwc2.c \
+	src/portable/synopsys/dwc2/dwc2_common.c \
 	$(ST_CMSIS)/Source/Templates/system_stm32$(ST_FAMILY)xx.c \
 	$(ST_HAL_DRIVER)/Src/stm32$(ST_FAMILY)xx_hal.c \
 	$(ST_HAL_DRIVER)/Src/stm32$(ST_FAMILY)xx_hal_cortex.c \
diff --git a/hw/bsp/stm32f7/boards/stlinkv3mini/board.cmake b/hw/bsp/stm32f7/boards/stlinkv3mini/board.cmake
index 378e736d4..6a1132ef8 100644
--- a/hw/bsp/stm32f7/boards/stlinkv3mini/board.cmake
+++ b/hw/bsp/stm32f7/boards/stlinkv3mini/board.cmake
@@ -3,12 +3,18 @@ set(JLINK_DEVICE stm32f723xx)
 
 set(LD_FILE_GNU ${CMAKE_CURRENT_LIST_DIR}/STM32F723xE_FLASH.ld)
 
+set(RHPORT_SPEED OPT_MODE_FULL_SPEED OPT_MODE_HIGH_SPEED)
+
+if (NOT DEFINED RHPORT_DEVICE)
+  set(RHPORT_DEVICE 1)
+endif()
+if (NOT DEFINED RHPORT_HOST)
+  set(RHPORT_HOST 1)
+endif()
+
 function(update_board TARGET)
   target_compile_definitions(${TARGET} PUBLIC
     STM32F723xx
     HSE_VALUE=25000000
-    # default to PORT 1 High Speed
-    BOARD_TUD_RHPORT=1
-    BOARD_TUD_MAX_SPEED=OPT_MODE_HIGH_SPEED
     )
 endfunction()
diff --git a/hw/bsp/stm32f7/boards/stlinkv3mini/board.h b/hw/bsp/stm32f7/boards/stlinkv3mini/board.h
index ed02df061..632fd99ed 100644
--- a/hw/bsp/stm32f7/boards/stlinkv3mini/board.h
+++ b/hw/bsp/stm32f7/boards/stlinkv3mini/board.h
@@ -31,29 +31,37 @@
  extern "C" {
 #endif
 
-#define LED_PORT              GPIOA
-#define LED_PIN               GPIO_PIN_10
-#define LED_STATE_ON          1
-
-// No physical button is populated
-#define BUTTON_PORT           GPIOA
-#define BUTTON_PIN            GPIO_PIN_0
-#define BUTTON_STATE_ACTIVE   1
-
 #define UART_DEV              USART6
 #define UART_CLK_EN           __HAL_RCC_USART6_CLK_ENABLE
-#define UART_GPIO_AF          GPIO_AF8_USART6
-
-#define UART_TX_PORT          GPIOG
-#define UART_TX_PIN           GPIO_PIN_9
-
-#define UART_RX_PORT          GPIOG
-#define UART_RX_PIN           GPIO_PIN_14
 
 // VBUS Sense detection
 #define OTG_FS_VBUS_SENSE     1
 #define OTG_HS_VBUS_SENSE     0
 
+#define PINID_LED      0
+#define PINID_UART_TX  1
+#define PINID_UART_RX  2
+//#define PINID_VBUS0_EN 4
+//#define PINID_VBUS1_EN 5
+
+static board_pindef_t board_pindef[] = {
+  { // LED
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_10, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // UART TX
+    .port = GPIOG,
+    .pin_init = { .Pin = GPIO_PIN_9, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF8_USART6 },
+    .active_state = 0
+  },
+  { // UART RX
+    .port = GPIOG,
+    .pin_init = { .Pin = GPIO_PIN_14, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF8_USART6 },
+    .active_state = 0
+  }
+};
+
 //--------------------------------------------------------------------+
 // RCC Clock
 //--------------------------------------------------------------------+
@@ -92,8 +100,15 @@ static inline void board_clock_init(void)
   RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
 
   HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_7);
+
+  UART_CLK_EN();
 }
 
+static inline void board_vbus_set(uint8_t rhport, bool state) {
+ (void) rhport; (void) state;
+}
+
+
 #ifdef __cplusplus
  }
 #endif
diff --git a/hw/bsp/stm32f7/boards/stlinkv3mini/board.mk b/hw/bsp/stm32f7/boards/stlinkv3mini/board.mk
index 94dce5e84..a19e455c7 100644
--- a/hw/bsp/stm32f7/boards/stlinkv3mini/board.mk
+++ b/hw/bsp/stm32f7/boards/stlinkv3mini/board.mk
@@ -1,6 +1,10 @@
 MCU_VARIANT = stm32f723xx
 
 # Only OTG-HS has a connector on this board
+RHPORT_SPEED = OPT_MODE_FULL_SPEED OPT_MODE_HIGH_SPEED
+RHPORT_DEVICE ?= 1
+RHPORT_HOST ?= 1
+
 PORT ?= 1
 SPEED ?= high
 
diff --git a/hw/bsp/stm32f7/boards/stm32f723disco/board.cmake b/hw/bsp/stm32f7/boards/stm32f723disco/board.cmake
index dd3c35b00..eb3ed407b 100644
--- a/hw/bsp/stm32f7/boards/stm32f723disco/board.cmake
+++ b/hw/bsp/stm32f7/boards/stm32f723disco/board.cmake
@@ -1,14 +1,21 @@
 set(MCU_VARIANT stm32f723xx)
 set(JLINK_DEVICE stm32f723ie)
-
+#set(JLINK_OPTION "-USB 000776606156")
 set(LD_FILE_GNU ${CMAKE_CURRENT_LIST_DIR}/STM32F723xE_FLASH.ld)
 
+set(RHPORT_SPEED OPT_MODE_FULL_SPEED OPT_MODE_HIGH_SPEED)
+
+# For Hardware test: device default to PORT 0, Host to port 1
+if (NOT DEFINED RHPORT_DEVICE)
+  set(RHPORT_DEVICE 0)
+endif()
+if (NOT DEFINED RHPORT_HOST)
+  set(RHPORT_HOST 1)
+endif()
+
 function(update_board TARGET)
   target_compile_definitions(${TARGET} PUBLIC
     STM32F723xx
     HSE_VALUE=25000000
-    # default to PORT 1 High Speed
-    BOARD_TUD_RHPORT=1
-    BOARD_TUD_MAX_SPEED=OPT_MODE_HIGH_SPEED
     )
 endfunction()
diff --git a/hw/bsp/stm32f7/boards/stm32f723disco/board.h b/hw/bsp/stm32f7/boards/stm32f723disco/board.h
index 258786b7f..d45ceec5c 100644
--- a/hw/bsp/stm32f7/boards/stm32f723disco/board.h
+++ b/hw/bsp/stm32f7/boards/stm32f723disco/board.h
@@ -31,33 +31,57 @@
  extern "C" {
 #endif
 
-#define LED_PORT              GPIOB
-#define LED_PIN               GPIO_PIN_1
-#define LED_STATE_ON          1
-
-#define BUTTON_PORT           GPIOA
-#define BUTTON_PIN            GPIO_PIN_0
-#define BUTTON_STATE_ACTIVE   1
-
 #define UART_DEV              USART6
 #define UART_CLK_EN           __HAL_RCC_USART6_CLK_ENABLE
-#define UART_GPIO_AF          GPIO_AF8_USART6
-
-#define UART_TX_PORT          GPIOC
-#define UART_TX_PIN           GPIO_PIN_6
-
-#define UART_RX_PORT          GPIOC
-#define UART_RX_PIN           GPIO_PIN_7
 
 // VBUS Sense detection
 #define OTG_FS_VBUS_SENSE     1
 #define OTG_HS_VBUS_SENSE     0
 
+#define PINID_LED      0
+#define PINID_BUTTON   1
+#define PINID_UART_TX  2
+#define PINID_UART_RX  3
+#define PINID_VBUS0_EN 4
+#define PINID_VBUS1_EN 5
+
+static board_pindef_t board_pindef[] = {
+{ // LED
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_1, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+{ // Button
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_0, .Mode = GPIO_MODE_INPUT, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+{ // UART TX
+    .port = GPIOC,
+    .pin_init = { .Pin = GPIO_PIN_6, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF8_USART6 },
+    .active_state = 0
+  },
+{ // UART RX
+    .port = GPIOC,
+    .pin_init = { .Pin = GPIO_PIN_7, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF8_USART6 },
+    .active_state = 0
+  },
+{ // VBUS0 EN
+    .port = GPIOG,
+    .pin_init = { .Pin = GPIO_PIN_8, .Mode = GPIO_MODE_OUTPUT_OD, .Pull = GPIO_NOPULL, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 0
+  },
+{ // VBUS1 EN
+    .port = GPIOH,
+    .pin_init = { .Pin = GPIO_PIN_12, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_NOPULL, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+};
+
 //--------------------------------------------------------------------+
 // RCC Clock
 //--------------------------------------------------------------------+
-static inline void board_clock_init(void)
-{
+static inline void board_clock_init(void) {
   RCC_ClkInitTypeDef RCC_ClkInitStruct;
   RCC_OscInitTypeDef RCC_OscInitStruct;
 
@@ -74,7 +98,7 @@ static inline void board_clock_init(void)
   RCC_OscInitStruct.HSEState = RCC_HSE_ON;
   RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
   RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
-  RCC_OscInitStruct.PLL.PLLM = HSE_VALUE/1000000;
+  RCC_OscInitStruct.PLL.PLLM = HSE_VALUE / 1000000;
   RCC_OscInitStruct.PLL.PLLN = 432;
   RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
   RCC_OscInitStruct.PLL.PLLQ = 9;
@@ -91,12 +115,14 @@ static inline void board_clock_init(void)
   RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
 
   HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_7);
+
+  UART_CLK_EN();
 }
 
-//static inline void board_vbus_sense_init(void)
-//{
-//
-//}
+static inline void board_vbus_set(uint8_t rhport, bool state) {
+  board_pindef_t* pindef = &board_pindef[rhport ? PINID_VBUS1_EN : PINID_VBUS0_EN];
+  HAL_GPIO_WritePin(pindef->port, pindef->pin_init.Pin, state == pindef->active_state ? GPIO_PIN_SET : GPIO_PIN_RESET);
+}
 
 #ifdef __cplusplus
  }
diff --git a/hw/bsp/stm32f7/boards/stm32f723disco/board.mk b/hw/bsp/stm32f7/boards/stm32f723disco/board.mk
index 43e8eafe1..9b8e7a969 100644
--- a/hw/bsp/stm32f7/boards/stm32f723disco/board.mk
+++ b/hw/bsp/stm32f7/boards/stm32f723disco/board.mk
@@ -1,7 +1,9 @@
 MCU_VARIANT = stm32f723xx
 
-PORT ?= 1
-SPEED ?= high
+# For Hardware test: device default to PORT 0, Host to port 1
+RHPORT_SPEED = OPT_MODE_FULL_SPEED OPT_MODE_HIGH_SPEED
+RHPORT_DEVICE ?= 0
+RHPORT_HOST ?= 1
 
 CFLAGS += \
   -DSTM32F723xx \
diff --git a/hw/bsp/stm32f7/boards/stm32f746disco/board.cmake b/hw/bsp/stm32f7/boards/stm32f746disco/board.cmake
index e44c164b8..bc26c6ef4 100644
--- a/hw/bsp/stm32f7/boards/stm32f746disco/board.cmake
+++ b/hw/bsp/stm32f7/boards/stm32f746disco/board.cmake
@@ -3,12 +3,19 @@ set(JLINK_DEVICE stm32f746xx)
 
 set(LD_FILE_GNU ${CMAKE_CURRENT_LIST_DIR}/STM32F746ZGTx_FLASH.ld)
 
+set(RHPORT_SPEED OPT_MODE_FULL_SPEED OPT_MODE_HIGH_SPEED)
+
+if (NOT DEFINED RHPORT_DEVICE)
+  set(RHPORT_DEVICE 1)
+endif()
+if (NOT DEFINED RHPORT_HOST)
+  set(RHPORT_HOST 0)
+endif()
+
+
 function(update_board TARGET)
   target_compile_definitions(${TARGET} PUBLIC
     STM32F746xx
     HSE_VALUE=25000000
-    # default to PORT 1 High Speed
-    BOARD_TUD_RHPORT=1
-    BOARD_TUD_MAX_SPEED=OPT_MODE_HIGH_SPEED
     )
 endfunction()
diff --git a/hw/bsp/stm32f7/boards/stm32f746disco/board.h b/hw/bsp/stm32f7/boards/stm32f746disco/board.h
index 1c7e6dd2b..d8e92931e 100644
--- a/hw/bsp/stm32f7/boards/stm32f746disco/board.h
+++ b/hw/bsp/stm32f7/boards/stm32f746disco/board.h
@@ -31,28 +31,43 @@
  extern "C" {
 #endif
 
-#define LED_PORT              GPIOI
-#define LED_PIN               GPIO_PIN_1
-#define LED_STATE_ON          1
-
-#define BUTTON_PORT           GPIOI
-#define BUTTON_PIN            GPIO_PIN_11
-#define BUTTON_STATE_ACTIVE   1
-
 #define UART_DEV              USART1
 #define UART_CLK_EN           __HAL_RCC_USART1_CLK_ENABLE
-#define UART_GPIO_AF          GPIO_AF7_USART1
-
-#define UART_TX_PORT          GPIOA
-#define UART_TX_PIN           GPIO_PIN_9
-
-#define UART_RX_PORT          GPIOB
-#define UART_RX_PIN           GPIO_PIN_7
 
 // VBUS Sense detection
 #define OTG_FS_VBUS_SENSE     0
 #define OTG_HS_VBUS_SENSE     0
 
+#define PINID_LED      0
+#define PINID_BUTTON   1
+#define PINID_UART_TX  2
+#define PINID_UART_RX  3
+//#define PINID_VBUS0_EN 4
+//#define PINID_VBUS1_EN 5
+
+static board_pindef_t board_pindef[] = {
+  { // LED
+    .port = GPIOI,
+    .pin_init = { .Pin = GPIO_PIN_1, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // Button
+   .port = GPIOI,
+   .pin_init = { .Pin = GPIO_PIN_11, .Mode = GPIO_MODE_INPUT, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+   .active_state = 1
+  },
+  { // UART TX
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_9, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART1 },
+    .active_state = 0
+  },
+  { // UART RX
+   .port = GPIOB,
+   .pin_init = { .Pin = GPIO_PIN_7, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART1 },
+   .active_state = 0
+  },
+};
+
 //--------------------------------------------------------------------+
 // RCC Clock
 //--------------------------------------------------------------------+
@@ -91,8 +106,15 @@ static inline void board_clock_init(void)
   RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
 
   HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_7);
+
+  UART_CLK_EN();
 }
 
+static inline void board_vbus_set(uint8_t rhport, bool state) {
+ (void) rhport; (void) state;
+}
+
+
 #ifdef __cplusplus
  }
 #endif
diff --git a/hw/bsp/stm32f7/boards/stm32f746disco/board.mk b/hw/bsp/stm32f7/boards/stm32f746disco/board.mk
index 61c0aaa7d..c2b54406e 100644
--- a/hw/bsp/stm32f7/boards/stm32f746disco/board.mk
+++ b/hw/bsp/stm32f7/boards/stm32f746disco/board.mk
@@ -1,5 +1,9 @@
 MCU_VARIANT = stm32f746xx
 
+RHPORT_SPEED = OPT_MODE_FULL_SPEED OPT_MODE_HIGH_SPEED
+RHPORT_DEVICE ?= 1
+RHPORT_HOST ?= 0
+
 PORT ?= 1
 SPEED ?= high
 
diff --git a/hw/bsp/stm32f7/boards/stm32f746nucleo/board.cmake b/hw/bsp/stm32f7/boards/stm32f746nucleo/board.cmake
index dd4d4a753..fd5c1ef11 100644
--- a/hw/bsp/stm32f7/boards/stm32f746nucleo/board.cmake
+++ b/hw/bsp/stm32f7/boards/stm32f746nucleo/board.cmake
@@ -3,11 +3,16 @@ set(JLINK_DEVICE stm32f746xx)
 
 set(LD_FILE_GNU ${CMAKE_CURRENT_LIST_DIR}/STM32F746ZGTx_FLASH.ld)
 
+if (NOT DEFINED RHPORT_DEVICE)
+  set(RHPORT_DEVICE 0)
+endif()
+if (NOT DEFINED RHPORT_HOST)
+  set(RHPORT_HOST 0)
+endif()
+
 function(update_board TARGET)
   target_compile_definitions(${TARGET} PUBLIC
     STM32F746xx
     HSE_VALUE=8000000
-    BOARD_TUD_RHPORT=0
-    BOARD_TUD_MAX_SPEED=OPT_MODE_FULL_SPEED
     )
 endfunction()
diff --git a/hw/bsp/stm32f7/boards/stm32f746nucleo/board.h b/hw/bsp/stm32f7/boards/stm32f746nucleo/board.h
index cf895af87..55e77fe5f 100644
--- a/hw/bsp/stm32f7/boards/stm32f746nucleo/board.h
+++ b/hw/bsp/stm32f7/boards/stm32f746nucleo/board.h
@@ -31,27 +31,43 @@
  extern "C" {
 #endif
 
-#define LED_PORT              GPIOB
-#define LED_PIN               GPIO_PIN_14
-#define LED_STATE_ON          1
-
-#define BUTTON_PORT           GPIOC
-#define BUTTON_PIN            GPIO_PIN_13
-#define BUTTON_STATE_ACTIVE   1
-
 #define UART_DEV              USART3
 #define UART_CLK_EN           __HAL_RCC_USART3_CLK_ENABLE
-#define UART_GPIO_AF          GPIO_AF7_USART3
-
-#define UART_TX_PORT          GPIOD
-#define UART_TX_PIN           GPIO_PIN_8
-#define UART_RX_PORT          GPIOD
-#define UART_RX_PIN           GPIO_PIN_9
 
 // VBUS Sense detection
 #define OTG_FS_VBUS_SENSE     1
 #define OTG_HS_VBUS_SENSE     0
 
+#define PINID_LED      0
+#define PINID_BUTTON   1
+#define PINID_UART_TX  2
+#define PINID_UART_RX  3
+//#define PINID_VBUS0_EN 4
+//#define PINID_VBUS1_EN 5
+
+static board_pindef_t board_pindef[] = {
+  { // LED
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_14, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // Button
+    .port = GPIOC,
+    .pin_init = { .Pin = GPIO_PIN_13, .Mode = GPIO_MODE_INPUT, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // UART TX
+    .port = GPIOD,
+    .pin_init = { .Pin = GPIO_PIN_8, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  },
+  { // UART RX
+    .port = GPIOD,
+    .pin_init = { .Pin = GPIO_PIN_9, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  },
+};
+
 static inline void board_clock_init(void)
 {
   RCC_ClkInitTypeDef RCC_ClkInitStruct;
@@ -89,6 +105,12 @@ static inline void board_clock_init(void)
   RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
 
   HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_7);
+
+  UART_CLK_EN();
+}
+
+static inline void board_vbus_set(uint8_t rhport, bool state) {
+ (void) rhport; (void) state;
 }
 
 #ifdef __cplusplus
diff --git a/hw/bsp/stm32f7/boards/stm32f746nucleo/board.mk b/hw/bsp/stm32f7/boards/stm32f746nucleo/board.mk
index 3683c79a5..fe7104eca 100644
--- a/hw/bsp/stm32f7/boards/stm32f746nucleo/board.mk
+++ b/hw/bsp/stm32f7/boards/stm32f746nucleo/board.mk
@@ -1,5 +1,8 @@
 MCU_VARIANT = stm32f746xx
 
+RHPORT_DEVICE ?= 0
+RHPORT_HOST ?= 0
+
 PORT ?= 0
 SPEED ?= full
 
diff --git a/hw/bsp/stm32f7/boards/stm32f767nucleo/board.cmake b/hw/bsp/stm32f7/boards/stm32f767nucleo/board.cmake
index 679a6ce87..e4cf4c5f5 100644
--- a/hw/bsp/stm32f7/boards/stm32f767nucleo/board.cmake
+++ b/hw/bsp/stm32f7/boards/stm32f767nucleo/board.cmake
@@ -3,11 +3,16 @@ set(JLINK_DEVICE stm32f767zi)
 
 set(LD_FILE_GNU ${CMAKE_CURRENT_LIST_DIR}/STM32F767ZITx_FLASH.ld)
 
+if (NOT DEFINED RHPORT_DEVICE)
+  set(RHPORT_DEVICE 0)
+endif()
+if (NOT DEFINED RHPORT_HOST)
+  set(RHPORT_HOST 0)
+endif()
+
 function(update_board TARGET)
   target_compile_definitions(${TARGET} PUBLIC
     STM32F767xx
     HSE_VALUE=8000000
-    BOARD_TUD_RHPORT=0
-    BOARD_TUD_MAX_SPEED=OPT_MODE_FULL_SPEED
     )
 endfunction()
diff --git a/hw/bsp/stm32f7/boards/stm32f767nucleo/board.h b/hw/bsp/stm32f7/boards/stm32f767nucleo/board.h
index e053572a7..81cb60aeb 100644
--- a/hw/bsp/stm32f7/boards/stm32f767nucleo/board.h
+++ b/hw/bsp/stm32f7/boards/stm32f767nucleo/board.h
@@ -31,28 +31,43 @@
  extern "C" {
 #endif
 
-#define LED_PORT              GPIOB
-#define LED_PIN               GPIO_PIN_14
-#define LED_STATE_ON          1
-
-#define BUTTON_PORT           GPIOC
-#define BUTTON_PIN            GPIO_PIN_13
-#define BUTTON_STATE_ACTIVE   1
-
 #define UART_DEV              USART3
 #define UART_CLK_EN           __HAL_RCC_USART3_CLK_ENABLE
-#define UART_GPIO_AF          GPIO_AF7_USART3
-
-#define UART_TX_PORT          GPIOD
-#define UART_TX_PIN           GPIO_PIN_8
-
-#define UART_RX_PORT          GPIOD
-#define UART_RX_PIN           GPIO_PIN_9
 
 // VBUS Sense detection
 #define OTG_FS_VBUS_SENSE     1
 #define OTG_HS_VBUS_SENSE     0
 
+#define PINID_LED      0
+#define PINID_BUTTON   1
+#define PINID_UART_TX  2
+#define PINID_UART_RX  3
+//#define PINID_VBUS0_EN 4
+//#define PINID_VBUS1_EN 5
+
+static board_pindef_t board_pindef[] = {
+  { // LED
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_14, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // Button
+    .port = GPIOC,
+    .pin_init = { .Pin = GPIO_PIN_13, .Mode = GPIO_MODE_INPUT, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // UART TX
+    .port = GPIOD,
+    .pin_init = { .Pin = GPIO_PIN_8, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  },
+  { // UART RX
+    .port = GPIOD,
+    .pin_init = { .Pin = GPIO_PIN_9, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  },
+};
+
 //--------------------------------------------------------------------+
 // RCC Clock
 //--------------------------------------------------------------------+
@@ -92,8 +107,13 @@ static inline void board_clock_init(void)
   RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
 
   HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_7);
+
+  UART_CLK_EN();
 }
 
+static inline void board_vbus_set(uint8_t rhport, bool state) {
+ (void) rhport; (void) state;
+}
 
 #ifdef __cplusplus
  }
diff --git a/hw/bsp/stm32f7/boards/stm32f767nucleo/board.mk b/hw/bsp/stm32f7/boards/stm32f767nucleo/board.mk
index 059ad166a..d61e0a00d 100644
--- a/hw/bsp/stm32f7/boards/stm32f767nucleo/board.mk
+++ b/hw/bsp/stm32f7/boards/stm32f767nucleo/board.mk
@@ -1,5 +1,8 @@
 MCU_VARIANT = stm32f767xx
 
+RHPORT_DEVICE ?= 0
+RHPORT_HOST ?= 0
+
 PORT ?= 0
 SPEED ?= full
 
diff --git a/hw/bsp/stm32f7/boards/stm32f769disco/board.cmake b/hw/bsp/stm32f7/boards/stm32f769disco/board.cmake
index 329ada093..2335b869e 100644
--- a/hw/bsp/stm32f7/boards/stm32f769disco/board.cmake
+++ b/hw/bsp/stm32f7/boards/stm32f769disco/board.cmake
@@ -3,12 +3,18 @@ set(JLINK_DEVICE stm32f769ni)
 
 set(LD_FILE_GNU ${CMAKE_CURRENT_LIST_DIR}/STM32F769ZITx_FLASH.ld)
 
+set(RHPORT_SPEED OPT_MODE_FULL_SPEED OPT_MODE_HIGH_SPEED)
+
+if (NOT DEFINED RHPORT_DEVICE)
+  set(RHPORT_DEVICE 1)
+endif()
+if (NOT DEFINED RHPORT_HOST)
+  set(RHPORT_HOST 1)
+endif()
+
 function(update_board TARGET)
   target_compile_definitions(${TARGET} PUBLIC
     STM32F769xx
     HSE_VALUE=25000000
-    # default to PORT 1 High Speed
-    BOARD_TUD_RHPORT=1
-    BOARD_TUD_MAX_SPEED=OPT_MODE_HIGH_SPEED
     )
 endfunction()
diff --git a/hw/bsp/stm32f7/boards/stm32f769disco/board.h b/hw/bsp/stm32f7/boards/stm32f769disco/board.h
index 472af0b98..268919b61 100644
--- a/hw/bsp/stm32f7/boards/stm32f769disco/board.h
+++ b/hw/bsp/stm32f7/boards/stm32f769disco/board.h
@@ -31,28 +31,43 @@
  extern "C" {
 #endif
 
-#define LED_PORT              GPIOJ
-#define LED_PIN               GPIO_PIN_12
-#define LED_STATE_ON          5
-
-#define BUTTON_PORT           GPIOA
-#define BUTTON_PIN            GPIO_PIN_0
-#define BUTTON_STATE_ACTIVE   1
-
 #define UART_DEV              USART1
 #define UART_CLK_EN           __HAL_RCC_USART1_CLK_ENABLE
-#define UART_GPIO_AF          GPIO_AF7_USART1
-
-#define UART_TX_PORT          GPIOA
-#define UART_TX_PIN           GPIO_PIN_9
-
-#define UART_RX_PORT          GPIOA
-#define UART_RX_PIN           GPIO_PIN_10
 
 // VBUS Sense detection
 #define OTG_FS_VBUS_SENSE     1
 #define OTG_HS_VBUS_SENSE     0
 
+#define PINID_LED      0
+#define PINID_BUTTON   1
+#define PINID_UART_TX  2
+#define PINID_UART_RX  3
+//#define PINID_VBUS0_EN 4
+//#define PINID_VBUS1_EN 5
+
+static board_pindef_t board_pindef[] = {
+  { // LED
+    .port = GPIOJ,
+    .pin_init = { .Pin = GPIO_PIN_12, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // Button
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_0, .Mode = GPIO_MODE_INPUT, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // UART TX
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_9, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART1 },
+    .active_state = 0
+  },
+  { // UART RX
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_10, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART1 },
+    .active_state = 0
+  },
+};
+
 //--------------------------------------------------------------------+
 // RCC Clock
 //--------------------------------------------------------------------+
@@ -92,6 +107,12 @@ static inline void board_clock_init(void)
   RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
 
   HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_7);
+
+  UART_CLK_EN();
+}
+
+static inline void board_vbus_set(uint8_t rhport, bool state) {
+  (void) rhport; (void) state;
 }
 
 #ifdef __cplusplus
diff --git a/hw/bsp/stm32f7/boards/stm32f769disco/board.mk b/hw/bsp/stm32f7/boards/stm32f769disco/board.mk
index 705f1a633..e756c9727 100644
--- a/hw/bsp/stm32f7/boards/stm32f769disco/board.mk
+++ b/hw/bsp/stm32f7/boards/stm32f769disco/board.mk
@@ -1,6 +1,10 @@
 MCU_VARIANT = stm32f769xx
 
 # Only OTG-HS has a connector on this board
+RHPORT_SPEED = OPT_MODE_FULL_SPEED OPT_MODE_HIGH_SPEED
+RHPORT_DEVICE ?= 1
+RHPORT_HOST ?= 1
+
 PORT ?= 1
 SPEED ?= high
 
diff --git a/hw/bsp/stm32f7/family.c b/hw/bsp/stm32f7/family.c
index 61f1d2a7f..527fbfe5c 100644
--- a/hw/bsp/stm32f7/family.c
+++ b/hw/bsp/stm32f7/family.c
@@ -28,27 +28,51 @@
 
 #include "stm32f7xx_hal.h"
 #include "bsp/board_api.h"
+
+typedef struct {
+  GPIO_TypeDef* port;
+  GPIO_InitTypeDef pin_init;
+  uint8_t active_state;
+} board_pindef_t;
+
 #include "board.h"
 
 //--------------------------------------------------------------------+
+// MACRO TYPEDEF CONSTANT ENUM
+//--------------------------------------------------------------------+
+
+#ifdef UART_DEV
+UART_HandleTypeDef UartHandle = {
+  .Instance = UART_DEV,
+  .Init = {
+    .BaudRate = CFG_BOARD_UART_BAUDRATE,
+    .WordLength = UART_WORDLENGTH_8B,
+    .StopBits = UART_STOPBITS_1,
+    .Parity = UART_PARITY_NONE,
+    .HwFlowCtl = UART_HWCONTROL_NONE,
+    .Mode = UART_MODE_TX_RX,
+    .OverSampling = UART_OVERSAMPLING_16,
+  }
+};
+#endif
+
+//--------------------------------------------------------------------+
 // Forward USB interrupt events to TinyUSB IRQ Handler
 //--------------------------------------------------------------------+
 void OTG_FS_IRQHandler(void) {
-  tud_int_handler(0);
+  tusb_int_handler(0, true);
 }
 
 // Despite being call USB2_OTG
 // OTG_HS is marked as RHPort1 by TinyUSB to be consistent across stm32 port
 void OTG_HS_IRQHandler(void) {
-  tud_int_handler(1);
+  tusb_int_handler(1, true);
 }
 
 //--------------------------------------------------------------------+
 // MACRO TYPEDEF CONSTANT ENUM
 //--------------------------------------------------------------------+
 
-UART_HandleTypeDef UartHandle;
-
 void board_init(void) {
   board_clock_init();
 
@@ -60,12 +84,13 @@ void board_init(void) {
   __HAL_RCC_GPIOG_CLK_ENABLE();
   __HAL_RCC_GPIOH_CLK_ENABLE();  // ULPI NXT
   __HAL_RCC_GPIOI_CLK_ENABLE();  // ULPI NXT
-
 #ifdef __HAL_RCC_GPIOJ_CLK_ENABLE
   __HAL_RCC_GPIOJ_CLK_ENABLE();
 #endif
 
-      UART_CLK_EN();
+  for (uint8_t i = 0; i < TU_ARRAY_SIZE(board_pindef); i++) {
+    HAL_GPIO_Init(board_pindef[i].port, &board_pindef[i].pin_init);
+  }
 
 #if CFG_TUSB_OS == OPT_OS_NONE
   // 1ms tick timer
@@ -80,51 +105,13 @@ void board_init(void) {
   NVIC_SetPriority(OTG_HS_IRQn, configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY );
 #endif
 
-  GPIO_InitTypeDef GPIO_InitStruct;
-
-  // LED
-  GPIO_InitStruct.Pin = LED_PIN;
-  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
-  GPIO_InitStruct.Pull = GPIO_PULLUP;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
-  HAL_GPIO_Init(LED_PORT, &GPIO_InitStruct);
-
-  // Button
-  GPIO_InitStruct.Pin = BUTTON_PIN;
-  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
-  GPIO_InitStruct.Pull = GPIO_NOPULL;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
-  HAL_GPIO_Init(BUTTON_PORT, &GPIO_InitStruct);
-
-  // Uart TX
-  GPIO_InitStruct.Pin = UART_TX_PIN;
-  GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
-  GPIO_InitStruct.Pull = GPIO_PULLUP;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
-  GPIO_InitStruct.Alternate = UART_GPIO_AF;
-  HAL_GPIO_Init(UART_TX_PORT, &GPIO_InitStruct);
-
-  // Uart RX
-  GPIO_InitStruct.Pin = UART_RX_PIN;
-  GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
-  GPIO_InitStruct.Pull = GPIO_PULLUP;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
-  GPIO_InitStruct.Alternate = UART_GPIO_AF;
-  HAL_GPIO_Init(UART_RX_PORT, &GPIO_InitStruct);
-
-  UartHandle.Instance = UART_DEV;
-  UartHandle.Init.BaudRate = CFG_BOARD_UART_BAUDRATE;
-  UartHandle.Init.WordLength = UART_WORDLENGTH_8B;
-  UartHandle.Init.StopBits = UART_STOPBITS_1;
-  UartHandle.Init.Parity = UART_PARITY_NONE;
-  UartHandle.Init.HwFlowCtl = UART_HWCONTROL_NONE;
-  UartHandle.Init.Mode = UART_MODE_TX_RX;
-  UartHandle.Init.OverSampling = UART_OVERSAMPLING_16;
+#ifdef UART_DEV
   HAL_UART_Init(&UartHandle);
+#endif
 
-#if BOARD_TUD_RHPORT == 0
-  // OTG_FS
+  GPIO_InitTypeDef GPIO_InitStruct;
 
+  //------------- rhport0: OTG_FS -------------//
   /* Configure DM DP Pins */
   GPIO_InitStruct.Pin = (GPIO_PIN_11 | GPIO_PIN_12);
   GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
@@ -171,10 +158,8 @@ void board_init(void) {
   USB_OTG_FS->GOTGCTL |= USB_OTG_GOTGCTL_BVALOVAL;
 #endif // vbus sense
 
-#else
-  // OTG_HS
-
-  #ifdef USB_HS_PHYC
+  //------------- rhport1: OTG_HS -------------//
+#ifdef USB_HS_PHYC
   // MCU with built-in HS PHY such as F723, F733, F730
 
   /* Configure DM DP Pins */
@@ -198,7 +183,7 @@ void board_init(void) {
   /* Enable PHYC Clocks */
   __HAL_RCC_OTGPHYC_CLK_ENABLE();
 
-  #else
+#else
   // MCU with external ULPI PHY
 
   /* ULPI CLK */
@@ -244,7 +229,7 @@ void board_init(void) {
   GPIO_InitStruct.Pull = GPIO_NOPULL;
   GPIO_InitStruct.Alternate = GPIO_AF10_OTG_HS;
   HAL_GPIO_Init(GPIOI, &GPIO_InitStruct);
-  #endif // USB_HS_PHYC
+#endif // USB_HS_PHYC
 
   // Enable USB HS & ULPI Clocks
   __HAL_RCC_USB_OTG_HS_ULPI_CLK_ENABLE();
@@ -261,12 +246,10 @@ void board_init(void) {
   USB_OTG_HS->GOTGCTL |= USB_OTG_GOTGCTL_BVALOVAL;
 #endif
 
-  // Force device mode
-  USB_OTG_HS->GUSBCFG &= ~USB_OTG_GUSBCFG_FHMOD;
-  USB_OTG_HS->GUSBCFG |= USB_OTG_GUSBCFG_FDMOD;
-
-#endif // BOARD_TUD_RHPORT
-
+  // Turn on host vbus
+#if CFG_TUH_ENABLED
+  board_vbus_set(BOARD_TUH_RHPORT, true);
+#endif
 }
 
 //--------------------------------------------------------------------+
@@ -274,12 +257,22 @@ void board_init(void) {
 //--------------------------------------------------------------------+
 
 void board_led_write(bool state) {
-  GPIO_PinState pin_state = (GPIO_PinState) (state ? LED_STATE_ON : (1 - LED_STATE_ON));
-  HAL_GPIO_WritePin(LED_PORT, LED_PIN, pin_state);
+#ifdef PINID_LED
+  board_pindef_t* pindef = &board_pindef[PINID_LED];
+  GPIO_PinState pin_state = state == pindef->active_state ? GPIO_PIN_SET : GPIO_PIN_RESET;
+  HAL_GPIO_WritePin(pindef->port, pindef->pin_init.Pin, pin_state);
+#else
+  (void) state;
+#endif
 }
 
 uint32_t board_button_read(void) {
-  return HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN);
+#ifdef PINID_BUTTON
+  board_pindef_t* pindef = &board_pindef[PINID_BUTTON];
+  return pindef->active_state == HAL_GPIO_ReadPin(pindef->port, pindef->pin_init.Pin);
+#else
+  return 0;
+#endif
 }
 
 size_t board_get_unique_id(uint8_t id[], size_t max_len) {
@@ -302,8 +295,13 @@ int board_uart_read(uint8_t *buf, int len) {
 }
 
 int board_uart_write(void const *buf, int len) {
+#ifdef UART_DEV
   HAL_UART_Transmit(&UartHandle, (uint8_t *) (uintptr_t) buf, len, 0xffff);
   return len;
+#else
+  (void) buf; (void) len;
+  return -1;
+#endif
 }
 
 #if CFG_TUSB_OS == OPT_OS_NONE
diff --git a/hw/bsp/stm32f7/family.cmake b/hw/bsp/stm32f7/family.cmake
index 938c2697a..dc134e9c0 100644
--- a/hw/bsp/stm32f7/family.cmake
+++ b/hw/bsp/stm32f7/family.cmake
@@ -16,6 +16,28 @@ set(CMAKE_TOOLCHAIN_FILE ${TOP}/examples/build_system/cmake/toolchain/arm_${TOOL
 
 set(FAMILY_MCUS STM32F7 CACHE INTERNAL "")
 
+# ----------------------
+# Port & Speed Selection
+# ----------------------
+if (NOT DEFINED RHPORT_DEVICE)
+  set(RHPORT_DEVICE 0)
+endif ()
+if (NOT DEFINED RHPORT_HOST)
+  set(RHPORT_HOST 0)
+endif ()
+
+if (NOT DEFINED RHPORT_SPEED)
+  # Most F7 does not has built-in HS PHY
+  set(RHPORT_SPEED OPT_MODE_FULL_SPEED OPT_MODE_FULL_SPEED)
+endif ()
+if (NOT DEFINED RHPORT_DEVICE_SPEED)
+  list(GET RHPORT_SPEED ${RHPORT_DEVICE} RHPORT_DEVICE_SPEED)
+endif ()
+if (NOT DEFINED RHPORT_HOST_SPEED)
+  list(GET RHPORT_SPEED ${RHPORT_HOST} RHPORT_HOST_SPEED)
+endif ()
+
+cmake_print_variables(RHPORT_DEVICE RHPORT_DEVICE_SPEED RHPORT_HOST RHPORT_HOST_SPEED)
 
 #------------------------------------
 # BOARD_TARGET
@@ -54,8 +76,12 @@ function(add_board_target BOARD_TARGET)
     ${ST_CMSIS}/Include
     ${ST_HAL_DRIVER}/Inc
     )
-  #target_compile_options(${BOARD_TARGET} PUBLIC)
-  #target_compile_definitions(${BOARD_TARGET} PUBLIC)
+  target_compile_definitions(${BOARD_TARGET} PUBLIC
+    BOARD_TUD_RHPORT=${RHPORT_DEVICE}
+    BOARD_TUD_MAX_SPEED=${RHPORT_DEVICE_SPEED}
+    BOARD_TUH_RHPORT=${RHPORT_HOST}
+    BOARD_TUH_MAX_SPEED=${RHPORT_HOST_SPEED}
+    )
 
   update_board(${BOARD_TARGET})
 
@@ -104,6 +130,8 @@ function(family_configure_example TARGET RTOS)
   family_add_tinyusb(${TARGET} OPT_MCU_STM32F7 ${RTOS})
   target_sources(${TARGET}-tinyusb PUBLIC
     ${TOP}/src/portable/synopsys/dwc2/dcd_dwc2.c
+    ${TOP}/src/portable/synopsys/dwc2/hcd_dwc2.c
+    ${TOP}/src/portable/synopsys/dwc2/dwc2_common.c
     )
   target_link_libraries(${TARGET}-tinyusb PUBLIC board_${BOARD})
 
diff --git a/hw/bsp/stm32f7/family.mk b/hw/bsp/stm32f7/family.mk
index bae7d6d5b..abeea784c 100644
--- a/hw/bsp/stm32f7/family.mk
+++ b/hw/bsp/stm32f7/family.mk
@@ -8,25 +8,53 @@ ST_HAL_DRIVER = hw/mcu/st/stm32$(ST_FAMILY)xx_hal_driver
 include $(TOP)/$(BOARD_PATH)/board.mk
 CPU_CORE ?= cortex-m7-fpsp
 
+# ----------------------
+# Port & Speed Selection
+# ----------------------
+RHPORT_SPEED ?= OPT_MODE_FULL_SPEED OPT_MODE_FULL_SPEED
+RHPORT_DEVICE ?= 0
+RHPORT_HOST ?= 0
+
+# Determine RHPORT_DEVICE_SPEED if not defined
+ifndef RHPORT_DEVICE_SPEED
+ifeq ($(RHPORT_DEVICE), 0)
+  RHPORT_DEVICE_SPEED = $(firstword $(RHPORT_SPEED))
+else
+  RHPORT_DEVICE_SPEED = $(lastword $(RHPORT_SPEED))
+endif
+endif
+
+# Determine RHPORT_HOST_SPEED if not defined
+ifndef RHPORT_HOST_SPEED
+ifeq ($(RHPORT_HOST), 0)
+  RHPORT_HOST_SPEED = $(firstword $(RHPORT_SPEED))
+else
+  RHPORT_HOST_SPEED = $(lastword $(RHPORT_SPEED))
+endif
+endif
+
 # --------------
 # Compiler Flags
 # --------------
 CFLAGS += \
   -DCFG_TUSB_MCU=OPT_MCU_STM32F7 \
-  -DBOARD_TUD_RHPORT=$(PORT)
+	-DBOARD_TUD_RHPORT=${RHPORT_DEVICE} \
+	-DBOARD_TUD_MAX_SPEED=${RHPORT_DEVICE_SPEED} \
+	-DBOARD_TUH_RHPORT=${RHPORT_HOST} \
+	-DBOARD_TUH_MAX_SPEED=${RHPORT_HOST_SPEED} \
 
-ifeq ($(PORT), 1)
-  ifeq ($(SPEED), high)
-    CFLAGS += -DBOARD_TUD_MAX_SPEED=OPT_MODE_HIGH_SPEED
-    $(info "Using OTG_HS in HighSpeed mode")
-  else
-    CFLAGS += -DBOARD_TUD_MAX_SPEED=OPT_MODE_FULL_SPEED
-    $(info "Using OTG_HS in FullSpeed mode")
-  endif
-else
-  CFLAGS += -DBOARD_TUD_MAX_SPEED=OPT_MODE_FULL_SPEED
-  $(info "Using OTG_FS")
-endif
+#ifeq ($(PORT), 1)
+#  ifeq ($(SPEED), high)
+#    CFLAGS += -DBOARD_TUD_MAX_SPEED=OPT_MODE_HIGH_SPEED
+#    $(info "Using OTG_HS in HighSpeed mode")
+#  else
+#    CFLAGS += -DBOARD_TUD_MAX_SPEED=OPT_MODE_FULL_SPEED
+#    $(info "Using OTG_HS in FullSpeed mode")
+#  endif
+#else
+#  CFLAGS += -DBOARD_TUD_MAX_SPEED=OPT_MODE_FULL_SPEED
+#  $(info "Using OTG_FS")
+#endif
 
 # GCC Flags
 CFLAGS_GCC += \
@@ -45,6 +73,8 @@ LDFLAGS_GCC += \
 
 SRC_C += \
 	src/portable/synopsys/dwc2/dcd_dwc2.c \
+	src/portable/synopsys/dwc2/hcd_dwc2.c \
+	src/portable/synopsys/dwc2/dwc2_common.c \
 	$(ST_CMSIS)/Source/Templates/system_stm32$(ST_FAMILY)xx.c \
 	$(ST_HAL_DRIVER)/Src/stm32$(ST_FAMILY)xx_hal.c \
 	$(ST_HAL_DRIVER)/Src/stm32$(ST_FAMILY)xx_hal_cortex.c \
diff --git a/hw/bsp/stm32h7/boards/daisyseed/board.cmake b/hw/bsp/stm32h7/boards/daisyseed/board.cmake
index 4811c97e8..ff22b3e22 100644
--- a/hw/bsp/stm32h7/boards/daisyseed/board.cmake
+++ b/hw/bsp/stm32h7/boards/daisyseed/board.cmake
@@ -8,8 +8,5 @@ function(update_board TARGET)
     STM32H750xx
     HSE_VALUE=16000000
     CORE_CM7
-    # default to PORT 0
-    BOARD_TUD_RHPORT=0
-    BOARD_TUD_MAX_SPEED=OPT_MODE_FULL_SPEED
     )
 endfunction()
diff --git a/hw/bsp/stm32h7/boards/daisyseed/board.h b/hw/bsp/stm32h7/boards/daisyseed/board.h
index abc07488b..2d681d640 100644
--- a/hw/bsp/stm32h7/boards/daisyseed/board.h
+++ b/hw/bsp/stm32h7/boards/daisyseed/board.h
@@ -31,32 +31,46 @@
  extern "C" {
 #endif
 
-#define LED_PORT              GPIOC
-#define LED_PIN               GPIO_PIN_7
-#define LED_STATE_ON          1
-
-// Blue push-button
-#define BUTTON_PORT           GPIOC
-#define BUTTON_PIN            GPIO_PIN_13
-#define BUTTON_STATE_ACTIVE   1
-
 // UART
 #define UART_DEV              USART3
 #define UART_CLK_EN           __HAL_RCC_USART3_CLK_ENABLE
-#define UART_GPIO_PORT        GPIOB
-#define UART_GPIO_AF          GPIO_AF7_USART3
-#define UART_TX_PIN           GPIO_PIN_10
-#define UART_RX_PIN           GPIO_PIN_11
 
 // VBUS Sense detection
 #define OTG_FS_VBUS_SENSE     1
 #define OTG_HS_VBUS_SENSE     0
 
+#define PINID_LED      0
+#define PINID_BUTTON   1
+#define PINID_UART_TX  2
+#define PINID_UART_RX  3
+
+static board_pindef_t board_pindef[] = {
+  { // LED
+    .port = GPIOC,
+    .pin_init = { .Pin = GPIO_PIN_7, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // Button
+    .port = GPIOC,
+    .pin_init = { .Pin = GPIO_PIN_13, .Mode = GPIO_MODE_INPUT, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // UART TX
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_10, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  },
+  { // UART RX
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_11, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  }
+};
+
 //--------------------------------------------------------------------+
 // RCC Clock
 //--------------------------------------------------------------------+
-static inline void SystemClock_Config(void)
-{
+static inline void SystemClock_Config(void) {
   RCC_ClkInitTypeDef RCC_ClkInitStruct = { 0 };
   RCC_OscInitTypeDef RCC_OscInitStruct = { 0 };
   RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = { 0 };
@@ -128,11 +142,14 @@ static inline void SystemClock_Config(void)
   HAL_EnableCompensationCell();
 }
 
-static inline void board_stm32h7_post_init(void)
-{
+static inline void board_init2(void) {
   // For this board does nothing
 }
 
+void board_vbus_set(uint8_t rhport, bool state) {
+  (void) rhport; (void) state;
+}
+
 #ifdef __cplusplus
  }
 #endif
diff --git a/hw/bsp/stm32h7/boards/daisyseed/board.mk b/hw/bsp/stm32h7/boards/daisyseed/board.mk
index da2eb4433..bb254cfc2 100644
--- a/hw/bsp/stm32h7/boards/daisyseed/board.mk
+++ b/hw/bsp/stm32h7/boards/daisyseed/board.mk
@@ -1,16 +1,8 @@
+MCU_VARIANT = stm32h750xx
 CFLAGS += -DSTM32H750xx -DCORE_CM7 -DHSE_VALUE=16000000
 
-# Default is FulSpeed port
-PORT ?= 0
-
-# GCC
-SRC_S_GCC += $(ST_CMSIS)/Source/Templates/gcc/startup_stm32h750xx.s
 LD_FILE_GCC = $(BOARD_PATH)/stm32h750ibkx_flash.ld
 
-# IAR
-SRC_S_IAR += $(ST_CMSIS)/Source/Templates/iar/startup_stm32h750xx.s
-LD_FILE_IAR = $(ST_CMSIS)/Source/Templates/iar/linker/stm32h750xx_flash.icf
-
 # For flash-jlink target
 JLINK_DEVICE = stm32h750ibk6_m7
 
diff --git a/hw/bsp/stm32h7/boards/stm32h723nucleo/board.cmake b/hw/bsp/stm32h7/boards/stm32h723nucleo/board.cmake
index b7d133dfa..a6f0a5c45 100644
--- a/hw/bsp/stm32h7/boards/stm32h723nucleo/board.cmake
+++ b/hw/bsp/stm32h7/boards/stm32h723nucleo/board.cmake
@@ -7,8 +7,5 @@ function(update_board TARGET)
   target_compile_definitions(${TARGET} PUBLIC
     STM32H723xx
     HSE_VALUE=8000000
-    # default to PORT 0
-    BOARD_TUD_RHPORT=0
-    BOARD_TUD_MAX_SPEED=OPT_MODE_FULL_SPEED
     )
 endfunction()
diff --git a/hw/bsp/stm32h7/boards/stm32h723nucleo/board.h b/hw/bsp/stm32h7/boards/stm32h723nucleo/board.h
index 3d9344a87..c5257901d 100644
--- a/hw/bsp/stm32h7/boards/stm32h723nucleo/board.h
+++ b/hw/bsp/stm32h7/boards/stm32h723nucleo/board.h
@@ -31,20 +31,8 @@
  extern "C" {
 #endif
 
-#define LED_PORT              GPIOB
-#define LED_PIN               GPIO_PIN_0
-#define LED_STATE_ON          1
-
-#define BUTTON_PORT           GPIOC
-#define BUTTON_PIN            GPIO_PIN_13
-#define BUTTON_STATE_ACTIVE   1
-
 #define UART_DEV              USART3
 #define UART_CLK_EN           __HAL_RCC_USART3_CLK_ENABLE
-#define UART_GPIO_PORT        GPIOD
-#define UART_GPIO_AF          GPIO_AF7_USART3
-#define UART_TX_PIN           GPIO_PIN_8
-#define UART_RX_PIN           GPIO_PIN_9
 
 // VBUS Sense detection
 #define OTG_FS_VBUS_SENSE     1
@@ -59,6 +47,40 @@
 #define GPIO_AF10_OTG2_HS               GPIO_AF10_OTG1_HS
 #define USB_OTG_FS                      USB_OTG_HS
 
+#define PINID_LED      0
+#define PINID_BUTTON   1
+#define PINID_UART_TX  2
+#define PINID_UART_RX  3
+#define PINID_VBUS0_EN 4
+
+static board_pindef_t board_pindef[] = {
+  { // LED
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_0, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // Button
+    .port = GPIOC,
+    .pin_init = { .Pin = GPIO_PIN_13, .Mode = GPIO_MODE_INPUT, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // UART TX
+    .port = GPIOD,
+    .pin_init = { .Pin = GPIO_PIN_8, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  },
+  { // UART RX
+    .port = GPIOD,
+    .pin_init = { .Pin = GPIO_PIN_9, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  },
+  { // VBUS0 EN
+    .port = GPIOD,
+    .pin_init = { .Pin = GPIO_PIN_10, .Mode = GPIO_MODE_OUTPUT_OD, .Pull = GPIO_NOPULL, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 0
+  }
+};
+
 //--------------------------------------------------------------------+
 // RCC Clock
 //--------------------------------------------------------------------+
@@ -118,11 +140,16 @@ static inline void SystemClock_Config(void)
   HAL_RCCEx_PeriphCLKConfig(&RCC_PeriphCLKInitStruct);
 }
 
-static inline void board_stm32h7_post_init(void)
-{
+static inline void board_init2(void) {
   // For this board does nothing
 }
 
+void board_vbus_set(uint8_t rhport, bool state) {
+  if (rhport == 0) {
+    board_pindef_t* pindef = &board_pindef[PINID_VBUS0_EN];
+    HAL_GPIO_WritePin(pindef->port, pindef->pin_init.Pin, state == pindef->active_state ? GPIO_PIN_SET : GPIO_PIN_RESET);
+  }
+}
 
 #ifdef __cplusplus
  }
diff --git a/hw/bsp/stm32h7/boards/stm32h723nucleo/board.mk b/hw/bsp/stm32h7/boards/stm32h723nucleo/board.mk
index 57a316f41..c1a98a025 100644
--- a/hw/bsp/stm32h7/boards/stm32h723nucleo/board.mk
+++ b/hw/bsp/stm32h7/boards/stm32h723nucleo/board.mk
@@ -1,15 +1,7 @@
+MCU_VARIANT = stm32h723xx
 CFLAGS += -DSTM32H723xx -DHSE_VALUE=8000000
 
-# Default is FulSpeed port
-PORT ?= 0
-
-# GCC
-SRC_S_GCC += $(ST_CMSIS)/Source/Templates/gcc/startup_stm32h723xx.s
-LD_FILE_GCC =  $(FAMILY_PATH)/linker/stm32h723xx_flash.ld
-
-# IAR
-SRC_S_IAR += $(ST_CMSIS)/Source/Templates/iar/startup_stm32h723xx.s
-LD_FILE_IAR = $(ST_CMSIS)/Source/Templates/iar/linker/stm32h723xx_flash.icf
+LD_FILE_GCC = $(FAMILY_PATH)/linker/${MCU_VARIANT}_flash.ld
 
 # For flash-jlink target
 JLINK_DEVICE = stm32h723zg
diff --git a/hw/bsp/stm32h7/boards/stm32h743eval/board.cmake b/hw/bsp/stm32h7/boards/stm32h743eval/board.cmake
index 6d7a97741..02c6bf5fa 100644
--- a/hw/bsp/stm32h7/boards/stm32h743eval/board.cmake
+++ b/hw/bsp/stm32h7/boards/stm32h743eval/board.cmake
@@ -1,15 +1,29 @@
 set(MCU_VARIANT stm32h743xx)
 set(JLINK_DEVICE stm32h743xi)
-# set(JLINK_OPTION "-USB jtrace")
+#set(JLINK_OPTION "-USB jtrace")
 
 set(LD_FILE_GNU ${CMAKE_CURRENT_LIST_DIR}/../../linker/${MCU_VARIANT}_flash.ld)
 
+set(RHPORT_SPEED OPT_MODE_FULL_SPEED OPT_MODE_HIGH_SPEED)
+
+# device default to PORT 1 High Speed
+if (NOT DEFINED RHPORT_DEVICE)
+  set(RHPORT_DEVICE 1)
+endif()
+if (NOT DEFINED RHPORT_HOST)
+  set(RHPORT_HOST 0)
+endif()
+
 function(update_board TARGET)
+  target_sources(${TARGET} PUBLIC
+    ${ST_MFXSTM32L152}/mfxstm32l152.c
+    ${ST_MFXSTM32L152}/mfxstm32l152_reg.c
+    )
+  target_include_directories(${TARGET} PUBLIC
+    ${ST_MFXSTM32L152}
+    )
   target_compile_definitions(${TARGET} PUBLIC
     STM32H743xx
     HSE_VALUE=25000000
-    # default to PORT 1 High Speed
-    BOARD_TUD_RHPORT=1
-    BOARD_TUD_MAX_SPEED=OPT_MODE_HIGH_SPEED
     )
 endfunction()
diff --git a/hw/bsp/stm32h7/boards/stm32h743eval/board.h b/hw/bsp/stm32h7/boards/stm32h743eval/board.h
index 22d66d735..fa9721be3 100644
--- a/hw/bsp/stm32h7/boards/stm32h743eval/board.h
+++ b/hw/bsp/stm32h7/boards/stm32h743eval/board.h
@@ -31,22 +31,11 @@
  extern "C" {
 #endif
 
-#define LED_PORT              GPIOA
-#define LED_PIN               GPIO_PIN_4
-#define LED_STATE_ON          1
-
-// Tamper push-button
-#define BUTTON_PORT           GPIOC
-#define BUTTON_PIN            GPIO_PIN_13
-#define BUTTON_STATE_ACTIVE   0
+#include "mfxstm32l152.h"
 
 // Need to change jumper setting J7 and J8 from RS-232 to STLink
 #define UART_DEV              USART1
 #define UART_CLK_EN           __HAL_RCC_USART1_CLK_ENABLE
-#define UART_GPIO_PORT        GPIOB
-#define UART_GPIO_AF          GPIO_AF4_USART1
-#define UART_TX_PIN           GPIO_PIN_14
-#define UART_RX_PIN           GPIO_PIN_15
 
 // VBUS Sense detection
 #define OTG_FS_VBUS_SENSE     1
@@ -58,6 +47,44 @@
   {GPIOB, GPIO_PIN_5 }, {GPIOB, GPIO_PIN_10}, {GPIOB, GPIO_PIN_11}, {GPIOB, GPIO_PIN_12}, \
   {GPIOB, GPIO_PIN_13}, {GPIOC, GPIO_PIN_0 }, {GPIOH, GPIO_PIN_4 }, {GPIOI, GPIO_PIN_11}
 
+#define PINID_LED      0
+#define PINID_BUTTON   1
+#define PINID_UART_TX  2
+#define PINID_UART_RX  3
+
+static board_pindef_t board_pindef[] = {
+  { // LED
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_4, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // Button
+    .port = GPIOC,
+    .pin_init = { .Pin = GPIO_PIN_13, .Mode = GPIO_MODE_INPUT, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 0
+  },
+  { // UART TX
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_14, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF4_USART1 },
+    .active_state = 0
+  },
+  { // UART RX
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_15, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF4_USART1 },
+    .active_state = 0
+  },
+  { // I2C SCL for MFX VBUS
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_6, .Mode = GPIO_MODE_AF_OD, .Pull = GPIO_NOPULL, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF4_I2C1 },
+    .active_state = 0
+  },
+  { // I2C SDA for MFX VBUS
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_7, .Mode = GPIO_MODE_AF_OD, .Pull = GPIO_NOPULL, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF4_I2C1 },
+    .active_state = 1
+  },
+};
+
 //--------------------------------------------------------------------+
 // RCC Clock
 //--------------------------------------------------------------------+
@@ -132,9 +159,95 @@ static inline void SystemClock_Config(void) {
   HAL_EnableCompensationCell();
 }
 
-static inline void board_stm32h7_post_init(void)
-{
-  // For this board does nothing
+//--------------------------------------------------------------------+
+// MFX
+//--------------------------------------------------------------------+
+static I2C_HandleTypeDef i2c_handle = {
+  .Instance = I2C1,
+  .Init = {
+    .Timing = 0x10C0ECFF,
+    .OwnAddress1 = 0,
+    .AddressingMode = I2C_ADDRESSINGMODE_7BIT,
+    .DualAddressMode = I2C_DUALADDRESS_DISABLE,
+    .OwnAddress2 = 0,
+    .OwnAddress2Masks = I2C_OA2_NOMASK,
+    .GeneralCallMode = I2C_GENERALCALL_DISABLE,
+    .NoStretchMode = I2C_NOSTRETCH_DISABLE,
+  }
+};
+static MFXSTM32L152_Object_t  mfx_obj = { 0 };
+static MFXSTM32L152_IO_Mode_t* mfx_io = NULL;
+static uint32_t mfx_vbus_pin[2] = { MFXSTM32L152_GPIO_PIN_7, MFXSTM32L152_GPIO_PIN_9 };
+
+int32_t board_i2c_init(void) {
+  __HAL_RCC_I2C1_CLK_ENABLE();
+  __HAL_RCC_I2C1_FORCE_RESET();
+  __HAL_RCC_I2C1_RELEASE_RESET();
+  if (HAL_I2C_Init(&i2c_handle) != HAL_OK) {
+    return HAL_ERROR;
+  }
+  if (HAL_I2CEx_ConfigAnalogFilter(&i2c_handle, I2C_ANALOGFILTER_ENABLE) != HAL_OK) {
+    return HAL_ERROR;
+  }
+  if (HAL_I2CEx_ConfigDigitalFilter(&i2c_handle, 0) != HAL_OK) {
+    return HAL_ERROR;
+  }
+  return 0;
+}
+
+int32_t board_i2c_deinit(void) {
+  return 0;
+}
+
+int32_t i2c_readreg(uint16_t DevAddr, uint16_t Reg, uint8_t *pData, uint16_t Length) {
+  TU_ASSERT (HAL_OK == HAL_I2C_Mem_Read(&i2c_handle, DevAddr, Reg, I2C_MEMADD_SIZE_8BIT, pData, Length, 10000));
+  return 0;
+}
+
+int32_t i2c_writereg(uint16_t DevAddr, uint16_t Reg, uint8_t *pData, uint16_t Length) {
+  TU_ASSERT(HAL_OK == HAL_I2C_Mem_Write(&i2c_handle, DevAddr, Reg, I2C_MEMADD_SIZE_8BIT, pData, Length, 10000));
+  return 0;
+}
+
+static inline void board_init2(void) {
+  // IO control via MFX
+  MFXSTM32L152_IO_t io_ctx;
+  io_ctx.Init        = board_i2c_init;
+  io_ctx.DeInit      = board_i2c_deinit;
+  io_ctx.ReadReg     = i2c_readreg;
+  io_ctx.WriteReg    = i2c_writereg;
+  io_ctx.GetTick     = (MFXSTM32L152_GetTick_Func) HAL_GetTick;
+
+  uint16_t i2c_addr[] = { 0x84, 0x86 };
+  for(uint8_t i = 0U; i < 2U; i++) {
+    uint32_t mfx_id;
+    io_ctx.Address = i2c_addr[i];
+    TU_ASSERT(MFXSTM32L152_RegisterBusIO(&mfx_obj, &io_ctx) == MFXSTM32L152_OK, );
+    TU_ASSERT(MFXSTM32L152_ReadID(&mfx_obj, &mfx_id) == MFXSTM32L152_OK, );
+    if ((mfx_id == MFXSTM32L152_ID) || (mfx_id == MFXSTM32L152_ID_2)) {
+      TU_ASSERT(MFXSTM32L152_Init(&mfx_obj) == MFXSTM32L152_OK, );
+      break;
+    }
+  }
+
+  mfx_io = &MFXSTM32L152_IO_Driver;
+  mfx_io->IO_Start(&mfx_obj, MFXSTM32L152_GPIO_PINS_ALL);
+
+  for(uint32_t i=0; i<2; i++) {
+    MFXSTM32L152_IO_Init_t io_init = {
+      .Pin = mfx_vbus_pin[i],
+      .Mode = MFXSTM32L152_GPIO_MODE_OUTPUT_PP,
+      .Pull = MFXSTM32L152_GPIO_PULLUP,
+    };
+    mfx_io->Init(&mfx_obj, &io_init);
+  }
+}
+
+// VBUS1 is actually controlled by USB3320C PHY (using dwc2 drivebus signal)
+void board_vbus_set(uint8_t rhport, bool state) {
+  if (mfx_io) {
+    mfx_io->IO_WritePin(&mfx_obj, mfx_vbus_pin[rhport], state);
+  }
 }
 
 #ifdef __cplusplus
diff --git a/hw/bsp/stm32h7/boards/stm32h743eval/board.mk b/hw/bsp/stm32h7/boards/stm32h743eval/board.mk
index 36882a0e5..67b403932 100644
--- a/hw/bsp/stm32h7/boards/stm32h743eval/board.mk
+++ b/hw/bsp/stm32h7/boards/stm32h743eval/board.mk
@@ -1,16 +1,17 @@
+MCU_VARIANT = stm32h743xx
 CFLAGS += -DSTM32H743xx -DHSE_VALUE=25000000
 
-# Default is Highspeed port
-PORT ?= 1
-SPEED ?= high
+RHPORT_SPEED = OPT_MODE_FULL_SPEED OPT_MODE_HIGH_SPEED
+RHPORT_DEVICE ?= 1
+RHPORT_HOST ?= 0
 
-# GCC
-SRC_S_GCC += $(ST_CMSIS)/Source/Templates/gcc/startup_stm32h743xx.s
-LD_FILE_GCC = $(FAMILY_PATH)/linker/stm32h743xx_flash.ld
+LD_FILE_GCC = $(FAMILY_PATH)/linker/${MCU_VARIANT}_flash.ld
 
-# IAR
-SRC_S_IAR += $(ST_CMSIS)/Source/Templates/iar/startup_stm32h743xx.s
-LD_FILE_IAR = $(ST_CMSIS)/Source/Templates/iar/linker/stm32h743xx_flash.icf
+SRC_C += \
+  ${ST_MFXSTM32L152}/mfxstm32l152.c \
+  ${ST_MFXSTM32L152}/mfxstm32l152_reg.c \
+
+INC += $(TOP)/${ST_MFXSTM32L152}
 
 # For flash-jlink target
 JLINK_DEVICE = stm32h743xi
diff --git a/hw/bsp/stm32h7/boards/stm32h743eval/cubemx/stm32h743eval.ioc b/hw/bsp/stm32h7/boards/stm32h743eval/cubemx/stm32h743eval.ioc
index 01458a0a9..0e5a4cc00 100644
--- a/hw/bsp/stm32h7/boards/stm32h743eval/cubemx/stm32h743eval.ioc
+++ b/hw/bsp/stm32h7/boards/stm32h743eval/cubemx/stm32h743eval.ioc
@@ -4,17 +4,20 @@ CAD.pinconfig=Project naming
 CAD.provider=
 File.Version=6
 GPIO.groupedBy=Group By Peripherals
+I2C1.IPParameters=Timing
+I2C1.Timing=0x10C0ECFF
 KeepUserPlacement=false
 Mcu.CPN=STM32H743XIH6
 Mcu.Family=STM32H7
 Mcu.IP0=CORTEX_M7
 Mcu.IP1=DEBUG
-Mcu.IP2=NVIC
-Mcu.IP3=RCC
-Mcu.IP4=SYS
-Mcu.IP5=USB_OTG_FS
-Mcu.IP6=USB_OTG_HS
-Mcu.IPNb=7
+Mcu.IP2=I2C1
+Mcu.IP3=NVIC
+Mcu.IP4=RCC
+Mcu.IP5=SYS
+Mcu.IP6=USB_OTG_FS
+Mcu.IP7=USB_OTG_HS
+Mcu.IPNb=8
 Mcu.Name=STM32H743XIHx
 Mcu.Package=TFBGA240
 Mcu.Pin0=PI6
@@ -190,8 +193,8 @@ Mcu.PinsNb=169
 Mcu.ThirdPartyNb=0
 Mcu.UserConstants=
 Mcu.UserName=STM32H743XIHx
-MxCube.Version=6.8.1
-MxDb.Version=DB.6.0.81
+MxCube.Version=6.10.0
+MxDb.Version=DB.6.0.100
 NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:false
 NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:false
 NVIC.ForceEnableDMAVector=true
@@ -325,10 +328,12 @@ PB5.Signal=USB_OTG_HS_ULPI_D7
 PB6.GPIOParameters=GPIO_Label
 PB6.GPIO_Label=I2C1_SCL [STM32L152CCT6_I2C_SCL]
 PB6.Locked=true
+PB6.Mode=I2C
 PB6.Signal=I2C1_SCL
 PB7.GPIOParameters=GPIO_Label
 PB7.GPIO_Label=I2C1_SDA [STM32L152CCT6_I2C_SDA]
 PB7.Locked=true
+PB7.Mode=I2C
 PB7.Signal=I2C1_SDA
 PB8.GPIOParameters=GPIO_Label
 PB8.GPIO_Label=SDIO1_CKIN
@@ -880,7 +885,7 @@ ProjectManager.FreePins=false
 ProjectManager.HalAssertFull=false
 ProjectManager.HeapSize=0x200
 ProjectManager.KeepUserCode=true
-ProjectManager.LastFirmware=true
+ProjectManager.LastFirmware=false
 ProjectManager.LibraryCopy=2
 ProjectManager.MainLocation=Src
 ProjectManager.NoMain=false
@@ -893,8 +898,10 @@ ProjectManager.RegisterCallBack=
 ProjectManager.StackSize=0x400
 ProjectManager.TargetToolchain=Makefile
 ProjectManager.ToolChainLocation=Src/
+ProjectManager.UAScriptAfterPath=
+ProjectManager.UAScriptBeforePath=
 ProjectManager.UnderRoot=false
-ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_USB_OTG_FS_PCD_Init-USB_OTG_FS-false-HAL-true,4-MX_USB_OTG_HS_PCD_Init-USB_OTG_HS-false-HAL-true,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true
+ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_USB_OTG_HS_PCD_Init-USB_OTG_HS-false-HAL-true,4-MX_I2C1_Init-I2C1-false-HAL-true,5-MX_USB_OTG_FS_PCD_Init-USB_OTG_FS-false-HAL-true,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true
 RCC.ADCFreq_Value=50390625
 RCC.AHB12Freq_Value=200000000
 RCC.AHB4Freq_Value=200000000
diff --git a/hw/bsp/stm32h7/boards/stm32h743nucleo/board.cmake b/hw/bsp/stm32h7/boards/stm32h743nucleo/board.cmake
index f1532a95f..021799775 100644
--- a/hw/bsp/stm32h7/boards/stm32h743nucleo/board.cmake
+++ b/hw/bsp/stm32h7/boards/stm32h743nucleo/board.cmake
@@ -7,8 +7,5 @@ function(update_board TARGET)
   target_compile_definitions(${TARGET} PUBLIC
     STM32H743xx
     HSE_VALUE=8000000
-    # default to PORT 0
-    BOARD_TUD_RHPORT=0
-    BOARD_TUD_MAX_SPEED=OPT_MODE_FULL_SPEED
     )
 endfunction()
diff --git a/hw/bsp/stm32h7/boards/stm32h743nucleo/board.h b/hw/bsp/stm32h7/boards/stm32h743nucleo/board.h
index 614e6e38b..0606f395a 100644
--- a/hw/bsp/stm32h7/boards/stm32h743nucleo/board.h
+++ b/hw/bsp/stm32h7/boards/stm32h743nucleo/board.h
@@ -31,25 +31,47 @@
  extern "C" {
 #endif
 
-#define LED_PORT              GPIOB
-#define LED_PIN               GPIO_PIN_0
-#define LED_STATE_ON          1
-
-#define BUTTON_PORT           GPIOC
-#define BUTTON_PIN            GPIO_PIN_13
-#define BUTTON_STATE_ACTIVE   1
-
 #define UART_DEV              USART3
 #define UART_CLK_EN           __HAL_RCC_USART3_CLK_ENABLE
-#define UART_GPIO_PORT        GPIOD
-#define UART_GPIO_AF          GPIO_AF7_USART3
-#define UART_TX_PIN           GPIO_PIN_8
-#define UART_RX_PIN           GPIO_PIN_9
 
 // VBUS Sense detection
 #define OTG_FS_VBUS_SENSE     1
 #define OTG_HS_VBUS_SENSE     0
 
+#define PINID_LED      0
+#define PINID_BUTTON   1
+#define PINID_UART_TX  2
+#define PINID_UART_RX  3
+#define PINID_VBUS0_EN 4
+
+static board_pindef_t board_pindef[] = {
+  { // LED
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_0, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // Button
+    .port = GPIOC,
+    .pin_init = { .Pin = GPIO_PIN_13, .Mode = GPIO_MODE_INPUT, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // UART TX
+    .port = GPIOD,
+    .pin_init = { .Pin = GPIO_PIN_8, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  },
+  { // UART RX
+    .port = GPIOD,
+    .pin_init = { .Pin = GPIO_PIN_9, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  },
+  { // VBUS0 EN
+    .port = GPIOD,
+    .pin_init = { .Pin = GPIO_PIN_10, .Mode = GPIO_MODE_OUTPUT_OD, .Pull = GPIO_NOPULL, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 0
+  }
+};
+
 //--------------------------------------------------------------------+
 // RCC Clock
 //--------------------------------------------------------------------+
@@ -100,8 +122,7 @@ static inline void SystemClock_Config(void) {
   RCC_ClkInitStruct.APB2CLKDivider = RCC_APB2_DIV2;
   RCC_ClkInitStruct.APB4CLKDivider = RCC_APB4_DIV2;
 
-  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
-  {
+  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK) {
     Error_Handler();
   }
 
@@ -116,17 +137,21 @@ static inline void SystemClock_Config(void) {
   PeriphClkInitStruct.PLL3.PLL3RGE = RCC_PLL3VCIRANGE_3;
   PeriphClkInitStruct.PLL3.PLL3FRACN = 0;
   PeriphClkInitStruct.UsbClockSelection = RCC_USBCLKSOURCE_PLL3;
-  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
-  {
+  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) {
     Error_Handler();
   }
 }
 
-static inline void board_stm32h7_post_init(void)
-{
+static inline void board_init2(void) {
   // For this board does nothing
 }
 
+void board_vbus_set(uint8_t rhport, bool state) {
+  if (rhport == 0) {
+    board_pindef_t* pindef = &board_pindef[PINID_VBUS0_EN];
+    HAL_GPIO_WritePin(pindef->port, pindef->pin_init.Pin, state == pindef->active_state ? GPIO_PIN_SET : GPIO_PIN_RESET);
+  }
+}
 
 #ifdef __cplusplus
  }
diff --git a/hw/bsp/stm32h7/boards/stm32h743nucleo/board.mk b/hw/bsp/stm32h7/boards/stm32h743nucleo/board.mk
index f641b77aa..d904de6d2 100644
--- a/hw/bsp/stm32h7/boards/stm32h743nucleo/board.mk
+++ b/hw/bsp/stm32h7/boards/stm32h743nucleo/board.mk
@@ -1,15 +1,7 @@
+MCU_VARIANT = stm32h743xx
 CFLAGS += -DSTM32H743xx -DHSE_VALUE=8000000
 
-# Default is FulSpeed port
-PORT ?= 0
-
-# GCC
-SRC_S_GCC += $(ST_CMSIS)/Source/Templates/gcc/startup_stm32h743xx.s
-LD_FILE_GCC = $(FAMILY_PATH)/linker/stm32h743xx_flash.ld
-
-# IAR
-SRC_S_IAR += $(ST_CMSIS)/Source/Templates/iar/startup_stm32h743xx.s
-LD_FILE_IAR = $(ST_CMSIS)/Source/Templates/iar/linker/stm32h743xx_flash.icf
+LD_FILE_GCC = $(FAMILY_PATH)/linker/${MCU_VARIANT}_flash.ld
 
 # For flash-jlink target
 JLINK_DEVICE = stm32h743zi
diff --git a/hw/bsp/stm32h7/boards/stm32h745disco/board.cmake b/hw/bsp/stm32h7/boards/stm32h745disco/board.cmake
index f1313d54e..39a9d5798 100644
--- a/hw/bsp/stm32h7/boards/stm32h745disco/board.cmake
+++ b/hw/bsp/stm32h7/boards/stm32h745disco/board.cmake
@@ -9,8 +9,5 @@ function(update_board TARGET)
     STM32H745xx
     HSE_VALUE=25000000
     CORE_CM7
-    # default to PORT 0
-    BOARD_TUD_RHPORT=0
-    BOARD_TUD_MAX_SPEED=OPT_MODE_FULL_SPEED
     )
 endfunction()
diff --git a/hw/bsp/stm32h7/boards/stm32h745disco/board.h b/hw/bsp/stm32h7/boards/stm32h745disco/board.h
index 6d1506ca1..b9d9cdea4 100644
--- a/hw/bsp/stm32h7/boards/stm32h745disco/board.h
+++ b/hw/bsp/stm32h7/boards/stm32h745disco/board.h
@@ -31,27 +31,48 @@
  extern "C" {
 #endif
 
-#define LED_PORT              GPIOJ
-#define LED_PIN               GPIO_PIN_2
-#define LED_STATE_ON          1
-
-// Blue push-button
-#define BUTTON_PORT           GPIOC
-#define BUTTON_PIN            GPIO_PIN_13
-#define BUTTON_STATE_ACTIVE   1
-
 // UART
 #define UART_DEV              USART3
 #define UART_CLK_EN           __HAL_RCC_USART3_CLK_ENABLE
-#define UART_GPIO_PORT        GPIOB
-#define UART_GPIO_AF          GPIO_AF7_USART3
-#define UART_TX_PIN           GPIO_PIN_10
-#define UART_RX_PIN           GPIO_PIN_11
 
 // VBUS Sense detection
 #define OTG_FS_VBUS_SENSE     1
 #define OTG_HS_VBUS_SENSE     0
 
+#define PINID_LED      0
+#define PINID_BUTTON   1
+#define PINID_UART_TX  2
+#define PINID_UART_RX  3
+#define PINID_VBUS0_EN 4
+
+static board_pindef_t board_pindef[] = {
+  { // LED
+    .port = GPIOJ,
+    .pin_init = { .Pin = GPIO_PIN_2, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // Button
+    .port = GPIOC,
+    .pin_init = { .Pin = GPIO_PIN_13, .Mode = GPIO_MODE_INPUT, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // UART TX
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_10, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  },
+  { // UART RX
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_11, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  },
+  { // VBUS0 EN
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_5, .Mode = GPIO_MODE_OUTPUT_OD, .Pull = GPIO_NOPULL, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  }
+};
+
 //--------------------------------------------------------------------+
 // RCC Clock
 //--------------------------------------------------------------------+
@@ -127,11 +148,17 @@ static inline void SystemClock_Config(void)
   HAL_EnableCompensationCell();
 }
 
-static inline void board_stm32h7_post_init(void)
-{
+static inline void board_init2(void) {
   // For this board does nothing
 }
 
+void board_vbus_set(uint8_t rhport, bool state) {
+  if (rhport == 0) {
+    board_pindef_t* pindef = &board_pindef[PINID_VBUS0_EN];
+    HAL_GPIO_WritePin(pindef->port, pindef->pin_init.Pin, state == pindef->active_state ? GPIO_PIN_SET : GPIO_PIN_RESET);
+  }
+}
+
 #ifdef __cplusplus
  }
 #endif
diff --git a/hw/bsp/stm32h7/boards/stm32h745disco/board.mk b/hw/bsp/stm32h7/boards/stm32h745disco/board.mk
index 9c3615f05..588620ce2 100644
--- a/hw/bsp/stm32h7/boards/stm32h745disco/board.mk
+++ b/hw/bsp/stm32h7/boards/stm32h745disco/board.mk
@@ -1,17 +1,12 @@
 # STM32H745I-DISCO uses OTG_FS
 # FIXME: Reset enumerates, un/replug USB plug does not enumerate
-
+MCU_VARIANT = stm32h745xx
 CFLAGS += -DSTM32H745xx -DCORE_CM7 -DHSE_VALUE=25000000
 
 # Default is FulSpeed port
 PORT ?= 0
 
-# GCC
-SRC_S_GCC += $(ST_CMSIS)/Source/Templates/gcc/startup_stm32h745xx.s
 LD_FILE_GCC = $(ST_CMSIS)/Source/Templates/gcc/linker/stm32h745xx_flash_CM7.ld
-
-# IAR
-SRC_S_IAR += $(ST_CMSIS)/Source/Templates/iar/startup_stm32h745xx.s
 LD_FILE_IAR = $(ST_CMSIS)/Source/Templates/iar/linker/stm32h745xx_flash_CM7.icf
 
 # For flash-jlink target
diff --git a/hw/bsp/stm32h7/boards/stm32h750_weact/board.cmake b/hw/bsp/stm32h7/boards/stm32h750_weact/board.cmake
index 6303ca462..6eab26c07 100644
--- a/hw/bsp/stm32h7/boards/stm32h750_weact/board.cmake
+++ b/hw/bsp/stm32h7/boards/stm32h750_weact/board.cmake
@@ -9,8 +9,5 @@ function(update_board TARGET)
     STM32H750xx
     HSE_VALUE=25000000
     CORE_CM7
-    # default to PORT 0
-    BOARD_TUD_RHPORT=0
-    BOARD_TUD_MAX_SPEED=OPT_MODE_FULL_SPEED
     )
 endfunction()
diff --git a/hw/bsp/stm32h7/boards/stm32h750_weact/board.h b/hw/bsp/stm32h7/boards/stm32h750_weact/board.h
index d117637a5..f1c363082 100644
--- a/hw/bsp/stm32h7/boards/stm32h750_weact/board.h
+++ b/hw/bsp/stm32h7/boards/stm32h750_weact/board.h
@@ -31,27 +31,42 @@
  extern "C" {
 #endif
 
-#define LED_PORT              GPIOE
-#define LED_PIN               GPIO_PIN_3
-#define LED_STATE_ON          1
-
-// Blue push-button
-#define BUTTON_PORT           GPIOC
-#define BUTTON_PIN            GPIO_PIN_13
-#define BUTTON_STATE_ACTIVE   1
-
 // UART
-//#define UART_DEV              USART3
-//#define UART_CLK_EN           __HAL_RCC_USART3_CLK_ENABLE
-//#define UART_GPIO_PORT        GPIOB
-//#define UART_GPIO_AF          GPIO_AF7_USART3
-//#define UART_TX_PIN           GPIO_PIN_10
-//#define UART_RX_PIN           GPIO_PIN_11
+#define UART_DEV              USART3
+#define UART_CLK_EN           __HAL_RCC_USART3_CLK_ENABLE
 
 // VBUS Sense detection
 #define OTG_FS_VBUS_SENSE     1
 #define OTG_HS_VBUS_SENSE     0
 
+#define PINID_LED      0
+#define PINID_BUTTON   1
+#define PINID_UART_TX  2
+#define PINID_UART_RX  3
+
+static board_pindef_t board_pindef[] = {
+  { // LED
+    .port = GPIOE,
+    .pin_init = { .Pin = GPIO_PIN_3, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // Button
+    .port = GPIOC,
+    .pin_init = { .Pin = GPIO_PIN_13, .Mode = GPIO_MODE_INPUT, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // UART TX
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_10, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  },
+  { // UART RX
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_11, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  }
+};
+
 //--------------------------------------------------------------------+
 // RCC Clock
 //--------------------------------------------------------------------+
@@ -121,10 +136,14 @@ static inline void SystemClock_Config(void) {
   HAL_PWREx_EnableUSBVoltageDetector();
 }
 
-static inline void board_stm32h7_post_init(void) {
+static inline void board_init2(void) {
   // For this board does nothing
 }
 
+void board_vbus_set(uint8_t rhport, bool state) {
+  (void) rhport; (void) state;
+}
+
 #ifdef __cplusplus
  }
 #endif
diff --git a/hw/bsp/stm32h7/boards/stm32h750_weact/board.mk b/hw/bsp/stm32h7/boards/stm32h750_weact/board.mk
index a50172cb0..988fed804 100644
--- a/hw/bsp/stm32h7/boards/stm32h750_weact/board.mk
+++ b/hw/bsp/stm32h7/boards/stm32h750_weact/board.mk
@@ -1,19 +1,10 @@
 # STM32H745I-DISCO uses OTG_FS
 # FIXME: Reset enumerates, un/replug USB plug does not enumerate
-
+MCU_VARIANT = stm32h750xx
 CFLAGS += -DSTM32H750xx -DCORE_CM7 -DHSE_VALUE=25000000
 
-# Default is FulSpeed port
-PORT ?= 0
-
-# GCC
-SRC_S_GCC += $(ST_CMSIS)/Source/Templates/gcc/startup_stm32h750xx.s
 LD_FILE_GCC = $(BOARD_PATH)/stm32h750xx_flash_CM7.ld
 
-# IAR
-SRC_S_IAR += $(ST_CMSIS)/Source/Templates/iar/startup_stm32h750xx.s
-LD_FILE_IAR = $(ST_CMSIS)/Source/Templates/iar/linker/stm32h750xx_flash.icf
-
 # For flash-jlink target
 JLINK_DEVICE = stm32h750vb
 
diff --git a/hw/bsp/stm32h7/boards/stm32h750bdk/board.cmake b/hw/bsp/stm32h7/boards/stm32h750bdk/board.cmake
index e87be8255..72a139ab6 100644
--- a/hw/bsp/stm32h7/boards/stm32h750bdk/board.cmake
+++ b/hw/bsp/stm32h7/boards/stm32h750bdk/board.cmake
@@ -9,8 +9,5 @@ function(update_board TARGET)
     STM32H750xx
     HSE_VALUE=25000000
     CORE_CM7
-    # default to PORT 0
-    BOARD_TUD_RHPORT=0
-    BOARD_TUD_MAX_SPEED=OPT_MODE_FULL_SPEED
     )
 endfunction()
diff --git a/hw/bsp/stm32h7/boards/stm32h750bdk/board.h b/hw/bsp/stm32h7/boards/stm32h750bdk/board.h
index c5922efc4..2895f0973 100644
--- a/hw/bsp/stm32h7/boards/stm32h750bdk/board.h
+++ b/hw/bsp/stm32h7/boards/stm32h750bdk/board.h
@@ -31,33 +31,47 @@
  extern "C" {
 #endif
 
-#define LED_PORT              GPIOJ
-#define LED_PIN               GPIO_PIN_2
-#define LED_STATE_ON          1
-
-// Blue push-button
-#define BUTTON_PORT           GPIOC
-#define BUTTON_PIN            GPIO_PIN_13
-#define BUTTON_STATE_ACTIVE   1
-
 // UART
 #define UART_DEV              USART3
 #define UART_CLK_EN           __HAL_RCC_USART3_CLK_ENABLE
-#define UART_GPIO_PORT        GPIOB
-#define UART_GPIO_AF          GPIO_AF7_USART3
-#define UART_TX_PIN           GPIO_PIN_10
-#define UART_RX_PIN           GPIO_PIN_11
 
 // VBUS Sense detection
 #define OTG_FS_VBUS_SENSE     1
 #define OTG_HS_VBUS_SENSE     0
 
-// USB HS External PHY Pin: CLK, STP, DIR, NXT, D0-D7
-#define ULPI_PINS \
-  {GPIOA, GPIO_PIN_3 }, {GPIOA, GPIO_PIN_5 }, {GPIOB, GPIO_PIN_0 }, {GPIOB, GPIO_PIN_1 }, \
-  {GPIOB, GPIO_PIN_5 }, {GPIOB, GPIO_PIN_10}, {GPIOB, GPIO_PIN_11}, {GPIOB, GPIO_PIN_12}, \
-  {GPIOB, GPIO_PIN_13}, {GPIOC, GPIO_PIN_0 }, {GPIOH, GPIO_PIN_4 }, {GPIOI, GPIO_PIN_11}
+#define PINID_LED      0
+#define PINID_BUTTON   1
+#define PINID_UART_TX  2
+#define PINID_UART_RX  3
+#define PINID_VBUS0_EN 4
 
+static board_pindef_t board_pindef[] = {
+  { // LED
+    .port = GPIOJ,
+    .pin_init = { .Pin = GPIO_PIN_2, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // Button
+    .port = GPIOC,
+    .pin_init = { .Pin = GPIO_PIN_13, .Mode = GPIO_MODE_INPUT, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // UART TX
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_10, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  },
+  { // UART RX
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_11, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  },
+  { // VBUS0 EN
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_5, .Mode = GPIO_MODE_OUTPUT_OD, .Pull = GPIO_NOPULL, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  }
+};
 //--------------------------------------------------------------------+
 // RCC Clock
 //--------------------------------------------------------------------+
@@ -132,11 +146,17 @@ static inline void SystemClock_Config(void)
   HAL_EnableCompensationCell();
 }
 
-static inline void board_stm32h7_post_init(void)
-{
+static inline void board_init2(void) {
   // For this board does nothing
 }
 
+void board_vbus_set(uint8_t rhport, bool state) {
+  if (rhport == 0) {
+    board_pindef_t* pindef = &board_pindef[PINID_VBUS0_EN];
+    HAL_GPIO_WritePin(pindef->port, pindef->pin_init.Pin, state == pindef->active_state ? GPIO_PIN_SET : GPIO_PIN_RESET);
+  }
+}
+
 #ifdef __cplusplus
  }
 #endif
diff --git a/hw/bsp/stm32h7/boards/stm32h750bdk/board.mk b/hw/bsp/stm32h7/boards/stm32h750bdk/board.mk
index 923c90753..6eb3eb498 100644
--- a/hw/bsp/stm32h7/boards/stm32h750bdk/board.mk
+++ b/hw/bsp/stm32h7/boards/stm32h750bdk/board.mk
@@ -1,19 +1,10 @@
 # STM32H745I-DISCO uses OTG_FS
 # FIXME: Reset enumerates, un/replug USB plug does not enumerate
-
+MCU_VARIANT = stm32h750xx
 CFLAGS += -DSTM32H750xx -DCORE_CM7 -DHSE_VALUE=25000000
 
-# Default is FulSpeed port
-PORT ?= 0
-
-# GCC
-SRC_S_GCC += $(ST_CMSIS)/Source/Templates/gcc/startup_stm32h750xx.s
 LD_FILE_GCC = $(BOARD_PATH)/stm32h750xx_flash_CM7.ld
 
-# IAR
-SRC_S_IAR += $(ST_CMSIS)/Source/Templates/iar/startup_stm32h750xx.s
-LD_FILE_IAR = $(ST_CMSIS)/Source/Templates/iar/linker/stm32h750xx_flash.icf
-
 # For flash-jlink target
 JLINK_DEVICE = stm32h750xb
 
diff --git a/hw/bsp/stm32h7/boards/waveshare_openh743i/board.cmake b/hw/bsp/stm32h7/boards/waveshare_openh743i/board.cmake
index 83c8d4833..3aaa81612 100644
--- a/hw/bsp/stm32h7/boards/waveshare_openh743i/board.cmake
+++ b/hw/bsp/stm32h7/boards/waveshare_openh743i/board.cmake
@@ -3,14 +3,21 @@ set(JLINK_DEVICE stm32h743xi)
 
 set(LD_FILE_GNU ${CMAKE_CURRENT_LIST_DIR}/../../linker/${MCU_VARIANT}_flash.ld)
 
+set(RHPORT_SPEED OPT_MODE_FULL_SPEED OPT_MODE_HIGH_SPEED)
+
+# device default to PORT 1 High Speed
+if (NOT DEFINED RHPORT_DEVICE)
+  set(RHPORT_DEVICE 1)
+endif()
+if (NOT DEFINED RHPORT_HOST)
+  set(RHPORT_HOST 0)
+endif()
+
 function(update_board TARGET)
   target_compile_definitions(${TARGET} PUBLIC
     STM32H743xx
     HSE_VALUE=8000000
     HAL_TIM_MODULE_ENABLED
-    # default to PORT 1 High Speed
-    BOARD_TUD_RHPORT=1
-    BOARD_TUD_MAX_SPEED=OPT_MODE_HIGH_SPEED
     )
   target_sources(${TARGET} PUBLIC
     ${ST_HAL_DRIVER}/Src/${ST_PREFIX}_hal_tim.c
diff --git a/hw/bsp/stm32h7/boards/waveshare_openh743i/board.h b/hw/bsp/stm32h7/boards/waveshare_openh743i/board.h
index 8f4af6f48..625c6a137 100644
--- a/hw/bsp/stm32h7/boards/waveshare_openh743i/board.h
+++ b/hw/bsp/stm32h7/boards/waveshare_openh743i/board.h
@@ -70,22 +70,9 @@
  extern "C" {
 #endif
 
-#define LED_PORT              GPIOB
-#define LED_PIN               GPIO_PIN_6
-#define LED_STATE_ON          1
-
-// Tamper push-button
-#define BUTTON_PORT           GPIOA
-#define BUTTON_PIN            GPIO_PIN_0
-#define BUTTON_STATE_ACTIVE   1
-
 // Need to change jumper setting J7 and J8 from RS-232 to STLink
 #define UART_DEV              USART3
 #define UART_CLK_EN           __HAL_RCC_USART3_CLK_ENABLE
-#define UART_GPIO_PORT        GPIOD
-#define UART_GPIO_AF          GPIO_AF7_USART3
-#define UART_TX_PIN           GPIO_PIN_8
-#define UART_RX_PIN           GPIO_PIN_9
 
 // VBUS Sense detection
 #define OTG_FS_VBUS_SENSE     1
@@ -101,6 +88,45 @@
 #define ULPI_RST_PORT GPIOD
 #define ULPI_RST_PIN GPIO_PIN_14
 
+#define PINID_LED      0
+#define PINID_BUTTON   1
+#define PINID_UART_TX  2
+#define PINID_UART_RX  3
+
+static board_pindef_t board_pindef[] = {
+  { // LED
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_6, .Mode = GPIO_MODE_OUTPUT_PP, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // Button
+    .port = GPIOA,
+    .pin_init = { .Pin = GPIO_PIN_0, .Mode = GPIO_MODE_INPUT, .Pull = GPIO_PULLDOWN, .Speed = GPIO_SPEED_HIGH, .Alternate = 0 },
+    .active_state = 1
+  },
+  { // UART TX
+    .port = GPIOD,
+    .pin_init = { .Pin = GPIO_PIN_8, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  },
+  { // UART RX
+    .port = GPIOD,
+    .pin_init = { .Pin = GPIO_PIN_9, .Mode = GPIO_MODE_AF_PP, .Pull = GPIO_PULLUP, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF7_USART3 },
+    .active_state = 0
+  },
+
+  { // I2C SCL for MFX VBUS
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_6, .Mode = GPIO_MODE_AF_OD, .Pull = GPIO_NOPULL, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF4_I2C1 },
+    .active_state = 0
+  },
+  { // I2C SDA for MFX VBUS
+    .port = GPIOB,
+    .pin_init = { .Pin = GPIO_PIN_7, .Mode = GPIO_MODE_AF_OD, .Pull = GPIO_NOPULL, .Speed = GPIO_SPEED_HIGH, .Alternate = GPIO_AF4_I2C1 },
+    .active_state = 1
+  },
+};
+
 //--------------------------------------------------------------------+
 // RCC Clock
 //--------------------------------------------------------------------+
@@ -178,13 +204,12 @@ static inline void SystemClock_Config(void)
 static inline void timer_board_delay(TIM_HandleTypeDef* tim_hdl, uint32_t ms)
 {
   uint32_t startMs = __HAL_TIM_GET_COUNTER(tim_hdl);
-  while ((__HAL_TIM_GET_COUNTER(tim_hdl) - startMs) < ms)
-  {
+  while ((__HAL_TIM_GET_COUNTER(tim_hdl) - startMs) < ms) {
     asm("nop"); //do nothing
   }
 }
 
-static inline void board_stm32h7_post_init(void)
+static inline void board_init2(void)
 {
   // walkaround for resetting the ULPI PHY using Timer since systick is not
   // available when RTOS is used.
@@ -230,6 +255,11 @@ static inline void board_stm32h7_post_init(void)
   __HAL_RCC_TIM2_CLK_DISABLE();
 }
 
+// need to short a jumper
+void board_vbus_set(uint8_t rhport, bool state) {
+  (void) rhport; (void) state;
+}
+
 #ifdef __cplusplus
  }
 #endif
diff --git a/hw/bsp/stm32h7/boards/waveshare_openh743i/board.mk b/hw/bsp/stm32h7/boards/waveshare_openh743i/board.mk
index cea4bfacb..5ff2f4165 100644
--- a/hw/bsp/stm32h7/boards/waveshare_openh743i/board.mk
+++ b/hw/bsp/stm32h7/boards/waveshare_openh743i/board.mk
@@ -1,7 +1,11 @@
+MCU_VARIANT = stm32h743xx
 CFLAGS += -DSTM32H743xx -DHSE_VALUE=8000000
 
-# Default is HS port
-PORT ?= 1
+RHPORT_SPEED = OPT_MODE_FULL_SPEED OPT_MODE_HIGH_SPEED
+RHPORT_DEVICE ?= 1
+RHPORT_HOST ?= 0
+
+LD_FILE_GCC = $(FAMILY_PATH)/linker/stm32h743xx_flash.ld
 
 # Use Timer module for ULPI PHY reset
 CFLAGS += -DHAL_TIM_MODULE_ENABLED
@@ -9,14 +13,6 @@ SRC_C += \
   $(ST_HAL_DRIVER)/Src/stm32$(ST_FAMILY)xx_hal_tim.c \
   $(ST_HAL_DRIVER)/Src/stm32$(ST_FAMILY)xx_hal_tim_ex.c
 
-# GCC
-SRC_S_GCC += $(ST_CMSIS)/Source/Templates/gcc/startup_stm32h743xx.s
-LD_FILE_GCC = $(FAMILY_PATH)/linker/stm32h743xx_flash.ld
-
-# IAR
-SRC_S_IAR += $(ST_CMSIS)/Source/Templates/iar/startup_stm32h743xx.s
-LD_FILE_IAR = $(ST_CMSIS)/Source/Templates/iar/linker/stm32h743xx_flash.icf
-
 # For flash-jlink target
 JLINK_DEVICE = stm32h743ii
 
diff --git a/hw/bsp/stm32h7/family.c b/hw/bsp/stm32h7/family.c
index 6e4b388d6..0be18350c 100644
--- a/hw/bsp/stm32h7/family.c
+++ b/hw/bsp/stm32h7/family.c
@@ -30,8 +30,13 @@
 #include "stm32h7xx_hal.h"
 #include "bsp/board_api.h"
 
-TU_ATTR_UNUSED static void Error_Handler(void) {
-}
+TU_ATTR_UNUSED static void Error_Handler(void) { }
+
+typedef struct {
+  GPIO_TypeDef* port;
+  GPIO_InitTypeDef pin_init;
+  uint8_t active_state;
+} board_pindef_t;
 
 #include "board.h"
 
@@ -39,7 +44,20 @@ TU_ATTR_UNUSED static void Error_Handler(void) {
 // MACRO TYPEDEF CONSTANT ENUM
 //--------------------------------------------------------------------+
 
-UART_HandleTypeDef UartHandle;
+#ifdef UART_DEV
+UART_HandleTypeDef UartHandle = {
+  .Instance = UART_DEV,
+  .Init = {
+    .BaudRate = CFG_BOARD_UART_BAUDRATE,
+    .WordLength = UART_WORDLENGTH_8B,
+    .StopBits = UART_STOPBITS_1,
+    .Parity = UART_PARITY_NONE,
+    .HwFlowCtl = UART_HWCONTROL_NONE,
+    .Mode = UART_MODE_TX_RX,
+    .OverSampling = UART_OVERSAMPLING_16,
+  }
+};
+#endif
 
 //--------------------------------------------------------------------+
 // Forward USB interrupt events to TinyUSB IRQ Handler
@@ -60,8 +78,6 @@ void OTG_HS_IRQHandler(void) {
 #ifdef TRACE_ETM
 void trace_etm_init(void) {
   // H7 trace pin is PE2 to PE6
-  // __HAL_RCC_GPIOE_CLK_ENABLE();
-
   GPIO_InitTypeDef  gpio_init;
   gpio_init.Pin       = GPIO_PIN_2 | GPIO_PIN_3 | GPIO_PIN_4 | GPIO_PIN_5 | GPIO_PIN_6;
   gpio_init.Mode      = GPIO_MODE_AF_PP;
@@ -83,20 +99,24 @@ void board_init(void) {
 
   // Enable All GPIOs clocks
   __HAL_RCC_GPIOA_CLK_ENABLE();
-  __HAL_RCC_GPIOB_CLK_ENABLE(); // USB ULPI NXT
-  __HAL_RCC_GPIOC_CLK_ENABLE(); // USB ULPI NXT
+  __HAL_RCC_GPIOB_CLK_ENABLE();
+  __HAL_RCC_GPIOC_CLK_ENABLE();
   __HAL_RCC_GPIOD_CLK_ENABLE();
   __HAL_RCC_GPIOE_CLK_ENABLE();
   __HAL_RCC_GPIOF_CLK_ENABLE();
   __HAL_RCC_GPIOG_CLK_ENABLE();
-  __HAL_RCC_GPIOH_CLK_ENABLE(); // USB ULPI NXT
+  __HAL_RCC_GPIOH_CLK_ENABLE();
 #ifdef __HAL_RCC_GPIOI_CLK_ENABLE
-  __HAL_RCC_GPIOI_CLK_ENABLE(); // USB ULPI NXT
+  __HAL_RCC_GPIOI_CLK_ENABLE();
 #endif
   __HAL_RCC_GPIOJ_CLK_ENABLE();
 
   trace_etm_init();
 
+  for (uint8_t i = 0; i < TU_ARRAY_SIZE(board_pindef); i++) {
+    HAL_GPIO_Init(board_pindef[i].port, &board_pindef[i].pin_init);
+  }
+
 #if CFG_TUSB_OS == OPT_OS_NONE
   // 1ms tick timer
   SysTick_Config(SystemCoreClock / 1000);
@@ -115,43 +135,12 @@ void board_init(void) {
 
   GPIO_InitTypeDef GPIO_InitStruct;
 
-  // LED
-  GPIO_InitStruct.Pin = LED_PIN;
-  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
-  GPIO_InitStruct.Pull = GPIO_PULLUP;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
-  HAL_GPIO_Init(LED_PORT, &GPIO_InitStruct);
-
-  // Button
-  GPIO_InitStruct.Pin = BUTTON_PIN;
-  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
-  GPIO_InitStruct.Pull = GPIO_NOPULL;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
-  HAL_GPIO_Init(BUTTON_PORT, &GPIO_InitStruct);
-
 #ifdef UART_DEV
-  // Uart
   UART_CLK_EN();
-
-  GPIO_InitStruct.Pin = UART_TX_PIN | UART_RX_PIN;
-  GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
-  GPIO_InitStruct.Pull = GPIO_PULLUP;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
-  GPIO_InitStruct.Alternate = UART_GPIO_AF;
-  HAL_GPIO_Init(UART_GPIO_PORT, &GPIO_InitStruct);
-
-  UartHandle.Instance = UART_DEV;
-  UartHandle.Init.BaudRate = CFG_BOARD_UART_BAUDRATE;
-  UartHandle.Init.WordLength = UART_WORDLENGTH_8B;
-  UartHandle.Init.StopBits = UART_STOPBITS_1;
-  UartHandle.Init.Parity = UART_PARITY_NONE;
-  UartHandle.Init.HwFlowCtl = UART_HWCONTROL_NONE;
-  UartHandle.Init.Mode = UART_MODE_TX_RX;
-  UartHandle.Init.OverSampling = UART_OVERSAMPLING_16;
   HAL_UART_Init(&UartHandle);
 #endif
 
-#if BOARD_TUD_RHPORT == 0
+  //------------- USB FS -------------//
   // Despite being call USB2_OTG
   // OTG_FS is marked as RHPort0 by TinyUSB to be consistent across stm32 port
   // PA9 VUSB, PA10 ID, PA11 DM, PA12 DP
@@ -195,10 +184,10 @@ void board_init(void) {
   USB_OTG_FS->GOTGCTL |= USB_OTG_GOTGCTL_BVALOVAL;
 #endif // vbus sense
 
-#elif BOARD_TUD_RHPORT == 1
+  //------------- USB HS -------------//
+#if (CFG_TUD_ENABLED && BOARD_TUD_RHPORT == 1) || (CFG_TUH_ENABLED && BOARD_TUH_RHPORT == 1)
   // Despite being call USB2_OTG
   // OTG_HS is marked as RHPort1 by TinyUSB to be consistent across stm32 port
-
   struct {
     GPIO_TypeDef* port;
     uint32_t pin;
@@ -233,12 +222,15 @@ void board_init(void) {
   // Force device mode
   USB_OTG_HS->GUSBCFG &= ~USB_OTG_GUSBCFG_FHMOD;
   USB_OTG_HS->GUSBCFG |= USB_OTG_GUSBCFG_FDMOD;
+#endif
 
   HAL_PWREx_EnableUSBVoltageDetector();
 
-  // For waveshare openh743 ULPI PHY reset walkaround
-  board_stm32h7_post_init();
-#endif // rhport = 1
+  board_init2(); // optional init
+
+#if CFG_TUH_ENABLED
+  board_vbus_set(BOARD_TUH_RHPORT, 1);
+#endif
 
 }
 
@@ -247,12 +239,22 @@ void board_init(void) {
 //--------------------------------------------------------------------+
 
 void board_led_write(bool state) {
-  GPIO_PinState pin_state = (GPIO_PinState)(state ? LED_STATE_ON : (1 - LED_STATE_ON));
-  HAL_GPIO_WritePin(LED_PORT, LED_PIN, pin_state);
+#ifdef PINID_LED
+  board_pindef_t* pindef = &board_pindef[PINID_LED];
+  GPIO_PinState pin_state = state == pindef->active_state ? GPIO_PIN_SET : GPIO_PIN_RESET;
+  HAL_GPIO_WritePin(pindef->port, pindef->pin_init.Pin, pin_state);
+#else
+  (void) state;
+#endif
 }
 
 uint32_t board_button_read(void) {
-  return (BUTTON_STATE_ACTIVE == HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) ? 1 : 0;
+#ifdef PINID_BUTTON
+  board_pindef_t* pindef = &board_pindef[PINID_BUTTON];
+  return pindef->active_state == HAL_GPIO_ReadPin(pindef->port, pindef->pin_init.Pin);
+#else
+  return 0;
+#endif
 }
 
 size_t board_get_unique_id(uint8_t id[], size_t max_len) {
@@ -278,14 +280,13 @@ int board_uart_write(void const *buf, int len) {
 #ifdef UART_DEV
   HAL_UART_Transmit(&UartHandle, (uint8_t * )(uintptr_t)
   buf, len, 0xffff);
+  return len;
 #else
-  (void) buf;
+  (void) buf; (void) len;
+  return -1;
 #endif
-
-  return len;
 }
 
-
 #if CFG_TUSB_OS == OPT_OS_NONE
 volatile uint32_t system_ticks = 0;
 
diff --git a/hw/bsp/stm32h7/family.cmake b/hw/bsp/stm32h7/family.cmake
index 026c7c3df..3e246faed 100644
--- a/hw/bsp/stm32h7/family.cmake
+++ b/hw/bsp/stm32h7/family.cmake
@@ -5,6 +5,7 @@ set(ST_PREFIX stm32${ST_FAMILY}xx)
 
 set(ST_HAL_DRIVER ${TOP}/hw/mcu/st/stm32${ST_FAMILY}xx_hal_driver)
 set(ST_CMSIS ${TOP}/hw/mcu/st/cmsis_device_${ST_FAMILY})
+set(ST_MFXSTM32L152 ${TOP}/hw/mcu/st/stm32-mfxstm32l152)
 set(CMSIS_5 ${TOP}/lib/CMSIS_5)
 
 # include board specific
@@ -16,6 +17,28 @@ set(CMAKE_TOOLCHAIN_FILE ${TOP}/examples/build_system/cmake/toolchain/arm_${TOOL
 
 set(FAMILY_MCUS STM32H7 CACHE INTERNAL "")
 
+# ----------------------
+# Port & Speed Selection
+# ----------------------
+if (NOT DEFINED RHPORT_DEVICE)
+  set(RHPORT_DEVICE 0)
+endif ()
+if (NOT DEFINED RHPORT_HOST)
+  set(RHPORT_HOST 0)
+endif ()
+
+if (NOT DEFINED RHPORT_SPEED)
+  # Most F7 does not has built-in HS PHY
+  set(RHPORT_SPEED OPT_MODE_FULL_SPEED OPT_MODE_FULL_SPEED)
+endif ()
+if (NOT DEFINED RHPORT_DEVICE_SPEED)
+  list(GET RHPORT_SPEED ${RHPORT_DEVICE} RHPORT_DEVICE_SPEED)
+endif ()
+if (NOT DEFINED RHPORT_HOST_SPEED)
+  list(GET RHPORT_SPEED ${RHPORT_HOST} RHPORT_HOST_SPEED)
+endif ()
+
+cmake_print_variables(RHPORT_DEVICE RHPORT_DEVICE_SPEED RHPORT_HOST RHPORT_HOST_SPEED)
 
 #------------------------------------
 # BOARD_TARGET
@@ -42,6 +65,8 @@ function(add_board_target BOARD_TARGET)
     ${ST_HAL_DRIVER}/Src/${ST_PREFIX}_hal_cortex.c
     ${ST_HAL_DRIVER}/Src/${ST_PREFIX}_hal_dma.c
     ${ST_HAL_DRIVER}/Src/${ST_PREFIX}_hal_gpio.c
+    ${ST_HAL_DRIVER}/Src/${ST_PREFIX}_hal_i2c.c
+    ${ST_HAL_DRIVER}/Src/${ST_PREFIX}_hal_i2c_ex.c
     ${ST_HAL_DRIVER}/Src/${ST_PREFIX}_hal_pwr.c
     ${ST_HAL_DRIVER}/Src/${ST_PREFIX}_hal_pwr_ex.c
     ${ST_HAL_DRIVER}/Src/${ST_PREFIX}_hal_rcc.c
@@ -56,8 +81,12 @@ function(add_board_target BOARD_TARGET)
     ${ST_CMSIS}/Include
     ${ST_HAL_DRIVER}/Inc
     )
-  #target_compile_options(${BOARD_TARGET} PUBLIC)
-  #target_compile_definitions(${BOARD_TARGET} PUBLIC)
+  target_compile_definitions(${BOARD_TARGET} PUBLIC
+    BOARD_TUD_RHPORT=${RHPORT_DEVICE}
+    BOARD_TUD_MAX_SPEED=${RHPORT_DEVICE_SPEED}
+    BOARD_TUH_RHPORT=${RHPORT_HOST}
+    BOARD_TUH_MAX_SPEED=${RHPORT_HOST_SPEED}
+    )
 
   update_board(${BOARD_TARGET})
 
@@ -106,6 +135,8 @@ function(family_configure_example TARGET RTOS)
   family_add_tinyusb(${TARGET} OPT_MCU_STM32H7 ${RTOS})
   target_sources(${TARGET}-tinyusb PUBLIC
     ${TOP}/src/portable/synopsys/dwc2/dcd_dwc2.c
+    ${TOP}/src/portable/synopsys/dwc2/hcd_dwc2.c
+    ${TOP}/src/portable/synopsys/dwc2/dwc2_common.c
     )
   target_link_libraries(${TARGET}-tinyusb PUBLIC board_${BOARD})
 
diff --git a/hw/bsp/stm32h7/family.mk b/hw/bsp/stm32h7/family.mk
index 40df190db..29b83cf7d 100644
--- a/hw/bsp/stm32h7/family.mk
+++ b/hw/bsp/stm32h7/family.mk
@@ -1,31 +1,48 @@
-UF2_FAMILY_ID = 0x6db66082
 ST_FAMILY = h7
-DEPS_SUBMODULES += lib/CMSIS_5 hw/mcu/st/cmsis_device_$(ST_FAMILY) hw/mcu/st/stm32$(ST_FAMILY)xx_hal_driver
-
+ST_PREFIX = stm32${ST_FAMILY}xx
 ST_CMSIS = hw/mcu/st/cmsis_device_$(ST_FAMILY)
-ST_HAL_DRIVER = hw/mcu/st/stm32$(ST_FAMILY)xx_hal_driver
+ST_HAL_DRIVER = hw/mcu/st/${ST_PREFIX}_hal_driver
+ST_MFXSTM32L152 = hw/mcu/st/stm32-mfxstm32l152
+
+UF2_FAMILY_ID = 0x6db66082
 
 include $(TOP)/$(BOARD_PATH)/board.mk
 CPU_CORE ?= cortex-m7
 
+# ----------------------
+# Port & Speed Selection
+# ----------------------
+RHPORT_SPEED ?= OPT_MODE_FULL_SPEED OPT_MODE_FULL_SPEED
+RHPORT_DEVICE ?= 0
+RHPORT_HOST ?= 0
+
+# Determine RHPORT_DEVICE_SPEED if not defined
+ifndef RHPORT_DEVICE_SPEED
+ifeq ($(RHPORT_DEVICE), 0)
+  RHPORT_DEVICE_SPEED = $(firstword $(RHPORT_SPEED))
+else
+  RHPORT_DEVICE_SPEED = $(lastword $(RHPORT_SPEED))
+endif
+endif
+
+# Determine RHPORT_HOST_SPEED if not defined
+ifndef RHPORT_HOST_SPEED
+ifeq ($(RHPORT_HOST), 0)
+  RHPORT_HOST_SPEED = $(firstword $(RHPORT_SPEED))
+else
+  RHPORT_HOST_SPEED = $(lastword $(RHPORT_SPEED))
+endif
+endif
+
 # --------------
 # Compiler Flags
 # --------------
 CFLAGS += \
   -DCFG_TUSB_MCU=OPT_MCU_STM32H7 \
-	-DBOARD_TUD_RHPORT=$(PORT)
-
-ifeq ($(PORT), 1)
-  ifeq ($(SPEED), high)
-    CFLAGS += -DBOARD_TUD_MAX_SPEED=OPT_MODE_HIGH_SPEED
-    $(info "Using OTG_HS in HighSpeed mode")
-  else
-    CFLAGS += -DBOARD_TUD_MAX_SPEED=OPT_MODE_FULL_SPEED
-    $(info "Using OTG_HS in FullSpeed mode")
-  endif
-else
-  $(info "Using OTG_FS")
-endif
+	-DBOARD_TUD_RHPORT=${RHPORT_DEVICE} \
+	-DBOARD_TUD_MAX_SPEED=${RHPORT_DEVICE_SPEED} \
+	-DBOARD_TUH_RHPORT=${RHPORT_HOST} \
+	-DBOARD_TUH_MAX_SPEED=${RHPORT_HOST_SPEED} \
 
 # GCC Flags
 CFLAGS_GCC += \
@@ -46,20 +63,31 @@ LDFLAGS_GCC += \
 
 SRC_C += \
 	src/portable/synopsys/dwc2/dcd_dwc2.c \
-	$(ST_CMSIS)/Source/Templates/system_stm32$(ST_FAMILY)xx.c \
-	$(ST_HAL_DRIVER)/Src/stm32$(ST_FAMILY)xx_hal.c \
-	$(ST_HAL_DRIVER)/Src/stm32$(ST_FAMILY)xx_hal_cortex.c \
-	$(ST_HAL_DRIVER)/Src/stm32$(ST_FAMILY)xx_hal_dma.c \
-	$(ST_HAL_DRIVER)/Src/stm32$(ST_FAMILY)xx_hal_gpio.c \
-	$(ST_HAL_DRIVER)/Src/stm32$(ST_FAMILY)xx_hal_pwr.c \
-	$(ST_HAL_DRIVER)/Src/stm32$(ST_FAMILY)xx_hal_pwr_ex.c \
-	$(ST_HAL_DRIVER)/Src/stm32$(ST_FAMILY)xx_hal_rcc.c \
-	$(ST_HAL_DRIVER)/Src/stm32$(ST_FAMILY)xx_hal_rcc_ex.c \
-	$(ST_HAL_DRIVER)/Src/stm32$(ST_FAMILY)xx_hal_uart.c \
-	$(ST_HAL_DRIVER)/Src/stm32$(ST_FAMILY)xx_hal_uart_ex.c \
+	src/portable/synopsys/dwc2/hcd_dwc2.c \
+	src/portable/synopsys/dwc2/dwc2_common.c \
+	$(ST_CMSIS)/Source/Templates/system_${ST_PREFIX}.c \
+	$(ST_HAL_DRIVER)/Src/${ST_PREFIX}_hal.c \
+	$(ST_HAL_DRIVER)/Src/${ST_PREFIX}_hal_cortex.c \
+	$(ST_HAL_DRIVER)/Src/${ST_PREFIX}_hal_dma.c \
+	$(ST_HAL_DRIVER)/Src/${ST_PREFIX}_hal_gpio.c \
+  ${ST_HAL_DRIVER}/Src/${ST_PREFIX}_hal_i2c.c \
+  ${ST_HAL_DRIVER}/Src/${ST_PREFIX}_hal_i2c_ex.c \
+  $(ST_HAL_DRIVER)/Src/${ST_PREFIX}_hal_pwr.c \
+	$(ST_HAL_DRIVER)/Src/${ST_PREFIX}_hal_pwr_ex.c \
+	$(ST_HAL_DRIVER)/Src/${ST_PREFIX}_hal_rcc.c \
+	$(ST_HAL_DRIVER)/Src/${ST_PREFIX}_hal_rcc_ex.c \
+	$(ST_HAL_DRIVER)/Src/${ST_PREFIX}_hal_uart.c \
+	$(ST_HAL_DRIVER)/Src/${ST_PREFIX}_hal_uart_ex.c \
 
 INC += \
 	$(TOP)/$(BOARD_PATH) \
 	$(TOP)/lib/CMSIS_5/CMSIS/Core/Include \
 	$(TOP)/$(ST_CMSIS)/Include \
 	$(TOP)/$(ST_HAL_DRIVER)/Inc
+
+# Startup
+SRC_S_GCC += $(ST_CMSIS)/Source/Templates/gcc/startup_$(MCU_VARIANT).s
+SRC_S_IAR += $(ST_CMSIS)/Source/Templates/iar/startup_$(MCU_VARIANT).s
+
+# Linker
+LD_FILE_IAR ?= $(ST_CMSIS)/Source/Templates/iar/linker/$(MCU_VARIANT)_flash.icf
diff --git a/hw/bsp/stm32h7/stm32h7xx_hal_conf.h b/hw/bsp/stm32h7/stm32h7xx_hal_conf.h
index 216fc82f2..303dcc137 100644
--- a/hw/bsp/stm32h7/stm32h7xx_hal_conf.h
+++ b/hw/bsp/stm32h7/stm32h7xx_hal_conf.h
@@ -70,7 +70,7 @@
 /* #define HAL_HCD_MODULE_ENABLED */
 /* #define HAL_HRTIM_MODULE_ENABLED */
 /* #define HAL_HSEM_MODULE_ENABLED */
-/* #define HAL_I2C_MODULE_ENABLED */
+#define HAL_I2C_MODULE_ENABLED
 /* #define HAL_I2S_MODULE_ENABLED */
 /* #define HAL_IRDA_MODULE_ENABLED */
 /* #define HAL_IWDG_MODULE_ENABLED */
diff --git a/hw/bsp/stm32l4/family.cmake b/hw/bsp/stm32l4/family.cmake
index b6eae7c7f..77876cb8b 100644
--- a/hw/bsp/stm32l4/family.cmake
+++ b/hw/bsp/stm32l4/family.cmake
@@ -104,6 +104,8 @@ function(family_configure_example TARGET RTOS)
   family_add_tinyusb(${TARGET} OPT_MCU_${FAMILY_MCUS} ${RTOS})
   target_sources(${TARGET}-tinyusb PUBLIC
     ${TOP}/src/portable/synopsys/dwc2/dcd_dwc2.c
+    ${TOP}/src/portable/synopsys/dwc2/hcd_dwc2.c
+    ${TOP}/src/portable/synopsys/dwc2/dwc2_common.c
     ${TOP}/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
     )
   target_link_libraries(${TARGET}-tinyusb PUBLIC board_${BOARD})
diff --git a/hw/bsp/stm32l4/family.mk b/hw/bsp/stm32l4/family.mk
index 411436cf6..950b6f9cb 100644
--- a/hw/bsp/stm32l4/family.mk
+++ b/hw/bsp/stm32l4/family.mk
@@ -32,6 +32,8 @@ LDFLAGS_GCC += \
 
 SRC_C += \
 	src/portable/synopsys/dwc2/dcd_dwc2.c \
+	src/portable/synopsys/dwc2/hcd_dwc2.c \
+	src/portable/synopsys/dwc2/dwc2_common.c \
 	src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c \
 	$(ST_CMSIS)/Source/Templates/system_stm32$(ST_FAMILY)xx.c \
 	$(ST_HAL_DRIVER)/Src/stm32$(ST_FAMILY)xx_hal.c \
diff --git a/hw/bsp/stm32u5/family.cmake b/hw/bsp/stm32u5/family.cmake
index f7a7aeb33..3ab7cdf8b 100644
--- a/hw/bsp/stm32u5/family.cmake
+++ b/hw/bsp/stm32u5/family.cmake
@@ -102,16 +102,13 @@ function(family_configure_example TARGET RTOS)
 
   # Add TinyUSB target and port source
   family_add_tinyusb(${TARGET} OPT_MCU_STM32U5 ${RTOS})
-  if ((${MCU_VARIANT} STREQUAL "stm32u535xx") OR (${MCU_VARIANT} STREQUAL "stm32u545xx"))
-    target_sources(${TARGET}-tinyusb PUBLIC
-      ${TOP}/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
-      )
-  else ()
-    target_sources(${TARGET}-tinyusb PUBLIC
-      ${TOP}/src/portable/synopsys/dwc2/dcd_dwc2.c
-      #${TOP}/src/portable/st/typec/typec_stm32.c
-      )
-  endif ()
+  target_sources(${TARGET}-tinyusb PUBLIC
+    ${TOP}/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
+    ${TOP}/src/portable/synopsys/dwc2/dcd_dwc2.c
+    ${TOP}/src/portable/synopsys/dwc2/hcd_dwc2.c
+    ${TOP}/src/portable/synopsys/dwc2/dwc2_common.c
+    #${TOP}/src/portable/st/typec/typec_stm32.c
+    )
   target_link_libraries(${TARGET}-tinyusb PUBLIC board_${BOARD})
 
   # Link dependencies
diff --git a/hw/bsp/stm32u5/family.mk b/hw/bsp/stm32u5/family.mk
index 89193b99f..05fe4608a 100644
--- a/hw/bsp/stm32u5/family.mk
+++ b/hw/bsp/stm32u5/family.mk
@@ -46,7 +46,9 @@ SRC_C += \
 	src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
 else
 SRC_C += \
-	src/portable/synopsys/dwc2/dcd_dwc2.c
+	src/portable/synopsys/dwc2/dcd_dwc2.c \
+	src/portable/synopsys/dwc2/hcd_dwc2.c \
+	src/portable/synopsys/dwc2/dwc2_common.c
 endif
 
 INC += \
diff --git a/hw/bsp/xmc4000/family.cmake b/hw/bsp/xmc4000/family.cmake
index d91e6f0b6..85444db28 100644
--- a/hw/bsp/xmc4000/family.cmake
+++ b/hw/bsp/xmc4000/family.cmake
@@ -86,6 +86,8 @@ function(family_configure_example TARGET RTOS)
   family_add_tinyusb(${TARGET} OPT_MCU_XMC4000 ${RTOS})
   target_sources(${TARGET}-tinyusb PUBLIC
     ${TOP}/src/portable/synopsys/dwc2/dcd_dwc2.c
+    ${TOP}/src/portable/synopsys/dwc2/hcd_dwc2.c
+    ${TOP}/src/portable/synopsys/dwc2/dwc2_common.c
     )
   target_link_libraries(${TARGET}-tinyusb PUBLIC board_${BOARD})
 
diff --git a/hw/bsp/xmc4000/family.mk b/hw/bsp/xmc4000/family.mk
index 80b38acef..a1679a2f0 100644
--- a/hw/bsp/xmc4000/family.mk
+++ b/hw/bsp/xmc4000/family.mk
@@ -16,6 +16,8 @@ CFLAGS += \
 
 SRC_C += \
 	src/portable/synopsys/dwc2/dcd_dwc2.c \
+	src/portable/synopsys/dwc2/hcd_dwc2.c \
+	src/portable/synopsys/dwc2/dwc2_common.c \
 	${SDK_DIR}/CMSIS/Infineon/COMPONENT_${MCU_VARIANT}/Source/system_${MCU_VARIANT}.c \
 	${SDK_DIR}/Newlib/syscalls.c \
 	${SDK_DIR}/XMCLib/src/xmc_gpio.c \