iio/multiplexer/iio-mux.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * IIO multiplexer driver
0004  *
0005  * Copyright (C) 2017 Axentia Technologies AB
0006  *
0007  * Author: Peter Rosin <peda@axentia.se>
0008  */
0009
0010 #include <linux/err.h>
0011 #include <linux/iio/consumer.h>
0012 #include <linux/iio/iio.h>
0013 #include <linux/mod_devicetable.h>
0014 #include <linux/module.h>
0015 #include <linux/mutex.h>
0016 #include <linux/mux/consumer.h>
0017 #include <linux/platform_device.h>
0018 #include <linux/property.h>
0019
0020 struct mux_ext_info_cache {
0021     char *data;
0022     ssize_t size;
0023 };
0024
0025 struct mux_child {
0026     struct mux_ext_info_cache *ext_info_cache;
0027 };
0028
0029 struct mux {
0030     int cached_state;
0031     struct mux_control *control;
0032     struct iio_channel *parent;
0033     struct iio_dev *indio_dev;
0034     struct iio_chan_spec *chan;
0035     struct iio_chan_spec_ext_info *ext_info;
0036     struct mux_child *child;
0037     u32 delay_us;
0038 };
0039
0040 static int iio_mux_select(struct mux *mux, int idx)
0041 {
0042     struct mux_child *child = &mux->child[idx];
0043     struct iio_chan_spec const *chan = &mux->chan[idx];
0044     int ret;
0045     int i;
0046
0047     ret = mux_control_select_delay(mux->control, chan->channel,
0048                        mux->delay_us);
0049     if (ret < 0) {
0050         mux->cached_state = -1;
0051         return ret;
0052     }
0053
0054     if (mux->cached_state == chan->channel)
0055         return 0;
0056
0057     if (chan->ext_info) {
0058         for (i = 0; chan->ext_info[i].name; ++i) {
0059             const char *attr = chan->ext_info[i].name;
0060             struct mux_ext_info_cache *cache;
0061
0062             cache = &child->ext_info_cache[i];
0063
0064             if (cache->size < 0)
0065                 continue;
0066
0067             ret = iio_write_channel_ext_info(mux->parent, attr,
0068                              cache->data,
0069                              cache->size);
0070
0071             if (ret < 0) {
0072                 mux_control_deselect(mux->control);
0073                 mux->cached_state = -1;
0074                 return ret;
0075             }
0076         }
0077     }
0078     mux->cached_state = chan->channel;
0079
0080     return 0;
0081 }
0082
0083 static void iio_mux_deselect(struct mux *mux)
0084 {
0085     mux_control_deselect(mux->control);
0086 }
0087
0088 static int mux_read_raw(struct iio_dev *indio_dev,
0089             struct iio_chan_spec const *chan,
0090             int *val, int *val2, long mask)
0091 {
0092     struct mux *mux = iio_priv(indio_dev);
0093     int idx = chan - mux->chan;
0094     int ret;
0095
0096     ret = iio_mux_select(mux, idx);
0097     if (ret < 0)
0098         return ret;
0099
0100     switch (mask) {
0101     case IIO_CHAN_INFO_RAW:
0102         ret = iio_read_channel_raw(mux->parent, val);
0103         break;
0104
0105     case IIO_CHAN_INFO_SCALE:
0106         ret = iio_read_channel_scale(mux->parent, val, val2);
0107         break;
0108
0109     default:
0110         ret = -EINVAL;
0111     }
0112
0113     iio_mux_deselect(mux);
0114
0115     return ret;
0116 }
0117
0118 static int mux_read_avail(struct iio_dev *indio_dev,
0119               struct iio_chan_spec const *chan,
0120               const int **vals, int *type, int *length,
0121               long mask)
0122 {
0123     struct mux *mux = iio_priv(indio_dev);
0124     int idx = chan - mux->chan;
0125     int ret;
0126
0127     ret = iio_mux_select(mux, idx);
0128     if (ret < 0)
0129         return ret;
0130
0131     switch (mask) {
0132     case IIO_CHAN_INFO_RAW:
0133         *type = IIO_VAL_INT;
0134         ret = iio_read_avail_channel_raw(mux->parent, vals, length);
0135         break;
0136
0137     default:
0138         ret = -EINVAL;
0139     }
0140
0141     iio_mux_deselect(mux);
0142
0143     return ret;
0144 }
0145
0146 static int mux_write_raw(struct iio_dev *indio_dev,
0147              struct iio_chan_spec const *chan,
0148              int val, int val2, long mask)
0149 {
0150     struct mux *mux = iio_priv(indio_dev);
0151     int idx = chan - mux->chan;
0152     int ret;
0153
0154     ret = iio_mux_select(mux, idx);
0155     if (ret < 0)
0156         return ret;
0157
0158     switch (mask) {
0159     case IIO_CHAN_INFO_RAW:
0160         ret = iio_write_channel_raw(mux->parent, val);
0161         break;
0162
0163     default:
0164         ret = -EINVAL;
0165     }
0166
0167     iio_mux_deselect(mux);
0168
0169     return ret;
0170 }
0171
0172 static const struct iio_info mux_info = {
0173     .read_raw = mux_read_raw,
0174     .read_avail = mux_read_avail,
0175     .write_raw = mux_write_raw,
0176 };
0177
0178 static ssize_t mux_read_ext_info(struct iio_dev *indio_dev, uintptr_t private,
0179                  struct iio_chan_spec const *chan, char *buf)
0180 {
0181     struct mux *mux = iio_priv(indio_dev);
0182     int idx = chan - mux->chan;
0183     ssize_t ret;
0184
0185     ret = iio_mux_select(mux, idx);
0186     if (ret < 0)
0187         return ret;
0188
0189     ret = iio_read_channel_ext_info(mux->parent,
0190                     mux->ext_info[private].name,
0191                     buf);
0192
0193     iio_mux_deselect(mux);
0194
0195     return ret;
0196 }
0197
0198 static ssize_t mux_write_ext_info(struct iio_dev *indio_dev, uintptr_t private,
0199                   struct iio_chan_spec const *chan,
0200                   const char *buf, size_t len)
0201 {
0202     struct device *dev = indio_dev->dev.parent;
0203     struct mux *mux = iio_priv(indio_dev);
0204     int idx = chan - mux->chan;
0205     char *new;
0206     ssize_t ret;
0207
0208     if (len >= PAGE_SIZE)
0209         return -EINVAL;
0210
0211     ret = iio_mux_select(mux, idx);
0212     if (ret < 0)
0213         return ret;
0214
0215     new = devm_kmemdup(dev, buf, len + 1, GFP_KERNEL);
0216     if (!new) {
0217         iio_mux_deselect(mux);
0218         return -ENOMEM;
0219     }
0220
0221     new[len] = 0;
0222
0223     ret = iio_write_channel_ext_info(mux->parent,
0224                      mux->ext_info[private].name,
0225                      buf, len);
0226     if (ret < 0) {
0227         iio_mux_deselect(mux);
0228         devm_kfree(dev, new);
0229         return ret;
0230     }
0231
0232     devm_kfree(dev, mux->child[idx].ext_info_cache[private].data);
0233     mux->child[idx].ext_info_cache[private].data = new;
0234     mux->child[idx].ext_info_cache[private].size = len;
0235
0236     iio_mux_deselect(mux);
0237
0238     return ret;
0239 }
0240
0241 static int mux_configure_channel(struct device *dev, struct mux *mux,
0242                  u32 state, const char *label, int idx)
0243 {
0244     struct mux_child *child = &mux->child[idx];
0245     struct iio_chan_spec *chan = &mux->chan[idx];
0246     struct iio_chan_spec const *pchan = mux->parent->channel;
0247     char *page = NULL;
0248     int num_ext_info;
0249     int i;
0250     int ret;
0251
0252     chan->indexed = 1;
0253     chan->output = pchan->output;
0254     chan->datasheet_name = label;
0255     chan->ext_info = mux->ext_info;
0256
0257     ret = iio_get_channel_type(mux->parent, &chan->type);
0258     if (ret < 0) {
0259         dev_err(dev, "failed to get parent channel type\n");
0260         return ret;
0261     }
0262
0263     if (iio_channel_has_info(pchan, IIO_CHAN_INFO_RAW))
0264         chan->info_mask_separate |= BIT(IIO_CHAN_INFO_RAW);
0265     if (iio_channel_has_info(pchan, IIO_CHAN_INFO_SCALE))
0266         chan->info_mask_separate |= BIT(IIO_CHAN_INFO_SCALE);
0267
0268     if (iio_channel_has_available(pchan, IIO_CHAN_INFO_RAW))
0269         chan->info_mask_separate_available |= BIT(IIO_CHAN_INFO_RAW);
0270
0271     if (state >= mux_control_states(mux->control)) {
0272         dev_err(dev, "too many channels\n");
0273         return -EINVAL;
0274     }
0275
0276     chan->channel = state;
0277
0278     num_ext_info = iio_get_channel_ext_info_count(mux->parent);
0279     if (num_ext_info) {
0280         page = devm_kzalloc(dev, PAGE_SIZE, GFP_KERNEL);
0281         if (!page)
0282             return -ENOMEM;
0283     }
0284     child->ext_info_cache = devm_kcalloc(dev,
0285                          num_ext_info,
0286                          sizeof(*child->ext_info_cache),
0287                          GFP_KERNEL);
0288     if (!child->ext_info_cache)
0289         return -ENOMEM;
0290
0291     for (i = 0; i < num_ext_info; ++i) {
0292         child->ext_info_cache[i].size = -1;
0293
0294         if (!pchan->ext_info[i].write)
0295             continue;
0296         if (!pchan->ext_info[i].read)
0297             continue;
0298
0299         ret = iio_read_channel_ext_info(mux->parent,
0300                         mux->ext_info[i].name,
0301                         page);
0302         if (ret < 0) {
0303             dev_err(dev, "failed to get ext_info '%s'\n",
0304                 pchan->ext_info[i].name);
0305             return ret;
0306         }
0307         if (ret >= PAGE_SIZE) {
0308             dev_err(dev, "too large ext_info '%s'\n",
0309                 pchan->ext_info[i].name);
0310             return -EINVAL;
0311         }
0312
0313         child->ext_info_cache[i].data = devm_kmemdup(dev, page, ret + 1,
0314                                  GFP_KERNEL);
0315         if (!child->ext_info_cache[i].data)
0316             return -ENOMEM;
0317
0318         child->ext_info_cache[i].data[ret] = 0;
0319         child->ext_info_cache[i].size = ret;
0320     }
0321
0322     if (page)
0323         devm_kfree(dev, page);
0324
0325     return 0;
0326 }
0327
0328 static int mux_probe(struct platform_device *pdev)
0329 {
0330     struct device *dev = &pdev->dev;
0331     struct iio_dev *indio_dev;
0332     struct iio_channel *parent;
0333     struct mux *mux;
0334     const char **labels;
0335     int all_children;
0336     int children;
0337     u32 state;
0338     int sizeof_ext_info;
0339     int sizeof_priv;
0340     int i;
0341     int ret;
0342
0343     parent = devm_iio_channel_get(dev, "parent");
0344     if (IS_ERR(parent))
0345         return dev_err_probe(dev, PTR_ERR(parent),
0346                      "failed to get parent channel\n");
0347
0348     sizeof_ext_info = iio_get_channel_ext_info_count(parent);
0349     if (sizeof_ext_info) {
0350         sizeof_ext_info += 1; /* one extra entry for the sentinel */
0351         sizeof_ext_info *= sizeof(*mux->ext_info);
0352     }
0353
0354     all_children = device_property_string_array_count(dev, "channels");
0355     if (all_children < 0)
0356         return all_children;
0357
0358     labels = devm_kmalloc_array(dev, all_children, sizeof(*labels), GFP_KERNEL);
0359     if (!labels)
0360         return -ENOMEM;
0361
0362     ret = device_property_read_string_array(dev, "channels", labels, all_children);
0363     if (ret < 0)
0364         return ret;
0365
0366     children = 0;
0367     for (state = 0; state < all_children; state++) {
0368         if (*labels[state])
0369             children++;
0370     }
0371     if (children <= 0) {
0372         dev_err(dev, "not even a single child\n");
0373         return -EINVAL;
0374     }
0375
0376     sizeof_priv = sizeof(*mux);
0377     sizeof_priv += sizeof(*mux->child) * children;
0378     sizeof_priv += sizeof(*mux->chan) * children;
0379     sizeof_priv += sizeof_ext_info;
0380
0381     indio_dev = devm_iio_device_alloc(dev, sizeof_priv);
0382     if (!indio_dev)
0383         return -ENOMEM;
0384
0385     mux = iio_priv(indio_dev);
0386     mux->child = (struct mux_child *)(mux + 1);
0387     mux->chan = (struct iio_chan_spec *)(mux->child + children);
0388
0389     platform_set_drvdata(pdev, indio_dev);
0390
0391     mux->parent = parent;
0392     mux->cached_state = -1;
0393
0394     mux->delay_us = 0;
0395     device_property_read_u32(dev, "settle-time-us", &mux->delay_us);
0396
0397     indio_dev->name = dev_name(dev);
0398     indio_dev->info = &mux_info;
0399     indio_dev->modes = INDIO_DIRECT_MODE;
0400     indio_dev->channels = mux->chan;
0401     indio_dev->num_channels = children;
0402     if (sizeof_ext_info) {
0403         mux->ext_info = devm_kmemdup(dev,
0404                          parent->channel->ext_info,
0405                          sizeof_ext_info, GFP_KERNEL);
0406         if (!mux->ext_info)
0407             return -ENOMEM;
0408
0409         for (i = 0; mux->ext_info[i].name; ++i) {
0410             if (parent->channel->ext_info[i].read)
0411                 mux->ext_info[i].read = mux_read_ext_info;
0412             if (parent->channel->ext_info[i].write)
0413                 mux->ext_info[i].write = mux_write_ext_info;
0414             mux->ext_info[i].private = i;
0415         }
0416     }
0417
0418     mux->control = devm_mux_control_get(dev, NULL);
0419     if (IS_ERR(mux->control)) {
0420         if (PTR_ERR(mux->control) != -EPROBE_DEFER)
0421             dev_err(dev, "failed to get control-mux\n");
0422         return PTR_ERR(mux->control);
0423     }
0424
0425     i = 0;
0426     for (state = 0; state < all_children; state++) {
0427         if (!*labels[state])
0428             continue;
0429
0430         ret = mux_configure_channel(dev, mux, state, labels[state], i++);
0431         if (ret < 0)
0432             return ret;
0433     }
0434
0435     ret = devm_iio_device_register(dev, indio_dev);
0436     if (ret) {
0437         dev_err(dev, "failed to register iio device\n");
0438         return ret;
0439     }
0440
0441     return 0;
0442 }
0443
0444 static const struct of_device_id mux_match[] = {
0445     { .compatible = "io-channel-mux" },
0446     { /* sentinel */ }
0447 };
0448 MODULE_DEVICE_TABLE(of, mux_match);
0449
0450 static struct platform_driver mux_driver = {
0451     .probe = mux_probe,
0452     .driver = {
0453         .name = "iio-mux",
0454         .of_match_table = mux_match,
0455     },
0456 };
0457 module_platform_driver(mux_driver);
0458
0459 MODULE_DESCRIPTION("IIO multiplexer driver");
0460 MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
0461 MODULE_LICENSE("GPL v2");