asurada: implement display port FCFS policy on aux switching

Asurada supports only one display out at a time. It has to
switch the aux path by first-come-first-serve policy. If a display
is connected to the either port, we should not output to another display
which connects later.

BUG=b:154565980, b:157858237
TEST=see display out on C0 and C1.
BRANCH=none

Change-Id: I6b818572aef6c5fa41e8c9ba3c62cbf0319aee7c
Signed-off-by: Eric Yilun Lin <yllin@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2289473
Reviewed-by: Ting Shen <phoenixshen@chromium.org>

2 files changed, 139 insertions, 1 deletions

diff --git a/board/asurada/gpio.inc b/board/asurada/gpio.inc
index e012bd2a7f..9584538a6a 100644
--- a/board/asurada/gpio.inc
+++ b/board/asurada/gpio.inc
@@ -75,6 +75,7 @@ GPIO(EC_BL_EN_OD,            PIN(B, 5), GPIO_ODR_LOW)
 GPIO(EC_INT_L,               PIN(E, 6), GPIO_ODR_HIGH) /* EC_AP_INT_ODL */
 
 /* USB and USBC Signals */
+GPIO(DP_AUX_PATH_SEL,        PIN(G, 0), GPIO_OUT_HIGH)
 GPIO(EC_DPBRDG_HPD_ODL,      PIN(J, 0), GPIO_ODR_HIGH)
 GPIO(EN_PP5000_USB_A0_VBUS,  PIN(B, 7), GPIO_OUT_LOW)
 
@@ -148,7 +149,6 @@ UNIMPLEMENTED_GPIO(CHARGER_PROCHOT_ODL,    PIN(C, 3))
 UNIMPLEMENTED_GPIO(USB_C0_FRS_EN,          PIN(H, 3))
 UNIMPLEMENTED_GPIO(PG_MT6315_GPU_ODL,      PIN(H, 6))
 UNIMPLEMENTED_GPIO(USB_C0_MUX_INT_L,       PIN(G, 3))
-UNIMPLEMENTED_GPIO(DP_AUX_PATH_SEL,        PIN(G, 0))
 UNIMPLEMENTED_GPIO(EN_PP3000_SD_U,         PIN(G, 1))
 /* reserved for future use */
 UNIMPLEMENTED_GPIO(CCD_MODE_ODL,           PIN(C, 4))
diff --git a/board/asurada/usb_pd_policy.c b/board/asurada/usb_pd_policy.c
index 842b7147d2..8cc8b0a922 100644
--- a/board/asurada/usb_pd_policy.c
+++ b/board/asurada/usb_pd_policy.c
@@ -3,6 +3,10 @@
  * found in the LICENSE file.
  */
 #include "charge_manager.h"
+#include "chipset.h"
+#include "timer.h"
+#include "usb_dp_alt_mode.h"
+#include "usb_mux.h"
 #include "usb_pd.h"
 #include "usbc_ppc.h"
 
@@ -10,6 +14,16 @@
 #error Asurada reference must define CONFIG_USB_PD_MAX_SINGLE_SOURCE_CURRENT
 #endif
 
+#define CPRINTS(format, args...) cprints(CC_USBPD, format, ## args)
+#define CPRINTF(format, args...) cprintf(CC_USBPD, format, ## args)
+
+/* The port that the aux channel is on. */
+static enum {
+	AUX_PORT_NONE = -1,
+	AUX_PORT_C0 = 0,
+	AUX_PORT_C1HDMI = 1,
+} aux_port = AUX_PORT_NONE;
+
 int svdm_get_hpd_gpio(int port)
 {
 	/* HPD is low active, inverse the result */
@@ -25,6 +39,130 @@ void svdm_set_hpd_gpio(int port, int en)
 	gpio_set_level(GPIO_EC_DPBRDG_HPD_ODL, !en);
 }
 
