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