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