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
aggr_add_gpio(struct gpio_aggregator * aggr,const char * key,int hwnum,unsigned int * n)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
aggr_parse(struct gpio_aggregator * aggr)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
new_device_store(struct device_driver * driver,const char * buf,size_t count)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
gpio_aggregator_free(struct gpio_aggregator * aggr)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
delete_device_store(struct device_driver * driver,const char * buf,size_t count)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
gpio_aggregator_idr_remove(int id,void * p,void * data)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
gpio_aggregator_remove_all(void)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
gpio_fwd_get_direction(struct gpio_chip * chip,unsigned int offset)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
gpio_fwd_direction_input(struct gpio_chip * chip,unsigned int offset)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
gpio_fwd_direction_output(struct gpio_chip * chip,unsigned int offset,int value)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
gpio_fwd_get(struct gpio_chip * chip,unsigned int offset)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
gpio_fwd_get_multiple(struct gpiochip_fwd * fwd,unsigned long * mask,unsigned long * bits)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
gpio_fwd_get_multiple_locked(struct gpio_chip * chip,unsigned long * mask,unsigned long * bits)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
gpio_fwd_set(struct gpio_chip * chip,unsigned int offset,int value)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
gpio_fwd_set_multiple(struct gpiochip_fwd * fwd,unsigned long * mask,unsigned long * bits)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
gpio_fwd_set_multiple_locked(struct gpio_chip * chip,unsigned long * mask,unsigned long * bits)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
gpio_fwd_set_config(struct gpio_chip * chip,unsigned int offset,unsigned long config)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
gpio_fwd_to_irq(struct gpio_chip * chip,unsigned int offset)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 */
gpiochip_fwd_create(struct device * dev,unsigned int ngpios,struct gpio_desc * descs[])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
gpio_aggregator_probe(struct platform_device * pdev)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
gpio_aggregator_init(void)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
gpio_aggregator_exit(void)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