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