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 gpio_chip *chip, unsigned long *mask,
 337                                  unsigned long *bits)
 338 {
 339         struct gpiochip_fwd *fwd = gpiochip_get_data(chip);
 340         unsigned long *values, flags = 0;
 341         struct gpio_desc **descs;
 342         unsigned int i, j = 0;
 343         int error;
 344
 345         if (chip->can_sleep)
 346                 mutex_lock(&fwd->mlock);
 347         else
 348                 spin_lock_irqsave(&fwd->slock, flags);
 349
 350         /* Both values bitmap and desc pointers are stored in tmp[] */
 351         values = &fwd->tmp[0];
 352         descs = (void *)&fwd->tmp[BITS_TO_LONGS(fwd->chip.ngpio)];
 353
 354         bitmap_clear(values, 0, fwd->chip.ngpio);
 355         for_each_set_bit(i, mask, fwd->chip.ngpio)
 356                 descs[j++] = fwd->descs[i];
 357
 358         error = gpiod_get_array_value(j, descs, NULL, values);
 359         if (!error) {
 360                 j = 0;
 361                 for_each_set_bit(i, mask, fwd->chip.ngpio)
 362                         __assign_bit(i, bits, test_bit(j++, values));
 363         }
 364
 365         if (chip->can_sleep)
 366                 mutex_unlock(&fwd->mlock);
 367         else
 368                 spin_unlock_irqrestore(&fwd->slock, flags);
 369
 370         return error;
 371 }
 372
 373 static void gpio_fwd_set(struct gpio_chip *chip, unsigned int offset, int value)
 374 {
 375         struct gpiochip_fwd *fwd = gpiochip_get_data(chip);
 376
 377         gpiod_set_value(fwd->descs[offset], value);
 378 }
 379
 380 static void gpio_fwd_set_multiple(struct gpio_chip *chip, unsigned long *mask,
 381                                   unsigned long *bits)
 382 {
 383         struct gpiochip_fwd *fwd = gpiochip_get_data(chip);
 384         unsigned long *values, flags = 0;
 385         struct gpio_desc **descs;
 386         unsigned int i, j = 0;
 387
 388         if (chip->can_sleep)
 389                 mutex_lock(&fwd->mlock);
 390         else
 391                 spin_lock_irqsave(&fwd->slock, flags);
 392
 393         /* Both values bitmap and desc pointers are stored in tmp[] */
 394         values = &fwd->tmp[0];
 395         descs = (void *)&fwd->tmp[BITS_TO_LONGS(fwd->chip.ngpio)];
 396
 397         for_each_set_bit(i, mask, fwd->chip.ngpio) {
 398                 __assign_bit(j, values, test_bit(i, bits));
 399                 descs[j++] = fwd->descs[i];
 400         }
 401
 402         gpiod_set_array_value(j, descs, NULL, values);
 403
 404         if (chip->can_sleep)
 405                 mutex_unlock(&fwd->mlock);
 406         else
 407                 spin_unlock_irqrestore(&fwd->slock, flags);
 408 }
 409
 410 static int gpio_fwd_set_config(struct gpio_chip *chip, unsigned int offset,
 411                                unsigned long config)
 412 {
 413         struct gpiochip_fwd *fwd = gpiochip_get_data(chip);
 414
 415         return gpiod_set_config(fwd->descs[offset], config);
 416 }
 417
 418 /**
 419  * gpiochip_fwd_create() - Create a new GPIO forwarder
 420  * @dev: Parent device pointer
 421  * @ngpios: Number of GPIOs in the forwarder.
 422  * @descs: Array containing the GPIO descriptors to forward to.
 423  *         This array must contain @ngpios entries, and must not be deallocated
 424  *         before the forwarder has been destroyed again.
 425  *
 426  * This function creates a new gpiochip, which forwards all GPIO operations to
 427  * the passed GPIO descriptors.
 428  *
 429  * Return: An opaque object pointer, or an ERR_PTR()-encoded negative error
 430  *         code on failure.
 431  */
 432 static struct gpiochip_fwd *gpiochip_fwd_create(struct device *dev,
 433                                                 unsigned int ngpios,
 434                                                 struct gpio_desc *descs[])
 435 {
 436         const char *label = dev_name(dev);
 437         struct gpiochip_fwd *fwd;
 438         struct gpio_chip *chip;
 439         unsigned int i;
 440         int error;
 441
 442         fwd = devm_kzalloc(dev, struct_size(fwd, tmp,
 443                            BITS_TO_LONGS(ngpios) + ngpios), GFP_KERNEL);
 444         if (!fwd)
 445                 return ERR_PTR(-ENOMEM);
 446
 447         chip = &fwd->chip;
 448
 449         /*
 450          * If any of the GPIO lines are sleeping, then the entire forwarder
 451          * will be sleeping.
 452          * If any of the chips support .set_config(), then the forwarder will
 453          * support setting configs.
 454          */
 455         for (i = 0; i < ngpios; i++) {
 456                 struct gpio_chip *parent = gpiod_to_chip(descs[i]);
 457
 458                 dev_dbg(dev, "%u => gpio-%d\n", i, desc_to_gpio(descs[i]));
 459
 460                 if (gpiod_cansleep(descs[i]))
 461                         chip->can_sleep = true;
 462                 if (parent && parent->set_config)
 463                         chip->set_config = gpio_fwd_set_config;
 464         }
 465
 466         chip->label = label;
 467         chip->parent = dev;
 468         chip->owner = THIS_MODULE;
 469         chip->get_direction = gpio_fwd_get_direction;
 470         chip->direction_input = gpio_fwd_direction_input;
 471         chip->direction_output = gpio_fwd_direction_output;
 472         chip->get = gpio_fwd_get;
 473         chip->get_multiple = gpio_fwd_get_multiple;
 474         chip->set = gpio_fwd_set;
 475         chip->set_multiple = gpio_fwd_set_multiple;
 476         chip->base = -1;
 477         chip->ngpio = ngpios;
 478         fwd->descs = descs;
 479
 480         if (chip->can_sleep)
 481                 mutex_init(&fwd->mlock);
 482         else
 483                 spin_lock_init(&fwd->slock);
 484
 485         error = devm_gpiochip_add_data(dev, chip, fwd);
 486         if (error)
 487                 return ERR_PTR(error);
 488
 489         return fwd;
 490 }
 491
 492
 493 /*
 494  *  GPIO Aggregator platform device
 495  */
 496
 497 static int gpio_aggregator_probe(struct platform_device *pdev)
 498 {
 499         struct device *dev = &pdev->dev;
 500         struct gpio_desc **descs;
 501         struct gpiochip_fwd *fwd;
 502         int i, n;
 503
 504         n = gpiod_count(dev, NULL);
 505         if (n < 0)
 506                 return n;
 507
 508         descs = devm_kmalloc_array(dev, n, sizeof(*descs), GFP_KERNEL);
 509         if (!descs)
 510                 return -ENOMEM;
 511
 512         for (i = 0; i < n; i++) {
 513                 descs[i] = devm_gpiod_get_index(dev, NULL, i, GPIOD_ASIS);
 514                 if (IS_ERR(descs[i]))
 515                         return PTR_ERR(descs[i]);
 516         }
 517
 518         fwd = gpiochip_fwd_create(dev, n, descs);
 519         if (IS_ERR(fwd))
 520                 return PTR_ERR(fwd);
 521
 522         platform_set_drvdata(pdev, fwd);
 523         return 0;
 524 }
 525
 526 #ifdef CONFIG_OF
 527 static const struct of_device_id gpio_aggregator_dt_ids[] = {
 528         /*
 529          * Add GPIO-operated devices controlled from userspace below,
 530          * or use "driver_override" in sysfs
 531          */
 532         {},
 533 };
 534 MODULE_DEVICE_TABLE(of, gpio_aggregator_dt_ids);
 535 #endif
 536
 537 static struct platform_driver gpio_aggregator_driver = {
 538         .probe = gpio_aggregator_probe,
 539         .driver = {
 540                 .name = DRV_NAME,
 541                 .groups = gpio_aggregator_groups,
 542                 .of_match_table = of_match_ptr(gpio_aggregator_dt_ids),
 543         },
 544 };
 545
 546 static int __init gpio_aggregator_init(void)
 547 {
 548         return platform_driver_register(&gpio_aggregator_driver);
 549 }
 550 module_init(gpio_aggregator_init);
 551
 552 static void __exit gpio_aggregator_exit(void)
 553 {
 554         gpio_aggregator_remove_all();
 555         platform_driver_unregister(&gpio_aggregator_driver);
 556 }
 557 module_exit(gpio_aggregator_exit);
 558
 559 MODULE_AUTHOR("Geert Uytterhoeven <geert+renesas@glider.be>");
 560 MODULE_DESCRIPTION("GPIO Aggregator");
 561 MODULE_LICENSE("GPL v2");