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