1 files changed, 229 insertions, 18 deletions

diff --git a/driver/usb_mux/usb_mux.c b/driver/usb_mux/usb_mux.c
index 155ba8fb3e..037757cd98 100644
--- a/driver/usb_mux/usb_mux.c
+++ b/driver/usb_mux/usb_mux.c
@@ -11,6 +11,7 @@
 #include "chipset.h"
 #include "hooks.h"
 #include "host_command.h"
+#include "queue.h"
 #include "task.h"
 #include "timer.h"
 #include "usb_mux.h"
@@ -46,6 +47,10 @@ static mutex_t mux_lock[CONFIG_USB_PD_PORT_MAX_COUNT];
 static task_id_t ack_task[CONFIG_USB_PD_PORT_MAX_COUNT] = {
 	[0 ... CONFIG_USB_PD_PORT_MAX_COUNT - 1] = TASK_ID_INVALID };
 
+static void perform_mux_set(int port, mux_state_t mux_mode,
+			    enum usb_switch usb_mode, int polarity);
+static void perform_mux_hpd_update(int port, mux_state_t hpd_state);
+
 enum mux_config_type {
 	USB_MUX_INIT,
 	USB_MUX_LOW_POWER,
@@ -55,19 +60,174 @@ enum mux_config_type {
 	USB_MUX_HPD_UPDATE,
 };
 
+/* Define a USB mux task ID for the purpose of linking */
+#ifndef HAS_TASK_USB_MUX
+#define TASK_ID_USB_MUX TASK_ID_INVALID
+#endif
+
+/*
+ * USB mux task
+ *
+ * Since USB mux sets can take extended periods of time (on the order of 100s of
+ * ms for some muxes), run a small task to complete those mux sets in order to
+ * not block the PD task.  Run HPD sets from this task as well, since they
+ * should be sequenced behind setting up the mux pins for DP.
+ *
+ * Depth must be a power of 2, which is normally enforced by the queue init
+ * code, but must be manually enforced here.
+ */
+#define MUX_QUEUE_DEPTH		4
+BUILD_ASSERT(POWER_OF_TWO(MUX_QUEUE_DEPTH));
+
+struct mux_queue_entry {
+	enum mux_config_type type;
+	mux_state_t mux_mode;		/* For both HPD and mux set */
+	enum usb_switch usb_config;	/* Set only */
+	int polarity;			/* Set only */
+};
+
+/*
+ * Note: test builds won't optimize out the mux task code and thereby require
+ * the queue to link
+ */
+#if defined(TEST_BUILD) || defined(HAS_TASK_USB_MUX)
+/*
+ * Note: QUEUE macros cannot be used to initialize this array, since they rely
+ * on anonymous data structs for allocation which results in all entries
+ * sharing the same state pointer and data buffers.
+ */
+static struct queue mux_queue[CONFIG_USB_PD_PORT_MAX_COUNT];
+__maybe_unused static struct queue_state
+				queue_states[CONFIG_USB_PD_PORT_MAX_COUNT];
+__maybe_unused static struct mux_queue_entry
+				queue_buffers[CONFIG_USB_PD_PORT_MAX_COUNT]
+					     [MUX_QUEUE_DEPTH];
+static mutex_t queue_lock[CONFIG_USB_PD_PORT_MAX_COUNT];
+#else
+extern struct queue const mux_queue[];
+extern mutex_t queue_lock[];
+#endif
+
 #ifdef CONFIG_ZEPHYR
 static int init_mux_mutex(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(&mux_lock[port]);
+
+		if (IS_ENABLED(HAS_TASK_USB_MUX))
+			k_mutex_init(&queue_lock[port]);
+	}
+
 	return 0;
 }
 SYS_INIT(init_mux_mutex, POST_KERNEL, 50);
 #endif /* CONFIG_ZEPHYR */
 
