[linux-block.git] / drivers / gpio / gpio-aggregator.c

// SPDX-License-Identifier: GPL-2.0-only
//
// GPIO Aggregator
//
// Copyright (C) 2019-2020 Glider bv

#define DRV_NAME       "gpio-aggregator"
#define pr_fmt(fmt)	DRV_NAME ": " fmt

#include <linux/bitmap.h>
#include <linux/bitops.h>
#include <linux/ctype.h>
#include <linux/delay.h>
#include <linux/idr.h>
#include <linux/kernel.h>
#include <linux/mod_devicetable.h>
#include <linux/module.h>
#include <linux/mutex.h>
#include <linux/overflow.h>
#include <linux/platform_device.h>
#include <linux/property.h>
#include <linux/slab.h>
#include <linux/spinlock.h>
#include <linux/string.h>

#include <linux/gpio/consumer.h>
#include <linux/gpio/driver.h>
#include <linux/gpio/machine.h>

#define AGGREGATOR_MAX_GPIOS 512

/*
 * GPIO Aggregator sysfs interface
 */

struct gpio_aggregator {
	struct gpiod_lookup_table *lookups;
	struct platform_device *pdev;
	char args[];
};

static DEFINE_MUTEX(gpio_aggregator_lock);	/* protects idr */
static DEFINE_IDR(gpio_aggregator_idr);

static int aggr_add_gpio(struct gpio_aggregator *aggr, const char *key,
			 int hwnum, unsigned int *n)
{
	struct gpiod_lookup_table *lookups;

	lookups = krealloc(aggr->lookups, struct_size(lookups, table, *n + 2),
			   GFP_KERNEL);
	if (!lookups)
		return -ENOMEM;

	lookups->table[*n] = GPIO_LOOKUP_IDX(key, hwnum, NULL, *n, 0);

	(*n)++;
	memset(&lookups->table[*n], 0, sizeof(lookups->table[*n]));

	aggr->lookups = lookups;
	return 0;
}

static int aggr_parse(struct gpio_aggregator *aggr)
{
	char *args = skip_spaces(aggr->args);
	char *name, *offsets, *p;
	unsigned long *bitmap;
	unsigned int i, n = 0;
	int error = 0;

	bitmap = bitmap_alloc(AGGREGATOR_MAX_GPIOS, GFP_KERNEL);
	if (!bitmap)
		return -ENOMEM;

	args = next_arg(args, &name, &p);
	while (*args) {
		args = next_arg(args, &offsets, &p);

		p = get_options(offsets, 0, &error);
		if (error == 0 || *p) {
			/* Named GPIO line */
			error = aggr_add_gpio(aggr, name, U16_MAX, &n);
			if (error)
				goto free_bitmap;

			name = offsets;
			continue;
		}

		/* GPIO chip + offset(s) */
		error = bitmap_parselist(offsets, bitmap, AGGREGATOR_MAX_GPIOS);
		if (error) {
			pr_err("Cannot parse %s: %d\n", offsets, error);
			goto free_bitmap;
		}

		for_each_set_bit(i, bitmap, AGGREGATOR_MAX_GPIOS) {
			error = aggr_add_gpio(aggr, name, i, &n);
			if (error)
				goto free_bitmap;
		}

		args = next_arg(args, &name, &p);
	}

	if (!n) {
		pr_err("No GPIOs specified\n");
		error = -EINVAL;
	}

free_bitmap:
	bitmap_free(bitmap);
	return error;
}

