Re: [PATCH v6 5/9] iio: multiplexer: new iio category and iio-mux driver

[Jonathan Cameron <jic23@kernel.org>]

On 30/11/16 08:16, Peter Rosin wrote:
> When a multiplexer changes how an iio device behaves (for example
> by feeding different signals to an ADC), this driver can be used
> to create one virtual iio channel for each multiplexer state.
> 
> Depends on the generic multiplexer subsystem.
> 
> Cache any ext_info values from the parent iio channel, creating a private
> copy of the ext_info attributes for each multiplexer state/channel.
> 
> Signed-off-by: Peter Rosin <peda@axentia.se>
Other than binding naming, I'm happy with this - so pending that changing...

Reviewed-by: Jonathan Cameron <jic23@kernel.org>

If it makes sense I can obviously take this via IIO once the core is in
place.

Jonathan
> ---
>  MAINTAINERS                       |   1 +
>  drivers/iio/Kconfig               |   1 +
>  drivers/iio/Makefile              |   1 +
>  drivers/iio/multiplexer/Kconfig   |  18 ++
>  drivers/iio/multiplexer/Makefile  |   6 +
>  drivers/iio/multiplexer/iio-mux.c | 456 ++++++++++++++++++++++++++++++++++++++
>  6 files changed, 483 insertions(+)
>  create mode 100644 drivers/iio/multiplexer/Kconfig
>  create mode 100644 drivers/iio/multiplexer/Makefile
>  create mode 100644 drivers/iio/multiplexer/iio-mux.c
> 
> diff --git a/MAINTAINERS b/MAINTAINERS
> index 77045ae15865..16490fbd1721 100644
> --- a/MAINTAINERS
> +++ b/MAINTAINERS
> @@ -6239,6 +6239,7 @@ M:	Peter Rosin <peda@axentia.se>
>  L:	linux-iio@vger.kernel.org
>  S:	Maintained
>  F:	Documentation/devicetree/bindings/iio/multiplexer/iio-mux.txt
> +F:	drivers/iio/multiplexer/iio-mux.c
>  
>  IIO SUBSYSTEM AND DRIVERS
>  M:	Jonathan Cameron <jic23@kernel.org>
> diff --git a/drivers/iio/Kconfig b/drivers/iio/Kconfig
> index a918270d6f54..b3c8c6ef0dff 100644
> --- a/drivers/iio/Kconfig
> +++ b/drivers/iio/Kconfig
> @@ -83,6 +83,7 @@ source "drivers/iio/humidity/Kconfig"
>  source "drivers/iio/imu/Kconfig"
>  source "drivers/iio/light/Kconfig"
>  source "drivers/iio/magnetometer/Kconfig"
> +source "drivers/iio/multiplexer/Kconfig"
>  source "drivers/iio/orientation/Kconfig"
>  if IIO_TRIGGER
>     source "drivers/iio/trigger/Kconfig"
> diff --git a/drivers/iio/Makefile b/drivers/iio/Makefile
> index 33fa4026f92c..93c769cd99bf 100644
> --- a/drivers/iio/Makefile
> +++ b/drivers/iio/Makefile
> @@ -28,6 +28,7 @@ obj-y += humidity/
>  obj-y += imu/
>  obj-y += light/
>  obj-y += magnetometer/
> +obj-y += multiplexer/
>  obj-y += orientation/
>  obj-y += potentiometer/
>  obj-y += potentiostat/
> diff --git a/drivers/iio/multiplexer/Kconfig b/drivers/iio/multiplexer/Kconfig
> new file mode 100644
> index 000000000000..70a044510686
> --- /dev/null
> +++ b/drivers/iio/multiplexer/Kconfig
> @@ -0,0 +1,18 @@
> +#
> +# Multiplexer drivers
> +#
> +# When adding new entries keep the list in alphabetical order
> +
> +menu "Multiplexers"
> +
> +config IIO_MUX
> +	tristate "IIO multiplexer driver"
> +	select MULTIPLEXER
> +	depends on OF
> +	help
> +	  Say yes here to build support for the IIO multiplexer.
> +
> +	  To compile this driver as a module, choose M here: the
> +	  module will be called iio-mux.
> +
> +endmenu
> diff --git a/drivers/iio/multiplexer/Makefile b/drivers/iio/multiplexer/Makefile
> new file mode 100644
> index 000000000000..68be3c4abd07
> --- /dev/null
> +++ b/drivers/iio/multiplexer/Makefile
> @@ -0,0 +1,6 @@
> +#
> +# Makefile for industrial I/O multiplexer drivers
> +#
> +
> +# When adding new entries keep the list in alphabetical order
> +obj-$(CONFIG_IIO_MUX) += iio-mux.o
> diff --git a/drivers/iio/multiplexer/iio-mux.c b/drivers/iio/multiplexer/iio-mux.c
> new file mode 100644
> index 000000000000..92dfee2dfed1
> --- /dev/null
> +++ b/drivers/iio/multiplexer/iio-mux.c
> @@ -0,0 +1,456 @@
> +/*
> + * IIO multiplexer driver
> + *
> + * Copyright (C) 2016 Axentia Technologies AB
> + *
> + * Author: Peter Rosin <peda@axentia.se>
> + *
> + * This program is free software; you can redistribute it and/or modify
> + * it under the terms of the GNU General Public License version 2 as
> + * published by the Free Software Foundation.
> + */
> +
> +#include <linux/err.h>
> +#include <linux/iio/consumer.h>
> +#include <linux/iio/iio.h>
> +#include <linux/module.h>
> +#include <linux/mutex.h>
> +#include <linux/mux.h>
> +#include <linux/of.h>
> +#include <linux/platform_device.h>
> +
> +struct mux_ext_info_cache {
> +	char *data;
> +	size_t size;
> +};
> +
> +struct mux_child {
> +	struct mux_ext_info_cache *ext_info_cache;
> +};
> +
> +struct mux {
> +	int cached_state;
> +	struct mux_control *control;
> +	struct iio_channel *parent;
> +	struct iio_dev *indio_dev;
> +	struct iio_chan_spec *chan;
> +	struct iio_chan_spec_ext_info *ext_info;
> +	struct mux_child *child;
> +};
> +
> +static int iio_mux_select(struct mux *mux, int idx)
> +{
> +	struct mux_child *child = &mux->child[idx];
> +	struct iio_chan_spec const *chan = &mux->chan[idx];
> +	int ret;
> +	int i;
> +
> +	ret = mux_control_select(mux->control, chan->channel);
> +	if (ret < 0) {
> +		mux->cached_state = -1;
> +		return ret;
> +	}
> +
> +	if (mux->cached_state == chan->channel)
> +		return 0;
> +
> +	if (chan->ext_info) {
> +		for (i = 0; chan->ext_info[i].name; ++i) {
> +			const char *attr = chan->ext_info[i].name;
> +			struct mux_ext_info_cache *cache;
> +
> +			cache = &child->ext_info_cache[i];
> +
> +			if (cache->size < 0)
> +				continue;
> +
> +			ret = iio_write_channel_ext_info(mux->parent, attr,
> +							 cache->data,
> +							 cache->size);
> +
> +			if (ret < 0) {
> +				mux_control_deselect(mux->control);
> +				mux->cached_state = -1;
> +				return ret;
> +			}
> +		}
> +	}
> +	mux->cached_state = chan->channel;
> +
> +	return 0;
> +}
> +
> +static void iio_mux_deselect(struct mux *mux)
> +{
> +	mux_control_deselect(mux->control);
> +}
> +
> +static int mux_read_raw(struct iio_dev *indio_dev,
> +			struct iio_chan_spec const *chan,
> +			int *val, int *val2, long mask)
> +{
> +	struct mux *mux = iio_priv(indio_dev);
> +	int idx = chan - mux->chan;
> +	int ret;
> +
> +	ret = iio_mux_select(mux, idx);
> +	if (ret < 0)
> +		return ret;
> +
> +	switch (mask) {
> +	case IIO_CHAN_INFO_RAW:
> +		ret = iio_read_channel_raw(mux->parent, val);
> +		break;
> +
> +	case IIO_CHAN_INFO_SCALE:
> +		ret = iio_read_channel_scale(mux->parent, val, val2);
> +		break;
> +
> +	default:
> +		ret = -EINVAL;
> +	}
> +
> +	iio_mux_deselect(mux);
> +
> +	return ret;
> +}
> +
> +static int mux_read_avail(struct iio_dev *indio_dev,
> +			  struct iio_chan_spec const *chan,
> +			  const int **vals, int *type, int *length,
> +			  long mask)
> +{
> +	struct mux *mux = iio_priv(indio_dev);
> +	int idx = chan - mux->chan;
> +	int ret;
> +
> +	ret = iio_mux_select(mux, idx);
> +	if (ret < 0)
> +		return ret;
> +
> +	switch (mask) {
> +	case IIO_CHAN_INFO_RAW:
> +		*type = IIO_VAL_INT;
> +		ret = iio_read_avail_channel_raw(mux->parent, vals, length);
> +		break;
> +
> +	default:
> +		ret = -EINVAL;
> +	}
> +
> +	iio_mux_deselect(mux);
> +
> +	return ret;
> +}
> +
> +static int mux_write_raw(struct iio_dev *indio_dev,
> +			 struct iio_chan_spec const *chan,
> +			 int val, int val2, long mask)
> +{
> +	struct mux *mux = iio_priv(indio_dev);
> +	int idx = chan - mux->chan;
> +	int ret;
> +
> +	ret = iio_mux_select(mux, idx);
> +	if (ret < 0)
> +		return ret;
> +
> +	switch (mask) {
> +	case IIO_CHAN_INFO_RAW:
> +		ret = iio_write_channel_raw(mux->parent, val);
> +		break;
> +
> +	default:
> +		ret = -EINVAL;
> +	}
> +
> +	iio_mux_deselect(mux);
> +
> +	return ret;
> +}
> +
> +static const struct iio_info mux_info = {
> +	.read_raw = mux_read_raw,
> +	.read_avail = mux_read_avail,
> +	.write_raw = mux_write_raw,
> +	.driver_module = THIS_MODULE,
> +};
> +
> +static ssize_t mux_read_ext_info(struct iio_dev *indio_dev, uintptr_t private,
> +				 struct iio_chan_spec const *chan, char *buf)
> +{
> +	struct mux *mux = iio_priv(indio_dev);
> +	int idx = chan - mux->chan;
> +	ssize_t ret;
> +
> +	ret = iio_mux_select(mux, idx);
> +	if (ret < 0)
> +		return ret;
> +
> +	ret = iio_read_channel_ext_info(mux->parent,
> +					mux->ext_info[private].name,
> +					buf);
> +
> +	iio_mux_deselect(mux);
> +
> +	return ret;
> +}
> +
> +static ssize_t mux_write_ext_info(struct iio_dev *indio_dev, uintptr_t private,
> +				  struct iio_chan_spec const *chan,
> +				  const char *buf, size_t len)
> +{
> +	struct device *dev = indio_dev->dev.parent;
> +	struct mux *mux = iio_priv(indio_dev);
> +	int idx = chan - mux->chan;
> +	char *new;
> +	ssize_t ret;
> +
> +	ret = iio_mux_select(mux, idx);
> +	if (ret < 0)
> +		return ret;
> +
> +	new = devm_kmemdup(dev, buf, len + 1, GFP_KERNEL);
> +	if (!new) {
> +		iio_mux_deselect(mux);
> +		return -ENOMEM;
> +	}
> +
> +	new[len] = 0;
> +
> +	ret = iio_write_channel_ext_info(mux->parent,
> +					 mux->ext_info[private].name,
> +					 buf, len);
> +	if (ret < 0) {
> +		iio_mux_deselect(mux);
> +		devm_kfree(dev, new);
> +		return ret;
> +	}
> +
> +	devm_kfree(dev, mux->child[idx].ext_info_cache[private].data);
> +	mux->child[idx].ext_info_cache[private].data = new;
> +	mux->child[idx].ext_info_cache[private].size = len;
> +
> +	iio_mux_deselect(mux);
> +
> +	return ret;
> +}
> +
> +static int mux_configure_channel(struct device *dev, struct mux *mux,
> +				 u32 state, const char *label, int idx)
> +{
> +	struct mux_child *child = &mux->child[idx];
> +	struct iio_chan_spec *chan = &mux->chan[idx];
> +	struct iio_chan_spec const *pchan = mux->parent->channel;
> +	char *page = NULL;
> +	int num_ext_info;
> +	int i;
> +	int ret;
> +
> +	chan->indexed = 1;
> +	chan->output = pchan->output;
> +	chan->datasheet_name = label;
> +	chan->ext_info = mux->ext_info;
> +
> +	ret = iio_get_channel_type(mux->parent, &chan->type);
> +	if (ret < 0) {
> +		dev_err(dev, "failed to get parent channel type\n");
> +		return ret;
> +	}
> +
> +	if (iio_channel_has_info(pchan, IIO_CHAN_INFO_RAW))
> +		chan->info_mask_separate |= BIT(IIO_CHAN_INFO_RAW);
> +	if (iio_channel_has_info(pchan, IIO_CHAN_INFO_SCALE))
> +		chan->info_mask_separate |= BIT(IIO_CHAN_INFO_SCALE);
> +
> +	if (iio_channel_has_available(pchan, IIO_CHAN_INFO_RAW))
> +		chan->info_mask_separate_available |= BIT(IIO_CHAN_INFO_RAW);
> +
> +	if (state >= mux->control->states) {
> +		dev_err(dev, "too many channels\n");
> +		return -EINVAL;
> +	}
> +
> +	chan->channel = state;
> +
> +	num_ext_info = iio_get_channel_ext_info_count(mux->parent);
> +	if (num_ext_info) {
> +		page = devm_kzalloc(dev, PAGE_SIZE, GFP_KERNEL);
> +		if (!page)
> +			return -ENOMEM;
> +	}
> +	child->ext_info_cache = devm_kzalloc(dev,
> +					     sizeof(*child->ext_info_cache) *
> +					     num_ext_info, GFP_KERNEL);
> +	for (i = 0; i < num_ext_info; ++i) {
> +		child->ext_info_cache[i].size = -1;
> +
> +		if (!pchan->ext_info[i].write)
> +			continue;
> +		if (!pchan->ext_info[i].read)
> +			continue;
> +
> +		ret = iio_read_channel_ext_info(mux->parent,
> +						mux->ext_info[i].name,
> +						page);
> +		if (ret < 0) {
> +			dev_err(dev, "failed to get ext_info '%s'\n",
> +				pchan->ext_info[i].name);
> +			return ret;
> +		}
> +		if (ret >= PAGE_SIZE) {
> +			dev_err(dev, "too large ext_info '%s'\n",
> +				pchan->ext_info[i].name);
> +			return -EINVAL;
> +		}
> +
> +		child->ext_info_cache[i].data = devm_kmemdup(dev, page, ret + 1,
> +							     GFP_KERNEL);
> +		child->ext_info_cache[i].data[ret] = 0;
> +		child->ext_info_cache[i].size = ret;
> +	}
> +
> +	if (page)
> +		devm_kfree(dev, page);
> +
> +	return 0;
> +}
> +
> +/*
> + * Same as of_property_for_each_string(), but also keeps track of the
> + * index of each string.
> + */
> +#define of_property_for_each_string_index(np, propname, prop, s, i)	\
> +	for (prop = of_find_property(np, propname, NULL),		\
> +	     s = of_prop_next_string(prop, NULL),			\
> +	     i = 0;							\
> +	     s;								\
> +	     s = of_prop_next_string(prop, s),				\
> +	     i++)
> +
> +static int mux_probe(struct platform_device *pdev)
> +{
> +	struct device *dev = &pdev->dev;
> +	struct device_node *np = pdev->dev.of_node;
> +	struct iio_dev *indio_dev;
> +	struct iio_channel *parent;
> +	struct mux *mux;
> +	struct property *prop;
> +	const char *label;
> +	u32 state;
> +	int sizeof_ext_info;
> +	int children;
> +	int sizeof_priv;
> +	int i;
> +	int ret;
> +
> +	if (!np)
> +		return -ENODEV;
> +
> +	parent = devm_iio_channel_get(dev, "parent");
> +	if (IS_ERR(parent)) {
> +		if (PTR_ERR(parent) != -EPROBE_DEFER)
> +			dev_err(dev, "failed to get parent channel\n");
> +		return PTR_ERR(parent);
> +	}
> +
> +	sizeof_ext_info = iio_get_channel_ext_info_count(parent);
> +	if (sizeof_ext_info) {
> +		sizeof_ext_info += 1; /* one extra entry for the sentinel */
> +		sizeof_ext_info *= sizeof(*mux->ext_info);
> +	}
> +
> +	children = 0;
> +	of_property_for_each_string(np, "channels", prop, label) {
> +		if (*label)
> +			children++;
> +	}
> +	if (children <= 0) {
> +		dev_err(dev, "not even a single child\n");
> +		return -EINVAL;
> +	}
> +
> +	sizeof_priv = sizeof(*mux);
> +	sizeof_priv += sizeof(*mux->child) * children;
> +	sizeof_priv += sizeof(*mux->chan) * children;
> +	sizeof_priv += sizeof_ext_info;
> +
> +	indio_dev = devm_iio_device_alloc(dev, sizeof_priv);
> +	if (!indio_dev)
> +		return -ENOMEM;
> +
> +	mux = iio_priv(indio_dev);
> +	mux->child = (struct mux_child *)(mux + 1);
> +	mux->chan = (struct iio_chan_spec *)(mux->child + children);
> +
> +	platform_set_drvdata(pdev, indio_dev);
> +
> +	mux->parent = parent;
> +	mux->cached_state = -1;
> +
> +	indio_dev->name = dev_name(dev);
> +	indio_dev->dev.parent = dev;
> +	indio_dev->info = &mux_info;
> +	indio_dev->modes = INDIO_DIRECT_MODE;
> +	indio_dev->channels = mux->chan;
> +	indio_dev->num_channels = children;
> +	if (sizeof_ext_info) {
> +		mux->ext_info = devm_kmemdup(dev,
> +					     parent->channel->ext_info,
> +					     sizeof_ext_info, GFP_KERNEL);
> +		if (!mux->ext_info)
> +			return -ENOMEM;
> +
> +		for (i = 0; mux->ext_info[i].name; ++i) {
> +			if (parent->channel->ext_info[i].read)
> +				mux->ext_info[i].read = mux_read_ext_info;
> +			if (parent->channel->ext_info[i].write)
> +				mux->ext_info[i].write = mux_write_ext_info;
> +			mux->ext_info[i].private = i;
> +		}
> +	}
> +
> +	mux->control = devm_mux_control_get(dev, NULL);
> +	if (IS_ERR(mux->control)) {
> +		if (PTR_ERR(mux->control) != -EPROBE_DEFER)
> +			dev_err(dev, "failed to get control-mux\n");
> +		return PTR_ERR(mux->control);
> +	}
> +
> +	i = 0;
> +	of_property_for_each_string_index(np, "channels", prop, label, state) {
> +		if (!*label)
> +			continue;
> +
> +		ret = mux_configure_channel(dev, mux, state, label, i++);
> +		if (ret < 0)
> +			return ret;
> +	}
> +
> +	ret = devm_iio_device_register(dev, indio_dev);
> +	if (ret) {
> +		dev_err(dev, "failed to register iio device\n");
> +		return ret;
> +	}
> +
> +	return 0;
> +}
> +
> +static const struct of_device_id mux_match[] = {
> +	{ .compatible = "iio-mux" },
> +	{ /* sentinel */ }
> +};
> +MODULE_DEVICE_TABLE(of, mux_match);
> +
> +static struct platform_driver mux_driver = {
> +	.probe = mux_probe,
> +	.driver = {
> +		.name = "iio-mux",
> +		.of_match_table = mux_match,
> +	},
> +};
> +module_platform_driver(mux_driver);
> +
> +MODULE_DESCRIPTION("IIO multiplexer driver");
> +MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
> +MODULE_LICENSE("GPL v2");
>