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@xxxxxxxxxx>
Reviewed-by: Heikki Krogerus <heikki.krogerus@xxxxxxxxxxxxxxx>
> ---
> 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
--
heikki