+__maybe_unused static void mux_task_enqueue(int port, enum mux_config_type type,
+					    mux_state_t mux_mode,
+					    enum usb_switch usb_config,
+					    int polarity)
+{
+	struct mux_queue_entry new_entry;
+
+	if (!IS_ENABLED(HAS_TASK_USB_MUX))
+		return;
+
+	new_entry.type = type;
+	new_entry.mux_mode = mux_mode;
+	new_entry.usb_config = usb_config;
+	new_entry.polarity = polarity;
+
+	mutex_lock(&queue_lock[port]);
+
+	if (queue_add_unit(&mux_queue[port], &new_entry) == 0)
+		CPRINTS("Error: Dropping port %d mux %d", port, type);
+	else
+		task_wake(TASK_ID_USB_MUX);
+
+	mutex_unlock(&queue_lock[port]);
+}
+
+#ifdef HAS_TASK_USB_MUX
+static void init_queue_structs(void)
+{
+	int i;
+
+	for (i = 0; i < CONFIG_USB_PD_PORT_MAX_COUNT; i++) {
+		mux_queue[i].state = &queue_states[i];
+		mux_queue[i].policy = &queue_policy_null;
+		mux_queue[i].buffer_units = MUX_QUEUE_DEPTH;
+		mux_queue[i].buffer_units_mask = MUX_QUEUE_DEPTH - 1;
+		mux_queue[i].unit_bytes = sizeof(struct mux_queue_entry);
+		mux_queue[i].buffer = (uint8_t *) &queue_buffers[i][0];
+	}
+}
+DECLARE_HOOK(HOOK_INIT, init_queue_structs, HOOK_PRIO_FIRST);
+#endif
+
+__maybe_unused void usb_mux_task(void *u)
+{
+	bool items_waiting = true;
+
+	while (1) {
+		int port;
+
+		/* Wait if we had no queue items to service */
+		if (!items_waiting)
+			task_wait_event(-1);
+
+		items_waiting = false;
+
+		/*
+		 * Round robin the ports, so no one port can monopolize the task
+		 */
+		for (port = 0; port < board_get_usb_pd_port_count(); port++) {
+			if (queue_count(&mux_queue[port])) {
+				/*
+				 * Process our first item.  Leave it in the
+				 * queue until we've completed its operation so
+				 * the PD task can tell it is still pending.
+				 * Note this should be safe to do unlocked, as
+				 * this task is the only one which changes the
+				 * queue head.
+				 */
+				struct mux_queue_entry next;
+
+				queue_peek_units(&mux_queue[port], &next, 0, 1);
+
+				if (next.type == USB_MUX_SET_MODE)
+					perform_mux_set(port, next.mux_mode,
+							next.usb_config,
+							next.polarity);
+				else if (next.type == USB_MUX_HPD_UPDATE)
+					perform_mux_hpd_update(port,
+							       next.mux_mode);
+				else
+					CPRINTS("Error: Unknown mux task type:"
+						"%d", next.type);
+
+				/*
+				 * Lock since the tail is changing, which would
+				 * disrupt any calls iterating the queue.
+				 */
+				mutex_lock(&queue_lock[port]);
+				queue_advance_head(&mux_queue[port], 1);
+				mutex_unlock(&queue_lock[port]);
+
+				/*
+				 * Force the task to run again if this queue has
+				 * more items to process.
+				 */
+				if (queue_count(&mux_queue[port]))
+					items_waiting = true;
+			}
+		}
+	}
+}
+
 /* Configure the MUX */
 static int configure_mux(int port,
 			 enum mux_config_type config,
@@ -173,8 +333,16 @@ static int configure_mux(int port,
 		mutex_unlock(&mux_lock[port]);
 
 		if (ack_required) {
-			/* This should only be called from the PD task */
-			assert(port == TASK_ID_TO_PD_PORT(task_get_current()));
+			/*
+			 * This should only be called from the PD task or usb
+			 * mux task
+			 */
+			if (IS_ENABLED(HAS_TASK_USB_MUX)) {
+				assert(task_get_current() == TASK_ID_USB_MUX);
+			} else {
+				assert(port ==
+				       TASK_ID_TO_PD_PORT(task_get_current()));
+			}
 
 			/*
 			 * Note: This task event could be generalized for more
@@ -254,22 +422,14 @@ void usb_mux_init(int port)
 		atomic_clear_bits(&flags[port], USB_MUX_FLAG_IN_LPM);
 }
 
-/*
- * TODO(crbug.com/505480): Setting muxes often involves I2C transcations,
- * which can block. Consider implementing an asynchronous task.
- */
-void usb_mux_set(int port, mux_state_t mux_mode,
-		 enum usb_switch usb_mode, int polarity)
+static void perform_mux_set(int port, mux_state_t mux_mode,
+			    enum usb_switch usb_mode, int polarity)
 {
 	mux_state_t mux_state;
 	const int should_enter_low_power_mode =
 		(mux_mode == USB_PD_MUX_NONE &&
 		usb_mode == USB_SWITCH_DISCONNECT);
 
-	if (port >= board_get_usb_pd_port_count()) {
-		return;
-	}
-
 	/* Perform initialization if not initialized yet */
 	if (!(flags[port] & USB_MUX_FLAG_INIT))
 		usb_mux_init(port);
@@ -310,6 +470,48 @@ void usb_mux_set(int port, mux_state_t mux_mode,
 		enter_low_power_mode(port);
 }
 
+void usb_mux_set(int port, mux_state_t mux_mode,
+		 enum usb_switch usb_mode, int polarity)
+{
+	if (port >= board_get_usb_pd_port_count())
+		return;
+
+	/* Block if we have no mux task, but otherwise queue it up and return */
+	if (IS_ENABLED(HAS_TASK_USB_MUX))
+		mux_task_enqueue(port, USB_MUX_SET_MODE, mux_mode,
+				 usb_mode, polarity);
+	else
+		perform_mux_set(port,  mux_mode, usb_mode, polarity);
+}
+
+bool usb_mux_set_completed(int port)
+{
+	bool sets_pending = false;
+	struct queue_iterator it;
+
+	/* No mux task, no items waiting to process */
+	if (!IS_ENABLED(HAS_TASK_USB_MUX))
+		return true;
+
+	/* Lock the queue so we can scroll through the items left to do */
+	mutex_lock(&queue_lock[port]);
+
+	for (queue_begin(&mux_queue[port], &it); it.ptr != NULL;
+					queue_next(&mux_queue[port], &it)) {
+		const struct mux_queue_entry *check =
+					(struct mux_queue_entry *) it.ptr;
+
+		if (check->type == USB_MUX_SET_MODE) {
+			sets_pending = true;
+			break;
+		}
+	}
+
+	mutex_unlock(&queue_lock[port]);
+
+	return !sets_pending;
+}
+
 mux_state_t usb_mux_get(int port)
 {
 	mux_state_t mux_state;
@@ -357,12 +559,8 @@ void usb_mux_flip(int port)
 	configure_mux(port, USB_MUX_SET_MODE, &mux_state);
 }
 
-void usb_mux_hpd_update(int port, mux_state_t hpd_state)
+static void perform_mux_hpd_update(int port, mux_state_t hpd_state)
 {
-	if (port >= board_get_usb_pd_port_count()) {
-		return;
-	}
-
 	/* Perform initialization if not initialized yet */
 	if (!(flags[port] & USB_MUX_FLAG_INIT))
 		usb_mux_init(port);
@@ -373,6 +571,19 @@ void usb_mux_hpd_update(int port, mux_state_t hpd_state)
 	configure_mux(port, USB_MUX_HPD_UPDATE, &hpd_state);
 }
 
+void usb_mux_hpd_update(int port, mux_state_t hpd_state)
+{
+	if (port >= board_get_usb_pd_port_count())
+		return;
+
+	/* Send to the mux task if present to maintain sequencing with sets */
+	if (IS_ENABLED(HAS_TASK_USB_MUX))
+		mux_task_enqueue(port, USB_MUX_HPD_UPDATE, hpd_state,
+				 0, 0);
+	else
+		perform_mux_hpd_update(port, hpd_state);
+}
+
 int usb_mux_retimer_fw_update_port_info(void)
 {
 	int i;