}
}
-static int ucsi_get_pdos(struct ucsi_connector *con, int is_partner,
- u32 *pdos, int offset, int num_pdos)
+static int ucsi_read_pdos(struct ucsi_connector *con,
+ enum typec_role role, int is_partner,
+ u32 *pdos, int offset, int num_pdos)
{
struct ucsi *ucsi = con->ucsi;
u64 command;
command |= UCSI_GET_PDOS_PARTNER_PDO(is_partner);
command |= UCSI_GET_PDOS_PDO_OFFSET(offset);
command |= UCSI_GET_PDOS_NUM_PDOS(num_pdos - 1);
- command |= UCSI_GET_PDOS_SRC_PDOS;
+ command |= is_source(role) ? UCSI_GET_PDOS_SRC_PDOS : 0;
ret = ucsi_send_command(ucsi, command, pdos + offset,
num_pdos * sizeof(u32));
if (ret < 0 && ret != -ETIMEDOUT)
return ret;
}
-static int ucsi_get_src_pdos(struct ucsi_connector *con)
+static int ucsi_get_pdos(struct ucsi_connector *con, enum typec_role role,
+ int is_partner, u32 *pdos)
{
+ u8 num_pdos;
int ret;
/* UCSI max payload means only getting at most 4 PDOs at a time */
- ret = ucsi_get_pdos(con, 1, con->src_pdos, 0, UCSI_MAX_PDOS);
+ ret = ucsi_read_pdos(con, role, is_partner, pdos, 0, UCSI_MAX_PDOS);
if (ret < 0)
return ret;
- con->num_pdos = ret / sizeof(u32); /* number of bytes to 32-bit PDOs */
- if (con->num_pdos < UCSI_MAX_PDOS)
- return 0;
+ num_pdos = ret / sizeof(u32); /* number of bytes to 32-bit PDOs */
+ if (num_pdos < UCSI_MAX_PDOS)
+ return num_pdos;
/* get the remaining PDOs, if any */
- ret = ucsi_get_pdos(con, 1, con->src_pdos, UCSI_MAX_PDOS,
- PDO_MAX_OBJECTS - UCSI_MAX_PDOS);
+ ret = ucsi_read_pdos(con, role, is_partner, pdos, UCSI_MAX_PDOS,
+ PDO_MAX_OBJECTS - UCSI_MAX_PDOS);
if (ret < 0)
return ret;
- con->num_pdos += ret / sizeof(u32);
+ return ret / sizeof(u32) + num_pdos;
+}
+
+static int ucsi_get_src_pdos(struct ucsi_connector *con)
+{
+ int ret;
+
+ ret = ucsi_get_pdos(con, TYPEC_SOURCE, 1, con->src_pdos);
+ if (ret < 0)
+ return ret;
+
+ con->num_pdos = ret;
ucsi_port_psy_changed(con);
- return 0;
+ return ret;
}
static int ucsi_check_altmodes(struct ucsi_connector *con)
return ret;
}
+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;
+
+ con->partner_pd = usb_power_delivery_register(NULL, &desc);
+ 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 = typec_partner_set_usb_power_delivery(con->partner, con->partner_pd);
+ if (ret) {
+ usb_power_delivery_unregister_capabilities(con->partner_source_caps);
+ return ret;
+ }
+ }
+
+ ret = ucsi_get_pdos(con, TYPEC_SINK, 1, caps.pdo);
+ if (ret > 0) {
+ if (ret < PDO_MAX_OBJECTS)
+ caps.pdo[ret] = 0;
+
+ caps.role = TYPEC_SINK;
+
+ cap = usb_power_delivery_register_capabilities(con->partner_pd, &caps);
+ if (IS_ERR(cap))
+ return PTR_ERR(cap);
+
+ con->partner_sink_caps = cap;
+
+ ret = typec_partner_set_usb_power_delivery(con->partner, con->partner_pd);
+ if (ret) {
+ usb_power_delivery_unregister_capabilities(con->partner_sink_caps);
+ return ret;
+ }
+ }
+
+ return 0;
+}
+
+static void ucsi_unregister_partner_pdos(struct ucsi_connector *con)
+{
+ usb_power_delivery_unregister_capabilities(con->partner_sink_caps);
+ con->partner_sink_caps = NULL;
+ usb_power_delivery_unregister_capabilities(con->partner_source_caps);
+ con->partner_source_caps = NULL;
+ usb_power_delivery_unregister(con->partner_pd);
+ con->partner_pd = NULL;
+}
+
static void ucsi_pwr_opmode_change(struct ucsi_connector *con)
{
switch (UCSI_CONSTAT_PWR_OPMODE(con->status.flags)) {
typec_set_pwr_opmode(con->port, TYPEC_PWR_MODE_PD);
ucsi_partner_task(con, ucsi_get_src_pdos, 30, 0);
ucsi_partner_task(con, ucsi_check_altmodes, 30, 0);
+ ucsi_partner_task(con, ucsi_register_partner_pdos, 1, HZ);
break;
case UCSI_CONSTAT_PWR_OPMODE_TYPEC1_5:
con->rdo = 0;
if (!con->partner)
return;
+ ucsi_unregister_partner_pdos(con);
ucsi_unregister_altmodes(con, UCSI_RECIPIENT_SOP);
typec_unregister_partner(con->partner);
con->partner = NULL;
if (con->status.flags & UCSI_CONSTAT_CONNECTED) {
ucsi_register_partner(con);
ucsi_partner_task(con, ucsi_check_connection, 1, HZ);
+
+ if (UCSI_CONSTAT_PWR_OPMODE(con->status.flags) ==
+ UCSI_CONSTAT_PWR_OPMODE_PD)
+ ucsi_partner_task(con, ucsi_register_partner_pdos, 1, HZ);
} else {
ucsi_unregister_partner(con);
}
static int ucsi_register_port(struct ucsi *ucsi, int index)
{
+ 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 ucsi_connector *con = &ucsi->connector[index];
struct typec_capability *cap = &con->typec_cap;
enum typec_accessory *accessory = cap->accessory;
goto out;
}
+ 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;
+ }
+
+ con->port_source_caps = pd_cap;
+ typec_port_set_usb_power_delivery(con->port, con->pd);
+ }
+
+ 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;
+ }
+
+ con->port_sink_caps = pd_cap;
+ typec_port_set_usb_power_delivery(con->port, con->pd);
+ }
+
/* Alternate modes */
ret = ucsi_register_altmodes(con, UCSI_RECIPIENT_CON);
if (ret) {
if (con->status.flags & UCSI_CONSTAT_CONNECTED) {
typec_set_pwr_role(con->port,
!!(con->status.flags & UCSI_CONSTAT_PWR_DIR));
- ucsi_pwr_opmode_change(con);
ucsi_register_partner(con);
+ ucsi_pwr_opmode_change(con);
ucsi_port_psy_changed(con);
}
ucsi_unregister_port_psy(con);
if (con->wq)
destroy_workqueue(con->wq);
+
+ usb_power_delivery_unregister_capabilities(con->port_sink_caps);
+ con->port_sink_caps = NULL;
+ usb_power_delivery_unregister_capabilities(con->port_source_caps);
+ con->port_source_caps = NULL;
+ usb_power_delivery_unregister(con->pd);
+ con->pd = NULL;
typec_unregister_port(con->port);
con->port = NULL;
}
mutex_unlock(&ucsi->connector[i].lock);
destroy_workqueue(ucsi->connector[i].wq);
}
+
+ usb_power_delivery_unregister_capabilities(ucsi->connector[i].port_sink_caps);
+ ucsi->connector[i].port_sink_caps = NULL;
+ usb_power_delivery_unregister_capabilities(ucsi->connector[i].port_source_caps);
+ ucsi->connector[i].port_source_caps = NULL;
+ usb_power_delivery_unregister(ucsi->connector[i].pd);
+ ucsi->connector[i].pd = NULL;
typec_unregister_port(ucsi->connector[i].port);
}
projects / platform / kernel / linux-starfive.git / blobdiff