1 // SPDX-License-Identifier: GPL-2.0-only 2 /* 3 * STMicroelectronics st_lsm6dsx FIFO buffer library driver 4 * 5 * Pattern FIFO: 6 * The FIFO buffer can be configured to store data from gyroscope and 7 * accelerometer. Samples are queued without any tag according to a 8 * specific pattern based on 'FIFO data sets' (6 bytes each): 9 * - 1st data set is reserved for gyroscope data 10 * - 2nd data set is reserved for accelerometer data 11 * The FIFO pattern changes depending on the ODRs and decimation factors 12 * assigned to the FIFO data sets. The first sequence of data stored in FIFO 13 * buffer contains the data of all the enabled FIFO data sets 14 * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated depending on the 15 * value of the decimation factor and ODR set for each FIFO data set. 16 * 17 * Supported devices: 18 * - ISM330DLC 19 * - LSM6DS3 20 * - LSM6DS3H 21 * - LSM6DS3TR-C 22 * - LSM6DSL 23 * - LSM6DSM 24 * 25 * Tagged FIFO: 26 * The FIFO buffer can be configured to store data from gyroscope and 27 * accelerometer. Each sample is queued with a tag (1B) indicating data 28 * source (gyroscope, accelerometer, hw timer). 29 * 30 * Supported devices: 31 * - ASM330LHB 32 * - ASM330LHH 33 * - ASM330LHHX 34 * - ASM330LHHXG1 35 * - ISM330DHCX 36 * - LSM6DSO 37 * - LSM6DSOP 38 * - LSM6DSOX 39 * - LSM6DSR 40 * - LSM6DSRX 41 * - LSM6DST 42 * - LSM6DSTX 43 * - LSM6DSV 44 * 45 * FIFO supported modes: 46 * - BYPASS: FIFO disabled 47 * - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index 48 * restarts from the beginning and the oldest sample is overwritten 49 * 50 * Copyright 2016 STMicroelectronics Inc. 51 * 52 * Lorenzo Bianconi <lorenzo.bianconi@st.com> 53 * Denis Ciocca <denis.ciocca@st.com> 54 */ 55 #include <linux/module.h> 56 #include <linux/iio/kfifo_buf.h> 57 #include <linux/iio/iio.h> 58 #include <linux/iio/buffer.h> 59 #include <linux/iio/sysfs.h> 60 #include <linux/regmap.h> 61 #include <linux/bitfield.h> 62 63 #include <linux/platform_data/st_sensors_pdata.h> 64 65 #include "st_lsm6dsx.h" 66 67 #define ST_LSM6DSX_REG_FIFO_MODE_ADDR 0x0a 68 #define ST_LSM6DSX_FIFO_MODE_MASK GENMASK(2, 0) 69 #define ST_LSM6DSX_FIFO_ODR_MASK GENMASK(6, 3) 70 #define ST_LSM6DSX_FIFO_EMPTY_MASK BIT(12) 71 #define ST_LSM6DSX_REG_FIFO_OUTL_ADDR 0x3e 72 #define ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR 0x78 73 #define ST_LSM6DSX_REG_TS_RESET_ADDR 0x42 74 75 #define ST_LSM6DSX_MAX_FIFO_ODR_VAL 0x08 76 77 #define ST_LSM6DSX_TS_RESET_VAL 0xaa 78 79 struct st_lsm6dsx_decimator_entry { 80 u8 decimator; 81 u8 val; 82 }; 83 84 enum st_lsm6dsx_fifo_tag { 85 ST_LSM6DSX_GYRO_TAG = 0x01, 86 ST_LSM6DSX_ACC_TAG = 0x02, 87 ST_LSM6DSX_TS_TAG = 0x04, 88 ST_LSM6DSX_EXT0_TAG = 0x0f, 89 ST_LSM6DSX_EXT1_TAG = 0x10, 90 ST_LSM6DSX_EXT2_TAG = 0x11, 91 ST_LSM6DSX_ROT_TAG = 0x13, 92 }; 93 94 static const 95 struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_table[] = { 96 { 0, 0x0 }, 97 { 1, 0x1 }, 98 { 2, 0x2 }, 99 { 3, 0x3 }, 100 { 4, 0x4 }, 101 { 8, 0x5 }, 102 { 16, 0x6 }, 103 { 32, 0x7 }, 104 }; 105 106 static int 107 st_lsm6dsx_get_decimator_val(struct st_lsm6dsx_sensor *sensor, u32 max_odr) 108 { 109 const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table); 110 u32 decimator = max_odr / sensor->hwfifo_odr_mHz; 111 int i; 112 113 if (decimator > 1) 114 decimator = round_down(decimator, 2); 115 116 for (i = 0; i < max_size; i++) { 117 if (st_lsm6dsx_decimator_table[i].decimator == decimator) 118 break; 119 } 120 121 sensor->decimator = decimator; 122 return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val; 123 } 124 125 static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw, 126 u32 *max_odr, u32 *min_odr) 127 { 128 struct st_lsm6dsx_sensor *sensor; 129 int i; 130 131 *max_odr = 0, *min_odr = ~0; 132 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { 133 if (!hw->iio_devs[i]) 134 continue; 135 136 sensor = iio_priv(hw->iio_devs[i]); 137 138 if (!(hw->enable_mask & BIT(sensor->id))) 139 continue; 140 141 *max_odr = max(*max_odr, sensor->hwfifo_odr_mHz); 142 *min_odr = min(*min_odr, sensor->hwfifo_odr_mHz); 143 } 144 } 145 146 static u8 st_lsm6dsx_get_sip(struct st_lsm6dsx_sensor *sensor, u32 min_odr) 147 { 148 u8 sip = sensor->hwfifo_odr_mHz / min_odr; 149 150 return sip > 1 ? round_down(sip, 2) : sip; 151 } 152 153 static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw) 154 { 155 const struct st_lsm6dsx_reg *ts_dec_reg; 156 struct st_lsm6dsx_sensor *sensor; 157 u16 sip = 0, ts_sip = 0; 158 u32 max_odr, min_odr; 159 int err = 0, i; 160 u8 data; 161 162 st_lsm6dsx_get_max_min_odr(hw, &max_odr, &min_odr); 163 164 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { 165 const struct st_lsm6dsx_reg *dec_reg; 166 167 if (!hw->iio_devs[i]) 168 continue; 169 170 sensor = iio_priv(hw->iio_devs[i]); 171 /* update fifo decimators and sample in pattern */ 172 if (hw->enable_mask & BIT(sensor->id)) { 173 sensor->sip = st_lsm6dsx_get_sip(sensor, min_odr); 174 data = st_lsm6dsx_get_decimator_val(sensor, max_odr); 175 } else { 176 sensor->sip = 0; 177 data = 0; 178 } 179 ts_sip = max_t(u16, ts_sip, sensor->sip); 180 181 dec_reg = &hw->settings->decimator[sensor->id]; 182 if (dec_reg->addr) { 183 int val = ST_LSM6DSX_SHIFT_VAL(data, dec_reg->mask); 184 185 err = st_lsm6dsx_update_bits_locked(hw, dec_reg->addr, 186 dec_reg->mask, 187 val); 188 if (err < 0) 189 return err; 190 } 191 sip += sensor->sip; 192 } 193 hw->sip = sip + ts_sip; 194 hw->ts_sip = ts_sip; 195 196 /* 197 * update hw ts decimator if necessary. Decimator for hw timestamp 198 * is always 1 or 0 in order to have a ts sample for each data 199 * sample in FIFO 200 */ 201 ts_dec_reg = &hw->settings->ts_settings.decimator; 202 if (ts_dec_reg->addr) { 203 int val, ts_dec = !!hw->ts_sip; 204 205 val = ST_LSM6DSX_SHIFT_VAL(ts_dec, ts_dec_reg->mask); 206 err = st_lsm6dsx_update_bits_locked(hw, ts_dec_reg->addr, 207 ts_dec_reg->mask, val); 208 } 209 return err; 210 } 211 212 static int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw, 213 enum st_lsm6dsx_fifo_mode fifo_mode) 214 { 215 unsigned int data; 216 217 data = FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK, fifo_mode); 218 return st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR, 219 ST_LSM6DSX_FIFO_MODE_MASK, data); 220 } 221 222 static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor, 223 bool enable) 224 { 225 struct st_lsm6dsx_hw *hw = sensor->hw; 226 const struct st_lsm6dsx_reg *batch_reg; 227 u8 data; 228 229 /* Only internal sensors have a FIFO ODR configuration register. */ 230 if (sensor->id >= ARRAY_SIZE(hw->settings->batch)) { 231 if (sensor->id == ST_LSM6DSX_ID_FUSION) 232 return st_lsm6dsx_fusion_set_odr(sensor, enable); 233 return 0; 234 } 235 236 batch_reg = &hw->settings->batch[sensor->id]; 237 if (batch_reg->addr) { 238 int val; 239 240 if (enable) { 241 int err; 242 243 err = st_lsm6dsx_check_odr(sensor, sensor->hwfifo_odr_mHz, 244 &data); 245 if (err < 0) 246 return err; 247 } else { 248 data = 0; 249 } 250 val = ST_LSM6DSX_SHIFT_VAL(data, batch_reg->mask); 251 return st_lsm6dsx_update_bits_locked(hw, batch_reg->addr, 252 batch_reg->mask, val); 253 } else { 254 data = hw->enable_mask ? ST_LSM6DSX_MAX_FIFO_ODR_VAL : 0; 255 return st_lsm6dsx_update_bits_locked(hw, 256 ST_LSM6DSX_REG_FIFO_MODE_ADDR, 257 ST_LSM6DSX_FIFO_ODR_MASK, 258 FIELD_PREP(ST_LSM6DSX_FIFO_ODR_MASK, 259 data)); 260 } 261 } 262 263 int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark) 264 { 265 u16 fifo_watermark = ~0, cur_watermark, fifo_th_mask; 266 struct st_lsm6dsx_hw *hw = sensor->hw; 267 struct st_lsm6dsx_sensor *cur_sensor; 268 int i, err, data; 269 __le16 wdata; 270 271 if (!hw->sip) 272 return 0; 273 274 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { 275 if (!hw->iio_devs[i]) 276 continue; 277 278 cur_sensor = iio_priv(hw->iio_devs[i]); 279 280 if (!(hw->enable_mask & BIT(cur_sensor->id))) 281 continue; 282 283 cur_watermark = (cur_sensor == sensor) ? watermark 284 : cur_sensor->watermark; 285 286 fifo_watermark = min_t(u16, fifo_watermark, cur_watermark); 287 } 288 289 fifo_watermark = max_t(u16, fifo_watermark, hw->sip); 290 fifo_watermark = (fifo_watermark / hw->sip) * hw->sip; 291 fifo_watermark = fifo_watermark * hw->settings->fifo_ops.th_wl; 292 293 mutex_lock(&hw->page_lock); 294 err = regmap_read(hw->regmap, hw->settings->fifo_ops.fifo_th.addr + 1, 295 &data); 296 if (err < 0) 297 goto out; 298 299 fifo_th_mask = hw->settings->fifo_ops.fifo_th.mask; 300 fifo_watermark = ((data << 8) & ~fifo_th_mask) | 301 (fifo_watermark & fifo_th_mask); 302 303 wdata = cpu_to_le16(fifo_watermark); 304 err = regmap_bulk_write(hw->regmap, 305 hw->settings->fifo_ops.fifo_th.addr, 306 &wdata, sizeof(wdata)); 307 out: 308 mutex_unlock(&hw->page_lock); 309 return err; 310 } 311 312 static int st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw *hw) 313 { 314 struct st_lsm6dsx_sensor *sensor; 315 int i, err; 316 317 /* reset hw ts counter */ 318 err = st_lsm6dsx_write_locked(hw, ST_LSM6DSX_REG_TS_RESET_ADDR, 319 ST_LSM6DSX_TS_RESET_VAL); 320 if (err < 0) 321 return err; 322 323 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { 324 if (!hw->iio_devs[i]) 325 continue; 326 327 sensor = iio_priv(hw->iio_devs[i]); 328 /* 329 * store enable buffer timestamp as reference for 330 * hw timestamp 331 */ 332 sensor->ts_ref = iio_get_time_ns(hw->iio_devs[i]); 333 } 334 return 0; 335 } 336 337 int st_lsm6dsx_resume_fifo(struct st_lsm6dsx_hw *hw) 338 { 339 int err; 340 341 /* reset hw ts counter */ 342 err = st_lsm6dsx_reset_hw_ts(hw); 343 if (err < 0) 344 return err; 345 346 return st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT); 347 } 348 349 /* 350 * Set max bulk read to ST_LSM6DSX_MAX_WORD_LEN/ST_LSM6DSX_MAX_TAGGED_WORD_LEN 351 * in order to avoid a kmalloc for each bus access 352 */ 353 static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr, 354 u8 *data, unsigned int data_len, 355 unsigned int max_word_len) 356 { 357 unsigned int word_len, read_len = 0; 358 int err; 359 360 while (read_len < data_len) { 361 word_len = min_t(unsigned int, data_len - read_len, 362 max_word_len); 363 err = st_lsm6dsx_read_locked(hw, addr, data + read_len, 364 word_len); 365 if (err < 0) 366 return err; 367 read_len += word_len; 368 } 369 return 0; 370 } 371 372 /** 373 * st_lsm6dsx_read_fifo() - hw FIFO read routine 374 * @hw: Pointer to instance of struct st_lsm6dsx_hw. 375 * 376 * Read samples from the hw FIFO and push them to IIO buffers. 377 * 378 * Return: Number of bytes read from the FIFO 379 */ 380 int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw) 381 { 382 struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor, *ext_sensor = NULL; 383 int err, sip, acc_sip, gyro_sip, ts_sip, ext_sip, read_len, offset; 384 u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE; 385 u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask; 386 bool reset_ts = false; 387 __le16 fifo_status; 388 s64 ts = 0; 389 390 err = st_lsm6dsx_read_locked(hw, 391 hw->settings->fifo_ops.fifo_diff.addr, 392 &fifo_status, sizeof(fifo_status)); 393 if (err < 0) { 394 dev_err(hw->dev, "failed to read fifo status (err=%d)\n", 395 err); 396 return err; 397 } 398 399 if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK)) 400 return 0; 401 402 if (!pattern_len) 403 pattern_len = ST_LSM6DSX_SAMPLE_SIZE; 404 405 fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) * 406 ST_LSM6DSX_CHAN_SIZE; 407 fifo_len = (fifo_len / pattern_len) * pattern_len; 408 409 acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]); 410 gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]); 411 if (hw->iio_devs[ST_LSM6DSX_ID_EXT0]) 412 ext_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_EXT0]); 413 414 for (read_len = 0; read_len < fifo_len; read_len += pattern_len) { 415 err = st_lsm6dsx_read_block(hw, ST_LSM6DSX_REG_FIFO_OUTL_ADDR, 416 hw->buff, pattern_len, 417 ST_LSM6DSX_MAX_WORD_LEN); 418 if (err < 0) { 419 dev_err(hw->dev, 420 "failed to read pattern from fifo (err=%d)\n", 421 err); 422 return err; 423 } 424 425 /* 426 * Data are written to the FIFO with a specific pattern 427 * depending on the configured ODRs. The first sequence of data 428 * stored in FIFO contains the data of all enabled sensors 429 * (e.g. Gx, Gy, Gz, Ax, Ay, Az, Ts), then data are repeated 430 * depending on the value of the decimation factor set for each 431 * sensor. 432 * 433 * Supposing the FIFO is storing data from gyroscope and 434 * accelerometer at different ODRs: 435 * - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz 436 * Since the gyroscope ODR is twice the accelerometer one, the 437 * following pattern is repeated every 9 samples: 438 * - Gx, Gy, Gz, Ax, Ay, Az, Ts, Gx, Gy, Gz, Ts, Gx, .. 439 */ 440 ext_sip = ext_sensor ? ext_sensor->sip : 0; 441 gyro_sip = gyro_sensor->sip; 442 acc_sip = acc_sensor->sip; 443 ts_sip = hw->ts_sip; 444 offset = 0; 445 sip = 0; 446 447 while (acc_sip > 0 || gyro_sip > 0 || ext_sip > 0) { 448 if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) { 449 memcpy(hw->scan[ST_LSM6DSX_ID_GYRO].channels, 450 &hw->buff[offset], 451 sizeof(hw->scan[ST_LSM6DSX_ID_GYRO].channels)); 452 offset += sizeof(hw->scan[ST_LSM6DSX_ID_GYRO].channels); 453 } 454 if (acc_sip > 0 && !(sip % acc_sensor->decimator)) { 455 memcpy(hw->scan[ST_LSM6DSX_ID_ACC].channels, 456 &hw->buff[offset], 457 sizeof(hw->scan[ST_LSM6DSX_ID_ACC].channels)); 458 offset += sizeof(hw->scan[ST_LSM6DSX_ID_ACC].channels); 459 } 460 if (ext_sip > 0 && !(sip % ext_sensor->decimator)) { 461 memcpy(hw->scan[ST_LSM6DSX_ID_EXT0].channels, 462 &hw->buff[offset], 463 sizeof(hw->scan[ST_LSM6DSX_ID_EXT0].channels)); 464 offset += sizeof(hw->scan[ST_LSM6DSX_ID_EXT0].channels); 465 } 466 467 if (ts_sip-- > 0) { 468 u8 data[ST_LSM6DSX_SAMPLE_SIZE]; 469 470 memcpy(data, &hw->buff[offset], sizeof(data)); 471 /* 472 * hw timestamp is 3B long and it is stored 473 * in FIFO using 6B as 4th FIFO data set 474 * according to this schema: 475 * B0 = ts[15:8], B1 = ts[23:16], B3 = ts[7:0] 476 */ 477 ts = data[1] << 16 | data[0] << 8 | data[3]; 478 /* 479 * check if hw timestamp engine is going to 480 * reset (the sensor generates an interrupt 481 * to signal the hw timestamp will reset in 482 * 1.638s) 483 */ 484 if (!reset_ts && ts >= 0xff0000) 485 reset_ts = true; 486 ts *= hw->ts_gain; 487 488 offset += ST_LSM6DSX_SAMPLE_SIZE; 489 } 490 491 if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) { 492 /* 493 * We need to discards gyro samples during 494 * filters settling time 495 */ 496 if (gyro_sensor->samples_to_discard > 0) 497 gyro_sensor->samples_to_discard--; 498 else 499 iio_push_to_buffers_with_timestamp( 500 hw->iio_devs[ST_LSM6DSX_ID_GYRO], 501 &hw->scan[ST_LSM6DSX_ID_GYRO], 502 gyro_sensor->ts_ref + ts); 503 gyro_sip--; 504 } 505 if (acc_sip > 0 && !(sip % acc_sensor->decimator)) { 506 /* 507 * We need to discards accel samples during 508 * filters settling time 509 */ 510 if (acc_sensor->samples_to_discard > 0) 511 acc_sensor->samples_to_discard--; 512 else 513 iio_push_to_buffers_with_timestamp( 514 hw->iio_devs[ST_LSM6DSX_ID_ACC], 515 &hw->scan[ST_LSM6DSX_ID_ACC], 516 acc_sensor->ts_ref + ts); 517 acc_sip--; 518 } 519 if (ext_sip > 0 && !(sip % ext_sensor->decimator)) { 520 iio_push_to_buffers_with_timestamp( 521 hw->iio_devs[ST_LSM6DSX_ID_EXT0], 522 &hw->scan[ST_LSM6DSX_ID_EXT0], 523 ext_sensor->ts_ref + ts); 524 ext_sip--; 525 } 526 sip++; 527 } 528 } 529 530 if (unlikely(reset_ts)) { 531 err = st_lsm6dsx_reset_hw_ts(hw); 532 if (err < 0) { 533 dev_err(hw->dev, "failed to reset hw ts (err=%d)\n", 534 err); 535 return err; 536 } 537 } 538 return read_len; 539 } 540 541 #define ST_LSM6DSX_INVALID_SAMPLE 0x7ffd 542 static bool st_lsm6dsx_check_data(u8 tag, __le16 *data) 543 { 544 if ((tag == ST_LSM6DSX_GYRO_TAG || tag == ST_LSM6DSX_ACC_TAG) && 545 (s16)le16_to_cpup(data) >= ST_LSM6DSX_INVALID_SAMPLE) 546 return false; 547 548 return true; 549 } 550 551 static int st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag, 552 __le16 *data, s64 ts) 553 { 554 struct st_lsm6dsx_sensor *sensor; 555 struct iio_dev *iio_dev; 556 557 /* invalid sample during bootstrap phase */ 558 if (!st_lsm6dsx_check_data(tag, data)) 559 return -EINVAL; 560 561 /* 562 * EXT_TAG are managed in FIFO fashion so ST_LSM6DSX_EXT0_TAG 563 * corresponds to the first enabled channel, ST_LSM6DSX_EXT1_TAG 564 * to the second one and ST_LSM6DSX_EXT2_TAG to the last enabled 565 * channel 566 */ 567 switch (tag) { 568 case ST_LSM6DSX_GYRO_TAG: 569 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_GYRO]; 570 break; 571 case ST_LSM6DSX_ACC_TAG: 572 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_ACC]; 573 break; 574 case ST_LSM6DSX_EXT0_TAG: 575 if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0)) 576 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT0]; 577 else if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1)) 578 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1]; 579 else 580 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2]; 581 break; 582 case ST_LSM6DSX_EXT1_TAG: 583 if ((hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0)) && 584 (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1))) 585 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1]; 586 else 587 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2]; 588 break; 589 case ST_LSM6DSX_EXT2_TAG: 590 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2]; 591 break; 592 case ST_LSM6DSX_ROT_TAG: 593 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_FUSION]; 594 break; 595 default: 596 return -EINVAL; 597 } 598 599 sensor = iio_priv(iio_dev); 600 iio_push_to_buffers_with_timestamp(iio_dev, data, 601 ts + sensor->ts_ref); 602 603 return 0; 604 } 605 606 /** 607 * st_lsm6dsx_read_tagged_fifo() - tagged hw FIFO read routine 608 * @hw: Pointer to instance of struct st_lsm6dsx_hw. 609 * 610 * Read samples from the hw FIFO and push them to IIO buffers. 611 * 612 * Return: Number of bytes read from the FIFO 613 */ 614 int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw) 615 { 616 u16 pattern_len = hw->sip * ST_LSM6DSX_TAGGED_SAMPLE_SIZE; 617 u16 fifo_len, fifo_diff_mask; 618 /* 619 * Alignment needed as this can ultimately be passed to a 620 * call to iio_push_to_buffers_with_timestamp() which 621 * must be passed a buffer that is aligned to 8 bytes so 622 * as to allow insertion of a naturally aligned timestamp. 623 */ 624 struct { 625 union { 626 __le16 data[3]; 627 __le32 fifo_ts; 628 }; 629 aligned_s64 timestamp; 630 } iio_buff = { }; 631 u8 tag; 632 bool reset_ts = false; 633 int i, err, read_len; 634 __le16 fifo_status; 635 s64 ts = 0; 636 637 err = st_lsm6dsx_read_locked(hw, 638 hw->settings->fifo_ops.fifo_diff.addr, 639 &fifo_status, sizeof(fifo_status)); 640 if (err < 0) { 641 dev_err(hw->dev, "failed to read fifo status (err=%d)\n", 642 err); 643 return err; 644 } 645 646 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask; 647 fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) * 648 ST_LSM6DSX_TAGGED_SAMPLE_SIZE; 649 if (!fifo_len) 650 return 0; 651 652 if (!pattern_len) 653 pattern_len = ST_LSM6DSX_TAGGED_SAMPLE_SIZE; 654 655 for (read_len = 0; read_len < fifo_len; read_len += pattern_len) { 656 err = st_lsm6dsx_read_block(hw, 657 ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR, 658 hw->buff, pattern_len, 659 ST_LSM6DSX_MAX_TAGGED_WORD_LEN); 660 if (err < 0) { 661 dev_err(hw->dev, 662 "failed to read pattern from fifo (err=%d)\n", 663 err); 664 return err; 665 } 666 667 for (i = 0; i < pattern_len; 668 i += ST_LSM6DSX_TAGGED_SAMPLE_SIZE) { 669 memcpy(&iio_buff, &hw->buff[i + ST_LSM6DSX_TAG_SIZE], 670 ST_LSM6DSX_SAMPLE_SIZE); 671 672 tag = hw->buff[i] >> 3; 673 if (tag == ST_LSM6DSX_TS_TAG) { 674 /* 675 * hw timestamp is 4B long and it is stored 676 * in FIFO according to this schema: 677 * B0 = ts[7:0], B1 = ts[15:8], B2 = ts[23:16], 678 * B3 = ts[31:24] 679 */ 680 ts = le32_to_cpu(iio_buff.fifo_ts); 681 /* 682 * check if hw timestamp engine is going to 683 * reset (the sensor generates an interrupt 684 * to signal the hw timestamp will reset in 685 * 1.638s) 686 */ 687 if (!reset_ts && ts >= 0xffff0000) 688 reset_ts = true; 689 ts *= hw->ts_gain; 690 } else { 691 st_lsm6dsx_push_tagged_data(hw, tag, 692 iio_buff.data, 693 ts); 694 } 695 } 696 } 697 698 if (unlikely(reset_ts)) { 699 err = st_lsm6dsx_reset_hw_ts(hw); 700 if (err < 0) 701 return err; 702 } 703 return read_len; 704 } 705 706 int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw) 707 { 708 int err; 709 710 if (!hw->settings->fifo_ops.read_fifo) 711 return -ENOTSUPP; 712 713 mutex_lock(&hw->fifo_lock); 714 715 hw->settings->fifo_ops.read_fifo(hw); 716 err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS); 717 718 mutex_unlock(&hw->fifo_lock); 719 720 return err; 721 } 722 723 static void 724 st_lsm6dsx_update_samples_to_discard(struct st_lsm6dsx_sensor *sensor) 725 { 726 const struct st_lsm6dsx_samples_to_discard *data; 727 struct st_lsm6dsx_hw *hw = sensor->hw; 728 int i; 729 730 if (sensor->id != ST_LSM6DSX_ID_GYRO && 731 sensor->id != ST_LSM6DSX_ID_ACC) 732 return; 733 734 /* check if drdy mask is supported in hw */ 735 if (hw->settings->drdy_mask.addr) 736 return; 737 738 data = &hw->settings->samples_to_discard[sensor->id]; 739 for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++) { 740 if (data->val[i].milli_hz == sensor->hwfifo_odr_mHz) { 741 sensor->samples_to_discard = data->val[i].samples; 742 return; 743 } 744 } 745 } 746 747 int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable) 748 { 749 struct st_lsm6dsx_hw *hw = sensor->hw; 750 u8 fifo_mask; 751 int err; 752 753 mutex_lock(&hw->conf_lock); 754 755 if (enable) 756 fifo_mask = hw->fifo_mask | BIT(sensor->id); 757 else 758 fifo_mask = hw->fifo_mask & ~BIT(sensor->id); 759 760 if (hw->fifo_mask) { 761 err = st_lsm6dsx_flush_fifo(hw); 762 if (err < 0) 763 goto out; 764 } 765 766 if (enable) 767 st_lsm6dsx_update_samples_to_discard(sensor); 768 769 err = st_lsm6dsx_device_set_enable(sensor, enable); 770 if (err < 0) 771 goto out; 772 773 err = st_lsm6dsx_set_fifo_odr(sensor, enable); 774 if (err < 0) 775 goto out; 776 777 err = st_lsm6dsx_update_decimators(hw); 778 if (err < 0) 779 goto out; 780 781 err = st_lsm6dsx_update_watermark(sensor, sensor->watermark); 782 if (err < 0) 783 goto out; 784 785 if (fifo_mask) { 786 err = st_lsm6dsx_resume_fifo(hw); 787 if (err < 0) 788 goto out; 789 } 790 791 hw->fifo_mask = fifo_mask; 792 793 out: 794 mutex_unlock(&hw->conf_lock); 795 796 return err; 797 } 798 799 static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev) 800 { 801 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); 802 struct st_lsm6dsx_hw *hw = sensor->hw; 803 804 if (!hw->settings->fifo_ops.update_fifo) 805 return -ENOTSUPP; 806 807 return hw->settings->fifo_ops.update_fifo(sensor, true); 808 } 809 810 static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev) 811 { 812 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); 813 struct st_lsm6dsx_hw *hw = sensor->hw; 814 815 if (!hw->settings->fifo_ops.update_fifo) 816 return -ENOTSUPP; 817 818 return hw->settings->fifo_ops.update_fifo(sensor, false); 819 } 820 821 static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = { 822 .preenable = st_lsm6dsx_buffer_preenable, 823 .postdisable = st_lsm6dsx_buffer_postdisable, 824 }; 825 826 static ssize_t st_lsm6dsx_hwfifo_odr_show(struct device *dev, 827 struct device_attribute *attr, char *buf) 828 { 829 struct st_lsm6dsx_sensor *sensor = iio_priv(dev_to_iio_dev(dev)); 830 831 return sysfs_emit(buf, "%d.%03d\n", sensor->hwfifo_odr_mHz / 1000, 832 sensor->hwfifo_odr_mHz % 1000); 833 } 834 835 static ssize_t st_lsm6dsx_hwfifo_odr_store(struct device *dev, 836 struct device_attribute *attr, 837 const char *buf, size_t len) 838 { 839 struct iio_dev *iio_dev = dev_to_iio_dev(dev); 840 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); 841 int integer, milli; 842 int ret; 843 u32 hwfifo_odr; 844 u8 data; 845 846 if (!iio_device_claim_direct(iio_dev)) 847 return -EBUSY; 848 849 ret = iio_str_to_fixpoint(buf, 100, &integer, &milli); 850 if (ret) 851 goto out; 852 853 hwfifo_odr = integer * 1000 + milli; 854 ret = st_lsm6dsx_check_odr(sensor, hwfifo_odr, &data); 855 if (ret < 0) 856 goto out; 857 858 hwfifo_odr = ret; 859 860 /* the batch data rate must not exceed the sensor output data rate */ 861 if (hwfifo_odr <= sensor->odr) 862 sensor->hwfifo_odr_mHz = hwfifo_odr; 863 else 864 ret = -EINVAL; 865 866 out: 867 iio_device_release_direct(iio_dev); 868 869 return ret < 0 ? ret : len; 870 } 871 872 static IIO_DEV_ATTR_SAMP_FREQ(0664, st_lsm6dsx_hwfifo_odr_show, st_lsm6dsx_hwfifo_odr_store); 873 874 static const struct iio_dev_attr *st_lsm6dsx_buffer_attrs[] = { 875 &iio_dev_attr_sampling_frequency, 876 NULL 877 }; 878 879 int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw) 880 { 881 int i, ret; 882 883 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { 884 const struct iio_dev_attr **attrs; 885 886 if (!hw->iio_devs[i]) 887 continue; 888 889 /* 890 * For the accelerometer, allow setting FIFO sampling frequency 891 * values different from the sensor sampling frequency, which 892 * may be needed to keep FIFO data rate low while sampling 893 * acceleration data at high rates for accurate event detection. 894 */ 895 attrs = i == ST_LSM6DSX_ID_ACC ? st_lsm6dsx_buffer_attrs : NULL; 896 ret = devm_iio_kfifo_buffer_setup_ext(hw->dev, hw->iio_devs[i], 897 &st_lsm6dsx_buffer_ops, 898 attrs); 899 if (ret) 900 return ret; 901 } 902 903 return 0; 904 } 905