pd: vdm: Handle VDM requests only through state machine.

Previously, handle_vdm_requests could dispatch another VDM message
via send_validate_message prior to main task returning to vdm state machine
(pd_vdm_send_state_machine).  While it hasn't been problematic to-date it would
make honoring VDM specific timers or PDO priority difficult.

CL changes behavior so that if VDM being handled requires another VDM to be sent
its copied to the one entry queue (queue_vdm) where it will be
serviced upon VDM state machine entry later.

With this simplification, CL expands interlocks between PDO & VDO.
VDOs are only sent when source/sink is in the ready state & no
incoming packet is on the CC line.  PDOs aren't sent when the VDM
state machine is busy.

CL also simplifies VDM console output to come only from request handler which
could save a few bytes.

Signed-off-by: Todd Broch <tbroch@chromium.org>

BRANCH=samus
BUG=chrome-os-partner:30645
TEST=manual,
1. dingdong/hoho still enter mode.
2. Can still update fw.

Change-Id: I2fe8643a6975205b2d0f510f4f1baf2d74c1e190
Reviewed-on: https://chromium-review.googlesource.com/235680
Reviewed-by: Alec Berg <alecaberg@chromium.org>
Commit-Queue: Todd Broch <tbroch@chromium.org>
Tested-by: Todd Broch <tbroch@chromium.org>

6 files changed, 63 insertions, 47 deletions

