diff options
| -rw-r--r-- | drivers/usb/typec/ucsi/ucsi.c | 53 |
1 files changed, 29 insertions, 24 deletions
diff --git a/drivers/usb/typec/ucsi/ucsi.c b/drivers/usb/typec/ucsi/ucsi.c index f0c2fa19f3e0..a168510675da 100644 --- a/drivers/usb/typec/ucsi/ucsi.c +++ b/drivers/usb/typec/ucsi/ucsi.c @@ -1186,6 +1186,32 @@ out_unlock: return ret; } +static void ucsi_unregister_connectors(struct ucsi *ucsi) +{ + struct ucsi_connector *con; + int i; + + if (!ucsi->connector) + return; + + for (i = 0; i < ucsi->cap.num_connectors; i++) { + con = &ucsi->connector[i]; + + if (!con->wq) + break; + + cancel_work_sync(&con->work); + ucsi_unregister_partner(con); + ucsi_unregister_altmodes(con, UCSI_RECIPIENT_CON); + ucsi_unregister_port_psy(con); + destroy_workqueue(con->wq); + typec_unregister_port(con->port); + } + + kfree(ucsi->connector); + ucsi->connector = NULL; +} + /** * ucsi_init - Initialize UCSI interface * @ucsi: UCSI to be initialized @@ -1194,7 +1220,6 @@ out_unlock: */ static int ucsi_init(struct ucsi *ucsi) { - struct ucsi_connector *con; u64 command; int ret; int i; @@ -1225,7 +1250,7 @@ static int ucsi_init(struct ucsi *ucsi) } /* Allocate the connectors. Released in ucsi_unregister() */ - ucsi->connector = kcalloc(ucsi->cap.num_connectors + 1, + ucsi->connector = kcalloc(ucsi->cap.num_connectors, sizeof(*ucsi->connector), GFP_KERNEL); if (!ucsi->connector) { ret = -ENOMEM; @@ -1249,15 +1274,7 @@ static int ucsi_init(struct ucsi *ucsi) return 0; err_unregister: - for (con = ucsi->connector; con->port; con++) { - ucsi_unregister_partner(con); - ucsi_unregister_altmodes(con, UCSI_RECIPIENT_CON); - ucsi_unregister_port_psy(con); - if (con->wq) - destroy_workqueue(con->wq); - typec_unregister_port(con->port); - con->port = NULL; - } + ucsi_unregister_connectors(ucsi); err_reset: memset(&ucsi->cap, 0, sizeof(ucsi->cap)); @@ -1363,7 +1380,6 @@ EXPORT_SYMBOL_GPL(ucsi_register); void ucsi_unregister(struct ucsi *ucsi) { u64 cmd = UCSI_SET_NOTIFICATION_ENABLE; - int i; /* Make sure that we are not in the middle of driver initialization */ cancel_work_sync(&ucsi->work); @@ -1371,18 +1387,7 @@ void ucsi_unregister(struct ucsi *ucsi) /* Disable notifications */ ucsi->ops->async_write(ucsi, UCSI_CONTROL, &cmd, sizeof(cmd)); - for (i = 0; i < ucsi->cap.num_connectors; i++) { - cancel_work_sync(&ucsi->connector[i].work); - ucsi_unregister_partner(&ucsi->connector[i]); - ucsi_unregister_altmodes(&ucsi->connector[i], - UCSI_RECIPIENT_CON); - ucsi_unregister_port_psy(&ucsi->connector[i]); - if (ucsi->connector[i].wq) - destroy_workqueue(ucsi->connector[i].wq); - typec_unregister_port(ucsi->connector[i].port); - } - - kfree(ucsi->connector); + ucsi_unregister_connectors(ucsi); } EXPORT_SYMBOL_GPL(ucsi_unregister); |