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