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_GYRO_CHAN(_modifier, _index, _ext_info) \ 24 { \ 25 .type = IIO_ANGL_VEL, \ 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_gyro_scan { 51 INV_ICM42600_GYRO_SCAN_X, 52 INV_ICM42600_GYRO_SCAN_Y, 53 INV_ICM42600_GYRO_SCAN_Z, 54 INV_ICM42600_GYRO_SCAN_TEMP, 55 INV_ICM42600_GYRO_SCAN_TIMESTAMP, 56 }; 57 58 static const struct iio_chan_spec_ext_info inv_icm42600_gyro_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_gyro_channels[] = { 64 INV_ICM42600_GYRO_CHAN(IIO_MOD_X, INV_ICM42600_GYRO_SCAN_X, 65 inv_icm42600_gyro_ext_infos), 66 INV_ICM42600_GYRO_CHAN(IIO_MOD_Y, INV_ICM42600_GYRO_SCAN_Y, 67 inv_icm42600_gyro_ext_infos), 68 INV_ICM42600_GYRO_CHAN(IIO_MOD_Z, INV_ICM42600_GYRO_SCAN_Z, 69 inv_icm42600_gyro_ext_infos), 70 INV_ICM42600_TEMP_CHAN(INV_ICM42600_GYRO_SCAN_TEMP), 71 IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_GYRO_SCAN_TIMESTAMP), 72 }; 73 74 /* 75 * IIO buffer data: size must be a power of 2 and timestamp aligned 76 * 16 bytes: 6 bytes angular velocity, 2 bytes temperature, 8 bytes timestamp 77 */ 78 struct inv_icm42600_gyro_buffer { 79 struct inv_icm42600_fifo_sensor_data gyro; 80 int16_t temp; 81 int64_t timestamp __aligned(8); 82 }; 83 84 #define INV_ICM42600_SCAN_MASK_GYRO_3AXIS \ 85 (BIT(INV_ICM42600_GYRO_SCAN_X) | \ 86 BIT(INV_ICM42600_GYRO_SCAN_Y) | \ 87 BIT(INV_ICM42600_GYRO_SCAN_Z)) 88 89 #define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_GYRO_SCAN_TEMP) 90 91 static const unsigned long inv_icm42600_gyro_scan_masks[] = { 92 /* 3-axis gyro + temperature */ 93 INV_ICM42600_SCAN_MASK_GYRO_3AXIS | INV_ICM42600_SCAN_MASK_TEMP, 94 0, 95 }; 96 97 /* enable gyroscope sensor and FIFO write */ 98 static int inv_icm42600_gyro_update_scan_mode(struct iio_dev *indio_dev, 99 const unsigned long *scan_mask) 100 { 101 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 102 struct inv_sensors_timestamp *ts = iio_priv(indio_dev); 103 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; 104 unsigned int fifo_en = 0; 105 unsigned int sleep_gyro = 0; 106 unsigned int sleep_temp = 0; 107 unsigned int sleep; 108 int ret; 109 110 mutex_lock(&st->lock); 111 112 if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) { 113 /* enable temp sensor */ 114 ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp); 115 if (ret) 116 goto out_unlock; 117 fifo_en |= INV_ICM42600_SENSOR_TEMP; 118 } 119 120 if (*scan_mask & INV_ICM42600_SCAN_MASK_GYRO_3AXIS) { 121 /* enable gyro sensor */ 122 conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE; 123 ret = inv_icm42600_set_gyro_conf(st, &conf, &sleep_gyro); 124 if (ret) 125 goto out_unlock; 126 fifo_en |= INV_ICM42600_SENSOR_GYRO; 127 } 128 129 /* update data FIFO write */ 130 inv_sensors_timestamp_apply_odr(ts, 0, 0, 0); 131 ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en); 132 if (ret) 133 goto out_unlock; 134 135 ret = inv_icm42600_buffer_update_watermark(st); 136 137 out_unlock: 138 mutex_unlock(&st->lock); 139 /* sleep maximum required time */ 140 sleep = max(sleep_gyro, sleep_temp); 141 if (sleep) 142 msleep(sleep); 143 return ret; 144 } 145 146 static int inv_icm42600_gyro_read_sensor(struct inv_icm42600_state *st, 147 struct iio_chan_spec const *chan, 148 int16_t *val) 149 { 150 struct device *dev = regmap_get_device(st->map); 151 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; 152 unsigned int reg; 153 __be16 *data; 154 int ret; 155 156 if (chan->type != IIO_ANGL_VEL) 157 return -EINVAL; 158 159 switch (chan->channel2) { 160 case IIO_MOD_X: 161 reg = INV_ICM42600_REG_GYRO_DATA_X; 162 break; 163 case IIO_MOD_Y: 164 reg = INV_ICM42600_REG_GYRO_DATA_Y; 165 break; 166 case IIO_MOD_Z: 167 reg = INV_ICM42600_REG_GYRO_DATA_Z; 168 break; 169 default: 170 return -EINVAL; 171 } 172 173 pm_runtime_get_sync(dev); 174 mutex_lock(&st->lock); 175 176 /* enable gyro sensor */ 177 conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE; 178 ret = inv_icm42600_set_gyro_conf(st, &conf, NULL); 179 if (ret) 180 goto exit; 181 182 /* read gyro register data */ 183 data = (__be16 *)&st->buffer[0]; 184 ret = regmap_bulk_read(st->map, reg, data, sizeof(*data)); 185 if (ret) 186 goto exit; 187 188 *val = (int16_t)be16_to_cpup(data); 189 if (*val == INV_ICM42600_DATA_INVALID) 190 ret = -EINVAL; 191 exit: 192 mutex_unlock(&st->lock); 193 pm_runtime_mark_last_busy(dev); 194 pm_runtime_put_autosuspend(dev); 195 return ret; 196 } 197 198 /* IIO format int + nano */ 199 static const int inv_icm42600_gyro_scale[] = { 200 /* +/- 2000dps => 0.001065264 rad/s */ 201 [2 * INV_ICM42600_GYRO_FS_2000DPS] = 0, 202 [2 * INV_ICM42600_GYRO_FS_2000DPS + 1] = 1065264, 203 /* +/- 1000dps => 0.000532632 rad/s */ 204 [2 * INV_ICM42600_GYRO_FS_1000DPS] = 0, 205 [2 * INV_ICM42600_GYRO_FS_1000DPS + 1] = 532632, 206 /* +/- 500dps => 0.000266316 rad/s */ 207 [2 * INV_ICM42600_GYRO_FS_500DPS] = 0, 208 [2 * INV_ICM42600_GYRO_FS_500DPS + 1] = 266316, 209 /* +/- 250dps => 0.000133158 rad/s */ 210 [2 * INV_ICM42600_GYRO_FS_250DPS] = 0, 211 [2 * INV_ICM42600_GYRO_FS_250DPS + 1] = 133158, 212 /* +/- 125dps => 0.000066579 rad/s */ 213 [2 * INV_ICM42600_GYRO_FS_125DPS] = 0, 214 [2 * INV_ICM42600_GYRO_FS_125DPS + 1] = 66579, 215 /* +/- 62.5dps => 0.000033290 rad/s */ 216 [2 * INV_ICM42600_GYRO_FS_62_5DPS] = 0, 217 [2 * INV_ICM42600_GYRO_FS_62_5DPS + 1] = 33290, 218 /* +/- 31.25dps => 0.000016645 rad/s */ 219 [2 * INV_ICM42600_GYRO_FS_31_25DPS] = 0, 220 [2 * INV_ICM42600_GYRO_FS_31_25DPS + 1] = 16645, 221 /* +/- 15.625dps => 0.000008322 rad/s */ 222 [2 * INV_ICM42600_GYRO_FS_15_625DPS] = 0, 223 [2 * INV_ICM42600_GYRO_FS_15_625DPS + 1] = 8322, 224 }; 225 226 static int inv_icm42600_gyro_read_scale(struct inv_icm42600_state *st, 227 int *val, int *val2) 228 { 229 unsigned int idx; 230 231 idx = st->conf.gyro.fs; 232 233 *val = inv_icm42600_gyro_scale[2 * idx]; 234 *val2 = inv_icm42600_gyro_scale[2 * idx + 1]; 235 return IIO_VAL_INT_PLUS_NANO; 236 } 237 238 static int inv_icm42600_gyro_write_scale(struct inv_icm42600_state *st, 239 int val, int val2) 240 { 241 struct device *dev = regmap_get_device(st->map); 242 unsigned int idx; 243 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; 244 int ret; 245 246 for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_gyro_scale); idx += 2) { 247 if (val == inv_icm42600_gyro_scale[idx] && 248 val2 == inv_icm42600_gyro_scale[idx + 1]) 249 break; 250 } 251 if (idx >= ARRAY_SIZE(inv_icm42600_gyro_scale)) 252 return -EINVAL; 253 254 conf.fs = idx / 2; 255 256 pm_runtime_get_sync(dev); 257 mutex_lock(&st->lock); 258 259 ret = inv_icm42600_set_gyro_conf(st, &conf, NULL); 260 261 mutex_unlock(&st->lock); 262 pm_runtime_mark_last_busy(dev); 263 pm_runtime_put_autosuspend(dev); 264 265 return ret; 266 } 267 268 /* IIO format int + micro */ 269 static const int inv_icm42600_gyro_odr[] = { 270 /* 12.5Hz */ 271 12, 500000, 272 /* 25Hz */ 273 25, 0, 274 /* 50Hz */ 275 50, 0, 276 /* 100Hz */ 277 100, 0, 278 /* 200Hz */ 279 200, 0, 280 /* 1kHz */ 281 1000, 0, 282 /* 2kHz */ 283 2000, 0, 284 /* 4kHz */ 285 4000, 0, 286 }; 287 288 static const int inv_icm42600_gyro_odr_conv[] = { 289 INV_ICM42600_ODR_12_5HZ, 290 INV_ICM42600_ODR_25HZ, 291 INV_ICM42600_ODR_50HZ, 292 INV_ICM42600_ODR_100HZ, 293 INV_ICM42600_ODR_200HZ, 294 INV_ICM42600_ODR_1KHZ_LN, 295 INV_ICM42600_ODR_2KHZ_LN, 296 INV_ICM42600_ODR_4KHZ_LN, 297 }; 298 299 static int inv_icm42600_gyro_read_odr(struct inv_icm42600_state *st, 300 int *val, int *val2) 301 { 302 unsigned int odr; 303 unsigned int i; 304 305 odr = st->conf.gyro.odr; 306 307 for (i = 0; i < ARRAY_SIZE(inv_icm42600_gyro_odr_conv); ++i) { 308 if (inv_icm42600_gyro_odr_conv[i] == odr) 309 break; 310 } 311 if (i >= ARRAY_SIZE(inv_icm42600_gyro_odr_conv)) 312 return -EINVAL; 313 314 *val = inv_icm42600_gyro_odr[2 * i]; 315 *val2 = inv_icm42600_gyro_odr[2 * i + 1]; 316 317 return IIO_VAL_INT_PLUS_MICRO; 318 } 319 320 static int inv_icm42600_gyro_write_odr(struct iio_dev *indio_dev, 321 int val, int val2) 322 { 323 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 324 struct inv_sensors_timestamp *ts = iio_priv(indio_dev); 325 struct device *dev = regmap_get_device(st->map); 326 unsigned int idx; 327 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; 328 int ret; 329 330 for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_gyro_odr); idx += 2) { 331 if (val == inv_icm42600_gyro_odr[idx] && 332 val2 == inv_icm42600_gyro_odr[idx + 1]) 333 break; 334 } 335 if (idx >= ARRAY_SIZE(inv_icm42600_gyro_odr)) 336 return -EINVAL; 337 338 conf.odr = inv_icm42600_gyro_odr_conv[idx / 2]; 339 340 pm_runtime_get_sync(dev); 341 mutex_lock(&st->lock); 342 343 ret = inv_sensors_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr), 344 iio_buffer_enabled(indio_dev)); 345 if (ret) 346 goto out_unlock; 347 348 ret = inv_icm42600_set_gyro_conf(st, &conf, NULL); 349 if (ret) 350 goto out_unlock; 351 inv_icm42600_buffer_update_fifo_period(st); 352 inv_icm42600_buffer_update_watermark(st); 353 354 out_unlock: 355 mutex_unlock(&st->lock); 356 pm_runtime_mark_last_busy(dev); 357 pm_runtime_put_autosuspend(dev); 358 359 return ret; 360 } 361 362 /* 363 * Calibration bias values, IIO range format int + nano. 364 * Value is limited to +/-64dps coded on 12 bits signed. Step is 1/32 dps. 365 */ 366 static int inv_icm42600_gyro_calibbias[] = { 367 -1, 117010721, /* min: -1.117010721 rad/s */ 368 0, 545415, /* step: 0.000545415 rad/s */ 369 1, 116465306, /* max: 1.116465306 rad/s */ 370 }; 371 372 static int inv_icm42600_gyro_read_offset(struct inv_icm42600_state *st, 373 struct iio_chan_spec const *chan, 374 int *val, int *val2) 375 { 376 struct device *dev = regmap_get_device(st->map); 377 int64_t val64; 378 int32_t bias; 379 unsigned int reg; 380 int16_t offset; 381 uint8_t data[2]; 382 int ret; 383 384 if (chan->type != IIO_ANGL_VEL) 385 return -EINVAL; 386 387 switch (chan->channel2) { 388 case IIO_MOD_X: 389 reg = INV_ICM42600_REG_OFFSET_USER0; 390 break; 391 case IIO_MOD_Y: 392 reg = INV_ICM42600_REG_OFFSET_USER1; 393 break; 394 case IIO_MOD_Z: 395 reg = INV_ICM42600_REG_OFFSET_USER3; 396 break; 397 default: 398 return -EINVAL; 399 } 400 401 pm_runtime_get_sync(dev); 402 mutex_lock(&st->lock); 403 404 ret = regmap_bulk_read(st->map, reg, st->buffer, sizeof(data)); 405 memcpy(data, st->buffer, sizeof(data)); 406 407 mutex_unlock(&st->lock); 408 pm_runtime_mark_last_busy(dev); 409 pm_runtime_put_autosuspend(dev); 410 if (ret) 411 return ret; 412 413 /* 12 bits signed value */ 414 switch (chan->channel2) { 415 case IIO_MOD_X: 416 offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11); 417 break; 418 case IIO_MOD_Y: 419 offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11); 420 break; 421 case IIO_MOD_Z: 422 offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11); 423 break; 424 default: 425 return -EINVAL; 426 } 427 428 /* 429 * convert raw offset to dps then to rad/s 430 * 12 bits signed raw max 64 to dps: 64 / 2048 431 * dps to rad: Pi / 180 432 * result in nano (1000000000) 433 * (offset * 64 * Pi * 1000000000) / (2048 * 180) 434 */ 435 val64 = (int64_t)offset * 64LL * 3141592653LL; 436 /* for rounding, add + or - divisor (2048 * 180) divided by 2 */ 437 if (val64 >= 0) 438 val64 += 2048 * 180 / 2; 439 else 440 val64 -= 2048 * 180 / 2; 441 bias = div_s64(val64, 2048 * 180); 442 *val = bias / 1000000000L; 443 *val2 = bias % 1000000000L; 444 445 return IIO_VAL_INT_PLUS_NANO; 446 } 447 448 static int inv_icm42600_gyro_write_offset(struct inv_icm42600_state *st, 449 struct iio_chan_spec const *chan, 450 int val, int val2) 451 { 452 struct device *dev = regmap_get_device(st->map); 453 int64_t val64, min, max; 454 unsigned int reg, regval; 455 int16_t offset; 456 int ret; 457 458 if (chan->type != IIO_ANGL_VEL) 459 return -EINVAL; 460 461 switch (chan->channel2) { 462 case IIO_MOD_X: 463 reg = INV_ICM42600_REG_OFFSET_USER0; 464 break; 465 case IIO_MOD_Y: 466 reg = INV_ICM42600_REG_OFFSET_USER1; 467 break; 468 case IIO_MOD_Z: 469 reg = INV_ICM42600_REG_OFFSET_USER3; 470 break; 471 default: 472 return -EINVAL; 473 } 474 475 /* inv_icm42600_gyro_calibbias: min - step - max in nano */ 476 min = (int64_t)inv_icm42600_gyro_calibbias[0] * 1000000000LL + 477 (int64_t)inv_icm42600_gyro_calibbias[1]; 478 max = (int64_t)inv_icm42600_gyro_calibbias[4] * 1000000000LL + 479 (int64_t)inv_icm42600_gyro_calibbias[5]; 480 val64 = (int64_t)val * 1000000000LL + (int64_t)val2; 481 if (val64 < min || val64 > max) 482 return -EINVAL; 483 484 /* 485 * convert rad/s to dps then to raw value 486 * rad to dps: 180 / Pi 487 * dps to raw 12 bits signed, max 64: 2048 / 64 488 * val in nano (1000000000) 489 * val * 180 * 2048 / (Pi * 1000000000 * 64) 490 */ 491 val64 = val64 * 180LL * 2048LL; 492 /* for rounding, add + or - divisor (3141592653 * 64) divided by 2 */ 493 if (val64 >= 0) 494 val64 += 3141592653LL * 64LL / 2LL; 495 else 496 val64 -= 3141592653LL * 64LL / 2LL; 497 offset = div64_s64(val64, 3141592653LL * 64LL); 498 499 /* clamp value limited to 12 bits signed */ 500 if (offset < -2048) 501 offset = -2048; 502 else if (offset > 2047) 503 offset = 2047; 504 505 pm_runtime_get_sync(dev); 506 mutex_lock(&st->lock); 507 508 switch (chan->channel2) { 509 case IIO_MOD_X: 510 /* OFFSET_USER1 register is shared */ 511 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER1, 512 ®val); 513 if (ret) 514 goto out_unlock; 515 st->buffer[0] = offset & 0xFF; 516 st->buffer[1] = (regval & 0xF0) | ((offset & 0xF00) >> 8); 517 break; 518 case IIO_MOD_Y: 519 /* OFFSET_USER1 register is shared */ 520 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER1, 521 ®val); 522 if (ret) 523 goto out_unlock; 524 st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F); 525 st->buffer[1] = offset & 0xFF; 526 break; 527 case IIO_MOD_Z: 528 /* OFFSET_USER4 register is shared */ 529 ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER4, 530 ®val); 531 if (ret) 532 goto out_unlock; 533 st->buffer[0] = offset & 0xFF; 534 st->buffer[1] = (regval & 0xF0) | ((offset & 0xF00) >> 8); 535 break; 536 default: 537 ret = -EINVAL; 538 goto out_unlock; 539 } 540 541 ret = regmap_bulk_write(st->map, reg, st->buffer, 2); 542 543 out_unlock: 544 mutex_unlock(&st->lock); 545 pm_runtime_mark_last_busy(dev); 546 pm_runtime_put_autosuspend(dev); 547 return ret; 548 } 549 550 static int inv_icm42600_gyro_read_raw(struct iio_dev *indio_dev, 551 struct iio_chan_spec const *chan, 552 int *val, int *val2, long mask) 553 { 554 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 555 int16_t data; 556 int ret; 557 558 switch (chan->type) { 559 case IIO_ANGL_VEL: 560 break; 561 case IIO_TEMP: 562 return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2, mask); 563 default: 564 return -EINVAL; 565 } 566 567 switch (mask) { 568 case IIO_CHAN_INFO_RAW: 569 ret = iio_device_claim_direct_mode(indio_dev); 570 if (ret) 571 return ret; 572 ret = inv_icm42600_gyro_read_sensor(st, chan, &data); 573 iio_device_release_direct_mode(indio_dev); 574 if (ret) 575 return ret; 576 *val = data; 577 return IIO_VAL_INT; 578 case IIO_CHAN_INFO_SCALE: 579 return inv_icm42600_gyro_read_scale(st, val, val2); 580 case IIO_CHAN_INFO_SAMP_FREQ: 581 return inv_icm42600_gyro_read_odr(st, val, val2); 582 case IIO_CHAN_INFO_CALIBBIAS: 583 return inv_icm42600_gyro_read_offset(st, chan, val, val2); 584 default: 585 return -EINVAL; 586 } 587 } 588 589 static int inv_icm42600_gyro_read_avail(struct iio_dev *indio_dev, 590 struct iio_chan_spec const *chan, 591 const int **vals, 592 int *type, int *length, long mask) 593 { 594 if (chan->type != IIO_ANGL_VEL) 595 return -EINVAL; 596 597 switch (mask) { 598 case IIO_CHAN_INFO_SCALE: 599 *vals = inv_icm42600_gyro_scale; 600 *type = IIO_VAL_INT_PLUS_NANO; 601 *length = ARRAY_SIZE(inv_icm42600_gyro_scale); 602 return IIO_AVAIL_LIST; 603 case IIO_CHAN_INFO_SAMP_FREQ: 604 *vals = inv_icm42600_gyro_odr; 605 *type = IIO_VAL_INT_PLUS_MICRO; 606 *length = ARRAY_SIZE(inv_icm42600_gyro_odr); 607 return IIO_AVAIL_LIST; 608 case IIO_CHAN_INFO_CALIBBIAS: 609 *vals = inv_icm42600_gyro_calibbias; 610 *type = IIO_VAL_INT_PLUS_NANO; 611 return IIO_AVAIL_RANGE; 612 default: 613 return -EINVAL; 614 } 615 } 616 617 static int inv_icm42600_gyro_write_raw(struct iio_dev *indio_dev, 618 struct iio_chan_spec const *chan, 619 int val, int val2, long mask) 620 { 621 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 622 int ret; 623 624 if (chan->type != IIO_ANGL_VEL) 625 return -EINVAL; 626 627 switch (mask) { 628 case IIO_CHAN_INFO_SCALE: 629 ret = iio_device_claim_direct_mode(indio_dev); 630 if (ret) 631 return ret; 632 ret = inv_icm42600_gyro_write_scale(st, val, val2); 633 iio_device_release_direct_mode(indio_dev); 634 return ret; 635 case IIO_CHAN_INFO_SAMP_FREQ: 636 return inv_icm42600_gyro_write_odr(indio_dev, val, val2); 637 case IIO_CHAN_INFO_CALIBBIAS: 638 ret = iio_device_claim_direct_mode(indio_dev); 639 if (ret) 640 return ret; 641 ret = inv_icm42600_gyro_write_offset(st, chan, val, val2); 642 iio_device_release_direct_mode(indio_dev); 643 return ret; 644 default: 645 return -EINVAL; 646 } 647 } 648 649 static int inv_icm42600_gyro_write_raw_get_fmt(struct iio_dev *indio_dev, 650 struct iio_chan_spec const *chan, 651 long mask) 652 { 653 if (chan->type != IIO_ANGL_VEL) 654 return -EINVAL; 655 656 switch (mask) { 657 case IIO_CHAN_INFO_SCALE: 658 return IIO_VAL_INT_PLUS_NANO; 659 case IIO_CHAN_INFO_SAMP_FREQ: 660 return IIO_VAL_INT_PLUS_MICRO; 661 case IIO_CHAN_INFO_CALIBBIAS: 662 return IIO_VAL_INT_PLUS_NANO; 663 default: 664 return -EINVAL; 665 } 666 } 667 668 static int inv_icm42600_gyro_hwfifo_set_watermark(struct iio_dev *indio_dev, 669 unsigned int val) 670 { 671 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 672 int ret; 673 674 mutex_lock(&st->lock); 675 676 st->fifo.watermark.gyro = val; 677 ret = inv_icm42600_buffer_update_watermark(st); 678 679 mutex_unlock(&st->lock); 680 681 return ret; 682 } 683 684 static int inv_icm42600_gyro_hwfifo_flush(struct iio_dev *indio_dev, 685 unsigned int count) 686 { 687 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 688 int ret; 689 690 if (count == 0) 691 return 0; 692 693 mutex_lock(&st->lock); 694 695 ret = inv_icm42600_buffer_hwfifo_flush(st, count); 696 if (!ret) 697 ret = st->fifo.nb.gyro; 698 699 mutex_unlock(&st->lock); 700 701 return ret; 702 } 703 704 static const struct iio_info inv_icm42600_gyro_info = { 705 .read_raw = inv_icm42600_gyro_read_raw, 706 .read_avail = inv_icm42600_gyro_read_avail, 707 .write_raw = inv_icm42600_gyro_write_raw, 708 .write_raw_get_fmt = inv_icm42600_gyro_write_raw_get_fmt, 709 .debugfs_reg_access = inv_icm42600_debugfs_reg, 710 .update_scan_mode = inv_icm42600_gyro_update_scan_mode, 711 .hwfifo_set_watermark = inv_icm42600_gyro_hwfifo_set_watermark, 712 .hwfifo_flush_to_buffer = inv_icm42600_gyro_hwfifo_flush, 713 }; 714 715 struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st) 716 { 717 struct device *dev = regmap_get_device(st->map); 718 const char *name; 719 struct inv_sensors_timestamp_chip ts_chip; 720 struct inv_sensors_timestamp *ts; 721 struct iio_dev *indio_dev; 722 int ret; 723 724 name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name); 725 if (!name) 726 return ERR_PTR(-ENOMEM); 727 728 indio_dev = devm_iio_device_alloc(dev, sizeof(*ts)); 729 if (!indio_dev) 730 return ERR_PTR(-ENOMEM); 731 732 /* 733 * clock period is 32kHz (31250ns) 734 * jitter is +/- 2% (20 per mille) 735 */ 736 ts_chip.clock_period = 31250; 737 ts_chip.jitter = 20; 738 ts_chip.init_period = inv_icm42600_odr_to_period(st->conf.accel.odr); 739 ts = iio_priv(indio_dev); 740 inv_sensors_timestamp_init(ts, &ts_chip); 741 742 iio_device_set_drvdata(indio_dev, st); 743 indio_dev->name = name; 744 indio_dev->info = &inv_icm42600_gyro_info; 745 indio_dev->modes = INDIO_DIRECT_MODE; 746 indio_dev->channels = inv_icm42600_gyro_channels; 747 indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_gyro_channels); 748 indio_dev->available_scan_masks = inv_icm42600_gyro_scan_masks; 749 indio_dev->setup_ops = &inv_icm42600_buffer_ops; 750 751 ret = devm_iio_kfifo_buffer_setup(dev, indio_dev, 752 &inv_icm42600_buffer_ops); 753 if (ret) 754 return ERR_PTR(ret); 755 756 ret = devm_iio_device_register(dev, indio_dev); 757 if (ret) 758 return ERR_PTR(ret); 759 760 return indio_dev; 761 } 762 763 int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev) 764 { 765 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 766 struct inv_sensors_timestamp *ts = iio_priv(indio_dev); 767 ssize_t i, size; 768 unsigned int no; 769 const void *accel, *gyro, *timestamp; 770 const int8_t *temp; 771 unsigned int odr; 772 int64_t ts_val; 773 struct inv_icm42600_gyro_buffer buffer; 774 775 /* parse all fifo packets */ 776 for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) { 777 size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i], 778 &accel, &gyro, &temp, ×tamp, &odr); 779 /* quit if error or FIFO is empty */ 780 if (size <= 0) 781 return size; 782 783 /* skip packet if no gyro data or data is invalid */ 784 if (gyro == NULL || !inv_icm42600_fifo_is_data_valid(gyro)) 785 continue; 786 787 /* update odr */ 788 if (odr & INV_ICM42600_SENSOR_GYRO) 789 inv_sensors_timestamp_apply_odr(ts, st->fifo.period, 790 st->fifo.nb.total, no); 791 792 /* buffer is copied to userspace, zeroing it to avoid any data leak */ 793 memset(&buffer, 0, sizeof(buffer)); 794 memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro)); 795 /* convert 8 bits FIFO temperature in high resolution format */ 796 buffer.temp = temp ? (*temp * 64) : 0; 797 ts_val = inv_sensors_timestamp_pop(ts); 798 iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val); 799 } 800 801 return 0; 802 } 803