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