guybrush: Refactor FP6 mux driver to retry in S0

FP6 USB mux is not ready until sometime after S0.
Refactor driver to keep trying every 1 second until mux is ready.

BUG=b:184680878, b:184966860
TEST=C0 and C1 display connects on B1 and B2
BRANCH=None

Signed-off-by: Rob Barnes <robbarnes@google.com>
Change-Id: If744182879461d8452426deaf0e74a84dacfd510
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2819023
Reviewed-by: Diana Z <dzigterman@chromium.org>

1 files changed, 80 insertions, 74 deletions

diff --git a/driver/usb_mux/amd_fp6.c b/driver/usb_mux/amd_fp6.c
index 462d7c9297..7f1e57cc78 100644
--- a/driver/usb_mux/amd_fp6.c
+++ b/driver/usb_mux/amd_fp6.c
@@ -14,33 +14,35 @@
 #include "timer.h"
 #include "usb_mux.h"
 
+#define CPRINTSUSB(format, args...) cprints(CC_USBCHARGE, format, ## args)
+#define CPRINTFUSB(format, args...) cprintf(CC_USBCHARGE, format, ## args)
+
 /*
- * This may be shorter than the internal MUX timeout.
- * Making it any longer could cause the PD task to miss messages.
+ * The recommendation from "3.3.2 Command Timeout" is 250ms,
+ * however empirical testing found that a 100ms timeout is sufficient.
  */
 #define WRITE_CMD_TIMEOUT_MS 100
 
