diff --git a/drivers/base/platform-msi.c b/drivers/base/platform-msi.c index 6148c78f51a7..44b8c0d816fe 100644 --- a/drivers/base/platform-msi.c +++ b/drivers/base/platform-msi.c @@ -189,6 +189,52 @@ struct irq_domain *platform_msi_create_irq_domain(struct fwnode_handle *fwnode, return domain; } +static struct platform_msi_priv_data * +platform_msi_alloc_priv_data(struct device *dev, unsigned int nvec, + irq_write_msi_msg_t write_msi_msg) +{ + struct platform_msi_priv_data *datap; + /* + * Limit the number of interrupts to 256 per device. Should we + * need to bump this up, DEV_ID_SHIFT should be adjusted + * accordingly (which would impact the max number of MSI + * capable devices). + */ + if (!dev->msi_domain || !write_msi_msg || !nvec || nvec > MAX_DEV_MSIS) + return ERR_PTR(-EINVAL); + + if (dev->msi_domain->bus_token != DOMAIN_BUS_PLATFORM_MSI) { + dev_err(dev, "Incompatible msi_domain, giving up\n"); + return ERR_PTR(-EINVAL); + } + + /* Already had a helping of MSI? Greed... */ + if (!list_empty(dev_to_msi_list(dev))) + return ERR_PTR(-EBUSY); + + datap = kzalloc(sizeof(*datap), GFP_KERNEL); + if (!datap) + return ERR_PTR(-ENOMEM); + + datap->devid = ida_simple_get(&platform_msi_devid_ida, + 0, 1 << DEV_ID_SHIFT, GFP_KERNEL); + if (datap->devid < 0) { + int err = datap->devid; + kfree(datap); + return ERR_PTR(err); + } + + datap->write_msg = write_msi_msg; + + return datap; +} + +static void platform_msi_free_priv_data(struct platform_msi_priv_data *data) +{ + ida_simple_remove(&platform_msi_devid_ida, data->devid); + kfree(data); +} + /** * platform_msi_domain_alloc_irqs - Allocate MSI interrupts for @dev * @dev: The device for which to allocate interrupts @@ -204,40 +250,13 @@ int platform_msi_domain_alloc_irqs(struct device *dev, unsigned int nvec, struct platform_msi_priv_data *priv_data; int err; - /* - * Limit the number of interrupts to 256 per device. Should we - * need to bump this up, DEV_ID_SHIFT should be adjusted - * accordingly (which would impact the max number of MSI - * capable devices). - */ - if (!dev->msi_domain || !write_msi_msg || !nvec || nvec > MAX_DEV_MSIS) - return -EINVAL; - - if (dev->msi_domain->bus_token != DOMAIN_BUS_PLATFORM_MSI) { - dev_err(dev, "Incompatible msi_domain, giving up\n"); - return -EINVAL; - } - - /* Already had a helping of MSI? Greed... */ - if (!list_empty(dev_to_msi_list(dev))) - return -EBUSY; - - priv_data = kzalloc(sizeof(*priv_data), GFP_KERNEL); - if (!priv_data) - return -ENOMEM; - - priv_data->devid = ida_simple_get(&platform_msi_devid_ida, - 0, 1 << DEV_ID_SHIFT, GFP_KERNEL); - if (priv_data->devid < 0) { - err = priv_data->devid; - goto out_free_data; - } - - priv_data->write_msg = write_msi_msg; + priv_data = platform_msi_alloc_priv_data(dev, nvec, write_msi_msg); + if (IS_ERR(priv_data)) + return PTR_ERR(priv_data); err = platform_msi_alloc_descs(dev, nvec, priv_data); if (err) - goto out_free_id; + goto out_free_priv_data; err = msi_domain_alloc_irqs(dev->msi_domain, dev, nvec); if (err) @@ -247,10 +266,8 @@ int platform_msi_domain_alloc_irqs(struct device *dev, unsigned int nvec, out_free_desc: platform_msi_free_descs(dev, 0, nvec); -out_free_id: - ida_simple_remove(&platform_msi_devid_ida, priv_data->devid); -out_free_data: - kfree(priv_data); +out_free_priv_data: + platform_msi_free_priv_data(priv_data); return err; } @@ -261,16 +278,11 @@ out_free_data: */ void platform_msi_domain_free_irqs(struct device *dev) { - struct msi_desc *desc; + if (!list_empty(dev_to_msi_list(dev))) { + struct msi_desc *desc; - desc = first_msi_entry(dev); - if (desc) { - struct platform_msi_priv_data *data; - - data = desc->platform.msi_priv_data; - - ida_simple_remove(&platform_msi_devid_ida, data->devid); - kfree(data); + desc = first_msi_entry(dev); + platform_msi_free_priv_data(desc->platform.msi_priv_data); } msi_domain_free_irqs(dev->msi_domain, dev);