static ssize_t new_device_store(struct device_driver *driver, const char *buf,
				size_t count)
{
	struct gpio_aggregator *aggr;
	struct platform_device *pdev;
	int res, id;

	/* kernfs guarantees string termination, so count + 1 is safe */
	aggr = kzalloc(sizeof(*aggr) + count + 1, GFP_KERNEL);
	if (!aggr)
		return -ENOMEM;

	memcpy(aggr->args, buf, count + 1);

	aggr->lookups = kzalloc(struct_size(aggr->lookups, table, 1),
				GFP_KERNEL);
	if (!aggr->lookups) {
		res = -ENOMEM;
		goto free_ga;
	}

	mutex_lock(&gpio_aggregator_lock);
	id = idr_alloc(&gpio_aggregator_idr, aggr, 0, 0, GFP_KERNEL);
	mutex_unlock(&gpio_aggregator_lock);

	if (id < 0) {
		res = id;
		goto free_table;
	}

	aggr->lookups->dev_id = kasprintf(GFP_KERNEL, "%s.%d", DRV_NAME, id);
	if (!aggr->lookups->dev_id) {
		res = -ENOMEM;
		goto remove_idr;
	}

	res = aggr_parse(aggr);
	if (res)
		goto free_dev_id;

	gpiod_add_lookup_table(aggr->lookups);

	pdev = platform_device_register_simple(DRV_NAME, id, NULL, 0);
	if (IS_ERR(pdev)) {
		res = PTR_ERR(pdev);
		goto remove_table;
	}

	aggr->pdev = pdev;
	return count;

remove_table:
	gpiod_remove_lookup_table(aggr->lookups);
free_dev_id:
	kfree(aggr->lookups->dev_id);
remove_idr:
	mutex_lock(&gpio_aggregator_lock);
	idr_remove(&gpio_aggregator_idr, id);
	mutex_unlock(&gpio_aggregator_lock);
free_table:
	kfree(aggr->lookups);
free_ga:
	kfree(aggr);
	return res;
}

static DRIVER_ATTR_WO(new_device);

static void gpio_aggregator_free(struct gpio_aggregator *aggr)
{
	platform_device_unregister(aggr->pdev);
	gpiod_remove_lookup_table(aggr->lookups);
	kfree(aggr->lookups->dev_id);
	kfree(aggr->lookups);
	kfree(aggr);
}

static ssize_t delete_device_store(struct device_driver *driver,
				   const char *buf, size_t count)
{
	struct gpio_aggregator *aggr;
	unsigned int id;
	int error;

	if (!str_has_prefix(buf, DRV_NAME "."))
		return -EINVAL;

	error = kstrtouint(buf + strlen(DRV_NAME "."), 10, &id);
	if (error)
		return error;

	mutex_lock(&gpio_aggregator_lock);
	aggr = idr_remove(&gpio_aggregator_idr, id);
	mutex_unlock(&gpio_aggregator_lock);
	if (!aggr)
		return -ENOENT;

	gpio_aggregator_free(aggr);
	return count;
}
static DRIVER_ATTR_WO(delete_device);

static struct attribute *gpio_aggregator_attrs[] = {
	&driver_attr_new_device.attr,
	&driver_attr_delete_device.attr,
	NULL
};
ATTRIBUTE_GROUPS(gpio_aggregator);

static int __exit gpio_aggregator_idr_remove(int id, void *p, void *data)
{
	gpio_aggregator_free(p);
	return 0;
}

static void __exit gpio_aggregator_remove_all(void)
{
	mutex_lock(&gpio_aggregator_lock);
	idr_for_each(&gpio_aggregator_idr, gpio_aggregator_idr_remove, NULL);
	idr_destroy(&gpio_aggregator_idr);
	mutex_unlock(&gpio_aggregator_lock);
}


/*
 *  GPIO Forwarder
 */

struct gpiochip_fwd_timing {
	u32 ramp_up_us;
	u32 ramp_down_us;
};

struct gpiochip_fwd {
	struct gpio_chip chip;
	struct gpio_desc **descs;
	union {
		struct mutex mlock;	/* protects tmp[] if can_sleep */
		spinlock_t slock;	/* protects tmp[] if !can_sleep */
	};
	struct gpiochip_fwd_timing *delay_timings;
	unsigned long tmp[];		/* values and descs for multiple ops */
};

#define fwd_tmp_values(fwd)	&(fwd)->tmp[0]
#define fwd_tmp_descs(fwd)	(void *)&(fwd)->tmp[BITS_TO_LONGS((fwd)->chip.ngpio)]

#define fwd_tmp_size(ngpios)	(BITS_TO_LONGS((ngpios)) + (ngpios))

static int gpio_fwd_get_direction(struct gpio_chip *chip, unsigned int offset)
{
	struct gpiochip_fwd *fwd = gpiochip_get_data(chip);

	return gpiod_get_direction(fwd->descs[offset]);
}

