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