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