inv_icm42600_accel.c (20340B)
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/iio/iio.h> 14#include <linux/iio/buffer.h> 15#include <linux/iio/kfifo_buf.h> 16 17#include "inv_icm42600.h" 18#include "inv_icm42600_temp.h" 19#include "inv_icm42600_buffer.h" 20#include "inv_icm42600_timestamp.h" 21 22#define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info) \ 23 { \ 24 .type = IIO_ACCEL, \ 25 .modified = 1, \ 26 .channel2 = _modifier, \ 27 .info_mask_separate = \ 28 BIT(IIO_CHAN_INFO_RAW) | \ 29 BIT(IIO_CHAN_INFO_CALIBBIAS), \ 30 .info_mask_shared_by_type = \ 31 BIT(IIO_CHAN_INFO_SCALE), \ 32 .info_mask_shared_by_type_available = \ 33 BIT(IIO_CHAN_INFO_SCALE) | \ 34 BIT(IIO_CHAN_INFO_CALIBBIAS), \ 35 .info_mask_shared_by_all = \ 36 BIT(IIO_CHAN_INFO_SAMP_FREQ), \ 37 .info_mask_shared_by_all_available = \ 38 BIT(IIO_CHAN_INFO_SAMP_FREQ), \ 39 .scan_index = _index, \ 40 .scan_type = { \ 41 .sign = 's', \ 42 .realbits = 16, \ 43 .storagebits = 16, \ 44 .endianness = IIO_BE, \ 45 }, \ 46 .ext_info = _ext_info, \ 47 } 48 49enum inv_icm42600_accel_scan { 50 INV_ICM42600_ACCEL_SCAN_X, 51 INV_ICM42600_ACCEL_SCAN_Y, 52 INV_ICM42600_ACCEL_SCAN_Z, 53 INV_ICM42600_ACCEL_SCAN_TEMP, 54 INV_ICM42600_ACCEL_SCAN_TIMESTAMP, 55}; 56 57static const struct iio_chan_spec_ext_info inv_icm42600_accel_ext_infos[] = { 58 IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, inv_icm42600_get_mount_matrix), 59 {}, 60}; 61 62static const struct iio_chan_spec inv_icm42600_accel_channels[] = { 63 INV_ICM42600_ACCEL_CHAN(IIO_MOD_X, INV_ICM42600_ACCEL_SCAN_X, 64 inv_icm42600_accel_ext_infos), 65 INV_ICM42600_ACCEL_CHAN(IIO_MOD_Y, INV_ICM42600_ACCEL_SCAN_Y, 66 inv_icm42600_accel_ext_infos), 67 INV_ICM42600_ACCEL_CHAN(IIO_MOD_Z, INV_ICM42600_ACCEL_SCAN_Z, 68 inv_icm42600_accel_ext_infos), 69 INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP), 70 IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_ACCEL_SCAN_TIMESTAMP), 71}; 72 73/* 74 * IIO buffer data: size must be a power of 2 and timestamp aligned 75 * 16 bytes: 6 bytes acceleration, 2 bytes temperature, 8 bytes timestamp 76 */ 77struct inv_icm42600_accel_buffer { 78 struct inv_icm42600_fifo_sensor_data accel; 79 int16_t temp; 80 int64_t timestamp __aligned(8); 81}; 82 83#define INV_ICM42600_SCAN_MASK_ACCEL_3AXIS \ 84 (BIT(INV_ICM42600_ACCEL_SCAN_X) | \ 85 BIT(INV_ICM42600_ACCEL_SCAN_Y) | \ 86 BIT(INV_ICM42600_ACCEL_SCAN_Z)) 87 88#define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_ACCEL_SCAN_TEMP) 89 90static const unsigned long inv_icm42600_accel_scan_masks[] = { 91 /* 3-axis accel + temperature */ 92 INV_ICM42600_SCAN_MASK_ACCEL_3AXIS | INV_ICM42600_SCAN_MASK_TEMP, 93 0, 94}; 95 96/* enable accelerometer sensor and FIFO write */ 97static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev, 98 const unsigned long *scan_mask) 99{ 100 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 101 struct inv_icm42600_timestamp *ts = iio_priv(indio_dev); 102 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; 103 unsigned int fifo_en = 0; 104 unsigned int sleep_temp = 0; 105 unsigned int sleep_accel = 0; 106 unsigned int sleep; 107 int ret; 108 109 mutex_lock(&st->lock); 110 111 if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) { 112 /* enable temp sensor */ 113 ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp); 114 if (ret) 115 goto out_unlock; 116 fifo_en |= INV_ICM42600_SENSOR_TEMP; 117 } 118 119 if (*scan_mask & INV_ICM42600_SCAN_MASK_ACCEL_3AXIS) { 120 /* enable accel sensor */ 121 conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE; 122 ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_accel); 123 if (ret) 124 goto out_unlock; 125 fifo_en |= INV_ICM42600_SENSOR_ACCEL; 126 } 127 128 /* update data FIFO write */ 129 inv_icm42600_timestamp_apply_odr(ts, 0, 0, 0); 130 ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en); 131 if (ret) 132 goto out_unlock; 133 134 ret = inv_icm42600_buffer_update_watermark(st); 135 136out_unlock: 137 mutex_unlock(&st->lock); 138 /* sleep maximum required time */ 139 if (sleep_accel > sleep_temp) 140 sleep = sleep_accel; 141 else 142 sleep = sleep_temp; 143 if (sleep) 144 msleep(sleep); 145 return ret; 146} 147 148static int inv_icm42600_accel_read_sensor(struct inv_icm42600_state *st, 149 struct iio_chan_spec const *chan, 150 int16_t *val) 151{ 152 struct device *dev = regmap_get_device(st->map); 153 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; 154 unsigned int reg; 155 __be16 *data; 156 int ret; 157 158 if (chan->type != IIO_ACCEL) 159 return -EINVAL; 160 161 switch (chan->channel2) { 162 case IIO_MOD_X: 163 reg = INV_ICM42600_REG_ACCEL_DATA_X; 164 break; 165 case IIO_MOD_Y: 166 reg = INV_ICM42600_REG_ACCEL_DATA_Y; 167 break; 168 case IIO_MOD_Z: 169 reg = INV_ICM42600_REG_ACCEL_DATA_Z; 170 break; 171 default: 172 return -EINVAL; 173 } 174 175 pm_runtime_get_sync(dev); 176 mutex_lock(&st->lock); 177 178 /* enable accel sensor */ 179 conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE; 180 ret = inv_icm42600_set_accel_conf(st, &conf, NULL); 181 if (ret) 182 goto exit; 183 184 /* read accel register data */ 185 data = (__be16 *)&st->buffer[0]; 186 ret = regmap_bulk_read(st->map, reg, data, sizeof(*data)); 187 if (ret) 188 goto exit; 189 190 *val = (int16_t)be16_to_cpup(data); 191 if (*val == INV_ICM42600_DATA_INVALID) 192 ret = -EINVAL; 193exit: 194 mutex_unlock(&st->lock); 195 pm_runtime_mark_last_busy(dev); 196 pm_runtime_put_autosuspend(dev); 197 return ret; 198} 199 200/* IIO format int + nano */ 201static const int inv_icm42600_accel_scale[] = { 202 /* +/- 16G => 0.004788403 m/s-2 */ 203 [2 * INV_ICM42600_ACCEL_FS_16G] = 0, 204 [2 * INV_ICM42600_ACCEL_FS_16G + 1] = 4788403, 205 /* +/- 8G => 0.002394202 m/s-2 */ 206 [2 * INV_ICM42600_ACCEL_FS_8G] = 0, 207 [2 * INV_ICM42600_ACCEL_FS_8G + 1] = 2394202, 208 /* +/- 4G => 0.001197101 m/s-2 */ 209 [2 * INV_ICM42600_ACCEL_FS_4G] = 0, 210 [2 * INV_ICM42600_ACCEL_FS_4G + 1] = 1197101, 211 /* +/- 2G => 0.000598550 m/s-2 */ 212 [2 * INV_ICM42600_ACCEL_FS_2G] = 0, 213 [2 * INV_ICM42600_ACCEL_FS_2G + 1] = 598550, 214}; 215 216static int inv_icm42600_accel_read_scale(struct inv_icm42600_state *st, 217 int *val, int *val2) 218{ 219 unsigned int idx; 220 221 idx = st->conf.accel.fs; 222 223 *val = inv_icm42600_accel_scale[2 * idx]; 224 *val2 = inv_icm42600_accel_scale[2 * idx + 1]; 225 return IIO_VAL_INT_PLUS_NANO; 226} 227 228static int inv_icm42600_accel_write_scale(struct inv_icm42600_state *st, 229 int val, int val2) 230{ 231 struct device *dev = regmap_get_device(st->map); 232 unsigned int idx; 233 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; 234 int ret; 235 236 for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_scale); idx += 2) { 237 if (val == inv_icm42600_accel_scale[idx] && 238 val2 == inv_icm42600_accel_scale[idx + 1]) 239 break; 240 } 241 if (idx >= ARRAY_SIZE(inv_icm42600_accel_scale)) 242 return -EINVAL; 243 244 conf.fs = idx / 2; 245 246 pm_runtime_get_sync(dev); 247 mutex_lock(&st->lock); 248 249 ret = inv_icm42600_set_accel_conf(st, &conf, NULL); 250 251 mutex_unlock(&st->lock); 252 pm_runtime_mark_last_busy(dev); 253 pm_runtime_put_autosuspend(dev); 254 255 return ret; 256} 257 258/* IIO format int + micro */ 259static const int inv_icm42600_accel_odr[] = { 260 /* 12.5Hz */ 261 12, 500000, 262 /* 25Hz */ 263 25, 0, 264 /* 50Hz */ 265 50, 0, 266 /* 100Hz */ 267 100, 0, 268 /* 200Hz */ 269 200, 0, 270 /* 1kHz */ 271 1000, 0, 272 /* 2kHz */ 273 2000, 0, 274 /* 4kHz */ 275 4000, 0, 276}; 277 278static const int inv_icm42600_accel_odr_conv[] = { 279 INV_ICM42600_ODR_12_5HZ, 280 INV_ICM42600_ODR_25HZ, 281 INV_ICM42600_ODR_50HZ, 282 INV_ICM42600_ODR_100HZ, 283 INV_ICM42600_ODR_200HZ, 284 INV_ICM42600_ODR_1KHZ_LN, 285 INV_ICM42600_ODR_2KHZ_LN, 286 INV_ICM42600_ODR_4KHZ_LN, 287}; 288 289static int inv_icm42600_accel_read_odr(struct inv_icm42600_state *st, 290 int *val, int *val2) 291{ 292 unsigned int odr; 293 unsigned int i; 294 295 odr = st->conf.accel.odr; 296 297 for (i = 0; i < ARRAY_SIZE(inv_icm42600_accel_odr_conv); ++i) { 298 if (inv_icm42600_accel_odr_conv[i] == odr) 299 break; 300 } 301 if (i >= ARRAY_SIZE(inv_icm42600_accel_odr_conv)) 302 return -EINVAL; 303 304 *val = inv_icm42600_accel_odr[2 * i]; 305 *val2 = inv_icm42600_accel_odr[2 * i + 1]; 306 307 return IIO_VAL_INT_PLUS_MICRO; 308} 309 310static int inv_icm42600_accel_write_odr(struct iio_dev *indio_dev, 311 int val, int val2) 312{ 313 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 314 struct inv_icm42600_timestamp *ts = iio_priv(indio_dev); 315 struct device *dev = regmap_get_device(st->map); 316 unsigned int idx; 317 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; 318 int ret; 319 320 for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_odr); idx += 2) { 321 if (val == inv_icm42600_accel_odr[idx] && 322 val2 == inv_icm42600_accel_odr[idx + 1]) 323 break; 324 } 325 if (idx >= ARRAY_SIZE(inv_icm42600_accel_odr)) 326 return -EINVAL; 327 328 conf.odr = inv_icm42600_accel_odr_conv[idx / 2]; 329 330 pm_runtime_get_sync(dev); 331 mutex_lock(&st->lock); 332 333 ret = inv_icm42600_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr), 334 iio_buffer_enabled(indio_dev)); 335 if (ret) 336 goto out_unlock; 337 338 ret = inv_icm42600_set_accel_conf(st, &conf, NULL); 339 if (ret) 340 goto out_unlock; 341 inv_icm42600_buffer_update_fifo_period(st); 342 inv_icm42600_buffer_update_watermark(st); 343 344out_unlock: 345 mutex_unlock(&st->lock); 346 pm_runtime_mark_last_busy(dev); 347 pm_runtime_put_autosuspend(dev); 348 349 return ret; 350} 351 352/* 353 * Calibration bias values, IIO range format int + micro. 354 * Value is limited to +/-1g coded on 12 bits signed. Step is 0.5mg. 355 */ 356static int inv_icm42600_accel_calibbias[] = { 357 -10, 42010, /* min: -10.042010 m/s² */ 358 0, 4903, /* step: 0.004903 m/s² */ 359 10, 37106, /* max: 10.037106 m/s² */ 360}; 361 362static int inv_icm42600_accel_read_offset(struct inv_icm42600_state *st, 363 struct iio_chan_spec const *chan, 364 int *val, int *val2) 365{ 366 struct device *dev = regmap_get_device(st->map); 367 int64_t val64; 368 int32_t bias; 369 unsigned int reg; 370 int16_t offset; 371 uint8_t data[2]; 372 int ret; 373 374 if (chan->type != IIO_ACCEL) 375 return -EINVAL; 376 377 switch (chan->channel2) { 378 case IIO_MOD_X: 379 reg = INV_ICM42600_REG_OFFSET_USER4; 380 break; 381 case IIO_MOD_Y: 382 reg = INV_ICM42600_REG_OFFSET_USER6; 383 break; 384 case IIO_MOD_Z: 385 reg = INV_ICM42600_REG_OFFSET_USER7; 386 break; 387 default: 388 return -EINVAL; 389 } 390 391 pm_runtime_get_sync(dev); 392 mutex_lock(&st->lock); 393 394 ret = regmap_bulk_read(st->map, reg, st->buffer, sizeof(data)); 395 memcpy(data, st->buffer, sizeof(data)); 396 397 mutex_unlock(&st->lock); 398 pm_runtime_mark_last_busy(dev); 399 pm_runtime_put_autosuspend(dev); 400 if (ret) 401 return ret; 402 403 /* 12 bits signed value */ 404 switch (chan->channel2) { 405 case IIO_MOD_X: 406 offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11); 407 break; 408 case IIO_MOD_Y: 409 offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11); 410 break; 411 case IIO_MOD_Z: 412 offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11); 413 break; 414 default: 415 return -EINVAL; 416 } 417 418 /* 419 * convert raw offset to g then to m/s² 420 * 12 bits signed raw step 0.5mg to g: 5 / 10000 421 * g to m/s²: 9.806650 422 * result in micro (1000000) 423 * (offset * 5 * 9.806650 * 1000000) / 10000 424 */ 425 val64 = (int64_t)offset * 5LL * 9806650LL; 426 /* for rounding, add + or - divisor (10000) divided by 2 */ 427 if (val64 >= 0) 428 val64 += 10000LL / 2LL; 429 else 430 val64 -= 10000LL / 2LL; 431 bias = div_s64(val64, 10000L); 432 *val = bias / 1000000L; 433 *val2 = bias % 1000000L; 434 435 return IIO_VAL_INT_PLUS_MICRO; 436} 437 438static int inv_icm42600_accel_write_offset(struct inv_icm42600_state *st, 439 struct iio_chan_spec const *chan, 440 int val, int val2) 441{ 442 struct device *dev = regmap_get_device(st->map); 443 int64_t val64; 444 int32_t min, max; 445 unsigned int reg, regval; 446 int16_t offset; 447 int ret; 448 449 if (chan->type != IIO_ACCEL) 450 return -EINVAL; 451 452 switch (chan->channel2) { 453 case IIO_MOD_X: 454 reg = INV_ICM42600_REG_OFFSET_USER4; 455 break; 456 case IIO_MOD_Y: 457 reg = INV_ICM42600_REG_OFFSET_USER6; 458 break; 459 case IIO_MOD_Z: 460 reg = INV_ICM42600_REG_OFFSET_USER7; 461 break; 462 default: 463 return -EINVAL; 464 } 465 466 /* inv_icm42600_accel_calibbias: min - step - max in micro */ 467 min = inv_icm42600_accel_calibbias[0] * 1000000L + 468 inv_icm42600_accel_calibbias[1]; 469 max = inv_icm42600_accel_calibbias[4] * 1000000L + 470 inv_icm42600_accel_calibbias[5]; 471 val64 = (int64_t)val * 1000000LL + (int64_t)val2; 472 if (val64 < min || val64 > max) 473 return -EINVAL; 474 475 /* 476 * convert m/s² to g then to raw value 477 * m/s² to g: 1 / 9.806650 478 * g to raw 12 bits signed, step 0.5mg: 10000 / 5 479 * val in micro (1000000) 480 * val * 10000 / (9.806650 * 1000000 * 5) 481 */ 482 val64 = val64 * 10000LL; 483 /* for rounding, add + or - divisor (9806650 * 5) divided by 2 */ 484 if (val64 >= 0) 485 val64 += 9806650 * 5 / 2; 486 else 487 val64 -= 9806650 * 5 / 2; 488 offset = div_s64(val64, 9806650 * 5); 489 490 /* clamp value limited to 12 bits signed */ 491 if (offset < -2048) 492 offset = -2048; 493 else if (offset > 2047) 494 offset = 2047; 495 496 pm_runtime_get_sync(dev); 497 mutex_lock(&st->lock); 498 499 switch (chan->channel2) { 500 case IIO_MOD_X: 501 /* OFFSET_USER4 register is shared */ 502 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER4, 503 ®val); 504 if (ret) 505 goto out_unlock; 506 st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F); 507 st->buffer[1] = offset & 0xFF; 508 break; 509 case IIO_MOD_Y: 510 /* OFFSET_USER7 register is shared */ 511 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER7, 512 ®val); 513 if (ret) 514 goto out_unlock; 515 st->buffer[0] = offset & 0xFF; 516 st->buffer[1] = ((offset & 0xF00) >> 8) | (regval & 0xF0); 517 break; 518 case IIO_MOD_Z: 519 /* OFFSET_USER7 register is shared */ 520 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER7, 521 ®val); 522 if (ret) 523 goto out_unlock; 524 st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F); 525 st->buffer[1] = offset & 0xFF; 526 break; 527 default: 528 ret = -EINVAL; 529 goto out_unlock; 530 } 531 532 ret = regmap_bulk_write(st->map, reg, st->buffer, 2); 533 534out_unlock: 535 mutex_unlock(&st->lock); 536 pm_runtime_mark_last_busy(dev); 537 pm_runtime_put_autosuspend(dev); 538 return ret; 539} 540 541static int inv_icm42600_accel_read_raw(struct iio_dev *indio_dev, 542 struct iio_chan_spec const *chan, 543 int *val, int *val2, long mask) 544{ 545 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 546 int16_t data; 547 int ret; 548 549 switch (chan->type) { 550 case IIO_ACCEL: 551 break; 552 case IIO_TEMP: 553 return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2, mask); 554 default: 555 return -EINVAL; 556 } 557 558 switch (mask) { 559 case IIO_CHAN_INFO_RAW: 560 ret = iio_device_claim_direct_mode(indio_dev); 561 if (ret) 562 return ret; 563 ret = inv_icm42600_accel_read_sensor(st, chan, &data); 564 iio_device_release_direct_mode(indio_dev); 565 if (ret) 566 return ret; 567 *val = data; 568 return IIO_VAL_INT; 569 case IIO_CHAN_INFO_SCALE: 570 return inv_icm42600_accel_read_scale(st, val, val2); 571 case IIO_CHAN_INFO_SAMP_FREQ: 572 return inv_icm42600_accel_read_odr(st, val, val2); 573 case IIO_CHAN_INFO_CALIBBIAS: 574 return inv_icm42600_accel_read_offset(st, chan, val, val2); 575 default: 576 return -EINVAL; 577 } 578} 579 580static int inv_icm42600_accel_read_avail(struct iio_dev *indio_dev, 581 struct iio_chan_spec const *chan, 582 const int **vals, 583 int *type, int *length, long mask) 584{ 585 if (chan->type != IIO_ACCEL) 586 return -EINVAL; 587 588 switch (mask) { 589 case IIO_CHAN_INFO_SCALE: 590 *vals = inv_icm42600_accel_scale; 591 *type = IIO_VAL_INT_PLUS_NANO; 592 *length = ARRAY_SIZE(inv_icm42600_accel_scale); 593 return IIO_AVAIL_LIST; 594 case IIO_CHAN_INFO_SAMP_FREQ: 595 *vals = inv_icm42600_accel_odr; 596 *type = IIO_VAL_INT_PLUS_MICRO; 597 *length = ARRAY_SIZE(inv_icm42600_accel_odr); 598 return IIO_AVAIL_LIST; 599 case IIO_CHAN_INFO_CALIBBIAS: 600 *vals = inv_icm42600_accel_calibbias; 601 *type = IIO_VAL_INT_PLUS_MICRO; 602 return IIO_AVAIL_RANGE; 603 default: 604 return -EINVAL; 605 } 606} 607 608static int inv_icm42600_accel_write_raw(struct iio_dev *indio_dev, 609 struct iio_chan_spec const *chan, 610 int val, int val2, long mask) 611{ 612 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 613 int ret; 614 615 if (chan->type != IIO_ACCEL) 616 return -EINVAL; 617 618 switch (mask) { 619 case IIO_CHAN_INFO_SCALE: 620 ret = iio_device_claim_direct_mode(indio_dev); 621 if (ret) 622 return ret; 623 ret = inv_icm42600_accel_write_scale(st, val, val2); 624 iio_device_release_direct_mode(indio_dev); 625 return ret; 626 case IIO_CHAN_INFO_SAMP_FREQ: 627 return inv_icm42600_accel_write_odr(indio_dev, val, val2); 628 case IIO_CHAN_INFO_CALIBBIAS: 629 ret = iio_device_claim_direct_mode(indio_dev); 630 if (ret) 631 return ret; 632 ret = inv_icm42600_accel_write_offset(st, chan, val, val2); 633 iio_device_release_direct_mode(indio_dev); 634 return ret; 635 default: 636 return -EINVAL; 637 } 638} 639 640static int inv_icm42600_accel_write_raw_get_fmt(struct iio_dev *indio_dev, 641 struct iio_chan_spec const *chan, 642 long mask) 643{ 644 if (chan->type != IIO_ACCEL) 645 return -EINVAL; 646 647 switch (mask) { 648 case IIO_CHAN_INFO_SCALE: 649 return IIO_VAL_INT_PLUS_NANO; 650 case IIO_CHAN_INFO_SAMP_FREQ: 651 return IIO_VAL_INT_PLUS_MICRO; 652 case IIO_CHAN_INFO_CALIBBIAS: 653 return IIO_VAL_INT_PLUS_MICRO; 654 default: 655 return -EINVAL; 656 } 657} 658 659static int inv_icm42600_accel_hwfifo_set_watermark(struct iio_dev *indio_dev, 660 unsigned int val) 661{ 662 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 663 int ret; 664 665 mutex_lock(&st->lock); 666 667 st->fifo.watermark.accel = val; 668 ret = inv_icm42600_buffer_update_watermark(st); 669 670 mutex_unlock(&st->lock); 671 672 return ret; 673} 674 675static int inv_icm42600_accel_hwfifo_flush(struct iio_dev *indio_dev, 676 unsigned int count) 677{ 678 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 679 int ret; 680 681 if (count == 0) 682 return 0; 683 684 mutex_lock(&st->lock); 685 686 ret = inv_icm42600_buffer_hwfifo_flush(st, count); 687 if (!ret) 688 ret = st->fifo.nb.accel; 689 690 mutex_unlock(&st->lock); 691 692 return ret; 693} 694 695static const struct iio_info inv_icm42600_accel_info = { 696 .read_raw = inv_icm42600_accel_read_raw, 697 .read_avail = inv_icm42600_accel_read_avail, 698 .write_raw = inv_icm42600_accel_write_raw, 699 .write_raw_get_fmt = inv_icm42600_accel_write_raw_get_fmt, 700 .debugfs_reg_access = inv_icm42600_debugfs_reg, 701 .update_scan_mode = inv_icm42600_accel_update_scan_mode, 702 .hwfifo_set_watermark = inv_icm42600_accel_hwfifo_set_watermark, 703 .hwfifo_flush_to_buffer = inv_icm42600_accel_hwfifo_flush, 704}; 705 706struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st) 707{ 708 struct device *dev = regmap_get_device(st->map); 709 const char *name; 710 struct inv_icm42600_timestamp *ts; 711 struct iio_dev *indio_dev; 712 int ret; 713 714 name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name); 715 if (!name) 716 return ERR_PTR(-ENOMEM); 717 718 indio_dev = devm_iio_device_alloc(dev, sizeof(*ts)); 719 if (!indio_dev) 720 return ERR_PTR(-ENOMEM); 721 722 ts = iio_priv(indio_dev); 723 inv_icm42600_timestamp_init(ts, inv_icm42600_odr_to_period(st->conf.accel.odr)); 724 725 iio_device_set_drvdata(indio_dev, st); 726 indio_dev->name = name; 727 indio_dev->info = &inv_icm42600_accel_info; 728 indio_dev->modes = INDIO_DIRECT_MODE; 729 indio_dev->channels = inv_icm42600_accel_channels; 730 indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_accel_channels); 731 indio_dev->available_scan_masks = inv_icm42600_accel_scan_masks; 732 733 ret = devm_iio_kfifo_buffer_setup(dev, indio_dev, 734 &inv_icm42600_buffer_ops); 735 if (ret) 736 return ERR_PTR(ret); 737 738 ret = devm_iio_device_register(dev, indio_dev); 739 if (ret) 740 return ERR_PTR(ret); 741 742 return indio_dev; 743} 744 745int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev) 746{ 747 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 748 struct inv_icm42600_timestamp *ts = iio_priv(indio_dev); 749 ssize_t i, size; 750 unsigned int no; 751 const void *accel, *gyro, *timestamp; 752 const int8_t *temp; 753 unsigned int odr; 754 int64_t ts_val; 755 struct inv_icm42600_accel_buffer buffer; 756 757 /* parse all fifo packets */ 758 for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) { 759 size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i], 760 &accel, &gyro, &temp, ×tamp, &odr); 761 /* quit if error or FIFO is empty */ 762 if (size <= 0) 763 return size; 764 765 /* skip packet if no accel data or data is invalid */ 766 if (accel == NULL || !inv_icm42600_fifo_is_data_valid(accel)) 767 continue; 768 769 /* update odr */ 770 if (odr & INV_ICM42600_SENSOR_ACCEL) 771 inv_icm42600_timestamp_apply_odr(ts, st->fifo.period, 772 st->fifo.nb.total, no); 773 774 /* buffer is copied to userspace, zeroing it to avoid any data leak */ 775 memset(&buffer, 0, sizeof(buffer)); 776 memcpy(&buffer.accel, accel, sizeof(buffer.accel)); 777 /* convert 8 bits FIFO temperature in high resolution format */ 778 buffer.temp = temp ? (*temp * 64) : 0; 779 ts_val = inv_icm42600_timestamp_pop(ts); 780 iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val); 781 } 782 783 return 0; 784}