drivers/gpio/gpio-aggregator.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 //
   3 // GPIO Aggregator
   4 //
   5 // Copyright (C) 2019-2020 Glider bv
   6
   7 #define DRV_NAME       "gpio-aggregator"
   8 #define pr_fmt(fmt)     DRV_NAME ": " fmt
   9
  10 #include <linux/bitmap.h>
  11 #include <linux/bitops.h>
  12 #include <linux/ctype.h>
  13 #include <linux/gpio.h>
  14 #include <linux/gpio/consumer.h>
  15 #include <linux/gpio/driver.h>
  16 #include <linux/gpio/machine.h>
  17 #include <linux/idr.h>
  18 #include <linux/kernel.h>
  19 #include <linux/module.h>
  20 #include <linux/mutex.h>
  21 #include <linux/overflow.h>
  22 #include <linux/platform_device.h>
  23 #include <linux/spinlock.h>
  24 #include <linux/string.h>
  25
  26 #define AGGREGATOR_MAX_GPIOS 512
  27
  28 /*
  29  * GPIO Aggregator sysfs interface
  30  */
  31
  32 struct gpio_aggregator {
  33         struct gpiod_lookup_table *lookups;
  34         struct platform_device *pdev;
  35         char args[];
  36 };
  37
  38 static DEFINE_MUTEX(gpio_aggregator_lock);      /* protects idr */
  39 static DEFINE_IDR(gpio_aggregator_idr);
  40
  41 static int aggr_add_gpio(struct gpio_aggregator *aggr, const char *key,
  42                          int hwnum, unsigned int *n)
  43 {
  44         struct gpiod_lookup_table *lookups;
  45
  46         lookups = krealloc(aggr->lookups, struct_size(lookups, table, *n + 2),
  47                            GFP_KERNEL);
  48         if (!lookups)
  49                 return -ENOMEM;
  50
  51         lookups->table[*n] = GPIO_LOOKUP_IDX(key, hwnum, NULL, *n, 0);
  52
  53         (*n)++;
  54         memset(&lookups->table[*n], 0, sizeof(lookups->table[*n]));
  55
  56         aggr->lookups = lookups;
  57         return 0;
  58 }
  59
  60 static int aggr_parse(struct gpio_aggregator *aggr)
  61 {
  62         char *args = skip_spaces(aggr->args);
  63         char *name, *offsets, *p;
  64         unsigned long *bitmap;
  65         unsigned int i, n = 0;
  66         int error = 0;
  67
  68         bitmap = bitmap_alloc(AGGREGATOR_MAX_GPIOS, GFP_KERNEL);
  69         if (!bitmap)
  70                 return -ENOMEM;
  71
  72         args = next_arg(args, &name, &p);
  73         while (*args) {
  74                 args = next_arg(args, &offsets, &p);
  75
  76                 p = get_options(offsets, 0, &error);
  77                 if (error == 0 || *p) {
  78                         /* Named GPIO line */
  79                         error = aggr_add_gpio(aggr, name, U16_MAX, &n);
  80                         if (error)
  81                                 goto free_bitmap;
  82
  83                         name = offsets;
  84                         continue;
  85                 }
  86
  87                 /* GPIO chip + offset(s) */
  88                 error = bitmap_parselist(offsets, bitmap, AGGREGATOR_MAX_GPIOS);
  89                 if (error) {
  90                         pr_err("Cannot parse %s: %d\n", offsets, error);
  91                         goto free_bitmap;
  92                 }
  93
  94                 for_each_set_bit(i, bitmap, AGGREGATOR_MAX_GPIOS) {
  95                         error = aggr_add_gpio(aggr, name, i, &n);
  96                         if (error)
  97                                 goto free_bitmap;
  98                 }
  99
 100                 args = next_arg(args, &name, &p);
 101         }
 102
 103         if (!n) {
 104                 pr_err("No GPIOs specified\n");
 105                 error = -EINVAL;
 106         }
 107
 108 free_bitmap:
 109         bitmap_free(bitmap);
 110         return error;
 111 }
 112
 113 static ssize_t new_device_store(struct device_driver *driver, const char *buf,
 114                                 size_t count)
 115 {
 116         struct gpio_aggregator *aggr;
 117         struct platform_device *pdev;
 118         int res, id;
 119
 120         /* kernfs guarantees string termination, so count + 1 is safe */
 121         aggr = kzalloc(sizeof(*aggr) + count + 1, GFP_KERNEL);
 122         if (!aggr)
 123                 return -ENOMEM;
 124
 125         memcpy(aggr->args, buf, count + 1);
 126
 127         aggr->lookups = kzalloc(struct_size(aggr->lookups, table, 1),
 128                                 GFP_KERNEL);
 129         if (!aggr->lookups) {
 130                 res = -ENOMEM;
 131                 goto free_ga;
 132         }
 133
 134         mutex_lock(&gpio_aggregator_lock);
 135         id = idr_alloc(&gpio_aggregator_idr, aggr, 0, 0, GFP_KERNEL);
 136         mutex_unlock(&gpio_aggregator_lock);
 137
 138         if (id < 0) {
 139                 res = id;
 140                 goto free_table;
 141         }
 142
 143         aggr->lookups->dev_id = kasprintf(GFP_KERNEL, "%s.%d", DRV_NAME, id);
 144         if (!aggr->lookups->dev_id) {
 145                 res = -ENOMEM;
 146                 goto remove_idr;
 147         }
 148
 149         res = aggr_parse(aggr);
 150         if (res)
 151                 goto free_dev_id;
 152
 153         gpiod_add_lookup_table(aggr->lookups);
 154
 155         pdev = platform_device_register_simple(DRV_NAME, id, NULL, 0);
 156         if (IS_ERR(pdev)) {
 157                 res = PTR_ERR(pdev);
 158                 goto remove_table;
 159         }
 160
 161         aggr->pdev = pdev;
 162         return count;
 163
 164 remove_table:
 165         gpiod_remove_lookup_table(aggr->lookups);
 166 free_dev_id:
 167         kfree(aggr->lookups->dev_id);
 168 remove_idr:
 169         mutex_lock(&gpio_aggregator_lock);
 170         idr_remove(&gpio_aggregator_idr, id);
 171         mutex_unlock(&gpio_aggregator_lock);
 172 free_table:
 173         kfree(aggr->lookups);
 174 free_ga:
 175         kfree(aggr);
 176         return res;
 177 }
 178
 179 static DRIVER_ATTR_WO(new_device);
 180
 181 static void gpio_aggregator_free(struct gpio_aggregator *aggr)
 182 {
 183         platform_device_unregister(aggr->pdev);
 184         gpiod_remove_lookup_table(aggr->lookups);
 185         kfree(aggr->lookups->dev_id);
 186         kfree(aggr->lookups);
 187         kfree(aggr);
 188 }
 189
 190 static ssize_t delete_device_store(struct device_driver *driver,
 191                                    const char *buf, size_t count)
 192 {
 193         struct gpio_aggregator *aggr;
 194         unsigned int id;
 195         int error;
 196
 197         if (!str_has_prefix(buf, DRV_NAME "."))
 198                 return -EINVAL;
 199
 200         error = kstrtouint(buf + strlen(DRV_NAME "."), 10, &id);
 201         if (error)
 202                 return error;
 203
 204         mutex_lock(&gpio_aggregator_lock);
 205         aggr = idr_remove(&gpio_aggregator_idr, id);
 206         mutex_unlock(&gpio_aggregator_lock);
 207         if (!aggr)
 208                 return -ENOENT;
 209
 210         gpio_aggregator_free(aggr);
 211         return count;
 212 }
 213 static DRIVER_ATTR_WO(delete_device);
 214
 215 static struct attribute *gpio_aggregator_attrs[] = {
 216         &driver_attr_new_device.attr,
 217         &driver_attr_delete_device.attr,
 218         NULL
 219 };
 220 ATTRIBUTE_GROUPS(gpio_aggregator);
 221
 222 static int __exit gpio_aggregator_idr_remove(int id, void *p, void *data)
 223 {
 224         gpio_aggregator_free(p);
 225         return 0;
 226 }
 227
 228 static void __exit gpio_aggregator_remove_all(void)
 229 {
 230         mutex_lock(&gpio_aggregator_lock);
 231         idr_for_each(&gpio_aggregator_idr, gpio_aggregator_idr_remove, NULL);
 232         idr_destroy(&gpio_aggregator_idr);
 233         mutex_unlock(&gpio_aggregator_lock);
 234 }
 235
 236
 237 /*
 238  *  GPIO Forwarder
 239  */
 240
 241 struct gpiochip_fwd {
 242         struct gpio_chip chip;
 243         struct gpio_desc **descs;
 244         union {
 245                 struct mutex mlock;     /* protects tmp[] if can_sleep */
 246                 spinlock_t slock;       /* protects tmp[] if !can_sleep */
 247         };
 248         unsigned long tmp[];            /* values and descs for multiple ops */
 249 };
 250
 251 #define fwd_tmp_values(fwd)     &(fwd)->tmp[0]
 252 #define fwd_tmp_descs(fwd)      (void *)&(fwd)->tmp[BITS_TO_LONGS((fwd)->chip.ngpio)]
 253
 254 #define fwd_tmp_size(ngpios)    (BITS_TO_LONGS((ngpios)) + (ngpios))
 255
 256 static int gpio_fwd_get_direction(struct gpio_chip *chip, unsigned int offset)
 257 {
 258         struct gpiochip_fwd *fwd = gpiochip_get_data(chip);
 259
 260         return gpiod_get_direction(fwd->descs[offset]);
 261 }
 262
 263 static int gpio_fwd_direction_input(struct gpio_chip *chip, unsigned int offset)
 264 {
 265         struct gpiochip_fwd *fwd = gpiochip_get_data(chip);
 266
 267         return gpiod_direction_input(fwd->descs[offset]);
 268 }
 269
 270 static int gpio_fwd_direction_output(struct gpio_chip *chip,
 271                                      unsigned int offset, int value)
 272 {
 273         struct gpiochip_fwd *fwd = gpiochip_get_data(chip);
 274
 275         return gpiod_direction_output(fwd->descs[offset], value);
 276 }
 277
 278 static int gpio_fwd_get(struct gpio_chip *chip, unsigned int offset)
 279 {
 280         struct gpiochip_fwd *fwd = gpiochip_get_data(chip);
 281
 282         return chip->can_sleep ? gpiod_get_value_cansleep(fwd->descs[offset])
 283                                : gpiod_get_value(fwd->descs[offset]);
 284 }
 285
 286 static int gpio_fwd_get_multiple(struct gpiochip_fwd *fwd, unsigned long *mask,
 287                                  unsigned long *bits)
 288 {
 289         struct gpio_desc **descs = fwd_tmp_descs(fwd);
 290         unsigned long *values = fwd_tmp_values(fwd);
 291         unsigned int i, j = 0;
 292         int error;
 293
 294         bitmap_clear(values, 0, fwd->chip.ngpio);
 295         for_each_set_bit(i, mask, fwd->chip.ngpio)
 296                 descs[j++] = fwd->descs[i];
 297
 298         if (fwd->chip.can_sleep)
 299                 error = gpiod_get_array_value_cansleep(j, descs, NULL, values);
 300         else
 301                 error = gpiod_get_array_value(j, descs, NULL, values);
 302         if (error)
 303                 return error;
 304
 305         j = 0;
 306         for_each_set_bit(i, mask, fwd->chip.ngpio)
 307                 __assign_bit(i, bits, test_bit(j++, values));
 308
 309         return 0;
 310 }
 311
 312 static int gpio_fwd_get_multiple_locked(struct gpio_chip *chip,
 313                                         unsigned long *mask, unsigned long *bits)
 314 {
 315         struct gpiochip_fwd *fwd = gpiochip_get_data(chip);
 316         unsigned long flags;
 317         int error;
 318
 319         if (chip->can_sleep) {
 320                 mutex_lock(&fwd->mlock);
 321                 error = gpio_fwd_get_multiple(fwd, mask, bits);
 322                 mutex_unlock(&fwd->mlock);
 323         } else {
 324                 spin_lock_irqsave(&fwd->slock, flags);
 325                 error = gpio_fwd_get_multiple(fwd, mask, bits);
 326                 spin_unlock_irqrestore(&fwd->slock, flags);
 327         }
 328
 329         return error;
 330 }
 331
 332 static void gpio_fwd_set(struct gpio_chip *chip, unsigned int offset, int value)
 333 {
 334         struct gpiochip_fwd *fwd = gpiochip_get_data(chip);
 335
 336         if (chip->can_sleep)
 337                 gpiod_set_value_cansleep(fwd->descs[offset], value);
 338         else
 339                 gpiod_set_value(fwd->descs[offset], value);
 340 }
 341
 342 static void gpio_fwd_set_multiple(struct gpiochip_fwd *fwd, unsigned long *mask,
 343                                   unsigned long *bits)
 344 {
 345         struct gpio_desc **descs = fwd_tmp_descs(fwd);
 346         unsigned long *values = fwd_tmp_values(fwd);
 347         unsigned int i, j = 0;
 348
 349         for_each_set_bit(i, mask, fwd->chip.ngpio) {
 350                 __assign_bit(j, values, test_bit(i, bits));
 351                 descs[j++] = fwd->descs[i];
 352         }
 353
 354         if (fwd->chip.can_sleep)
 355                 gpiod_set_array_value_cansleep(j, descs, NULL, values);
 356         else
 357                 gpiod_set_array_value(j, descs, NULL, values);
 358 }
 359
 360 static void gpio_fwd_set_multiple_locked(struct gpio_chip *chip,
 361                                          unsigned long *mask, unsigned long *bits)
 362 {
 363         struct gpiochip_fwd *fwd = gpiochip_get_data(chip);
 364         unsigned long flags;
 365
 366         if (chip->can_sleep) {
 367                 mutex_lock(&fwd->mlock);
 368                 gpio_fwd_set_multiple(fwd, mask, bits);
 369                 mutex_unlock(&fwd->mlock);
 370         } else {
 371                 spin_lock_irqsave(&fwd->slock, flags);
 372                 gpio_fwd_set_multiple(fwd, mask, bits);
 373                 spin_unlock_irqrestore(&fwd->slock, flags);
 374         }
 375 }
 376
 377 static int gpio_fwd_set_config(struct gpio_chip *chip, unsigned int offset,
 378                                unsigned long config)
 379 {
 380         struct gpiochip_fwd *fwd = gpiochip_get_data(chip);
 381
 382         return gpiod_set_config(fwd->descs[offset], config);
 383 }
 384
 385 static int gpio_fwd_to_irq(struct gpio_chip *chip, unsigned int offset)
 386 {
 387         struct gpiochip_fwd *fwd = gpiochip_get_data(chip);
 388
 389         return gpiod_to_irq(fwd->descs[offset]);
 390 }
 391
 392 /**
 393  * gpiochip_fwd_create() - Create a new GPIO forwarder
 394  * @dev: Parent device pointer
 395  * @ngpios: Number of GPIOs in the forwarder.
 396  * @descs: Array containing the GPIO descriptors to forward to.
 397  *         This array must contain @ngpios entries, and must not be deallocated
 398  *         before the forwarder has been destroyed again.
 399  *
 400  * This function creates a new gpiochip, which forwards all GPIO operations to
 401  * the passed GPIO descriptors.
 402  *
 403  * Return: An opaque object pointer, or an ERR_PTR()-encoded negative error
 404  *         code on failure.
 405  */
 406 static struct gpiochip_fwd *gpiochip_fwd_create(struct device *dev,
 407                                                 unsigned int ngpios,
 408                                                 struct gpio_desc *descs[])
 409 {
 410         const char *label = dev_name(dev);
 411         struct gpiochip_fwd *fwd;
 412         struct gpio_chip *chip;
 413         unsigned int i;
 414         int error;
 415
 416         fwd = devm_kzalloc(dev, struct_size(fwd, tmp, fwd_tmp_size(ngpios)),
 417                            GFP_KERNEL);
 418         if (!fwd)
 419                 return ERR_PTR(-ENOMEM);
 420
 421         chip = &fwd->chip;
 422
 423         /*
 424          * If any of the GPIO lines are sleeping, then the entire forwarder
 425          * will be sleeping.
 426          * If any of the chips support .set_config(), then the forwarder will
 427          * support setting configs.
 428          */
 429         for (i = 0; i < ngpios; i++) {
 430                 struct gpio_chip *parent = gpiod_to_chip(descs[i]);
 431
 432                 dev_dbg(dev, "%u => gpio %d irq %d\n", i,
 433                         desc_to_gpio(descs[i]), gpiod_to_irq(descs[i]));
 434
 435                 if (gpiod_cansleep(descs[i]))
 436                         chip->can_sleep = true;
 437                 if (parent && parent->set_config)
 438                         chip->set_config = gpio_fwd_set_config;
 439         }
 440
 441         chip->label = label;
 442         chip->parent = dev;
 443         chip->owner = THIS_MODULE;
 444         chip->get_direction = gpio_fwd_get_direction;
 445         chip->direction_input = gpio_fwd_direction_input;
 446         chip->direction_output = gpio_fwd_direction_output;
 447         chip->get = gpio_fwd_get;
 448         chip->get_multiple = gpio_fwd_get_multiple_locked;
 449         chip->set = gpio_fwd_set;
 450         chip->set_multiple = gpio_fwd_set_multiple_locked;
 451         chip->to_irq = gpio_fwd_to_irq;
 452         chip->base = -1;
 453         chip->ngpio = ngpios;
 454         fwd->descs = descs;
 455
 456         if (chip->can_sleep)
 457                 mutex_init(&fwd->mlock);
 458         else
 459                 spin_lock_init(&fwd->slock);
 460
 461         error = devm_gpiochip_add_data(dev, chip, fwd);
 462         if (error)
 463                 return ERR_PTR(error);
 464
 465         return fwd;
 466 }
 467
 468
 469 /*
 470  *  GPIO Aggregator platform device
 471  */
 472
 473 static int gpio_aggregator_probe(struct platform_device *pdev)
 474 {
 475         struct device *dev = &pdev->dev;
 476         struct gpio_desc **descs;
 477         struct gpiochip_fwd *fwd;
 478         int i, n;
 479
 480         n = gpiod_count(dev, NULL);
 481         if (n < 0)
 482                 return n;
 483
 484         descs = devm_kmalloc_array(dev, n, sizeof(*descs), GFP_KERNEL);
 485         if (!descs)
 486                 return -ENOMEM;
 487
 488         for (i = 0; i < n; i++) {
 489                 descs[i] = devm_gpiod_get_index(dev, NULL, i, GPIOD_ASIS);
 490                 if (IS_ERR(descs[i]))
 491                         return PTR_ERR(descs[i]);
 492         }
 493
 494         fwd = gpiochip_fwd_create(dev, n, descs);
 495         if (IS_ERR(fwd))
 496                 return PTR_ERR(fwd);
 497
 498         platform_set_drvdata(pdev, fwd);
 499         return 0;
 500 }
 501
 502 #ifdef CONFIG_OF
 503 static const struct of_device_id gpio_aggregator_dt_ids[] = {
 504         /*
 505          * Add GPIO-operated devices controlled from userspace below,
 506          * or use "driver_override" in sysfs
 507          */
 508         {}
 509 };
 510 MODULE_DEVICE_TABLE(of, gpio_aggregator_dt_ids);
 511 #endif
 512
 513 static struct platform_driver gpio_aggregator_driver = {
 514         .probe = gpio_aggregator_probe,
 515         .driver = {
 516                 .name = DRV_NAME,
 517                 .groups = gpio_aggregator_groups,
 518                 .of_match_table = of_match_ptr(gpio_aggregator_dt_ids),
 519         },
 520 };
 521
 522 static int __init gpio_aggregator_init(void)
 523 {
 524         return platform_driver_register(&gpio_aggregator_driver);
 525 }
 526 module_init(gpio_aggregator_init);
 527
 528 static void __exit gpio_aggregator_exit(void)
 529 {
 530         gpio_aggregator_remove_all();
 531         platform_driver_unregister(&gpio_aggregator_driver);
 532 }
 533 module_exit(gpio_aggregator_exit);
 534
 535 MODULE_AUTHOR("Geert Uytterhoeven <[email protected]>");
 536 MODULE_DESCRIPTION("GPIO Aggregator");
 537 MODULE_LICENSE("GPL v2");