+/* Command retry interval */
+#define CMD_RETRY_INTERVAL_MS 1000
+
 /*
  * Local data structure for saving mux state so it can be restored after
  * an AP reset.
  */
 static struct {
 	const struct usb_mux *mux;
-	mux_state_t state;
+	uint8_t val;
+	bool write_pending;
 } saved_mux_state[USBC_PORT_COUNT];
 
 static int amd_fp6_mux_port0_read(const struct usb_mux *me, uint8_t *val)
 {
 	uint8_t payload[3] = { 0 };
-	int rv;
 	bool mux_ready;
 
-	if (chipset_in_state(CHIPSET_STATE_HARD_OFF))
-		return EC_ERROR_NOT_POWERED;
-
-	rv = i2c_xfer(me->i2c_port, me->i2c_addr_flags, NULL, 0, payload, 3);
-	if (rv)
-		return rv;
+	RETURN_ERROR(i2c_xfer(me->i2c_port, me->i2c_addr_flags, NULL, 0,
+			      payload, 3));
 
 	/*
 	 * payload[0]: Status/ID
@@ -49,6 +51,7 @@ static int amd_fp6_mux_port0_read(const struct usb_mux *me, uint8_t *val)
 	 */
 	mux_ready = !!((payload[0] >> AMD_FP6_MUX_PD_STATUS_OFFSET)
 						& AMD_FP6_MUX_PD_STATUS_READY);
+
 	if (!mux_ready)
 		return EC_ERROR_BUSY;
 	*val = payload[1];
@@ -58,20 +61,16 @@ static int amd_fp6_mux_port0_read(const struct usb_mux *me, uint8_t *val)
 
 static int amd_fp6_mux_port0_write(const struct usb_mux *me, uint8_t write_val)
 {
-	int rv;
 	uint8_t read_val;
 	uint8_t port_status;
 	timestamp_t start;
 
 	/* Check if mux is ready */
-	rv = amd_fp6_mux_port0_read(me, &read_val);
-	if (rv)
-		return rv;
+	RETURN_ERROR(amd_fp6_mux_port0_read(me, &read_val));
 
 	/* Write control register */
-	rv = i2c_write8(me->i2c_port, me->i2c_addr_flags, 0, write_val);
-	if (rv)
-		return rv;
+	RETURN_ERROR(
+		i2c_write8(me->i2c_port, me->i2c_addr_flags, 0, write_val));
 
 	/*
 	 * Read status until write command finishes or times out.
@@ -80,10 +79,8 @@ static int amd_fp6_mux_port0_write(const struct usb_mux *me, uint8_t write_val)
 	 */
 	start = get_time();
 	while (time_since32(start) < WRITE_CMD_TIMEOUT_MS * MSEC) {
-		rv = amd_fp6_mux_port0_read(me, &read_val);
-		if (rv)
-			return rv;
 
+		RETURN_ERROR(amd_fp6_mux_port0_read(me, &read_val));
 		port_status = read_val >> AMD_FP6_MUX_PORT_STATUS_OFFSET;
 
 		if (port_status == AMD_FP6_MUX_PORT_CMD_COMPLETE)
@@ -91,7 +88,7 @@ static int amd_fp6_mux_port0_write(const struct usb_mux *me, uint8_t write_val)
 		else if (port_status == AMD_FP6_MUX_PORT_CMD_TIMEOUT)
 			return EC_ERROR_TIMEOUT;
 		else if (port_status == AMD_FP6_MUX_PORT_CMD_BUSY)
-			msleep(WRITE_CMD_TIMEOUT_MS / 5);
+			msleep(5);
 		else
 			return EC_ERROR_UNKNOWN;
 	}
@@ -99,18 +96,47 @@ static int amd_fp6_mux_port0_write(const struct usb_mux *me, uint8_t write_val)
 	return EC_ERROR_TIMEOUT;
 }
 
-static int amd_fp6_init(const struct usb_mux *me)
+/*
+ * Keep trying to write the saved mux state until successful or SOC leaves
+ * S0 power state.
+ */
+static void amd_fp6_set_mux_retry(void);
+DECLARE_DEFERRED(amd_fp6_set_mux_retry);
+static void amd_fp6_set_mux_retry(void)
 {
-	return EC_SUCCESS;
+	int rv;
+	bool try_again = false;
+
+	/*
+	 * Mux can only be written in S0, stop trying.
+	 * Will try again on chipset_resume.
+	 */
+	if (!chipset_in_state(CHIPSET_STATE_ON))
+		return;
+
+	for (int i = 0; i < ARRAY_SIZE(saved_mux_state); i++) {
+		/* Make sure mux_state is initialized */
+		if (saved_mux_state[i].mux == NULL ||
+		    !saved_mux_state[i].write_pending)
+			continue;
+
+		rv = amd_fp6_mux_port0_write(saved_mux_state[i].mux,
+					     saved_mux_state[i].val);
+
+		if (rv)
+			try_again = true;
+		else
+			saved_mux_state[i].write_pending = false;
+	}
+	if (try_again)
+		hook_call_deferred(&amd_fp6_set_mux_retry_data,
+				   CMD_RETRY_INTERVAL_MS * MSEC);
 }
 
+
 static int amd_fp6_set_mux(const struct usb_mux *me, mux_state_t mux_state)
 {
 	uint8_t val;
-	int rv;
-
-	saved_mux_state[me->usb_port].mux = me;
-	saved_mux_state[me->usb_port].state = mux_state;
 
 	if (mux_state == USB_PD_MUX_NONE)
 		/*
@@ -126,23 +152,26 @@ static int amd_fp6_set_mux(const struct usb_mux *me, mux_state_t mux_state)
 	else if (mux_state & USB_PD_MUX_DP_ENABLED)
 		val = AMD_FP6_MUX_MODE_DP;
 	else {
-		ccprintf("Unhandled mux_state %x\n", mux_state);
+		CPRINTSUSB("C%d: unhandled mux_state %x\n", me->usb_port,
+			   mux_state);
 		return EC_ERROR_INVAL;
 	}
 
 	if (mux_state & USB_PD_MUX_POLARITY_INVERTED)
 		val |= AMD_FP6_MUX_ORIENTATION;
 
-	rv = amd_fp6_mux_port0_write(me, val);
-	/*
-	 * This MUX is on the FP6 SoC.  If that device is not powered then
-	 * we either have to complain that it is not powered or if we were
-	 * setting the state to OFF, then go ahead and report that we did
-	 * it because a powered down MUX is off.
-	 */
-	if (rv == EC_ERROR_NOT_POWERED && mux_state == USB_PD_MUX_NONE)
-		rv = EC_SUCCESS;
-	return rv;
+	saved_mux_state[me->usb_port].mux = me;
+	saved_mux_state[me->usb_port].val = val;
+
+	/* Mux is not powered in Z1 */
+	if (chipset_in_state(CHIPSET_STATE_HARD_OFF))
+		return (mux_state == USB_PD_MUX_NONE) ? EC_SUCCESS
+						      : EC_ERROR_NOT_POWERED;
+
+	saved_mux_state[me->usb_port].write_pending = true;
+	amd_fp6_set_mux_retry();
+
+	return EC_SUCCESS;
 }
 
 static int amd_fp6_get_mux(const struct usb_mux *me, mux_state_t *mux_state)
@@ -150,17 +179,12 @@ static int amd_fp6_get_mux(const struct usb_mux *me, mux_state_t *mux_state)
 	uint8_t val;
 	bool inverted;
 	uint8_t mode;
-	int rv;
 
-	rv = amd_fp6_mux_port0_read(me, &val);
-	/*
-	 * This MUX is on the FP6 SoC. If that device is not powered then claim
-	 * thestate to be NONE, which is SAFE.
-	 */
-	if (rv == EC_ERROR_NOT_POWERED)
-		val = 0;
-	else if (rv)
-		return rv;
+	/* Mux is not powered in Z1 */
+	if (chipset_in_state(CHIPSET_STATE_HARD_OFF))
+		return USB_PD_MUX_NONE;
+
+	RETURN_ERROR(amd_fp6_mux_port0_read(me, &val));
 
 	mode = (val & AMD_FP6_MUX_MODE_MASK);
 	inverted = !!(val & AMD_FP6_MUX_ORIENTATION);
@@ -180,38 +204,20 @@ static int amd_fp6_get_mux(const struct usb_mux *me, mux_state_t *mux_state)
 	return EC_SUCCESS;
 }
 
