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