Add USB-PD mode commands.

These commands (pdgetmode & pdsetmode) will provide host with ability
to identify USB-PD alternate mode devices SVIDs and supported modes.
It will also allow host to set mode on devices which support multiple
alternate modes.

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

BRANCH=samus
BUG=chrome-os-partner:33946
TEST=manual

Plug hoho/dingdong into samus at port <port>

  ectool --name cros_pd pdgetmode <port>
  *SVID:0xff01 *0x00001085  0x00000000  0x00000000  0x00000000  0x00000000  0x00000000
   SVID:0x18d1  0x00000001  0x00000000  0x00000000  0x00000000  0x00000000  0x00000000

  ectool --name cros_pd pdsetmode <port> 0x18d1 1
  ectool --name cros_pd pdgetmode <port>
   SVID:0xff01  0x00001085  0x00000000  0x00000000  0x00000000  0x00000000  0x00000000
  *SVID:0x18d1 *0x00000001  0x00000000  0x00000000  0x00000000  0x00000000  0x00000000

  ectool --name cros_pd infopddev <port>
  Port:1 DevId:4.1 Hash: 0x042cc79c 0x30cc12e3 0xe27a36e5 0x3f7eba5f 0x053c91d1
  Port:1 ptype:5 vid:0x18d1 pid:0x5010

Also from samus_pd console see proper result for 'typec <port>'

  typec 1
  Port C1: CC1 178 mV  CC2 427 mV (polarity:CC2)
  No Superspeed connection

Also visually inspect packets via twinkie.

Originial-Change-Id: I4e442bcb39ec1ff3cb6efff196a660819077ad76
Reviewed-on: https://chromium-review.googlesource.com/231834
Tested-by: Todd Broch <tbroch@chromium.org>
Reviewed-by: Alec Berg <alecaberg@chromium.org>
(cherry picked from commit abecc13a87fe445bb52194bc105cd70093bc8739)
Signed-off-by: Alec Berg <alecaberg@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/241430
(cherry picked from commit 409466a7d7c5e30fb806decc3aadc258342fdb91)
Signed-off-by: Todd Broch <tbroch@chromium.org>

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

4 files changed, 185 insertions, 13 deletions

diff --git a/common/usb_pd_policy.c b/common/usb_pd_policy.c
index 519d72787d..9aa0e7e083 100644
--- a/common/usb_pd_policy.c
+++ b/common/usb_pd_policy.c
@@ -260,20 +260,23 @@ int pd_alt_mode(int port)
 	return pe[port].amode.index + 1;
 }
 
