Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * IIO rescale driver
0004  *
0005  * Copyright (C) 2018 Axentia Technologies AB
0006  * Copyright (C) 2022 Liam Beguin <liambeguin@gmail.com>
0007  *
0008  * Author: Peter Rosin <peda@axentia.se>
0009  */
0010 
0011 #include <linux/err.h>
0012 #include <linux/gcd.h>
0013 #include <linux/mod_devicetable.h>
0014 #include <linux/module.h>
0015 #include <linux/platform_device.h>
0016 #include <linux/property.h>
0017 
0018 #include <linux/iio/afe/rescale.h>
0019 #include <linux/iio/consumer.h>
0020 #include <linux/iio/iio.h>
0021 
0022 int rescale_process_scale(struct rescale *rescale, int scale_type,
0023               int *val, int *val2)
0024 {
0025     s64 tmp;
0026     int _val, _val2;
0027     s32 rem, rem2;
0028     u32 mult;
0029     u32 neg;
0030 
0031     switch (scale_type) {
0032     case IIO_VAL_INT:
0033         *val *= rescale->numerator;
0034         if (rescale->denominator == 1)
0035             return scale_type;
0036         *val2 = rescale->denominator;
0037         return IIO_VAL_FRACTIONAL;
0038     case IIO_VAL_FRACTIONAL:
0039         /*
0040          * When the product of both scales doesn't overflow, avoid
0041          * potential accuracy loss (for in kernel consumers) by
0042          * keeping a fractional representation.
0043          */
0044         if (!check_mul_overflow(*val, rescale->numerator, &_val) &&
0045             !check_mul_overflow(*val2, rescale->denominator, &_val2)) {
0046             *val = _val;
0047             *val2 = _val2;
0048             return IIO_VAL_FRACTIONAL;
0049         }
0050         fallthrough;
0051     case IIO_VAL_FRACTIONAL_LOG2:
0052         tmp = (s64)*val * 1000000000LL;
0053         tmp = div_s64(tmp, rescale->denominator);
0054         tmp *= rescale->numerator;
0055 
0056         tmp = div_s64_rem(tmp, 1000000000LL, &rem);
0057         *val = tmp;
0058 
0059         if (!rem)
0060             return scale_type;
0061 
0062         if (scale_type == IIO_VAL_FRACTIONAL)
0063             tmp = *val2;
0064         else
0065             tmp = ULL(1) << *val2;
0066 
0067         rem2 = *val % (int)tmp;
0068         *val = *val / (int)tmp;
0069 
0070         *val2 = rem / (int)tmp;
0071         if (rem2)
0072             *val2 += div_s64((s64)rem2 * 1000000000LL, tmp);
0073 
0074         return IIO_VAL_INT_PLUS_NANO;
0075     case IIO_VAL_INT_PLUS_NANO:
0076     case IIO_VAL_INT_PLUS_MICRO:
0077         mult = scale_type == IIO_VAL_INT_PLUS_NANO ? 1000000000L : 1000000L;
0078 
0079         /*
0080          * For IIO_VAL_INT_PLUS_{MICRO,NANO} scale types if either *val
0081          * OR *val2 is negative the schan scale is negative, i.e.
0082          * *val = 1 and *val2 = -0.5 yields -1.5 not -0.5.
0083          */
0084         neg = *val < 0 || *val2 < 0;
0085 
0086         tmp = (s64)abs(*val) * abs(rescale->numerator);
0087         *val = div_s64_rem(tmp, abs(rescale->denominator), &rem);
0088 
0089         tmp = (s64)rem * mult + (s64)abs(*val2) * abs(rescale->numerator);
0090         tmp = div_s64(tmp, abs(rescale->denominator));
0091 
0092         *val += div_s64_rem(tmp, mult, val2);
0093 
0094         /*
0095          * If only one of the rescaler elements or the schan scale is
0096          * negative, the combined scale is negative.
0097          */
0098         if (neg ^ ((rescale->numerator < 0) ^ (rescale->denominator < 0))) {
0099             if (*val)
0100                 *val = -*val;
0101             else
0102                 *val2 = -*val2;
0103         }
0104 
0105         return scale_type;
0106     default:
0107         return -EOPNOTSUPP;
0108     }
0109 }
0110 EXPORT_SYMBOL_NS_GPL(rescale_process_scale, IIO_RESCALE);
0111 
0112 int rescale_process_offset(struct rescale *rescale, int scale_type,
0113                int scale, int scale2, int schan_off,
0114                int *val, int *val2)
0115 {
0116     s64 tmp, tmp2;
0117 
0118     switch (scale_type) {
0119     case IIO_VAL_FRACTIONAL:
0120         tmp = (s64)rescale->offset * scale2;
0121         *val = div_s64(tmp, scale) + schan_off;
0122         return IIO_VAL_INT;
0123     case IIO_VAL_INT:
0124         *val = div_s64(rescale->offset, scale) + schan_off;
0125         return IIO_VAL_INT;
0126     case IIO_VAL_FRACTIONAL_LOG2:
0127         tmp = (s64)rescale->offset * (1 << scale2);
0128         *val = div_s64(tmp, scale) + schan_off;
0129         return IIO_VAL_INT;
0130     case IIO_VAL_INT_PLUS_NANO:
0131         tmp = (s64)rescale->offset * 1000000000LL;
0132         tmp2 = ((s64)scale * 1000000000LL) + scale2;
0133         *val = div64_s64(tmp, tmp2) + schan_off;
0134         return IIO_VAL_INT;
0135     case IIO_VAL_INT_PLUS_MICRO:
0136         tmp = (s64)rescale->offset * 1000000LL;
0137         tmp2 = ((s64)scale * 1000000LL) + scale2;
0138         *val = div64_s64(tmp, tmp2) + schan_off;
0139         return IIO_VAL_INT;
0140     default:
0141         return -EOPNOTSUPP;
0142     }
0143 }
0144 EXPORT_SYMBOL_NS_GPL(rescale_process_offset, IIO_RESCALE);
0145 
0146 static int rescale_read_raw(struct iio_dev *indio_dev,
0147                 struct iio_chan_spec const *chan,
0148                 int *val, int *val2, long mask)
0149 {
0150     struct rescale *rescale = iio_priv(indio_dev);
0151     int scale, scale2;
0152     int schan_off = 0;
0153     int ret;
0154 
0155     switch (mask) {
0156     case IIO_CHAN_INFO_RAW:
0157         if (rescale->chan_processed)
0158             /*
0159              * When only processed channels are supported, we
0160              * read the processed data and scale it by 1/1
0161              * augmented with whatever the rescaler has calculated.
0162              */
0163             return iio_read_channel_processed(rescale->source, val);
0164         else
0165             return iio_read_channel_raw(rescale->source, val);
0166 
0167     case IIO_CHAN_INFO_SCALE:
0168         if (rescale->chan_processed) {
0169             /*
0170              * Processed channels are scaled 1-to-1
0171              */
0172             *val = 1;
0173             *val2 = 1;
0174             ret = IIO_VAL_FRACTIONAL;
0175         } else {
0176             ret = iio_read_channel_scale(rescale->source, val, val2);
0177         }
0178         return rescale_process_scale(rescale, ret, val, val2);
0179     case IIO_CHAN_INFO_OFFSET:
0180         /*
0181          * Processed channels are scaled 1-to-1 and source offset is
0182          * already taken into account.
0183          *
0184          * In other cases, real world measurement are expressed as:
0185          *
0186          *  schan_scale * (raw + schan_offset)
0187          *
0188          * Given that the rescaler parameters are applied recursively:
0189          *
0190          *  rescaler_scale * (schan_scale * (raw + schan_offset) +
0191          *      rescaler_offset)
0192          *
0193          * Or,
0194          *
0195          *  (rescaler_scale * schan_scale) * (raw +
0196          *      (schan_offset + rescaler_offset / schan_scale)
0197          *
0198          * Thus, reusing the original expression the parameters exposed
0199          * to userspace are:
0200          *
0201          *  scale = schan_scale * rescaler_scale
0202          *  offset = schan_offset + rescaler_offset / schan_scale
0203          */
0204         if (rescale->chan_processed) {
0205             *val = rescale->offset;
0206             return IIO_VAL_INT;
0207         }
0208 
0209         if (iio_channel_has_info(rescale->source->channel,
0210                      IIO_CHAN_INFO_OFFSET)) {
0211             ret = iio_read_channel_offset(rescale->source,
0212                               &schan_off, NULL);
0213             if (ret != IIO_VAL_INT)
0214                 return ret < 0 ? ret : -EOPNOTSUPP;
0215         }
0216 
0217         ret = iio_read_channel_scale(rescale->source, &scale, &scale2);
0218         return rescale_process_offset(rescale, ret, scale, scale2,
0219                           schan_off, val, val2);
0220     default:
0221         return -EINVAL;
0222     }
0223 }
0224 
0225 static int rescale_read_avail(struct iio_dev *indio_dev,
0226                   struct iio_chan_spec const *chan,
0227                   const int **vals, int *type, int *length,
0228                   long mask)
0229 {
0230     struct rescale *rescale = iio_priv(indio_dev);
0231 
0232     switch (mask) {
0233     case IIO_CHAN_INFO_RAW:
0234         *type = IIO_VAL_INT;
0235         return iio_read_avail_channel_raw(rescale->source,
0236                           vals, length);
0237     default:
0238         return -EINVAL;
0239     }
0240 }
0241 
0242 static const struct iio_info rescale_info = {
0243     .read_raw = rescale_read_raw,
0244     .read_avail = rescale_read_avail,
0245 };
0246 
0247 static ssize_t rescale_read_ext_info(struct iio_dev *indio_dev,
0248                      uintptr_t private,
0249                      struct iio_chan_spec const *chan,
0250                      char *buf)
0251 {
0252     struct rescale *rescale = iio_priv(indio_dev);
0253 
0254     return iio_read_channel_ext_info(rescale->source,
0255                      rescale->ext_info[private].name,
0256                      buf);
0257 }
0258 
0259 static ssize_t rescale_write_ext_info(struct iio_dev *indio_dev,
0260                       uintptr_t private,
0261                       struct iio_chan_spec const *chan,
0262                       const char *buf, size_t len)
0263 {
0264     struct rescale *rescale = iio_priv(indio_dev);
0265 
0266     return iio_write_channel_ext_info(rescale->source,
0267                       rescale->ext_info[private].name,
0268                       buf, len);
0269 }
0270 
0271 static int rescale_configure_channel(struct device *dev,
0272                      struct rescale *rescale)
0273 {
0274     struct iio_chan_spec *chan = &rescale->chan;
0275     struct iio_chan_spec const *schan = rescale->source->channel;
0276 
0277     chan->indexed = 1;
0278     chan->output = schan->output;
0279     chan->ext_info = rescale->ext_info;
0280     chan->type = rescale->cfg->type;
0281 
0282     if (iio_channel_has_info(schan, IIO_CHAN_INFO_RAW) &&
0283         iio_channel_has_info(schan, IIO_CHAN_INFO_SCALE)) {
0284         dev_info(dev, "using raw+scale source channel\n");
0285     } else if (iio_channel_has_info(schan, IIO_CHAN_INFO_PROCESSED)) {
0286         dev_info(dev, "using processed channel\n");
0287         rescale->chan_processed = true;
0288     } else {
0289         dev_err(dev, "source channel is not supported\n");
0290         return -EINVAL;
0291     }
0292 
0293     chan->info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
0294         BIT(IIO_CHAN_INFO_SCALE);
0295 
0296     if (rescale->offset)
0297         chan->info_mask_separate |= BIT(IIO_CHAN_INFO_OFFSET);
0298 
0299     /*
0300      * Using .read_avail() is fringe to begin with and makes no sense
0301      * whatsoever for processed channels, so we make sure that this cannot
0302      * be called on a processed channel.
0303      */
0304     if (iio_channel_has_available(schan, IIO_CHAN_INFO_RAW) &&
0305         !rescale->chan_processed)
0306         chan->info_mask_separate_available |= BIT(IIO_CHAN_INFO_RAW);
0307 
0308     return 0;
0309 }
0310 
0311 static int rescale_current_sense_amplifier_props(struct device *dev,
0312                          struct rescale *rescale)
0313 {
0314     u32 sense;
0315     u32 gain_mult = 1;
0316     u32 gain_div = 1;
0317     u32 factor;
0318     int ret;
0319 
0320     ret = device_property_read_u32(dev, "sense-resistor-micro-ohms",
0321                        &sense);
0322     if (ret) {
0323         dev_err(dev, "failed to read the sense resistance: %d\n", ret);
0324         return ret;
0325     }
0326 
0327     device_property_read_u32(dev, "sense-gain-mult", &gain_mult);
0328     device_property_read_u32(dev, "sense-gain-div", &gain_div);
0329 
0330     /*
0331      * Calculate the scaling factor, 1 / (gain * sense), or
0332      * gain_div / (gain_mult * sense), while trying to keep the
0333      * numerator/denominator from overflowing.
0334      */
0335     factor = gcd(sense, 1000000);
0336     rescale->numerator = 1000000 / factor;
0337     rescale->denominator = sense / factor;
0338 
0339     factor = gcd(rescale->numerator, gain_mult);
0340     rescale->numerator /= factor;
0341     rescale->denominator *= gain_mult / factor;
0342 
0343     factor = gcd(rescale->denominator, gain_div);
0344     rescale->numerator *= gain_div / factor;
0345     rescale->denominator /= factor;
0346 
0347     return 0;
0348 }
0349 
0350 static int rescale_current_sense_shunt_props(struct device *dev,
0351                          struct rescale *rescale)
0352 {
0353     u32 shunt;
0354     u32 factor;
0355     int ret;
0356 
0357     ret = device_property_read_u32(dev, "shunt-resistor-micro-ohms",
0358                        &shunt);
0359     if (ret) {
0360         dev_err(dev, "failed to read the shunt resistance: %d\n", ret);
0361         return ret;
0362     }
0363 
0364     factor = gcd(shunt, 1000000);
0365     rescale->numerator = 1000000 / factor;
0366     rescale->denominator = shunt / factor;
0367 
0368     return 0;
0369 }
0370 
0371 static int rescale_voltage_divider_props(struct device *dev,
0372                      struct rescale *rescale)
0373 {
0374     int ret;
0375     u32 factor;
0376 
0377     ret = device_property_read_u32(dev, "output-ohms",
0378                        &rescale->denominator);
0379     if (ret) {
0380         dev_err(dev, "failed to read output-ohms: %d\n", ret);
0381         return ret;
0382     }
0383 
0384     ret = device_property_read_u32(dev, "full-ohms",
0385                        &rescale->numerator);
0386     if (ret) {
0387         dev_err(dev, "failed to read full-ohms: %d\n", ret);
0388         return ret;
0389     }
0390 
0391     factor = gcd(rescale->numerator, rescale->denominator);
0392     rescale->numerator /= factor;
0393     rescale->denominator /= factor;
0394 
0395     return 0;
0396 }
0397 
0398 static int rescale_temp_sense_rtd_props(struct device *dev,
0399                     struct rescale *rescale)
0400 {
0401     u32 factor;
0402     u32 alpha;
0403     u32 iexc;
0404     u32 tmp;
0405     int ret;
0406     u32 r0;
0407 
0408     ret = device_property_read_u32(dev, "excitation-current-microamp",
0409                        &iexc);
0410     if (ret) {
0411         dev_err(dev, "failed to read excitation-current-microamp: %d\n",
0412             ret);
0413         return ret;
0414     }
0415 
0416     ret = device_property_read_u32(dev, "alpha-ppm-per-celsius", &alpha);
0417     if (ret) {
0418         dev_err(dev, "failed to read alpha-ppm-per-celsius: %d\n",
0419             ret);
0420         return ret;
0421     }
0422 
0423     ret = device_property_read_u32(dev, "r-naught-ohms", &r0);
0424     if (ret) {
0425         dev_err(dev, "failed to read r-naught-ohms: %d\n", ret);
0426         return ret;
0427     }
0428 
0429     tmp = r0 * iexc * alpha / 1000000;
0430     factor = gcd(tmp, 1000000);
0431     rescale->numerator = 1000000 / factor;
0432     rescale->denominator = tmp / factor;
0433 
0434     rescale->offset = -1 * ((r0 * iexc) / 1000);
0435 
0436     return 0;
0437 }
0438 
0439 static int rescale_temp_transducer_props(struct device *dev,
0440                      struct rescale *rescale)
0441 {
0442     s32 offset = 0;
0443     s32 sense = 1;
0444     s32 alpha;
0445     int ret;
0446 
0447     device_property_read_u32(dev, "sense-offset-millicelsius", &offset);
0448     device_property_read_u32(dev, "sense-resistor-ohms", &sense);
0449     ret = device_property_read_u32(dev, "alpha-ppm-per-celsius", &alpha);
0450     if (ret) {
0451         dev_err(dev, "failed to read alpha-ppm-per-celsius: %d\n", ret);
0452         return ret;
0453     }
0454 
0455     rescale->numerator = 1000000;
0456     rescale->denominator = alpha * sense;
0457 
0458     rescale->offset = div_s64((s64)offset * rescale->denominator,
0459                   rescale->numerator);
0460 
0461     return 0;
0462 }
0463 
0464 enum rescale_variant {
0465     CURRENT_SENSE_AMPLIFIER,
0466     CURRENT_SENSE_SHUNT,
0467     VOLTAGE_DIVIDER,
0468     TEMP_SENSE_RTD,
0469     TEMP_TRANSDUCER,
0470 };
0471 
0472 static const struct rescale_cfg rescale_cfg[] = {
0473     [CURRENT_SENSE_AMPLIFIER] = {
0474         .type = IIO_CURRENT,
0475         .props = rescale_current_sense_amplifier_props,
0476     },
0477     [CURRENT_SENSE_SHUNT] = {
0478         .type = IIO_CURRENT,
0479         .props = rescale_current_sense_shunt_props,
0480     },
0481     [VOLTAGE_DIVIDER] = {
0482         .type = IIO_VOLTAGE,
0483         .props = rescale_voltage_divider_props,
0484     },
0485     [TEMP_SENSE_RTD] = {
0486         .type = IIO_TEMP,
0487         .props = rescale_temp_sense_rtd_props,
0488     },
0489     [TEMP_TRANSDUCER] = {
0490         .type = IIO_TEMP,
0491         .props = rescale_temp_transducer_props,
0492     },
0493 };
0494 
0495 static const struct of_device_id rescale_match[] = {
0496     { .compatible = "current-sense-amplifier",
0497       .data = &rescale_cfg[CURRENT_SENSE_AMPLIFIER], },
0498     { .compatible = "current-sense-shunt",
0499       .data = &rescale_cfg[CURRENT_SENSE_SHUNT], },
0500     { .compatible = "voltage-divider",
0501       .data = &rescale_cfg[VOLTAGE_DIVIDER], },
0502     { .compatible = "temperature-sense-rtd",
0503       .data = &rescale_cfg[TEMP_SENSE_RTD], },
0504     { .compatible = "temperature-transducer",
0505       .data = &rescale_cfg[TEMP_TRANSDUCER], },
0506     { /* sentinel */ }
0507 };
0508 MODULE_DEVICE_TABLE(of, rescale_match);
0509 
0510 static int rescale_probe(struct platform_device *pdev)
0511 {
0512     struct device *dev = &pdev->dev;
0513     struct iio_dev *indio_dev;
0514     struct iio_channel *source;
0515     struct rescale *rescale;
0516     int sizeof_ext_info;
0517     int sizeof_priv;
0518     int i;
0519     int ret;
0520 
0521     source = devm_iio_channel_get(dev, NULL);
0522     if (IS_ERR(source))
0523         return dev_err_probe(dev, PTR_ERR(source),
0524                      "failed to get source channel\n");
0525 
0526     sizeof_ext_info = iio_get_channel_ext_info_count(source);
0527     if (sizeof_ext_info) {
0528         sizeof_ext_info += 1; /* one extra entry for the sentinel */
0529         sizeof_ext_info *= sizeof(*rescale->ext_info);
0530     }
0531 
0532     sizeof_priv = sizeof(*rescale) + sizeof_ext_info;
0533 
0534     indio_dev = devm_iio_device_alloc(dev, sizeof_priv);
0535     if (!indio_dev)
0536         return -ENOMEM;
0537 
0538     rescale = iio_priv(indio_dev);
0539 
0540     rescale->cfg = device_get_match_data(dev);
0541     rescale->numerator = 1;
0542     rescale->denominator = 1;
0543     rescale->offset = 0;
0544 
0545     ret = rescale->cfg->props(dev, rescale);
0546     if (ret)
0547         return ret;
0548 
0549     if (!rescale->numerator || !rescale->denominator) {
0550         dev_err(dev, "invalid scaling factor.\n");
0551         return -EINVAL;
0552     }
0553 
0554     platform_set_drvdata(pdev, indio_dev);
0555 
0556     rescale->source = source;
0557 
0558     indio_dev->name = dev_name(dev);
0559     indio_dev->info = &rescale_info;
0560     indio_dev->modes = INDIO_DIRECT_MODE;
0561     indio_dev->channels = &rescale->chan;
0562     indio_dev->num_channels = 1;
0563     if (sizeof_ext_info) {
0564         rescale->ext_info = devm_kmemdup(dev,
0565                          source->channel->ext_info,
0566                          sizeof_ext_info, GFP_KERNEL);
0567         if (!rescale->ext_info)
0568             return -ENOMEM;
0569 
0570         for (i = 0; rescale->ext_info[i].name; ++i) {
0571             struct iio_chan_spec_ext_info *ext_info =
0572                 &rescale->ext_info[i];
0573 
0574             if (source->channel->ext_info[i].read)
0575                 ext_info->read = rescale_read_ext_info;
0576             if (source->channel->ext_info[i].write)
0577                 ext_info->write = rescale_write_ext_info;
0578             ext_info->private = i;
0579         }
0580     }
0581 
0582     ret = rescale_configure_channel(dev, rescale);
0583     if (ret)
0584         return ret;
0585 
0586     return devm_iio_device_register(dev, indio_dev);
0587 }
0588 
0589 static struct platform_driver rescale_driver = {
0590     .probe = rescale_probe,
0591     .driver = {
0592         .name = "iio-rescale",
0593         .of_match_table = rescale_match,
0594     },
0595 };
0596 module_platform_driver(rescale_driver);
0597 
0598 MODULE_DESCRIPTION("IIO rescale driver");
0599 MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
0600 MODULE_LICENSE("GPL v2");
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!