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