1 // SPDX-License-Identifier: GPL-2.0-or-later 2 /* 3 * Copyright (C) 2020 Invensense, Inc. 4 */ 5 6 #include <linux/kernel.h> 7 #include <linux/device.h> 8 #include <linux/mutex.h> 9 #include <linux/pm_runtime.h> 10 #include <linux/regmap.h> 11 #include <linux/delay.h> 12 #include <linux/math64.h> 13 #include <linux/minmax.h> 14 #include <linux/units.h> 15 16 #include <linux/iio/buffer.h> 17 #include <linux/iio/common/inv_sensors_timestamp.h> 18 #include <linux/iio/events.h> 19 #include <linux/iio/iio.h> 20 #include <linux/iio/kfifo_buf.h> 21 22 #include "inv_icm42600.h" 23 #include "inv_icm42600_temp.h" 24 #include "inv_icm42600_buffer.h" 25 26 #define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info) \ 27 { \ 28 .type = IIO_ACCEL, \ 29 .modified = 1, \ 30 .channel2 = _modifier, \ 31 .info_mask_separate = \ 32 BIT(IIO_CHAN_INFO_RAW) | \ 33 BIT(IIO_CHAN_INFO_CALIBBIAS), \ 34 .info_mask_shared_by_type = \ 35 BIT(IIO_CHAN_INFO_SCALE), \ 36 .info_mask_shared_by_type_available = \ 37 BIT(IIO_CHAN_INFO_SCALE) | \ 38 BIT(IIO_CHAN_INFO_CALIBBIAS), \ 39 .info_mask_shared_by_all = \ 40 BIT(IIO_CHAN_INFO_SAMP_FREQ), \ 41 .info_mask_shared_by_all_available = \ 42 BIT(IIO_CHAN_INFO_SAMP_FREQ), \ 43 .scan_index = _index, \ 44 .scan_type = { \ 45 .sign = 's', \ 46 .realbits = 16, \ 47 .storagebits = 16, \ 48 .endianness = IIO_BE, \ 49 }, \ 50 .ext_info = _ext_info, \ 51 } 52 53 #define INV_ICM42600_ACCEL_EVENT_CHAN(_modifier, _events, _events_nb) \ 54 { \ 55 .type = IIO_ACCEL, \ 56 .modified = 1, \ 57 .channel2 = _modifier, \ 58 .event_spec = _events, \ 59 .num_event_specs = _events_nb, \ 60 .scan_index = -1, \ 61 } 62 63 enum inv_icm42600_accel_scan { 64 INV_ICM42600_ACCEL_SCAN_X, 65 INV_ICM42600_ACCEL_SCAN_Y, 66 INV_ICM42600_ACCEL_SCAN_Z, 67 INV_ICM42600_ACCEL_SCAN_TEMP, 68 INV_ICM42600_ACCEL_SCAN_TIMESTAMP, 69 }; 70 71 static const char * const inv_icm42600_accel_power_mode_items[] = { 72 "low-noise", 73 "low-power", 74 }; 75 static const int inv_icm42600_accel_power_mode_values[] = { 76 INV_ICM42600_SENSOR_MODE_LOW_NOISE, 77 INV_ICM42600_SENSOR_MODE_LOW_POWER, 78 }; 79 static const int inv_icm42600_accel_filter_values[] = { 80 INV_ICM42600_FILTER_BW_ODR_DIV_2, 81 INV_ICM42600_FILTER_AVG_16X, 82 }; 83 84 static int inv_icm42600_accel_power_mode_set(struct iio_dev *indio_dev, 85 const struct iio_chan_spec *chan, 86 unsigned int idx) 87 { 88 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 89 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev); 90 int power_mode, filter; 91 92 if (chan->type != IIO_ACCEL) 93 return -EINVAL; 94 95 if (idx >= ARRAY_SIZE(inv_icm42600_accel_power_mode_values)) 96 return -EINVAL; 97 98 power_mode = inv_icm42600_accel_power_mode_values[idx]; 99 filter = inv_icm42600_accel_filter_values[idx]; 100 101 guard(mutex)(&st->lock); 102 103 /* cannot change if accel sensor is on */ 104 if (st->conf.accel.mode != INV_ICM42600_SENSOR_MODE_OFF) 105 return -EBUSY; 106 107 /* prevent change if power mode is not supported by the ODR */ 108 switch (power_mode) { 109 case INV_ICM42600_SENSOR_MODE_LOW_NOISE: 110 if (st->conf.accel.odr >= INV_ICM42600_ODR_6_25HZ_LP && 111 st->conf.accel.odr <= INV_ICM42600_ODR_1_5625HZ_LP) 112 return -EPERM; 113 break; 114 case INV_ICM42600_SENSOR_MODE_LOW_POWER: 115 default: 116 if (st->conf.accel.odr <= INV_ICM42600_ODR_1KHZ_LN) 117 return -EPERM; 118 break; 119 } 120 121 accel_st->power_mode = power_mode; 122 accel_st->filter = filter; 123 124 return 0; 125 } 126 127 static int inv_icm42600_accel_power_mode_get(struct iio_dev *indio_dev, 128 const struct iio_chan_spec *chan) 129 { 130 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 131 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev); 132 unsigned int idx; 133 int power_mode; 134 135 if (chan->type != IIO_ACCEL) 136 return -EINVAL; 137 138 guard(mutex)(&st->lock); 139 140 /* if sensor is on, returns actual power mode and not configured one */ 141 switch (st->conf.accel.mode) { 142 case INV_ICM42600_SENSOR_MODE_LOW_POWER: 143 case INV_ICM42600_SENSOR_MODE_LOW_NOISE: 144 power_mode = st->conf.accel.mode; 145 break; 146 default: 147 power_mode = accel_st->power_mode; 148 break; 149 } 150 151 for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_power_mode_values); ++idx) { 152 if (power_mode == inv_icm42600_accel_power_mode_values[idx]) 153 break; 154 } 155 if (idx >= ARRAY_SIZE(inv_icm42600_accel_power_mode_values)) 156 return -EINVAL; 157 158 return idx; 159 } 160 161 static const struct iio_enum inv_icm42600_accel_power_mode_enum = { 162 .items = inv_icm42600_accel_power_mode_items, 163 .num_items = ARRAY_SIZE(inv_icm42600_accel_power_mode_items), 164 .set = inv_icm42600_accel_power_mode_set, 165 .get = inv_icm42600_accel_power_mode_get, 166 }; 167 168 static const struct iio_chan_spec_ext_info inv_icm42600_accel_ext_infos[] = { 169 IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, inv_icm42600_get_mount_matrix), 170 IIO_ENUM_AVAILABLE("power_mode", IIO_SHARED_BY_TYPE, 171 &inv_icm42600_accel_power_mode_enum), 172 IIO_ENUM("power_mode", IIO_SHARED_BY_TYPE, 173 &inv_icm42600_accel_power_mode_enum), 174 { } 175 }; 176 177 /* WoM event: rising ROC */ 178 static const struct iio_event_spec inv_icm42600_wom_events[] = { 179 { 180 .type = IIO_EV_TYPE_ROC, 181 .dir = IIO_EV_DIR_RISING, 182 .mask_separate = BIT(IIO_EV_INFO_ENABLE) | 183 BIT(IIO_EV_INFO_VALUE), 184 }, 185 }; 186 187 static const struct iio_chan_spec inv_icm42600_accel_channels[] = { 188 INV_ICM42600_ACCEL_CHAN(IIO_MOD_X, INV_ICM42600_ACCEL_SCAN_X, 189 inv_icm42600_accel_ext_infos), 190 INV_ICM42600_ACCEL_CHAN(IIO_MOD_Y, INV_ICM42600_ACCEL_SCAN_Y, 191 inv_icm42600_accel_ext_infos), 192 INV_ICM42600_ACCEL_CHAN(IIO_MOD_Z, INV_ICM42600_ACCEL_SCAN_Z, 193 inv_icm42600_accel_ext_infos), 194 INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP), 195 IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_ACCEL_SCAN_TIMESTAMP), 196 INV_ICM42600_ACCEL_EVENT_CHAN(IIO_MOD_X_OR_Y_OR_Z, inv_icm42600_wom_events, 197 ARRAY_SIZE(inv_icm42600_wom_events)), 198 }; 199 200 /* 201 * IIO buffer data: size must be a power of 2 and timestamp aligned 202 * 16 bytes: 6 bytes acceleration, 2 bytes temperature, 8 bytes timestamp 203 */ 204 struct inv_icm42600_accel_buffer { 205 struct inv_icm42600_fifo_sensor_data accel; 206 s16 temp; 207 aligned_s64 timestamp; 208 }; 209 210 #define INV_ICM42600_SCAN_MASK_ACCEL_3AXIS \ 211 (BIT(INV_ICM42600_ACCEL_SCAN_X) | \ 212 BIT(INV_ICM42600_ACCEL_SCAN_Y) | \ 213 BIT(INV_ICM42600_ACCEL_SCAN_Z)) 214 215 #define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_ACCEL_SCAN_TEMP) 216 217 static const unsigned long inv_icm42600_accel_scan_masks[] = { 218 /* 3-axis accel + temperature */ 219 INV_ICM42600_SCAN_MASK_ACCEL_3AXIS | INV_ICM42600_SCAN_MASK_TEMP, 220 0, 221 }; 222 223 /* enable accelerometer sensor and FIFO write */ 224 static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev, 225 const unsigned long *scan_mask) 226 { 227 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 228 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev); 229 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; 230 unsigned int fifo_en = 0; 231 unsigned int sleep_temp = 0; 232 unsigned int sleep_accel = 0; 233 unsigned int sleep; 234 int ret; 235 236 mutex_lock(&st->lock); 237 238 if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) { 239 /* enable temp sensor */ 240 ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp); 241 if (ret) 242 goto out_unlock; 243 fifo_en |= INV_ICM42600_SENSOR_TEMP; 244 } 245 246 if (*scan_mask & INV_ICM42600_SCAN_MASK_ACCEL_3AXIS) { 247 /* enable accel sensor */ 248 conf.mode = accel_st->power_mode; 249 conf.filter = accel_st->filter; 250 ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_accel); 251 if (ret) 252 goto out_unlock; 253 fifo_en |= INV_ICM42600_SENSOR_ACCEL; 254 } 255 256 /* update data FIFO write */ 257 ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en); 258 259 out_unlock: 260 mutex_unlock(&st->lock); 261 /* sleep maximum required time */ 262 sleep = max(sleep_accel, sleep_temp); 263 if (sleep) 264 msleep(sleep); 265 return ret; 266 } 267 268 static int inv_icm42600_accel_read_sensor(struct iio_dev *indio_dev, 269 struct iio_chan_spec const *chan, 270 s16 *val) 271 { 272 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 273 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev); 274 struct device *dev = regmap_get_device(st->map); 275 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; 276 unsigned int reg; 277 __be16 *data; 278 int ret; 279 280 if (chan->type != IIO_ACCEL) 281 return -EINVAL; 282 283 switch (chan->channel2) { 284 case IIO_MOD_X: 285 reg = INV_ICM42600_REG_ACCEL_DATA_X; 286 break; 287 case IIO_MOD_Y: 288 reg = INV_ICM42600_REG_ACCEL_DATA_Y; 289 break; 290 case IIO_MOD_Z: 291 reg = INV_ICM42600_REG_ACCEL_DATA_Z; 292 break; 293 default: 294 return -EINVAL; 295 } 296 297 pm_runtime_get_sync(dev); 298 mutex_lock(&st->lock); 299 300 /* enable accel sensor */ 301 conf.mode = accel_st->power_mode; 302 conf.filter = accel_st->filter; 303 ret = inv_icm42600_set_accel_conf(st, &conf, NULL); 304 if (ret) 305 goto exit; 306 307 /* read accel register data */ 308 data = (__be16 *)&st->buffer[0]; 309 ret = regmap_bulk_read(st->map, reg, data, sizeof(*data)); 310 if (ret) 311 goto exit; 312 313 *val = (s16)be16_to_cpup(data); 314 if (*val == INV_ICM42600_DATA_INVALID) 315 ret = -EINVAL; 316 exit: 317 mutex_unlock(&st->lock); 318 pm_runtime_mark_last_busy(dev); 319 pm_runtime_put_autosuspend(dev); 320 return ret; 321 } 322 323 static unsigned int inv_icm42600_accel_convert_roc_to_wom(u64 roc, 324 int accel_hz, int accel_uhz) 325 { 326 /* 1000/256mg per LSB converted in µm/s² */ 327 const unsigned int convert = (9807U * (MICRO / MILLI)) / 256U; 328 u64 value; 329 u64 freq_uhz; 330 331 /* return 0 only if roc is 0 */ 332 if (roc == 0) 333 return 0; 334 335 freq_uhz = (u64)accel_hz * MICRO + (u64)accel_uhz; 336 value = div64_u64(roc * MICRO, freq_uhz * (u64)convert); 337 338 /* limit value to 8 bits and prevent 0 */ 339 return clamp(value, 1, 255); 340 } 341 342 static u64 inv_icm42600_accel_convert_wom_to_roc(unsigned int threshold, 343 int accel_hz, int accel_uhz) 344 { 345 /* 1000/256mg per LSB converted in µm/s² */ 346 const unsigned int convert = (9807U * (MICRO / MILLI)) / 256U; 347 u64 value; 348 u64 freq_uhz; 349 350 value = threshold * convert; 351 freq_uhz = (u64)accel_hz * MICRO + (u64)accel_uhz; 352 353 /* compute the differential by multiplying by the frequency */ 354 return div_u64(value * freq_uhz, MICRO); 355 } 356 357 static int inv_icm42600_accel_set_wom_threshold(struct inv_icm42600_state *st, 358 u64 value, 359 int accel_hz, int accel_uhz) 360 { 361 unsigned int threshold; 362 int ret; 363 364 /* convert roc to wom threshold and convert back to handle clipping */ 365 threshold = inv_icm42600_accel_convert_roc_to_wom(value, accel_hz, accel_uhz); 366 value = inv_icm42600_accel_convert_wom_to_roc(threshold, accel_hz, accel_uhz); 367 368 dev_dbg(regmap_get_device(st->map), "wom_threshold: 0x%x\n", threshold); 369 370 /* set accel WoM threshold for the 3 axes */ 371 st->buffer[0] = threshold; 372 st->buffer[1] = threshold; 373 st->buffer[2] = threshold; 374 ret = regmap_bulk_write(st->map, INV_ICM42600_REG_ACCEL_WOM_X_THR, st->buffer, 3); 375 if (ret) 376 return ret; 377 378 st->apex.wom.value = value; 379 380 return 0; 381 } 382 383 static int _inv_icm42600_accel_enable_wom(struct iio_dev *indio_dev) 384 { 385 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 386 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev); 387 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; 388 unsigned int sleep_ms = 0; 389 int ret; 390 391 scoped_guard(mutex, &st->lock) { 392 /* turn on accel sensor */ 393 conf.mode = accel_st->power_mode; 394 conf.filter = accel_st->filter; 395 ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_ms); 396 if (ret) 397 return ret; 398 } 399 400 if (sleep_ms) 401 msleep(sleep_ms); 402 403 scoped_guard(mutex, &st->lock) { 404 ret = inv_icm42600_enable_wom(st); 405 if (ret) 406 return ret; 407 st->apex.on++; 408 st->apex.wom.enable = true; 409 } 410 411 return 0; 412 } 413 414 static int inv_icm42600_accel_enable_wom(struct iio_dev *indio_dev) 415 { 416 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 417 struct device *pdev = regmap_get_device(st->map); 418 int ret; 419 420 ret = pm_runtime_resume_and_get(pdev); 421 if (ret) 422 return ret; 423 424 ret = _inv_icm42600_accel_enable_wom(indio_dev); 425 if (ret) { 426 pm_runtime_mark_last_busy(pdev); 427 pm_runtime_put_autosuspend(pdev); 428 return ret; 429 } 430 431 return 0; 432 } 433 434 static int _inv_icm42600_accel_disable_wom(struct iio_dev *indio_dev) 435 { 436 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 437 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; 438 unsigned int sleep_ms = 0; 439 int ret; 440 441 scoped_guard(mutex, &st->lock) { 442 /* 443 * Consider that turning off WoM is always working to avoid 444 * blocking the chip in on mode and prevent going back to sleep. 445 * If there is an error, the chip will anyway go back to sleep 446 * and the feature will not work anymore. 447 */ 448 st->apex.wom.enable = false; 449 st->apex.on--; 450 ret = inv_icm42600_disable_wom(st); 451 if (ret) 452 return ret; 453 /* turn off accel sensor if not used */ 454 if (!st->apex.on && !iio_buffer_enabled(indio_dev)) { 455 conf.mode = INV_ICM42600_SENSOR_MODE_OFF; 456 ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_ms); 457 if (ret) 458 return ret; 459 } 460 } 461 462 if (sleep_ms) 463 msleep(sleep_ms); 464 465 return 0; 466 } 467 468 static int inv_icm42600_accel_disable_wom(struct iio_dev *indio_dev) 469 { 470 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 471 struct device *pdev = regmap_get_device(st->map); 472 int ret; 473 474 ret = _inv_icm42600_accel_disable_wom(indio_dev); 475 476 pm_runtime_mark_last_busy(pdev); 477 pm_runtime_put_autosuspend(pdev); 478 479 return ret; 480 } 481 482 void inv_icm42600_accel_handle_events(struct iio_dev *indio_dev, 483 unsigned int status2, unsigned int status3, 484 s64 timestamp) 485 { 486 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 487 u64 ev_code; 488 489 /* handle WoM event */ 490 if (st->apex.wom.enable && (status2 & INV_ICM42600_INT_STATUS2_WOM_INT)) { 491 ev_code = IIO_MOD_EVENT_CODE(IIO_ACCEL, 0, IIO_MOD_X_OR_Y_OR_Z, 492 IIO_EV_TYPE_ROC, IIO_EV_DIR_RISING); 493 iio_push_event(indio_dev, ev_code, timestamp); 494 } 495 } 496 497 /* IIO format int + nano */ 498 static const int inv_icm42600_accel_scale[] = { 499 /* +/- 16G => 0.004788403 m/s-2 */ 500 [2 * INV_ICM42600_ACCEL_FS_16G] = 0, 501 [2 * INV_ICM42600_ACCEL_FS_16G + 1] = 4788403, 502 /* +/- 8G => 0.002394202 m/s-2 */ 503 [2 * INV_ICM42600_ACCEL_FS_8G] = 0, 504 [2 * INV_ICM42600_ACCEL_FS_8G + 1] = 2394202, 505 /* +/- 4G => 0.001197101 m/s-2 */ 506 [2 * INV_ICM42600_ACCEL_FS_4G] = 0, 507 [2 * INV_ICM42600_ACCEL_FS_4G + 1] = 1197101, 508 /* +/- 2G => 0.000598550 m/s-2 */ 509 [2 * INV_ICM42600_ACCEL_FS_2G] = 0, 510 [2 * INV_ICM42600_ACCEL_FS_2G + 1] = 598550, 511 }; 512 static const int inv_icm42686_accel_scale[] = { 513 /* +/- 32G => 0.009576807 m/s-2 */ 514 [2 * INV_ICM42686_ACCEL_FS_32G] = 0, 515 [2 * INV_ICM42686_ACCEL_FS_32G + 1] = 9576807, 516 /* +/- 16G => 0.004788403 m/s-2 */ 517 [2 * INV_ICM42686_ACCEL_FS_16G] = 0, 518 [2 * INV_ICM42686_ACCEL_FS_16G + 1] = 4788403, 519 /* +/- 8G => 0.002394202 m/s-2 */ 520 [2 * INV_ICM42686_ACCEL_FS_8G] = 0, 521 [2 * INV_ICM42686_ACCEL_FS_8G + 1] = 2394202, 522 /* +/- 4G => 0.001197101 m/s-2 */ 523 [2 * INV_ICM42686_ACCEL_FS_4G] = 0, 524 [2 * INV_ICM42686_ACCEL_FS_4G + 1] = 1197101, 525 /* +/- 2G => 0.000598550 m/s-2 */ 526 [2 * INV_ICM42686_ACCEL_FS_2G] = 0, 527 [2 * INV_ICM42686_ACCEL_FS_2G + 1] = 598550, 528 }; 529 530 static int inv_icm42600_accel_read_scale(struct iio_dev *indio_dev, 531 int *val, int *val2) 532 { 533 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 534 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev); 535 unsigned int idx; 536 537 idx = st->conf.accel.fs; 538 539 *val = accel_st->scales[2 * idx]; 540 *val2 = accel_st->scales[2 * idx + 1]; 541 return IIO_VAL_INT_PLUS_NANO; 542 } 543 544 static int inv_icm42600_accel_write_scale(struct iio_dev *indio_dev, 545 int val, int val2) 546 { 547 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 548 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev); 549 struct device *dev = regmap_get_device(st->map); 550 unsigned int idx; 551 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; 552 int ret; 553 554 for (idx = 0; idx < accel_st->scales_len; idx += 2) { 555 if (val == accel_st->scales[idx] && 556 val2 == accel_st->scales[idx + 1]) 557 break; 558 } 559 if (idx >= accel_st->scales_len) 560 return -EINVAL; 561 562 conf.fs = idx / 2; 563 564 pm_runtime_get_sync(dev); 565 mutex_lock(&st->lock); 566 567 ret = inv_icm42600_set_accel_conf(st, &conf, NULL); 568 569 mutex_unlock(&st->lock); 570 pm_runtime_mark_last_busy(dev); 571 pm_runtime_put_autosuspend(dev); 572 573 return ret; 574 } 575 576 /* IIO format int + micro */ 577 static const int inv_icm42600_accel_odr[] = { 578 /* 1.5625Hz */ 579 1, 562500, 580 /* 3.125Hz */ 581 3, 125000, 582 /* 6.25Hz */ 583 6, 250000, 584 /* 12.5Hz */ 585 12, 500000, 586 /* 25Hz */ 587 25, 0, 588 /* 50Hz */ 589 50, 0, 590 /* 100Hz */ 591 100, 0, 592 /* 200Hz */ 593 200, 0, 594 /* 1kHz */ 595 1000, 0, 596 /* 2kHz */ 597 2000, 0, 598 /* 4kHz */ 599 4000, 0, 600 }; 601 602 static const int inv_icm42600_accel_odr_conv[] = { 603 INV_ICM42600_ODR_1_5625HZ_LP, 604 INV_ICM42600_ODR_3_125HZ_LP, 605 INV_ICM42600_ODR_6_25HZ_LP, 606 INV_ICM42600_ODR_12_5HZ, 607 INV_ICM42600_ODR_25HZ, 608 INV_ICM42600_ODR_50HZ, 609 INV_ICM42600_ODR_100HZ, 610 INV_ICM42600_ODR_200HZ, 611 INV_ICM42600_ODR_1KHZ_LN, 612 INV_ICM42600_ODR_2KHZ_LN, 613 INV_ICM42600_ODR_4KHZ_LN, 614 }; 615 616 static int inv_icm42600_accel_read_odr(struct inv_icm42600_state *st, 617 int *val, int *val2) 618 { 619 unsigned int odr; 620 unsigned int i; 621 622 odr = st->conf.accel.odr; 623 624 for (i = 0; i < ARRAY_SIZE(inv_icm42600_accel_odr_conv); ++i) { 625 if (inv_icm42600_accel_odr_conv[i] == odr) 626 break; 627 } 628 if (i >= ARRAY_SIZE(inv_icm42600_accel_odr_conv)) 629 return -EINVAL; 630 631 *val = inv_icm42600_accel_odr[2 * i]; 632 *val2 = inv_icm42600_accel_odr[2 * i + 1]; 633 634 return IIO_VAL_INT_PLUS_MICRO; 635 } 636 637 static int inv_icm42600_accel_write_odr(struct iio_dev *indio_dev, 638 int val, int val2) 639 { 640 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 641 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev); 642 struct inv_sensors_timestamp *ts = &accel_st->ts; 643 struct device *dev = regmap_get_device(st->map); 644 unsigned int idx; 645 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; 646 int ret; 647 648 for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_odr); idx += 2) { 649 if (val == inv_icm42600_accel_odr[idx] && 650 val2 == inv_icm42600_accel_odr[idx + 1]) 651 break; 652 } 653 if (idx >= ARRAY_SIZE(inv_icm42600_accel_odr)) 654 return -EINVAL; 655 656 conf.odr = inv_icm42600_accel_odr_conv[idx / 2]; 657 658 pm_runtime_get_sync(dev); 659 mutex_lock(&st->lock); 660 661 ret = inv_sensors_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr), 662 iio_buffer_enabled(indio_dev)); 663 if (ret) 664 goto out_unlock; 665 666 ret = inv_icm42600_set_accel_conf(st, &conf, NULL); 667 if (ret) 668 goto out_unlock; 669 /* update wom threshold since roc is dependent on sampling frequency */ 670 ret = inv_icm42600_accel_set_wom_threshold(st, st->apex.wom.value, val, val2); 671 if (ret) 672 goto out_unlock; 673 inv_icm42600_buffer_update_fifo_period(st); 674 inv_icm42600_buffer_update_watermark(st); 675 676 out_unlock: 677 mutex_unlock(&st->lock); 678 pm_runtime_mark_last_busy(dev); 679 pm_runtime_put_autosuspend(dev); 680 681 return ret; 682 } 683 684 /* 685 * Calibration bias values, IIO range format int + micro. 686 * Value is limited to +/-1g coded on 12 bits signed. Step is 0.5mg. 687 */ 688 static int inv_icm42600_accel_calibbias[] = { 689 -10, 42010, /* min: -10.042010 m/s² */ 690 0, 4903, /* step: 0.004903 m/s² */ 691 10, 37106, /* max: 10.037106 m/s² */ 692 }; 693 694 static int inv_icm42600_accel_read_offset(struct inv_icm42600_state *st, 695 struct iio_chan_spec const *chan, 696 int *val, int *val2) 697 { 698 struct device *dev = regmap_get_device(st->map); 699 s64 val64; 700 s32 bias; 701 unsigned int reg; 702 s16 offset; 703 u8 data[2]; 704 int ret; 705 706 if (chan->type != IIO_ACCEL) 707 return -EINVAL; 708 709 switch (chan->channel2) { 710 case IIO_MOD_X: 711 reg = INV_ICM42600_REG_OFFSET_USER4; 712 break; 713 case IIO_MOD_Y: 714 reg = INV_ICM42600_REG_OFFSET_USER6; 715 break; 716 case IIO_MOD_Z: 717 reg = INV_ICM42600_REG_OFFSET_USER7; 718 break; 719 default: 720 return -EINVAL; 721 } 722 723 pm_runtime_get_sync(dev); 724 mutex_lock(&st->lock); 725 726 ret = regmap_bulk_read(st->map, reg, st->buffer, sizeof(data)); 727 memcpy(data, st->buffer, sizeof(data)); 728 729 mutex_unlock(&st->lock); 730 pm_runtime_mark_last_busy(dev); 731 pm_runtime_put_autosuspend(dev); 732 if (ret) 733 return ret; 734 735 /* 12 bits signed value */ 736 switch (chan->channel2) { 737 case IIO_MOD_X: 738 offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11); 739 break; 740 case IIO_MOD_Y: 741 offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11); 742 break; 743 case IIO_MOD_Z: 744 offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11); 745 break; 746 default: 747 return -EINVAL; 748 } 749 750 /* 751 * convert raw offset to g then to m/s² 752 * 12 bits signed raw step 0.5mg to g: 5 / 10000 753 * g to m/s²: 9.806650 754 * result in micro (1000000) 755 * (offset * 5 * 9.806650 * 1000000) / 10000 756 */ 757 val64 = (s64)offset * 5LL * 9806650LL; 758 /* for rounding, add + or - divisor (10000) divided by 2 */ 759 if (val64 >= 0) 760 val64 += 10000LL / 2LL; 761 else 762 val64 -= 10000LL / 2LL; 763 bias = div_s64(val64, 10000L); 764 *val = bias / 1000000L; 765 *val2 = bias % 1000000L; 766 767 return IIO_VAL_INT_PLUS_MICRO; 768 } 769 770 static int inv_icm42600_accel_write_offset(struct inv_icm42600_state *st, 771 struct iio_chan_spec const *chan, 772 int val, int val2) 773 { 774 struct device *dev = regmap_get_device(st->map); 775 s64 val64; 776 s32 min, max; 777 unsigned int reg, regval; 778 s16 offset; 779 int ret; 780 781 if (chan->type != IIO_ACCEL) 782 return -EINVAL; 783 784 switch (chan->channel2) { 785 case IIO_MOD_X: 786 reg = INV_ICM42600_REG_OFFSET_USER4; 787 break; 788 case IIO_MOD_Y: 789 reg = INV_ICM42600_REG_OFFSET_USER6; 790 break; 791 case IIO_MOD_Z: 792 reg = INV_ICM42600_REG_OFFSET_USER7; 793 break; 794 default: 795 return -EINVAL; 796 } 797 798 /* inv_icm42600_accel_calibbias: min - step - max in micro */ 799 min = inv_icm42600_accel_calibbias[0] * 1000000L + 800 inv_icm42600_accel_calibbias[1]; 801 max = inv_icm42600_accel_calibbias[4] * 1000000L + 802 inv_icm42600_accel_calibbias[5]; 803 val64 = (s64)val * 1000000LL + (s64)val2; 804 if (val64 < min || val64 > max) 805 return -EINVAL; 806 807 /* 808 * convert m/s² to g then to raw value 809 * m/s² to g: 1 / 9.806650 810 * g to raw 12 bits signed, step 0.5mg: 10000 / 5 811 * val in micro (1000000) 812 * val * 10000 / (9.806650 * 1000000 * 5) 813 */ 814 val64 = val64 * 10000LL; 815 /* for rounding, add + or - divisor (9806650 * 5) divided by 2 */ 816 if (val64 >= 0) 817 val64 += 9806650 * 5 / 2; 818 else 819 val64 -= 9806650 * 5 / 2; 820 offset = div_s64(val64, 9806650 * 5); 821 822 /* clamp value limited to 12 bits signed */ 823 if (offset < -2048) 824 offset = -2048; 825 else if (offset > 2047) 826 offset = 2047; 827 828 pm_runtime_get_sync(dev); 829 mutex_lock(&st->lock); 830 831 switch (chan->channel2) { 832 case IIO_MOD_X: 833 /* OFFSET_USER4 register is shared */ 834 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER4, 835 ®val); 836 if (ret) 837 goto out_unlock; 838 st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F); 839 st->buffer[1] = offset & 0xFF; 840 break; 841 case IIO_MOD_Y: 842 /* OFFSET_USER7 register is shared */ 843 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER7, 844 ®val); 845 if (ret) 846 goto out_unlock; 847 st->buffer[0] = offset & 0xFF; 848 st->buffer[1] = ((offset & 0xF00) >> 8) | (regval & 0xF0); 849 break; 850 case IIO_MOD_Z: 851 /* OFFSET_USER7 register is shared */ 852 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER7, 853 ®val); 854 if (ret) 855 goto out_unlock; 856 st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F); 857 st->buffer[1] = offset & 0xFF; 858 break; 859 default: 860 ret = -EINVAL; 861 goto out_unlock; 862 } 863 864 ret = regmap_bulk_write(st->map, reg, st->buffer, 2); 865 866 out_unlock: 867 mutex_unlock(&st->lock); 868 pm_runtime_mark_last_busy(dev); 869 pm_runtime_put_autosuspend(dev); 870 return ret; 871 } 872 873 static int inv_icm42600_accel_read_raw(struct iio_dev *indio_dev, 874 struct iio_chan_spec const *chan, 875 int *val, int *val2, long mask) 876 { 877 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 878 s16 data; 879 int ret; 880 881 switch (chan->type) { 882 case IIO_ACCEL: 883 break; 884 case IIO_TEMP: 885 return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2, mask); 886 default: 887 return -EINVAL; 888 } 889 890 switch (mask) { 891 case IIO_CHAN_INFO_RAW: 892 if (!iio_device_claim_direct(indio_dev)) 893 return -EBUSY; 894 ret = inv_icm42600_accel_read_sensor(indio_dev, chan, &data); 895 iio_device_release_direct(indio_dev); 896 if (ret) 897 return ret; 898 *val = data; 899 return IIO_VAL_INT; 900 case IIO_CHAN_INFO_SCALE: 901 return inv_icm42600_accel_read_scale(indio_dev, val, val2); 902 case IIO_CHAN_INFO_SAMP_FREQ: 903 return inv_icm42600_accel_read_odr(st, val, val2); 904 case IIO_CHAN_INFO_CALIBBIAS: 905 return inv_icm42600_accel_read_offset(st, chan, val, val2); 906 default: 907 return -EINVAL; 908 } 909 } 910 911 static int inv_icm42600_accel_read_avail(struct iio_dev *indio_dev, 912 struct iio_chan_spec const *chan, 913 const int **vals, 914 int *type, int *length, long mask) 915 { 916 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev); 917 918 if (chan->type != IIO_ACCEL) 919 return -EINVAL; 920 921 switch (mask) { 922 case IIO_CHAN_INFO_SCALE: 923 *vals = accel_st->scales; 924 *type = IIO_VAL_INT_PLUS_NANO; 925 *length = accel_st->scales_len; 926 return IIO_AVAIL_LIST; 927 case IIO_CHAN_INFO_SAMP_FREQ: 928 *vals = inv_icm42600_accel_odr; 929 *type = IIO_VAL_INT_PLUS_MICRO; 930 *length = ARRAY_SIZE(inv_icm42600_accel_odr); 931 return IIO_AVAIL_LIST; 932 case IIO_CHAN_INFO_CALIBBIAS: 933 *vals = inv_icm42600_accel_calibbias; 934 *type = IIO_VAL_INT_PLUS_MICRO; 935 return IIO_AVAIL_RANGE; 936 default: 937 return -EINVAL; 938 } 939 } 940 941 static int inv_icm42600_accel_write_raw(struct iio_dev *indio_dev, 942 struct iio_chan_spec const *chan, 943 int val, int val2, long mask) 944 { 945 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 946 int ret; 947 948 if (chan->type != IIO_ACCEL) 949 return -EINVAL; 950 951 switch (mask) { 952 case IIO_CHAN_INFO_SCALE: 953 if (!iio_device_claim_direct(indio_dev)) 954 return -EBUSY; 955 ret = inv_icm42600_accel_write_scale(indio_dev, val, val2); 956 iio_device_release_direct(indio_dev); 957 return ret; 958 case IIO_CHAN_INFO_SAMP_FREQ: 959 return inv_icm42600_accel_write_odr(indio_dev, val, val2); 960 case IIO_CHAN_INFO_CALIBBIAS: 961 if (!iio_device_claim_direct(indio_dev)) 962 return -EBUSY; 963 ret = inv_icm42600_accel_write_offset(st, chan, val, val2); 964 iio_device_release_direct(indio_dev); 965 return ret; 966 default: 967 return -EINVAL; 968 } 969 } 970 971 static int inv_icm42600_accel_write_raw_get_fmt(struct iio_dev *indio_dev, 972 struct iio_chan_spec const *chan, 973 long mask) 974 { 975 if (chan->type != IIO_ACCEL) 976 return -EINVAL; 977 978 switch (mask) { 979 case IIO_CHAN_INFO_SCALE: 980 return IIO_VAL_INT_PLUS_NANO; 981 case IIO_CHAN_INFO_SAMP_FREQ: 982 return IIO_VAL_INT_PLUS_MICRO; 983 case IIO_CHAN_INFO_CALIBBIAS: 984 return IIO_VAL_INT_PLUS_MICRO; 985 default: 986 return -EINVAL; 987 } 988 } 989 990 static int inv_icm42600_accel_hwfifo_set_watermark(struct iio_dev *indio_dev, 991 unsigned int val) 992 { 993 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 994 int ret; 995 996 mutex_lock(&st->lock); 997 998 st->fifo.watermark.accel = val; 999 ret = inv_icm42600_buffer_update_watermark(st); 1000 1001 mutex_unlock(&st->lock); 1002 1003 return ret; 1004 } 1005 1006 static int inv_icm42600_accel_hwfifo_flush(struct iio_dev *indio_dev, 1007 unsigned int count) 1008 { 1009 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 1010 int ret; 1011 1012 if (count == 0) 1013 return 0; 1014 1015 mutex_lock(&st->lock); 1016 1017 ret = inv_icm42600_buffer_hwfifo_flush(st, count); 1018 if (!ret) 1019 ret = st->fifo.nb.accel; 1020 1021 mutex_unlock(&st->lock); 1022 1023 return ret; 1024 } 1025 1026 static int inv_icm42600_accel_read_event_config(struct iio_dev *indio_dev, 1027 const struct iio_chan_spec *chan, 1028 enum iio_event_type type, 1029 enum iio_event_direction dir) 1030 { 1031 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 1032 1033 /* handle only WoM (roc rising) event */ 1034 if (type != IIO_EV_TYPE_ROC || dir != IIO_EV_DIR_RISING) 1035 return -EINVAL; 1036 1037 guard(mutex)(&st->lock); 1038 1039 return st->apex.wom.enable ? 1 : 0; 1040 } 1041 1042 static int inv_icm42600_accel_write_event_config(struct iio_dev *indio_dev, 1043 const struct iio_chan_spec *chan, 1044 enum iio_event_type type, 1045 enum iio_event_direction dir, 1046 bool state) 1047 { 1048 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 1049 1050 /* handle only WoM (roc rising) event */ 1051 if (type != IIO_EV_TYPE_ROC || dir != IIO_EV_DIR_RISING) 1052 return -EINVAL; 1053 1054 scoped_guard(mutex, &st->lock) { 1055 if (st->apex.wom.enable == state) 1056 return 0; 1057 } 1058 1059 if (state) 1060 return inv_icm42600_accel_enable_wom(indio_dev); 1061 1062 return inv_icm42600_accel_disable_wom(indio_dev); 1063 } 1064 1065 static int inv_icm42600_accel_read_event_value(struct iio_dev *indio_dev, 1066 const struct iio_chan_spec *chan, 1067 enum iio_event_type type, 1068 enum iio_event_direction dir, 1069 enum iio_event_info info, 1070 int *val, int *val2) 1071 { 1072 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 1073 u32 rem; 1074 1075 /* handle only WoM (roc rising) event value */ 1076 if (type != IIO_EV_TYPE_ROC || dir != IIO_EV_DIR_RISING) 1077 return -EINVAL; 1078 1079 guard(mutex)(&st->lock); 1080 1081 /* return value in micro */ 1082 *val = div_u64_rem(st->apex.wom.value, MICRO, &rem); 1083 *val2 = rem; 1084 return IIO_VAL_INT_PLUS_MICRO; 1085 } 1086 1087 static int _inv_icm42600_accel_wom_value(struct inv_icm42600_state *st, 1088 int val, int val2) 1089 { 1090 u64 value; 1091 unsigned int accel_hz, accel_uhz; 1092 int ret; 1093 1094 guard(mutex)(&st->lock); 1095 1096 ret = inv_icm42600_accel_read_odr(st, &accel_hz, &accel_uhz); 1097 if (ret < 0) 1098 return ret; 1099 1100 value = (u64)val * MICRO + (u64)val2; 1101 1102 return inv_icm42600_accel_set_wom_threshold(st, value, 1103 accel_hz, accel_uhz); 1104 } 1105 1106 static int inv_icm42600_accel_write_event_value(struct iio_dev *indio_dev, 1107 const struct iio_chan_spec *chan, 1108 enum iio_event_type type, 1109 enum iio_event_direction dir, 1110 enum iio_event_info info, 1111 int val, int val2) 1112 { 1113 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 1114 struct device *dev = regmap_get_device(st->map); 1115 int ret; 1116 1117 /* handle only WoM (roc rising) event value */ 1118 if (type != IIO_EV_TYPE_ROC || dir != IIO_EV_DIR_RISING) 1119 return -EINVAL; 1120 1121 if (val < 0 || val2 < 0) 1122 return -EINVAL; 1123 1124 ret = pm_runtime_resume_and_get(dev); 1125 if (ret) 1126 return ret; 1127 1128 ret = _inv_icm42600_accel_wom_value(st, val, val2); 1129 1130 pm_runtime_mark_last_busy(dev); 1131 pm_runtime_put_autosuspend(dev); 1132 1133 return ret; 1134 } 1135 1136 static const struct iio_info inv_icm42600_accel_info = { 1137 .read_raw = inv_icm42600_accel_read_raw, 1138 .read_avail = inv_icm42600_accel_read_avail, 1139 .write_raw = inv_icm42600_accel_write_raw, 1140 .write_raw_get_fmt = inv_icm42600_accel_write_raw_get_fmt, 1141 .debugfs_reg_access = inv_icm42600_debugfs_reg, 1142 .update_scan_mode = inv_icm42600_accel_update_scan_mode, 1143 .hwfifo_set_watermark = inv_icm42600_accel_hwfifo_set_watermark, 1144 .hwfifo_flush_to_buffer = inv_icm42600_accel_hwfifo_flush, 1145 .read_event_config = inv_icm42600_accel_read_event_config, 1146 .write_event_config = inv_icm42600_accel_write_event_config, 1147 .read_event_value = inv_icm42600_accel_read_event_value, 1148 .write_event_value = inv_icm42600_accel_write_event_value, 1149 }; 1150 1151 struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st) 1152 { 1153 struct device *dev = regmap_get_device(st->map); 1154 const char *name; 1155 struct inv_icm42600_sensor_state *accel_st; 1156 struct inv_sensors_timestamp_chip ts_chip; 1157 struct iio_dev *indio_dev; 1158 int ret; 1159 1160 name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name); 1161 if (!name) 1162 return ERR_PTR(-ENOMEM); 1163 1164 indio_dev = devm_iio_device_alloc(dev, sizeof(*accel_st)); 1165 if (!indio_dev) 1166 return ERR_PTR(-ENOMEM); 1167 accel_st = iio_priv(indio_dev); 1168 1169 switch (st->chip) { 1170 case INV_CHIP_ICM42686: 1171 accel_st->scales = inv_icm42686_accel_scale; 1172 accel_st->scales_len = ARRAY_SIZE(inv_icm42686_accel_scale); 1173 break; 1174 default: 1175 accel_st->scales = inv_icm42600_accel_scale; 1176 accel_st->scales_len = ARRAY_SIZE(inv_icm42600_accel_scale); 1177 break; 1178 } 1179 /* low-power by default at init */ 1180 accel_st->power_mode = INV_ICM42600_SENSOR_MODE_LOW_POWER; 1181 accel_st->filter = INV_ICM42600_FILTER_AVG_16X; 1182 1183 /* 1184 * clock period is 32kHz (31250ns) 1185 * jitter is +/- 2% (20 per mille) 1186 */ 1187 ts_chip.clock_period = 31250; 1188 ts_chip.jitter = 20; 1189 ts_chip.init_period = inv_icm42600_odr_to_period(st->conf.accel.odr); 1190 inv_sensors_timestamp_init(&accel_st->ts, &ts_chip); 1191 1192 iio_device_set_drvdata(indio_dev, st); 1193 indio_dev->name = name; 1194 indio_dev->info = &inv_icm42600_accel_info; 1195 indio_dev->modes = INDIO_DIRECT_MODE; 1196 indio_dev->channels = inv_icm42600_accel_channels; 1197 indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_accel_channels); 1198 indio_dev->available_scan_masks = inv_icm42600_accel_scan_masks; 1199 1200 ret = devm_iio_kfifo_buffer_setup(dev, indio_dev, 1201 &inv_icm42600_buffer_ops); 1202 if (ret) 1203 return ERR_PTR(ret); 1204 1205 ret = devm_iio_device_register(dev, indio_dev); 1206 if (ret) 1207 return ERR_PTR(ret); 1208 1209 /* accel events are wakeup capable */ 1210 ret = devm_device_init_wakeup(&indio_dev->dev); 1211 if (ret) 1212 return ERR_PTR(ret); 1213 1214 return indio_dev; 1215 } 1216 1217 int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev) 1218 { 1219 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 1220 struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev); 1221 struct inv_sensors_timestamp *ts = &accel_st->ts; 1222 ssize_t i, size; 1223 unsigned int no; 1224 const void *accel, *gyro, *timestamp; 1225 const int8_t *temp; 1226 unsigned int odr; 1227 int64_t ts_val; 1228 /* buffer is copied to userspace, zeroing it to avoid any data leak */ 1229 struct inv_icm42600_accel_buffer buffer = { }; 1230 1231 /* parse all fifo packets */ 1232 for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) { 1233 size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i], 1234 &accel, &gyro, &temp, ×tamp, &odr); 1235 /* quit if error or FIFO is empty */ 1236 if (size <= 0) 1237 return size; 1238 1239 /* skip packet if no accel data or data is invalid */ 1240 if (accel == NULL || !inv_icm42600_fifo_is_data_valid(accel)) 1241 continue; 1242 1243 /* update odr */ 1244 if (odr & INV_ICM42600_SENSOR_ACCEL) 1245 inv_sensors_timestamp_apply_odr(ts, st->fifo.period, 1246 st->fifo.nb.total, no); 1247 1248 memcpy(&buffer.accel, accel, sizeof(buffer.accel)); 1249 /* convert 8 bits FIFO temperature in high resolution format */ 1250 buffer.temp = temp ? (*temp * 64) : 0; 1251 ts_val = inv_sensors_timestamp_pop(ts); 1252 iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val); 1253 } 1254 1255 return 0; 1256 } 1257