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