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