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