USB-PD: Continue mode discovery after failure

pd_get_next_mode was searching the svid_mode_data
looking for the next entry to discover, if there
are any. Failed and needs discovery were handled
the same way, return that entry. So if you had a
discovery entry that fails that gets returned and
causes the entry to stop, over and over again

This issue was found using an ikling hub with
attached AC connected to Morphius the SVID
discovery finds two SVIDs, DP and Nintendo.  The
Nintendo fails and this caused the alt-mode to
not enter for the valid DP mode.

The change is to continue discovery even if there
is a failure discovered. If any of the modes
succeeded then it will be considered success if
there are no new modes to discover.

This is the order of returned values:
  no entries -> successful.
  an entry that need to be discovered -> return discovery entry.
  everything discovered properly in the list -> successful.
  discovered some good and some failed -> successful.
  all discovered fail -> return first fail.

BUG=b:168804124
BRANCH=none
TEST=verify morphius monitor when attached ikling powered hub

Signed-off-by: Denis Brockus <dbrockus@google.com>
Change-Id: Id2064079614c276f3d2054c5bcb194cdb997514c
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2416990
Tested-by: Denis Brockus <dbrockus@chromium.org>
Reviewed-by: Denis Brockus <dbrockus@chromium.org>
Commit-Queue: Denis Brockus <dbrockus@chromium.org>
Auto-Submit: Denis Brockus <dbrockus@chromium.org>

1 files changed, 20 insertions, 3 deletions

diff --git a/common/usb_pd_alt_mode_dfp.c b/common/usb_pd_alt_mode_dfp.c
index 737b4e2a76..75d7744597 100644
--- a/common/usb_pd_alt_mode_dfp.c
+++ b/common/usb_pd_alt_mode_dfp.c
@@ -590,17 +590,34 @@ struct svid_mode_data *pd_get_next_mode(int port,
 		enum tcpm_transmit_type type)
 {
 	struct pd_discovery *disc = pd_get_am_discovery(port, type);
+	struct svid_mode_data *failed_mode_data = NULL;
+	bool svid_good_discovery = false;
 	int svid_idx;
 
+	/* Walk through all of the discovery mode entries */
 	for (svid_idx = 0; svid_idx < disc->svid_cnt; ++svid_idx) {
 		struct svid_mode_data *mode_data = &disc->svids[svid_idx];
 
-		if (mode_data->discovery == PD_DISC_COMPLETE)
-			continue;
+		/* Discovery is needed, so send this one back now */
+		if (mode_data->discovery == PD_DISC_NEEDED)
+			return mode_data;
 
-		return mode_data;
+		/* Discovery already succeeded, save that it was seen */
+		if (mode_data->discovery == PD_DISC_COMPLETE)
+			svid_good_discovery = true;
+		/* Discovery already failed, save first failure */
+		else if (!failed_mode_data)
+			failed_mode_data = mode_data;
 	}
 
+	/* If no good entries were located, then return last failed */
+	if (!svid_good_discovery)
+		return failed_mode_data;
+
+	/*
+	 * Mode discovery has been attempted for every discovered SVID (if
+	 * any exist)
+	 */
 	return NULL;
 }