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