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