-static void amd_fp6_chipset_reset_delay(void)
-{
-	int rv;
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(saved_mux_state); i++) {
-		/* Check if saved_mux_state has been initialized */
-		if (saved_mux_state[i].mux == NULL)
-			continue;
-		rv = amd_fp6_set_mux(saved_mux_state[i].mux,
-				     saved_mux_state[i].state);
-		if (rv)
-			ccprints("C%d restore mux rv:%d", i, rv);
-	}
-
-}
-DECLARE_DEFERRED(amd_fp6_chipset_reset_delay);
-
 /*
- * The AP's internal USB-C mux is reset when AP resets, so wait for
- * it to be ready and then restore the previous setting.
+ * The FP6 USB Mux will not be ready for writing until *sometime* after S0.
  */
-static int amd_fp6_chipset_reset(const struct usb_mux *mux)
+static void amd_fp6_chipset_resume(void)
 {
-	/* TODO: Tune 200ms delay for FP6 */
-	hook_call_deferred(&amd_fp6_chipset_reset_delay_data, 200 * MSEC);
-	return EC_SUCCESS;
+	for (int i = 0; i < ARRAY_SIZE(saved_mux_state); i++)
+		saved_mux_state[i].write_pending = true;
+	hook_call_deferred(&amd_fp6_set_mux_retry_data,
+			   CMD_RETRY_INTERVAL_MS * MSEC);
 }
+DECLARE_HOOK(HOOK_CHIPSET_RESUME, amd_fp6_chipset_resume, HOOK_PRIO_DEFAULT);
 
 const struct usb_mux_driver amd_fp6_usb_mux_driver = {
-	.init = &amd_fp6_init,
 	.set = &amd_fp6_set_mux,
 	.get = &amd_fp6_get_mux,
-	.chipset_reset = &amd_fp6_chipset_reset,
+	/* .chipset_reset is handled by amd_fp6_chipset_resume hook */
 };