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/minmax.h> 9 #include <linux/mutex.h> 10 #include <linux/pm_runtime.h> 11 #include <linux/regmap.h> 12 #include <linux/delay.h> 13 14 #include <linux/iio/buffer.h> 15 #include <linux/iio/common/inv_sensors_timestamp.h> 16 #include <linux/iio/iio.h> 17 18 #include "inv_icm42600.h" 19 #include "inv_icm42600_buffer.h" 20 21 /* FIFO header: 1 byte */ 22 #define INV_ICM42600_FIFO_HEADER_MSG BIT(7) 23 #define INV_ICM42600_FIFO_HEADER_ACCEL BIT(6) 24 #define INV_ICM42600_FIFO_HEADER_GYRO BIT(5) 25 #define INV_ICM42600_FIFO_HEADER_TMST_FSYNC GENMASK(3, 2) 26 #define INV_ICM42600_FIFO_HEADER_ODR_ACCEL BIT(1) 27 #define INV_ICM42600_FIFO_HEADER_ODR_GYRO BIT(0) 28 29 struct inv_icm42600_fifo_1sensor_packet { 30 u8 header; 31 struct inv_icm42600_fifo_sensor_data data; 32 s8 temp; 33 } __packed; 34 #define INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE 8 35 36 struct inv_icm42600_fifo_2sensors_packet { 37 u8 header; 38 struct inv_icm42600_fifo_sensor_data accel; 39 struct inv_icm42600_fifo_sensor_data gyro; 40 s8 temp; 41 __be16 timestamp; 42 } __packed; 43 #define INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE 16 44 45 ssize_t inv_icm42600_fifo_decode_packet(const void *packet, const void **accel, 46 const void **gyro, const s8 **temp, 47 const void **timestamp, unsigned int *odr) 48 { 49 const struct inv_icm42600_fifo_1sensor_packet *pack1 = packet; 50 const struct inv_icm42600_fifo_2sensors_packet *pack2 = packet; 51 u8 header = *((const u8 *)packet); 52 53 /* FIFO empty */ 54 if (header & INV_ICM42600_FIFO_HEADER_MSG) { 55 *accel = NULL; 56 *gyro = NULL; 57 *temp = NULL; 58 *timestamp = NULL; 59 *odr = 0; 60 return 0; 61 } 62 63 /* handle odr flags */ 64 *odr = 0; 65 if (header & INV_ICM42600_FIFO_HEADER_ODR_GYRO) 66 *odr |= INV_ICM42600_SENSOR_GYRO; 67 if (header & INV_ICM42600_FIFO_HEADER_ODR_ACCEL) 68 *odr |= INV_ICM42600_SENSOR_ACCEL; 69 70 /* accel + gyro */ 71 if ((header & INV_ICM42600_FIFO_HEADER_ACCEL) && 72 (header & INV_ICM42600_FIFO_HEADER_GYRO)) { 73 *accel = &pack2->accel; 74 *gyro = &pack2->gyro; 75 *temp = &pack2->temp; 76 *timestamp = &pack2->timestamp; 77 return INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE; 78 } 79 80 /* accel only */ 81 if (header & INV_ICM42600_FIFO_HEADER_ACCEL) { 82 *accel = &pack1->data; 83 *gyro = NULL; 84 *temp = &pack1->temp; 85 *timestamp = NULL; 86 return INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE; 87 } 88 89 /* gyro only */ 90 if (header & INV_ICM42600_FIFO_HEADER_GYRO) { 91 *accel = NULL; 92 *gyro = &pack1->data; 93 *temp = &pack1->temp; 94 *timestamp = NULL; 95 return INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE; 96 } 97 98 /* invalid packet if here */ 99 return -EINVAL; 100 } 101 102 void inv_icm42600_buffer_update_fifo_period(struct inv_icm42600_state *st) 103 { 104 u32 period_gyro, period_accel; 105 106 if (st->fifo.en & INV_ICM42600_SENSOR_GYRO) 107 period_gyro = inv_icm42600_odr_to_period(st->conf.gyro.odr); 108 else 109 period_gyro = U32_MAX; 110 111 if (st->fifo.en & INV_ICM42600_SENSOR_ACCEL) 112 period_accel = inv_icm42600_odr_to_period(st->conf.accel.odr); 113 else 114 period_accel = U32_MAX; 115 116 st->fifo.period = min(period_gyro, period_accel); 117 } 118 119 int inv_icm42600_buffer_set_fifo_en(struct inv_icm42600_state *st, 120 unsigned int fifo_en) 121 { 122 unsigned int mask, val; 123 int ret; 124 125 /* update only FIFO EN bits */ 126 mask = INV_ICM42600_FIFO_CONFIG1_TMST_FSYNC_EN | 127 INV_ICM42600_FIFO_CONFIG1_TEMP_EN | 128 INV_ICM42600_FIFO_CONFIG1_GYRO_EN | 129 INV_ICM42600_FIFO_CONFIG1_ACCEL_EN; 130 131 val = 0; 132 if (fifo_en & INV_ICM42600_SENSOR_GYRO) 133 val |= INV_ICM42600_FIFO_CONFIG1_GYRO_EN; 134 if (fifo_en & INV_ICM42600_SENSOR_ACCEL) 135 val |= INV_ICM42600_FIFO_CONFIG1_ACCEL_EN; 136 if (fifo_en & INV_ICM42600_SENSOR_TEMP) 137 val |= INV_ICM42600_FIFO_CONFIG1_TEMP_EN; 138 139 ret = regmap_update_bits(st->map, INV_ICM42600_REG_FIFO_CONFIG1, mask, val); 140 if (ret) 141 return ret; 142 143 st->fifo.en = fifo_en; 144 inv_icm42600_buffer_update_fifo_period(st); 145 146 return 0; 147 } 148 149 static size_t inv_icm42600_get_packet_size(unsigned int fifo_en) 150 { 151 size_t packet_size; 152 153 if ((fifo_en & INV_ICM42600_SENSOR_GYRO) && 154 (fifo_en & INV_ICM42600_SENSOR_ACCEL)) 155 packet_size = INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE; 156 else 157 packet_size = INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE; 158 159 return packet_size; 160 } 161 162 static unsigned int inv_icm42600_wm_truncate(unsigned int watermark, 163 size_t packet_size) 164 { 165 size_t wm_size; 166 unsigned int wm; 167 168 wm_size = watermark * packet_size; 169 if (wm_size > INV_ICM42600_FIFO_WATERMARK_MAX) 170 wm_size = INV_ICM42600_FIFO_WATERMARK_MAX; 171 172 wm = wm_size / packet_size; 173 174 return wm; 175 } 176 177 /** 178 * inv_icm42600_buffer_update_watermark - update watermark FIFO threshold 179 * @st: driver internal state 180 * 181 * Returns 0 on success, a negative error code otherwise. 182 * 183 * FIFO watermark threshold is computed based on the required watermark values 184 * set for gyro and accel sensors. Since watermark is all about acceptable data 185 * latency, use the smallest setting between the 2. It means choosing the 186 * smallest latency but this is not as simple as choosing the smallest watermark 187 * value. Latency depends on watermark and ODR. It requires several steps: 188 * 1) compute gyro and accel latencies and choose the smallest value. 189 * 2) adapt the choosen latency so that it is a multiple of both gyro and accel 190 * ones. Otherwise it is possible that you don't meet a requirement. (for 191 * example with gyro @100Hz wm 4 and accel @100Hz with wm 6, choosing the 192 * value of 4 will not meet accel latency requirement because 6 is not a 193 * multiple of 4. You need to use the value 2.) 194 * 3) Since all periods are multiple of each others, watermark is computed by 195 * dividing this computed latency by the smallest period, which corresponds 196 * to the FIFO frequency. Beware that this is only true because we are not 197 * using 500Hz frequency which is not a multiple of the others. 198 */ 199 int inv_icm42600_buffer_update_watermark(struct inv_icm42600_state *st) 200 { 201 size_t packet_size, wm_size; 202 unsigned int wm_gyro, wm_accel, watermark; 203 u32 period_gyro, period_accel; 204 u32 latency_gyro, latency_accel, latency; 205 bool restore; 206 __le16 raw_wm; 207 int ret; 208 209 packet_size = inv_icm42600_get_packet_size(st->fifo.en); 210 211 /* compute sensors latency, depending on sensor watermark and odr */ 212 wm_gyro = inv_icm42600_wm_truncate(st->fifo.watermark.gyro, packet_size); 213 wm_accel = inv_icm42600_wm_truncate(st->fifo.watermark.accel, packet_size); 214 /* use us for odr to avoid overflow using 32 bits values */ 215 period_gyro = inv_icm42600_odr_to_period(st->conf.gyro.odr) / 1000UL; 216 period_accel = inv_icm42600_odr_to_period(st->conf.accel.odr) / 1000UL; 217 latency_gyro = period_gyro * wm_gyro; 218 latency_accel = period_accel * wm_accel; 219 220 /* 0 value for watermark means that the sensor is turned off */ 221 if (wm_gyro == 0 && wm_accel == 0) 222 return 0; 223 224 if (latency_gyro == 0) { 225 watermark = wm_accel; 226 st->fifo.watermark.eff_accel = wm_accel; 227 } else if (latency_accel == 0) { 228 watermark = wm_gyro; 229 st->fifo.watermark.eff_gyro = wm_gyro; 230 } else { 231 /* compute the smallest latency that is a multiple of both */ 232 if (latency_gyro <= latency_accel) 233 latency = latency_gyro - (latency_accel % latency_gyro); 234 else 235 latency = latency_accel - (latency_gyro % latency_accel); 236 /* all this works because periods are multiple of each others */ 237 watermark = latency / min(period_gyro, period_accel); 238 if (watermark < 1) 239 watermark = 1; 240 /* update effective watermark */ 241 st->fifo.watermark.eff_gyro = latency / period_gyro; 242 if (st->fifo.watermark.eff_gyro < 1) 243 st->fifo.watermark.eff_gyro = 1; 244 st->fifo.watermark.eff_accel = latency / period_accel; 245 if (st->fifo.watermark.eff_accel < 1) 246 st->fifo.watermark.eff_accel = 1; 247 } 248 249 /* compute watermark value in bytes */ 250 wm_size = watermark * packet_size; 251 252 /* changing FIFO watermark requires to turn off watermark interrupt */ 253 ret = regmap_update_bits_check(st->map, INV_ICM42600_REG_INT_SOURCE0, 254 INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN, 255 0, &restore); 256 if (ret) 257 return ret; 258 259 raw_wm = INV_ICM42600_FIFO_WATERMARK_VAL(wm_size); 260 memcpy(st->buffer, &raw_wm, sizeof(raw_wm)); 261 ret = regmap_bulk_write(st->map, INV_ICM42600_REG_FIFO_WATERMARK, 262 st->buffer, sizeof(raw_wm)); 263 if (ret) 264 return ret; 265 266 /* restore watermark interrupt */ 267 if (restore) { 268 ret = regmap_set_bits(st->map, INV_ICM42600_REG_INT_SOURCE0, 269 INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN); 270 if (ret) 271 return ret; 272 } 273 274 return 0; 275 } 276 277 static int inv_icm42600_buffer_preenable(struct iio_dev *indio_dev) 278 { 279 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 280 struct device *dev = regmap_get_device(st->map); 281 struct inv_icm42600_sensor_state *sensor_st = iio_priv(indio_dev); 282 struct inv_sensors_timestamp *ts = &sensor_st->ts; 283 284 pm_runtime_get_sync(dev); 285 286 guard(mutex)(&st->lock); 287 inv_sensors_timestamp_reset(ts); 288 289 return 0; 290 } 291 292 /* 293 * update_scan_mode callback is turning sensors on and setting data FIFO enable 294 * bits. 295 */ 296 static int inv_icm42600_buffer_postenable(struct iio_dev *indio_dev) 297 { 298 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 299 int ret; 300 301 guard(mutex)(&st->lock); 302 303 if (st->fifo.on) { 304 st->fifo.on++; 305 return 0; 306 } 307 308 /* set FIFO threshold interrupt */ 309 ret = regmap_set_bits(st->map, INV_ICM42600_REG_INT_SOURCE0, 310 INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN); 311 if (ret) 312 return ret; 313 314 /* flush FIFO data */ 315 ret = regmap_write(st->map, INV_ICM42600_REG_SIGNAL_PATH_RESET, 316 INV_ICM42600_SIGNAL_PATH_RESET_FIFO_FLUSH); 317 if (ret) 318 return ret; 319 320 /* set FIFO in streaming mode */ 321 ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG, 322 INV_ICM42600_FIFO_CONFIG_STREAM); 323 if (ret) 324 return ret; 325 326 /* workaround: first read of FIFO count after reset is always 0 */ 327 ret = regmap_bulk_read(st->map, INV_ICM42600_REG_FIFO_COUNT, st->buffer, 2); 328 if (ret) 329 return ret; 330 331 st->fifo.on++; 332 333 return 0; 334 } 335 336 static int inv_icm42600_buffer_predisable(struct iio_dev *indio_dev) 337 { 338 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 339 int ret; 340 341 guard(mutex)(&st->lock); 342 343 if (st->fifo.on > 1) { 344 st->fifo.on--; 345 return 0; 346 } 347 348 /* set FIFO in bypass mode */ 349 ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG, 350 INV_ICM42600_FIFO_CONFIG_BYPASS); 351 if (ret) 352 return ret; 353 354 /* flush FIFO data */ 355 ret = regmap_write(st->map, INV_ICM42600_REG_SIGNAL_PATH_RESET, 356 INV_ICM42600_SIGNAL_PATH_RESET_FIFO_FLUSH); 357 if (ret) 358 return ret; 359 360 /* disable FIFO threshold interrupt */ 361 ret = regmap_clear_bits(st->map, INV_ICM42600_REG_INT_SOURCE0, 362 INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN); 363 if (ret) 364 return ret; 365 366 st->fifo.on--; 367 368 return 0; 369 } 370 371 static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev) 372 { 373 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); 374 struct inv_icm42600_sensor_state *sensor_st = iio_priv(indio_dev); 375 struct inv_sensors_timestamp *ts = &sensor_st->ts; 376 struct device *dev = regmap_get_device(st->map); 377 unsigned int sensor; 378 unsigned int *watermark; 379 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; 380 unsigned int sleep_temp = 0; 381 unsigned int sleep_sensor = 0; 382 unsigned int sleep; 383 int ret; 384 385 if (indio_dev == st->indio_gyro) { 386 sensor = INV_ICM42600_SENSOR_GYRO; 387 watermark = &st->fifo.watermark.gyro; 388 } else if (indio_dev == st->indio_accel) { 389 sensor = INV_ICM42600_SENSOR_ACCEL; 390 watermark = &st->fifo.watermark.accel; 391 } else { 392 return -EINVAL; 393 } 394 395 mutex_lock(&st->lock); 396 397 inv_sensors_timestamp_apply_odr(ts, 0, 0, 0); 398 399 ret = inv_icm42600_buffer_set_fifo_en(st, st->fifo.en & ~sensor); 400 if (ret) 401 goto out_unlock; 402 403 *watermark = 0; 404 ret = inv_icm42600_buffer_update_watermark(st); 405 if (ret) 406 goto out_unlock; 407 408 conf.mode = INV_ICM42600_SENSOR_MODE_OFF; 409 if (sensor == INV_ICM42600_SENSOR_GYRO) 410 ret = inv_icm42600_set_gyro_conf(st, &conf, &sleep_sensor); 411 else if (!st->apex.on) 412 ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_sensor); 413 if (ret) 414 goto out_unlock; 415 416 /* if FIFO is off, turn temperature off */ 417 if (!st->fifo.on) 418 ret = inv_icm42600_set_temp_conf(st, false, &sleep_temp); 419 420 out_unlock: 421 mutex_unlock(&st->lock); 422 423 /* sleep maximum required time */ 424 sleep = max(sleep_sensor, sleep_temp); 425 if (sleep) 426 msleep(sleep); 427 428 pm_runtime_put_autosuspend(dev); 429 430 return ret; 431 } 432 433 const struct iio_buffer_setup_ops inv_icm42600_buffer_ops = { 434 .preenable = inv_icm42600_buffer_preenable, 435 .postenable = inv_icm42600_buffer_postenable, 436 .predisable = inv_icm42600_buffer_predisable, 437 .postdisable = inv_icm42600_buffer_postdisable, 438 }; 439 440 int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st, 441 unsigned int max) 442 { 443 size_t max_count; 444 __be16 *raw_fifo_count; 445 ssize_t i, size; 446 const void *accel, *gyro, *timestamp; 447 const s8 *temp; 448 unsigned int odr; 449 int ret; 450 451 /* reset all samples counters */ 452 st->fifo.count = 0; 453 st->fifo.nb.gyro = 0; 454 st->fifo.nb.accel = 0; 455 st->fifo.nb.total = 0; 456 457 /* compute maximum FIFO read size */ 458 if (max == 0) 459 max_count = sizeof(st->fifo.data); 460 else 461 max_count = max * inv_icm42600_get_packet_size(st->fifo.en); 462 463 /* read FIFO count value */ 464 raw_fifo_count = (__be16 *)st->buffer; 465 ret = regmap_bulk_read(st->map, INV_ICM42600_REG_FIFO_COUNT, 466 raw_fifo_count, sizeof(*raw_fifo_count)); 467 if (ret) 468 return ret; 469 st->fifo.count = be16_to_cpup(raw_fifo_count); 470 471 /* check and clamp FIFO count value */ 472 if (st->fifo.count == 0) 473 return 0; 474 if (st->fifo.count > max_count) 475 st->fifo.count = max_count; 476 477 /* read all FIFO data in internal buffer */ 478 ret = regmap_noinc_read(st->map, INV_ICM42600_REG_FIFO_DATA, 479 st->fifo.data, st->fifo.count); 480 if (ret) 481 return ret; 482 483 /* compute number of samples for each sensor */ 484 for (i = 0; i < st->fifo.count; i += size) { 485 size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i], 486 &accel, &gyro, &temp, ×tamp, &odr); 487 if (size <= 0) 488 break; 489 if (gyro != NULL && inv_icm42600_fifo_is_data_valid(gyro)) 490 st->fifo.nb.gyro++; 491 if (accel != NULL && inv_icm42600_fifo_is_data_valid(accel)) 492 st->fifo.nb.accel++; 493 st->fifo.nb.total++; 494 } 495 496 return 0; 497 } 498 499 int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state *st) 500 { 501 struct inv_icm42600_sensor_state *gyro_st = iio_priv(st->indio_gyro); 502 struct inv_icm42600_sensor_state *accel_st = iio_priv(st->indio_accel); 503 struct inv_sensors_timestamp *ts; 504 int ret; 505 506 if (st->fifo.nb.total == 0) 507 return 0; 508 509 /* handle gyroscope timestamp and FIFO data parsing */ 510 if (st->fifo.nb.gyro > 0) { 511 ts = &gyro_st->ts; 512 inv_sensors_timestamp_interrupt(ts, st->fifo.watermark.eff_gyro, 513 st->timestamp.gyro); 514 ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro); 515 if (ret) 516 return ret; 517 } 518 519 /* handle accelerometer timestamp and FIFO data parsing */ 520 if (st->fifo.nb.accel > 0) { 521 ts = &accel_st->ts; 522 inv_sensors_timestamp_interrupt(ts, st->fifo.watermark.eff_accel, 523 st->timestamp.accel); 524 ret = inv_icm42600_accel_parse_fifo(st->indio_accel); 525 if (ret) 526 return ret; 527 } 528 529 return 0; 530 } 531 532 int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st, 533 unsigned int count) 534 { 535 struct inv_icm42600_sensor_state *gyro_st = iio_priv(st->indio_gyro); 536 struct inv_icm42600_sensor_state *accel_st = iio_priv(st->indio_accel); 537 struct inv_sensors_timestamp *ts; 538 s64 gyro_ts, accel_ts; 539 int ret; 540 541 gyro_ts = iio_get_time_ns(st->indio_gyro); 542 accel_ts = iio_get_time_ns(st->indio_accel); 543 544 ret = inv_icm42600_buffer_fifo_read(st, count); 545 if (ret) 546 return ret; 547 548 if (st->fifo.nb.total == 0) 549 return 0; 550 551 if (st->fifo.nb.gyro > 0) { 552 ts = &gyro_st->ts; 553 inv_sensors_timestamp_interrupt(ts, st->fifo.nb.gyro, gyro_ts); 554 ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro); 555 if (ret) 556 return ret; 557 } 558 559 if (st->fifo.nb.accel > 0) { 560 ts = &accel_st->ts; 561 inv_sensors_timestamp_interrupt(ts, st->fifo.nb.accel, accel_ts); 562 ret = inv_icm42600_accel_parse_fifo(st->indio_accel); 563 if (ret) 564 return ret; 565 } 566 567 return 0; 568 } 569 570 int inv_icm42600_buffer_init(struct inv_icm42600_state *st) 571 { 572 unsigned int val; 573 int ret; 574 575 st->fifo.watermark.eff_gyro = 1; 576 st->fifo.watermark.eff_accel = 1; 577 578 /* 579 * Default FIFO configuration (bits 7 to 5) 580 * - use invalid value 581 * - FIFO count in bytes 582 * - FIFO count in big endian 583 */ 584 val = INV_ICM42600_INTF_CONFIG0_FIFO_COUNT_ENDIAN; 585 ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0, 586 GENMASK(7, 5), val); 587 if (ret) 588 return ret; 589 590 /* 591 * Enable FIFO partial read and continuous watermark interrupt. 592 * Disable all FIFO EN bits. 593 */ 594 val = INV_ICM42600_FIFO_CONFIG1_RESUME_PARTIAL_RD | 595 INV_ICM42600_FIFO_CONFIG1_WM_GT_TH; 596 return regmap_update_bits(st->map, INV_ICM42600_REG_FIFO_CONFIG1, 597 GENMASK(6, 5) | GENMASK(3, 0), val); 598 } 599