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