diff --git a/board/dingdong/usb_pd_policy.c b/board/dingdong/usb_pd_policy.c
index 29c90282e9..cf151a700b 100644
--- a/board/dingdong/usb_pd_policy.c
+++ b/board/dingdong/usb_pd_policy.c
@@ -233,21 +233,17 @@ const struct svdm_response svdm_rsp = {
 static int pd_custom_vdm(int port, int cnt, uint32_t *payload,
 			 uint32_t **rpayload)
 {
-	int cmd = PD_VDO_CMD(payload[0]);
 	int rsize;
 
 	if (PD_VDO_VID(payload[0]) != USB_VID_GOOGLE || !gfu_mode)
 		return 0;
 
-	CPRINTF("VDM/%d [%d] %08x\n", cnt, cmd, payload[0]);
-
 	*rpayload = payload;
 
 	rsize = pd_custom_flash_vdm(port, cnt, payload);
 	if (!rsize)
 		return 0;
 
-	CPRINTS("DONE");
 	/* respond (positively) to the request */
 	payload[0] |= VDO_SRC_RESPONDER;
 
diff --git a/board/fruitpie/usb_pd_policy.c b/board/fruitpie/usb_pd_policy.c
index 9a70c1b70a..e9c3b25ac4 100644
--- a/board/fruitpie/usb_pd_policy.c
+++ b/board/fruitpie/usb_pd_policy.c
@@ -127,7 +127,6 @@ static int pd_custom_vdm(int port, int cnt, uint32_t *payload,
 {
 	int cmd = PD_VDO_CMD(payload[0]);
 	uint16_t dev_id = 0;
-	CPRINTF("VDM/%d [%d] %08x\n", cnt, cmd, payload[0]);
 
 	/* make sure we have some payload */
 	if (cnt == 0)
diff --git a/board/hoho/usb_pd_policy.c b/board/hoho/usb_pd_policy.c
index 195aaacb25..204fb0d70f 100644
--- a/board/hoho/usb_pd_policy.c
+++ b/board/hoho/usb_pd_policy.c
@@ -232,21 +232,17 @@ const struct svdm_response svdm_rsp = {
 static int pd_custom_vdm(int port, int cnt, uint32_t *payload,
 			 uint32_t **rpayload)
 {
-	int cmd = PD_VDO_CMD(payload[0]);
 	int rsize;
 
 	if (PD_VDO_VID(payload[0]) != USB_VID_GOOGLE || !gfu_mode)
 		return 0;
 
-	CPRINTF("VDM/%d [%d] %08x\n", cnt, cmd, payload[0]);
-
 	*rpayload = payload;
 
 	rsize = pd_custom_flash_vdm(port, cnt, payload);
 	if (!rsize)
 		return 0;
 
-	CPRINTS("DONE");
 	/* respond (positively) to the request */
 	payload[0] |= VDO_SRC_RESPONDER;
 
diff --git a/board/ryu_p1/usb_pd_policy.c b/board/ryu_p1/usb_pd_policy.c
index fba4db9a4c..e292f84ba8 100644
--- a/board/ryu_p1/usb_pd_policy.c
+++ b/board/ryu_p1/usb_pd_policy.c
@@ -121,7 +121,6 @@ static int pd_custom_vdm(int port, int cnt, uint32_t *payload,
 {
 	int cmd = PD_VDO_CMD(payload[0]);
 	uint16_t dev_id = 0;
-	CPRINTF("VDM/%d [%d] %08x\n", cnt, cmd, payload[0]);
 
 	/* make sure we have some payload */
 	if (cnt == 0)
diff --git a/common/usb_pd_policy.c b/common/usb_pd_policy.c
index bfaccc8e29..d9f66e440b 100644
--- a/common/usb_pd_policy.c
+++ b/common/usb_pd_policy.c
@@ -239,13 +239,13 @@ static void dfp_consume_svids(int port, uint32_t *payload)
 static int dfp_discover_modes(int port, uint32_t *payload)
 {
 	uint16_t svid = pe[port].svids[pe[port].svid_idx].svid;
-	if (!pe[port].svid_cnt)
+	if (pe[port].svid_idx >= pe[port].svid_cnt)
 		return 0;
 	payload[0] = VDO(svid, 1, CMD_DISCOVER_MODES);
 	return 1;
 }
 
-static int dfp_consume_modes(int port, int cnt, uint32_t *payload)
+static void dfp_consume_modes(int port, int cnt, uint32_t *payload)
 {
 	int idx = pe[port].svid_idx;
 	pe[port].svids[idx].mode_cnt = cnt - 1;
@@ -257,7 +257,6 @@ static int dfp_consume_modes(int port, int cnt, uint32_t *payload)
 	}
 
 	pe[port].svid_idx++;
-	return (pe[port].svid_idx < pe[port].svid_cnt);
 }
 
 int pd_alt_mode(int port)
@@ -404,16 +403,11 @@ DECLARE_CONSOLE_COMMAND(pe, command_pe,
 
 int pd_svdm(int port, int cnt, uint32_t *payload, uint32_t **rpayload)
 {
-	int i;
 	int cmd = PD_VDO_CMD(payload[0]);
 	int cmd_type = PD_VDO_CMDT(payload[0]);
 	int (*func)(int port, uint32_t *payload) = NULL;
 
 	int rsize = 1; /* VDM header at a minimum */
-	CPRINTF("SVDM/%d [%d] %08x", cnt, cmd, payload[0]);
-	for (i = 1; i < cnt; i++)
-		CPRINTF(" %08x", payload[i]);
-	CPRINTF("\n");
 
 	payload[0] &= ~VDO_CMDT_MASK;
 	*rpayload = payload;
@@ -479,9 +473,9 @@ int pd_svdm(int port, int cnt, uint32_t *payload, uint32_t **rpayload)
 			rsize = dfp_discover_modes(port, payload);
 			break;
 		case CMD_DISCOVER_MODES:
-			if (dfp_consume_modes(port, cnt, payload))
-				rsize = dfp_discover_modes(port, payload);
-			else
+			dfp_consume_modes(port, cnt, payload);
+			rsize = dfp_discover_modes(port, payload);
+			if (!rsize)
 				rsize = dfp_enter_mode(port, payload);
 			break;
 		case CMD_ENTER_MODE:
@@ -551,7 +545,6 @@ int pd_svdm(int port, int cnt, uint32_t *payload, uint32_t **rpayload)
 	} else {
 		CPRINTF("PE ERR: unknown cmd type %d\n", cmd);
 	}
-	CPRINTS("DONE");
 	return rsize;
 }
 
diff --git a/common/usb_pd_protocol.c b/common/usb_pd_protocol.c
index 873c3473d0..515ec40d47 100644
--- a/common/usb_pd_protocol.c
+++ b/common/usb_pd_protocol.c
@@ -690,22 +690,34 @@ static void bist_mode_2_rx(int port)
 	}
 }
 
+static void queue_vdm(int port, uint32_t *header, const uint32_t *data,
+			     int data_cnt)
+{
+	pd[port].vdo_count = data_cnt + 1;
+	pd[port].vdo_data[0] = header[0];
+	memcpy(&pd[port].vdo_data[1], data, sizeof(uint32_t) * data_cnt);
+	/* Set ready, pd task will actually send */
+	pd[port].vdm_state = VDM_STATE_READY;
+}
+
 static void handle_vdm_request(int port, int cnt, uint32_t *payload)
 {
-	int rlen = 0;
+	int rlen = 0, i;
 	uint32_t *rdata;
 
-	if (pd[port].vdm_state == VDM_STATE_BUSY)
+	if (pd[port].vdm_state == VDM_STATE_BUSY) {
 		pd[port].vdm_state = VDM_STATE_DONE;
+		CPRINTF("VDM/%d [%02d] %08x", cnt, PD_VDO_CMD(payload[0]),
+			payload[0]);
+		if (PD_VDO_SVDM(payload[0]))
+			for (i = 1; i < cnt; i++)
+				CPRINTF(" %08x", payload[i]);
+		CPRINTF("\n");
+	}
 
 	rlen = pd_vdm(port, cnt, payload, &rdata);
 	if (rlen > 0) {
-		uint16_t header = PD_HEADER(PD_DATA_VENDOR_DEF,
-					    pd[port].power_role,
-					    pd[port].data_role,
-					    pd[port].msg_id,
-					    rlen);
-		send_validate_message(port, header, rlen, rdata);
+		queue_vdm(port, rdata, &rdata[1], rlen - 1);
 		return;
 	}
 	if (debug_level >= 1)
@@ -1291,26 +1303,34 @@ packet_err:
 void pd_send_vdm(int port, uint32_t vid, int cmd, const uint32_t *data,
 		 int count)
 {
-	int i;
-
 	if (count > VDO_MAX_SIZE - 1) {
 		CPRINTF("VDM over max size\n");
 		return;
 	}
 
+	/* set VDM header with VID & CMD */
 	pd[port].vdo_data[0] = VDO(vid, ((vid & USB_SID_PD) == USB_SID_PD) ?
-				   1 : 0, cmd);
+				   1 : (PD_VDO_CMD(cmd) < CMD_ATTENTION), cmd);
+	queue_vdm(port, pd[port].vdo_data, data, count);
 
-	pd[port].vdo_count = count + 1;
-	for (i = 1; i < count + 1; i++)
-		pd[port].vdo_data[i] = data[i-1];
-
-	/* Set ready, pd task will actually send */
-	pd[port].vdm_state = VDM_STATE_READY;
 	task_wake(PORT_TO_TASK_ID(port));
 }
 
-static void pd_vdm_send_state_machine(int port)
+static inline int pdo_busy(int port)
+{
+	/*
+	 * Note, main PDO state machine (pd_task) uses READY state exclusively
+	 * to denote port partners have successfully negociated a contract.  All
+	 * other protocol actions force state transitions.
+	 */
+	int rv = (pd[port].task_state != PD_STATE_SRC_READY);
+#ifdef CONFIG_USB_PD_DUAL_ROLE
+	rv &= (pd[port].task_state != PD_STATE_SNK_READY);
+#endif
+	return rv;
+}
+
+static void pd_vdm_send_state_machine(int port, int incoming_packet)
 {
 	int res;
 	uint16_t header;
@@ -1318,7 +1338,16 @@ static void pd_vdm_send_state_machine(int port)
 
 	switch (pd[port].vdm_state) {
 	case VDM_STATE_READY:
-		/* Only transmit VDM if connected */
+		/*
+		 * if there's traffic or we're not in PDO ready state don't send
+		 * a VDM */
+		if (incoming_packet || pdo_busy(port))
+			break;
+
+		/*
+		 * Only transmit VDM if connected.  Should follow busy logic
+		 * (above) as custom VDMs can leave port in disconnected state
+		 */
 		if (!pd_is_connected(port)) {
 			pd[port].vdm_state = VDM_STATE_ERR_BUSY;
 			break;
@@ -1509,7 +1538,7 @@ void pd_task(void)
 	uint32_t payload[7];
 	int timeout = 10*MSEC;
 	int cc1_volt, cc2_volt;
-	int res, incoming_packet;
+	int res, incoming_packet = 0;
 #ifdef CONFIG_USB_PD_DUAL_ROLE
 	uint64_t next_role_swap = PD_T_DRP_SNK;
 	int hard_reset_count = 0;
@@ -1543,7 +1572,7 @@ void pd_task(void)
 
 	while (1) {
 		/* process VDM messages last */
-		pd_vdm_send_state_machine(port);
+		pd_vdm_send_state_machine(port, incoming_packet);
 
 		/* monitor for incoming packet if in a connected state */
 		if (pd_is_connected(port) && pd_comm_enabled)
@@ -1765,9 +1794,11 @@ void pd_task(void)
 
 			/*
 			 * Don't send any PD traffic if we woke up due to
-			 * incoming packet to avoid collisions
+			 * incoming packet or if VDO response pending to avoid
+			 * collisions.
 			 */
-			if (incoming_packet)
+			if (incoming_packet ||
+			    (pd[port].vdm_state == VDM_STATE_BUSY))
 				break;
 
 			/* Send get sink cap if haven't received it yet */
@@ -2081,9 +2112,11 @@ void pd_task(void)
 
 			/*
 			 * Don't send any PD traffic if we woke up due to
-			 * incoming packet to avoid collisions
+			 * incoming packet or if VDO response pending to avoid
+			 * collisions.
 			 */
-			if (incoming_packet)
+			if (incoming_packet ||
+			    (pd[port].vdm_state == VDM_STATE_BUSY))
 				break;
 
 			/* Check for new power to request */