#include <linux/cleanup.h>
#include <linux/err.h>
#include <linux/iio/consumer.h>
#include <linux/iio/iio.h>
#include <linux/mod_devicetable.h>
#include <linux/module.h>
#include <linux/mutex.h>
#include <linux/mux/consumer.h>
#include <linux/platform_device.h>
#include <linux/property.h>
struct mux_ext_info_cache {
char *data;
ssize_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_chan_spec *chan;
struct iio_chan_spec_ext_info *ext_info;
struct mux_child *child;
u32 delay_us;
};
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_delay(mux->control, chan->channel,
mux->delay_us);
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,
};
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;
if (len >= PAGE_SIZE)
return -EINVAL;
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_chan_ext_info(struct device *dev, struct mux *mux,
int idx, int num_ext_info)
{
struct mux_child *child = &mux->child[idx];
struct iio_chan_spec const *pchan = mux->parent->channel;
int i;
int ret;
char *page __free(kfree) = kzalloc(PAGE_SIZE, GFP_KERNEL);
if (!page)
return -ENOMEM;
child->ext_info_cache = devm_kcalloc(dev,
num_ext_info,
sizeof(*child->ext_info_cache),
GFP_KERNEL);
if (!child->ext_info_cache)
return -ENOMEM;
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);
if (!child->ext_info_cache[i].data)
return -ENOMEM;
child->ext_info_cache[i].data[ret] = 0;
child->ext_info_cache[i].size = ret;
}
return 0;
}
static int mux_configure_channel(struct device *dev, struct mux *mux, u32 state,
const char *label, int idx)
{
struct iio_chan_spec *chan = &mux->chan[idx];
struct iio_chan_spec const *pchan = mux->parent->channel;
int num_ext_info;
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(mux->control)) {
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)
return mux_configure_chan_ext_info(dev, mux, idx, num_ext_info);
return 0;
}
static int mux_probe(struct platform_device *pdev)
{
struct device *dev = &pdev->dev;
struct iio_dev *indio_dev;
struct iio_channel *parent;
struct mux *mux;
const char **labels;
int all_children;
int children;
u32 state;
int sizeof_ext_info;
int sizeof_priv;
int i;
int ret;
parent = devm_iio_channel_get(dev, "parent");
if (IS_ERR(parent))
return dev_err_probe(dev, PTR_ERR(parent),
"failed to get parent channel\n");
sizeof_ext_info = iio_get_channel_ext_info_count(parent);
if (sizeof_ext_info) {
sizeof_ext_info += 1;
sizeof_ext_info *= sizeof(*mux->ext_info);
}
all_children = device_property_string_array_count(dev, "channels");
if (all_children < 0)
return all_children;
labels = devm_kmalloc_array(dev, all_children, sizeof(*labels), GFP_KERNEL);
if (!labels)
return -ENOMEM;
ret = device_property_read_string_array(dev, "channels", labels, all_children);
if (ret < 0)
return ret;
children = 0;
for (state = 0; state < all_children; state++) {
if (*labels[state])
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;
mux->delay_us = 0;
device_property_read_u32(dev, "settle-time-us", &mux->delay_us);
indio_dev->name = dev_name(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))
return dev_err_probe(dev, PTR_ERR(mux->control),
"failed to get control-mux\n");
i = 0;
for (state = 0; state < all_children; state++) {
if (!*labels[state])
continue;
ret = mux_configure_channel(dev, mux, state, labels[state], 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 = "io-channel-mux" },
{ }
};
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");