+static void aux_switch_port(int port)
+{
+	if (port != AUX_PORT_NONE)
+		gpio_set_level_verbose(CC_USBPD, GPIO_DP_AUX_PATH_SEL, port);
+	aux_port = port;
+}
+
+static void aux_display_disconnected(int port)
+{
+	/* Gets the other port. C0 -> C1, C1 -> C0. */
+	int other_port = !port;
+
+	/* If the current port is not the aux port, nothing needs to be done. */
+	if (aux_port != port)
+		return;
+
+	/* If the other port is connected to a external display, switch aux. */
+	if (dp_status[other_port] & DP_FLAGS_DP_ON)
+		aux_switch_port(other_port);
+	else
+		aux_switch_port(AUX_PORT_NONE);
+}
+
+__override int svdm_dp_attention(int port, uint32_t *payload)
+{
+	int lvl = PD_VDO_DPSTS_HPD_LVL(payload[1]);
+	int irq = PD_VDO_DPSTS_HPD_IRQ(payload[1]);
+#ifdef CONFIG_USB_PD_DP_HPD_GPIO
+	int cur_lvl = svdm_get_hpd_gpio(port);
+#endif /* CONFIG_USB_PD_DP_HPD_GPIO */
+
+	dp_status[port] = payload[1];
+
+	if (chipset_in_state(CHIPSET_STATE_ANY_SUSPEND) &&
+	    (irq || lvl))
+		/*
+		 * Wake up the AP.  IRQ or level high indicates a DP sink is now
+		 * present.
+		 */
+		if (IS_ENABLED(CONFIG_MKBP_EVENT))
+			pd_notify_dp_alt_mode_entry();
+
+	/* Its initial DP status message prior to config */
+	if (!(dp_flags[port] & DP_FLAGS_DP_ON)) {
+		if (lvl)
+			dp_flags[port] |= DP_FLAGS_HPD_HI_PENDING;
+		return 1;
+	}
+
+#ifdef CONFIG_USB_PD_DP_HPD_GPIO
+	if (irq && !lvl) {
+		/*
+		 * IRQ can only be generated when the level is high, because
+		 * the IRQ is signaled by a short low pulse from the high level.
+		 */
+		CPRINTF("ERR:HPD:IRQ&LOW\n");
+		return 0; /* nak */
+	}
+
+	if (irq && cur_lvl) {
+		uint64_t now = get_time().val;
+		/* wait for the minimum spacing between IRQ_HPD if needed */
+		if (now < svdm_hpd_deadline[port])
+			usleep(svdm_hpd_deadline[port] - now);
+
+		/* generate IRQ_HPD pulse */
+		svdm_set_hpd_gpio(port, 0);
+		usleep(HPD_DSTREAM_DEBOUNCE_IRQ);
+		svdm_set_hpd_gpio(port, 1);
+	} else {
+		svdm_set_hpd_gpio(port, lvl);
+	}
+
+	/*
+	 * Asurada can only output to 1 display port at a time.
+	 * This implements FCFS policy by changing the aux channel. If a
+	 * display is connected to the either port (says A), and the port A
+	 * will be served until the display is disconnected from port A.
+	 * It won't output to the other display which connects to port B.
+	 */
+	if (lvl && aux_port == AUX_PORT_NONE)
+		/*
+		 * A display is connected, and no display was plugged on either
+		 * port.
+		 */
+		aux_switch_port(port);
+	else if (!lvl)
+		aux_display_disconnected(port);
+
+
+	/* set the minimum time delay (2ms) for the next HPD IRQ */
+	svdm_hpd_deadline[port] = get_time().val + HPD_USTREAM_DEBOUNCE_LVL;
+#endif /* CONFIG_USB_PD_DP_HPD_GPIO */
+
+	usb_mux_hpd_update(port, lvl, irq);
+
+#ifdef USB_PD_PORT_TCPC_MST
+	if (port == USB_PD_PORT_TCPC_MST)
+		baseboard_mst_enable_control(port, lvl);
+#endif
+
+	/* ack */
+	return 1;
+}
+
+__override void svdm_exit_dp_mode(int port)
+{
+	svdm_safe_dp_mode(port);
+#ifdef CONFIG_USB_PD_DP_HPD_GPIO
+	svdm_set_hpd_gpio(port, 0);
+#endif /* CONFIG_USB_PD_DP_HPD_GPIO */
+	usb_mux_hpd_update(port, 0, 0);
+
+	aux_display_disconnected(port);
+
+#ifdef USB_PD_PORT_TCPC_MST
+	if (port == USB_PD_PORT_TCPC_MST)
+		baseboard_mst_enable_control(port, 0);
+#endif
+#ifdef CONFIG_USB_PD_TCPMV2
+	dp_teardown(port);
+#endif
+}
+
 int pd_snk_is_vbus_provided(int port)
 {
 	return ppc_is_vbus_present(port);