static int gpio_fwd_direction_input(struct gpio_chip *chip, unsigned int offset)
{
	struct gpiochip_fwd *fwd = gpiochip_get_data(chip);

	return gpiod_direction_input(fwd->descs[offset]);
}

static int gpio_fwd_direction_output(struct gpio_chip *chip,
				     unsigned int offset, int value)
{
	struct gpiochip_fwd *fwd = gpiochip_get_data(chip);

	return gpiod_direction_output(fwd->descs[offset], value);
}

static int gpio_fwd_get(struct gpio_chip *chip, unsigned int offset)
{
	struct gpiochip_fwd *fwd = gpiochip_get_data(chip);

	return chip->can_sleep ? gpiod_get_value_cansleep(fwd->descs[offset])
			       : gpiod_get_value(fwd->descs[offset]);
}

static int gpio_fwd_get_multiple(struct gpiochip_fwd *fwd, unsigned long *mask,
				 unsigned long *bits)
{
	struct gpio_desc **descs = fwd_tmp_descs(fwd);
	unsigned long *values = fwd_tmp_values(fwd);
	unsigned int i, j = 0;
	int error;

	bitmap_clear(values, 0, fwd->chip.ngpio);
	for_each_set_bit(i, mask, fwd->chip.ngpio)
		descs[j++] = fwd->descs[i];

	if (fwd->chip.can_sleep)
		error = gpiod_get_array_value_cansleep(j, descs, NULL, values);
	else
		error = gpiod_get_array_value(j, descs, NULL, values);
	if (error)
		return error;

	j = 0;
	for_each_set_bit(i, mask, fwd->chip.ngpio)
		__assign_bit(i, bits, test_bit(j++, values));

	return 0;
}

static int gpio_fwd_get_multiple_locked(struct gpio_chip *chip,
					unsigned long *mask, unsigned long *bits)
{
	struct gpiochip_fwd *fwd = gpiochip_get_data(chip);
	unsigned long flags;
	int error;

	if (chip->can_sleep) {
		mutex_lock(&fwd->mlock);
		error = gpio_fwd_get_multiple(fwd, mask, bits);
		mutex_unlock(&fwd->mlock);
	} else {
		spin_lock_irqsave(&fwd->slock, flags);
		error = gpio_fwd_get_multiple(fwd, mask, bits);
		spin_unlock_irqrestore(&fwd->slock, flags);
	}

	return error;
}

static void gpio_fwd_delay(struct gpio_chip *chip, unsigned int offset, int value)
{
	struct gpiochip_fwd *fwd = gpiochip_get_data(chip);
	const struct gpiochip_fwd_timing *delay_timings;
	bool is_active_low = gpiod_is_active_low(fwd->descs[offset]);
	u32 delay_us;

	delay_timings = &fwd->delay_timings[offset];
	if ((!is_active_low && value) || (is_active_low && !value))
		delay_us = delay_timings->ramp_up_us;
	else
		delay_us = delay_timings->ramp_down_us;
	if (!delay_us)
		return;

	if (chip->can_sleep)
		fsleep(delay_us);
	else
		udelay(delay_us);
}

static void gpio_fwd_set(struct gpio_chip *chip, unsigned int offset, int value)
{
	struct gpiochip_fwd *fwd = gpiochip_get_data(chip);

	if (chip->can_sleep)
		gpiod_set_value_cansleep(fwd->descs[offset], value);
	else
		gpiod_set_value(fwd->descs[offset], value);

	if (fwd->delay_timings)
		gpio_fwd_delay(chip, offset, value);
}

static void gpio_fwd_set_multiple(struct gpiochip_fwd *fwd, unsigned long *mask,
				  unsigned long *bits)
{
	struct gpio_desc **descs = fwd_tmp_descs(fwd);
	unsigned long *values = fwd_tmp_values(fwd);
	unsigned int i, j = 0;

	for_each_set_bit(i, mask, fwd->chip.ngpio) {
		__assign_bit(j, values, test_bit(i, bits));
		descs[j++] = fwd->descs[i];
	}

	if (fwd->chip.can_sleep)
		gpiod_set_array_value_cansleep(j, descs, NULL, values);
	else
		gpiod_set_array_value(j, descs, NULL, values);
}

