1/* 2 * STMicroelectronics sensors core library driver 3 * 4 * Copyright 2012-2013 STMicroelectronics Inc. 5 * 6 * Denis Ciocca <denis.ciocca@st.com> 7 * 8 * Licensed under the GPL-2. 9 */ 10 11#include <linux/kernel.h> 12#include <linux/module.h> 13#include <linux/slab.h> 14#include <linux/delay.h> 15#include <linux/iio/iio.h> 16#include <linux/regulator/consumer.h> 17#include <linux/of.h> 18#include <asm/unaligned.h> 19#include <linux/iio/common/st_sensors.h> 20 21 22#define ST_SENSORS_WAI_ADDRESS 0x0f 23 24static inline u32 st_sensors_get_unaligned_le24(const u8 *p) 25{ 26 return (s32)((p[0] | p[1] << 8 | p[2] << 16) << 8) >> 8; 27} 28 29static int st_sensors_write_data_with_mask(struct iio_dev *indio_dev, 30 u8 reg_addr, u8 mask, u8 data) 31{ 32 int err; 33 u8 new_data; 34 struct st_sensor_data *sdata = iio_priv(indio_dev); 35 36 err = sdata->tf->read_byte(&sdata->tb, sdata->dev, reg_addr, &new_data); 37 if (err < 0) 38 goto st_sensors_write_data_with_mask_error; 39 40 new_data = ((new_data & (~mask)) | ((data << __ffs(mask)) & mask)); 41 err = sdata->tf->write_byte(&sdata->tb, sdata->dev, reg_addr, new_data); 42 43st_sensors_write_data_with_mask_error: 44 return err; 45} 46 47static int st_sensors_match_odr(struct st_sensors *sensor, 48 unsigned int odr, struct st_sensor_odr_avl *odr_out) 49{ 50 int i, ret = -EINVAL; 51 52 for (i = 0; i < ST_SENSORS_ODR_LIST_MAX; i++) { 53 if (sensor->odr.odr_avl[i].hz == 0) 54 goto st_sensors_match_odr_error; 55 56 if (sensor->odr.odr_avl[i].hz == odr) { 57 odr_out->hz = sensor->odr.odr_avl[i].hz; 58 odr_out->value = sensor->odr.odr_avl[i].value; 59 ret = 0; 60 break; 61 } 62 } 63 64st_sensors_match_odr_error: 65 return ret; 66} 67 68int st_sensors_set_odr(struct iio_dev *indio_dev, unsigned int odr) 69{ 70 int err; 71 struct st_sensor_odr_avl odr_out = {0, 0}; 72 struct st_sensor_data *sdata = iio_priv(indio_dev); 73 74 err = st_sensors_match_odr(sdata->sensor, odr, &odr_out); 75 if (err < 0) 76 goto st_sensors_match_odr_error; 77 78 if ((sdata->sensor->odr.addr == sdata->sensor->pw.addr) && 79 (sdata->sensor->odr.mask == sdata->sensor->pw.mask)) { 80 if (sdata->enabled == true) { 81 err = st_sensors_write_data_with_mask(indio_dev, 82 sdata->sensor->odr.addr, 83 sdata->sensor->odr.mask, 84 odr_out.value); 85 } else { 86 err = 0; 87 } 88 } else { 89 err = st_sensors_write_data_with_mask(indio_dev, 90 sdata->sensor->odr.addr, sdata->sensor->odr.mask, 91 odr_out.value); 92 } 93 if (err >= 0) 94 sdata->odr = odr_out.hz; 95 96st_sensors_match_odr_error: 97 return err; 98} 99EXPORT_SYMBOL(st_sensors_set_odr); 100 101static int st_sensors_match_fs(struct st_sensors *sensor, 102 unsigned int fs, int *index_fs_avl) 103{ 104 int i, ret = -EINVAL; 105 106 for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) { 107 if (sensor->fs.fs_avl[i].num == 0) 108 goto st_sensors_match_odr_error; 109 110 if (sensor->fs.fs_avl[i].num == fs) { 111 *index_fs_avl = i; 112 ret = 0; 113 break; 114 } 115 } 116 117st_sensors_match_odr_error: 118 return ret; 119} 120 121static int st_sensors_set_fullscale(struct iio_dev *indio_dev, 122 unsigned int fs) 123{ 124 int err, i = 0; 125 struct st_sensor_data *sdata = iio_priv(indio_dev); 126 127 err = st_sensors_match_fs(sdata->sensor, fs, &i); 128 if (err < 0) 129 goto st_accel_set_fullscale_error; 130 131 err = st_sensors_write_data_with_mask(indio_dev, 132 sdata->sensor->fs.addr, 133 sdata->sensor->fs.mask, 134 sdata->sensor->fs.fs_avl[i].value); 135 if (err < 0) 136 goto st_accel_set_fullscale_error; 137 138 sdata->current_fullscale = (struct st_sensor_fullscale_avl *) 139 &sdata->sensor->fs.fs_avl[i]; 140 return err; 141 142st_accel_set_fullscale_error: 143 dev_err(&indio_dev->dev, "failed to set new fullscale.\n"); 144 return err; 145} 146 147int st_sensors_set_enable(struct iio_dev *indio_dev, bool enable) 148{ 149 u8 tmp_value; 150 int err = -EINVAL; 151 bool found = false; 152 struct st_sensor_odr_avl odr_out = {0, 0}; 153 struct st_sensor_data *sdata = iio_priv(indio_dev); 154 155 if (enable) { 156 tmp_value = sdata->sensor->pw.value_on; 157 if ((sdata->sensor->odr.addr == sdata->sensor->pw.addr) && 158 (sdata->sensor->odr.mask == sdata->sensor->pw.mask)) { 159 err = st_sensors_match_odr(sdata->sensor, 160 sdata->odr, &odr_out); 161 if (err < 0) 162 goto set_enable_error; 163 tmp_value = odr_out.value; 164 found = true; 165 } 166 err = st_sensors_write_data_with_mask(indio_dev, 167 sdata->sensor->pw.addr, 168 sdata->sensor->pw.mask, tmp_value); 169 if (err < 0) 170 goto set_enable_error; 171 172 sdata->enabled = true; 173 174 if (found) 175 sdata->odr = odr_out.hz; 176 } else { 177 err = st_sensors_write_data_with_mask(indio_dev, 178 sdata->sensor->pw.addr, 179 sdata->sensor->pw.mask, 180 sdata->sensor->pw.value_off); 181 if (err < 0) 182 goto set_enable_error; 183 184 sdata->enabled = false; 185 } 186 187set_enable_error: 188 return err; 189} 190EXPORT_SYMBOL(st_sensors_set_enable); 191 192int st_sensors_set_axis_enable(struct iio_dev *indio_dev, u8 axis_enable) 193{ 194 struct st_sensor_data *sdata = iio_priv(indio_dev); 195 196 return st_sensors_write_data_with_mask(indio_dev, 197 sdata->sensor->enable_axis.addr, 198 sdata->sensor->enable_axis.mask, axis_enable); 199} 200EXPORT_SYMBOL(st_sensors_set_axis_enable); 201 202void st_sensors_power_enable(struct iio_dev *indio_dev) 203{ 204 struct st_sensor_data *pdata = iio_priv(indio_dev); 205 int err; 206 207 /* Regulators not mandatory, but if requested we should enable them. */ 208 pdata->vdd = devm_regulator_get_optional(indio_dev->dev.parent, "vdd"); 209 if (!IS_ERR(pdata->vdd)) { 210 err = regulator_enable(pdata->vdd); 211 if (err != 0) 212 dev_warn(&indio_dev->dev, 213 "Failed to enable specified Vdd supply\n"); 214 } 215 216 pdata->vdd_io = devm_regulator_get_optional(indio_dev->dev.parent, "vddio"); 217 if (!IS_ERR(pdata->vdd_io)) { 218 err = regulator_enable(pdata->vdd_io); 219 if (err != 0) 220 dev_warn(&indio_dev->dev, 221 "Failed to enable specified Vdd_IO supply\n"); 222 } 223} 224EXPORT_SYMBOL(st_sensors_power_enable); 225 226void st_sensors_power_disable(struct iio_dev *indio_dev) 227{ 228 struct st_sensor_data *pdata = iio_priv(indio_dev); 229 230 if (!IS_ERR(pdata->vdd)) 231 regulator_disable(pdata->vdd); 232 233 if (!IS_ERR(pdata->vdd_io)) 234 regulator_disable(pdata->vdd_io); 235} 236EXPORT_SYMBOL(st_sensors_power_disable); 237 238static int st_sensors_set_drdy_int_pin(struct iio_dev *indio_dev, 239 struct st_sensors_platform_data *pdata) 240{ 241 struct st_sensor_data *sdata = iio_priv(indio_dev); 242 243 switch (pdata->drdy_int_pin) { 244 case 1: 245 if (sdata->sensor->drdy_irq.mask_int1 == 0) { 246 dev_err(&indio_dev->dev, 247 "DRDY on INT1 not available.\n"); 248 return -EINVAL; 249 } 250 sdata->drdy_int_pin = 1; 251 break; 252 case 2: 253 if (sdata->sensor->drdy_irq.mask_int2 == 0) { 254 dev_err(&indio_dev->dev, 255 "DRDY on INT2 not available.\n"); 256 return -EINVAL; 257 } 258 sdata->drdy_int_pin = 2; 259 break; 260 default: 261 dev_err(&indio_dev->dev, "DRDY on pdata not valid.\n"); 262 return -EINVAL; 263 } 264 265 return 0; 266} 267 268#ifdef CONFIG_OF 269static struct st_sensors_platform_data *st_sensors_of_probe(struct device *dev, 270 struct st_sensors_platform_data *defdata) 271{ 272 struct st_sensors_platform_data *pdata; 273 struct device_node *np = dev->of_node; 274 u32 val; 275 276 if (!np) 277 return NULL; 278 279 pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL); 280 if (!of_property_read_u32(np, "st,drdy-int-pin", &val) && (val <= 2)) 281 pdata->drdy_int_pin = (u8) val; 282 else 283 pdata->drdy_int_pin = defdata ? defdata->drdy_int_pin : 1; 284 285 return pdata; 286} 287#else 288static struct st_sensors_platform_data *st_sensors_of_probe(struct device *dev, 289 struct st_sensors_platform_data *defdata) 290{ 291 return NULL; 292} 293#endif 294 295int st_sensors_init_sensor(struct iio_dev *indio_dev, 296 struct st_sensors_platform_data *pdata) 297{ 298 struct st_sensor_data *sdata = iio_priv(indio_dev); 299 struct st_sensors_platform_data *of_pdata; 300 int err = 0; 301 302 mutex_init(&sdata->tb.buf_lock); 303 304 /* If OF/DT pdata exists, it will take precedence of anything else */ 305 of_pdata = st_sensors_of_probe(indio_dev->dev.parent, pdata); 306 if (of_pdata) 307 pdata = of_pdata; 308 309 if (pdata) { 310 err = st_sensors_set_drdy_int_pin(indio_dev, pdata); 311 if (err < 0) 312 return err; 313 } 314 315 err = st_sensors_set_enable(indio_dev, false); 316 if (err < 0) 317 return err; 318 319 if (sdata->current_fullscale) { 320 err = st_sensors_set_fullscale(indio_dev, 321 sdata->current_fullscale->num); 322 if (err < 0) 323 return err; 324 } else 325 dev_info(&indio_dev->dev, "Full-scale not possible\n"); 326 327 err = st_sensors_set_odr(indio_dev, sdata->odr); 328 if (err < 0) 329 return err; 330 331 /* set BDU */ 332 err = st_sensors_write_data_with_mask(indio_dev, 333 sdata->sensor->bdu.addr, sdata->sensor->bdu.mask, true); 334 if (err < 0) 335 return err; 336 337 err = st_sensors_set_axis_enable(indio_dev, ST_SENSORS_ENABLE_ALL_AXIS); 338 339 return err; 340} 341EXPORT_SYMBOL(st_sensors_init_sensor); 342 343int st_sensors_set_dataready_irq(struct iio_dev *indio_dev, bool enable) 344{ 345 int err; 346 u8 drdy_mask; 347 struct st_sensor_data *sdata = iio_priv(indio_dev); 348 349 if (!sdata->sensor->drdy_irq.addr) 350 return 0; 351 352 /* Enable/Disable the interrupt generator 1. */ 353 if (sdata->sensor->drdy_irq.ig1.en_addr > 0) { 354 err = st_sensors_write_data_with_mask(indio_dev, 355 sdata->sensor->drdy_irq.ig1.en_addr, 356 sdata->sensor->drdy_irq.ig1.en_mask, (int)enable); 357 if (err < 0) 358 goto st_accel_set_dataready_irq_error; 359 } 360 361 if (sdata->drdy_int_pin == 1) 362 drdy_mask = sdata->sensor->drdy_irq.mask_int1; 363 else 364 drdy_mask = sdata->sensor->drdy_irq.mask_int2; 365 366 /* Enable/Disable the interrupt generator for data ready. */ 367 err = st_sensors_write_data_with_mask(indio_dev, 368 sdata->sensor->drdy_irq.addr, drdy_mask, (int)enable); 369 370st_accel_set_dataready_irq_error: 371 return err; 372} 373EXPORT_SYMBOL(st_sensors_set_dataready_irq); 374 375int st_sensors_set_fullscale_by_gain(struct iio_dev *indio_dev, int scale) 376{ 377 int err = -EINVAL, i; 378 struct st_sensor_data *sdata = iio_priv(indio_dev); 379 380 for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) { 381 if ((sdata->sensor->fs.fs_avl[i].gain == scale) && 382 (sdata->sensor->fs.fs_avl[i].gain != 0)) { 383 err = 0; 384 break; 385 } 386 } 387 if (err < 0) 388 goto st_sensors_match_scale_error; 389 390 err = st_sensors_set_fullscale(indio_dev, 391 sdata->sensor->fs.fs_avl[i].num); 392 393st_sensors_match_scale_error: 394 return err; 395} 396EXPORT_SYMBOL(st_sensors_set_fullscale_by_gain); 397 398static int st_sensors_read_axis_data(struct iio_dev *indio_dev, 399 struct iio_chan_spec const *ch, int *data) 400{ 401 int err; 402 u8 *outdata; 403 struct st_sensor_data *sdata = iio_priv(indio_dev); 404 unsigned int byte_for_channel = ch->scan_type.storagebits >> 3; 405 406 outdata = kmalloc(byte_for_channel, GFP_KERNEL); 407 if (!outdata) 408 return -ENOMEM; 409 410 err = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev, 411 ch->address, byte_for_channel, 412 outdata, sdata->multiread_bit); 413 if (err < 0) 414 goto st_sensors_free_memory; 415 416 if (byte_for_channel == 2) 417 *data = (s16)get_unaligned_le16(outdata); 418 else if (byte_for_channel == 3) 419 *data = (s32)st_sensors_get_unaligned_le24(outdata); 420 421st_sensors_free_memory: 422 kfree(outdata); 423 424 return err; 425} 426 427int st_sensors_read_info_raw(struct iio_dev *indio_dev, 428 struct iio_chan_spec const *ch, int *val) 429{ 430 int err; 431 struct st_sensor_data *sdata = iio_priv(indio_dev); 432 433 mutex_lock(&indio_dev->mlock); 434 if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { 435 err = -EBUSY; 436 goto out; 437 } else { 438 err = st_sensors_set_enable(indio_dev, true); 439 if (err < 0) 440 goto out; 441 442 msleep((sdata->sensor->bootime * 1000) / sdata->odr); 443 err = st_sensors_read_axis_data(indio_dev, ch, val); 444 if (err < 0) 445 goto out; 446 447 *val = *val >> ch->scan_type.shift; 448 449 err = st_sensors_set_enable(indio_dev, false); 450 } 451out: 452 mutex_unlock(&indio_dev->mlock); 453 454 return err; 455} 456EXPORT_SYMBOL(st_sensors_read_info_raw); 457 458int st_sensors_check_device_support(struct iio_dev *indio_dev, 459 int num_sensors_list, const struct st_sensors *sensors) 460{ 461 u8 wai; 462 int i, n, err; 463 struct st_sensor_data *sdata = iio_priv(indio_dev); 464 465 err = sdata->tf->read_byte(&sdata->tb, sdata->dev, 466 ST_SENSORS_DEFAULT_WAI_ADDRESS, &wai); 467 if (err < 0) { 468 dev_err(&indio_dev->dev, "failed to read Who-Am-I register.\n"); 469 goto read_wai_error; 470 } 471 472 for (i = 0; i < num_sensors_list; i++) { 473 if (sensors[i].wai == wai) 474 break; 475 } 476 if (i == num_sensors_list) 477 goto device_not_supported; 478 479 for (n = 0; n < ARRAY_SIZE(sensors[i].sensors_supported); n++) { 480 if (strcmp(indio_dev->name, 481 &sensors[i].sensors_supported[n][0]) == 0) 482 break; 483 } 484 if (n == ARRAY_SIZE(sensors[i].sensors_supported)) { 485 dev_err(&indio_dev->dev, "device name and WhoAmI mismatch.\n"); 486 goto sensor_name_mismatch; 487 } 488 489 sdata->sensor = (struct st_sensors *)&sensors[i]; 490 491 return i; 492 493device_not_supported: 494 dev_err(&indio_dev->dev, "device not supported: WhoAmI (0x%x).\n", wai); 495sensor_name_mismatch: 496 err = -ENODEV; 497read_wai_error: 498 return err; 499} 500EXPORT_SYMBOL(st_sensors_check_device_support); 501 502ssize_t st_sensors_sysfs_sampling_frequency_avail(struct device *dev, 503 struct device_attribute *attr, char *buf) 504{ 505 int i, len = 0; 506 struct iio_dev *indio_dev = dev_get_drvdata(dev); 507 struct st_sensor_data *sdata = iio_priv(indio_dev); 508 509 mutex_lock(&indio_dev->mlock); 510 for (i = 0; i < ST_SENSORS_ODR_LIST_MAX; i++) { 511 if (sdata->sensor->odr.odr_avl[i].hz == 0) 512 break; 513 514 len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", 515 sdata->sensor->odr.odr_avl[i].hz); 516 } 517 mutex_unlock(&indio_dev->mlock); 518 buf[len - 1] = '\n'; 519 520 return len; 521} 522EXPORT_SYMBOL(st_sensors_sysfs_sampling_frequency_avail); 523 524ssize_t st_sensors_sysfs_scale_avail(struct device *dev, 525 struct device_attribute *attr, char *buf) 526{ 527 int i, len = 0; 528 struct iio_dev *indio_dev = dev_get_drvdata(dev); 529 struct st_sensor_data *sdata = iio_priv(indio_dev); 530 531 mutex_lock(&indio_dev->mlock); 532 for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) { 533 if (sdata->sensor->fs.fs_avl[i].num == 0) 534 break; 535 536 len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ", 537 sdata->sensor->fs.fs_avl[i].gain); 538 } 539 mutex_unlock(&indio_dev->mlock); 540 buf[len - 1] = '\n'; 541 542 return len; 543} 544EXPORT_SYMBOL(st_sensors_sysfs_scale_avail); 545 546MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>"); 547MODULE_DESCRIPTION("STMicroelectronics ST-sensors core"); 548MODULE_LICENSE("GPL v2"); 549