usb: typec: ucsi: extract code to read PD caps

Extract function to read PDOs from the port and set PD capabilities
accordingly.

Signed-off-by: Dmitry Baryshkov <dmitry.baryshkov@linaro.org>
Reviewed-by: Heikki Krogerus <heikki.krogerus@linux.intel.com>
Link: https://lore.kernel.org/r/20240329-qcom-ucsi-fixes-v2-6-0f5d37ed04db@linaro.org
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

diff --git a/drivers/usb/typec/ucsi/ucsi.c b/drivers/usb/typec/ucsi/ucsi.c
index 0da30b90e3f73377f73248fba0616e7c7f03de53..649bb69caed4c322ec198b6ad586d51503f42ad0 100644 (file)
--- a/drivers/usb/typec/ucsi/ucsi.c
+++ b/drivers/usb/typec/ucsi/ucsi.c
@@ -677,6 +677,26 @@ static int ucsi_get_src_pdos(struct ucsi_connector *con)
        return ret;
 }
 
+static struct usb_power_delivery_capabilities *ucsi_get_pd_caps(struct ucsi_connector *con,
+                                                               enum typec_role role,
+                                                               bool is_partner)
+{
+       struct usb_power_delivery_capabilities_desc pd_caps;
+       int ret;
+
+       ret = ucsi_get_pdos(con, role, is_partner, pd_caps.pdo);
+       if (ret <= 0)
+               return ERR_PTR(ret);
+
+       if (ret < PDO_MAX_OBJECTS)
+               pd_caps.pdo[ret] = 0;
+
+       pd_caps.role = role;
+
+       return usb_power_delivery_register_capabilities(is_partner ? con->partner_pd : con->pd,
+                                                       &pd_caps);
+}
+
 static int ucsi_read_identity(struct ucsi_connector *con, u8 recipient,
                              u8 offset, u8 bytes, void *resp)
 {
@@ -804,9 +824,7 @@ static int ucsi_check_altmodes(struct ucsi_connector *con)
 static int ucsi_register_partner_pdos(struct ucsi_connector *con)
 {
        struct usb_power_delivery_desc desc = { con->ucsi->cap.pd_version };
-       struct usb_power_delivery_capabilities_desc caps;
        struct usb_power_delivery_capabilities *cap;
-       int ret;
 
        if (con->partner_pd)
                return 0;
@@ -815,32 +833,17 @@ static int ucsi_register_partner_pdos(struct ucsi_connector *con)
        if (IS_ERR(con->partner_pd))
                return PTR_ERR(con->partner_pd);
 
-       ret = ucsi_get_pdos(con, TYPEC_SOURCE, 1, caps.pdo);
-       if (ret > 0) {
-               if (ret < PDO_MAX_OBJECTS)
-                       caps.pdo[ret] = 0;
-
-               caps.role = TYPEC_SOURCE;
-               cap = usb_power_delivery_register_capabilities(con->partner_pd, &caps);
-               if (IS_ERR(cap))
-                       return PTR_ERR(cap);
-
-               con->partner_source_caps = cap;
-       }
-
-       ret = ucsi_get_pdos(con, TYPEC_SINK, 1, caps.pdo);
-       if (ret > 0) {
-               if (ret < PDO_MAX_OBJECTS)
-                       caps.pdo[ret] = 0;
+       cap = ucsi_get_pd_caps(con, TYPEC_SOURCE, true);
+       if (IS_ERR(cap))
+           return PTR_ERR(cap);
 
-               caps.role = TYPEC_SINK;
+       con->partner_source_caps = cap;
 
-               cap = usb_power_delivery_register_capabilities(con->partner_pd, &caps);
-               if (IS_ERR(cap))
-                       return PTR_ERR(cap);
+       cap = ucsi_get_pd_caps(con, TYPEC_SINK, true);
+       if (IS_ERR(cap))
+           return PTR_ERR(cap);
 
-               con->partner_sink_caps = cap;
-       }
+       con->partner_sink_caps = cap;
 
        return typec_partner_set_usb_power_delivery(con->partner, con->partner_pd);
 }
@@ -1469,7 +1472,6 @@ static struct fwnode_handle *ucsi_find_fwnode(struct ucsi_connector *con)
 static int ucsi_register_port(struct ucsi *ucsi, struct ucsi_connector *con)
 {
        struct usb_power_delivery_desc desc = { ucsi->cap.pd_version};
-       struct usb_power_delivery_capabilities_desc pd_caps;
        struct usb_power_delivery_capabilities *pd_cap;
        struct typec_capability *cap = &con->typec_cap;
        enum typec_accessory *accessory = cap->accessory;
@@ -1550,36 +1552,13 @@ static int ucsi_register_port(struct ucsi *ucsi, struct ucsi_connector *con)
 
        con->pd = usb_power_delivery_register(ucsi->dev, &desc);
 
-       ret = ucsi_get_pdos(con, TYPEC_SOURCE, 0, pd_caps.pdo);
-       if (ret > 0) {
-               if (ret < PDO_MAX_OBJECTS)
-                       pd_caps.pdo[ret] = 0;
-
-               pd_caps.role = TYPEC_SOURCE;
-               pd_cap = usb_power_delivery_register_capabilities(con->pd, &pd_caps);
-               if (IS_ERR(pd_cap)) {
-                       ret = PTR_ERR(pd_cap);
-                       goto out;
-               }
-
+       pd_cap = ucsi_get_pd_caps(con, TYPEC_SOURCE, false);
+       if (!IS_ERR(pd_cap))
                con->port_source_caps = pd_cap;
-       }
-
-       memset(&pd_caps, 0, sizeof(pd_caps));
-       ret = ucsi_get_pdos(con, TYPEC_SINK, 0, pd_caps.pdo);
-       if (ret > 0) {
-               if (ret < PDO_MAX_OBJECTS)
-                       pd_caps.pdo[ret] = 0;
-
-               pd_caps.role = TYPEC_SINK;
-               pd_cap = usb_power_delivery_register_capabilities(con->pd, &pd_caps);
-               if (IS_ERR(pd_cap)) {
-                       ret = PTR_ERR(pd_cap);
-                       goto out;
-               }
 
+       pd_cap = ucsi_get_pd_caps(con, TYPEC_SINK, false);
+       if (!IS_ERR(pd_cap))
                con->port_sink_caps = pd_cap;
-       }
 
        typec_port_set_usb_power_delivery(con->port, con->pd);