drivers/iio/multiplexer/iio-mux.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * IIO multiplexer driver
   4  *
   5  * Copyright (C) 2017 Axentia Technologies AB
   6  *
   7  * Author: Peter Rosin <peda@axentia.se>
   8  */
   9
  10 #include <linux/err.h>
  11 #include <linux/iio/consumer.h>
  12 #include <linux/iio/iio.h>
  13 #include <linux/mod_devicetable.h>
  14 #include <linux/module.h>
  15 #include <linux/mutex.h>
  16 #include <linux/mux/consumer.h>
  17 #include <linux/platform_device.h>
  18 #include <linux/property.h>
  19
  20 struct mux_ext_info_cache {
  21         char *data;
  22         ssize_t size;
  23 };
  24
  25 struct mux_child {
  26         struct mux_ext_info_cache *ext_info_cache;
  27 };
  28
  29 struct mux {
  30         int cached_state;
  31         struct mux_control *control;
  32         struct iio_channel *parent;
  33         struct iio_chan_spec *chan;
  34         struct iio_chan_spec_ext_info *ext_info;
  35         struct mux_child *child;
  36         u32 delay_us;
  37 };
  38
  39 static int iio_mux_select(struct mux *mux, int idx)
  40 {
  41         struct mux_child *child = &mux->child[idx];
  42         struct iio_chan_spec const *chan = &mux->chan[idx];
  43         int ret;
  44         int i;
  45
  46         ret = mux_control_select_delay(mux->control, chan->channel,
  47                                        mux->delay_us);
  48         if (ret < 0) {
  49                 mux->cached_state = -1;
  50                 return ret;
  51         }
  52
  53         if (mux->cached_state == chan->channel)
  54                 return 0;
  55
  56         if (chan->ext_info) {
  57                 for (i = 0; chan->ext_info[i].name; ++i) {
  58                         const char *attr = chan->ext_info[i].name;
  59                         struct mux_ext_info_cache *cache;
  60
  61                         cache = &child->ext_info_cache[i];
  62
  63                         if (cache->size < 0)
  64                                 continue;
  65
  66                         ret = iio_write_channel_ext_info(mux->parent, attr,
  67                                                          cache->data,
  68                                                          cache->size);
  69
  70                         if (ret < 0) {
  71                                 mux_control_deselect(mux->control);
  72                                 mux->cached_state = -1;
  73                                 return ret;
  74                         }
  75                 }
  76         }
  77         mux->cached_state = chan->channel;
  78
  79         return 0;
  80 }
  81
  82 static void iio_mux_deselect(struct mux *mux)
  83 {
  84         mux_control_deselect(mux->control);
  85 }
  86
  87 static int mux_read_raw(struct iio_dev *indio_dev,
  88                         struct iio_chan_spec const *chan,
  89                         int *val, int *val2, long mask)
  90 {
  91         struct mux *mux = iio_priv(indio_dev);
  92         int idx = chan - mux->chan;
  93         int ret;
  94
  95         ret = iio_mux_select(mux, idx);
  96         if (ret < 0)
  97                 return ret;
  98
  99         switch (mask) {
 100         case IIO_CHAN_INFO_RAW:
 101                 ret = iio_read_channel_raw(mux->parent, val);
 102                 break;
 103
 104         case IIO_CHAN_INFO_SCALE:
 105                 ret = iio_read_channel_scale(mux->parent, val, val2);
 106                 break;
 107
 108         default:
 109                 ret = -EINVAL;
 110         }
 111
 112         iio_mux_deselect(mux);
 113
 114         return ret;
 115 }
 116
 117 static int mux_read_avail(struct iio_dev *indio_dev,
 118                           struct iio_chan_spec const *chan,
 119                           const int **vals, int *type, int *length,
 120                           long mask)
 121 {
 122         struct mux *mux = iio_priv(indio_dev);
 123         int idx = chan - mux->chan;
 124         int ret;
 125
 126         ret = iio_mux_select(mux, idx);
 127         if (ret < 0)
 128                 return ret;
 129
 130         switch (mask) {
 131         case IIO_CHAN_INFO_RAW:
 132                 *type = IIO_VAL_INT;
 133                 ret = iio_read_avail_channel_raw(mux->parent, vals, length);
 134                 break;
 135
 136         default:
 137                 ret = -EINVAL;
 138         }
 139
 140         iio_mux_deselect(mux);
 141
 142         return ret;
 143 }
 144
 145 static int mux_write_raw(struct iio_dev *indio_dev,
 146                          struct iio_chan_spec const *chan,
 147                          int val, int val2, long mask)
 148 {
 149         struct mux *mux = iio_priv(indio_dev);
 150         int idx = chan - mux->chan;
 151         int ret;
 152
 153         ret = iio_mux_select(mux, idx);
 154         if (ret < 0)
 155                 return ret;
 156
 157         switch (mask) {
 158         case IIO_CHAN_INFO_RAW:
 159                 ret = iio_write_channel_raw(mux->parent, val);
 160                 break;
 161
 162         default:
 163                 ret = -EINVAL;
 164         }
 165
 166         iio_mux_deselect(mux);
 167
 168         return ret;
 169 }
 170
 171 static const struct iio_info mux_info = {
 172         .read_raw = mux_read_raw,
 173         .read_avail = mux_read_avail,
 174         .write_raw = mux_write_raw,
 175 };
 176
 177 static ssize_t mux_read_ext_info(struct iio_dev *indio_dev, uintptr_t private,
 178                                  struct iio_chan_spec const *chan, char *buf)
 179 {
 180         struct mux *mux = iio_priv(indio_dev);
 181         int idx = chan - mux->chan;
 182         ssize_t ret;
 183
 184         ret = iio_mux_select(mux, idx);
 185         if (ret < 0)
 186                 return ret;
 187
 188         ret = iio_read_channel_ext_info(mux->parent,
 189                                         mux->ext_info[private].name,
 190                                         buf);
 191
 192         iio_mux_deselect(mux);
 193
 194         return ret;
 195 }
 196
 197 static ssize_t mux_write_ext_info(struct iio_dev *indio_dev, uintptr_t private,
 198                                   struct iio_chan_spec const *chan,
 199                                   const char *buf, size_t len)
 200 {
 201         struct device *dev = indio_dev->dev.parent;
 202         struct mux *mux = iio_priv(indio_dev);
 203         int idx = chan - mux->chan;
 204         char *new;
 205         ssize_t ret;
 206
 207         if (len >= PAGE_SIZE)
 208                 return -EINVAL;
 209
 210         ret = iio_mux_select(mux, idx);
 211         if (ret < 0)
 212                 return ret;
 213
 214         new = devm_kmemdup(dev, buf, len + 1, GFP_KERNEL);
 215         if (!new) {
 216                 iio_mux_deselect(mux);
 217                 return -ENOMEM;
 218         }
 219
 220         new[len] = 0;
 221
 222         ret = iio_write_channel_ext_info(mux->parent,
 223                                          mux->ext_info[private].name,
 224                                          buf, len);
 225         if (ret < 0) {
 226                 iio_mux_deselect(mux);
 227                 devm_kfree(dev, new);
 228                 return ret;
 229         }
 230
 231         devm_kfree(dev, mux->child[idx].ext_info_cache[private].data);
 232         mux->child[idx].ext_info_cache[private].data = new;
 233         mux->child[idx].ext_info_cache[private].size = len;
 234
 235         iio_mux_deselect(mux);
 236
 237         return ret;
 238 }
 239
 240 static int mux_configure_channel(struct device *dev, struct mux *mux,
 241                                  u32 state, const char *label, int idx)
 242 {
 243         struct mux_child *child = &mux->child[idx];
 244         struct iio_chan_spec *chan = &mux->chan[idx];
 245         struct iio_chan_spec const *pchan = mux->parent->channel;
 246         char *page = NULL;
 247         int num_ext_info;
 248         int i;
 249         int ret;
 250
 251         chan->indexed = 1;
 252         chan->output = pchan->output;
 253         chan->datasheet_name = label;
 254         chan->ext_info = mux->ext_info;
 255
 256         ret = iio_get_channel_type(mux->parent, &chan->type);
 257         if (ret < 0) {
 258                 dev_err(dev, "failed to get parent channel type\n");
 259                 return ret;
 260         }
 261
 262         if (iio_channel_has_info(pchan, IIO_CHAN_INFO_RAW))
 263                 chan->info_mask_separate |= BIT(IIO_CHAN_INFO_RAW);
 264         if (iio_channel_has_info(pchan, IIO_CHAN_INFO_SCALE))
 265                 chan->info_mask_separate |= BIT(IIO_CHAN_INFO_SCALE);
 266
 267         if (iio_channel_has_available(pchan, IIO_CHAN_INFO_RAW))
 268                 chan->info_mask_separate_available |= BIT(IIO_CHAN_INFO_RAW);
 269
 270         if (state >= mux_control_states(mux->control)) {
 271                 dev_err(dev, "too many channels\n");
 272                 return -EINVAL;
 273         }
 274
 275         chan->channel = state;
 276
 277         num_ext_info = iio_get_channel_ext_info_count(mux->parent);
 278         if (num_ext_info) {
 279                 page = devm_kzalloc(dev, PAGE_SIZE, GFP_KERNEL);
 280                 if (!page)
 281                         return -ENOMEM;
 282         }
 283         child->ext_info_cache = devm_kcalloc(dev,
 284                                              num_ext_info,
 285                                              sizeof(*child->ext_info_cache),
 286                                              GFP_KERNEL);
 287         if (!child->ext_info_cache)
 288                 return -ENOMEM;
 289
 290         for (i = 0; i < num_ext_info; ++i) {
 291                 child->ext_info_cache[i].size = -1;
 292
 293                 if (!pchan->ext_info[i].write)
 294                         continue;
 295                 if (!pchan->ext_info[i].read)
 296                         continue;
 297
 298                 ret = iio_read_channel_ext_info(mux->parent,
 299                                                 mux->ext_info[i].name,
 300                                                 page);
 301                 if (ret < 0) {
 302                         dev_err(dev, "failed to get ext_info '%s'\n",
 303                                 pchan->ext_info[i].name);
 304                         return ret;
 305                 }
 306                 if (ret >= PAGE_SIZE) {
 307                         dev_err(dev, "too large ext_info '%s'\n",
 308                                 pchan->ext_info[i].name);
 309                         return -EINVAL;
 310                 }
 311
 312                 child->ext_info_cache[i].data = devm_kmemdup(dev, page, ret + 1,
 313                                                              GFP_KERNEL);
 314                 if (!child->ext_info_cache[i].data)
 315                         return -ENOMEM;
 316
 317                 child->ext_info_cache[i].data[ret] = 0;
 318                 child->ext_info_cache[i].size = ret;
 319         }
 320
 321         if (page)
 322                 devm_kfree(dev, page);
 323
 324         return 0;
 325 }
 326
 327 static int mux_probe(struct platform_device *pdev)
 328 {
 329         struct device *dev = &pdev->dev;
 330         struct iio_dev *indio_dev;
 331         struct iio_channel *parent;
 332         struct mux *mux;
 333         const char **labels;
 334         int all_children;
 335         int children;
 336         u32 state;
 337         int sizeof_ext_info;
 338         int sizeof_priv;
 339         int i;
 340         int ret;
 341
 342         parent = devm_iio_channel_get(dev, "parent");
 343         if (IS_ERR(parent))
 344                 return dev_err_probe(dev, PTR_ERR(parent),
 345                                      "failed to get parent channel\n");
 346
 347         sizeof_ext_info = iio_get_channel_ext_info_count(parent);
 348         if (sizeof_ext_info) {
 349                 sizeof_ext_info += 1; /* one extra entry for the sentinel */
 350                 sizeof_ext_info *= sizeof(*mux->ext_info);
 351         }
 352
 353         all_children = device_property_string_array_count(dev, "channels");
 354         if (all_children < 0)
 355                 return all_children;
 356
 357         labels = devm_kmalloc_array(dev, all_children, sizeof(*labels), GFP_KERNEL);
 358         if (!labels)
 359                 return -ENOMEM;
 360
 361         ret = device_property_read_string_array(dev, "channels", labels, all_children);
 362         if (ret < 0)
 363                 return ret;
 364
 365         children = 0;
 366         for (state = 0; state < all_children; state++) {
 367                 if (*labels[state])
 368                         children++;
 369         }
 370         if (children <= 0) {
 371                 dev_err(dev, "not even a single child\n");
 372                 return -EINVAL;
 373         }
 374
 375         sizeof_priv = sizeof(*mux);
 376         sizeof_priv += sizeof(*mux->child) * children;
 377         sizeof_priv += sizeof(*mux->chan) * children;
 378         sizeof_priv += sizeof_ext_info;
 379
 380         indio_dev = devm_iio_device_alloc(dev, sizeof_priv);
 381         if (!indio_dev)
 382                 return -ENOMEM;
 383
 384         mux = iio_priv(indio_dev);
 385         mux->child = (struct mux_child *)(mux + 1);
 386         mux->chan = (struct iio_chan_spec *)(mux->child + children);
 387
 388         platform_set_drvdata(pdev, indio_dev);
 389
 390         mux->parent = parent;
 391         mux->cached_state = -1;
 392
 393         mux->delay_us = 0;
 394         device_property_read_u32(dev, "settle-time-us", &mux->delay_us);
 395
 396         indio_dev->name = dev_name(dev);
 397         indio_dev->info = &mux_info;
 398         indio_dev->modes = INDIO_DIRECT_MODE;
 399         indio_dev->channels = mux->chan;
 400         indio_dev->num_channels = children;
 401         if (sizeof_ext_info) {
 402                 mux->ext_info = devm_kmemdup(dev,
 403                                              parent->channel->ext_info,
 404                                              sizeof_ext_info, GFP_KERNEL);
 405                 if (!mux->ext_info)
 406                         return -ENOMEM;
 407
 408                 for (i = 0; mux->ext_info[i].name; ++i) {
 409                         if (parent->channel->ext_info[i].read)
 410                                 mux->ext_info[i].read = mux_read_ext_info;
 411                         if (parent->channel->ext_info[i].write)
 412                                 mux->ext_info[i].write = mux_write_ext_info;
 413                         mux->ext_info[i].private = i;
 414                 }
 415         }
 416
 417         mux->control = devm_mux_control_get(dev, NULL);
 418         if (IS_ERR(mux->control))
 419                 return dev_err_probe(dev, PTR_ERR(mux->control),
 420                                      "failed to get control-mux\n");
 421
 422         i = 0;
 423         for (state = 0; state < all_children; state++) {
 424                 if (!*labels[state])
 425                         continue;
 426
 427                 ret = mux_configure_channel(dev, mux, state, labels[state], i++);
 428                 if (ret < 0)
 429                         return ret;
 430         }
 431
 432         ret = devm_iio_device_register(dev, indio_dev);
 433         if (ret) {
 434                 dev_err(dev, "failed to register iio device\n");
 435                 return ret;
 436         }
 437
 438         return 0;
 439 }
 440
 441 static const struct of_device_id mux_match[] = {
 442         { .compatible = "io-channel-mux" },
 443         { /* sentinel */ }
 444 };
 445 MODULE_DEVICE_TABLE(of, mux_match);
 446
 447 static struct platform_driver mux_driver = {
 448         .probe = mux_probe,
 449         .driver = {
 450                 .name = "iio-mux",
 451                 .of_match_table = mux_match,
 452         },
 453 };
 454 module_platform_driver(mux_driver);
 455
 456 MODULE_DESCRIPTION("IIO multiplexer driver");
 457 MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
 458 MODULE_LICENSE("GPL v2");