@@ -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);
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(-)