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