drivers/iio/multiplexer/iio-mux.c

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