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 #include "st_sensors_core.h" 22 23 static inline u32 st_sensors_get_unaligned_le24(const u8 *p) 24 { 25 return (s32)((p[0] | p[1] << 8 | p[2] << 16) << 8) >> 8; 26 } 27 28 int st_sensors_write_data_with_mask(struct iio_dev *indio_dev, 29 u8 reg_addr, u8 mask, u8 data) 30 { 31 int err; 32 u8 new_data; 33 struct st_sensor_data *sdata = iio_priv(indio_dev); 34 35 err = sdata->tf->read_byte(&sdata->tb, sdata->dev, reg_addr, &new_data); 36 if (err < 0) 37 goto st_sensors_write_data_with_mask_error; 38 39 new_data = ((new_data & (~mask)) | ((data << __ffs(mask)) & mask)); 40 err = sdata->tf->write_byte(&sdata->tb, sdata->dev, reg_addr, new_data); 41 42 st_sensors_write_data_with_mask_error: 43 return err; 44 } 45 46 int st_sensors_debugfs_reg_access(struct iio_dev *indio_dev, 47 unsigned reg, unsigned writeval, 48 unsigned *readval) 49 { 50 struct st_sensor_data *sdata = iio_priv(indio_dev); 51 u8 readdata; 52 int err; 53 54 if (!readval) 55 return sdata->tf->write_byte(&sdata->tb, sdata->dev, 56 (u8)reg, (u8)writeval); 57 58 err = sdata->tf->read_byte(&sdata->tb, sdata->dev, (u8)reg, &readdata); 59 if (err < 0) 60 return err; 61 62 *readval = (unsigned)readdata; 63 64 return 0; 65 } 66 EXPORT_SYMBOL(st_sensors_debugfs_reg_access); 67 68 static int st_sensors_match_odr(struct st_sensor_settings *sensor_settings, 69 unsigned int odr, struct st_sensor_odr_avl *odr_out) 70 { 71 int i, ret = -EINVAL; 72 73 for (i = 0; i < ST_SENSORS_ODR_LIST_MAX; i++) { 74 if (sensor_settings->odr.odr_avl[i].hz == 0) 75 goto st_sensors_match_odr_error; 76 77 if (sensor_settings->odr.odr_avl[i].hz == odr) { 78 odr_out->hz = sensor_settings->odr.odr_avl[i].hz; 79 odr_out->value = sensor_settings->odr.odr_avl[i].value; 80 ret = 0; 81 break; 82 } 83 } 84 85 st_sensors_match_odr_error: 86 return ret; 87 } 88 89 int st_sensors_set_odr(struct iio_dev *indio_dev, unsigned int odr) 90 { 91 int err; 92 struct st_sensor_odr_avl odr_out = {0, 0}; 93 struct st_sensor_data *sdata = iio_priv(indio_dev); 94 95 err = st_sensors_match_odr(sdata->sensor_settings, odr, &odr_out); 96 if (err < 0) 97 goto st_sensors_match_odr_error; 98 99 if ((sdata->sensor_settings->odr.addr == 100 sdata->sensor_settings->pw.addr) && 101 (sdata->sensor_settings->odr.mask == 102 sdata->sensor_settings->pw.mask)) { 103 if (sdata->enabled == true) { 104 err = st_sensors_write_data_with_mask(indio_dev, 105 sdata->sensor_settings->odr.addr, 106 sdata->sensor_settings->odr.mask, 107 odr_out.value); 108 } else { 109 err = 0; 110 } 111 } else { 112 err = st_sensors_write_data_with_mask(indio_dev, 113 sdata->sensor_settings->odr.addr, 114 sdata->sensor_settings->odr.mask, 115 odr_out.value); 116 } 117 if (err >= 0) 118 sdata->odr = odr_out.hz; 119 120 st_sensors_match_odr_error: 121 return err; 122 } 123 EXPORT_SYMBOL(st_sensors_set_odr); 124 125 static int st_sensors_match_fs(struct st_sensor_settings *sensor_settings, 126 unsigned int fs, int *index_fs_avl) 127 { 128 int i, ret = -EINVAL; 129 130 for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) { 131 if (sensor_settings->fs.fs_avl[i].num == 0) 132 goto st_sensors_match_odr_error; 133 134 if (sensor_settings->fs.fs_avl[i].num == fs) { 135 *index_fs_avl = i; 136 ret = 0; 137 break; 138 } 139 } 140 141 st_sensors_match_odr_error: 142 return ret; 143 } 144 145 static int st_sensors_set_fullscale(struct iio_dev *indio_dev, unsigned int fs) 146 { 147 int err, i = 0; 148 struct st_sensor_data *sdata = iio_priv(indio_dev); 149 150 if (sdata->sensor_settings->fs.addr == 0) 151 return 0; 152 153 err = st_sensors_match_fs(sdata->sensor_settings, fs, &i); 154 if (err < 0) 155 goto st_accel_set_fullscale_error; 156 157 err = st_sensors_write_data_with_mask(indio_dev, 158 sdata->sensor_settings->fs.addr, 159 sdata->sensor_settings->fs.mask, 160 sdata->sensor_settings->fs.fs_avl[i].value); 161 if (err < 0) 162 goto st_accel_set_fullscale_error; 163 164 sdata->current_fullscale = (struct st_sensor_fullscale_avl *) 165 &sdata->sensor_settings->fs.fs_avl[i]; 166 return err; 167 168 st_accel_set_fullscale_error: 169 dev_err(&indio_dev->dev, "failed to set new fullscale.\n"); 170 return err; 171 } 172 173 int st_sensors_set_enable(struct iio_dev *indio_dev, bool enable) 174 { 175 u8 tmp_value; 176 int err = -EINVAL; 177 bool found = false; 178 struct st_sensor_odr_avl odr_out = {0, 0}; 179 struct st_sensor_data *sdata = iio_priv(indio_dev); 180 181 if (enable) { 182 tmp_value = sdata->sensor_settings->pw.value_on; 183 if ((sdata->sensor_settings->odr.addr == 184 sdata->sensor_settings->pw.addr) && 185 (sdata->sensor_settings->odr.mask == 186 sdata->sensor_settings->pw.mask)) { 187 err = st_sensors_match_odr(sdata->sensor_settings, 188 sdata->odr, &odr_out); 189 if (err < 0) 190 goto set_enable_error; 191 tmp_value = odr_out.value; 192 found = true; 193 } 194 err = st_sensors_write_data_with_mask(indio_dev, 195 sdata->sensor_settings->pw.addr, 196 sdata->sensor_settings->pw.mask, tmp_value); 197 if (err < 0) 198 goto set_enable_error; 199 200 sdata->enabled = true; 201 202 if (found) 203 sdata->odr = odr_out.hz; 204 } else { 205 err = st_sensors_write_data_with_mask(indio_dev, 206 sdata->sensor_settings->pw.addr, 207 sdata->sensor_settings->pw.mask, 208 sdata->sensor_settings->pw.value_off); 209 if (err < 0) 210 goto set_enable_error; 211 212 sdata->enabled = false; 213 } 214 215 set_enable_error: 216 return err; 217 } 218 EXPORT_SYMBOL(st_sensors_set_enable); 219 220 int st_sensors_set_axis_enable(struct iio_dev *indio_dev, u8 axis_enable) 221 { 222 struct st_sensor_data *sdata = iio_priv(indio_dev); 223 224 return st_sensors_write_data_with_mask(indio_dev, 225 sdata->sensor_settings->enable_axis.addr, 226 sdata->sensor_settings->enable_axis.mask, 227 axis_enable); 228 } 229 EXPORT_SYMBOL(st_sensors_set_axis_enable); 230 231 int st_sensors_power_enable(struct iio_dev *indio_dev) 232 { 233 struct st_sensor_data *pdata = iio_priv(indio_dev); 234 int err; 235 236 /* Regulators not mandatory, but if requested we should enable them. */ 237 pdata->vdd = devm_regulator_get(indio_dev->dev.parent, "vdd"); 238 if (IS_ERR(pdata->vdd)) { 239 dev_err(&indio_dev->dev, "unable to get Vdd supply\n"); 240 return PTR_ERR(pdata->vdd); 241 } 242 err = regulator_enable(pdata->vdd); 243 if (err != 0) { 244 dev_warn(&indio_dev->dev, 245 "Failed to enable specified Vdd supply\n"); 246 return err; 247 } 248 249 pdata->vdd_io = devm_regulator_get(indio_dev->dev.parent, "vddio"); 250 if (IS_ERR(pdata->vdd_io)) { 251 dev_err(&indio_dev->dev, "unable to get Vdd_IO supply\n"); 252 err = PTR_ERR(pdata->vdd_io); 253 goto st_sensors_disable_vdd; 254 } 255 err = regulator_enable(pdata->vdd_io); 256 if (err != 0) { 257 dev_warn(&indio_dev->dev, 258 "Failed to enable specified Vdd_IO supply\n"); 259 goto st_sensors_disable_vdd; 260 } 261 262 return 0; 263 264 st_sensors_disable_vdd: 265 regulator_disable(pdata->vdd); 266 return err; 267 } 268 EXPORT_SYMBOL(st_sensors_power_enable); 269 270 void st_sensors_power_disable(struct iio_dev *indio_dev) 271 { 272 struct st_sensor_data *pdata = iio_priv(indio_dev); 273 274 regulator_disable(pdata->vdd); 275 regulator_disable(pdata->vdd_io); 276 } 277 EXPORT_SYMBOL(st_sensors_power_disable); 278 279 static int st_sensors_set_drdy_int_pin(struct iio_dev *indio_dev, 280 struct st_sensors_platform_data *pdata) 281 { 282 struct st_sensor_data *sdata = iio_priv(indio_dev); 283 284 /* Sensor does not support interrupts */ 285 if (sdata->sensor_settings->drdy_irq.addr == 0) { 286 if (pdata->drdy_int_pin) 287 dev_info(&indio_dev->dev, 288 "DRDY on pin INT%d specified, but sensor " 289 "does not support interrupts\n", 290 pdata->drdy_int_pin); 291 return 0; 292 } 293 294 switch (pdata->drdy_int_pin) { 295 case 1: 296 if (sdata->sensor_settings->drdy_irq.mask_int1 == 0) { 297 dev_err(&indio_dev->dev, 298 "DRDY on INT1 not available.\n"); 299 return -EINVAL; 300 } 301 sdata->drdy_int_pin = 1; 302 break; 303 case 2: 304 if (sdata->sensor_settings->drdy_irq.mask_int2 == 0) { 305 dev_err(&indio_dev->dev, 306 "DRDY on INT2 not available.\n"); 307 return -EINVAL; 308 } 309 sdata->drdy_int_pin = 2; 310 break; 311 default: 312 dev_err(&indio_dev->dev, "DRDY on pdata not valid.\n"); 313 return -EINVAL; 314 } 315 316 if (pdata->open_drain) { 317 if (!sdata->sensor_settings->drdy_irq.addr_od) 318 dev_err(&indio_dev->dev, 319 "open drain requested but unsupported.\n"); 320 else 321 sdata->int_pin_open_drain = true; 322 } 323 324 return 0; 325 } 326 327 #ifdef CONFIG_OF 328 static struct st_sensors_platform_data *st_sensors_of_probe(struct device *dev, 329 struct st_sensors_platform_data *defdata) 330 { 331 struct st_sensors_platform_data *pdata; 332 struct device_node *np = dev->of_node; 333 u32 val; 334 335 if (!np) 336 return NULL; 337 338 pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL); 339 if (!of_property_read_u32(np, "st,drdy-int-pin", &val) && (val <= 2)) 340 pdata->drdy_int_pin = (u8) val; 341 else 342 pdata->drdy_int_pin = defdata ? defdata->drdy_int_pin : 0; 343 344 pdata->open_drain = of_property_read_bool(np, "drive-open-drain"); 345 346 return pdata; 347 } 348 #else 349 static struct st_sensors_platform_data *st_sensors_of_probe(struct device *dev, 350 struct st_sensors_platform_data *defdata) 351 { 352 return NULL; 353 } 354 #endif 355 356 int st_sensors_init_sensor(struct iio_dev *indio_dev, 357 struct st_sensors_platform_data *pdata) 358 { 359 struct st_sensor_data *sdata = iio_priv(indio_dev); 360 struct st_sensors_platform_data *of_pdata; 361 int err = 0; 362 363 /* If OF/DT pdata exists, it will take precedence of anything else */ 364 of_pdata = st_sensors_of_probe(indio_dev->dev.parent, pdata); 365 if (of_pdata) 366 pdata = of_pdata; 367 368 if (pdata) { 369 err = st_sensors_set_drdy_int_pin(indio_dev, pdata); 370 if (err < 0) 371 return err; 372 } 373 374 err = st_sensors_set_enable(indio_dev, false); 375 if (err < 0) 376 return err; 377 378 /* Disable DRDY, this might be still be enabled after reboot. */ 379 err = st_sensors_set_dataready_irq(indio_dev, false); 380 if (err < 0) 381 return err; 382 383 if (sdata->current_fullscale) { 384 err = st_sensors_set_fullscale(indio_dev, 385 sdata->current_fullscale->num); 386 if (err < 0) 387 return err; 388 } else 389 dev_info(&indio_dev->dev, "Full-scale not possible\n"); 390 391 err = st_sensors_set_odr(indio_dev, sdata->odr); 392 if (err < 0) 393 return err; 394 395 /* set BDU */ 396 if (sdata->sensor_settings->bdu.addr) { 397 err = st_sensors_write_data_with_mask(indio_dev, 398 sdata->sensor_settings->bdu.addr, 399 sdata->sensor_settings->bdu.mask, true); 400 if (err < 0) 401 return err; 402 } 403 404 /* set DAS */ 405 if (sdata->sensor_settings->das.addr) { 406 err = st_sensors_write_data_with_mask(indio_dev, 407 sdata->sensor_settings->das.addr, 408 sdata->sensor_settings->das.mask, 1); 409 if (err < 0) 410 return err; 411 } 412 413 if (sdata->int_pin_open_drain) { 414 dev_info(&indio_dev->dev, 415 "set interrupt line to open drain mode\n"); 416 err = st_sensors_write_data_with_mask(indio_dev, 417 sdata->sensor_settings->drdy_irq.addr_od, 418 sdata->sensor_settings->drdy_irq.mask_od, 1); 419 if (err < 0) 420 return err; 421 } 422 423 err = st_sensors_set_axis_enable(indio_dev, ST_SENSORS_ENABLE_ALL_AXIS); 424 425 return err; 426 } 427 EXPORT_SYMBOL(st_sensors_init_sensor); 428 429 int st_sensors_set_dataready_irq(struct iio_dev *indio_dev, bool enable) 430 { 431 int err; 432 u8 drdy_mask; 433 struct st_sensor_data *sdata = iio_priv(indio_dev); 434 435 if (!sdata->sensor_settings->drdy_irq.addr) 436 return 0; 437 438 /* Enable/Disable the interrupt generator 1. */ 439 if (sdata->sensor_settings->drdy_irq.ig1.en_addr > 0) { 440 err = st_sensors_write_data_with_mask(indio_dev, 441 sdata->sensor_settings->drdy_irq.ig1.en_addr, 442 sdata->sensor_settings->drdy_irq.ig1.en_mask, 443 (int)enable); 444 if (err < 0) 445 goto st_accel_set_dataready_irq_error; 446 } 447 448 if (sdata->drdy_int_pin == 1) 449 drdy_mask = sdata->sensor_settings->drdy_irq.mask_int1; 450 else 451 drdy_mask = sdata->sensor_settings->drdy_irq.mask_int2; 452 453 /* Flag to the poll function that the hardware trigger is in use */ 454 sdata->hw_irq_trigger = enable; 455 456 /* Enable/Disable the interrupt generator for data ready. */ 457 err = st_sensors_write_data_with_mask(indio_dev, 458 sdata->sensor_settings->drdy_irq.addr, 459 drdy_mask, (int)enable); 460 461 st_accel_set_dataready_irq_error: 462 return err; 463 } 464 EXPORT_SYMBOL(st_sensors_set_dataready_irq); 465 466 int st_sensors_set_fullscale_by_gain(struct iio_dev *indio_dev, int scale) 467 { 468 int err = -EINVAL, i; 469 struct st_sensor_data *sdata = iio_priv(indio_dev); 470 471 for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) { 472 if ((sdata->sensor_settings->fs.fs_avl[i].gain == scale) && 473 (sdata->sensor_settings->fs.fs_avl[i].gain != 0)) { 474 err = 0; 475 break; 476 } 477 } 478 if (err < 0) 479 goto st_sensors_match_scale_error; 480 481 err = st_sensors_set_fullscale(indio_dev, 482 sdata->sensor_settings->fs.fs_avl[i].num); 483 484 st_sensors_match_scale_error: 485 return err; 486 } 487 EXPORT_SYMBOL(st_sensors_set_fullscale_by_gain); 488 489 static int st_sensors_read_axis_data(struct iio_dev *indio_dev, 490 struct iio_chan_spec const *ch, int *data) 491 { 492 int err; 493 u8 *outdata; 494 struct st_sensor_data *sdata = iio_priv(indio_dev); 495 unsigned int byte_for_channel; 496 497 byte_for_channel = DIV_ROUND_UP(ch->scan_type.realbits + 498 ch->scan_type.shift, 8); 499 outdata = kmalloc(byte_for_channel, GFP_KERNEL); 500 if (!outdata) 501 return -ENOMEM; 502 503 err = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev, 504 ch->address, byte_for_channel, 505 outdata, sdata->multiread_bit); 506 if (err < 0) 507 goto st_sensors_free_memory; 508 509 if (byte_for_channel == 1) 510 *data = (s8)*outdata; 511 else if (byte_for_channel == 2) 512 *data = (s16)get_unaligned_le16(outdata); 513 else if (byte_for_channel == 3) 514 *data = (s32)st_sensors_get_unaligned_le24(outdata); 515 516 st_sensors_free_memory: 517 kfree(outdata); 518 519 return err; 520 } 521 522 int st_sensors_read_info_raw(struct iio_dev *indio_dev, 523 struct iio_chan_spec const *ch, int *val) 524 { 525 int err; 526 struct st_sensor_data *sdata = iio_priv(indio_dev); 527 528 mutex_lock(&indio_dev->mlock); 529 if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { 530 err = -EBUSY; 531 goto out; 532 } else { 533 err = st_sensors_set_enable(indio_dev, true); 534 if (err < 0) 535 goto out; 536 537 msleep((sdata->sensor_settings->bootime * 1000) / sdata->odr); 538 err = st_sensors_read_axis_data(indio_dev, ch, val); 539 if (err < 0) 540 goto out; 541 542 *val = *val >> ch->scan_type.shift; 543 544 err = st_sensors_set_enable(indio_dev, false); 545 } 546 out: 547 mutex_unlock(&indio_dev->mlock); 548 549 return err; 550 } 551 EXPORT_SYMBOL(st_sensors_read_info_raw); 552 553 int st_sensors_check_device_support(struct iio_dev *indio_dev, 554 int num_sensors_list, 555 const struct st_sensor_settings *sensor_settings) 556 { 557 int i, n, err = 0; 558 u8 wai; 559 struct st_sensor_data *sdata = iio_priv(indio_dev); 560 561 for (i = 0; i < num_sensors_list; i++) { 562 for (n = 0; n < ST_SENSORS_MAX_4WAI; n++) { 563 if (strcmp(indio_dev->name, 564 sensor_settings[i].sensors_supported[n]) == 0) { 565 break; 566 } 567 } 568 if (n < ST_SENSORS_MAX_4WAI) 569 break; 570 } 571 if (i == num_sensors_list) { 572 dev_err(&indio_dev->dev, "device name %s not recognized.\n", 573 indio_dev->name); 574 return -ENODEV; 575 } 576 577 if (sensor_settings[i].wai_addr) { 578 err = sdata->tf->read_byte(&sdata->tb, sdata->dev, 579 sensor_settings[i].wai_addr, &wai); 580 if (err < 0) { 581 dev_err(&indio_dev->dev, 582 "failed to read Who-Am-I register.\n"); 583 return err; 584 } 585 586 if (sensor_settings[i].wai != wai) { 587 dev_err(&indio_dev->dev, 588 "%s: WhoAmI mismatch (0x%x).\n", 589 indio_dev->name, wai); 590 return -EINVAL; 591 } 592 } 593 594 sdata->sensor_settings = 595 (struct st_sensor_settings *)&sensor_settings[i]; 596 597 return i; 598 } 599 EXPORT_SYMBOL(st_sensors_check_device_support); 600 601 ssize_t st_sensors_sysfs_sampling_frequency_avail(struct device *dev, 602 struct device_attribute *attr, char *buf) 603 { 604 int i, len = 0; 605 struct iio_dev *indio_dev = dev_get_drvdata(dev); 606 struct st_sensor_data *sdata = iio_priv(indio_dev); 607 608 mutex_lock(&indio_dev->mlock); 609 for (i = 0; i < ST_SENSORS_ODR_LIST_MAX; i++) { 610 if (sdata->sensor_settings->odr.odr_avl[i].hz == 0) 611 break; 612 613 len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", 614 sdata->sensor_settings->odr.odr_avl[i].hz); 615 } 616 mutex_unlock(&indio_dev->mlock); 617 buf[len - 1] = '\n'; 618 619 return len; 620 } 621 EXPORT_SYMBOL(st_sensors_sysfs_sampling_frequency_avail); 622 623 ssize_t st_sensors_sysfs_scale_avail(struct device *dev, 624 struct device_attribute *attr, char *buf) 625 { 626 int i, len = 0, q, r; 627 struct iio_dev *indio_dev = dev_get_drvdata(dev); 628 struct st_sensor_data *sdata = iio_priv(indio_dev); 629 630 mutex_lock(&indio_dev->mlock); 631 for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) { 632 if (sdata->sensor_settings->fs.fs_avl[i].num == 0) 633 break; 634 635 q = sdata->sensor_settings->fs.fs_avl[i].gain / 1000000; 636 r = sdata->sensor_settings->fs.fs_avl[i].gain % 1000000; 637 638 len += scnprintf(buf + len, PAGE_SIZE - len, "%u.%06u ", q, r); 639 } 640 mutex_unlock(&indio_dev->mlock); 641 buf[len - 1] = '\n'; 642 643 return len; 644 } 645 EXPORT_SYMBOL(st_sensors_sysfs_scale_avail); 646 647 MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>"); 648 MODULE_DESCRIPTION("STMicroelectronics ST-sensors core"); 649 MODULE_LICENSE("GPL v2"); 650