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 device *dev = regmap_get_device(st->map); 375 unsigned int sensor; 376 unsigned int *watermark; 377 struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; 378 unsigned int sleep_temp = 0; 379 unsigned int sleep_sensor = 0; 380 unsigned int sleep; 381 int ret; 382 383 if (indio_dev == st->indio_gyro) { 384 sensor = INV_ICM42600_SENSOR_GYRO; 385 watermark = &st->fifo.watermark.gyro; 386 } else if (indio_dev == st->indio_accel) { 387 sensor = INV_ICM42600_SENSOR_ACCEL; 388 watermark = &st->fifo.watermark.accel; 389 } else { 390 return -EINVAL; 391 } 392 393 mutex_lock(&st->lock); 394 395 ret = inv_icm42600_buffer_set_fifo_en(st, st->fifo.en & ~sensor); 396 if (ret) 397 goto out_unlock; 398 399 *watermark = 0; 400 ret = inv_icm42600_buffer_update_watermark(st); 401 if (ret) 402 goto out_unlock; 403 404 conf.mode = INV_ICM42600_SENSOR_MODE_OFF; 405 if (sensor == INV_ICM42600_SENSOR_GYRO) 406 ret = inv_icm42600_set_gyro_conf(st, &conf, &sleep_sensor); 407 else if (!st->apex.on) 408 ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_sensor); 409 if (ret) 410 goto out_unlock; 411 412 /* if FIFO is off, turn temperature off */ 413 if (!st->fifo.on) 414 ret = inv_icm42600_set_temp_conf(st, false, &sleep_temp); 415 416 out_unlock: 417 mutex_unlock(&st->lock); 418 419 /* sleep maximum required time */ 420 sleep = max(sleep_sensor, sleep_temp); 421 if (sleep) 422 msleep(sleep); 423 424 pm_runtime_put_autosuspend(dev); 425 426 return ret; 427 } 428 429 const struct iio_buffer_setup_ops inv_icm42600_buffer_ops = { 430 .preenable = inv_icm42600_buffer_preenable, 431 .postenable = inv_icm42600_buffer_postenable, 432 .predisable = inv_icm42600_buffer_predisable, 433 .postdisable = inv_icm42600_buffer_postdisable, 434 }; 435 436 int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st, 437 unsigned int max) 438 { 439 size_t max_count; 440 __be16 *raw_fifo_count; 441 ssize_t i, size; 442 const void *accel, *gyro, *timestamp; 443 const s8 *temp; 444 unsigned int odr; 445 int ret; 446 447 /* reset all samples counters */ 448 st->fifo.count = 0; 449 st->fifo.nb.gyro = 0; 450 st->fifo.nb.accel = 0; 451 st->fifo.nb.total = 0; 452 453 /* compute maximum FIFO read size */ 454 if (max == 0) 455 max_count = sizeof(st->fifo.data); 456 else 457 max_count = max * inv_icm42600_get_packet_size(st->fifo.en); 458 459 /* read FIFO count value */ 460 raw_fifo_count = (__be16 *)st->buffer; 461 ret = regmap_bulk_read(st->map, INV_ICM42600_REG_FIFO_COUNT, 462 raw_fifo_count, sizeof(*raw_fifo_count)); 463 if (ret) 464 return ret; 465 st->fifo.count = be16_to_cpup(raw_fifo_count); 466 467 /* check and clamp FIFO count value */ 468 if (st->fifo.count == 0) 469 return 0; 470 if (st->fifo.count > max_count) 471 st->fifo.count = max_count; 472 473 /* read all FIFO data in internal buffer */ 474 ret = regmap_noinc_read(st->map, INV_ICM42600_REG_FIFO_DATA, 475 st->fifo.data, st->fifo.count); 476 if (ret) 477 return ret; 478 479 /* compute number of samples for each sensor */ 480 for (i = 0; i < st->fifo.count; i += size) { 481 size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i], 482 &accel, &gyro, &temp, ×tamp, &odr); 483 if (size <= 0) 484 break; 485 if (gyro != NULL && inv_icm42600_fifo_is_data_valid(gyro)) 486 st->fifo.nb.gyro++; 487 if (accel != NULL && inv_icm42600_fifo_is_data_valid(accel)) 488 st->fifo.nb.accel++; 489 st->fifo.nb.total++; 490 } 491 492 return 0; 493 } 494 495 int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state *st) 496 { 497 struct inv_icm42600_sensor_state *gyro_st = iio_priv(st->indio_gyro); 498 struct inv_icm42600_sensor_state *accel_st = iio_priv(st->indio_accel); 499 struct inv_sensors_timestamp *ts; 500 int ret; 501 502 if (st->fifo.nb.total == 0) 503 return 0; 504 505 /* handle gyroscope timestamp and FIFO data parsing */ 506 if (st->fifo.nb.gyro > 0) { 507 ts = &gyro_st->ts; 508 inv_sensors_timestamp_interrupt(ts, st->fifo.watermark.eff_gyro, 509 st->timestamp.gyro); 510 ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro); 511 if (ret) 512 return ret; 513 } 514 515 /* handle accelerometer timestamp and FIFO data parsing */ 516 if (st->fifo.nb.accel > 0) { 517 ts = &accel_st->ts; 518 inv_sensors_timestamp_interrupt(ts, st->fifo.watermark.eff_accel, 519 st->timestamp.accel); 520 ret = inv_icm42600_accel_parse_fifo(st->indio_accel); 521 if (ret) 522 return ret; 523 } 524 525 return 0; 526 } 527 528 int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st, 529 unsigned int count) 530 { 531 struct inv_icm42600_sensor_state *gyro_st = iio_priv(st->indio_gyro); 532 struct inv_icm42600_sensor_state *accel_st = iio_priv(st->indio_accel); 533 struct inv_sensors_timestamp *ts; 534 s64 gyro_ts, accel_ts; 535 int ret; 536 537 gyro_ts = iio_get_time_ns(st->indio_gyro); 538 accel_ts = iio_get_time_ns(st->indio_accel); 539 540 ret = inv_icm42600_buffer_fifo_read(st, count); 541 if (ret) 542 return ret; 543 544 if (st->fifo.nb.total == 0) 545 return 0; 546 547 if (st->fifo.nb.gyro > 0) { 548 ts = &gyro_st->ts; 549 inv_sensors_timestamp_interrupt(ts, st->fifo.nb.gyro, gyro_ts); 550 ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro); 551 if (ret) 552 return ret; 553 } 554 555 if (st->fifo.nb.accel > 0) { 556 ts = &accel_st->ts; 557 inv_sensors_timestamp_interrupt(ts, st->fifo.nb.accel, accel_ts); 558 ret = inv_icm42600_accel_parse_fifo(st->indio_accel); 559 if (ret) 560 return ret; 561 } 562 563 return 0; 564 } 565 566 int inv_icm42600_buffer_init(struct inv_icm42600_state *st) 567 { 568 unsigned int val; 569 int ret; 570 571 st->fifo.watermark.eff_gyro = 1; 572 st->fifo.watermark.eff_accel = 1; 573 574 /* 575 * Default FIFO configuration (bits 7 to 5) 576 * - use invalid value 577 * - FIFO count in bytes 578 * - FIFO count in big endian 579 */ 580 val = INV_ICM42600_INTF_CONFIG0_FIFO_COUNT_ENDIAN; 581 ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0, 582 GENMASK(7, 5), val); 583 if (ret) 584 return ret; 585 586 /* 587 * Enable FIFO partial read and continuous watermark interrupt. 588 * Disable all FIFO EN bits. 589 */ 590 val = INV_ICM42600_FIFO_CONFIG1_RESUME_PARTIAL_RD | 591 INV_ICM42600_FIFO_CONFIG1_WM_GT_TH; 592 return regmap_update_bits(st->map, INV_ICM42600_REG_FIFO_CONFIG1, 593 GENMASK(6, 5) | GENMASK(3, 0), val); 594 } 595