Merge pull request #3616 from michaelajax/fix-stm32-usbc

typec/stm32: fix UCPD sink bugs affecting compatibility with newer PD sources

14 files changed, 312 insertions, 56 deletions

diff --git a/examples/typec/CMakeLists.txt b/examples/typec/CMakeLists.txt
index 24c25908a..cb6c5b5a7 100644
--- a/examples/typec/CMakeLists.txt
+++ b/examples/typec/CMakeLists.txt
@@ -2,8 +2,8 @@ cmake_minimum_required(VERSION 3.20)
 
 include(${CMAKE_CURRENT_SOURCE_DIR}/../../hw/bsp/family_support.cmake)
 
-project(tinyusb_host_examples C CXX ASM)
-family_initialize_project(tinyusb_host_examples ${CMAKE_CURRENT_LIST_DIR})
+project(tinyusb_typec_examples C CXX ASM)
+family_initialize_project(tinyusb_typec_examples ${CMAKE_CURRENT_LIST_DIR})
 
 # family_add_subdirectory will filter what to actually add based on selected FAMILY
 family_add_subdirectory(power_delivery)
diff --git a/examples/typec/power_delivery/only.txt b/examples/typec/power_delivery/only.txt
index 657aeaac5..fc458560d 100644
--- a/examples/typec/power_delivery/only.txt
+++ b/examples/typec/power_delivery/only.txt
@@ -1 +1,2 @@
 mcu:STM32G4
+mcu:STM32U5
diff --git a/examples/typec/power_delivery/skip.txt b/examples/typec/power_delivery/skip.txt
new file mode 100644
index 000000000..f25559549
--- /dev/null
+++ b/examples/typec/power_delivery/skip.txt
@@ -0,0 +1 @@
+board:stm32u545nucleo
diff --git a/examples/typec/power_delivery/src/main.c b/examples/typec/power_delivery/src/main.c
index f6191bfe8..78e29d166 100644
--- a/examples/typec/power_delivery/src/main.c
+++ b/examples/typec/power_delivery/src/main.c
@@ -39,31 +39,31 @@
 // defined here. Otherwise, you may damage your board, smoke can come out
 #define VOLTAGE_MAX_MV       5000 // maximum voltage in mV
 #define CURRENT_MAX_MA       500  // maximum current in mA
-#define CURRENT_OPERATING_MA 100  // operating current in mA
+#define CURRENT_OPERATING_MA 300  // operating current in mA
 
 /* Blink pattern
- * - 250 ms  : button is not pressed
- * - 1000 ms : button is pressed (and hold)
+ * - 250 ms  : Type-C disconnected
+ * - 1000 ms : Type-C connected
  */
 enum  {
-  BLINK_PRESSED = 250,
-  BLINK_UNPRESSED = 1000
+  BLINK_DISCONNECTED = 250,
+  BLINK_CONNECTED = 1000
 };
 
-static uint32_t blink_interval_ms = BLINK_UNPRESSED;
+static uint32_t blink_interval_ms = BLINK_CONNECTED;
 
+void typec_connect_task(void);
 void led_blinking_task(void);
 
