diff mbox series

[v2,06/11] usb: typec: ucsi: extract code to read PD caps

Message ID 20240329-qcom-ucsi-fixes-v2-6-0f5d37ed04db@linaro.org
State New
Headers show
Series usb: typec: ucsi: fix several issues manifesting on Qualcomm platforms | expand

Commit Message

Dmitry Baryshkov March 29, 2024, 6:15 a.m. UTC
Extract function to read PDOs from the port and set PD capabilities
accordingly.

Signed-off-by: Dmitry Baryshkov <dmitry.baryshkov@linaro.org>
---
 drivers/usb/typec/ucsi/ucsi.c | 85 ++++++++++++++++---------------------------
 1 file changed, 32 insertions(+), 53 deletions(-)

Comments

Heikki Krogerus April 2, 2024, 10:41 a.m. UTC | #1
On Fri, Mar 29, 2024 at 08:15:38AM +0200, Dmitry Baryshkov wrote:
> 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>

> ---
>  drivers/usb/typec/ucsi/ucsi.c | 85 ++++++++++++++++---------------------------
>  1 file changed, 32 insertions(+), 53 deletions(-)
> 
> diff --git a/drivers/usb/typec/ucsi/ucsi.c b/drivers/usb/typec/ucsi/ucsi.c
> index 35366b1a3d78..18b2e4ffc57e 100644
> --- 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);
>  }
> @@ -1463,7 +1466,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;
> @@ -1544,36 +1546,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);
>  
> 
> -- 
> 2.39.2
diff mbox series

Patch

diff --git a/drivers/usb/typec/ucsi/ucsi.c b/drivers/usb/typec/ucsi/ucsi.c
index 35366b1a3d78..18b2e4ffc57e 100644
--- 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);
 }
@@ -1463,7 +1466,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;
@@ -1544,36 +1546,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);