[PATCH 2/2] usb: typec: tcpm: create helper function for DISCOVER_MODES

[Sebastian Reichel]

The ACK and NAK response handling for DISCOVER_MODES is almost exactly
the same. Create a helper function to reduce code duplication.

Signed-off-by: Sebastian Reichel <sebastian.reichel@xxxxxxxxxxxxx>
---
 drivers/usb/typec/tcpm/tcpm.c | 129 +++++++++++++++++++-----------------------
 1 file changed, 59 insertions(+), 70 deletions(-)

diff --git a/drivers/usb/typec/tcpm/tcpm.c b/drivers/usb/typec/tcpm/tcpm.c
index 88cc27ad9514..dd4e7cd2db9e 100644
--- a/drivers/usb/typec/tcpm/tcpm.c
+++ b/drivers/usb/typec/tcpm/tcpm.c
@@ -1995,6 +1995,57 @@ static bool tcpm_cable_vdm_supported(struct tcpm_port *port)
 	       tcpm_can_communicate_sop_prime(port);
 }
 
+static void tcpm_handle_discover_mode(struct tcpm_port *port,
+				      const u32 *p, int cnt, u32 *response,
+				      enum tcpm_transmit_type rx_sop_type,
+				      enum tcpm_transmit_type *response_tx_sop_type,
+				      struct pd_mode_data *modep,
+				      struct pd_mode_data *modep_prime,
+				      int svdm_version, int *rlen,
+				      bool success)
+
+{
+	struct typec_port *typec = port->typec_port;
+
+	if (rx_sop_type == TCPC_TX_SOP) {
+		/* 6.4.4.3.3 */
+		if (success)
+			svdm_consume_modes(port, p, cnt, rx_sop_type);
+		modep->svid_index++;
+		if (modep->svid_index < modep->nsvids) {
+			u16 svid = modep->svids[modep->svid_index];
+			*response_tx_sop_type = TCPC_TX_SOP;
+			response[0] = VDO(svid, 1, svdm_version,
+					  CMD_DISCOVER_MODES);
+			*rlen = 1;
+		} else if (tcpm_cable_vdm_supported(port)) {
+			*response_tx_sop_type = TCPC_TX_SOP_PRIME;
+			response[0] = VDO(USB_SID_PD, 1,
+					  typec_get_cable_svdm_version(typec),
+					  CMD_DISCOVER_SVID);
+			*rlen = 1;
+		} else {
+			tcpm_register_partner_altmodes(port);
+		}
+	} else if (rx_sop_type == TCPC_TX_SOP_PRIME) {
+		/* 6.4.4.3.3 */
+		if (success)
+			svdm_consume_modes(port, p, cnt, rx_sop_type);
+		modep_prime->svid_index++;
+		if (modep_prime->svid_index < modep_prime->nsvids) {
+			u16 svid = modep_prime->svids[modep_prime->svid_index];
+			*response_tx_sop_type = TCPC_TX_SOP_PRIME;
+			response[0] = VDO(svid, 1,
+					  typec_get_cable_svdm_version(typec),
+					  CMD_DISCOVER_MODES);
+			*rlen = 1;
+		} else {
+			tcpm_register_plug_altmodes(port);
+			tcpm_register_partner_altmodes(port);
+		}
+	}
+}
+
 static int tcpm_pd_svdm(struct tcpm_port *port, struct typec_altmode *adev,
 			const u32 *p, int cnt, u32 *response,
 			enum adev_actions *adev_action,
@@ -2252,41 +2303,10 @@ static int tcpm_pd_svdm(struct tcpm_port *port, struct typec_altmode *adev,
 			}
 			break;
 		case CMD_DISCOVER_MODES:
-			if (rx_sop_type == TCPC_TX_SOP) {
-				/* 6.4.4.3.3 */
-				svdm_consume_modes(port, p, cnt, rx_sop_type);
-				modep->svid_index++;
-				if (modep->svid_index < modep->nsvids) {
-					u16 svid = modep->svids[modep->svid_index];
-					*response_tx_sop_type = TCPC_TX_SOP;
-					response[0] = VDO(svid, 1, svdm_version,
-							  CMD_DISCOVER_MODES);
-					rlen = 1;
-				} else if (tcpm_cable_vdm_supported(port)) {
-					*response_tx_sop_type = TCPC_TX_SOP_PRIME;
-					response[0] = VDO(USB_SID_PD, 1,
-							  typec_get_cable_svdm_version(typec),
-							  CMD_DISCOVER_SVID);
-					rlen = 1;
-				} else {
-					tcpm_register_partner_altmodes(port);
-				}
-			} else if (rx_sop_type == TCPC_TX_SOP_PRIME) {
-				/* 6.4.4.3.3 */
-				svdm_consume_modes(port, p, cnt, rx_sop_type);
-				modep_prime->svid_index++;
-				if (modep_prime->svid_index < modep_prime->nsvids) {
-					u16 svid = modep_prime->svids[modep_prime->svid_index];
-					*response_tx_sop_type = TCPC_TX_SOP_PRIME;
-					response[0] = VDO(svid, 1,
-							  typec_get_cable_svdm_version(typec),
-							  CMD_DISCOVER_MODES);
-					rlen = 1;
-				} else {
-					tcpm_register_plug_altmodes(port);
-					tcpm_register_partner_altmodes(port);
-				}
-			}
+			tcpm_handle_discover_mode(port, p, cnt, response,
+						  rx_sop_type, response_tx_sop_type,
+						  modep, modep_prime, svdm_version,
+						  &rlen, true);
 			break;
 		case CMD_ENTER_MODE:
 			*response_tx_sop_type = rx_sop_type;
@@ -2334,41 +2354,10 @@ static int tcpm_pd_svdm(struct tcpm_port *port, struct typec_altmode *adev,
 		case CMD_DISCOVER_MODES:
 			tcpm_log(port, "Skip SVID 0x%04x (failed to discover mode)",
 				 PD_VDO_SVID_SVID0(p[0]));
-
-			if (rx_sop_type == TCPC_TX_SOP) {
-				/* 6.4.4.3.3 */
-				modep->svid_index++;
-				if (modep->svid_index < modep->nsvids) {
-					u16 svid = modep->svids[modep->svid_index];
-					*response_tx_sop_type = TCPC_TX_SOP;
-					response[0] = VDO(svid, 1, svdm_version,
-							  CMD_DISCOVER_MODES);
-					rlen = 1;
-				} else if (tcpm_cable_vdm_supported(port)) {
-					*response_tx_sop_type = TCPC_TX_SOP_PRIME;
-					response[0] = VDO(USB_SID_PD, 1,
-							  typec_get_cable_svdm_version(typec),
-							  CMD_DISCOVER_SVID);
-					rlen = 1;
-				} else {
-					tcpm_register_partner_altmodes(port);
-				}
-			} else if (rx_sop_type == TCPC_TX_SOP_PRIME) {
-				/* 6.4.4.3.3 */
-				modep_prime->svid_index++;
-				if (modep_prime->svid_index < modep_prime->nsvids) {
-					u16 svid = modep_prime->svids[modep_prime->svid_index];
-					*response_tx_sop_type = TCPC_TX_SOP_PRIME;
-					response[0] = VDO(svid, 1,
-							  typec_get_cable_svdm_version(typec),
-							  CMD_DISCOVER_MODES);
-					rlen = 1;
-				} else {
-					tcpm_register_plug_altmodes(port);
-					tcpm_register_partner_altmodes(port);
-				}
-			}
-
+			tcpm_handle_discover_mode(port, p, cnt, response,
+						  rx_sop_type, response_tx_sop_type,
+						  modep, modep_prime, svdm_version,
+						  &rlen, false);
 			break;
 		case CMD_ENTER_MODE:
 			/* Back to USB Operation */

-- 
2.51.0