-/* TODO(tbroch) this function likely needs to move up the stack to where system
- * policy decisions are made. */
-static int dfp_enter_mode(int port, uint32_t *payload)
+/* Enter default mode or attempt to enter mode via svid & index arguments */
+static int dfp_enter_mode(int port, uint32_t *payload, int use_payload)
 {
 	int i, j, done;
 	struct svdm_amode_data *modep = &pe[port].amode;
+	uint16_t svid = (use_payload) ? PD_VDO_VID(payload[0]) : 0;
+	uint8_t opos = (use_payload) ? PD_VDO_OPOS(payload[0]) : 0;
+
 	for (i = 0, done = 0; !done && (i < supported_modes_cnt); i++) {
 		for (j = 0; j < pe[port].svid_cnt; j++) {
-			if (pe[port].svids[j].svid != supported_modes[i].svid)
+			struct svdm_svid_data *svidp = &pe[port].svids[j];
+			if ((svidp->svid != supported_modes[i].svid) ||
+			    (svid && (svidp->svid != svid)))
 				continue;
-			pe[port].amode.fx = &supported_modes[i];
-			pe[port].amode.mode_caps =
-				pe[port].svids[j].mode_vdo[0];
-			pe[port].amode.index = 0;
+			modep->fx = &supported_modes[i];
+			modep->mode_caps = pe[port].svids[j].mode_vdo[0];
+			modep->index = (opos && (opos < 7)) ? opos - 1 : 0;
 			done = 1;
 			break;
 		}
@@ -478,16 +481,19 @@ int pd_svdm(int port, int cnt, uint32_t *payload, uint32_t **rpayload)
 			dfp_consume_modes(port, cnt, payload);
 			rsize = dfp_discover_modes(port, payload);
 			if (!rsize)
-				rsize = dfp_enter_mode(port, payload);
+				rsize = dfp_enter_mode(port, payload, 0);
 			break;
 		case CMD_ENTER_MODE:
+			/*
+			 * TODO(crosbug.com/p/33946): Fix won't allow multiple
+			 * mode entry.
+			 */
+			if (!AMODE_VALID(port))
+				dfp_enter_mode(port, payload, 1);
 			if (AMODE_VALID(port)) {
 				rsize = pe[port].amode.fx->status(port,
 								  payload);
-				payload[0] |=
-					VDO_OPOS(pd_alt_mode(port));
-			} else {
-				rsize = 0;
+				payload[0] |= VDO_OPOS(pd_alt_mode(port));
 			}
 			break;
 		case CMD_DP_STATUS:
@@ -602,6 +608,37 @@ static int hc_remote_pd_discovery(struct host_cmd_handler_args *args)
 DECLARE_HOST_COMMAND(EC_CMD_USB_PD_DISCOVERY,
 		     hc_remote_pd_discovery,
 		     EC_VER_MASK(0));
+
+static int hc_remote_pd_get_amode(struct host_cmd_handler_args *args)
+{
+	const struct ec_params_usb_pd_get_mode_request *p = args->params;
+	struct ec_params_usb_pd_get_mode_response *r = args->response;
+
+	if (p->port >= PD_PORT_COUNT)
+		return EC_RES_INVALID_PARAM;
+
+	/* no more to send */
+	if (p->svid_idx >= pe[p->port].svid_cnt) {
+		r->svid = 0;
+		args->response_size = sizeof(r->svid);
+		return EC_RES_SUCCESS;
+	}
+
+	r->svid = pe[p->port].svids[p->svid_idx].svid;
+	r->active = 0;
+	memcpy(r->vdo, pe[p->port].svids[p->svid_idx].mode_vdo, 24);
+
+	if (AMODE_VALID(p->port) && pe[p->port].amode.fx->svid == r->svid) {
+		r->active = 1;
+		r->idx = pd_alt_mode(p->port) - 1;
+	}
+	args->response_size = sizeof(*r);
+	return EC_RES_SUCCESS;
+}
+DECLARE_HOST_COMMAND(EC_CMD_USB_PD_GET_AMODE,
+		     hc_remote_pd_get_amode,
+		     EC_VER_MASK(0));
+
 #endif
 
 #define FW_RW_END (CONFIG_FW_RW_OFF + CONFIG_FW_RW_SIZE)
diff --git a/common/usb_pd_protocol.c b/common/usb_pd_protocol.c
index 011d033747..76d08be20f 100644
--- a/common/usb_pd_protocol.c
+++ b/common/usb_pd_protocol.c
@@ -1726,6 +1726,11 @@ void pd_task(void)
 	/* Initialize physical layer */
 	pd_hw_init(port);
 
+#ifdef CONFIG_USB_PD_ALT_MODE_DFP
+	/* Initialize PD Policy engine */
+	pd_dfp_pe_init(port);
+#endif
+
 	while (1) {
 		/* process VDM messages last */
 		pd_vdm_send_state_machine(port);
@@ -3371,4 +3376,36 @@ DECLARE_HOST_COMMAND(EC_CMD_USB_PD_DEV_INFO,
 		     hc_remote_pd_dev_info,
 		     EC_VER_MASK(0));
 
+#ifdef CONFIG_USB_PD_ALT_MODE_DFP
+static int hc_remote_pd_set_amode(struct host_cmd_handler_args *args)
+{
+	const struct ec_params_usb_pd_set_mode_request *p = args->params;
+
+	if (p->port >= PD_PORT_COUNT)
+		return EC_RES_INVALID_PARAM;
+
+	/* if in a mode exit it */
+	/* TODO(crosbug.com/p/33946): allow entry of multiple modes */
+	if (pd_alt_mode(p->port)) {
+		uint32_t vdo = pd_dfp_exit_mode(p->port);
+		if (vdo) {
+			queue_vdm(p->port, &vdo, NULL, 0);
+			task_wake(PORT_TO_TASK_ID(p->port));
+			/* Wait until exit VDM is done */
+			while (pd[p->port].vdm_state > 0)
+				task_wait_event(PD_T_VDM_E_MODE);
+		}
+	}
+
+	/* now try to enter new one. */
+	pd_send_vdm(p->port, p->svid,
+		    CMD_ENTER_MODE | VDO_OPOS(p->opos), NULL, 0);
+
+	return EC_RES_SUCCESS;
+}
+DECLARE_HOST_COMMAND(EC_CMD_USB_PD_SET_AMODE,
+		     hc_remote_pd_set_amode,
+		     EC_VER_MASK(0));
+#endif /* CONFIG_USB_PD_ALT_MODE_DFP */
+
 #endif /* CONFIG_COMMON_RUNTIME */
diff --git a/include/ec_commands.h b/include/ec_commands.h
index 357c1de432..1c4efdfdcc 100644
--- a/include/ec_commands.h
+++ b/include/ec_commands.h
@@ -2920,6 +2920,28 @@ struct ec_response_pd_log {
 #define PS_FAULT_OVP                          3
 #define PS_FAULT_DISCH                        4
 
+/* Get/Set USB-PD Alternate mode info */
+#define EC_CMD_USB_PD_GET_AMODE 0x116
+struct ec_params_usb_pd_get_mode_request {
+	uint16_t svid_idx; /* SVID index to get */
+	uint8_t port;      /* port */
+} __packed;
+
+struct ec_params_usb_pd_get_mode_response {
+	uint16_t svid;   /* SVID */
+	uint8_t active;  /* Active SVID */
+	uint8_t idx;     /* Index of active mode VDO. Ignored if !active */
+	uint32_t vdo[6]; /* Mode VDOs */
+} __packed;
+
+#define EC_CMD_USB_PD_SET_AMODE 0x117
+struct ec_params_usb_pd_set_mode_request {
+	int opos;      /* Object Position */
+	int svid_idx;  /* Index of svid to get */
+	uint16_t svid; /* SVID to set */
+	uint8_t port;  /* port */
+} __packed;
+
 #endif  /* !__ACPI__ */
 
 /*****************************************************************************/
diff --git a/util/ectool.c b/util/ectool.c
index 142c248d3f..e78ef5d65f 100644
--- a/util/ectool.c
+++ b/util/ectool.c
@@ -140,6 +140,10 @@ const char help_str[] =
 	"      Whether or not the AP should pause in S5 on shutdown\n"
 	"  pdlog\n"
 	"      Prints the PD event log entries\n"
+	"  pdgetmode <port>\n"
+	"      Get All USB-PD alternate SVIDs and modes on <port>\n"
+	"  pdsetmode <port> <svid> <opos>\n"
+	"      Set USB-PD alternate SVID and mode on <port>\n"
 	"  port80flood\n"
 	"      Rapidly write bytes to port 80\n"
 	"  port80read\n"
@@ -1000,6 +1004,76 @@ pd_flash_error:
 	return -1;
 }
 
+int cmd_pd_set_amode(int argc, char *argv[])
+{
+	char *e;
+	struct ec_params_usb_pd_set_mode_request *p =
+		(struct ec_params_usb_pd_set_mode_request *)ec_outbuf;
+
+	if (argc < 4) {
+		fprintf(stderr, "Usage: %s <port> <svid> <opos>\n", argv[0]);
+		return -1;
+	}
+
+	p->port = strtol(argv[1], &e, 0);
+	if (e && *e) {
+		fprintf(stderr, "Bad port\n");
+		return -1;
+	}
+
+	p->svid = strtol(argv[2], &e, 0);
+	if (e && *e) {
+		fprintf(stderr, "Bad svid\n");
+		return -1;
+	}
+
+	p->opos = strtol(argv[3], &e, 0);
+	if (e && *e) {
+		fprintf(stderr, "Bad mode\n");
+		return -1;
+	}
+
+	return ec_command(EC_CMD_USB_PD_SET_AMODE, 0, p, sizeof(*p), NULL, 0);
+}
+
+int cmd_pd_get_amode(int argc, char *argv[])
+{
+	int i;
+	char *e;
+	struct ec_params_usb_pd_get_mode_request *p =
+		(struct ec_params_usb_pd_get_mode_request *)ec_outbuf;
+	struct ec_params_usb_pd_get_mode_response *r =
+		(struct ec_params_usb_pd_get_mode_response *)ec_inbuf;
+
+	if (argc < 2) {
+		fprintf(stderr, "Usage: %s <port>\n", argv[0]);
+		return -1;
+	}
+
+	p->port = strtol(argv[1], &e, 0);
+	if (e && *e) {
+		fprintf(stderr, "Bad port\n");
+		return -1;
+	}
+
+	p->svid_idx = 0;
+	do {
+		ec_command(EC_CMD_USB_PD_GET_AMODE, 0, p, sizeof(*p),
+			   ec_inbuf, ec_max_insize);
+		if (!r->svid)
+			break;
+		printf("%cSVID:0x%04x ", (r->active) ? '*' : ' ',
+		       r->svid);
+		for (i = 0; i < PDO_MODES; i++) {
+			printf("%c0x%08x ", (r->active && (r->idx == i)) ?
+			       '*' : ' ', r->vdo[i]);
+		}
+		printf("\n");
+		p->svid_idx++;
+	} while (p->svid_idx < SVID_DISCOVERY_MAX);
+	return -1;
+}
+
 #ifdef __x86_64
 #include <sys/io.h>
 
@@ -5334,6 +5408,8 @@ const struct command commands[] = {
 	{"nextevent", cmd_next_event},
 	{"panicinfo", cmd_panic_info},
 	{"pause_in_s5", cmd_s5},
+	{"pdgetmode", cmd_pd_get_amode},
+	{"pdsetmode", cmd_pd_set_amode},
 	{"port80read", cmd_port80_read},
 	{"pdlog", cmd_pd_log},
 	{"powerinfo", cmd_power_info},