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