drivers/iio/afe/iio-rescale.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * IIO rescale driver
   4  *
   5  * Copyright (C) 2018 Axentia Technologies AB
   6  *
   7  * Author: Peter Rosin <peda@axentia.se>
   8  */
   9
  10 #include <linux/err.h>
  11 #include <linux/gcd.h>
  12 #include <linux/iio/consumer.h>
  13 #include <linux/iio/iio.h>
  14 #include <linux/module.h>
  15 #include <linux/of.h>
  16 #include <linux/of_device.h>
  17 #include <linux/platform_device.h>
  18 #include <linux/property.h>
  19
  20 struct rescale;
  21
  22 struct rescale_cfg {
  23         enum iio_chan_type type;
  24         int (*props)(struct device *dev, struct rescale *rescale);
  25 };
  26
  27 struct rescale {
  28         const struct rescale_cfg *cfg;
  29         struct iio_channel *source;
  30         struct iio_chan_spec chan;
  31         struct iio_chan_spec_ext_info *ext_info;
  32         bool chan_processed;
  33         s32 numerator;
  34         s32 denominator;
  35 };
  36
  37 static int rescale_read_raw(struct iio_dev *indio_dev,
  38                             struct iio_chan_spec const *chan,
  39                             int *val, int *val2, long mask)
  40 {
  41         struct rescale *rescale = iio_priv(indio_dev);
  42         s64 tmp;
  43         int ret;
  44
  45         switch (mask) {
  46         case IIO_CHAN_INFO_RAW:
  47                 if (rescale->chan_processed)
  48                         /*
  49                          * When only processed channels are supported, we
  50                          * read the processed data and scale it by 1/1
  51                          * augmented with whatever the rescaler has calculated.
  52                          */
  53                         return iio_read_channel_processed(rescale->source, val);
  54                 else
  55                         return iio_read_channel_raw(rescale->source, val);
  56
  57         case IIO_CHAN_INFO_SCALE:
  58                 if (rescale->chan_processed) {
  59                         /*
  60                          * Processed channels are scaled 1-to-1
  61                          */
  62                         *val = 1;
  63                         *val2 = 1;
  64                         ret = IIO_VAL_FRACTIONAL;
  65                 } else {
  66                         ret = iio_read_channel_scale(rescale->source, val, val2);
  67                 }
  68                 switch (ret) {
  69                 case IIO_VAL_FRACTIONAL:
  70                         *val *= rescale->numerator;
  71                         *val2 *= rescale->denominator;
  72                         return ret;
  73                 case IIO_VAL_INT:
  74                         *val *= rescale->numerator;
  75                         if (rescale->denominator == 1)
  76                                 return ret;
  77                         *val2 = rescale->denominator;
  78                         return IIO_VAL_FRACTIONAL;
  79                 case IIO_VAL_FRACTIONAL_LOG2:
  80                         tmp = (s64)*val * 1000000000LL;
  81                         tmp = div_s64(tmp, rescale->denominator);
  82                         tmp *= rescale->numerator;
  83                         tmp = div_s64(tmp, 1000000000LL);
  84                         *val = tmp;
  85                         return ret;
  86                 default:
  87                         return -EOPNOTSUPP;
  88                 }
  89         default:
  90                 return -EINVAL;
  91         }
  92 }
  93
  94 static int rescale_read_avail(struct iio_dev *indio_dev,
  95                               struct iio_chan_spec const *chan,
  96                               const int **vals, int *type, int *length,
  97                               long mask)
  98 {
  99         struct rescale *rescale = iio_priv(indio_dev);
 100
 101         switch (mask) {
 102         case IIO_CHAN_INFO_RAW:
 103                 *type = IIO_VAL_INT;
 104                 return iio_read_avail_channel_raw(rescale->source,
 105                                                   vals, length);
 106         default:
 107                 return -EINVAL;
 108         }
 109 }
 110
 111 static const struct iio_info rescale_info = {
 112         .read_raw = rescale_read_raw,
 113         .read_avail = rescale_read_avail,
 114 };
 115
 116 static ssize_t rescale_read_ext_info(struct iio_dev *indio_dev,
 117                                      uintptr_t private,
 118                                      struct iio_chan_spec const *chan,
 119                                      char *buf)
 120 {
 121         struct rescale *rescale = iio_priv(indio_dev);
 122
 123         return iio_read_channel_ext_info(rescale->source,
 124                                          rescale->ext_info[private].name,
 125                                          buf);
 126 }
 127
 128 static ssize_t rescale_write_ext_info(struct iio_dev *indio_dev,
 129                                       uintptr_t private,
 130                                       struct iio_chan_spec const *chan,
 131                                       const char *buf, size_t len)
 132 {
 133         struct rescale *rescale = iio_priv(indio_dev);
 134
 135         return iio_write_channel_ext_info(rescale->source,
 136                                           rescale->ext_info[private].name,
 137                                           buf, len);
 138 }
 139
 140 static int rescale_configure_channel(struct device *dev,
 141                                      struct rescale *rescale)
 142 {
 143         struct iio_chan_spec *chan = &rescale->chan;
 144         struct iio_chan_spec const *schan = rescale->source->channel;
 145
 146         chan->indexed = 1;
 147         chan->output = schan->output;
 148         chan->ext_info = rescale->ext_info;
 149         chan->type = rescale->cfg->type;
 150
 151         if (iio_channel_has_info(schan, IIO_CHAN_INFO_RAW) ||
 152             iio_channel_has_info(schan, IIO_CHAN_INFO_SCALE)) {
 153                 dev_info(dev, "using raw+scale source channel\n");
 154         } else if (iio_channel_has_info(schan, IIO_CHAN_INFO_PROCESSED)) {
 155                 dev_info(dev, "using processed channel\n");
 156                 rescale->chan_processed = true;
 157         } else {
 158                 dev_err(dev, "source channel is not supported\n");
 159                 return -EINVAL;
 160         }
 161
 162         chan->info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
 163                 BIT(IIO_CHAN_INFO_SCALE);
 164
 165         /*
 166          * Using .read_avail() is fringe to begin with and makes no sense
 167          * whatsoever for processed channels, so we make sure that this cannot
 168          * be called on a processed channel.
 169          */
 170         if (iio_channel_has_available(schan, IIO_CHAN_INFO_RAW) &&
 171             !rescale->chan_processed)
 172                 chan->info_mask_separate_available |= BIT(IIO_CHAN_INFO_RAW);
 173
 174         return 0;
 175 }
 176
 177 static int rescale_current_sense_amplifier_props(struct device *dev,
 178                                                  struct rescale *rescale)
 179 {
 180         u32 sense;
 181         u32 gain_mult = 1;
 182         u32 gain_div = 1;
 183         u32 factor;
 184         int ret;
 185
 186         ret = device_property_read_u32(dev, "sense-resistor-micro-ohms",
 187                                        &sense);
 188         if (ret) {
 189                 dev_err(dev, "failed to read the sense resistance: %d\n", ret);
 190                 return ret;
 191         }
 192
 193         device_property_read_u32(dev, "sense-gain-mult", &gain_mult);
 194         device_property_read_u32(dev, "sense-gain-div", &gain_div);
 195
 196         /*
 197          * Calculate the scaling factor, 1 / (gain * sense), or
 198          * gain_div / (gain_mult * sense), while trying to keep the
 199          * numerator/denominator from overflowing.
 200          */
 201         factor = gcd(sense, 1000000);
 202         rescale->numerator = 1000000 / factor;
 203         rescale->denominator = sense / factor;
 204
 205         factor = gcd(rescale->numerator, gain_mult);
 206         rescale->numerator /= factor;
 207         rescale->denominator *= gain_mult / factor;
 208
 209         factor = gcd(rescale->denominator, gain_div);
 210         rescale->numerator *= gain_div / factor;
 211         rescale->denominator /= factor;
 212
 213         return 0;
 214 }
 215
 216 static int rescale_current_sense_shunt_props(struct device *dev,
 217                                              struct rescale *rescale)
 218 {
 219         u32 shunt;
 220         u32 factor;
 221         int ret;
 222
 223         ret = device_property_read_u32(dev, "shunt-resistor-micro-ohms",
 224                                        &shunt);
 225         if (ret) {
 226                 dev_err(dev, "failed to read the shunt resistance: %d\n", ret);
 227                 return ret;
 228         }
 229
 230         factor = gcd(shunt, 1000000);
 231         rescale->numerator = 1000000 / factor;
 232         rescale->denominator = shunt / factor;
 233
 234         return 0;
 235 }
 236
 237 static int rescale_voltage_divider_props(struct device *dev,
 238                                          struct rescale *rescale)
 239 {
 240         int ret;
 241         u32 factor;
 242
 243         ret = device_property_read_u32(dev, "output-ohms",
 244                                        &rescale->denominator);
 245         if (ret) {
 246                 dev_err(dev, "failed to read output-ohms: %d\n", ret);
 247                 return ret;
 248         }
 249
 250         ret = device_property_read_u32(dev, "full-ohms",
 251                                        &rescale->numerator);
 252         if (ret) {
 253                 dev_err(dev, "failed to read full-ohms: %d\n", ret);
 254                 return ret;
 255         }
 256
 257         factor = gcd(rescale->numerator, rescale->denominator);
 258         rescale->numerator /= factor;
 259         rescale->denominator /= factor;
 260
 261         return 0;
 262 }
 263
 264 enum rescale_variant {
 265         CURRENT_SENSE_AMPLIFIER,
 266         CURRENT_SENSE_SHUNT,
 267         VOLTAGE_DIVIDER,
 268 };
 269
 270 static const struct rescale_cfg rescale_cfg[] = {
 271         [CURRENT_SENSE_AMPLIFIER] = {
 272                 .type = IIO_CURRENT,
 273                 .props = rescale_current_sense_amplifier_props,
 274         },
 275         [CURRENT_SENSE_SHUNT] = {
 276                 .type = IIO_CURRENT,
 277                 .props = rescale_current_sense_shunt_props,
 278         },
 279         [VOLTAGE_DIVIDER] = {
 280                 .type = IIO_VOLTAGE,
 281                 .props = rescale_voltage_divider_props,
 282         },
 283 };
 284
 285 static const struct of_device_id rescale_match[] = {
 286         { .compatible = "current-sense-amplifier",
 287           .data = &rescale_cfg[CURRENT_SENSE_AMPLIFIER], },
 288         { .compatible = "current-sense-shunt",
 289           .data = &rescale_cfg[CURRENT_SENSE_SHUNT], },
 290         { .compatible = "voltage-divider",
 291           .data = &rescale_cfg[VOLTAGE_DIVIDER], },
 292         { /* sentinel */ }
 293 };
 294 MODULE_DEVICE_TABLE(of, rescale_match);
 295
 296 static int rescale_probe(struct platform_device *pdev)
 297 {
 298         struct device *dev = &pdev->dev;
 299         struct iio_dev *indio_dev;
 300         struct iio_channel *source;
 301         struct rescale *rescale;
 302         int sizeof_ext_info;
 303         int sizeof_priv;
 304         int i;
 305         int ret;
 306
 307         source = devm_iio_channel_get(dev, NULL);
 308         if (IS_ERR(source))
 309                 return dev_err_probe(dev, PTR_ERR(source),
 310                                      "failed to get source channel\n");
 311
 312         sizeof_ext_info = iio_get_channel_ext_info_count(source);
 313         if (sizeof_ext_info) {
 314                 sizeof_ext_info += 1; /* one extra entry for the sentinel */
 315                 sizeof_ext_info *= sizeof(*rescale->ext_info);
 316         }
 317
 318         sizeof_priv = sizeof(*rescale) + sizeof_ext_info;
 319
 320         indio_dev = devm_iio_device_alloc(dev, sizeof_priv);
 321         if (!indio_dev)
 322                 return -ENOMEM;
 323
 324         rescale = iio_priv(indio_dev);
 325
 326         rescale->cfg = of_device_get_match_data(dev);
 327         rescale->numerator = 1;
 328         rescale->denominator = 1;
 329
 330         ret = rescale->cfg->props(dev, rescale);
 331         if (ret)
 332                 return ret;
 333
 334         if (!rescale->numerator || !rescale->denominator) {
 335                 dev_err(dev, "invalid scaling factor.\n");
 336                 return -EINVAL;
 337         }
 338
 339         platform_set_drvdata(pdev, indio_dev);
 340
 341         rescale->source = source;
 342
 343         indio_dev->name = dev_name(dev);
 344         indio_dev->info = &rescale_info;
 345         indio_dev->modes = INDIO_DIRECT_MODE;
 346         indio_dev->channels = &rescale->chan;
 347         indio_dev->num_channels = 1;
 348         if (sizeof_ext_info) {
 349                 rescale->ext_info = devm_kmemdup(dev,
 350                                                  source->channel->ext_info,
 351                                                  sizeof_ext_info, GFP_KERNEL);
 352                 if (!rescale->ext_info)
 353                         return -ENOMEM;
 354
 355                 for (i = 0; rescale->ext_info[i].name; ++i) {
 356                         struct iio_chan_spec_ext_info *ext_info =
 357                                 &rescale->ext_info[i];
 358
 359                         if (source->channel->ext_info[i].read)
 360                                 ext_info->read = rescale_read_ext_info;
 361                         if (source->channel->ext_info[i].write)
 362                                 ext_info->write = rescale_write_ext_info;
 363                         ext_info->private = i;
 364                 }
 365         }
 366
 367         ret = rescale_configure_channel(dev, rescale);
 368         if (ret)
 369                 return ret;
 370
 371         return devm_iio_device_register(dev, indio_dev);
 372 }
 373
 374 static struct platform_driver rescale_driver = {
 375         .probe = rescale_probe,
 376         .driver = {
 377                 .name = "iio-rescale",
 378                 .of_match_table = rescale_match,
 379         },
 380 };
 381 module_platform_driver(rescale_driver);
 382
 383 MODULE_DESCRIPTION("IIO rescale driver");
 384 MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
 385 MODULE_LICENSE("GPL v2");