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