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