diff options
| -rw-r--r-- | drivers/soc/fsl/qe/gpio.c | 98 |
1 files changed, 53 insertions, 45 deletions
diff --git a/drivers/soc/fsl/qe/gpio.c b/drivers/soc/fsl/qe/gpio.c index 8df1e8fa86a5..04b44fc2bb58 100644 --- a/drivers/soc/fsl/qe/gpio.c +++ b/drivers/soc/fsl/qe/gpio.c @@ -12,16 +12,17 @@ #include <linux/spinlock.h> #include <linux/err.h> #include <linux/io.h> -#include <linux/of.h> #include <linux/gpio/legacy-of-mm-gpiochip.h> #include <linux/gpio/consumer.h> #include <linux/gpio/driver.h> #include <linux/slab.h> #include <linux/export.h> -#include <linux/property.h> +#include <linux/platform_device.h> #include <soc/fsl/qe/qe.h> +#define PIN_MASK(gpio) (1UL << (QE_PIO_PINS - 1 - (gpio))) + struct qe_gpio_chip { struct of_mm_gpio_chip mm_gc; spinlock_t lock; @@ -52,7 +53,7 @@ static int qe_gpio_get(struct gpio_chip *gc, unsigned int gpio) { struct of_mm_gpio_chip *mm_gc = to_of_mm_gpio_chip(gc); struct qe_pio_regs __iomem *regs = mm_gc->regs; - u32 pin_mask = 1 << (QE_PIO_PINS - 1 - gpio); + u32 pin_mask = PIN_MASK(gpio); return !!(ioread32be(®s->cpdata) & pin_mask); } @@ -63,7 +64,7 @@ static int qe_gpio_set(struct gpio_chip *gc, unsigned int gpio, int val) struct qe_gpio_chip *qe_gc = gpiochip_get_data(gc); struct qe_pio_regs __iomem *regs = mm_gc->regs; unsigned long flags; - u32 pin_mask = 1 << (QE_PIO_PINS - 1 - gpio); + u32 pin_mask = PIN_MASK(gpio); spin_lock_irqsave(&qe_gc->lock, flags); @@ -95,9 +96,9 @@ static int qe_gpio_set_multiple(struct gpio_chip *gc, break; if (__test_and_clear_bit(i, mask)) { if (test_bit(i, bits)) - qe_gc->cpdata |= (1U << (QE_PIO_PINS - 1 - i)); + qe_gc->cpdata |= PIN_MASK(i); else - qe_gc->cpdata &= ~(1U << (QE_PIO_PINS - 1 - i)); + qe_gc->cpdata &= ~PIN_MASK(i); } } @@ -295,45 +296,52 @@ void qe_pin_set_gpio(struct qe_pin *qe_pin) } EXPORT_SYMBOL(qe_pin_set_gpio); -static int __init qe_add_gpiochips(void) +static int qe_gpio_probe(struct platform_device *ofdev) { - struct device_node *np; - - for_each_compatible_node(np, NULL, "fsl,mpc8323-qe-pario-bank") { - int ret; - struct qe_gpio_chip *qe_gc; - struct of_mm_gpio_chip *mm_gc; - struct gpio_chip *gc; - - qe_gc = kzalloc(sizeof(*qe_gc), GFP_KERNEL); - if (!qe_gc) { - ret = -ENOMEM; - goto err; - } + struct device *dev = &ofdev->dev; + struct device_node *np = dev->of_node; + struct qe_gpio_chip *qe_gc; + struct of_mm_gpio_chip *mm_gc; + struct gpio_chip *gc; - spin_lock_init(&qe_gc->lock); - - mm_gc = &qe_gc->mm_gc; - gc = &mm_gc->gc; - - mm_gc->save_regs = qe_gpio_save_regs; - gc->ngpio = QE_PIO_PINS; - gc->direction_input = qe_gpio_dir_in; - gc->direction_output = qe_gpio_dir_out; - gc->get = qe_gpio_get; - gc->set = qe_gpio_set; - gc->set_multiple = qe_gpio_set_multiple; - - ret = of_mm_gpiochip_add_data(np, mm_gc, qe_gc); - if (ret) - goto err; - continue; -err: - pr_err("%pOF: registration failed with status %d\n", - np, ret); - kfree(qe_gc); - /* try others anyway */ - } - return 0; + qe_gc = devm_kzalloc(dev, sizeof(*qe_gc), GFP_KERNEL); + if (!qe_gc) + return -ENOMEM; + + spin_lock_init(&qe_gc->lock); + + mm_gc = &qe_gc->mm_gc; + gc = &mm_gc->gc; + + mm_gc->save_regs = qe_gpio_save_regs; + gc->ngpio = QE_PIO_PINS; + gc->direction_input = qe_gpio_dir_in; + gc->direction_output = qe_gpio_dir_out; + gc->get = qe_gpio_get; + gc->set = qe_gpio_set; + gc->set_multiple = qe_gpio_set_multiple; + + return of_mm_gpiochip_add_data(np, mm_gc, qe_gc); +} + +static const struct of_device_id qe_gpio_match[] = { + { + .compatible = "fsl,mpc8323-qe-pario-bank", + }, + {}, +}; +MODULE_DEVICE_TABLE(of, qe_gpio_match); + +static struct platform_driver qe_gpio_driver = { + .probe = qe_gpio_probe, + .driver = { + .name = "qe-gpio", + .of_match_table = qe_gpio_match, + }, +}; + +static int __init qe_gpio_init(void) +{ + return platform_driver_register(&qe_gpio_driver); } -arch_initcall(qe_add_gpiochips); +arch_initcall(qe_gpio_init); |