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 static int gpio_fwd_get_direction(struct gpio_chip *chip, unsigned int offset) 251 { 252 struct gpiochip_fwd *fwd = gpiochip_get_data(chip); 253 254 return gpiod_get_direction(fwd->descs[offset]); 255 } 256 257 static int gpio_fwd_direction_input(struct gpio_chip *chip, unsigned int offset) 258 { 259 struct gpiochip_fwd *fwd = gpiochip_get_data(chip); 260 261 return gpiod_direction_input(fwd->descs[offset]); 262 } 263 264 static int gpio_fwd_direction_output(struct gpio_chip *chip, 265 unsigned int offset, int value) 266 { 267 struct gpiochip_fwd *fwd = gpiochip_get_data(chip); 268 269 return gpiod_direction_output(fwd->descs[offset], value); 270 } 271 272 static int gpio_fwd_get(struct gpio_chip *chip, unsigned int offset) 273 { 274 struct gpiochip_fwd *fwd = gpiochip_get_data(chip); 275 276 return gpiod_get_value(fwd->descs[offset]); 277 } 278 279 static int gpio_fwd_get_multiple(struct gpiochip_fwd *fwd, unsigned long *mask, 280 unsigned long *bits) 281 { 282 struct gpio_desc **descs; 283 unsigned long *values; 284 unsigned int i, j = 0; 285 int error; 286 287 /* Both values bitmap and desc pointers are stored in tmp[] */ 288 values = &fwd->tmp[0]; 289 descs = (void *)&fwd->tmp[BITS_TO_LONGS(fwd->chip.ngpio)]; 290 291 bitmap_clear(values, 0, fwd->chip.ngpio); 292 for_each_set_bit(i, mask, fwd->chip.ngpio) 293 descs[j++] = fwd->descs[i]; 294 295 error = gpiod_get_array_value(j, descs, NULL, values); 296 if (error) 297 return error; 298 299 j = 0; 300 for_each_set_bit(i, mask, fwd->chip.ngpio) 301 __assign_bit(i, bits, test_bit(j++, values)); 302 303 return 0; 304 } 305 306 static int gpio_fwd_get_multiple_locked(struct gpio_chip *chip, 307 unsigned long *mask, unsigned long *bits) 308 { 309 struct gpiochip_fwd *fwd = gpiochip_get_data(chip); 310 unsigned long flags; 311 int error; 312 313 if (chip->can_sleep) { 314 mutex_lock(&fwd->mlock); 315 error = gpio_fwd_get_multiple(fwd, mask, bits); 316 mutex_unlock(&fwd->mlock); 317 } else { 318 spin_lock_irqsave(&fwd->slock, flags); 319 error = gpio_fwd_get_multiple(fwd, mask, bits); 320 spin_unlock_irqrestore(&fwd->slock, flags); 321 } 322 323 return error; 324 } 325 326 static void gpio_fwd_set(struct gpio_chip *chip, unsigned int offset, int value) 327 { 328 struct gpiochip_fwd *fwd = gpiochip_get_data(chip); 329 330 gpiod_set_value(fwd->descs[offset], value); 331 } 332 333 static void gpio_fwd_set_multiple(struct gpiochip_fwd *fwd, unsigned long *mask, 334 unsigned long *bits) 335 { 336 struct gpio_desc **descs; 337 unsigned long *values; 338 unsigned int i, j = 0; 339 340 /* Both values bitmap and desc pointers are stored in tmp[] */ 341 values = &fwd->tmp[0]; 342 descs = (void *)&fwd->tmp[BITS_TO_LONGS(fwd->chip.ngpio)]; 343 344 for_each_set_bit(i, mask, fwd->chip.ngpio) { 345 __assign_bit(j, values, test_bit(i, bits)); 346 descs[j++] = fwd->descs[i]; 347 } 348 349 gpiod_set_array_value(j, descs, NULL, values); 350 } 351 352 static void gpio_fwd_set_multiple_locked(struct gpio_chip *chip, 353 unsigned long *mask, unsigned long *bits) 354 { 355 struct gpiochip_fwd *fwd = gpiochip_get_data(chip); 356 unsigned long flags; 357 358 if (chip->can_sleep) { 359 mutex_lock(&fwd->mlock); 360 gpio_fwd_set_multiple(fwd, mask, bits); 361 mutex_unlock(&fwd->mlock); 362 } else { 363 spin_lock_irqsave(&fwd->slock, flags); 364 gpio_fwd_set_multiple(fwd, mask, bits); 365 spin_unlock_irqrestore(&fwd->slock, flags); 366 } 367 } 368 369 static int gpio_fwd_set_config(struct gpio_chip *chip, unsigned int offset, 370 unsigned long config) 371 { 372 struct gpiochip_fwd *fwd = gpiochip_get_data(chip); 373 374 return gpiod_set_config(fwd->descs[offset], config); 375 } 376 377 /** 378 * gpiochip_fwd_create() - Create a new GPIO forwarder 379 * @dev: Parent device pointer 380 * @ngpios: Number of GPIOs in the forwarder. 381 * @descs: Array containing the GPIO descriptors to forward to. 382 * This array must contain @ngpios entries, and must not be deallocated 383 * before the forwarder has been destroyed again. 384 * 385 * This function creates a new gpiochip, which forwards all GPIO operations to 386 * the passed GPIO descriptors. 387 * 388 * Return: An opaque object pointer, or an ERR_PTR()-encoded negative error 389 * code on failure. 390 */ 391 static struct gpiochip_fwd *gpiochip_fwd_create(struct device *dev, 392 unsigned int ngpios, 393 struct gpio_desc *descs[]) 394 { 395 const char *label = dev_name(dev); 396 struct gpiochip_fwd *fwd; 397 struct gpio_chip *chip; 398 unsigned int i; 399 int error; 400 401 fwd = devm_kzalloc(dev, struct_size(fwd, tmp, 402 BITS_TO_LONGS(ngpios) + ngpios), GFP_KERNEL); 403 if (!fwd) 404 return ERR_PTR(-ENOMEM); 405 406 chip = &fwd->chip; 407 408 /* 409 * If any of the GPIO lines are sleeping, then the entire forwarder 410 * will be sleeping. 411 * If any of the chips support .set_config(), then the forwarder will 412 * support setting configs. 413 */ 414 for (i = 0; i < ngpios; i++) { 415 struct gpio_chip *parent = gpiod_to_chip(descs[i]); 416 417 dev_dbg(dev, "%u => gpio-%d\n", i, desc_to_gpio(descs[i])); 418 419 if (gpiod_cansleep(descs[i])) 420 chip->can_sleep = true; 421 if (parent && parent->set_config) 422 chip->set_config = gpio_fwd_set_config; 423 } 424 425 chip->label = label; 426 chip->parent = dev; 427 chip->owner = THIS_MODULE; 428 chip->get_direction = gpio_fwd_get_direction; 429 chip->direction_input = gpio_fwd_direction_input; 430 chip->direction_output = gpio_fwd_direction_output; 431 chip->get = gpio_fwd_get; 432 chip->get_multiple = gpio_fwd_get_multiple_locked; 433 chip->set = gpio_fwd_set; 434 chip->set_multiple = gpio_fwd_set_multiple_locked; 435 chip->base = -1; 436 chip->ngpio = ngpios; 437 fwd->descs = descs; 438 439 if (chip->can_sleep) 440 mutex_init(&fwd->mlock); 441 else 442 spin_lock_init(&fwd->slock); 443 444 error = devm_gpiochip_add_data(dev, chip, fwd); 445 if (error) 446 return ERR_PTR(error); 447 448 return fwd; 449 } 450 451 452 /* 453 * GPIO Aggregator platform device 454 */ 455 456 static int gpio_aggregator_probe(struct platform_device *pdev) 457 { 458 struct device *dev = &pdev->dev; 459 struct gpio_desc **descs; 460 struct gpiochip_fwd *fwd; 461 int i, n; 462 463 n = gpiod_count(dev, NULL); 464 if (n < 0) 465 return n; 466 467 descs = devm_kmalloc_array(dev, n, sizeof(*descs), GFP_KERNEL); 468 if (!descs) 469 return -ENOMEM; 470 471 for (i = 0; i < n; i++) { 472 descs[i] = devm_gpiod_get_index(dev, NULL, i, GPIOD_ASIS); 473 if (IS_ERR(descs[i])) 474 return PTR_ERR(descs[i]); 475 } 476 477 fwd = gpiochip_fwd_create(dev, n, descs); 478 if (IS_ERR(fwd)) 479 return PTR_ERR(fwd); 480 481 platform_set_drvdata(pdev, fwd); 482 return 0; 483 } 484 485 #ifdef CONFIG_OF 486 static const struct of_device_id gpio_aggregator_dt_ids[] = { 487 /* 488 * Add GPIO-operated devices controlled from userspace below, 489 * or use "driver_override" in sysfs 490 */ 491 {} 492 }; 493 MODULE_DEVICE_TABLE(of, gpio_aggregator_dt_ids); 494 #endif 495 496 static struct platform_driver gpio_aggregator_driver = { 497 .probe = gpio_aggregator_probe, 498 .driver = { 499 .name = DRV_NAME, 500 .groups = gpio_aggregator_groups, 501 .of_match_table = of_match_ptr(gpio_aggregator_dt_ids), 502 }, 503 }; 504 505 static int __init gpio_aggregator_init(void) 506 { 507 return platform_driver_register(&gpio_aggregator_driver); 508 } 509 module_init(gpio_aggregator_init); 510 511 static void __exit gpio_aggregator_exit(void) 512 { 513 gpio_aggregator_remove_all(); 514 platform_driver_unregister(&gpio_aggregator_driver); 515 } 516 module_exit(gpio_aggregator_exit); 517 518 MODULE_AUTHOR("Geert Uytterhoeven <geert+renesas@glider.be>"); 519 MODULE_DESCRIPTION("GPIO Aggregator"); 520 MODULE_LICENSE("GPL v2"); 521