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