able to parse SOURCE_CAP message from dfp

2 files changed, 109 insertions, 7 deletions

diff --git a/src/typec/utcd.c b/src/typec/utcd.c
index fb69b01e8..abbe8b43e 100644
--- a/src/typec/utcd.c
+++ b/src/typec/utcd.c
@@ -90,13 +90,50 @@ bool tuc_init(uint8_t rhport, tusb_typec_port_type_t port_type) {
 //
 //--------------------------------------------------------------------+
 
-//bool parse_message(uint8_t const * data, uint16_t len, pd_msg_t * msg) {
-//  // TODO
-//  (void) data;
-//  (void) len;
-//  (void) msg;
-//  return false;
-//}
+bool parse_message(uint8_t rhport, uint8_t const* buf, uint16_t len) {
+  (void) rhport;
+  uint8_t const* p_end = buf + len;
+  tusb_pd_header_t const* header = (tusb_pd_header_t const*) buf;
+  uint8_t const * ptr = buf + sizeof(tusb_pd_header_t);
+
+  if (header->n_data_obj == 0) {
+    // control message
+  } else {
+    // data message
+    switch (header->msg_type) {
+      case TUSB_PD_DATA_SOURCE_CAP: {
+        for(size_t i=0; i<header->n_data_obj; i++) {
+          TU_VERIFY(ptr < p_end);
+          uint32_t const pdo = tu_le32toh(tu_unaligned_read32(ptr));
+
+          switch ((pdo >> 30) & 0x03ul) {
+            case PD_PDO_TYPE_FIXED: {
+              pd_pdo_fixed_t const* fixed = (pd_pdo_fixed_t const*) &pdo;
+              TU_LOG3("[Fixed] %u mV %u mA\r\n", fixed->voltage_50mv*50, fixed->current_max_10ma*10);
+              break;
+            }
+
+            case PD_PDO_TYPE_BATTERY:
+              break;
+
+            case PD_PDO_TYPE_VARIABLE:
+              break;
+
+            case PD_PDO_TYPE_APDO:
+              break;
+          }
+
+          ptr += 4;
+        }
+        break;
+      }
+
+      default: break;
+    }
+  }
+
+  return true;
+}
 
 void tcd_event_handler(tcd_event_t const * event, bool in_isr) {
   (void) in_isr;
@@ -112,10 +149,15 @@ void tcd_event_handler(tcd_event_t const * event, bool in_isr) {
 
     case TCD_EVENT_RX_COMPLETE:
       // TODO process message here in ISR, move to thread later
+      if (event->rx_complete.result == XFER_RESULT_SUCCESS) {
+        parse_message(event->rhport, _rx_buf, event->rx_complete.xferred_bytes);
+      }
 
       // start new rx
       tcd_rx_start(event->rhport, _rx_buf, sizeof(_rx_buf));
       break;
+
+    default: break;
   }
 }
 
diff --git a/src/typec/utcd.h b/src/typec/utcd.h
index 75ff6f1db..dbc909592 100644
--- a/src/typec/utcd.h
+++ b/src/typec/utcd.h
@@ -41,6 +41,66 @@ extern "C" {
 #define CFG_TUC_TASK_QUEUE_SZ   8
 #endif
 
+//--------------------------------------------------------------------+
+//
+//--------------------------------------------------------------------+
+
+// All table references are from USBPD Specification rev3.1 version 1.8
+enum {
+  PD_PDO_TYPE_FIXED = 0, // Vmin = Vmax
+  PD_PDO_TYPE_BATTERY,
+  PD_PDO_TYPE_VARIABLE, // non-battery
+  PD_PDO_TYPE_APDO, // Augmented Power Data Object
+};
+
+// Fixed Power Data Object (PDO) table 6-9
+typedef struct TU_ATTR_PACKED {
+  uint32_t current_max_10ma          : 10; // [9..0] Max current in 10mA unit
+  uint32_t voltage_50mv              : 10; // [19..10] Voltage in 50mV unit
+  uint32_t current_peak              :  2; // [21..20] Peak current
+  uint32_t reserved                  :  1; // [22] Reserved
+  uint32_t epr_mode_capable          :  1; // [23] epr_mode_capable
+  uint32_t unchunked_ext_msg_support :  1; // [24] UnChunked Extended Message Supported
+  uint32_t dual_role_data            :  1; // [25] Dual Role Data
+  uint32_t usb_comm_capable          :  1; // [26] USB Communications Capable
+  uint32_t unconstrained_power       :  1; // [27] Unconstrained Power
+  uint32_t usb_suspend_supported     :  1; // [28] USB Suspend Supported
+  uint32_t dual_role_power           :  1; // [29] Dual Role Power
+  uint32_t type                      :  2; // [30] Fixed Supply type = PD_PDO_TYPE_FIXED
+} pd_pdo_fixed_t;
+TU_VERIFY_STATIC(sizeof(pd_pdo_fixed_t) == 4, "Invalid size");
+
+// Battery Power Data Object (PDO) table 6-12
+typedef struct TU_ATTR_PACKED {
+  uint32_t power_max_250mw   : 10; // [9..0] Max allowable power in 250mW unit
+  uint32_t voltage_min_50mv  : 10; // [19..10] Minimum voltage in 50mV unit
+  uint32_t voltage_max_50mv  : 10; // [29..20] Maximum voltage in 50mV unit
+  uint32_t type              :  2; // [31..30] Battery type = PD_PDO_TYPE_BATTERY
+} pd_pdo_battery_t;
+TU_VERIFY_STATIC(sizeof(pd_pdo_battery_t) == 4, "Invalid size");
+
+// Variable Power Data Object (PDO) table 6-11
+typedef struct TU_ATTR_PACKED {
+  uint32_t current_max_10ma  : 10; // [9..0] Max current in 10mA unit
+  uint32_t voltage_min_50mv  : 10; // [19..10] Minimum voltage in 50mV unit
+  uint32_t voltage_max_50mv  : 10; // [29..20] Maximum voltage in 50mV unit
+  uint32_t type              :  2; // [31..30] Variable Supply type = PD_PDO_TYPE_VARIABLE
+} pd_pdo_variable_t;
+TU_VERIFY_STATIC(sizeof(pd_pdo_variable_t) == 4, "Invalid size");
+
+// Augmented Power Data Object (PDO) table 6-13
+typedef struct TU_ATTR_PACKED {
+  uint32_t current_max_50ma  :  7; // [6..0] Max current in 50mA unit
+  uint32_t reserved1         :  1; // [7] Reserved
+  uint32_t voltage_min_100mv :  8; // [15..8] Minimum Voltage in 100mV unit
+  uint32_t reserved2         :  1; // [16] Reserved
+  uint32_t voltage_max_100mv :  8; // [24..17] Maximum Voltage in 100mV unit
+  uint32_t reserved3         :  2; // [26..25] Reserved
+  uint32_t pps_power_limited :  1; // [27] PPS Power Limited
+  uint32_t spr_programmable  :  2; // [29..28] SPR Programmable Power Supply
+  uint32_t type              :  2; // [31..30] Augmented Power Data Object = PD_PDO_TYPE_APDO
+} pd_pdo_apdo_t;
+TU_VERIFY_STATIC(sizeof(pd_pdo_apdo_t) == 4, "Invalid size");
 
 //--------------------------------------------------------------------+
 // Application API