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"); 599