TCPMv2: Separate discovery and mode init

Don't clear discovery data when resetting active modes during mode exit.

BUG=b:141363146
TEST=make buildall
BRANCH=firmware-volteer-13672.B-main

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

5 files changed, 34 insertions, 10 deletions

diff --git a/common/usb_pd_alt_mode_dfp.c b/common/usb_pd_alt_mode_dfp.c
index 71e3c051a5..d04c9e18b2 100644
--- a/common/usb_pd_alt_mode_dfp.c
+++ b/common/usb_pd_alt_mode_dfp.c
@@ -298,7 +298,7 @@ int pd_dfp_exit_mode(int port, enum tcpci_msg_type type, uint16_t svid,
 			if (active->amodes[idx].fx)
 				active->amodes[idx].fx->exit(port);
 
-		pd_dfp_discovery_init(port);
+		pd_dfp_mode_init(port);
 		return 0;
 	}
 
diff --git a/common/usb_pd_policy.c b/common/usb_pd_policy.c
index 959feb4728..08827abe21 100644
--- a/common/usb_pd_policy.c
+++ b/common/usb_pd_policy.c
@@ -354,6 +354,10 @@ static bool is_cable_ready_to_enter_usb4(int port, int cnt)
 void pd_dfp_discovery_init(int port)
 {
 	memset(&discovery[port], 0, sizeof(struct pd_discovery));
+}
+
+void pd_dfp_mode_init(int port)
+{
 	memset(&partner_amodes[port], 0, sizeof(partner_amodes[0]));
 }
 
@@ -428,6 +432,7 @@ static int process_am_discover_ident_sop(int port, int cnt, uint32_t head,
 					 enum tcpci_msg_type *rtype)
 {
 	pd_dfp_discovery_init(port);
+	pd_dfp_mode_init(port);
 	dfp_consume_identity(port, TCPCI_MSG_SOP, cnt, payload);
 
 	if (IS_ENABLED(CONFIG_USB_PD_DECODE_SOP) && is_sop_prime_ready(port) &&
diff --git a/common/usb_pd_protocol.c b/common/usb_pd_protocol.c
index 378960c91e..07b50363b9 100644
--- a/common/usb_pd_protocol.c
+++ b/common/usb_pd_protocol.c
@@ -3042,6 +3042,7 @@ void pd_task(void *u)
 #ifdef CONFIG_USB_PD_ALT_MODE_DFP
 	/* Initialize PD Policy engine */
 	pd_dfp_discovery_init(port);
+	pd_dfp_mode_init(port);
 #endif
 
 #ifdef CONFIG_CHARGE_MANAGER
diff --git a/common/usbc/usb_pe_drp_sm.c b/common/usbc/usb_pe_drp_sm.c
index bf134c5449..59d45b0e28 100644
--- a/common/usbc/usb_pe_drp_sm.c
+++ b/common/usbc/usb_pe_drp_sm.c
@@ -1479,8 +1479,7 @@ static bool common_src_snk_dpm_requests(int port)
 		/* Currently only support sending soft reset to SOP */
 		pe_send_soft_reset(port, TCPCI_MSG_SOP);
 		return true;
-	} else if (PE_CHK_DPM_REQUEST(port,
-					DPM_REQUEST_PORT_DISCOVERY)) {
+	} else if (PE_CHK_DPM_REQUEST(port, DPM_REQUEST_PORT_DISCOVERY)) {
 		pe_set_dpm_curr_request(port, DPM_REQUEST_PORT_DISCOVERY);
 		if (!PE_CHK_FLAG(port, PE_FLAGS_MODAL_OPERATION)) {
 			/*
@@ -1489,6 +1488,11 @@ static bool common_src_snk_dpm_requests(int port)
 			 * requests.
 			 */
 			pd_dfp_discovery_init(port);
+			/*
+			 * TODO(b/189353401): Do not reinitialize modes when no
+			 * longer required.
+			 */
+			pd_dfp_mode_init(port);
 			pe[port].dr_swap_attempt_counter = 0;
 			pe[port].discover_identity_counter = 0;
 			pd_timer_enable(port, PE_TIMER_DISCOVER_IDENTITY,
@@ -2157,8 +2161,9 @@ static void pe_src_startup_entry(int port)
 		 */
 		pd_timer_enable(port, PE_TIMER_DISCOVER_IDENTITY, 0);
 
-		/* Clear port discovery flags */
+		/* Clear port discovery/mode flags */
 		pd_dfp_discovery_init(port);
+		pd_dfp_mode_init(port);
 		pe[port].ama_vdo = PD_VDO_INVALID;
 		pe[port].vpd_vdo = PD_VDO_INVALID;
 		pe[port].discover_identity_counter = 0;
@@ -3010,8 +3015,9 @@ static void pe_snk_startup_entry(int port)
 		 */
 		pd_timer_enable(port, PE_TIMER_DISCOVER_IDENTITY, 0);
 
-		/* Clear port discovery flags */
+		/* Clear port discovery/mode flags */
 		pd_dfp_discovery_init(port);
+		pd_dfp_mode_init(port);
 		pe[port].discover_identity_counter = 0;
 
 		/* Reset dr swap attempt counter */
@@ -6993,6 +6999,16 @@ uint32_t task_access[CONFIG_USB_PD_PORT_MAX_COUNT][DISCOVERY_TYPE_COUNT];
 
 void pd_dfp_discovery_init(int port)
 {
+	atomic_or(&task_access[port][TCPCI_MSG_SOP], BIT(task_get_current()));
+	atomic_or(&task_access[port][TCPCI_MSG_SOP_PRIME],
+		  BIT(task_get_current()));
+
+	memset(pe[port].discovery, 0, sizeof(pe[port].discovery));
+
+}
+
+void pd_dfp_mode_init(int port)
+{
 	/*
 	 * Clear the VDM Setup Done and Modal Operation flags so we will
 	 * have a fresh discovery
@@ -7000,11 +7016,6 @@ void pd_dfp_discovery_init(int port)
 	PE_CLR_FLAG(port, PE_FLAGS_VDM_SETUP_DONE |
 			  PE_FLAGS_MODAL_OPERATION);
 
-	atomic_or(&task_access[port][TCPCI_MSG_SOP], BIT(task_get_current()));
-	atomic_or(&task_access[port][TCPCI_MSG_SOP_PRIME],
-		  BIT(task_get_current()));
-
-	memset(pe[port].discovery, 0, sizeof(pe[port].discovery));
 	memset(pe[port].partner_amodes, 0, sizeof(pe[port].partner_amodes));
 
 	/* Reset the DPM and DP modules to enable alternate mode entry. */
diff --git a/include/usb_pd.h b/include/usb_pd.h
index 9670b2d8aa..e57f4a4d2a 100644
--- a/include/usb_pd.h
+++ b/include/usb_pd.h
@@ -1886,6 +1886,13 @@ uint8_t get_vpd_ct_hw_version(int port);
 void pd_dfp_discovery_init(int port);
 
 /**
+ * Initialize active mode info (alternate mode or USB mode) for DFP
+ *
+ * @param port USB-C port number
+ */
+void pd_dfp_mode_init(int port);
+
+/**
  * Set identity discovery state for this type and port
  *
  * @param port  USB-C port number