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"); 562