TCPMv2: Queue Attention messages for consumption

Set up a queue within the DPM to hold VDM:Attention messages which the
AP will be able to deplete at its leisure.

BRANCH=None
BUG=b:208884535
TEST=builds with and without AP VDM control

Change-Id: Ia73ac3c6164251ab021776fdca252033646797dd
Signed-off-by: Diana Z <dzigterman@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/4171490
Code-Coverage: Aaron Massey <aaronmassey@google.com>
Reviewed-by: Abe Levkoy <alevkoy@chromium.org>

5 files changed, 142 insertions, 6 deletions

diff --git a/common/mock/usb_pd_dpm_mock.c b/common/mock/usb_pd_dpm_mock.c
index 999b9e3651..22df6ed9a7 100644
--- a/common/mock/usb_pd_dpm_mock.c
+++ b/common/mock/usb_pd_dpm_mock.c
@@ -45,6 +45,10 @@ void dpm_vdm_naked(int port, enum tcpci_msg_type type, uint16_t svid,
 {
 }
 
+void dpm_notify_attention(int port, size_t vdo_objects, uint32_t *buf)
+{
+}
+
 void dpm_set_mode_exit_request(int port)
 {
 }
diff --git a/common/usbc/usb_pd_dpm.c b/common/usbc/usb_pd_dpm.c
index fc01783b87..e71cf99b86 100644
--- a/common/usbc/usb_pd_dpm.c
+++ b/common/usbc/usb_pd_dpm.c
@@ -17,6 +17,7 @@
 #include "hooks.h"
 #include "power.h"
 #include "power_button.h"
+#include "queue.h"
 #include "system.h"
 #include "task.h"
 #include "tcpm/tcpm.h"
@@ -46,7 +47,21 @@
 #endif
 
 /* Max Attention length is header + 1 VDO */
-#define DPM_ATTENION_MAX_VDO 2
+#define DPM_ATTENTION_MAX_VDO 2
+
+/*
+ * VDM:Attention queue for boards using AP-driven VDMs
+ *
+ * Depth must be a power of 2, which is normally enforced by the queue init
+ * code, but must be manually enforced here.
+ */
+#define DPM_ATTENTION_QUEUE_DEPTH 8
+BUILD_ASSERT(POWER_OF_TWO(DPM_ATTENTION_QUEUE_DEPTH));
+
+struct attention_queue_entry {
+	int objects;
+	uint32_t attention[DPM_ATTENTION_MAX_VDO];
+};
 
 static struct {
 	/* state machine context */
@@ -61,6 +76,10 @@ static struct {
 	uint32_t vdm_reply[VDO_MAX_SIZE];
 	uint8_t vdm_reply_cnt;
 	enum tcpci_msg_type vdm_reply_type;
+	struct queue attention_queue;
+	struct queue_state queue_state;
+	struct attention_queue_entry queue_buffer[DPM_ATTENTION_QUEUE_DEPTH];
+	mutex_t queue_lock;
 #endif
 } dpm[CONFIG_USB_PD_PORT_MAX_COUNT];
 
@@ -124,20 +143,88 @@ static void print_current_state(const int port)
 }
 
 #ifdef CONFIG_ZEPHYR
-static int init_vdm_req_mutex(const struct device *dev)
+static int init_dpm_mutexes(const struct device *dev)
 {
 	int port;
 
 	ARG_UNUSED(dev);
 
-	for (port = 0; port < CONFIG_USB_PD_PORT_MAX_COUNT; port++)
+	for (port = 0; port < CONFIG_USB_PD_PORT_MAX_COUNT; port++) {
 		k_mutex_init(&dpm[port].vdm_req_mutex);
 
+#ifdef CONFIG_USB_PD_VDM_AP_CONTROL
+		k_mutex_init(&dpm[port].queue_lock);
+#endif
+	}
+
 	return 0;
 }
-SYS_INIT(init_vdm_req_mutex, POST_KERNEL, 50);
+SYS_INIT(init_dpm_mutexes, POST_KERNEL, 50);
 #endif /* CONFIG_ZEPHYR */
 
+#ifdef CONFIG_USB_PD_VDM_AP_CONTROL
+static void init_attention_queue_structs(void)
+{
+	int i;
+
+	for (i = 0; i < CONFIG_USB_PD_PORT_MAX_COUNT; i++) {
+		dpm[i].attention_queue.state = &dpm[i].queue_state;
+		dpm[i].attention_queue.policy = &queue_policy_null;
+		dpm[i].attention_queue.buffer_units = DPM_ATTENTION_QUEUE_DEPTH;
+		dpm[i].attention_queue.buffer_units_mask =
+			DPM_ATTENTION_QUEUE_DEPTH - 1;
+		dpm[i].attention_queue.unit_bytes =
+			sizeof(struct attention_queue_entry);
+		dpm[i].attention_queue.buffer =
+			(uint8_t *)&dpm[i].queue_buffer[0];
+	}
+}
+DECLARE_HOOK(HOOK_INIT, init_attention_queue_structs, HOOK_PRIO_FIRST);
+#endif
+
+static void vdm_attention_enqueue(int port, int length, uint32_t *buf)
+{
+#ifdef CONFIG_USB_PD_VDM_AP_CONTROL
+	struct attention_queue_entry new_entry;
+
+	new_entry.objects = length;
+	memcpy(new_entry.attention, buf, length * sizeof(uint32_t));
+
+	mutex_lock(&dpm[port].queue_lock);
+
+	/* If the queue is already full, discard the last entry */
+	if (queue_is_full(&dpm[port].attention_queue))
+		queue_advance_head(&dpm[port].attention_queue, 1);
+
+	/* Note: this should not happen, but log anyway */
+	if (queue_add_unit(&dpm[port].attention_queue, &new_entry) == 0)
+		CPRINTS("Error: Dropping port %d Attention", port);
+
+	mutex_unlock(&dpm[port].queue_lock);
+#endif /* CONFIG_USB_PD_VDM_AP_CONTROL */
+}
+
+uint8_t dpm_vdm_attention_pop(int port, uint32_t *buf, uint8_t *items_left)
+{
+	int length = 0;
+#ifdef CONFIG_USB_PD_VDM_AP_CONTROL
+	struct attention_queue_entry popped_entry;
+
+	mutex_lock(&dpm[port].queue_lock);
+
+	if (!queue_is_empty(&dpm[port].attention_queue)) {
+		queue_remove_unit(&dpm[port].attention_queue, &popped_entry);
+
+		length = popped_entry.objects;
+		memcpy(buf, popped_entry.attention, length * sizeof(buf[0]));
+	}
+	*items_left = queue_count(&dpm[port].attention_queue);
+
+	mutex_unlock(&dpm[port].queue_lock);
+#endif /* CONFIG_USB_PD_VDM_AP_CONTROL */
+	return length;
+}
+
 __overridable bool board_is_tbt_usb4_port(int port)
 {
 	return true;
@@ -162,7 +249,7 @@ enum ec_status pd_request_vdm(int port, const uint32_t *data, int vdo_count,
 
 	/* SVDM Attention message must be 1 or 2 VDOs in length */
 	if (PD_VDO_SVDM(data[0]) && (PD_VDO_CMD(data[0]) == CMD_ATTENTION) &&
-	    vdo_count > DPM_ATTENION_MAX_VDO) {
+	    vdo_count > DPM_ATTENTION_MAX_VDO) {
 		mutex_unlock(&dpm[port].vdm_req_mutex);
 		return EC_RES_INVALID_PARAM;
 	}
@@ -232,6 +319,7 @@ void dpm_init(int port)
 #ifdef CONFIG_USB_PD_VDM_AP_CONTROL
 	/* Clear any stored AP messages */
 	dpm[port].vdm_reply_cnt = 0;
+	queue_init(&dpm[port].attention_queue);
 #endif
 
 	/* Ensure that DPM state machine gets reset */
@@ -424,6 +512,20 @@ static void dpm_send_req_vdm(int port)
 		DPM_CLR_FLAG(port, DPM_FLAG_SEND_VDM_REQ);
 }
 
+void dpm_notify_attention(int port, size_t vdo_objects, uint32_t *buf)
+{
+	/*
+	 * Note: legacy code just assumes 1 VDO, but the spec allows 0
+	 * This should be fine because baked-in EC logic will only be
+	 * handling DP:Attention messages, which are defined to have 1
+	 * VDO
+	 */
+	dfp_consume_attention(port, buf);
+
+	if (IS_ENABLED(CONFIG_USB_PD_VDM_AP_CONTROL))
+		vdm_attention_enqueue(port, vdo_objects, buf);
+}
+
 void dpm_handle_alert(int port, uint32_t ado)
 {
 	if (ado & ADO_EXTENDED_ALERT_EVENT) {
diff --git a/common/usbc/usb_pe_drp_sm.c b/common/usbc/usb_pe_drp_sm.c
index a505a4922d..f1bf22afa2 100644
--- a/common/usbc/usb_pe_drp_sm.c
+++ b/common/usbc/usb_pe_drp_sm.c
@@ -6308,7 +6308,8 @@ static void pe_vdm_response_entry(int port)
 		 * attention is only SVDM with no response
 		 * (just goodCRC) return zero here.
 		 */
-		dfp_consume_attention(port, rx_payload);
+		dpm_notify_attention(port, PD_HEADER_CNT(rx_emsg[port].header),
+				     rx_payload);
 		pe_set_ready_state(port);
 		return;
 #endif
diff --git a/include/usb_pd_dpm_sm.h b/include/usb_pd_dpm_sm.h
index df84ae5e49..8c06a0700c 100644
--- a/include/usb_pd_dpm_sm.h
+++ b/include/usb_pd_dpm_sm.h
@@ -98,6 +98,31 @@ void dpm_vdm_naked(int port, enum tcpci_msg_type type, uint16_t svid,
  */
 enum ec_status dpm_copy_vdm_reply(int port, uint8_t *type, uint8_t *size,
 				  uint32_t *buf);
+
+/*
+ * Informs the DPM of a received Attention message.  Note: all Attention
+ * messages are assumed to be SOP since cables are disallowed from sending
+ * this type of VDM.
+ *
+ * @param[in] port		USB-C port number
+ * @param[in] vdo_objects	Number of objects filled in
+ * @param[in] buf		Buffer containing received VDM (header and VDO)
+ */
+void dpm_notify_attention(int port, size_t vdo_objects, uint32_t *buf);
+
+/*
+ * Copy and pop the last VDM:Attention from the DPM queue.
+ *
+ * It is assumed that buf points to a uint32_t array with at least two elements
+ * to hold the minimum possible Attention response.
+ *
+ * @param[in] port		USB-C port number
+ * @param[out] buf		Buffer to copy VDM header and VDO
+ * @param[out] items_left	Number of Attention messages left in the queue
+ * @return			Number of 32-bit objects filled in (0 if empty)
+ */
+uint8_t dpm_vdm_attention_pop(int port, uint32_t *buf, uint8_t *items_left);
+
 /*
  * Determines the current allocation for the connection, past the basic
  * CONFIG_USB_PD_PULLUP value set by the TC (generally 1.5 A)
diff --git a/test/fake_usbc.c b/test/fake_usbc.c
index d4bd1e8393..19e14ba5c8 100644
--- a/test/fake_usbc.c
+++ b/test/fake_usbc.c
@@ -304,6 +304,10 @@ void dpm_set_mode_exit_request(int port)
 {
 }
 
+void dpm_notify_attention(int port, size_t vdo_objects, uint32_t *buf)
+{
+}
+
 void dpm_run(int port, int evt, int enable)
 {
 }