diff options
| -rw-r--r-- | drivers/mfd/mfd-core.c | 95 | ||||
| -rw-r--r-- | include/linux/mfd/core.h | 10 |
2 files changed, 93 insertions, 12 deletions
diff --git a/drivers/mfd/mfd-core.c b/drivers/mfd/mfd-core.c index 720e5c8b1588..b201842f82ad 100644 --- a/drivers/mfd/mfd-core.c +++ b/drivers/mfd/mfd-core.c @@ -10,6 +10,7 @@ #include <linux/kernel.h> #include <linux/platform_device.h> #include <linux/acpi.h> +#include <linux/list.h> #include <linux/property.h> #include <linux/mfd/core.h> #include <linux/pm_runtime.h> @@ -17,8 +18,17 @@ #include <linux/module.h> #include <linux/irqdomain.h> #include <linux/of.h> +#include <linux/of_address.h> #include <linux/regulator/consumer.h> +static LIST_HEAD(mfd_of_node_list); + +struct mfd_of_node_entry { + struct list_head list; + struct device *dev; + struct device_node *np; +}; + static struct device_type mfd_dev_type = { .name = "mfd_device", }; @@ -107,6 +117,55 @@ static inline void mfd_acpi_add_device(const struct mfd_cell *cell, } #endif +static int mfd_match_of_node_to_dev(struct platform_device *pdev, + struct device_node *np, + const struct mfd_cell *cell) +{ +#if IS_ENABLED(CONFIG_OF) + struct mfd_of_node_entry *of_entry; + const __be32 *reg; + u64 of_node_addr; + + /* Skip devices 'disabled' by Device Tree */ + if (!of_device_is_available(np)) + return -ENODEV; + + /* Skip if OF node has previously been allocated to a device */ + list_for_each_entry(of_entry, &mfd_of_node_list, list) + if (of_entry->np == np) + return -EAGAIN; + + if (!cell->use_of_reg) + /* No of_reg defined - allocate first free compatible match */ + goto allocate_of_node; + + /* We only care about each node's first defined address */ + reg = of_get_address(np, 0, NULL, NULL); + if (!reg) + /* OF node does not contatin a 'reg' property to match to */ + return -EAGAIN; + + of_node_addr = of_read_number(reg, of_n_addr_cells(np)); + + if (cell->of_reg != of_node_addr) + /* No match */ + return -EAGAIN; + +allocate_of_node: + of_entry = kzalloc(sizeof(*of_entry), GFP_KERNEL); + if (!of_entry) + return -ENOMEM; + + of_entry->dev = &pdev->dev; + of_entry->np = np; + list_add_tail(&of_entry->list, &mfd_of_node_list); + + pdev->dev.of_node = np; + pdev->dev.fwnode = &np->fwnode; +#endif + return 0; +} + static int mfd_add_device(struct device *parent, int id, const struct mfd_cell *cell, struct resource *mem_base, @@ -115,6 +174,7 @@ static int mfd_add_device(struct device *parent, int id, struct resource *res; struct platform_device *pdev; struct device_node *np = NULL; + struct mfd_of_node_entry *of_entry, *tmp; int ret = -ENOMEM; int platform_id; int r; @@ -149,19 +209,22 @@ static int mfd_add_device(struct device *parent, int id, if (ret < 0) goto fail_res; - if (parent->of_node && cell->of_compatible) { + if (IS_ENABLED(CONFIG_OF) && parent->of_node && cell->of_compatible) { for_each_child_of_node(parent->of_node, np) { if (of_device_is_compatible(np, cell->of_compatible)) { - if (!of_device_is_available(np)) { - /* Ignore disabled devices error free */ - ret = 0; + ret = mfd_match_of_node_to_dev(pdev, np, cell); + if (ret == -EAGAIN) + continue; + if (ret) goto fail_alias; - } - pdev->dev.of_node = np; - pdev->dev.fwnode = &np->fwnode; + break; } } + + if (!pdev->dev.of_node) + pr_warn("%s: Failed to locate of_node [id: %d]\n", + cell->name, platform_id); } mfd_acpi_add_device(cell, pdev); @@ -170,13 +233,13 @@ static int mfd_add_device(struct device *parent, int id, ret = platform_device_add_data(pdev, cell->platform_data, cell->pdata_size); if (ret) - goto fail_alias; + goto fail_of_entry; } if (cell->properties) { ret = platform_device_add_properties(pdev, cell->properties); if (ret) - goto fail_alias; + goto fail_of_entry; } for (r = 0; r < cell->num_resources; r++) { @@ -213,18 +276,18 @@ static int mfd_add_device(struct device *parent, int id, if (has_acpi_companion(&pdev->dev)) { ret = acpi_check_resource_conflict(&res[r]); if (ret) - goto fail_alias; + goto fail_of_entry; } } } ret = platform_device_add_resources(pdev, res, cell->num_resources); if (ret) - goto fail_alias; + goto fail_of_entry; ret = platform_device_add(pdev); if (ret) - goto fail_alias; + goto fail_of_entry; if (cell->pm_runtime_no_callbacks) pm_runtime_no_callbacks(&pdev->dev); @@ -233,6 +296,12 @@ static int mfd_add_device(struct device *parent, int id, return 0; +fail_of_entry: + list_for_each_entry_safe(of_entry, tmp, &mfd_of_node_list, list) + if (of_entry->dev == &pdev->dev) { + list_del(&of_entry->list); + kfree(of_entry); + } fail_alias: regulator_bulk_unregister_supply_alias(&pdev->dev, cell->parent_supplies, @@ -297,6 +366,8 @@ static int mfd_remove_devices_fn(struct device *dev, void *data) regulator_bulk_unregister_supply_alias(dev, cell->parent_supplies, cell->num_parent_supplies); + kfree(cell); + platform_device_unregister(pdev); return 0; } diff --git a/include/linux/mfd/core.h b/include/linux/mfd/core.h index ab76cdd06199..c437a73b43a3 100644 --- a/include/linux/mfd/core.h +++ b/include/linux/mfd/core.h @@ -78,6 +78,16 @@ struct mfd_cell { */ const char *of_compatible; + /* + * Address as defined in Device Tree. Used to compement 'of_compatible' + * (above) when matching OF nodes with devices that have identical + * compatible strings + */ + const u64 of_reg; + + /* Set to 'true' to use 'of_reg' (above) - allows for of_reg=0 */ + bool use_of_reg; + /* Matches ACPI */ const struct mfd_cell_acpi_match *acpi_match; |