-#define HELLO_STR   "Hello from TinyUSB\r\n"
-
 int main(void)
 {
   board_init();
   board_led_write(true);
 
-  tuc_init(0, TUSB_TYPEC_PORT_SNK);
+  tuc_init(0, TUSB_TYPEC_PORT_DISCONNECTED);
 
   while (1) {
+    typec_connect_task();
     led_blinking_task();
 
     // tinyusb typec task
@@ -87,6 +87,7 @@ bool tuc_pd_data_received_cb(uint8_t rhport, pd_header_t const* header, uint8_t
       printf("PD Source Capabilities\r\n");
       // Examine source capability and select a suitable PDO (starting from 1 with safe5v)
       uint8_t selected_pos = 1;
+      bool voltage_available = false;
 
       for(size_t i=0; i<header->n_data_obj; i++) {
         TU_VERIFY(dobj < p_end);
@@ -97,11 +98,15 @@ bool tuc_pd_data_received_cb(uint8_t rhport, pd_header_t const* header, uint8_t
             pd_pdo_fixed_t const* fixed = (pd_pdo_fixed_t const*) &pdo;
             uint32_t const voltage_mv = fixed->voltage_50mv*50;
             uint32_t const current_ma = fixed->current_max_10ma*10;
-            printf("[Fixed] %"PRIu32" mV %"PRIu32" mA\r\n", voltage_mv, current_ma);
+            printf("[Fixed] PDO%"PRIu8" %"PRIu32" mV %"PRIu32" mA\r\n", i+1, voltage_mv, current_ma);
+
+            if (voltage_mv <= VOLTAGE_MAX_MV) {
+              voltage_available = true;
 
-            if (voltage_mv <= VOLTAGE_MAX_MV && current_ma >= CURRENT_MAX_MA) {
-              // Found a suitable PDO
-              selected_pos = i+1;
+              if (current_ma >= CURRENT_MAX_MA) {
+                // Found a suitable PDO
+                selected_pos = i+1;
+              }
             }
 
             break;
@@ -124,12 +129,17 @@ bool tuc_pd_data_received_cb(uint8_t rhport, pd_header_t const* header, uint8_t
       // Be careful and make sure your board can withstand the selected PDO
       // voltage other than safe5v e.g 12v or 20v
 
+      if (!voltage_available) {
+        printf("No PDO within %"PRIu32" mV, skip request\r\n", (uint32_t) VOLTAGE_MAX_MV);
+        break;
+      }
+
       printf("Selected PDO %u\r\n", selected_pos);
 
       // Send request with selected PDO position as response to Source Cap
       pd_rdo_fixed_variable_t rdo = {
-          .current_extremum_10ma = 50, // max 500mA
-          .current_operate_10ma = 30, // 300mA
+          .current_extremum_10ma = CURRENT_MAX_MA / 10,
+          .current_operate_10ma = CURRENT_OPERATING_MA / 10,
           .reserved = 0,
           .epr_mode_capable = 0,
           .unchunked_ext_msg_support = 0,
@@ -176,6 +186,33 @@ bool tuc_pd_control_received_cb(uint8_t rhport, pd_header_t const* header) {
 }
 
 //--------------------------------------------------------------------+
+// TypeC CONNECT TASK
+//--------------------------------------------------------------------+
+void typec_connect_task(void)
+{
+  static uint32_t btn_prev = 0;
+  static bool connected = false;
+
+  uint32_t const btn = board_button_read();
+
+  if ((btn_prev == 0u) && (btn != 0u)) {
+    bool const connect = !connected;
+
+    if (connect && tuc_connect(0)) {
+      connected = true;
+      blink_interval_ms = BLINK_CONNECTED;
+      printf("Type-C connect\r\n");
+    } else if (!connect && tuc_disconnect(0)) {
+      connected = false;
+      blink_interval_ms = BLINK_DISCONNECTED;
+      printf("Type-C disconnect\r\n");
+    }
+  }
+
+  btn_prev = btn;
+}
+
+//--------------------------------------------------------------------+
 // BLINKING TASK
 //--------------------------------------------------------------------+
 void led_blinking_task(void)
diff --git a/hw/bsp/stm32u5/boards/stm32u5a5nucleo/board.h b/hw/bsp/stm32u5/boards/stm32u5a5nucleo/board.h
index 48a9ae9fb..8b3118d79 100644
--- a/hw/bsp/stm32u5/boards/stm32u5a5nucleo/board.h
+++ b/hw/bsp/stm32u5/boards/stm32u5a5nucleo/board.h
@@ -49,6 +49,10 @@ extern "C"
 #define BUTTON_PIN GPIO_PIN_13
 #define BUTTON_STATE_ACTIVE 1
 
+// UCPD DBn
+#define UCPD_DBn_PORT GPIOB
+#define UCPD_DBn_PIN GPIO_PIN_5
+
 // UART Enable for STLink VCOM
 #define UART_ID  1
 #define UART_GPIO_PORT GPIOA
diff --git a/hw/bsp/stm32u5/family.c b/hw/bsp/stm32u5/family.c
index 7969b1c70..b722b4e98 100644
--- a/hw/bsp/stm32u5/family.c
+++ b/hw/bsp/stm32u5/family.c
@@ -82,6 +82,11 @@ void OTG_HS_IRQHandler(void) {
   tusb_int_handler(0, true);
 }
 #endif
+
+// USB PD
+void UCPD1_IRQHandler(void) {
+  tuc_int_handler(0);
+}
 //--------------------------------------------------------------------+
 // MACRO TYPEDEF CONSTANT ENUM
 //--------------------------------------------------------------------+
@@ -243,6 +248,29 @@ void board_init(void) {
 
   /* Non-standard VBus sense settings */
   board_vbus_sense_init();
+
+#if CFG_TUC_ENABLED
+  // USB PD
+  // Default CC1/CC2 is PA15/PB15
+
+  // Enable pwr for disabling dead battery feature in Power's CR3
+  __HAL_RCC_PWR_CLK_ENABLE();
+  __HAL_RCC_CRC_CLK_ENABLE();
+  __HAL_RCC_UCPD_CLK_ENABLE();
+
+  // Enable DMA for USB PD
+  __HAL_RCC_GPDMA1_CLK_ENABLE();
+
+  #ifdef UCPD_DBn_PIN
+  // Configure DBn pin
+  GPIO_InitStruct.Pin = UCPD_DBn_PIN;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(UCPD_DBn_PORT, &GPIO_InitStruct);
+  HAL_GPIO_WritePin(UCPD_DBn_PORT, UCPD_DBn_PIN, GPIO_PIN_SET);
+  #endif
+
+#endif
 }
 
 //--------------------------------------------------------------------+
diff --git a/hw/bsp/stm32u5/family.cmake b/hw/bsp/stm32u5/family.cmake
index 3d23c554d..c580892b8 100644
--- a/hw/bsp/stm32u5/family.cmake
+++ b/hw/bsp/stm32u5/family.cmake
@@ -74,6 +74,7 @@ function(family_configure_example TARGET RTOS)
     ${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
     ${STARTUP_FILE_${CMAKE_C_COMPILER_ID}}
     )
   target_include_directories(${TARGET} PUBLIC
diff --git a/hw/bsp/stm32u5/family.mk b/hw/bsp/stm32u5/family.mk
index e7bc4f299..bc2525448 100644
--- a/hw/bsp/stm32u5/family.mk
+++ b/hw/bsp/stm32u5/family.mk
@@ -53,6 +53,8 @@ SRC_C += \
 	src/portable/synopsys/dwc2/hcd_dwc2.c \
 	src/portable/synopsys/dwc2/dwc2_common.c
 endif
+SRC_C += \
+	src/portable/st/typec/typec_stm32.c
 
 INC += \
 	$(TOP)/lib/CMSIS_5/CMSIS/Core/Include \
diff --git a/src/common/tusb_mcu.h b/src/common/tusb_mcu.h
index b5390a59d..35de47030 100644
--- a/src/common/tusb_mcu.h
+++ b/src/common/tusb_mcu.h
@@ -400,6 +400,10 @@
     #endif
   #endif
 
+  // TypeC controller
+  #define TUP_USBIP_TYPEC_STM32
+  #define TUP_TYPEC_RHPORTS_NUM 1
+
 #elif TU_CHECK_MCU(OPT_MCU_STM32WB)
   #define TUP_USBIP_FSDEV
   #define TUP_USBIP_FSDEV_STM32
diff --git a/src/portable/st/typec/typec_stm32.c b/src/portable/st/typec/typec_stm32.c
index 8da2e39ba..b4288968f 100644
--- a/src/portable/st/typec/typec_stm32.c
+++ b/src/portable/st/typec/typec_stm32.c
@@ -32,6 +32,10 @@
 #if CFG_TUSB_MCU == OPT_MCU_STM32G4
   #include "stm32g4xx.h"
   #include "stm32g4xx_ll_dma.h" // for UCPD REQID
+#elif CFG_TUSB_MCU == OPT_MCU_STM32U5
+  #include "stm32u5xx.h"
+  #include "stm32u5xx_ll_dma.h" // for UCPD REQID
+  #include "stm32u5xx_ll_system.h" // for DevID/RevID
 #else
   #error "Unsupported STM32 family"
 #endif
@@ -43,7 +47,9 @@
 enum {
   IMR_ATTACHED = UCPD_IMR_TXMSGDISCIE | UCPD_IMR_TXMSGSENTIE | UCPD_IMR_TXMSGABTIE | UCPD_IMR_TXUNDIE |
                  UCPD_IMR_RXHRSTDETIE | UCPD_IMR_RXOVRIE | UCPD_IMR_RXMSGENDIE | UCPD_IMR_RXORDDETIE |
-                 UCPD_IMR_HRSTDISCIE | UCPD_IMR_HRSTSENTIE | UCPD_IMR_FRSEVTIE
+                 UCPD_IMR_HRSTDISCIE | UCPD_IMR_HRSTSENTIE | UCPD_IMR_FRSEVTIE,
+
+  RX_ORDERED_SET_SOP = 0
 };
 
 #define PHY_SYNC1 0x18u
@@ -62,10 +68,13 @@ enum {
 #define PHY_ORDERED_SET_SOP_PP_DEBUG (PHY_SYNC1 | (PHY_RST2<<5u)  | (PHY_SYNC3<<10u) | (PHY_SYNC2<<15u)) // SOP'' Debug Ordered set coding
 
 
-static uint8_t const* _rx_buf;
+static uint8_t* _rx_buf;
+static uint16_t _rx_buf_len;
 static uint8_t const* _tx_pending_buf;
 static uint16_t _tx_pending_bytes;
 static uint16_t _tx_xferring_bytes;
+static bool _cc_enabled[TUP_TYPEC_RHPORTS_NUM];
+static uint32_t _rx_ordered_set;
 
 static pd_header_t _good_crc = {
     .msg_type   = PD_CTRL_GOOD_CRC,
@@ -77,8 +86,15 @@ static pd_header_t _good_crc = {
     .extended   = 0
 };
 
+#ifdef DMA1
 // address of DMA channel rx, tx for each port
 #define CFG_TUC_STM32_DMA  { { DMA1_Channel1_BASE, DMA1_Channel2_BASE } }
+#elif defined(GPDMA1)
+// address of DMA channel rx, tx for each port
+#define CFG_TUC_STM32_DMA  { { GPDMA1_Channel0_BASE, GPDMA1_Channel1_BASE } }
+#else
+#error "Unsupported STM32 family"
+#endif
 
 //--------------------------------------------------------------------+
 // DMA
@@ -91,6 +107,10 @@ TU_ATTR_ALWAYS_INLINE static inline uint32_t dma_get_addr(uint8_t rhport, bool i
 }
 
 static void dma_init(uint8_t rhport, bool is_rx) {
+  (void) rhport;
+  (void) is_rx;
+
+#if CFG_TUSB_MCU == OPT_MCU_STM32G4
   uint32_t dma_addr = dma_get_addr(rhport, is_rx);
   DMA_Channel_TypeDef* dma_ch = (DMA_Channel_TypeDef*) dma_addr;
   uint32_t req_id;
@@ -129,19 +149,45 @@ static void dma_init(uint8_t rhport, bool is_rx) {
   uint32_t mux_ccr = mux_ch->CCR & ~(DMAMUX_CxCR_DMAREQ_ID);
   mux_ccr |= req_id;
   mux_ch->CCR = mux_ccr;
+#endif
 }
 
 TU_ATTR_ALWAYS_INLINE static inline void dma_start(uint8_t rhport, bool is_rx, void const* buf, uint16_t len) {
   DMA_Channel_TypeDef* dma_ch = (DMA_Channel_TypeDef*) dma_get_addr(rhport, is_rx);
-
+#if CFG_TUSB_MCU == OPT_MCU_STM32G4
   dma_ch->CMAR = (uint32_t) buf;
   dma_ch->CNDTR = len;
   dma_ch->CCR |= DMA_CCR_EN;
+#elif CFG_TUSB_MCU == OPT_MCU_STM32U5
+  if (is_rx) {
+    // Peripheral -> Memory, Memory inc, 8-bit
+    dma_ch->CTR1 = DMA_CTR1_DINC | DMA_CTR1_DAP;
+    dma_ch->CTR2 = LL_GPDMA1_REQUEST_UCPD1_RX;
+    dma_ch->CSAR = (uint32_t) &UCPD1->RXDR;
+    dma_ch->CDAR = (uint32_t) buf;
+    dma_ch->CBR1 = len;
+  } else {
+    // Memory -> Peripheral, Memory inc, 8-bit
+    dma_ch->CTR1 = DMA_CTR1_SINC | DMA_CTR1_DAP;
+    dma_ch->CTR2 = DMA_CTR2_DREQ | LL_GPDMA1_REQUEST_UCPD1_TX;
+    dma_ch->CSAR = (uint32_t) buf;
+    dma_ch->CDAR = (uint32_t) &UCPD1->TXDR;
+    dma_ch->CBR1 = len;
+  }
+  // High priority
+  dma_ch->CCR = DMA_CCR_PRIO | DMA_CCR_EN;
+#endif
 }
 
 TU_ATTR_ALWAYS_INLINE static inline void dma_stop(uint8_t rhport, bool is_rx) {
   DMA_Channel_TypeDef* dma_ch = (DMA_Channel_TypeDef*) dma_get_addr(rhport, is_rx);
+#if CFG_TUSB_MCU == OPT_MCU_STM32G4
   dma_ch->CCR &= ~DMA_CCR_EN;
+#elif CFG_TUSB_MCU == OPT_MCU_STM32U5
+  dma_ch->CCR |= DMA_CCR_SUSP;
+  while((dma_ch->CSR & (DMA_CSR_SUSPF | DMA_CSR_IDLEF)) == 0U);
+  dma_ch->CCR = DMA_CCR_RESET;
+#endif
 }
 
 TU_ATTR_ALWAYS_INLINE static inline bool dma_enabled(uint8_t rhport, bool is_rx) {
@@ -176,30 +222,51 @@ bool tcd_init(uint8_t rhport, uint32_t port_type) {
                 (0x01 << UCPD_CFG1_PSC_UCPDCLK_Pos) | (0x1f << UCPD_CFG1_RXORDSETEN_Pos);
   UCPD1->CFG1 |= UCPD_CFG1_UCPDEN;
 
-  // General programming sequence (with UCPD configured then enabled)
-  if (port_type == TUSB_TYPEC_PORT_SNK) {
-    // Set analog mode enable both CC Phy
-    UCPD1->CR = (0x01 << UCPD_CR_ANAMODE_Pos) | (UCPD_CR_CCENABLE_0 | UCPD_CR_CCENABLE_1);
+#ifdef UCPD_CFG2_RXAFILTEN
+  // Enable Rx analog filter
+  UCPD1->CFG2 |= UCPD_CFG2_RXAFILTEN;
+#endif
 
-    // Read Voltage State on CC1 & CC2 fore initial state
-    uint32_t v_cc[2];
-    (void) v_cc;
-    v_cc[0] = (UCPD1->SR >> UCPD_SR_TYPEC_VSTATE_CC1_Pos) & 0x03;
-    v_cc[1] = (UCPD1->SR >> UCPD_SR_TYPEC_VSTATE_CC2_Pos) & 0x03;
-    TU_LOG1("Initial VState CC1 = %lu, CC2 = %lu\r\n", v_cc[0], v_cc[1]);
+  // The CC pull-up (Rp) and pull-down (Rd) must be trimmed for some STM32U5 devices.
+#ifdef UCPD_CFG3_TRIM_CC1_RP
+  uint32_t dev_id = LL_DBGMCU_GetDeviceID();
+  uint32_t rev_id = LL_DBGMCU_GetRevisionID();
 
-    // Enable CC1 & CC2 Interrupt
-    UCPD1->IMR = UCPD_IMR_TYPECEVT1IE | UCPD_IMR_TYPECEVT2IE;
+  // Values taken from STM32U5 UCPD middleware.
+  // The list might be not complete, a case has been raised to ST.
+  if (((dev_id == 0x482UL) && (rev_id == 0x3000UL)) ||
+      ((dev_id == 0x481UL) && (rev_id == 0x2001UL)) ||
+      ((dev_id == 0x481UL) && (rev_id == 0x3000UL)) ||
+      ((dev_id == 0x481UL) && (rev_id == 0x3001UL)) ||
+      ((dev_id == 0x476UL) && (rev_id == 0x1000UL))) {
+    const uint8_t trim_3a0_cc1 = (*(uint8_t*) 0x0BFA0545) & 0x0F;
+    const uint8_t trim_3a0_cc2 = (*(uint8_t*) 0x0BFA0547) & 0x0F;
+    const uint8_t trim_rd_cc1  = (*(uint8_t*) 0x0BFA0544) & 0x0F;
+    const uint8_t trim_rd_cc2  = (*(uint8_t*) 0x0BFA0546) & 0x0F;
+
+    UCPD1->CFG3 = trim_3a0_cc1 << UCPD_CFG3_TRIM_CC1_RP_Pos | trim_3a0_cc2 << UCPD_CFG3_TRIM_CC2_RP_Pos |
+                  trim_rd_cc1  << UCPD_CFG3_TRIM_CC1_RD_Pos | trim_rd_cc2  << UCPD_CFG3_TRIM_CC2_RD_Pos;
   }
+#endif
 
+  // General programming sequence (with UCPD configured then enabled)
+  if (port_type == TUSB_TYPEC_PORT_SNK) {
+    tcd_connect(rhport);
+  }
+
+#if CFG_TUSB_MCU == OPT_MCU_STM32G4
   // Disable dead battery in PWR's CR3
   PWR->CR3 |= PWR_CR3_UCPD_DBDIS;
+#elif CFG_TUSB_MCU == OPT_MCU_STM32U5
+  // Disable dead battery in PWR's UCPDR
+  PWR->UCPDR |= PWR_UCPDR_UCPD_DBDIS;
+#endif
 
   return true;
 }
 
 // Enable interrupt
-void tcd_int_enable (uint8_t rhport) {
+void tcd_int_enable(uint8_t rhport) {
   (void) rhport;
   NVIC_EnableIRQ(UCPD1_IRQn);
 }
@@ -210,8 +277,60 @@ void tcd_int_disable(uint8_t rhport) {
   NVIC_DisableIRQ(UCPD1_IRQn);
 }
 
+void tcd_connect(uint8_t rhport) {
+  if (_cc_enabled[rhport]) {
+    return;
+  }
+
+  _cc_enabled[rhport] = true;
+
+  // Set analog mode and enable both CC Phy as sink.
+  uint32_t cr = UCPD1->CR;
+  cr &= ~(UCPD_CR_PHYRXEN | UCPD_CR_PHYCCSEL | UCPD_CR_CCENABLE);
+  cr |= (0x01 << UCPD_CR_ANAMODE_Pos) | UCPD_CR_CCENABLE_0 | UCPD_CR_CCENABLE_1;
+  UCPD1->CR = cr;
+
+  UCPD1->ICR = UCPD_ICR_TYPECEVT1CF | UCPD_ICR_TYPECEVT2CF;
+  UCPD1->IMR |= UCPD_IMR_TYPECEVT1IE | UCPD_IMR_TYPECEVT2IE;
+}
+
+void tcd_disconnect(uint8_t rhport) {
+  if (!_cc_enabled[rhport]) {
+    return;
+  }
+
+  // Mask UCPD interrupts/DMA early to avoid the ISR touching DMA/pointers while we tear down.
+  UCPD1->IMR &= ~(UCPD_IMR_TYPECEVT1IE | UCPD_IMR_TYPECEVT2IE | IMR_ATTACHED);
+  UCPD1->CFG1 &= ~(UCPD_CFG1_RXDMAEN | UCPD_CFG1_TXDMAEN);
+
+  _cc_enabled[rhport] = false;
+
+  if (dma_enabled(rhport, true)) {
+    dma_stop(rhport, true);
+  }
+
+  if (dma_enabled(rhport, false)) {
+    dma_tx_stop(rhport);
+  }
+
+  _rx_buf = NULL;
+  _rx_buf_len = 0;
+  _tx_pending_buf = NULL;
+  _tx_pending_bytes = 0;
+  _tx_xferring_bytes = 0;
+
+  uint32_t cr = UCPD1->CR;
+  cr &= ~(UCPD_CR_PHYRXEN | UCPD_CR_PHYCCSEL | UCPD_CR_CCENABLE);
+  UCPD1->CR = cr;
+
+  UCPD1->ICR = UCPD_ICR_TYPECEVT1CF | UCPD_ICR_TYPECEVT2CF;
+
+  tcd_event_cc_changed(rhport, 0, 0, false);
+}
+
 bool tcd_msg_receive(uint8_t rhport, uint8_t* buffer, uint16_t total_bytes) {
   _rx_buf = buffer;
+  _rx_buf_len = total_bytes;
   dma_start(rhport, true, buffer, total_bytes);
   return true;
 }
@@ -251,14 +370,12 @@ void tcd_int_handler(uint8_t rhport) {
     uint32_t cr = UCPD1->CR;
 
     // TODO only support SNK for now, required highest voltage for now
-    // Enable PHY on active CC and disable Rd on other CC
-    // FIXME somehow CC2 is vstate is not correct, always 1 even not attached.
-    // on DPOW1 board, it is connected to PA10 (USBPD_DBCC2), we probably miss something.
-    if ((sr & UCPD_SR_TYPECEVT1) && (v_cc[0] == 3)) {
+    // Determine attach/detach by checking both CC vstates.
+    if (v_cc[0] >= 1) {
       TU_LOG3("Attach CC1\r\n");
       cr &= ~(UCPD_CR_PHYCCSEL | UCPD_CR_CCENABLE);
       cr |= UCPD_CR_PHYRXEN | UCPD_CR_CCENABLE_0;
-    } else if ((sr & UCPD_SR_TYPECEVT2) && (v_cc[1] == 3)) {
+    } else if (v_cc[1] >= 1) {
       TU_LOG3("Attach CC2\r\n");
       cr &= ~UCPD_CR_CCENABLE;
       cr |= (UCPD_CR_PHYCCSEL | UCPD_CR_PHYRXEN | UCPD_CR_CCENABLE_1);
@@ -272,7 +389,7 @@ void tcd_int_handler(uint8_t rhport) {
       // Attached
       UCPD1->IMR |= IMR_ATTACHED;
       UCPD1->CFG1 |= UCPD_CFG1_RXDMAEN | UCPD_CFG1_TXDMAEN;
-    }else {
+    } else {
       // Detached
       UCPD1->CFG1 &= ~(UCPD_CFG1_RXDMAEN | UCPD_CFG1_TXDMAEN);
       UCPD1->IMR &= ~IMR_ATTACHED;
@@ -290,8 +407,8 @@ void tcd_int_handler(uint8_t rhport) {
   //------------- RX -------------//
   if (sr & UCPD_SR_RXORDDET) {
     // SOP: Start of Packet.
-    TU_LOG3("SOP\r\n");
-    // UCPD1->RX_ORDSET & UCPD_RX_ORDSET_RXORDSET_Msk;
+    _rx_ordered_set = UCPD1->RX_ORDSET & UCPD_RX_ORDSET_RXORDSET;
+    TU_LOG3("SOP %lu\r\n", _rx_ordered_set);
 
     // ack
     UCPD1->ICR = UCPD_ICR_RXORDDETCF;
@@ -304,27 +421,40 @@ void tcd_int_handler(uint8_t rhport) {
     // stop TX
     dma_stop(rhport, true);
 
-    uint8_t result;
+    if (_rx_ordered_set == RX_ORDERED_SET_SOP) {
+      uint8_t result;
+
+      if (!(sr & UCPD_SR_RXERR)) {
+        // Send GoodCRC in response, unless the received message is itself a GoodCRC.
+        // TODO move this to usbc stack
+        if (_rx_buf) {
+          pd_header_t const* rx_header = (pd_header_t const*) _rx_buf;
+          bool is_good_crc = (rx_header->n_data_obj == 0) && (rx_header->msg_type == PD_CTRL_GOOD_CRC);
 
-    if (!(sr & UCPD_SR_RXERR)) {
-      // response with good crc
-      // TODO move this to usbc stack
-      if (_rx_buf) {
-        _good_crc.msg_id = ((pd_header_t const *) _rx_buf)->msg_id;
-        dma_tx_start(rhport, &_good_crc, 2);
+          if (!is_good_crc) {
+            _good_crc.msg_id = rx_header->msg_id;
+            dma_tx_start(rhport, &_good_crc, 2);
+          }
+        }
+
+        result = XFER_RESULT_SUCCESS;
+      } else {
+        // CRC failed
+        result = XFER_RESULT_FAILED;
       }
 
-      result = XFER_RESULT_SUCCESS;
-    }else {
-      // CRC failed
-      result = XFER_RESULT_FAILED;
+      // notify stack
+      tcd_event_rx_complete(rhport, UCPD1->RX_PAYSZ, result, true);
+    } else {
+      TU_LOG3("Ignore SOP* RX %lu\r\n", _rx_ordered_set);
     }
 
-    // notify stack
-    tcd_event_rx_complete(rhport, UCPD1->RX_PAYSZ, result, true);
-
     // ack
     UCPD1->ICR = UCPD_ICR_RXMSGENDCF;
+
+    if (_rx_ordered_set != RX_ORDERED_SET_SOP && _rx_buf != NULL && _rx_buf_len > 0) {
+      tcd_msg_receive(rhport, _rx_buf, _rx_buf_len);
+    }
   }
 
   if (sr & UCPD_SR_RXOVR) {
@@ -333,6 +463,17 @@ void tcd_int_handler(uint8_t rhport) {
     UCPD1->ICR = UCPD_ICR_RXOVRCF;
   }
 
+  // Handle Hard Reset received from source. If not cleared here, the
+  // flag will remain set and re-enter the ISR immediately
+  if (sr & UCPD_SR_RXHRSTDET) {
+    TU_LOG3("Hard Reset received\r\n");
+    if (_rx_buf != NULL && _rx_buf_len > 0) {
+      dma_stop(rhport, true);
+      tcd_msg_receive(rhport, _rx_buf, _rx_buf_len);
+    }
+    UCPD1->ICR = UCPD_ICR_RXHRSTDETCF;
+  }
+
   //------------- TX -------------//
   // All tx events: complete and error
   if (sr & (UCPD_SR_TXMSGSENT | (UCPD_SR_TXMSGDISC | UCPD_SR_TXMSGABT | UCPD_SR_TXUND))) {
diff --git a/src/typec/pd_types.h b/src/typec/pd_types.h
index 950f4d488..eebcec632 100644
--- a/src/typec/pd_types.h
+++ b/src/typec/pd_types.h
@@ -46,7 +46,8 @@ TU_ATTR_BIT_FIELD_ORDER_BEGIN
 typedef enum {
   TUSB_TYPEC_PORT_SRC,
   TUSB_TYPEC_PORT_SNK,
-  TUSB_TYPEC_PORT_DRP
+  TUSB_TYPEC_PORT_DRP,
+  TUSB_TYPEC_PORT_DISCONNECTED,
 } tusb_typec_port_type_t;
 
 enum {
diff --git a/src/typec/tcd.h b/src/typec/tcd.h
index da7ab4b13..6e18cd063 100644
--- a/src/typec/tcd.h
+++ b/src/typec/tcd.h
@@ -78,6 +78,12 @@ void tcd_int_enable (uint8_t rhport);
 // Disable interrupt
 void tcd_int_disable(uint8_t rhport);
 
+// Enable Type-C port terminations
+void tcd_connect(uint8_t rhport);
+
+// Disable Type-C port terminations
+void tcd_disconnect(uint8_t rhport);
+
 // Interrupt Handler
 void tcd_int_handler(uint8_t rhport);
 
diff --git a/src/typec/usbc.c b/src/typec/usbc.c
index 20abd1700..8071e93c3 100644
--- a/src/typec/usbc.c
+++ b/src/typec/usbc.c
@@ -76,6 +76,14 @@ TU_ATTR_WEAK bool tuc_pd_control_received_cb(uint8_t rhport, pd_header_t const*
   return false;
 }
 
+TU_ATTR_WEAK void tcd_connect(uint8_t rhport) {
+  (void) rhport;
+}
+
+TU_ATTR_WEAK void tcd_disconnect(uint8_t rhport) {
+  (void) rhport;
+}
+
 //--------------------------------------------------------------------+
 //
 //--------------------------------------------------------------------+
@@ -83,6 +91,20 @@ bool tuc_inited(uint8_t rhport) {
   return _usbc_inited && _port_inited[rhport];
 }
 
+bool tuc_connect(uint8_t rhport) {
+  TU_VERIFY(rhport < TUP_TYPEC_RHPORTS_NUM && tuc_inited(rhport));
+
+  tcd_connect(rhport);
+  return true;
+}
+
+bool tuc_disconnect(uint8_t rhport) {
+  TU_VERIFY(rhport < TUP_TYPEC_RHPORTS_NUM && tuc_inited(rhport));
+
+  tcd_disconnect(rhport);
+  return true;
+}
+
 bool tuc_init(uint8_t rhport, uint32_t port_type) {
   // Initialize stack
   if (!_usbc_inited) {
diff --git a/src/typec/usbc.h b/src/typec/usbc.h
index 711119596..ed8595d27 100644
--- a/src/typec/usbc.h
+++ b/src/typec/usbc.h
@@ -52,6 +52,14 @@ bool tuc_init(uint8_t rhport, uint32_t port_type);
 // Check if typec port is initialized
 bool tuc_inited(uint8_t rhport);
 
+// Enable Type-C port terminations
+// Return false if port is not initialized
+bool tuc_connect(uint8_t rhport);
+
+// Disable Type-C port terminations
+// Return false if port is not initialized
+bool tuc_disconnect(uint8_t rhport);
+
 // Task function should be called in main/rtos loop, extended version of tud_task()
 // - timeout_ms: millisecond to wait, zero = no wait, 0xFFFFFFFF = wait forever
 // - in_isr: if function is called in ISR