diff --git a/drivers/gpio/gpiolib-of.c b/drivers/gpio/gpiolib-of.c index 2b47f9388607..ccc449df3792 100644 --- a/drivers/gpio/gpiolib-of.c +++ b/drivers/gpio/gpiolib-of.c @@ -628,6 +628,10 @@ static int of_gpiochip_add_hog(struct gpio_chip *chip, struct device_node *hog) ret = gpiod_hog(desc, name, lflags, dflags); if (ret < 0) return ret; + +#ifdef CONFIG_OF_DYNAMIC + desc->hog = hog; +#endif } return 0; @@ -655,11 +659,97 @@ static int of_gpiochip_scan_gpios(struct gpio_chip *chip) of_node_put(np); return ret; } + + of_node_set_flag(np, OF_POPULATED); } return 0; } +#ifdef CONFIG_OF_DYNAMIC +/** + * of_gpiochip_remove_hog - Remove all hogs in a hog device node + * @chip: gpio chip to act on + * @hog: device node describing the hogs + */ +static void of_gpiochip_remove_hog(struct gpio_chip *chip, + struct device_node *hog) +{ + struct gpio_desc *descs = chip->gpiodev->descs; + unsigned int i; + + for (i = 0; i < chip->ngpio; i++) { + if (test_bit(FLAG_IS_HOGGED, &descs[i].flags) && + descs[i].hog == hog) + gpiochip_free_own_desc(&descs[i]); + } +} + +static int of_gpiochip_match_node(struct gpio_chip *chip, void *data) +{ + return chip->gpiodev->dev.of_node == data; +} + +static struct gpio_chip *of_find_gpiochip_by_node(struct device_node *np) +{ + return gpiochip_find(np, of_gpiochip_match_node); +} + +static int of_gpio_notify(struct notifier_block *nb, unsigned long action, + void *arg) +{ + struct of_reconfig_data *rd = arg; + struct gpio_chip *chip; + int ret; + + /* + * This only supports adding and removing complete gpio-hog nodes. + * Modifying an existing gpio-hog node is not supported (except for + * changing its "status" property, which is treated the same as + * addition/removal). + */ + switch (of_reconfig_get_state_change(action, arg)) { + case OF_RECONFIG_CHANGE_ADD: + if (!of_property_read_bool(rd->dn, "gpio-hog")) + return NOTIFY_OK; /* not for us */ + + if (of_node_test_and_set_flag(rd->dn, OF_POPULATED)) + return NOTIFY_OK; + + chip = of_find_gpiochip_by_node(rd->dn->parent); + if (chip == NULL) + return NOTIFY_OK; /* not for us */ + + ret = of_gpiochip_add_hog(chip, rd->dn); + if (ret < 0) { + pr_err("%s: failed to add hogs for %pOF\n", __func__, + rd->dn); + of_node_clear_flag(rd->dn, OF_POPULATED); + return notifier_from_errno(ret); + } + break; + + case OF_RECONFIG_CHANGE_REMOVE: + if (!of_node_check_flag(rd->dn, OF_POPULATED)) + return NOTIFY_OK; /* already depopulated */ + + chip = of_find_gpiochip_by_node(rd->dn->parent); + if (chip == NULL) + return NOTIFY_OK; /* not for us */ + + of_gpiochip_remove_hog(chip, rd->dn); + of_node_clear_flag(rd->dn, OF_POPULATED); + break; + } + + return NOTIFY_OK; +} + +struct notifier_block gpio_of_notifier = { + .notifier_call = of_gpio_notify, +}; +#endif /* CONFIG_OF_DYNAMIC */ + /** * of_gpio_simple_xlate - translate gpiospec to the GPIO number and flags * @gc: pointer to the gpio_chip structure diff --git a/drivers/gpio/gpiolib-of.h b/drivers/gpio/gpiolib-of.h index 9768831b1fe2..ed26664f1537 100644 --- a/drivers/gpio/gpiolib-of.h +++ b/drivers/gpio/gpiolib-of.h @@ -35,4 +35,6 @@ static inline bool of_gpio_need_valid_mask(const struct gpio_chip *gc) } #endif /* CONFIG_OF_GPIO */ +extern struct notifier_block gpio_of_notifier; + #endif /* GPIOLIB_OF_H */ diff --git a/drivers/gpio/gpiolib.c b/drivers/gpio/gpiolib.c index 2c3a9c1c76be..9ad86477af9c 100644 --- a/drivers/gpio/gpiolib.c +++ b/drivers/gpio/gpiolib.c @@ -3103,6 +3103,9 @@ static bool gpiod_free_commit(struct gpio_desc *desc) clear_bit(FLAG_PULL_DOWN, &desc->flags); clear_bit(FLAG_BIAS_DISABLE, &desc->flags); clear_bit(FLAG_IS_HOGGED, &desc->flags); +#ifdef CONFIG_OF_DYNAMIC + desc->hog = NULL; +#endif ret = true; } @@ -5281,10 +5284,15 @@ static int __init gpiolib_dev_init(void) if (ret < 0) { pr_err("gpiolib: failed to allocate char dev region\n"); bus_unregister(&gpio_bus_type); - } else { - gpiolib_initialized = true; - gpiochip_setup_devs(); + return ret; } + + gpiolib_initialized = true; + gpiochip_setup_devs(); + + if (IS_ENABLED(CONFIG_OF_DYNAMIC)) + WARN_ON(of_reconfig_notifier_register(&gpio_of_notifier)); + return ret; } core_initcall(gpiolib_dev_init); diff --git a/drivers/gpio/gpiolib.h b/drivers/gpio/gpiolib.h index 5ab90746b519..853ce681b4a4 100644 --- a/drivers/gpio/gpiolib.h +++ b/drivers/gpio/gpiolib.h @@ -120,6 +120,9 @@ struct gpio_desc { const char *label; /* Name of the GPIO */ const char *name; +#ifdef CONFIG_OF_DYNAMIC + struct device_node *hog; +#endif }; int gpiod_request(struct gpio_desc *desc, const char *label);