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  * Copyright (C) 2022 Liam Beguin <liambeguin@gmail.com>
   7  *
   8  * Author: Peter Rosin <peda@axentia.se>
   9  */
  10
  11 #include <linux/err.h>
  12 #include <linux/gcd.h>
  13 #include <linux/mod_devicetable.h>
  14 #include <linux/module.h>
  15 #include <linux/platform_device.h>
  16 #include <linux/property.h>
  17
  18 #include <linux/iio/afe/rescale.h>
  19 #include <linux/iio/consumer.h>
  20 #include <linux/iio/iio.h>
  21
  22 int rescale_process_scale(struct rescale *rescale, int scale_type,
  23                           int *val, int *val2)
  24 {
  25         s64 tmp;
  26         int _val, _val2;
  27         s32 rem, rem2;
  28         u32 mult;
  29         u32 neg;
  30
  31         switch (scale_type) {
  32         case IIO_VAL_INT:
  33                 *val *= rescale->numerator;
  34                 if (rescale->denominator == 1)
  35                         return scale_type;
  36                 *val2 = rescale->denominator;
  37                 return IIO_VAL_FRACTIONAL;
  38         case IIO_VAL_FRACTIONAL:
  39                 /*
  40                  * When the product of both scales doesn't overflow, avoid
  41                  * potential accuracy loss (for in kernel consumers) by
  42                  * keeping a fractional representation.
  43                  */
  44                 if (!check_mul_overflow(*val, rescale->numerator, &_val) &&
  45                     !check_mul_overflow(*val2, rescale->denominator, &_val2)) {
  46                         *val = _val;
  47                         *val2 = _val2;
  48                         return IIO_VAL_FRACTIONAL;
  49                 }
  50                 fallthrough;
  51         case IIO_VAL_FRACTIONAL_LOG2:
  52                 tmp = (s64)*val * 1000000000LL;
  53                 tmp = div_s64(tmp, rescale->denominator);
  54                 tmp *= rescale->numerator;
  55
  56                 tmp = div_s64_rem(tmp, 1000000000LL, &rem);
  57                 *val = tmp;
  58
  59                 if (!rem)
  60                         return scale_type;
  61
  62                 if (scale_type == IIO_VAL_FRACTIONAL)
  63                         tmp = *val2;
  64                 else
  65                         tmp = ULL(1) << *val2;
  66
  67                 rem2 = *val % (int)tmp;
  68                 *val = *val / (int)tmp;
  69
  70                 *val2 = rem / (int)tmp;
  71                 if (rem2)
  72                         *val2 += div_s64((s64)rem2 * 1000000000LL, tmp);
  73
  74                 return IIO_VAL_INT_PLUS_NANO;
  75         case IIO_VAL_INT_PLUS_NANO:
  76         case IIO_VAL_INT_PLUS_MICRO:
  77                 mult = scale_type == IIO_VAL_INT_PLUS_NANO ? 1000000000L : 1000000L;
  78
  79                 /*
  80                  * For IIO_VAL_INT_PLUS_{MICRO,NANO} scale types if either *val
  81                  * OR *val2 is negative the schan scale is negative, i.e.
  82                  * *val = 1 and *val2 = -0.5 yields -1.5 not -0.5.
  83                  */
  84                 neg = *val < 0 || *val2 < 0;
  85
  86                 tmp = (s64)abs(*val) * abs(rescale->numerator);
  87                 *val = div_s64_rem(tmp, abs(rescale->denominator), &rem);
  88
  89                 tmp = (s64)rem * mult + (s64)abs(*val2) * abs(rescale->numerator);
  90                 tmp = div_s64(tmp, abs(rescale->denominator));
  91
  92                 *val += div_s64_rem(tmp, mult, val2);
  93
  94                 /*
  95                  * If only one of the rescaler elements or the schan scale is
  96                  * negative, the combined scale is negative.
  97                  */
  98                 if (neg ^ ((rescale->numerator < 0) ^ (rescale->denominator < 0))) {
  99                         if (*val)
 100                                 *val = -*val;
 101                         else
 102                                 *val2 = -*val2;
 103                 }
 104
 105                 return scale_type;
 106         default:
 107                 return -EOPNOTSUPP;
 108         }
 109 }
 110 EXPORT_SYMBOL_NS_GPL(rescale_process_scale, "IIO_RESCALE");
 111
 112 int rescale_process_offset(struct rescale *rescale, int scale_type,
 113                            int scale, int scale2, int schan_off,
 114                            int *val, int *val2)
 115 {
 116         s64 tmp, tmp2;
 117
 118         switch (scale_type) {
 119         case IIO_VAL_FRACTIONAL:
 120                 tmp = (s64)rescale->offset * scale2;
 121                 *val = div_s64(tmp, scale) + schan_off;
 122                 return IIO_VAL_INT;
 123         case IIO_VAL_INT:
 124                 *val = div_s64(rescale->offset, scale) + schan_off;
 125                 return IIO_VAL_INT;
 126         case IIO_VAL_FRACTIONAL_LOG2:
 127                 tmp = (s64)rescale->offset * (1 << scale2);
 128                 *val = div_s64(tmp, scale) + schan_off;
 129                 return IIO_VAL_INT;
 130         case IIO_VAL_INT_PLUS_NANO:
 131                 tmp = (s64)rescale->offset * 1000000000LL;
 132                 tmp2 = ((s64)scale * 1000000000LL) + scale2;
 133                 *val = div64_s64(tmp, tmp2) + schan_off;
 134                 return IIO_VAL_INT;
 135         case IIO_VAL_INT_PLUS_MICRO:
 136                 tmp = (s64)rescale->offset * 1000000LL;
 137                 tmp2 = ((s64)scale * 1000000LL) + scale2;
 138                 *val = div64_s64(tmp, tmp2) + schan_off;
 139                 return IIO_VAL_INT;
 140         default:
 141                 return -EOPNOTSUPP;
 142         }
 143 }
 144 EXPORT_SYMBOL_NS_GPL(rescale_process_offset, "IIO_RESCALE");
 145
 146 static int rescale_read_raw(struct iio_dev *indio_dev,
 147                             struct iio_chan_spec const *chan,
 148                             int *val, int *val2, long mask)
 149 {
 150         struct rescale *rescale = iio_priv(indio_dev);
 151         int scale, scale2;
 152         int schan_off = 0;
 153         int ret;
 154
 155         switch (mask) {
 156         case IIO_CHAN_INFO_RAW:
 157                 if (rescale->chan_processed)
 158                         /*
 159                          * When only processed channels are supported, we
 160                          * read the processed data and scale it by 1/1
 161                          * augmented with whatever the rescaler has calculated.
 162                          */
 163                         return iio_read_channel_processed(rescale->source, val);
 164                 else
 165                         return iio_read_channel_raw(rescale->source, val);
 166
 167         case IIO_CHAN_INFO_SCALE:
 168                 if (rescale->chan_processed) {
 169                         /*
 170                          * Processed channels are scaled 1-to-1
 171                          */
 172                         *val = 1;
 173                         *val2 = 1;
 174                         ret = IIO_VAL_FRACTIONAL;
 175                 } else {
 176                         ret = iio_read_channel_scale(rescale->source, val, val2);
 177                 }
 178                 return rescale_process_scale(rescale, ret, val, val2);
 179         case IIO_CHAN_INFO_OFFSET:
 180                 /*
 181                  * Processed channels are scaled 1-to-1 and source offset is
 182                  * already taken into account.
 183                  *
 184                  * In other cases, real world measurement are expressed as:
 185                  *
 186                  *      schan_scale * (raw + schan_offset)
 187                  *
 188                  * Given that the rescaler parameters are applied recursively:
 189                  *
 190                  *      rescaler_scale * (schan_scale * (raw + schan_offset) +
 191                  *              rescaler_offset)
 192                  *
 193                  * Or,
 194                  *
 195                  *      (rescaler_scale * schan_scale) * (raw +
 196                  *              (schan_offset + rescaler_offset / schan_scale)
 197                  *
 198                  * Thus, reusing the original expression the parameters exposed
 199                  * to userspace are:
 200                  *
 201                  *      scale = schan_scale * rescaler_scale
 202                  *      offset = schan_offset + rescaler_offset / schan_scale
 203                  */
 204                 if (rescale->chan_processed) {
 205                         *val = rescale->offset;
 206                         return IIO_VAL_INT;
 207                 }
 208
 209                 if (iio_channel_has_info(rescale->source->channel,
 210                                          IIO_CHAN_INFO_OFFSET)) {
 211                         ret = iio_read_channel_offset(rescale->source,
 212                                                       &schan_off, NULL);
 213                         if (ret != IIO_VAL_INT)
 214                                 return ret < 0 ? ret : -EOPNOTSUPP;
 215                 }
 216
 217                 if (iio_channel_has_info(rescale->source->channel,
 218                                          IIO_CHAN_INFO_SCALE)) {
 219                         ret = iio_read_channel_scale(rescale->source, &scale, &scale2);
 220                         return rescale_process_offset(rescale, ret, scale, scale2,
 221                                                       schan_off, val, val2);
 222                 }
 223
 224                 /*
 225                  * If we get here we have no scale so scale 1:1 but apply
 226                  * rescaler and offset, if any.
 227                  */
 228                 return rescale_process_offset(rescale, IIO_VAL_FRACTIONAL, 1, 1,
 229                                               schan_off, val, val2);
 230         default:
 231                 return -EINVAL;
 232         }
 233 }
 234
 235 static int rescale_read_avail(struct iio_dev *indio_dev,
 236                               struct iio_chan_spec const *chan,
 237                               const int **vals, int *type, int *length,
 238                               long mask)
 239 {
 240         struct rescale *rescale = iio_priv(indio_dev);
 241
 242         switch (mask) {
 243         case IIO_CHAN_INFO_RAW:
 244                 *type = IIO_VAL_INT;
 245                 return iio_read_avail_channel_raw(rescale->source,
 246                                                   vals, length);
 247         default:
 248                 return -EINVAL;
 249         }
 250 }
 251
 252 static const struct iio_info rescale_info = {
 253         .read_raw = rescale_read_raw,
 254         .read_avail = rescale_read_avail,
 255 };
 256
 257 static ssize_t rescale_read_ext_info(struct iio_dev *indio_dev,
 258                                      uintptr_t private,
 259                                      struct iio_chan_spec const *chan,
 260                                      char *buf)
 261 {
 262         struct rescale *rescale = iio_priv(indio_dev);
 263
 264         return iio_read_channel_ext_info(rescale->source,
 265                                          rescale->ext_info[private].name,
 266                                          buf);
 267 }
 268
 269 static ssize_t rescale_write_ext_info(struct iio_dev *indio_dev,
 270                                       uintptr_t private,
 271                                       struct iio_chan_spec const *chan,
 272                                       const char *buf, size_t len)
 273 {
 274         struct rescale *rescale = iio_priv(indio_dev);
 275
 276         return iio_write_channel_ext_info(rescale->source,
 277                                           rescale->ext_info[private].name,
 278                                           buf, len);
 279 }
 280
 281 static int rescale_configure_channel(struct device *dev,
 282                                      struct rescale *rescale)
 283 {
 284         struct iio_chan_spec *chan = &rescale->chan;
 285         struct iio_chan_spec const *schan = rescale->source->channel;
 286
 287         chan->indexed = 1;
 288         chan->output = schan->output;
 289         chan->ext_info = rescale->ext_info;
 290         chan->type = rescale->cfg->type;
 291
 292         if (iio_channel_has_info(schan, IIO_CHAN_INFO_RAW) &&
 293             (iio_channel_has_info(schan, IIO_CHAN_INFO_SCALE) ||
 294              iio_channel_has_info(schan, IIO_CHAN_INFO_OFFSET))) {
 295                 dev_info(dev, "using raw+scale/offset source channel\n");
 296         } else if (iio_channel_has_info(schan, IIO_CHAN_INFO_PROCESSED)) {
 297                 dev_info(dev, "using processed channel\n");
 298                 rescale->chan_processed = true;
 299         } else {
 300                 dev_err(dev, "source channel is not supported\n");
 301                 return -EINVAL;
 302         }
 303
 304         chan->info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
 305                 BIT(IIO_CHAN_INFO_SCALE);
 306
 307         if (rescale->offset)
 308                 chan->info_mask_separate |= BIT(IIO_CHAN_INFO_OFFSET);
 309
 310         /*
 311          * Using .read_avail() is fringe to begin with and makes no sense
 312          * whatsoever for processed channels, so we make sure that this cannot
 313          * be called on a processed channel.
 314          */
 315         if (iio_channel_has_available(schan, IIO_CHAN_INFO_RAW) &&
 316             !rescale->chan_processed)
 317                 chan->info_mask_separate_available |= BIT(IIO_CHAN_INFO_RAW);
 318
 319         return 0;
 320 }
 321
 322 static int rescale_current_sense_amplifier_props(struct device *dev,
 323                                                  struct rescale *rescale)
 324 {
 325         u32 sense;
 326         u32 gain_mult = 1;
 327         u32 gain_div = 1;
 328         u32 factor;
 329         int ret;
 330
 331         ret = device_property_read_u32(dev, "sense-resistor-micro-ohms",
 332                                        &sense);
 333         if (ret) {
 334                 dev_err(dev, "failed to read the sense resistance: %d\n", ret);
 335                 return ret;
 336         }
 337
 338         device_property_read_u32(dev, "sense-gain-mult", &gain_mult);
 339         device_property_read_u32(dev, "sense-gain-div", &gain_div);
 340
 341         /*
 342          * Calculate the scaling factor, 1 / (gain * sense), or
 343          * gain_div / (gain_mult * sense), while trying to keep the
 344          * numerator/denominator from overflowing.
 345          */
 346         factor = gcd(sense, 1000000);
 347         rescale->numerator = 1000000 / factor;
 348         rescale->denominator = sense / factor;
 349
 350         factor = gcd(rescale->numerator, gain_mult);
 351         rescale->numerator /= factor;
 352         rescale->denominator *= gain_mult / factor;
 353
 354         factor = gcd(rescale->denominator, gain_div);
 355         rescale->numerator *= gain_div / factor;
 356         rescale->denominator /= factor;
 357
 358         return 0;
 359 }
 360
 361 static int rescale_current_sense_shunt_props(struct device *dev,
 362                                              struct rescale *rescale)
 363 {
 364         u32 shunt;
 365         u32 factor;
 366         int ret;
 367
 368         ret = device_property_read_u32(dev, "shunt-resistor-micro-ohms",
 369                                        &shunt);
 370         if (ret) {
 371                 dev_err(dev, "failed to read the shunt resistance: %d\n", ret);
 372                 return ret;
 373         }
 374
 375         factor = gcd(shunt, 1000000);
 376         rescale->numerator = 1000000 / factor;
 377         rescale->denominator = shunt / factor;
 378
 379         return 0;
 380 }
 381
 382 static int rescale_voltage_divider_props(struct device *dev,
 383                                          struct rescale *rescale)
 384 {
 385         int ret;
 386         u32 factor;
 387
 388         ret = device_property_read_u32(dev, "output-ohms",
 389                                        &rescale->denominator);
 390         if (ret) {
 391                 dev_err(dev, "failed to read output-ohms: %d\n", ret);
 392                 return ret;
 393         }
 394
 395         ret = device_property_read_u32(dev, "full-ohms",
 396                                        &rescale->numerator);
 397         if (ret) {
 398                 dev_err(dev, "failed to read full-ohms: %d\n", ret);
 399                 return ret;
 400         }
 401
 402         factor = gcd(rescale->numerator, rescale->denominator);
 403         rescale->numerator /= factor;
 404         rescale->denominator /= factor;
 405
 406         return 0;
 407 }
 408
 409 static int rescale_temp_sense_rtd_props(struct device *dev,
 410                                         struct rescale *rescale)
 411 {
 412         u32 factor;
 413         u32 alpha;
 414         u32 iexc;
 415         u32 tmp;
 416         int ret;
 417         u32 r0;
 418
 419         ret = device_property_read_u32(dev, "excitation-current-microamp",
 420                                        &iexc);
 421         if (ret) {
 422                 dev_err(dev, "failed to read excitation-current-microamp: %d\n",
 423                         ret);
 424                 return ret;
 425         }
 426
 427         ret = device_property_read_u32(dev, "alpha-ppm-per-celsius", &alpha);
 428         if (ret) {
 429                 dev_err(dev, "failed to read alpha-ppm-per-celsius: %d\n",
 430                         ret);
 431                 return ret;
 432         }
 433
 434         ret = device_property_read_u32(dev, "r-naught-ohms", &r0);
 435         if (ret) {
 436                 dev_err(dev, "failed to read r-naught-ohms: %d\n", ret);
 437                 return ret;
 438         }
 439
 440         tmp = r0 * iexc * alpha / 1000000;
 441         factor = gcd(tmp, 1000000);
 442         rescale->numerator = 1000000 / factor;
 443         rescale->denominator = tmp / factor;
 444
 445         rescale->offset = -1 * ((r0 * iexc) / 1000);
 446
 447         return 0;
 448 }
 449
 450 static int rescale_temp_transducer_props(struct device *dev,
 451                                          struct rescale *rescale)
 452 {
 453         s32 offset = 0;
 454         s32 sense = 1;
 455         s32 alpha;
 456         int ret;
 457
 458         device_property_read_u32(dev, "sense-offset-millicelsius", &offset);
 459         device_property_read_u32(dev, "sense-resistor-ohms", &sense);
 460         ret = device_property_read_u32(dev, "alpha-ppm-per-celsius", &alpha);
 461         if (ret) {
 462                 dev_err(dev, "failed to read alpha-ppm-per-celsius: %d\n", ret);
 463                 return ret;
 464         }
 465
 466         rescale->numerator = 1000000;
 467         rescale->denominator = alpha * sense;
 468
 469         rescale->offset = div_s64((s64)offset * rescale->denominator,
 470                                   rescale->numerator);
 471
 472         return 0;
 473 }
 474
 475 enum rescale_variant {
 476         CURRENT_SENSE_AMPLIFIER,
 477         CURRENT_SENSE_SHUNT,
 478         VOLTAGE_DIVIDER,
 479         TEMP_SENSE_RTD,
 480         TEMP_TRANSDUCER,
 481 };
 482
 483 static const struct rescale_cfg rescale_cfg[] = {
 484         [CURRENT_SENSE_AMPLIFIER] = {
 485                 .type = IIO_CURRENT,
 486                 .props = rescale_current_sense_amplifier_props,
 487         },
 488         [CURRENT_SENSE_SHUNT] = {
 489                 .type = IIO_CURRENT,
 490                 .props = rescale_current_sense_shunt_props,
 491         },
 492         [VOLTAGE_DIVIDER] = {
 493                 .type = IIO_VOLTAGE,
 494                 .props = rescale_voltage_divider_props,
 495         },
 496         [TEMP_SENSE_RTD] = {
 497                 .type = IIO_TEMP,
 498                 .props = rescale_temp_sense_rtd_props,
 499         },
 500         [TEMP_TRANSDUCER] = {
 501                 .type = IIO_TEMP,
 502                 .props = rescale_temp_transducer_props,
 503         },
 504 };
 505
 506 static const struct of_device_id rescale_match[] = {
 507         { .compatible = "current-sense-amplifier",
 508           .data = &rescale_cfg[CURRENT_SENSE_AMPLIFIER], },
 509         { .compatible = "current-sense-shunt",
 510           .data = &rescale_cfg[CURRENT_SENSE_SHUNT], },
 511         { .compatible = "voltage-divider",
 512           .data = &rescale_cfg[VOLTAGE_DIVIDER], },
 513         { .compatible = "temperature-sense-rtd",
 514           .data = &rescale_cfg[TEMP_SENSE_RTD], },
 515         { .compatible = "temperature-transducer",
 516           .data = &rescale_cfg[TEMP_TRANSDUCER], },
 517         { /* sentinel */ }
 518 };
 519 MODULE_DEVICE_TABLE(of, rescale_match);
 520
 521 static int rescale_probe(struct platform_device *pdev)
 522 {
 523         struct device *dev = &pdev->dev;
 524         struct iio_dev *indio_dev;
 525         struct iio_channel *source;
 526         struct rescale *rescale;
 527         int sizeof_ext_info;
 528         int sizeof_priv;
 529         int i;
 530         int ret;
 531
 532         source = devm_iio_channel_get(dev, NULL);
 533         if (IS_ERR(source))
 534                 return dev_err_probe(dev, PTR_ERR(source),
 535                                      "failed to get source channel\n");
 536
 537         sizeof_ext_info = iio_get_channel_ext_info_count(source);
 538         if (sizeof_ext_info) {
 539                 sizeof_ext_info += 1; /* one extra entry for the sentinel */
 540                 sizeof_ext_info *= sizeof(*rescale->ext_info);
 541         }
 542
 543         sizeof_priv = sizeof(*rescale) + sizeof_ext_info;
 544
 545         indio_dev = devm_iio_device_alloc(dev, sizeof_priv);
 546         if (!indio_dev)
 547                 return -ENOMEM;
 548
 549         rescale = iio_priv(indio_dev);
 550
 551         rescale->cfg = device_get_match_data(dev);
 552         rescale->numerator = 1;
 553         rescale->denominator = 1;
 554         rescale->offset = 0;
 555
 556         ret = rescale->cfg->props(dev, rescale);
 557         if (ret)
 558                 return ret;
 559
 560         if (!rescale->numerator || !rescale->denominator) {
 561                 dev_err(dev, "invalid scaling factor.\n");
 562                 return -EINVAL;
 563         }
 564
 565         platform_set_drvdata(pdev, indio_dev);
 566
 567         rescale->source = source;
 568
 569         indio_dev->name = dev_name(dev);
 570         indio_dev->info = &rescale_info;
 571         indio_dev->modes = INDIO_DIRECT_MODE;
 572         indio_dev->channels = &rescale->chan;
 573         indio_dev->num_channels = 1;
 574         if (sizeof_ext_info) {
 575                 rescale->ext_info = devm_kmemdup(dev,
 576                                                  source->channel->ext_info,
 577                                                  sizeof_ext_info, GFP_KERNEL);
 578                 if (!rescale->ext_info)
 579                         return -ENOMEM;
 580
 581                 for (i = 0; rescale->ext_info[i].name; ++i) {
 582                         struct iio_chan_spec_ext_info *ext_info =
 583                                 &rescale->ext_info[i];
 584
 585                         if (source->channel->ext_info[i].read)
 586                                 ext_info->read = rescale_read_ext_info;
 587                         if (source->channel->ext_info[i].write)
 588                                 ext_info->write = rescale_write_ext_info;
 589                         ext_info->private = i;
 590                 }
 591         }
 592
 593         ret = rescale_configure_channel(dev, rescale);
 594         if (ret)
 595                 return ret;
 596
 597         return devm_iio_device_register(dev, indio_dev);
 598 }
 599
 600 static struct platform_driver rescale_driver = {
 601         .probe = rescale_probe,
 602         .driver = {
 603                 .name = "iio-rescale",
 604                 .of_match_table = rescale_match,
 605         },
 606 };
 607 module_platform_driver(rescale_driver);
 608
 609 MODULE_DESCRIPTION("IIO rescale driver");
 610 MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
 611 MODULE_LICENSE("GPL v2");