4 files changed, 21 insertions, 0 deletions

diff --git a/drivers/staging/typec/tcpci.c b/drivers/staging/typec/tcpci.c
index cfd80a2aa883..a047b3376eec 100644
--- a/drivers/staging/typec/tcpci.c
+++ b/drivers/staging/typec/tcpci.c
@@ -426,6 +426,15 @@ static int tcpci_vbus_detect(struct tcpc_dev *tcpc, bool enable)
 	return 0;
 }
 
+static void tcpci_bist_mode(struct tcpc_dev *tcpc, bool enable)
+{
+	struct tcpci *tcpci = tcpc_to_tcpci(tcpc);
+
+	regmap_update_bits(tcpci->regmap, TCPC_TCPC_CTRL,
+			   TCPC_TCPC_CTRL_BIST_MODE,
+			   enable ? TCPC_TCPC_CTRL_BIST_MODE : 0);
+}
+
 static int tcpci_init(struct tcpc_dev *tcpc)
 {
 	struct tcpci *tcpci = tcpc_to_tcpci(tcpc);
@@ -602,6 +611,7 @@ static int tcpci_probe(struct i2c_client *client,
 	tcpci->tcpc.vbus_detect = tcpci_vbus_detect;
 	tcpci->tcpc.vbus_discharge = tcpci_vbus_force_discharge;
 	tcpci->tcpc.get_vbus_vol = tcpci_get_vbus_vol;
+	tcpci->tcpc.bist_mode = tcpci_bist_mode;
 
 	tcpci->tcpc.set_pd_rx = tcpci_set_pd_rx;
 	tcpci->tcpc.set_roles = tcpci_set_roles;
diff --git a/drivers/staging/typec/tcpci.h b/drivers/staging/typec/tcpci.h
index d74423fe377e..75a9666b1822 100644
--- a/drivers/staging/typec/tcpci.h
+++ b/drivers/staging/typec/tcpci.h
@@ -45,6 +45,7 @@
 #define TCPC_CONFIG_STD_OUTPUT		0x18
 
 #define TCPC_TCPC_CTRL			0x19
+#define TCPC_TCPC_CTRL_BIST_MODE	BIT(1)
 #define TCPC_TCPC_CTRL_ORIENTATION	BIT(0)
 
 #define TCPC_ROLE_CTRL			0x1a
diff --git a/drivers/staging/typec/tcpm.c b/drivers/staging/typec/tcpm.c
index b4e77b23a068..73a9e3689af3 100644
--- a/drivers/staging/typec/tcpm.c
+++ b/drivers/staging/typec/tcpm.c
@@ -1893,6 +1893,13 @@ static void tcpm_typec_connect(struct tcpm_port *port)
 	}
 }
 
+static void tcpm_bist_handle(struct tcpm_port *port, bool enable)
+{
+	/* Enable or disable BIST test mode */
+	if (port->tcpc && port->tcpc->bist_mode)
+		port->tcpc->bist_mode(port->tcpc, enable);
+}
+
 static int tcpm_src_attach(struct tcpm_port *port)
 {
 	enum typec_cc_polarity polarity =
@@ -1980,6 +1987,7 @@ static void tcpm_reset_port(struct tcpm_port *port)
 	port->attached = false;
 	port->pd_capable = false;
 
+	tcpm_bist_handle(port, false);
 	port->tcpc->set_pd_rx(port->tcpc, false);
 	tcpm_init_vbus(port);	/* also disables charging */
 	tcpm_init_vconn(port);
@@ -2676,6 +2684,7 @@ static void run_state_machine(struct tcpm_port *port)
 		break;
 
 	case BIST_RX:
+		tcpm_bist_handle(port, true);
 		switch (BDO_MODE_MASK(port->bist_request)) {
 		case BDO_MODE_CARRIER2:
 			tcpm_pd_transmit(port, TCPC_TX_BIST_MODE_2, NULL);
diff --git a/drivers/staging/typec/tcpm.h b/drivers/staging/typec/tcpm.h
index 08f103db017c..b8a57425914a 100644
--- a/drivers/staging/typec/tcpm.h
+++ b/drivers/staging/typec/tcpm.h
@@ -128,6 +128,7 @@ struct tcpc_dev {
 			   const struct pd_message *msg);
 	int (*vbus_detect)(struct tcpc_dev *dev, bool enable);
 	int (*vbus_discharge)(struct tcpc_dev *tcpc, bool enable);
+	void (*bist_mode)(struct tcpc_dev *tcpc, bool enable);
 
 	struct tcpc_mux_dev *mux;
 };