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