PD: add stub handler for Vendor Defined Messages

noahwilliamsson · Jan 31, 2021 · c24b232 · c24b232
1 parent 571ed6b
commit c24b232
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 0 deletions.
diff --git a/include/pd_sink.h b/include/pd_sink.h
@@ -113,6 +113,7 @@ struct pd_sink {
 
 private:
     void handle_msg(uint16_t header, const uint8_t* payload);
+    void handle_vd_msg(uint16_t header, const uint8_t* payload);
     void handle_src_cap_msg(uint16_t header, const uint8_t* payload);
     bool update_protocol();
     void notify(callback_event event);

diff --git a/src/pd_sink.cpp b/src/pd_sink.cpp
@@ -68,6 +68,9 @@ void pd_sink::handle_msg(uint16_t header, const uint8_t* payload)
         DEBUG_LOG("RX--> PD: data_source_capabilities header: 0x%04x\r\n", header);
         handle_src_cap_msg(header, payload);
         break;
+    case pd_msg_type::data_vendor_defined:
+        handle_vd_msg(header, payload);
+        break;
     case pd_msg_type::ctrl_get_sink_cap:
         DEBUG_LOG("RX--> PD: ctrl_get_sink_cap header: 0x%04x\r\n", header);
         break;
@@ -148,6 +151,19 @@ void pd_sink::handle_src_cap_msg(uint16_t header, const uint8_t* payload)
     notify(callback_event::source_caps_changed);
 }
 
+void pd_sink::handle_vd_msg(uint16_t header, const uint8_t* payload) {
+    pd_msg_type type = pd_header::message_type(header);
+    int n = pd_header::num_data_objs(header);
+    uint32_t *msg = (uint32_t *)payload, *data = msg;
+
+    DEBUG_LOG("RX--> PD: data_vendor_defined header: 0x%04x\r\n", header);
+    DEBUG_LOG("RX-->   type: 0x%02x\r\n", (uint8_t)type);
+    DEBUG_LOG("RX-->   objs: 0x%02x\r\n", n);
+    for (int i = 0; i < n; i++) {
+        DEBUG_LOG("RX-->   data: 0x%08x\r\n", *data++);
+    }
+}
+
 bool pd_sink::update_protocol()
 {
     auto old_protocol = protocol_;