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 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 static 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 void 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_optional(indio_dev->dev.parent, "vdd"); 239 if (!IS_ERR(pdata->vdd)) { 240 err = regulator_enable(pdata->vdd); 241 if (err != 0) 242 dev_warn(&indio_dev->dev, 243 "Failed to enable specified Vdd supply\n"); 244 } 245 246 pdata->vdd_io = devm_regulator_get_optional(indio_dev->dev.parent, "vddio"); 247 if (!IS_ERR(pdata->vdd_io)) { 248 err = regulator_enable(pdata->vdd_io); 249 if (err != 0) 250 dev_warn(&indio_dev->dev, 251 "Failed to enable specified Vdd_IO supply\n"); 252 } 253 } 254 EXPORT_SYMBOL(st_sensors_power_enable); 255 256 void st_sensors_power_disable(struct iio_dev *indio_dev) 257 { 258 struct st_sensor_data *pdata = iio_priv(indio_dev); 259 260 if (!IS_ERR(pdata->vdd)) 261 regulator_disable(pdata->vdd); 262 263 if (!IS_ERR(pdata->vdd_io)) 264 regulator_disable(pdata->vdd_io); 265 } 266 EXPORT_SYMBOL(st_sensors_power_disable); 267 268 static int st_sensors_set_drdy_int_pin(struct iio_dev *indio_dev, 269 struct st_sensors_platform_data *pdata) 270 { 271 struct st_sensor_data *sdata = iio_priv(indio_dev); 272 273 /* Sensor does not support interrupts */ 274 if (sdata->sensor_settings->drdy_irq.addr == 0) { 275 if (pdata->drdy_int_pin) 276 dev_info(&indio_dev->dev, 277 "DRDY on pin INT%d specified, but sensor " 278 "does not support interrupts\n", 279 pdata->drdy_int_pin); 280 return 0; 281 } 282 283 switch (pdata->drdy_int_pin) { 284 case 1: 285 if (sdata->sensor_settings->drdy_irq.mask_int1 == 0) { 286 dev_err(&indio_dev->dev, 287 "DRDY on INT1 not available.\n"); 288 return -EINVAL; 289 } 290 sdata->drdy_int_pin = 1; 291 break; 292 case 2: 293 if (sdata->sensor_settings->drdy_irq.mask_int2 == 0) { 294 dev_err(&indio_dev->dev, 295 "DRDY on INT2 not available.\n"); 296 return -EINVAL; 297 } 298 sdata->drdy_int_pin = 2; 299 break; 300 default: 301 dev_err(&indio_dev->dev, "DRDY on pdata not valid.\n"); 302 return -EINVAL; 303 } 304 305 return 0; 306 } 307 308 #ifdef CONFIG_OF 309 static struct st_sensors_platform_data *st_sensors_of_probe(struct device *dev, 310 struct st_sensors_platform_data *defdata) 311 { 312 struct st_sensors_platform_data *pdata; 313 struct device_node *np = dev->of_node; 314 u32 val; 315 316 if (!np) 317 return NULL; 318 319 pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL); 320 if (!of_property_read_u32(np, "st,drdy-int-pin", &val) && (val <= 2)) 321 pdata->drdy_int_pin = (u8) val; 322 else 323 pdata->drdy_int_pin = defdata ? defdata->drdy_int_pin : 0; 324 325 return pdata; 326 } 327 #else 328 static struct st_sensors_platform_data *st_sensors_of_probe(struct device *dev, 329 struct st_sensors_platform_data *defdata) 330 { 331 return NULL; 332 } 333 #endif 334 335 int st_sensors_init_sensor(struct iio_dev *indio_dev, 336 struct st_sensors_platform_data *pdata) 337 { 338 struct st_sensor_data *sdata = iio_priv(indio_dev); 339 struct st_sensors_platform_data *of_pdata; 340 int err = 0; 341 342 /* If OF/DT pdata exists, it will take precedence of anything else */ 343 of_pdata = st_sensors_of_probe(indio_dev->dev.parent, pdata); 344 if (of_pdata) 345 pdata = of_pdata; 346 347 if (pdata) { 348 err = st_sensors_set_drdy_int_pin(indio_dev, pdata); 349 if (err < 0) 350 return err; 351 } 352 353 err = st_sensors_set_enable(indio_dev, false); 354 if (err < 0) 355 return err; 356 357 if (sdata->current_fullscale) { 358 err = st_sensors_set_fullscale(indio_dev, 359 sdata->current_fullscale->num); 360 if (err < 0) 361 return err; 362 } else 363 dev_info(&indio_dev->dev, "Full-scale not possible\n"); 364 365 err = st_sensors_set_odr(indio_dev, sdata->odr); 366 if (err < 0) 367 return err; 368 369 /* set BDU */ 370 if (sdata->sensor_settings->bdu.addr) { 371 err = st_sensors_write_data_with_mask(indio_dev, 372 sdata->sensor_settings->bdu.addr, 373 sdata->sensor_settings->bdu.mask, true); 374 if (err < 0) 375 return err; 376 } 377 378 err = st_sensors_set_axis_enable(indio_dev, ST_SENSORS_ENABLE_ALL_AXIS); 379 380 return err; 381 } 382 EXPORT_SYMBOL(st_sensors_init_sensor); 383 384 int st_sensors_set_dataready_irq(struct iio_dev *indio_dev, bool enable) 385 { 386 int err; 387 u8 drdy_mask; 388 struct st_sensor_data *sdata = iio_priv(indio_dev); 389 390 if (!sdata->sensor_settings->drdy_irq.addr) 391 return 0; 392 393 /* Enable/Disable the interrupt generator 1. */ 394 if (sdata->sensor_settings->drdy_irq.ig1.en_addr > 0) { 395 err = st_sensors_write_data_with_mask(indio_dev, 396 sdata->sensor_settings->drdy_irq.ig1.en_addr, 397 sdata->sensor_settings->drdy_irq.ig1.en_mask, 398 (int)enable); 399 if (err < 0) 400 goto st_accel_set_dataready_irq_error; 401 } 402 403 if (sdata->drdy_int_pin == 1) 404 drdy_mask = sdata->sensor_settings->drdy_irq.mask_int1; 405 else 406 drdy_mask = sdata->sensor_settings->drdy_irq.mask_int2; 407 408 /* Enable/Disable the interrupt generator for data ready. */ 409 err = st_sensors_write_data_with_mask(indio_dev, 410 sdata->sensor_settings->drdy_irq.addr, 411 drdy_mask, (int)enable); 412 413 st_accel_set_dataready_irq_error: 414 return err; 415 } 416 EXPORT_SYMBOL(st_sensors_set_dataready_irq); 417 418 int st_sensors_set_fullscale_by_gain(struct iio_dev *indio_dev, int scale) 419 { 420 int err = -EINVAL, i; 421 struct st_sensor_data *sdata = iio_priv(indio_dev); 422 423 for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) { 424 if ((sdata->sensor_settings->fs.fs_avl[i].gain == scale) && 425 (sdata->sensor_settings->fs.fs_avl[i].gain != 0)) { 426 err = 0; 427 break; 428 } 429 } 430 if (err < 0) 431 goto st_sensors_match_scale_error; 432 433 err = st_sensors_set_fullscale(indio_dev, 434 sdata->sensor_settings->fs.fs_avl[i].num); 435 436 st_sensors_match_scale_error: 437 return err; 438 } 439 EXPORT_SYMBOL(st_sensors_set_fullscale_by_gain); 440 441 static int st_sensors_read_axis_data(struct iio_dev *indio_dev, 442 struct iio_chan_spec const *ch, int *data) 443 { 444 int err; 445 u8 *outdata; 446 struct st_sensor_data *sdata = iio_priv(indio_dev); 447 unsigned int byte_for_channel = ch->scan_type.storagebits >> 3; 448 449 outdata = kmalloc(byte_for_channel, GFP_KERNEL); 450 if (!outdata) 451 return -ENOMEM; 452 453 err = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev, 454 ch->address, byte_for_channel, 455 outdata, sdata->multiread_bit); 456 if (err < 0) 457 goto st_sensors_free_memory; 458 459 if (byte_for_channel == 1) 460 *data = (s8)*outdata; 461 else if (byte_for_channel == 2) 462 *data = (s16)get_unaligned_le16(outdata); 463 else if (byte_for_channel == 3) 464 *data = (s32)st_sensors_get_unaligned_le24(outdata); 465 466 st_sensors_free_memory: 467 kfree(outdata); 468 469 return err; 470 } 471 472 int st_sensors_read_info_raw(struct iio_dev *indio_dev, 473 struct iio_chan_spec const *ch, int *val) 474 { 475 int err; 476 struct st_sensor_data *sdata = iio_priv(indio_dev); 477 478 mutex_lock(&indio_dev->mlock); 479 if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { 480 err = -EBUSY; 481 goto out; 482 } else { 483 err = st_sensors_set_enable(indio_dev, true); 484 if (err < 0) 485 goto out; 486 487 msleep((sdata->sensor_settings->bootime * 1000) / sdata->odr); 488 err = st_sensors_read_axis_data(indio_dev, ch, val); 489 if (err < 0) 490 goto out; 491 492 *val = *val >> ch->scan_type.shift; 493 494 err = st_sensors_set_enable(indio_dev, false); 495 } 496 out: 497 mutex_unlock(&indio_dev->mlock); 498 499 return err; 500 } 501 EXPORT_SYMBOL(st_sensors_read_info_raw); 502 503 int st_sensors_check_device_support(struct iio_dev *indio_dev, 504 int num_sensors_list, 505 const struct st_sensor_settings *sensor_settings) 506 { 507 int i, n, err; 508 u8 wai; 509 struct st_sensor_data *sdata = iio_priv(indio_dev); 510 511 for (i = 0; i < num_sensors_list; i++) { 512 for (n = 0; n < ST_SENSORS_MAX_4WAI; n++) { 513 if (strcmp(indio_dev->name, 514 sensor_settings[i].sensors_supported[n]) == 0) { 515 break; 516 } 517 } 518 if (n < ST_SENSORS_MAX_4WAI) 519 break; 520 } 521 if (i == num_sensors_list) { 522 dev_err(&indio_dev->dev, "device name %s not recognized.\n", 523 indio_dev->name); 524 return -ENODEV; 525 } 526 527 err = sdata->tf->read_byte(&sdata->tb, sdata->dev, 528 sensor_settings[i].wai_addr, &wai); 529 if (err < 0) { 530 dev_err(&indio_dev->dev, "failed to read Who-Am-I register.\n"); 531 return err; 532 } 533 534 if (sensor_settings[i].wai != wai) { 535 dev_err(&indio_dev->dev, "%s: WhoAmI mismatch (0x%x).\n", 536 indio_dev->name, wai); 537 return -EINVAL; 538 } 539 540 sdata->sensor_settings = 541 (struct st_sensor_settings *)&sensor_settings[i]; 542 543 return i; 544 } 545 EXPORT_SYMBOL(st_sensors_check_device_support); 546 547 ssize_t st_sensors_sysfs_sampling_frequency_avail(struct device *dev, 548 struct device_attribute *attr, char *buf) 549 { 550 int i, len = 0; 551 struct iio_dev *indio_dev = dev_get_drvdata(dev); 552 struct st_sensor_data *sdata = iio_priv(indio_dev); 553 554 mutex_lock(&indio_dev->mlock); 555 for (i = 0; i < ST_SENSORS_ODR_LIST_MAX; i++) { 556 if (sdata->sensor_settings->odr.odr_avl[i].hz == 0) 557 break; 558 559 len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", 560 sdata->sensor_settings->odr.odr_avl[i].hz); 561 } 562 mutex_unlock(&indio_dev->mlock); 563 buf[len - 1] = '\n'; 564 565 return len; 566 } 567 EXPORT_SYMBOL(st_sensors_sysfs_sampling_frequency_avail); 568 569 ssize_t st_sensors_sysfs_scale_avail(struct device *dev, 570 struct device_attribute *attr, char *buf) 571 { 572 int i, len = 0; 573 struct iio_dev *indio_dev = dev_get_drvdata(dev); 574 struct st_sensor_data *sdata = iio_priv(indio_dev); 575 576 mutex_lock(&indio_dev->mlock); 577 for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) { 578 if (sdata->sensor_settings->fs.fs_avl[i].num == 0) 579 break; 580 581 len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ", 582 sdata->sensor_settings->fs.fs_avl[i].gain); 583 } 584 mutex_unlock(&indio_dev->mlock); 585 buf[len - 1] = '\n'; 586 587 return len; 588 } 589 EXPORT_SYMBOL(st_sensors_sysfs_scale_avail); 590 591 MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>"); 592 MODULE_DESCRIPTION("STMicroelectronics ST-sensors core"); 593 MODULE_LICENSE("GPL v2"); 594