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