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