static void gpio_fwd_set_multiple_locked(struct gpio_chip *chip,
					 unsigned long *mask, unsigned long *bits)
{
	struct gpiochip_fwd *fwd = gpiochip_get_data(chip);
	unsigned long flags;

	if (chip->can_sleep) {
		mutex_lock(&fwd->mlock);
		gpio_fwd_set_multiple(fwd, mask, bits);
		mutex_unlock(&fwd->mlock);
	} else {
		spin_lock_irqsave(&fwd->slock, flags);
		gpio_fwd_set_multiple(fwd, mask, bits);
		spin_unlock_irqrestore(&fwd->slock, flags);
	}
}

static int gpio_fwd_set_config(struct gpio_chip *chip, unsigned int offset,
			       unsigned long config)
{
	struct gpiochip_fwd *fwd = gpiochip_get_data(chip);

	return gpiod_set_config(fwd->descs[offset], config);
}

static int gpio_fwd_to_irq(struct gpio_chip *chip, unsigned int offset)
{
	struct gpiochip_fwd *fwd = gpiochip_get_data(chip);

	return gpiod_to_irq(fwd->descs[offset]);
}

/*
 * The GPIO delay provides a way to configure platform specific delays
 * for the GPIO ramp-up or ramp-down delays. This can serve the following
 * purposes:
 *   - Open-drain output using an RC filter
 */
#define FWD_FEATURE_DELAY		BIT(0)

#ifdef CONFIG_OF_GPIO
static int gpiochip_fwd_delay_of_xlate(struct gpio_chip *chip,
				       const struct of_phandle_args *gpiospec,
				       u32 *flags)
{
	struct gpiochip_fwd *fwd = gpiochip_get_data(chip);
	struct gpiochip_fwd_timing *timings;
	u32 line;

	if (gpiospec->args_count != chip->of_gpio_n_cells)
		return -EINVAL;

	line = gpiospec->args[0];
	if (line >= chip->ngpio)
		return -EINVAL;

	timings = &fwd->delay_timings[line];
	timings->ramp_up_us = gpiospec->args[1];
	timings->ramp_down_us = gpiospec->args[2];

	return line;
}

static int gpiochip_fwd_setup_delay_line(struct device *dev, struct gpio_chip *chip,
					 struct gpiochip_fwd *fwd)
{
	fwd->delay_timings = devm_kcalloc(dev, chip->ngpio,
					  sizeof(*fwd->delay_timings),
					  GFP_KERNEL);
	if (!fwd->delay_timings)
		return -ENOMEM;

	chip->of_xlate = gpiochip_fwd_delay_of_xlate;
	chip->of_gpio_n_cells = 3;

	return 0;
}
#else
static int gpiochip_fwd_setup_delay_line(struct device *dev, struct gpio_chip *chip,
					 struct gpiochip_fwd *fwd)
{
	return 0;
}
#endif	/* !CONFIG_OF_GPIO */

/**
 * gpiochip_fwd_create() - Create a new GPIO forwarder
 * @dev: Parent device pointer
 * @ngpios: Number of GPIOs in the forwarder.
 * @descs: Array containing the GPIO descriptors to forward to.
 *         This array must contain @ngpios entries, and must not be deallocated
 *         before the forwarder has been destroyed again.
 * @features: Bitwise ORed features as defined with FWD_FEATURE_*.
 *
 * This function creates a new gpiochip, which forwards all GPIO operations to
 * the passed GPIO descriptors.
 *
 * Return: An opaque object pointer, or an ERR_PTR()-encoded negative error
 *         code on failure.
 */
