From 193a68011fdc002cc03d6e6edabf941251df5690 Mon Sep 17 00:00:00 2001 From: Guenter Roeck Date: Tue, 9 May 2017 09:04:58 -0700 Subject: [PATCH] staging: typec: tcpm: Respond to Discover Identity commands If the lower level driver provided a list of VDOs in its configuration data, send it to the partner as response to a Discover Identity command if in device mode (UFP). Cc: Yueyao Zhu Originally-from: Puma Hsu Signed-off-by: Guenter Roeck Signed-off-by: Greg Kroah-Hartman --- drivers/staging/typec/pd_vdo.h | 4 +++- drivers/staging/typec/tcpm.c | 27 +++++++++++++++++++++++++++ drivers/staging/typec/tcpm.h | 3 +++ 3 files changed, 33 insertions(+), 1 deletion(-) diff --git a/drivers/staging/typec/pd_vdo.h b/drivers/staging/typec/pd_vdo.h index dba172e0e0d1..d92259f8de0a 100644 --- a/drivers/staging/typec/pd_vdo.h +++ b/drivers/staging/typec/pd_vdo.h @@ -22,6 +22,9 @@ * VDM object is minimum of VDM header + 6 additional data objects. */ +#define VDO_MAX_OBJECTS 6 +#define VDO_MAX_SIZE (VDO_MAX_OBJECTS + 1) + /* * VDM header * ---------- @@ -34,7 +37,6 @@ * <5> :: reserved (SVDM), command type (UVDM) * <4:0> :: command */ -#define VDO_MAX_SIZE 7 #define VDO(vid, type, custom) \ (((vid) << 16) | \ ((type) << 15) | \ diff --git a/drivers/staging/typec/tcpm.c b/drivers/staging/typec/tcpm.c index a385f7e2a6fd..c749e980ddf9 100644 --- a/drivers/staging/typec/tcpm.c +++ b/drivers/staging/typec/tcpm.c @@ -252,6 +252,8 @@ struct tcpm_port { unsigned int nr_src_pdo; u32 snk_pdo[PDO_MAX_OBJECTS]; unsigned int nr_snk_pdo; + u32 snk_vdo[VDO_MAX_OBJECTS]; + unsigned int nr_snk_vdo; unsigned int max_snk_mv; unsigned int max_snk_ma; @@ -998,6 +1000,7 @@ static int tcpm_pd_svdm(struct tcpm_port *port, const __le32 *payload, int cnt, struct pd_mode_data *modep; int rlen = 0; u16 svid; + int i; tcpm_log(port, "Rx VDM cmd 0x%x type %d cmd %d len %d", p0, cmd_type, cmd, cnt); @@ -1008,6 +1011,14 @@ static int tcpm_pd_svdm(struct tcpm_port *port, const __le32 *payload, int cnt, case CMDT_INIT: switch (cmd) { case CMD_DISCOVER_IDENT: + /* 6.4.4.3.1: Only respond as UFP (device) */ + if (port->data_role == TYPEC_DEVICE && + port->nr_snk_vdo) { + for (i = 0; i < port->nr_snk_vdo; i++) + response[i + 1] + = cpu_to_le32(port->snk_vdo[i]); + rlen = port->nr_snk_vdo + 1; + } break; case CMD_DISCOVER_SVID: break; @@ -3320,6 +3331,20 @@ static int tcpm_copy_pdos(u32 *dest_pdo, const u32 *src_pdo, return nr_pdo; } +static int tcpm_copy_vdos(u32 *dest_vdo, const u32 *src_vdo, + unsigned int nr_vdo) +{ + unsigned int i; + + if (nr_vdo > VDO_MAX_OBJECTS) + nr_vdo = VDO_MAX_OBJECTS; + + for (i = 0; i < nr_vdo; i++) + dest_vdo[i] = src_vdo[i]; + + return nr_vdo; +} + void tcpm_update_source_capabilities(struct tcpm_port *port, const u32 *pdo, unsigned int nr_pdo) { @@ -3410,6 +3435,8 @@ struct tcpm_port *tcpm_register_port(struct device *dev, struct tcpc_dev *tcpc) tcpc->config->nr_src_pdo); port->nr_snk_pdo = tcpm_copy_pdos(port->snk_pdo, tcpc->config->snk_pdo, tcpc->config->nr_snk_pdo); + port->nr_snk_vdo = tcpm_copy_vdos(port->snk_vdo, tcpc->config->snk_vdo, + tcpc->config->nr_snk_vdo); port->max_snk_mv = tcpc->config->max_snk_mv; port->max_snk_ma = tcpc->config->max_snk_ma; diff --git a/drivers/staging/typec/tcpm.h b/drivers/staging/typec/tcpm.h index 969b365e6549..19c307d31a5a 100644 --- a/drivers/staging/typec/tcpm.h +++ b/drivers/staging/typec/tcpm.h @@ -60,6 +60,9 @@ struct tcpc_config { const u32 *snk_pdo; unsigned int nr_snk_pdo; + const u32 *snk_vdo; + unsigned int nr_snk_vdo; + unsigned int max_snk_mv; unsigned int max_snk_ma; unsigned int max_snk_mw; -- 2.20.1