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 rescale_process_scale(struct rescale * rescale,int scale_type,int * val,int * val2)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 rescale_process_offset(struct rescale * rescale,int scale_type,int scale,int scale2,int schan_off,int * val,int * val2)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 rescale_read_raw(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int * val,int * val2,long mask)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 rescale_read_avail(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,const int ** vals,int * type,int * length,long mask)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 rescale_read_ext_info(struct iio_dev * indio_dev,uintptr_t private,struct iio_chan_spec const * chan,char * buf)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 rescale_write_ext_info(struct iio_dev * indio_dev,uintptr_t private,struct iio_chan_spec const * chan,const char * buf,size_t len)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 rescale_configure_channel(struct device * dev,struct rescale * rescale)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 rescale_current_sense_amplifier_props(struct device * dev,struct rescale * rescale)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 rescale_current_sense_shunt_props(struct device * dev,struct rescale * rescale)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 rescale_voltage_divider_props(struct device * dev,struct rescale * rescale)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 rescale_temp_sense_rtd_props(struct device * dev,struct rescale * rescale)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 rescale_temp_transducer_props(struct device * dev,struct rescale * rescale)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 rescale_probe(struct platform_device * pdev)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"); 612