static struct gpiochip_fwd *gpiochip_fwd_create(struct device *dev,
						unsigned int ngpios,
						struct gpio_desc *descs[],
						unsigned long features)
{
	const char *label = dev_name(dev);
	struct gpiochip_fwd *fwd;
	struct gpio_chip *chip;
	unsigned int i;
	int error;

	fwd = devm_kzalloc(dev, struct_size(fwd, tmp, fwd_tmp_size(ngpios)),
			   GFP_KERNEL);
	if (!fwd)
		return ERR_PTR(-ENOMEM);

	chip = &fwd->chip;

	/*
	 * If any of the GPIO lines are sleeping, then the entire forwarder
	 * will be sleeping.
	 * If any of the chips support .set_config(), then the forwarder will
	 * support setting configs.
	 */
	for (i = 0; i < ngpios; i++) {
		struct gpio_chip *parent = gpiod_to_chip(descs[i]);

		dev_dbg(dev, "%u => gpio %d irq %d\n", i,
			desc_to_gpio(descs[i]), gpiod_to_irq(descs[i]));

		if (gpiod_cansleep(descs[i]))
			chip->can_sleep = true;
		if (parent && parent->set_config)
			chip->set_config = gpio_fwd_set_config;
	}

	chip->label = label;
	chip->parent = dev;
	chip->owner = THIS_MODULE;
	chip->get_direction = gpio_fwd_get_direction;
	chip->direction_input = gpio_fwd_direction_input;
	chip->direction_output = gpio_fwd_direction_output;
	chip->get = gpio_fwd_get;
	chip->get_multiple = gpio_fwd_get_multiple_locked;
	chip->set = gpio_fwd_set;
	chip->set_multiple = gpio_fwd_set_multiple_locked;
	chip->to_irq = gpio_fwd_to_irq;
	chip->base = -1;
	chip->ngpio = ngpios;
	fwd->descs = descs;

	if (chip->can_sleep)
		mutex_init(&fwd->mlock);
	else
		spin_lock_init(&fwd->slock);

	if (features & FWD_FEATURE_DELAY) {
		error = gpiochip_fwd_setup_delay_line(dev, chip, fwd);
		if (error)
			return ERR_PTR(error);
	}

	error = devm_gpiochip_add_data(dev, chip, fwd);
	if (error)
		return ERR_PTR(error);

	return fwd;
}


/*
 *  GPIO Aggregator platform device
 */

static int gpio_aggregator_probe(struct platform_device *pdev)
{
	struct device *dev = &pdev->dev;
	struct gpio_desc **descs;
	struct gpiochip_fwd *fwd;
	unsigned long features;
	int i, n;

	n = gpiod_count(dev, NULL);
	if (n < 0)
		return n;

	descs = devm_kmalloc_array(dev, n, sizeof(*descs), GFP_KERNEL);
	if (!descs)
		return -ENOMEM;

	for (i = 0; i < n; i++) {
		descs[i] = devm_gpiod_get_index(dev, NULL, i, GPIOD_ASIS);
		if (IS_ERR(descs[i]))
			return PTR_ERR(descs[i]);
	}

	features = (uintptr_t)device_get_match_data(dev);
	fwd = gpiochip_fwd_create(dev, n, descs, features);
	if (IS_ERR(fwd))
		return PTR_ERR(fwd);

	platform_set_drvdata(pdev, fwd);
	return 0;
}

static const struct of_device_id gpio_aggregator_dt_ids[] = {
	{
		.compatible = "gpio-delay",
		.data = (void *)FWD_FEATURE_DELAY,
	},
	/*
	 * Add GPIO-operated devices controlled from userspace below,
	 * or use "driver_override" in sysfs.
	 */
	{}
};
MODULE_DEVICE_TABLE(of, gpio_aggregator_dt_ids);

static struct platform_driver gpio_aggregator_driver = {
	.probe = gpio_aggregator_probe,
	.driver = {
		.name = DRV_NAME,
		.groups = gpio_aggregator_groups,
		.of_match_table = gpio_aggregator_dt_ids,
	},
};

static int __init gpio_aggregator_init(void)
{
	return platform_driver_register(&gpio_aggregator_driver);
}
module_init(gpio_aggregator_init);

static void __exit gpio_aggregator_exit(void)
{
	gpio_aggregator_remove_all();
	platform_driver_unregister(&gpio_aggregator_driver);
}
module_exit(gpio_aggregator_exit);

MODULE_AUTHOR("Geert Uytterhoeven <geert+renesas@glider.be>");
MODULE_DESCRIPTION("GPIO Aggregator");
MODULE_LICENSE("GPL v2");
Commit	Line	Data
828546e2 GU	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>
b466622c	13	#include <linux/delay.h>
828546e2 GU	14	#include <linux/idr.h>
828546e2 GU	15	#include <linux/kernel.h>
b89a9e98	16	#include <linux/mod_devicetable.h>
828546e2 GU	17	#include <linux/module.h>
	18	#include <linux/mutex.h>
	19	#include <linux/overflow.h>
	20	#include <linux/platform_device.h>
