ServoV4pX: Correct DUT port SRC_CAP fixed flags

ServoV4p1 has SRC_CAP readvertisement capability between DUTCHG and DUT
ports.

A longstanding issue is ServoV4pX noncompliantly sets vSafe5v fixed
status flags on all PDO indices, not just the first passthrough object.

This CL corrects that oversight for ServoV4p1 and ServoV4p0.

Note: This is partial excerpt from abandoned major refactor 313b82b.

BUG=b:238142672
BRANCH=main
TEST=1. Connect PD analyzer and Charger to Servo in pdsrc mode
     2. Connect Chromebook
     3. Monitor PD trace for SRC_CAP flags

Change-Id: I32a7370fad0e9f4b13c488f7b4089dde2d544b9f
Signed-off-by: Nathan K <nkolluru@google.com>
Signed-off-by: Łukasz Hajec <lha@semihalf.com>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/3750316
Tested-by: Łukasz Hajec <hajec@google.com>
Commit-Queue: Wai-Hong Tam <waihong@google.com>
Reviewed-by: Wai-Hong Tam <waihong@google.com>

2 files changed, 102 insertions, 8 deletions

diff --git a/board/servo_v4/usb_pd_policy.c b/board/servo_v4/usb_pd_policy.c
index 1dbe586585..4fcded6842 100644
--- a/board/servo_v4/usb_pd_policy.c
+++ b/board/servo_v4/usb_pd_policy.c
@@ -259,6 +259,20 @@ static void update_ports(void)
 			src_index = 0;
 			snk_index = -1;
 
+			/*
+			 * TODO: This code artificially limits PDO
+			 * to entries in pd_src_voltages_mv table
+			 *
+			 * This is artificially overconstrainted.
+			 *
+			 * Allow non-standard PDO objects so long
+			 * as they are valid. See: crrev/c/730877
+			 * for where this started.
+			 *
+			 * This needs to be rearchitected in order
+			 * to support Variable PDO passthrough.
+			 */
+
 			for (i = 0; i < ARRAY_SIZE(pd_src_voltages_mv); ++i) {
 				/* Adhere to board voltage limits */
 				if (pd_src_voltages_mv[i] >
@@ -280,11 +294,40 @@ static void update_ports(void)
 				snk_index = pdo_index;
 				pd_extract_pdo_power(pdo, &max_ma, &max_mv,
 						     &unused);
-				pd_src_chg_pdo[src_index++] =
+				pd_src_chg_pdo[src_index] =
 					PDO_FIXED_VOLT(max_mv) |
-					PDO_FIXED_CURR(max_ma) |
-					DUT_PDO_FIXED_FLAGS |
-					PDO_FIXED_UNCONSTRAINED;
+					PDO_FIXED_CURR(max_ma);
+
+				if (src_index == 0) {
+					/*
+					 * TODO: 1st PDO *should* always be
+					 * vSafe5v PDO. But not always with bad
+					 * DUT. Should re-index and re-map.
+					 *
+					 * TODO: Add variable voltage PDO
+					 * conversion.
+					 */
+					pd_src_chg_pdo[src_index] &=
+						~(DUT_PDO_FIXED_FLAGS |
+						  PDO_FIXED_UNCONSTRAINED);
+
+					/*
+					 * TODO: Keep Unconstrained Power knobs
+					 * exposed and well-defined.
+					 *
+					 * Current method is workaround that
+					 * force-rejects PR_SWAPs in lieu of UP.
+					 *
+					 * Migrate to use config flag such as:
+					 * ((cc_config &
+					 * CC_UNCONSTRAINED_POWER)?
+					 * PDO_FIXED_UNCONSTRAINED:0)
+					 */
+					pd_src_chg_pdo[src_index] |=
+						(DUT_PDO_FIXED_FLAGS |
+						 PDO_FIXED_UNCONSTRAINED);
+				}
+				src_index++;
 			}
 			chg_pdo_cnt = src_index;
 		} else {
@@ -293,6 +336,10 @@ static void update_ports(void)
 					    PDO_FIXED_CURR(vbus[CHG].ma) |
 					    DUT_PDO_FIXED_FLAGS |
 					    PDO_FIXED_UNCONSTRAINED;
+			/*
+			 * TODO: Keep Unconstrained Power knobs
+			 * exposed and well-defined.
+			 */
 
 			chg_pdo_cnt = 1;
 		}
diff --git a/board/servo_v4p1/usb_pd_policy.c b/board/servo_v4p1/usb_pd_policy.c
index 98d6255c29..7f5b6f35bf 100644
--- a/board/servo_v4p1/usb_pd_policy.c
+++ b/board/servo_v4p1/usb_pd_policy.c
@@ -286,6 +286,20 @@ static void update_ports(void)
 			src_index = 0;
 			snk_index = -1;
 
+			/*
+			 * TODO: This code artificially limits PDO
+			 * to entries in pd_src_voltages_mv table
+			 *
+			 * This is artificially overconstrainted.
+			 *
+			 * Allow non-standard PDO objects so long
+			 * as they are valid. See: crrev/c/730877
+			 * for where this started.
+			 *
+			 * This needs to be rearchitected in order
+			 * to support Variable PDO passthrough.
+			 */
+
 			for (i = 0; i < ARRAY_SIZE(pd_src_voltages_mv); ++i) {
 				/* Adhere to board voltage limits */
 				if (pd_src_voltages_mv[i] >
@@ -307,11 +321,40 @@ static void update_ports(void)
 				snk_index = pdo_index;
 				pd_extract_pdo_power(pdo, &max_ma, &max_mv,
 						     &unused);
-				pd_src_chg_pdo[src_index++] =
+				pd_src_chg_pdo[src_index] =
 					PDO_FIXED_VOLT(max_mv) |
-					PDO_FIXED_CURR(max_ma) |
-					DUT_PDO_FIXED_FLAGS |
-					PDO_FIXED_UNCONSTRAINED;
+					PDO_FIXED_CURR(max_ma);
+
+				if (src_index == 0) {
+					/*
+					 * TODO: 1st PDO *should* always be
+					 * vSafe5v PDO. But not always with bad
+					 * DUT. Should re-index and re-map.
+					 *
+					 * TODO: Add variable voltage PDO
+					 * conversion.
+					 */
+					pd_src_chg_pdo[src_index] &=
+						~(DUT_PDO_FIXED_FLAGS |
+						  PDO_FIXED_UNCONSTRAINED);
+
+					/*
+					 * TODO: Keep Unconstrained Power knobs
+					 * exposed and well-defined.
+					 *
+					 * Current method is workaround that
+					 * force-rejects PR_SWAPs in lieu of UP.
+					 *
+					 * Migrate to use config flag such as:
+					 * ((cc_config &
+					 * CC_UNCONSTRAINED_POWER)?
+					 * PDO_FIXED_UNCONSTRAINED:0)
+					 */
+					pd_src_chg_pdo[src_index] |=
+						(DUT_PDO_FIXED_FLAGS |
+						 PDO_FIXED_UNCONSTRAINED);
+				}
+				src_index++;
 			}
 			chg_pdo_cnt = src_index;
 		} else {
@@ -320,6 +363,10 @@ static void update_ports(void)
 					    PDO_FIXED_CURR(vbus[CHG].ma) |
 					    DUT_PDO_FIXED_FLAGS |
 					    PDO_FIXED_UNCONSTRAINED;
+			/*
+			 * TODO: Keep Unconstrained Power knobs
+			 * exposed and well-defined.
+			 */
 
 			chg_pdo_cnt = 1;
 		}