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