81674bea	21	#include <linux/property.h>
39ebbd52	22	#include <linux/slab.h>
828546e2 GU	23	#include <linux/spinlock.h>
	24	#include <linux/string.h>
	25
39ebbd52 AS	26	#include <linux/gpio/consumer.h>
	27	#include <linux/gpio/driver.h>
	28	#include <linux/gpio/machine.h>
	29
95b39792	30	#define AGGREGATOR_MAX_GPIOS 512
828546e2 GU	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
828546e2 GU	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
b2498cb8	55	lookups->table[n] = GPIO_LOOKUP_IDX(key, hwnum, NULL, n, 0);
828546e2 GU	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	{
ac505b6f	66	char *args = skip_spaces(aggr->args);
deb631c4	67	char name, offsets, *p;
ec75039d GU	68	unsigned long *bitmap;
ec75039d GU	69	unsigned int i, n = 0;
ec75039d GU	70	int error = 0;
ec75039d GU	71
95b39792	72	bitmap = bitmap_alloc(AGGREGATOR_MAX_GPIOS, GFP_KERNEL);
ec75039d GU	73	if (!bitmap)
ec75039d GU	74	return -ENOMEM;
828546e2	75
ac505b6f AS	76	args = next_arg(args, &name, &p);
	77	while (*args) {
	78	args = next_arg(args, &offsets, &p);
828546e2	79
deb631c4 AS	80	p = get_options(offsets, 0, &error);
deb631c4 AS	81	if (error == 0 \|\| *p) {
828546e2 GU	82	/* Named GPIO line */
	83	error = aggr_add_gpio(aggr, name, U16_MAX, &n);
	84	if (error)
ec75039d	85	goto free_bitmap;
828546e2 GU	86
	87	name = offsets;
	88	continue;
	89	}
	90
	91	/* GPIO chip + offset(s) */
95b39792	92	error = bitmap_parselist(offsets, bitmap, AGGREGATOR_MAX_GPIOS);
ec75039d GU	93	if (error) {
	94	pr_err("Cannot parse %s: %d\n", offsets, error);
	95	goto free_bitmap;
	96	}
	97
95b39792	98	for_each_set_bit(i, bitmap, AGGREGATOR_MAX_GPIOS) {
ec75039d GU	99	error = aggr_add_gpio(aggr, name, i, &n);
	100	if (error)
	101	goto free_bitmap;
828546e2 GU	102	}
828546e2 GU	103
ac505b6f	104	args = next_arg(args, &name, &p);
828546e2 GU	105	}
	106
	107	if (!n) {
	108	pr_err("No GPIOs specified\n");
ec75039d	109	error = -EINVAL;
828546e2 GU	110	}
828546e2 GU	111
ec75039d GU	112	free_bitmap:
	113	bitmap_free(bitmap);
	114	return error;
828546e2 GU	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,
6e004a98	222	NULL
828546e2 GU	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
b466622c AS	245	struct gpiochip_fwd_timing {
	246	u32 ramp_up_us;
	247	u32 ramp_down_us;
	248	};
	249
828546e2 GU	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	};
b466622c	257	struct gpiochip_fwd_timing *delay_timings;
828546e2 GU	258	unsigned long tmp[]; /* values and descs for multiple ops */
	259	};
	260
aa4858eb GU	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
828546e2 GU	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
2cba0545 GU	292	return chip->can_sleep ? gpiod_get_value_cansleep(fwd->descs[offset])
2cba0545 GU	293	: gpiod_get_value(fwd->descs[offset]);
828546e2 GU	294	}
828546e2 GU	295
01e8d85b	296	static int gpio_fwd_get_multiple(struct gpiochip_fwd fwd, unsigned long mask,
828546e2 GU	297	unsigned long *bits)
828546e2 GU	298	{
aa4858eb GU	299	struct gpio_desc **descs = fwd_tmp_descs(fwd);
aa4858eb GU	300	unsigned long *values = fwd_tmp_values(fwd);
828546e2 GU	301	unsigned int i, j = 0;
	302	int error;
	303
828546e2 GU	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
2cba0545 GU	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);
01e8d85b AS	312	if (error)
01e8d85b AS	313	return error;
828546e2	314
01e8d85b AS	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);
828546e2	332	mutex_unlock(&fwd->mlock);
01e8d85b AS	333	} else {
	334	spin_lock_irqsave(&fwd->slock, flags);
	335	error = gpio_fwd_get_multiple(fwd, mask, bits);
828546e2	336	spin_unlock_irqrestore(&fwd->slock, flags);
01e8d85b	337	}
828546e2 GU	338
	339	return error;
	340	}
	341
