usb: typec: tcpm: improve handling of DISCOVER_MODES failures

UGREEN USB-C Multifunction Adapter Model CM512 (AKA "Revodok 107")
exposes two SVIDs: 0xff01 (DP Alt Mode) and 0x1d5c. The DISCOVER_MODES
step succeeds for 0xff01 and gets a NAK for 0x1d5c. Currently this
results in DP Alt Mode not being registered either, since the modes
are only registered once all of them have been discovered. The NAK
results in the processing being stopped and thus no Alt modes being
registered.

Improve the situation by handling the NAK gracefully and continue
processing the other modes.

Before this change, the TCPM log ends like this:

(more log entries before this)
[    5.028287] AMS DISCOVER_SVIDS finished
[    5.028291] cc:=4
[    5.040040] SVID 1: 0xff01
[    5.040054] SVID 2: 0x1d5c
[    5.040082] AMS DISCOVER_MODES start
[    5.040096] PD TX, header: 0x1b6f
[    5.050946] PD TX complete, status: 0
[    5.059609] PD RX, header: 0x264f [1]
[    5.059626] Rx VDM cmd 0xff018043 type 1 cmd 3 len 2
[    5.059640] AMS DISCOVER_MODES finished
[    5.059644] cc:=4
[    5.069994]  Alternate mode 0: SVID 0xff01, VDO 1: 0x000c0045
[    5.070029] AMS DISCOVER_MODES start
[    5.070043] PD TX, header: 0x1d6f
[    5.081139] PD TX complete, status: 0
[    5.087498] PD RX, header: 0x184f [1]
[    5.087515] Rx VDM cmd 0x1d5c8083 type 2 cmd 3 len 1
[    5.087529] AMS DISCOVER_MODES finished
[    5.087534] cc:=4
(no further log entries after this point)

After this patch the TCPM log looks exactly the same, but then
continues like this:

[    5.100222] Skip SVID 0x1d5c (failed to discover mode)
[    5.101699] AMS DFP_TO_UFP_ENTER_MODE start
(log goes on as the system initializes DP AltMode)

Cc: stable <stable@kernel.org>
Fixes: 41d9d75344d9 ("usb: typec: tcpm: add discover svids and discover modes support for sop'")
Reviewed-by: Heikki Krogerus <heikki.krogerus@linux.intel.com>
Signed-off-by: Sebastian Reichel <sebastian.reichel@collabora.com>
Reviewed-by: RD Babiera <rdbabiera@google.com>
Reviewed-by: Badhri Jagan Sridharan <badhri@google.com>
Link: https://patch.msgid.link/20260429-tcpm-discover-modes-nak-fix-v4-1-75945d0ed30f@collabora.com
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

diff --git a/drivers/usb/typec/tcpm/tcpm.c b/drivers/usb/typec/tcpm/tcpm.c
index ed5f745a823134c017778e019b72d4d5b5d003af..7ef746a90a17746ff892e94e1049d4f1a8712776 100644 (file)
--- a/drivers/usb/typec/tcpm/tcpm.c
+++ b/drivers/usb/typec/tcpm/tcpm.c
@@ -2149,6 +2149,55 @@ static bool tcpm_cable_vdm_supported(struct tcpm_port *port)
               tcpm_can_communicate_sop_prime(port);
 }
 
+static int tcpm_handle_discover_mode(struct tcpm_port *port, u32 *response,
+                                    enum tcpm_transmit_type rx_sop_type,
+                                    enum tcpm_transmit_type *response_tx_sop_type)
+{
+       struct typec_port *typec = port->typec_port;
+       struct pd_mode_data *modep;
+
+       if (rx_sop_type == TCPC_TX_SOP) {
+               modep = &port->mode_data;
+               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,
+                                         typec_get_negotiated_svdm_version(typec),
+                                         CMD_DISCOVER_MODES);
+                       return 1;
+               }
+
+               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);
+                       return 1;
+               }
+
+               tcpm_register_partner_altmodes(port);
+       } else if (rx_sop_type == TCPC_TX_SOP_PRIME) {
+               modep = &port->mode_data_prime;
+               modep->svid_index++;
+
+               if (modep->svid_index < modep->nsvids) {
+                       u16 svid = modep->svids[modep->svid_index];
+                       *response_tx_sop_type = TCPC_TX_SOP_PRIME;
+                       response[0] = VDO(svid, 1,
+                                         typec_get_cable_svdm_version(typec),
+                                         CMD_DISCOVER_MODES);
+                       return 1;
+               }
+
+               tcpm_register_plug_altmodes(port);
+               tcpm_register_partner_altmodes(port);
+       }
+
+       return 0;
+}
+
 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,
@@ -2406,41 +2455,11 @@ 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);
-                               }
-                       }
+                       /* 6.4.4.3.3 */
+                       svdm_consume_modes(port, p, cnt, rx_sop_type);
+                       rlen = tcpm_handle_discover_mode(port, response,
+                                                        rx_sop_type,
+                                                        response_tx_sop_type);
                        break;
                case CMD_ENTER_MODE:
                        *response_tx_sop_type = rx_sop_type;
@@ -2483,9 +2502,15 @@ static int tcpm_pd_svdm(struct tcpm_port *port, struct typec_altmode *adev,
                switch (cmd) {
                case CMD_DISCOVER_IDENT:
                case CMD_DISCOVER_SVID:
-               case CMD_DISCOVER_MODES:
                case VDO_CMD_VENDOR(0) ... VDO_CMD_VENDOR(15):
                        break;
+               case CMD_DISCOVER_MODES:
+                       tcpm_log(port, "Skip SVID 0x%04x (failed to discover mode)",
+                                PD_VDO_SVID_SVID0(p[0]));
+                       rlen = tcpm_handle_discover_mode(port, response,
+                                                        rx_sop_type,
+                                                        response_tx_sop_type);
+                       break;
                case CMD_ENTER_MODE:
                        /* Back to USB Operation */
                        *adev_action = ADEV_NOTIFY_USB_AND_QUEUE_VDM;