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

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

6 files changed, 221 insertions, 39 deletions

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