1 // SPDX-License-Identifier: GPL-2.0-only 2 /* 3 * STMicroelectronics st_lsm6dsx FIFO buffer library driver 4 * 5 * LSM6DS3/LSM6DS3H/LSM6DSL/LSM6DSM/ISM330DLC: The FIFO buffer can be 6 * configured to store data from gyroscope and accelerometer. Samples are 7 * queued without any tag according to a specific pattern based on 8 * '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 * LSM6DSO/LSM6DSOX/ASM330LHH/LSM6DSR: The FIFO buffer can be configured to 18 * store data from gyroscope and accelerometer. Each sample is queued with 19 * a tag (1B) indicating data source (gyroscope, accelerometer, hw timer). 20 * 21 * FIFO supported modes: 22 * - BYPASS: FIFO disabled 23 * - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index 24 * restarts from the beginning and the oldest sample is overwritten 25 * 26 * Copyright 2016 STMicroelectronics Inc. 27 * 28 * Lorenzo Bianconi <lorenzo.bianconi@st.com> 29 * Denis Ciocca <denis.ciocca@st.com> 30 */ 31 #include <linux/module.h> 32 #include <linux/interrupt.h> 33 #include <linux/irq.h> 34 #include <linux/iio/kfifo_buf.h> 35 #include <linux/iio/iio.h> 36 #include <linux/iio/buffer.h> 37 #include <linux/regmap.h> 38 #include <linux/bitfield.h> 39 40 #include <linux/platform_data/st_sensors_pdata.h> 41 42 #include "st_lsm6dsx.h" 43 44 #define ST_LSM6DSX_REG_HLACTIVE_ADDR 0x12 45 #define ST_LSM6DSX_REG_HLACTIVE_MASK BIT(5) 46 #define ST_LSM6DSX_REG_PP_OD_ADDR 0x12 47 #define ST_LSM6DSX_REG_PP_OD_MASK BIT(4) 48 #define ST_LSM6DSX_REG_FIFO_MODE_ADDR 0x0a 49 #define ST_LSM6DSX_FIFO_MODE_MASK GENMASK(2, 0) 50 #define ST_LSM6DSX_FIFO_ODR_MASK GENMASK(6, 3) 51 #define ST_LSM6DSX_FIFO_EMPTY_MASK BIT(12) 52 #define ST_LSM6DSX_REG_FIFO_OUTL_ADDR 0x3e 53 #define ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR 0x78 54 #define ST_LSM6DSX_REG_TS_RESET_ADDR 0x42 55 56 #define ST_LSM6DSX_MAX_FIFO_ODR_VAL 0x08 57 58 #define ST_LSM6DSX_TS_SENSITIVITY 25000UL /* 25us */ 59 #define ST_LSM6DSX_TS_RESET_VAL 0xaa 60 61 struct st_lsm6dsx_decimator_entry { 62 u8 decimator; 63 u8 val; 64 }; 65 66 enum st_lsm6dsx_fifo_tag { 67 ST_LSM6DSX_GYRO_TAG = 0x01, 68 ST_LSM6DSX_ACC_TAG = 0x02, 69 ST_LSM6DSX_TS_TAG = 0x04, 70 ST_LSM6DSX_EXT0_TAG = 0x0f, 71 ST_LSM6DSX_EXT1_TAG = 0x10, 72 ST_LSM6DSX_EXT2_TAG = 0x11, 73 }; 74 75 static const 76 struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_table[] = { 77 { 0, 0x0 }, 78 { 1, 0x1 }, 79 { 2, 0x2 }, 80 { 3, 0x3 }, 81 { 4, 0x4 }, 82 { 8, 0x5 }, 83 { 16, 0x6 }, 84 { 32, 0x7 }, 85 }; 86 87 static int st_lsm6dsx_get_decimator_val(u8 val) 88 { 89 const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table); 90 int i; 91 92 for (i = 0; i < max_size; i++) 93 if (st_lsm6dsx_decimator_table[i].decimator == val) 94 break; 95 96 return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val; 97 } 98 99 static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw, 100 u16 *max_odr, u16 *min_odr) 101 { 102 struct st_lsm6dsx_sensor *sensor; 103 int i; 104 105 *max_odr = 0, *min_odr = ~0; 106 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { 107 if (!hw->iio_devs[i]) 108 continue; 109 110 sensor = iio_priv(hw->iio_devs[i]); 111 112 if (!(hw->enable_mask & BIT(sensor->id))) 113 continue; 114 115 *max_odr = max_t(u16, *max_odr, sensor->odr); 116 *min_odr = min_t(u16, *min_odr, sensor->odr); 117 } 118 } 119 120 static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw) 121 { 122 u16 max_odr, min_odr, sip = 0, ts_sip = 0; 123 const struct st_lsm6dsx_reg *ts_dec_reg; 124 struct st_lsm6dsx_sensor *sensor; 125 int err = 0, i; 126 u8 data; 127 128 st_lsm6dsx_get_max_min_odr(hw, &max_odr, &min_odr); 129 130 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { 131 const struct st_lsm6dsx_reg *dec_reg; 132 133 if (!hw->iio_devs[i]) 134 continue; 135 136 sensor = iio_priv(hw->iio_devs[i]); 137 /* update fifo decimators and sample in pattern */ 138 if (hw->enable_mask & BIT(sensor->id)) { 139 sensor->sip = sensor->odr / min_odr; 140 sensor->decimator = max_odr / sensor->odr; 141 data = st_lsm6dsx_get_decimator_val(sensor->decimator); 142 } else { 143 sensor->sip = 0; 144 sensor->decimator = 0; 145 data = 0; 146 } 147 ts_sip = max_t(u16, ts_sip, sensor->sip); 148 149 dec_reg = &hw->settings->decimator[sensor->id]; 150 if (dec_reg->addr) { 151 int val = ST_LSM6DSX_SHIFT_VAL(data, dec_reg->mask); 152 153 err = st_lsm6dsx_update_bits_locked(hw, dec_reg->addr, 154 dec_reg->mask, 155 val); 156 if (err < 0) 157 return err; 158 } 159 sip += sensor->sip; 160 } 161 hw->sip = sip + ts_sip; 162 hw->ts_sip = ts_sip; 163 164 /* 165 * update hw ts decimator if necessary. Decimator for hw timestamp 166 * is always 1 or 0 in order to have a ts sample for each data 167 * sample in FIFO 168 */ 169 ts_dec_reg = &hw->settings->ts_settings.decimator; 170 if (ts_dec_reg->addr) { 171 int val, ts_dec = !!hw->ts_sip; 172 173 val = ST_LSM6DSX_SHIFT_VAL(ts_dec, ts_dec_reg->mask); 174 err = st_lsm6dsx_update_bits_locked(hw, ts_dec_reg->addr, 175 ts_dec_reg->mask, val); 176 } 177 return err; 178 } 179 180 int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw, 181 enum st_lsm6dsx_fifo_mode fifo_mode) 182 { 183 unsigned int data; 184 int err; 185 186 data = FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK, fifo_mode); 187 err = st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR, 188 ST_LSM6DSX_FIFO_MODE_MASK, data); 189 if (err < 0) 190 return err; 191 192 hw->fifo_mode = fifo_mode; 193 194 return 0; 195 } 196 197 static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor, 198 bool enable) 199 { 200 struct st_lsm6dsx_hw *hw = sensor->hw; 201 const struct st_lsm6dsx_reg *batch_reg; 202 u8 data; 203 204 batch_reg = &hw->settings->batch[sensor->id]; 205 if (batch_reg->addr) { 206 int val; 207 208 if (enable) { 209 int err; 210 211 err = st_lsm6dsx_check_odr(sensor, sensor->odr, 212 &data); 213 if (err < 0) 214 return err; 215 } else { 216 data = 0; 217 } 218 val = ST_LSM6DSX_SHIFT_VAL(data, batch_reg->mask); 219 return st_lsm6dsx_update_bits_locked(hw, batch_reg->addr, 220 batch_reg->mask, val); 221 } else { 222 data = hw->enable_mask ? ST_LSM6DSX_MAX_FIFO_ODR_VAL : 0; 223 return st_lsm6dsx_update_bits_locked(hw, 224 ST_LSM6DSX_REG_FIFO_MODE_ADDR, 225 ST_LSM6DSX_FIFO_ODR_MASK, 226 FIELD_PREP(ST_LSM6DSX_FIFO_ODR_MASK, 227 data)); 228 } 229 } 230 231 int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark) 232 { 233 u16 fifo_watermark = ~0, cur_watermark, fifo_th_mask; 234 struct st_lsm6dsx_hw *hw = sensor->hw; 235 struct st_lsm6dsx_sensor *cur_sensor; 236 int i, err, data; 237 __le16 wdata; 238 239 if (!hw->sip) 240 return 0; 241 242 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { 243 if (!hw->iio_devs[i]) 244 continue; 245 246 cur_sensor = iio_priv(hw->iio_devs[i]); 247 248 if (!(hw->enable_mask & BIT(cur_sensor->id))) 249 continue; 250 251 cur_watermark = (cur_sensor == sensor) ? watermark 252 : cur_sensor->watermark; 253 254 fifo_watermark = min_t(u16, fifo_watermark, cur_watermark); 255 } 256 257 fifo_watermark = max_t(u16, fifo_watermark, hw->sip); 258 fifo_watermark = (fifo_watermark / hw->sip) * hw->sip; 259 fifo_watermark = fifo_watermark * hw->settings->fifo_ops.th_wl; 260 261 mutex_lock(&hw->page_lock); 262 err = regmap_read(hw->regmap, hw->settings->fifo_ops.fifo_th.addr + 1, 263 &data); 264 if (err < 0) 265 goto out; 266 267 fifo_th_mask = hw->settings->fifo_ops.fifo_th.mask; 268 fifo_watermark = ((data << 8) & ~fifo_th_mask) | 269 (fifo_watermark & fifo_th_mask); 270 271 wdata = cpu_to_le16(fifo_watermark); 272 err = regmap_bulk_write(hw->regmap, 273 hw->settings->fifo_ops.fifo_th.addr, 274 &wdata, sizeof(wdata)); 275 out: 276 mutex_unlock(&hw->page_lock); 277 return err; 278 } 279 280 static int st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw *hw) 281 { 282 struct st_lsm6dsx_sensor *sensor; 283 int i, err; 284 285 /* reset hw ts counter */ 286 err = st_lsm6dsx_write_locked(hw, ST_LSM6DSX_REG_TS_RESET_ADDR, 287 ST_LSM6DSX_TS_RESET_VAL); 288 if (err < 0) 289 return err; 290 291 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { 292 if (!hw->iio_devs[i]) 293 continue; 294 295 sensor = iio_priv(hw->iio_devs[i]); 296 /* 297 * store enable buffer timestamp as reference for 298 * hw timestamp 299 */ 300 sensor->ts_ref = iio_get_time_ns(hw->iio_devs[i]); 301 } 302 return 0; 303 } 304 305 /* 306 * Set max bulk read to ST_LSM6DSX_MAX_WORD_LEN/ST_LSM6DSX_MAX_TAGGED_WORD_LEN 307 * in order to avoid a kmalloc for each bus access 308 */ 309 static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr, 310 u8 *data, unsigned int data_len, 311 unsigned int max_word_len) 312 { 313 unsigned int word_len, read_len = 0; 314 int err; 315 316 while (read_len < data_len) { 317 word_len = min_t(unsigned int, data_len - read_len, 318 max_word_len); 319 err = st_lsm6dsx_read_locked(hw, addr, data + read_len, 320 word_len); 321 if (err < 0) 322 return err; 323 read_len += word_len; 324 } 325 return 0; 326 } 327 328 #define ST_LSM6DSX_IIO_BUFF_SIZE (ALIGN(ST_LSM6DSX_SAMPLE_SIZE, \ 329 sizeof(s64)) + sizeof(s64)) 330 /** 331 * st_lsm6dsx_read_fifo() - hw FIFO read routine 332 * @hw: Pointer to instance of struct st_lsm6dsx_hw. 333 * 334 * Read samples from the hw FIFO and push them to IIO buffers. 335 * 336 * Return: Number of bytes read from the FIFO 337 */ 338 int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw) 339 { 340 u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE; 341 u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask; 342 int err, acc_sip, gyro_sip, ts_sip, read_len, offset; 343 struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor; 344 u8 gyro_buff[ST_LSM6DSX_IIO_BUFF_SIZE]; 345 u8 acc_buff[ST_LSM6DSX_IIO_BUFF_SIZE]; 346 bool reset_ts = false; 347 __le16 fifo_status; 348 s64 ts = 0; 349 350 err = st_lsm6dsx_read_locked(hw, 351 hw->settings->fifo_ops.fifo_diff.addr, 352 &fifo_status, sizeof(fifo_status)); 353 if (err < 0) { 354 dev_err(hw->dev, "failed to read fifo status (err=%d)\n", 355 err); 356 return err; 357 } 358 359 if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK)) 360 return 0; 361 362 fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) * 363 ST_LSM6DSX_CHAN_SIZE; 364 fifo_len = (fifo_len / pattern_len) * pattern_len; 365 366 acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]); 367 gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]); 368 369 for (read_len = 0; read_len < fifo_len; read_len += pattern_len) { 370 err = st_lsm6dsx_read_block(hw, ST_LSM6DSX_REG_FIFO_OUTL_ADDR, 371 hw->buff, pattern_len, 372 ST_LSM6DSX_MAX_WORD_LEN); 373 if (err < 0) { 374 dev_err(hw->dev, 375 "failed to read pattern from fifo (err=%d)\n", 376 err); 377 return err; 378 } 379 380 /* 381 * Data are written to the FIFO with a specific pattern 382 * depending on the configured ODRs. The first sequence of data 383 * stored in FIFO contains the data of all enabled sensors 384 * (e.g. Gx, Gy, Gz, Ax, Ay, Az, Ts), then data are repeated 385 * depending on the value of the decimation factor set for each 386 * sensor. 387 * 388 * Supposing the FIFO is storing data from gyroscope and 389 * accelerometer at different ODRs: 390 * - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz 391 * Since the gyroscope ODR is twice the accelerometer one, the 392 * following pattern is repeated every 9 samples: 393 * - Gx, Gy, Gz, Ax, Ay, Az, Ts, Gx, Gy, Gz, Ts, Gx, .. 394 */ 395 gyro_sip = gyro_sensor->sip; 396 acc_sip = acc_sensor->sip; 397 ts_sip = hw->ts_sip; 398 offset = 0; 399 400 while (acc_sip > 0 || gyro_sip > 0) { 401 if (gyro_sip > 0) { 402 memcpy(gyro_buff, &hw->buff[offset], 403 ST_LSM6DSX_SAMPLE_SIZE); 404 offset += ST_LSM6DSX_SAMPLE_SIZE; 405 } 406 if (acc_sip > 0) { 407 memcpy(acc_buff, &hw->buff[offset], 408 ST_LSM6DSX_SAMPLE_SIZE); 409 offset += ST_LSM6DSX_SAMPLE_SIZE; 410 } 411 412 if (ts_sip-- > 0) { 413 u8 data[ST_LSM6DSX_SAMPLE_SIZE]; 414 415 memcpy(data, &hw->buff[offset], sizeof(data)); 416 /* 417 * hw timestamp is 3B long and it is stored 418 * in FIFO using 6B as 4th FIFO data set 419 * according to this schema: 420 * B0 = ts[15:8], B1 = ts[23:16], B3 = ts[7:0] 421 */ 422 ts = data[1] << 16 | data[0] << 8 | data[3]; 423 /* 424 * check if hw timestamp engine is going to 425 * reset (the sensor generates an interrupt 426 * to signal the hw timestamp will reset in 427 * 1.638s) 428 */ 429 if (!reset_ts && ts >= 0xff0000) 430 reset_ts = true; 431 ts *= ST_LSM6DSX_TS_SENSITIVITY; 432 433 offset += ST_LSM6DSX_SAMPLE_SIZE; 434 } 435 436 if (gyro_sip-- > 0) 437 iio_push_to_buffers_with_timestamp( 438 hw->iio_devs[ST_LSM6DSX_ID_GYRO], 439 gyro_buff, gyro_sensor->ts_ref + ts); 440 if (acc_sip-- > 0) 441 iio_push_to_buffers_with_timestamp( 442 hw->iio_devs[ST_LSM6DSX_ID_ACC], 443 acc_buff, acc_sensor->ts_ref + ts); 444 } 445 } 446 447 if (unlikely(reset_ts)) { 448 err = st_lsm6dsx_reset_hw_ts(hw); 449 if (err < 0) { 450 dev_err(hw->dev, "failed to reset hw ts (err=%d)\n", 451 err); 452 return err; 453 } 454 } 455 return read_len; 456 } 457 458 static int 459 st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag, 460 u8 *data, s64 ts) 461 { 462 struct st_lsm6dsx_sensor *sensor; 463 struct iio_dev *iio_dev; 464 465 /* 466 * EXT_TAG are managed in FIFO fashion so ST_LSM6DSX_EXT0_TAG 467 * corresponds to the first enabled channel, ST_LSM6DSX_EXT1_TAG 468 * to the second one and ST_LSM6DSX_EXT2_TAG to the last enabled 469 * channel 470 */ 471 switch (tag) { 472 case ST_LSM6DSX_GYRO_TAG: 473 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_GYRO]; 474 break; 475 case ST_LSM6DSX_ACC_TAG: 476 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_ACC]; 477 break; 478 case ST_LSM6DSX_EXT0_TAG: 479 if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0)) 480 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT0]; 481 else if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1)) 482 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1]; 483 else 484 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2]; 485 break; 486 case ST_LSM6DSX_EXT1_TAG: 487 if ((hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0)) && 488 (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1))) 489 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1]; 490 else 491 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2]; 492 break; 493 case ST_LSM6DSX_EXT2_TAG: 494 iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2]; 495 break; 496 default: 497 return -EINVAL; 498 } 499 500 sensor = iio_priv(iio_dev); 501 iio_push_to_buffers_with_timestamp(iio_dev, data, 502 ts + sensor->ts_ref); 503 504 return 0; 505 } 506 507 /** 508 * st_lsm6dsx_read_tagged_fifo() - tagged hw FIFO read routine 509 * @hw: Pointer to instance of struct st_lsm6dsx_hw. 510 * 511 * Read samples from the hw FIFO and push them to IIO buffers. 512 * 513 * Return: Number of bytes read from the FIFO 514 */ 515 int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw) 516 { 517 u16 pattern_len = hw->sip * ST_LSM6DSX_TAGGED_SAMPLE_SIZE; 518 u16 fifo_len, fifo_diff_mask; 519 u8 iio_buff[ST_LSM6DSX_IIO_BUFF_SIZE], tag; 520 bool reset_ts = false; 521 int i, err, read_len; 522 __le16 fifo_status; 523 s64 ts = 0; 524 525 err = st_lsm6dsx_read_locked(hw, 526 hw->settings->fifo_ops.fifo_diff.addr, 527 &fifo_status, sizeof(fifo_status)); 528 if (err < 0) { 529 dev_err(hw->dev, "failed to read fifo status (err=%d)\n", 530 err); 531 return err; 532 } 533 534 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask; 535 fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) * 536 ST_LSM6DSX_TAGGED_SAMPLE_SIZE; 537 if (!fifo_len) 538 return 0; 539 540 for (read_len = 0; read_len < fifo_len; read_len += pattern_len) { 541 err = st_lsm6dsx_read_block(hw, 542 ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR, 543 hw->buff, pattern_len, 544 ST_LSM6DSX_MAX_TAGGED_WORD_LEN); 545 if (err < 0) { 546 dev_err(hw->dev, 547 "failed to read pattern from fifo (err=%d)\n", 548 err); 549 return err; 550 } 551 552 for (i = 0; i < pattern_len; 553 i += ST_LSM6DSX_TAGGED_SAMPLE_SIZE) { 554 memcpy(iio_buff, &hw->buff[i + ST_LSM6DSX_TAG_SIZE], 555 ST_LSM6DSX_SAMPLE_SIZE); 556 557 tag = hw->buff[i] >> 3; 558 if (tag == ST_LSM6DSX_TS_TAG) { 559 /* 560 * hw timestamp is 4B long and it is stored 561 * in FIFO according to this schema: 562 * B0 = ts[7:0], B1 = ts[15:8], B2 = ts[23:16], 563 * B3 = ts[31:24] 564 */ 565 ts = le32_to_cpu(*((__le32 *)iio_buff)); 566 /* 567 * check if hw timestamp engine is going to 568 * reset (the sensor generates an interrupt 569 * to signal the hw timestamp will reset in 570 * 1.638s) 571 */ 572 if (!reset_ts && ts >= 0xffff0000) 573 reset_ts = true; 574 ts *= ST_LSM6DSX_TS_SENSITIVITY; 575 } else { 576 st_lsm6dsx_push_tagged_data(hw, tag, iio_buff, 577 ts); 578 } 579 } 580 } 581 582 if (unlikely(reset_ts)) { 583 err = st_lsm6dsx_reset_hw_ts(hw); 584 if (err < 0) 585 return err; 586 } 587 return read_len; 588 } 589 590 int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw) 591 { 592 int err; 593 594 mutex_lock(&hw->fifo_lock); 595 596 hw->settings->fifo_ops.read_fifo(hw); 597 err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS); 598 599 mutex_unlock(&hw->fifo_lock); 600 601 return err; 602 } 603 604 static int st_lsm6dsx_update_fifo(struct iio_dev *iio_dev, bool enable) 605 { 606 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); 607 struct st_lsm6dsx_hw *hw = sensor->hw; 608 int err; 609 610 mutex_lock(&hw->conf_lock); 611 612 if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS) { 613 err = st_lsm6dsx_flush_fifo(hw); 614 if (err < 0) 615 goto out; 616 } 617 618 if (sensor->id == ST_LSM6DSX_ID_EXT0 || 619 sensor->id == ST_LSM6DSX_ID_EXT1 || 620 sensor->id == ST_LSM6DSX_ID_EXT2) { 621 err = st_lsm6dsx_shub_set_enable(sensor, enable); 622 if (err < 0) 623 goto out; 624 } else { 625 err = st_lsm6dsx_sensor_set_enable(sensor, enable); 626 if (err < 0) 627 goto out; 628 629 err = st_lsm6dsx_set_fifo_odr(sensor, enable); 630 if (err < 0) 631 goto out; 632 } 633 634 err = st_lsm6dsx_update_decimators(hw); 635 if (err < 0) 636 goto out; 637 638 err = st_lsm6dsx_update_watermark(sensor, sensor->watermark); 639 if (err < 0) 640 goto out; 641 642 if (hw->enable_mask) { 643 /* reset hw ts counter */ 644 err = st_lsm6dsx_reset_hw_ts(hw); 645 if (err < 0) 646 goto out; 647 648 err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT); 649 } 650 651 out: 652 mutex_unlock(&hw->conf_lock); 653 654 return err; 655 } 656 657 static irqreturn_t st_lsm6dsx_handler_irq(int irq, void *private) 658 { 659 struct st_lsm6dsx_hw *hw = private; 660 661 return hw->sip > 0 ? IRQ_WAKE_THREAD : IRQ_NONE; 662 } 663 664 static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private) 665 { 666 struct st_lsm6dsx_hw *hw = private; 667 int count; 668 669 mutex_lock(&hw->fifo_lock); 670 count = hw->settings->fifo_ops.read_fifo(hw); 671 mutex_unlock(&hw->fifo_lock); 672 673 return !count ? IRQ_NONE : IRQ_HANDLED; 674 } 675 676 static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev) 677 { 678 return st_lsm6dsx_update_fifo(iio_dev, true); 679 } 680 681 static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev) 682 { 683 return st_lsm6dsx_update_fifo(iio_dev, false); 684 } 685 686 static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = { 687 .preenable = st_lsm6dsx_buffer_preenable, 688 .postdisable = st_lsm6dsx_buffer_postdisable, 689 }; 690 691 int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw) 692 { 693 struct device_node *np = hw->dev->of_node; 694 struct st_sensors_platform_data *pdata; 695 struct iio_buffer *buffer; 696 unsigned long irq_type; 697 bool irq_active_low; 698 int i, err; 699 700 irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq)); 701 702 switch (irq_type) { 703 case IRQF_TRIGGER_HIGH: 704 case IRQF_TRIGGER_RISING: 705 irq_active_low = false; 706 break; 707 case IRQF_TRIGGER_LOW: 708 case IRQF_TRIGGER_FALLING: 709 irq_active_low = true; 710 break; 711 default: 712 dev_info(hw->dev, "mode %lx unsupported\n", irq_type); 713 return -EINVAL; 714 } 715 716 err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_HLACTIVE_ADDR, 717 ST_LSM6DSX_REG_HLACTIVE_MASK, 718 FIELD_PREP(ST_LSM6DSX_REG_HLACTIVE_MASK, 719 irq_active_low)); 720 if (err < 0) 721 return err; 722 723 pdata = (struct st_sensors_platform_data *)hw->dev->platform_data; 724 if ((np && of_property_read_bool(np, "drive-open-drain")) || 725 (pdata && pdata->open_drain)) { 726 err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_PP_OD_ADDR, 727 ST_LSM6DSX_REG_PP_OD_MASK, 728 FIELD_PREP(ST_LSM6DSX_REG_PP_OD_MASK, 729 1)); 730 if (err < 0) 731 return err; 732 733 irq_type |= IRQF_SHARED; 734 } 735 736 err = devm_request_threaded_irq(hw->dev, hw->irq, 737 st_lsm6dsx_handler_irq, 738 st_lsm6dsx_handler_thread, 739 irq_type | IRQF_ONESHOT, 740 "lsm6dsx", hw); 741 if (err) { 742 dev_err(hw->dev, "failed to request trigger irq %d\n", 743 hw->irq); 744 return err; 745 } 746 747 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { 748 if (!hw->iio_devs[i]) 749 continue; 750 751 buffer = devm_iio_kfifo_allocate(hw->dev); 752 if (!buffer) 753 return -ENOMEM; 754 755 iio_device_attach_buffer(hw->iio_devs[i], buffer); 756 hw->iio_devs[i]->modes |= INDIO_BUFFER_SOFTWARE; 757 hw->iio_devs[i]->setup_ops = &st_lsm6dsx_buffer_ops; 758 } 759 760 return 0; 761 } 762