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