612 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * IIO rescale driver4 *5 * Copyright (C) 2018 Axentia Technologies AB6 * 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 22int 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, avoid41 * potential accuracy loss (for in kernel consumers) by42 * 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 else65 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 *val81 * 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 is96 * negative, the combined scale is negative.97 */98 if (neg ^ ((rescale->numerator < 0) ^ (rescale->denominator < 0))) {99 if (*val)100 *val = -*val;101 else102 *val2 = -*val2;103 }104 105 return scale_type;106 default:107 return -EOPNOTSUPP;108 }109}110EXPORT_SYMBOL_NS_GPL(rescale_process_scale, IIO_RESCALE);111 112int 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}144EXPORT_SYMBOL_NS_GPL(rescale_process_offset, IIO_RESCALE);145 146static 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, we160 * read the processed data and scale it by 1/1161 * augmented with whatever the rescaler has calculated.162 */163 return iio_read_channel_processed(rescale->source, val);164 else165 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-1171 */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 is182 * 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 exposed199 * to userspace are:200 *201 * scale = schan_scale * rescaler_scale202 * offset = schan_offset + rescaler_offset / schan_scale203 */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 apply226 * 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 235static 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 252static const struct iio_info rescale_info = {253 .read_raw = rescale_read_raw,254 .read_avail = rescale_read_avail,255};256 257static 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 269static 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 281static 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 sense312 * whatsoever for processed channels, so we make sure that this cannot313 * 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 322static 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), or343 * gain_div / (gain_mult * sense), while trying to keep the344 * 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 361static 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 382static 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 409static 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 450static 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 475enum rescale_variant {476 CURRENT_SENSE_AMPLIFIER,477 CURRENT_SENSE_SHUNT,478 VOLTAGE_DIVIDER,479 TEMP_SENSE_RTD,480 TEMP_TRANSDUCER,481};482 483static 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 506static 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};519MODULE_DEVICE_TABLE(of, rescale_match);520 521static 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 600static struct platform_driver rescale_driver = {601 .probe = rescale_probe,602 .driver = {603 .name = "iio-rescale",604 .of_match_table = rescale_match,605 },606};607module_platform_driver(rescale_driver);608 609MODULE_DESCRIPTION("IIO rescale driver");610MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");611MODULE_LICENSE("GPL v2");612