diff --git a/drivers/usb/typec/ucsi/ucsi.c b/drivers/usb/typec/ucsi/ucsi.c index 6a6723e8fb12..f56897e23df8 100644 --- a/drivers/usb/typec/ucsi/ucsi.c +++ b/drivers/usb/typec/ucsi/ucsi.c @@ -1845,6 +1845,42 @@ static int ucsi_register_port(struct ucsi *ucsi, struct ucsi_connector *con) return ret; } +static void ucsi_unregister_port(struct ucsi_connector *con) +{ + struct ucsi_work *uwork; + + if (con->wq) { + mutex_lock(&con->lock); + ucsi_unregister_partner(con); + /* + * queue delayed items immediately so they can execute + * and free themselves before the wq is destroyed + */ + list_for_each_entry(uwork, &con->partner_tasks, node) { + if (cancel_delayed_work(&uwork->work)) + queue_delayed_work(con->wq, &uwork->work, 0); + } + mutex_unlock(&con->lock); + + destroy_workqueue(con->wq); + con->wq = NULL; + } else { + ucsi_unregister_partner(con); + } + + ucsi_unregister_altmodes(con, UCSI_RECIPIENT_CON); + ucsi_unregister_port_psy(con); + + 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; +} + static u64 ucsi_get_supported_notifications(struct ucsi *ucsi) { u16 features = ucsi->cap.features; @@ -1971,22 +2007,8 @@ static int ucsi_init(struct ucsi *ucsi) for (i = 0; i < ucsi->cap.num_connectors; i++) lockdep_unregister_key(&connector[i].lock_key); - for (con = connector; con->port; con++) { - if (con->wq) - destroy_workqueue(con->wq); - ucsi_unregister_partner(con); - ucsi_unregister_altmodes(con, UCSI_RECIPIENT_CON); - ucsi_unregister_port_psy(con); - - 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; - } + for (con = connector; con->port; con++) + ucsi_unregister_port(con); kfree(connector); err_reset: memset(&ucsi->cap, 0, sizeof(ucsi->cap)); @@ -2214,33 +2236,7 @@ void ucsi_unregister(struct ucsi *ucsi) for (i = 0; i < ucsi->cap.num_connectors; i++) { cancel_work_sync(&ucsi->connector[i].work); - - if (ucsi->connector[i].wq) { - struct ucsi_work *uwork; - - mutex_lock(&ucsi->connector[i].lock); - /* - * queue delayed items immediately so they can execute - * and free themselves before the wq is destroyed - */ - list_for_each_entry(uwork, &ucsi->connector[i].partner_tasks, node) - mod_delayed_work(ucsi->connector[i].wq, &uwork->work, 0); - mutex_unlock(&ucsi->connector[i].lock); - destroy_workqueue(ucsi->connector[i].wq); - } - - ucsi_unregister_partner(&ucsi->connector[i]); - ucsi_unregister_altmodes(&ucsi->connector[i], - UCSI_RECIPIENT_CON); - ucsi_unregister_port_psy(&ucsi->connector[i]); - - 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); + ucsi_unregister_port(&ucsi->connector[i]); lockdep_unregister_key(&ucsi->connector[i].lock_key); }