b466622c AS	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
828546e2 GU	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
2cba0545 GU	367	if (chip->can_sleep)
	368	gpiod_set_value_cansleep(fwd->descs[offset], value);
	369	else
	370	gpiod_set_value(fwd->descs[offset], value);
b466622c AS	371
	372	if (fwd->delay_timings)
	373	gpio_fwd_delay(chip, offset, value);
828546e2 GU	374	}
828546e2 GU	375
01e8d85b	376	static void gpio_fwd_set_multiple(struct gpiochip_fwd fwd, unsigned long mask,
828546e2 GU	377	unsigned long *bits)
828546e2 GU	378	{
aa4858eb GU	379	struct gpio_desc **descs = fwd_tmp_descs(fwd);
aa4858eb GU	380	unsigned long *values = fwd_tmp_values(fwd);
828546e2 GU	381	unsigned int i, j = 0;
828546e2 GU	382
828546e2 GU	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
2cba0545 GU	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);
01e8d85b	392	}
828546e2	393
01e8d85b AS	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);
828546e2	403	mutex_unlock(&fwd->mlock);
01e8d85b AS	404	} else {
	405	spin_lock_irqsave(&fwd->slock, flags);
	406	gpio_fwd_set_multiple(fwd, mask, bits);
828546e2	407	spin_unlock_irqrestore(&fwd->slock, flags);
01e8d85b	408	}
828546e2 GU	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
a00128df GU	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
81674bea AS	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
828546e2 GU	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.
81674bea	486	* @features: Bitwise ORed features as defined with FWD_FEATURE_*.
828546e2 GU	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,
81674bea AS	496	struct gpio_desc *descs[],
81674bea AS	497	unsigned long features)
828546e2 GU	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
aa4858eb GU	505	fwd = devm_kzalloc(dev, struct_size(fwd, tmp, fwd_tmp_size(ngpios)),
aa4858eb GU	506	GFP_KERNEL);
828546e2 GU	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
a00128df GU	521	dev_dbg(dev, "%u => gpio %d irq %d\n", i,
a00128df GU	522	desc_to_gpio(descs[i]), gpiod_to_irq(descs[i]));
828546e2 GU	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;
01e8d85b	537	chip->get_multiple = gpio_fwd_get_multiple_locked;
828546e2	538	chip->set = gpio_fwd_set;
01e8d85b	539	chip->set_multiple = gpio_fwd_set_multiple_locked;
a00128df	540	chip->to_irq = gpio_fwd_to_irq;
828546e2 GU	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
81674bea AS	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
828546e2 GU	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;
81674bea	573	unsigned long features;
828546e2 GU	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
81674bea AS	590	features = (uintptr_t)device_get_match_data(dev);
81674bea AS	591	fwd = gpiochip_fwd_create(dev, n, descs, features);
828546e2 GU	592	if (IS_ERR(fwd))
	593	return PTR_ERR(fwd);
	594
	595	platform_set_drvdata(pdev, fwd);
	596	return 0;
	597	}
	598
828546e2	599	static const struct of_device_id gpio_aggregator_dt_ids[] = {
81674bea AS	600	{
	601	.compatible = "gpio-delay",
	602	.data = (void *)FWD_FEATURE_DELAY,
	603	},
828546e2 GU	604	/*
828546e2 GU	605	* Add GPIO-operated devices controlled from userspace below,
b89a9e98	606	* or use "driver_override" in sysfs.
828546e2	607	*/
6e004a98	608	{}
828546e2 GU	609	};
828546e2 GU	610	MODULE_DEVICE_TABLE(of, gpio_aggregator_dt_ids);
828546e2 GU	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,
b89a9e98	617	.of_match_table = gpio_aggregator_dt_ids,
828546e2 GU	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");