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