USB MUX: Create a task to perform mux sets

Some platforms may have very extended times to complete mux sets (in the
100s of ms range), so isolate these mux sets to a dedicated task, which
the PD task may query to know when they have completed.

Also enqueue HPD sets, since they will need to be in correct sequence
with some mux sets such as setting the DP pins.

BRANCH=None
BUG=b:186777984,b:172222942
TEST=make -j buildall

Signed-off-by: Diana Z <dzigterman@chromium.org>
Change-Id: I45ec7bd9a0c42112ec5b59f2f23988a8b810b57c
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/3078415
Reviewed-by: Abe Levkoy <alevkoy@chromium.org>

2 files changed, 236 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;
diff --git a/include/usb_mux.h b/include/usb_mux.h
index 9909f1c1c5..41e5881a81 100644
--- a/include/usb_mux.h
+++ b/include/usb_mux.h
@@ -262,4 +262,11 @@ void usb_mux_hpd_update(int port, mux_state_t mux_state);
  */
 int usb_mux_retimer_fw_update_port_info(void);
 
+/**
+ * Check whether this port has pending mux sets
+ *
+ * @param  port USB-C port number
+ * @return True if all pending mux sets have completed
+ */
+bool usb_mux_set_completed(int port);
 #endif