1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 * IIO rescale driver 4 * 5 * Copyright (C) 2018 Axentia Technologies AB 6 * 7 * Author: Peter Rosin <peda@axentia.se> 8 */ 9 10 #include <linux/err.h> 11 #include <linux/gcd.h> 12 #include <linux/module.h> 13 #include <linux/of.h> 14 #include <linux/of_device.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 22 int rescale_process_scale(struct rescale *rescale, int scale_type, 23 int *val, int *val2) 24 { 25 s64 tmp; 26 27 switch (scale_type) { 28 case IIO_VAL_FRACTIONAL: 29 *val *= rescale->numerator; 30 *val2 *= rescale->denominator; 31 return 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_LOG2: 39 tmp = (s64)*val * 1000000000LL; 40 tmp = div_s64(tmp, rescale->denominator); 41 tmp *= rescale->numerator; 42 tmp = div_s64(tmp, 1000000000LL); 43 *val = tmp; 44 return scale_type; 45 default: 46 return -EOPNOTSUPP; 47 } 48 } 49 50 static int rescale_read_raw(struct iio_dev *indio_dev, 51 struct iio_chan_spec const *chan, 52 int *val, int *val2, long mask) 53 { 54 struct rescale *rescale = iio_priv(indio_dev); 55 int ret; 56 57 switch (mask) { 58 case IIO_CHAN_INFO_RAW: 59 if (rescale->chan_processed) 60 /* 61 * When only processed channels are supported, we 62 * read the processed data and scale it by 1/1 63 * augmented with whatever the rescaler has calculated. 64 */ 65 return iio_read_channel_processed(rescale->source, val); 66 else 67 return iio_read_channel_raw(rescale->source, val); 68 69 case IIO_CHAN_INFO_SCALE: 70 if (rescale->chan_processed) { 71 /* 72 * Processed channels are scaled 1-to-1 73 */ 74 *val = 1; 75 *val2 = 1; 76 ret = IIO_VAL_FRACTIONAL; 77 } else { 78 ret = iio_read_channel_scale(rescale->source, val, val2); 79 } 80 return rescale_process_scale(rescale, ret, val, val2); 81 default: 82 return -EINVAL; 83 } 84 } 85 86 static int rescale_read_avail(struct iio_dev *indio_dev, 87 struct iio_chan_spec const *chan, 88 const int **vals, int *type, int *length, 89 long mask) 90 { 91 struct rescale *rescale = iio_priv(indio_dev); 92 93 switch (mask) { 94 case IIO_CHAN_INFO_RAW: 95 *type = IIO_VAL_INT; 96 return iio_read_avail_channel_raw(rescale->source, 97 vals, length); 98 default: 99 return -EINVAL; 100 } 101 } 102 103 static const struct iio_info rescale_info = { 104 .read_raw = rescale_read_raw, 105 .read_avail = rescale_read_avail, 106 }; 107 108 static ssize_t rescale_read_ext_info(struct iio_dev *indio_dev, 109 uintptr_t private, 110 struct iio_chan_spec const *chan, 111 char *buf) 112 { 113 struct rescale *rescale = iio_priv(indio_dev); 114 115 return iio_read_channel_ext_info(rescale->source, 116 rescale->ext_info[private].name, 117 buf); 118 } 119 120 static ssize_t rescale_write_ext_info(struct iio_dev *indio_dev, 121 uintptr_t private, 122 struct iio_chan_spec const *chan, 123 const char *buf, size_t len) 124 { 125 struct rescale *rescale = iio_priv(indio_dev); 126 127 return iio_write_channel_ext_info(rescale->source, 128 rescale->ext_info[private].name, 129 buf, len); 130 } 131 132 static int rescale_configure_channel(struct device *dev, 133 struct rescale *rescale) 134 { 135 struct iio_chan_spec *chan = &rescale->chan; 136 struct iio_chan_spec const *schan = rescale->source->channel; 137 138 chan->indexed = 1; 139 chan->output = schan->output; 140 chan->ext_info = rescale->ext_info; 141 chan->type = rescale->cfg->type; 142 143 if (iio_channel_has_info(schan, IIO_CHAN_INFO_RAW) || 144 iio_channel_has_info(schan, IIO_CHAN_INFO_SCALE)) { 145 dev_info(dev, "using raw+scale source channel\n"); 146 } else if (iio_channel_has_info(schan, IIO_CHAN_INFO_PROCESSED)) { 147 dev_info(dev, "using processed channel\n"); 148 rescale->chan_processed = true; 149 } else { 150 dev_err(dev, "source channel is not supported\n"); 151 return -EINVAL; 152 } 153 154 chan->info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | 155 BIT(IIO_CHAN_INFO_SCALE); 156 157 /* 158 * Using .read_avail() is fringe to begin with and makes no sense 159 * whatsoever for processed channels, so we make sure that this cannot 160 * be called on a processed channel. 161 */ 162 if (iio_channel_has_available(schan, IIO_CHAN_INFO_RAW) && 163 !rescale->chan_processed) 164 chan->info_mask_separate_available |= BIT(IIO_CHAN_INFO_RAW); 165 166 return 0; 167 } 168 169 static int rescale_current_sense_amplifier_props(struct device *dev, 170 struct rescale *rescale) 171 { 172 u32 sense; 173 u32 gain_mult = 1; 174 u32 gain_div = 1; 175 u32 factor; 176 int ret; 177 178 ret = device_property_read_u32(dev, "sense-resistor-micro-ohms", 179 &sense); 180 if (ret) { 181 dev_err(dev, "failed to read the sense resistance: %d\n", ret); 182 return ret; 183 } 184 185 device_property_read_u32(dev, "sense-gain-mult", &gain_mult); 186 device_property_read_u32(dev, "sense-gain-div", &gain_div); 187 188 /* 189 * Calculate the scaling factor, 1 / (gain * sense), or 190 * gain_div / (gain_mult * sense), while trying to keep the 191 * numerator/denominator from overflowing. 192 */ 193 factor = gcd(sense, 1000000); 194 rescale->numerator = 1000000 / factor; 195 rescale->denominator = sense / factor; 196 197 factor = gcd(rescale->numerator, gain_mult); 198 rescale->numerator /= factor; 199 rescale->denominator *= gain_mult / factor; 200 201 factor = gcd(rescale->denominator, gain_div); 202 rescale->numerator *= gain_div / factor; 203 rescale->denominator /= factor; 204 205 return 0; 206 } 207 208 static int rescale_current_sense_shunt_props(struct device *dev, 209 struct rescale *rescale) 210 { 211 u32 shunt; 212 u32 factor; 213 int ret; 214 215 ret = device_property_read_u32(dev, "shunt-resistor-micro-ohms", 216 &shunt); 217 if (ret) { 218 dev_err(dev, "failed to read the shunt resistance: %d\n", ret); 219 return ret; 220 } 221 222 factor = gcd(shunt, 1000000); 223 rescale->numerator = 1000000 / factor; 224 rescale->denominator = shunt / factor; 225 226 return 0; 227 } 228 229 static int rescale_voltage_divider_props(struct device *dev, 230 struct rescale *rescale) 231 { 232 int ret; 233 u32 factor; 234 235 ret = device_property_read_u32(dev, "output-ohms", 236 &rescale->denominator); 237 if (ret) { 238 dev_err(dev, "failed to read output-ohms: %d\n", ret); 239 return ret; 240 } 241 242 ret = device_property_read_u32(dev, "full-ohms", 243 &rescale->numerator); 244 if (ret) { 245 dev_err(dev, "failed to read full-ohms: %d\n", ret); 246 return ret; 247 } 248 249 factor = gcd(rescale->numerator, rescale->denominator); 250 rescale->numerator /= factor; 251 rescale->denominator /= factor; 252 253 return 0; 254 } 255 256 enum rescale_variant { 257 CURRENT_SENSE_AMPLIFIER, 258 CURRENT_SENSE_SHUNT, 259 VOLTAGE_DIVIDER, 260 }; 261 262 static const struct rescale_cfg rescale_cfg[] = { 263 [CURRENT_SENSE_AMPLIFIER] = { 264 .type = IIO_CURRENT, 265 .props = rescale_current_sense_amplifier_props, 266 }, 267 [CURRENT_SENSE_SHUNT] = { 268 .type = IIO_CURRENT, 269 .props = rescale_current_sense_shunt_props, 270 }, 271 [VOLTAGE_DIVIDER] = { 272 .type = IIO_VOLTAGE, 273 .props = rescale_voltage_divider_props, 274 }, 275 }; 276 277 static const struct of_device_id rescale_match[] = { 278 { .compatible = "current-sense-amplifier", 279 .data = &rescale_cfg[CURRENT_SENSE_AMPLIFIER], }, 280 { .compatible = "current-sense-shunt", 281 .data = &rescale_cfg[CURRENT_SENSE_SHUNT], }, 282 { .compatible = "voltage-divider", 283 .data = &rescale_cfg[VOLTAGE_DIVIDER], }, 284 { /* sentinel */ } 285 }; 286 MODULE_DEVICE_TABLE(of, rescale_match); 287 288 static int rescale_probe(struct platform_device *pdev) 289 { 290 struct device *dev = &pdev->dev; 291 struct iio_dev *indio_dev; 292 struct iio_channel *source; 293 struct rescale *rescale; 294 int sizeof_ext_info; 295 int sizeof_priv; 296 int i; 297 int ret; 298 299 source = devm_iio_channel_get(dev, NULL); 300 if (IS_ERR(source)) 301 return dev_err_probe(dev, PTR_ERR(source), 302 "failed to get source channel\n"); 303 304 sizeof_ext_info = iio_get_channel_ext_info_count(source); 305 if (sizeof_ext_info) { 306 sizeof_ext_info += 1; /* one extra entry for the sentinel */ 307 sizeof_ext_info *= sizeof(*rescale->ext_info); 308 } 309 310 sizeof_priv = sizeof(*rescale) + sizeof_ext_info; 311 312 indio_dev = devm_iio_device_alloc(dev, sizeof_priv); 313 if (!indio_dev) 314 return -ENOMEM; 315 316 rescale = iio_priv(indio_dev); 317 318 rescale->cfg = of_device_get_match_data(dev); 319 rescale->numerator = 1; 320 rescale->denominator = 1; 321 322 ret = rescale->cfg->props(dev, rescale); 323 if (ret) 324 return ret; 325 326 if (!rescale->numerator || !rescale->denominator) { 327 dev_err(dev, "invalid scaling factor.\n"); 328 return -EINVAL; 329 } 330 331 platform_set_drvdata(pdev, indio_dev); 332 333 rescale->source = source; 334 335 indio_dev->name = dev_name(dev); 336 indio_dev->info = &rescale_info; 337 indio_dev->modes = INDIO_DIRECT_MODE; 338 indio_dev->channels = &rescale->chan; 339 indio_dev->num_channels = 1; 340 if (sizeof_ext_info) { 341 rescale->ext_info = devm_kmemdup(dev, 342 source->channel->ext_info, 343 sizeof_ext_info, GFP_KERNEL); 344 if (!rescale->ext_info) 345 return -ENOMEM; 346 347 for (i = 0; rescale->ext_info[i].name; ++i) { 348 struct iio_chan_spec_ext_info *ext_info = 349 &rescale->ext_info[i]; 350 351 if (source->channel->ext_info[i].read) 352 ext_info->read = rescale_read_ext_info; 353 if (source->channel->ext_info[i].write) 354 ext_info->write = rescale_write_ext_info; 355 ext_info->private = i; 356 } 357 } 358 359 ret = rescale_configure_channel(dev, rescale); 360 if (ret) 361 return ret; 362 363 return devm_iio_device_register(dev, indio_dev); 364 } 365 366 static struct platform_driver rescale_driver = { 367 .probe = rescale_probe, 368 .driver = { 369 .name = "iio-rescale", 370 .of_match_table = rescale_match, 371 }, 372 }; 373 module_platform_driver(rescale_driver); 374 375 MODULE_DESCRIPTION("IIO rescale driver"); 376 MODULE_AUTHOR("Peter Rosin <peda@axentia.se>"); 377 MODULE_LICENSE("GPL v2"); 378