1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3 * STMicroelectronics st_lsm6dsr FIFO buffer library driver
4 *
5 * Copyright 2020 STMicroelectronics Inc.
6 *
7 * Lorenzo Bianconi <lorenzo.bianconi@st.com>
8 */
9 #include <linux/module.h>
10 #include <linux/interrupt.h>
11 #include <linux/irq.h>
12 #include <linux/iio/iio.h>
13 #include <linux/iio/kfifo_buf.h>
14 #include <linux/iio/events.h>
15 #include <asm/unaligned.h>
16 #include <linux/iio/trigger_consumer.h>
17 #include <linux/iio/triggered_buffer.h>
18 #include <linux/iio/trigger.h>
19 #include <linux/iio/buffer.h>
20 #include "st_lsm6dsr.h"
21
22 #define ST_LSM6DSR_REG_EMB_FUNC_STATUS_MAINPAGE 0x35
23 #define ST_LSM6DSR_REG_INT_STEP_DET_MASK BIT(3)
24 #define ST_LSM6DSR_REG_INT_TILT_MASK BIT(4)
25 #define ST_LSM6DSR_REG_INT_SIGMOT_MASK BIT(5)
26 #define ST_LSM6DSR_REG_INT_GLANCE_MASK BIT(0)
27 #define ST_LSM6DSR_REG_INT_MOTION_MASK BIT(1)
28 #define ST_LSM6DSR_REG_INT_NO_MOTION_MASK BIT(2)
29 #define ST_LSM6DSR_REG_INT_WAKEUP_MASK BIT(3)
30 #define ST_LSM6DSR_REG_INT_PICKUP_MASK BIT(4)
31 #define ST_LSM6DSR_REG_INT_ORIENTATION_MASK BIT(5)
32 #define ST_LSM6DSR_REG_INT_WRIST_MASK BIT(6)
33
34 #define ST_LSM6DSR_SAMPLE_DISCHARD 0x7ffd
35
36 #define ST_LSM6DSR_EWMA_LEVEL 120
37 #define ST_LSM6DSR_EWMA_DIV 128
38
39 enum {
40 ST_LSM6DSR_GYRO_TAG = 0x01,
41 ST_LSM6DSR_ACC_TAG = 0x02,
42 ST_LSM6DSR_TEMP_TAG = 0x03,
43 ST_LSM6DSR_TS_TAG = 0x04,
44 ST_LSM6DSR_EXT0_TAG = 0x0f,
45 ST_LSM6DSR_EXT1_TAG = 0x10,
46 ST_LSM6DSR_SC_TAG = 0x12,
47 };
48
49 /**
50 * Get Linux timestamp (SW)
51 *
52 * @return timestamp in ns
53 */
st_lsm6dsr_get_time_ns(void)54 static inline s64 st_lsm6dsr_get_time_ns(void)
55 {
56 return ktime_to_ns(ktime_get_boottime());
57 }
58
59 /**
60 * Timestamp low pass filter
61 *
62 * @param old: ST IMU MEMS hw instance
63 * @param new: ST IMU MEMS hw instance
64 * @param weight: ST IMU MEMS hw instance
65 * @return estimation of the timestamp average
66 */
st_lsm6dsr_ewma(s64 old,s64 new,int weight)67 static inline s64 st_lsm6dsr_ewma(s64 old, s64 new, int weight)
68 {
69 s64 diff, incr;
70
71 diff = new - old;
72 incr = div_s64((ST_LSM6DSR_EWMA_DIV - weight) * diff,
73 ST_LSM6DSR_EWMA_DIV);
74
75 return old + incr;
76 }
77
78 /**
79 * Reset HW Timestamp counter and clear timestamp data structure
80 *
81 * @param hw: ST IMU MEMS hw instance
82 * @return < 0 if error, 0 otherwise
83 */
st_lsm6dsr_reset_hwts(struct st_lsm6dsr_hw * hw)84 inline int st_lsm6dsr_reset_hwts(struct st_lsm6dsr_hw *hw)
85 {
86 u8 data = 0xaa;
87
88 hw->ts = st_lsm6dsr_get_time_ns();
89 hw->ts_offset = hw->ts;
90 hw->val_ts_old = 0;
91 hw->hw_ts_high = 0;
92 hw->tsample = 0ull;
93
94 return st_lsm6dsr_write_atomic(hw, ST_LSM6DSR_REG_TIMESTAMP2_ADDR,
95 sizeof(data), &data);
96 }
97
98 /**
99 * Setting FIFO mode
100 *
101 * @param hw: ST IMU MEMS hw instance
102 * @param fifo_mode: ST_LSM6DSR_FIFO_BYPASS or ST_LSM6DSR_FIFO_CONT
103 * @return 0 FIFO configured accordingly, non zero otherwise
104 */
st_lsm6dsr_set_fifo_mode(struct st_lsm6dsr_hw * hw,enum st_lsm6dsr_fifo_mode fifo_mode)105 int st_lsm6dsr_set_fifo_mode(struct st_lsm6dsr_hw *hw,
106 enum st_lsm6dsr_fifo_mode fifo_mode)
107 {
108 int err;
109
110 err = st_lsm6dsr_write_with_mask(hw, ST_LSM6DSR_REG_FIFO_CTRL4_ADDR,
111 ST_LSM6DSR_REG_FIFO_MODE_MASK,
112 fifo_mode);
113 if (err < 0)
114 return err;
115
116 hw->fifo_mode = fifo_mode;
117
118 return 0;
119 }
120
121 /**
122 * Setting sensor ODR in batching mode
123 *
124 * @param sensor: ST IMU sensor instance
125 * @param enable: enable or disable batching mode
126 * @return 0 FIFO configured accordingly, non zero otherwise
127 */
__st_lsm6dsr_set_sensor_batching_odr(struct st_lsm6dsr_sensor * sensor,bool enable)128 int __st_lsm6dsr_set_sensor_batching_odr(struct st_lsm6dsr_sensor *sensor,
129 bool enable)
130 {
131 struct st_lsm6dsr_hw *hw = sensor->hw;
132 u8 data = 0;
133 int err;
134 int podr, puodr;
135
136 if (enable) {
137 err = st_lsm6dsr_get_odr_val(sensor->id, sensor->odr,
138 sensor->uodr, &podr, &puodr,
139 &data);
140 if (err < 0)
141 return err;
142 }
143
144 err = __st_lsm6dsr_write_with_mask(hw, sensor->batch_reg.addr,
145 sensor->batch_reg.mask, data);
146 return err < 0 ? err : 0;
147 }
148
149 /**
150 * Setting timestamp ODR in batching mode
151 *
152 * @param hw: ST IMU MEMS hw instance
153 * @return Timestamp ODR
154 */
st_lsm6dsr_ts_odr(struct st_lsm6dsr_hw * hw)155 static int st_lsm6dsr_ts_odr(struct st_lsm6dsr_hw *hw)
156 {
157 struct st_lsm6dsr_sensor *sensor;
158 int odr = 0;
159 u8 i;
160
161 for (i = ST_LSM6DSR_ID_GYRO; i <= ST_LSM6DSR_ID_EXT1; i++) {
162 if (!hw->iio_devs[i])
163 continue;
164
165 sensor = iio_priv(hw->iio_devs[i]);
166 if (hw->enable_mask & BIT(sensor->id))
167 odr = max_t(int, odr, sensor->odr);
168 }
169
170 return odr;
171 }
172
173 /**
174 * Setting sensor ODR in batching mode
175 *
176 * @param sensor: ST IMU sensor instance
177 * @param enable: enable or disable batching mode
178 * @return 0 FIFO configured accordingly, non zero otherwise
179 */
180 static inline int
st_lsm6dsr_set_sensor_batching_odr(struct st_lsm6dsr_sensor * sensor,bool enable)181 st_lsm6dsr_set_sensor_batching_odr(struct st_lsm6dsr_sensor *sensor,
182 bool enable)
183 {
184 struct st_lsm6dsr_hw *hw = sensor->hw;
185 int err;
186
187 mutex_lock(&hw->page_lock);
188 err = __st_lsm6dsr_set_sensor_batching_odr(sensor, enable);
189 mutex_unlock(&hw->page_lock);
190
191 return err;
192 }
193
194 /**
195 * Update watermark level in FIFO
196 *
197 * @param sensor: ST IMU sensor instance
198 * @param watermark: New watermark level
199 * @return 0 if FIFO configured, non zero for error
200 */
st_lsm6dsr_update_watermark(struct st_lsm6dsr_sensor * sensor,u16 watermark)201 int st_lsm6dsr_update_watermark(struct st_lsm6dsr_sensor *sensor,
202 u16 watermark)
203 {
204 u16 fifo_watermark = ST_LSM6DSR_MAX_FIFO_DEPTH, cur_watermark = 0;
205 struct st_lsm6dsr_hw *hw = sensor->hw;
206 struct st_lsm6dsr_sensor *cur_sensor;
207 __le16 wdata;
208 int i, err;
209 u8 data;
210
211 for (i = ST_LSM6DSR_ID_GYRO; i <= ST_LSM6DSR_ID_STEP_COUNTER; i++) {
212 if (!hw->iio_devs[i])
213 continue;
214
215 cur_sensor = iio_priv(hw->iio_devs[i]);
216
217 if (!(hw->enable_mask & BIT(cur_sensor->id)))
218 continue;
219
220 cur_watermark = (cur_sensor == sensor) ? watermark :
221 cur_sensor->watermark;
222
223 fifo_watermark = min_t(u16, fifo_watermark, cur_watermark);
224 }
225
226 fifo_watermark = max_t(u16, fifo_watermark, 2);
227
228 err = st_lsm6dsr_read_atomic(hw, ST_LSM6DSR_REG_FIFO_CTRL1_ADDR + 1,
229 sizeof(data), &data);
230 if (err < 0)
231 goto out;
232
233 fifo_watermark = ((data << 8) & ~ST_LSM6DSR_REG_FIFO_WTM_MASK) |
234 (fifo_watermark & ST_LSM6DSR_REG_FIFO_WTM_MASK);
235 wdata = cpu_to_le16(fifo_watermark);
236 err = st_lsm6dsr_write_atomic(hw, ST_LSM6DSR_REG_FIFO_CTRL1_ADDR,
237 sizeof(wdata), (u8 *)&wdata);
238 out:
239 return err < 0 ? err : 0;
240 }
241
242 /**
243 * Timestamp correlation finction
244 *
245 * @param hw: ST IMU MEMS hw instance
246 * @param ts: New timestamp
247 */
st_lsm6dsr_sync_hw_ts(struct st_lsm6dsr_hw * hw,s64 ts)248 static inline void st_lsm6dsr_sync_hw_ts(struct st_lsm6dsr_hw *hw, s64 ts)
249 {
250 s64 delta = ts - hw->hw_ts;
251
252 hw->ts_offset = st_lsm6dsr_ewma(hw->ts_offset, delta,
253 ST_LSM6DSR_EWMA_LEVEL);
254 }
255
256 /**
257 * Return the iio device structure based on FIFO TAG ID
258 *
259 * @param hw: ST IMU MEMS hw instance
260 * @param tag: FIFO sample TAG ID
261 * @return 0 if FIFO configured, non zero for error
262 */
263 static struct
st_lsm6dsr_get_iiodev_from_tag(struct st_lsm6dsr_hw * hw,u8 tag)264 iio_dev *st_lsm6dsr_get_iiodev_from_tag(struct st_lsm6dsr_hw *hw,
265 u8 tag)
266 {
267 struct iio_dev *iio_dev;
268
269 switch (tag) {
270 case ST_LSM6DSR_GYRO_TAG:
271 iio_dev = hw->iio_devs[ST_LSM6DSR_ID_GYRO];
272 break;
273 case ST_LSM6DSR_ACC_TAG:
274 iio_dev = hw->iio_devs[ST_LSM6DSR_ID_ACC];
275 break;
276 case ST_LSM6DSR_TEMP_TAG:
277 iio_dev = hw->iio_devs[ST_LSM6DSR_ID_TEMP];
278 break;
279 case ST_LSM6DSR_EXT0_TAG:
280 if (hw->enable_mask & BIT(ST_LSM6DSR_ID_EXT0))
281 iio_dev = hw->iio_devs[ST_LSM6DSR_ID_EXT0];
282 else
283 iio_dev = hw->iio_devs[ST_LSM6DSR_ID_EXT1];
284 break;
285 case ST_LSM6DSR_EXT1_TAG:
286 iio_dev = hw->iio_devs[ST_LSM6DSR_ID_EXT1];
287 break;
288 case ST_LSM6DSR_SC_TAG:
289 iio_dev = hw->iio_devs[ST_LSM6DSR_ID_STEP_COUNTER];
290 break;
291 default:
292 iio_dev = NULL;
293 break;
294 }
295
296 return iio_dev;
297 }
298
299 /**
300 * Read all FIFO data stored after WTM FIFO irq fired interrupt
301 *
302 * @param hw: ST IMU MEMS hw instance
303 * @return Number of read bytes in FIFO or error if negative
304 */
st_lsm6dsr_read_fifo(struct st_lsm6dsr_hw * hw)305 static int st_lsm6dsr_read_fifo(struct st_lsm6dsr_hw *hw)
306 {
307 u8 iio_buf[ALIGN(ST_LSM6DSR_SAMPLE_SIZE, sizeof(s64)) + sizeof(s64)];
308 /* acc + gyro + 2 ext + ts + sc */
309 u8 buf[6 * ST_LSM6DSR_FIFO_SAMPLE_SIZE], tag, *ptr;
310 int i, err, word_len, fifo_len, read_len;
311 struct st_lsm6dsr_sensor *sensor;
312 struct iio_dev *iio_dev;
313 s64 ts_irq, hw_ts_old;
314 __le16 fifo_status;
315 u16 fifo_depth;
316 s16 drdymask;
317 u32 val;
318
319 ts_irq = hw->ts - hw->delta_ts;
320
321 err = st_lsm6dsr_read_atomic(hw, ST_LSM6DSR_REG_FIFO_STATUS1_ADDR,
322 sizeof(fifo_status), (u8 *)&fifo_status);
323 if (err < 0)
324 return err;
325
326 fifo_depth = le16_to_cpu(fifo_status) & ST_LSM6DSR_REG_FIFO_STATUS_DIFF;
327 if (!fifo_depth)
328 return 0;
329
330 fifo_len = fifo_depth * ST_LSM6DSR_FIFO_SAMPLE_SIZE;
331 read_len = 0;
332
333 while (read_len < fifo_len) {
334 word_len = min_t(int, fifo_len - read_len, sizeof(buf));
335 err = st_lsm6dsr_read_atomic(hw,
336 ST_LSM6DSR_REG_FIFO_DATA_OUT_TAG_ADDR,
337 word_len, buf);
338 if (err < 0)
339 return err;
340
341 for (i = 0; i < word_len; i += ST_LSM6DSR_FIFO_SAMPLE_SIZE) {
342 ptr = &buf[i + ST_LSM6DSR_TAG_SIZE];
343 tag = buf[i] >> 3;
344
345 if (tag == ST_LSM6DSR_TS_TAG) {
346 val = get_unaligned_le32(ptr);
347
348 if (hw->val_ts_old > val)
349 hw->hw_ts_high++;
350
351 hw_ts_old = hw->hw_ts;
352
353 /* check hw rollover */
354 hw->val_ts_old = val;
355 hw->hw_ts = (val +
356 ((s64)hw->hw_ts_high << 32)) *
357 hw->ts_delta_ns;
358 hw->ts_offset = st_lsm6dsr_ewma(hw->ts_offset,
359 ts_irq - hw->hw_ts,
360 ST_LSM6DSR_EWMA_LEVEL);
361
362 if (!test_bit(ST_LSM6DSR_HW_FLUSH, &hw->state))
363 /* sync ap timestamp and sensor one */
364 st_lsm6dsr_sync_hw_ts(hw, ts_irq);
365
366 ts_irq += hw->hw_ts;
367
368 if (!hw->tsample)
369 hw->tsample = hw->ts_offset + hw->hw_ts;
370 else
371 hw->tsample = hw->tsample +
372 hw->hw_ts - hw_ts_old;
373 } else {
374 iio_dev =
375 st_lsm6dsr_get_iiodev_from_tag(hw, tag);
376 if (!iio_dev)
377 continue;
378
379 sensor = iio_priv(iio_dev);
380
381 /* skip samples if not ready */
382 drdymask =
383 (s16)le16_to_cpu(get_unaligned_le16(ptr));
384 if (unlikely(drdymask >=
385 ST_LSM6DSR_SAMPLE_DISCHARD)) {
386 continue;
387 }
388
389 /*
390 * hw ts in not queued in FIFO if only step
391 * counter enabled
392 */
393 if (sensor->id == ST_LSM6DSR_ID_STEP_COUNTER) {
394 val = get_unaligned_le32(ptr + 2);
395 hw->tsample = (val +
396 ((s64)hw->hw_ts_high << 32)) *
397 hw->ts_delta_ns;
398
399 /* avoid samples in the future */
400 hw->tsample = min_t(s64,
401 st_lsm6dsr_get_time_ns(),
402 hw->tsample);
403 } else {
404 hw->tsample = st_lsm6dsr_get_time_ns();
405 }
406
407 memcpy(iio_buf, ptr, ST_LSM6DSR_SAMPLE_SIZE);
408
409 sensor->last_fifo_timestamp = hw->tsample;
410
411 /* support decimation for ODR < 12.5 Hz */
412 if (sensor->dec_counter > 0) {
413 sensor->dec_counter--;
414 } else {
415 sensor->dec_counter = sensor->decimator;
416 iio_push_to_buffers_with_timestamp(iio_dev,
417 iio_buf,
418 hw->tsample);
419 }
420 }
421 }
422 read_len += word_len;
423 }
424
425 return read_len;
426 }
427
428 /**
429 * Report events after WTM FIFO irq fired interrupt
430 *
431 * @param hw: ST IMU MEMS hw instance
432 * @return 0 if OK, non zero for error
433 */
st_lsm6dsr_report_events(struct st_lsm6dsr_hw * hw)434 static int st_lsm6dsr_report_events(struct st_lsm6dsr_hw *hw)
435 {
436 struct iio_dev *iio_dev;
437 u8 status[3];
438 s64 event;
439 int err;
440
441 if (hw->enable_mask & (BIT(ST_LSM6DSR_ID_STEP_DETECTOR) |
442 BIT(ST_LSM6DSR_ID_SIGN_MOTION) |
443 BIT(ST_LSM6DSR_ID_TILT) |
444 BIT(ST_LSM6DSR_ID_MOTION) |
445 BIT(ST_LSM6DSR_ID_NO_MOTION) |
446 BIT(ST_LSM6DSR_ID_WAKEUP) |
447 BIT(ST_LSM6DSR_ID_PICKUP) |
448 BIT(ST_LSM6DSR_ID_ORIENTATION) |
449 BIT(ST_LSM6DSR_ID_WRIST_TILT) |
450 BIT(ST_LSM6DSR_ID_GLANCE))) {
451
452 err = hw->tf->read(hw->dev,
453 ST_LSM6DSR_REG_EMB_FUNC_STATUS_MAINPAGE,
454 sizeof(status), status);
455 if (err < 0)
456 return err;
457
458 /* embedded function sensors */
459 if (status[0] & ST_LSM6DSR_REG_INT_STEP_DET_MASK) {
460 iio_dev = hw->iio_devs[ST_LSM6DSR_ID_STEP_DETECTOR];
461 event = IIO_UNMOD_EVENT_CODE(IIO_STEP_DETECTOR, -1,
462 IIO_EV_TYPE_THRESH,
463 IIO_EV_DIR_RISING);
464 iio_push_event(iio_dev, event,
465 st_lsm6dsr_get_time_ns());
466 }
467 if (status[0] & ST_LSM6DSR_REG_INT_SIGMOT_MASK) {
468 iio_dev = hw->iio_devs[ST_LSM6DSR_ID_SIGN_MOTION];
469 event = IIO_UNMOD_EVENT_CODE(IIO_SIGN_MOTION, -1,
470 IIO_EV_TYPE_THRESH,
471 IIO_EV_DIR_RISING);
472 iio_push_event(iio_dev, event,
473 st_lsm6dsr_get_time_ns());
474 }
475 if (status[0] & ST_LSM6DSR_REG_INT_TILT_MASK) {
476 iio_dev = hw->iio_devs[ST_LSM6DSR_ID_TILT];
477 event = IIO_UNMOD_EVENT_CODE(IIO_TILT, -1,
478 IIO_EV_TYPE_THRESH,
479 IIO_EV_DIR_RISING);
480 iio_push_event(iio_dev, event,
481 st_lsm6dsr_get_time_ns());
482 }
483 /* fsm sensors */
484 if (status[1] & ST_LSM6DSR_REG_INT_GLANCE_MASK) {
485 iio_dev = hw->iio_devs[ST_LSM6DSR_ID_GLANCE];
486 event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1,
487 IIO_EV_TYPE_THRESH,
488 IIO_EV_DIR_RISING);
489 iio_push_event(iio_dev, event,
490 st_lsm6dsr_get_time_ns());
491 }
492 if (status[1] & ST_LSM6DSR_REG_INT_MOTION_MASK) {
493 iio_dev = hw->iio_devs[ST_LSM6DSR_ID_MOTION];
494 event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1,
495 IIO_EV_TYPE_THRESH,
496 IIO_EV_DIR_RISING);
497 iio_push_event(iio_dev, event,
498 st_lsm6dsr_get_time_ns());
499 }
500 if (status[1] & ST_LSM6DSR_REG_INT_NO_MOTION_MASK) {
501 iio_dev = hw->iio_devs[ST_LSM6DSR_ID_NO_MOTION];
502 event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1,
503 IIO_EV_TYPE_THRESH,
504 IIO_EV_DIR_RISING);
505 iio_push_event(iio_dev, event,
506 st_lsm6dsr_get_time_ns());
507 }
508 if (status[1] & ST_LSM6DSR_REG_INT_WAKEUP_MASK) {
509 iio_dev = hw->iio_devs[ST_LSM6DSR_ID_WAKEUP];
510 event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1,
511 IIO_EV_TYPE_THRESH,
512 IIO_EV_DIR_RISING);
513 iio_push_event(iio_dev, event,
514 st_lsm6dsr_get_time_ns());
515 }
516 if (status[1] & ST_LSM6DSR_REG_INT_PICKUP_MASK) {
517 iio_dev = hw->iio_devs[ST_LSM6DSR_ID_PICKUP];
518 event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1,
519 IIO_EV_TYPE_THRESH,
520 IIO_EV_DIR_RISING);
521 iio_push_event(iio_dev, event,
522 st_lsm6dsr_get_time_ns());
523 }
524 if (status[1] & ST_LSM6DSR_REG_INT_ORIENTATION_MASK) {
525 struct st_lsm6dsr_sensor *sensor;
526
527 iio_dev = hw->iio_devs[ST_LSM6DSR_ID_ORIENTATION];
528 sensor = iio_priv(iio_dev);
529 iio_trigger_poll_chained(sensor->trig);
530 }
531 if (status[1] & ST_LSM6DSR_REG_INT_WRIST_MASK) {
532 iio_dev = hw->iio_devs[ST_LSM6DSR_ID_WRIST_TILT];
533 event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1,
534 IIO_EV_TYPE_THRESH,
535 IIO_EV_DIR_RISING);
536 iio_push_event(iio_dev, event,
537 st_lsm6dsr_get_time_ns());
538 }
539 }
540
541 return 0;
542 }
543
544 /**
545 * Return the max FIFO watermark level accepted
546 *
547 * @param dev: Linux Device
548 * @param attr: Device Attribute
549 * @param buf: User Buffer
550 * @return Number of chars printed into the buffer
551 */
st_lsm6dsr_get_max_watermark(struct device * dev,struct device_attribute * attr,char * buf)552 ssize_t st_lsm6dsr_get_max_watermark(struct device *dev,
553 struct device_attribute *attr,
554 char *buf)
555 {
556 struct iio_dev *iio_dev = dev_get_drvdata(dev);
557 struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
558
559 return sprintf(buf, "%d\n", sensor->max_watermark);
560 }
561
562 /**
563 * Return the FIFO watermark level
564 *
565 * @param dev: Linux Device
566 * @param attr: Device Attribute
567 * @param buf: User Buffer
568 * @return Number of chars printed into the buffer
569 */
st_lsm6dsr_get_watermark(struct device * dev,struct device_attribute * attr,char * buf)570 ssize_t st_lsm6dsr_get_watermark(struct device *dev,
571 struct device_attribute *attr,
572 char *buf)
573 {
574 struct iio_dev *iio_dev = dev_get_drvdata(dev);
575 struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
576
577 return sprintf(buf, "%d\n", sensor->watermark);
578 }
579
580 /**
581 * Set the FIFO watermark level
582 *
583 * @param dev: Linux Device
584 * @param attr: Device Attribute
585 * @param buf: User Buffer
586 * @param size: New FIFO watermark level
587 * @return Watermark level if >= 0, error otherwise
588 */
st_lsm6dsr_set_watermark(struct device * dev,struct device_attribute * attr,const char * buf,size_t size)589 ssize_t st_lsm6dsr_set_watermark(struct device *dev,
590 struct device_attribute *attr,
591 const char *buf, size_t size)
592 {
593 struct iio_dev *iio_dev = dev_get_drvdata(dev);
594 struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
595 int err, val;
596
597 mutex_lock(&iio_dev->mlock);
598
599 err = kstrtoint(buf, 10, &val);
600 if (err < 0)
601 goto out;
602
603 err = st_lsm6dsr_update_watermark(sensor, val);
604 if (err < 0)
605 goto out;
606
607 sensor->watermark = val;
608
609 out:
610 mutex_unlock(&iio_dev->mlock);
611
612 return err < 0 ? err : size;
613 }
614
615 /**
616 * Flush internal HW FIFO
617 *
618 * @param dev: Linux Device
619 * @param attr: Device Attribute
620 * @param buf: User Buffer
621 * @param size: unused
622 * @return Watermark level if >= 0, error otherwise
623 */
st_lsm6dsr_flush_fifo(struct device * dev,struct device_attribute * attr,const char * buf,size_t size)624 ssize_t st_lsm6dsr_flush_fifo(struct device *dev,
625 struct device_attribute *attr,
626 const char *buf, size_t size)
627 {
628 struct iio_dev *iio_dev = dev_get_drvdata(dev);
629 struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
630 struct st_lsm6dsr_hw *hw = sensor->hw;
631 int count;
632 s64 ts;
633 #ifdef CONFIG_NO_GKI
634 s64 type;
635 s64 event;
636 s64 fts;
637 #endif
638
639 mutex_lock(&hw->fifo_lock);
640 ts = st_lsm6dsr_get_time_ns();
641 hw->delta_ts = ts - hw->ts;
642 hw->ts = ts;
643 set_bit(ST_LSM6DSR_HW_FLUSH, &hw->state);
644 count = st_lsm6dsr_read_fifo(hw);
645 sensor->dec_counter = 0;
646 mutex_unlock(&hw->fifo_lock);
647
648 #ifdef CONFIG_NO_GKI
649 if (count > 0)
650 fts = sensor->last_fifo_timestamp;
651 else
652 fts = ts;
653
654 type = count > 0 ? IIO_EV_DIR_FIFO_DATA : IIO_EV_DIR_FIFO_EMPTY;
655 event = IIO_UNMOD_EVENT_CODE(iio_dev->channels[0].type, -1,
656 IIO_EV_TYPE_FIFO_FLUSH, type);
657 iio_push_event(iio_dev, event, fts);
658 #endif
659
660 return size;
661 }
662
663 /**
664 * Empty FIFO and set HW FIFO in Bypass mode
665 *
666 * @param hw: ST IMU MEMS hw instance
667 * @return Watermark level if >= 0, error otherwise
668 */
st_lsm6dsr_suspend_fifo(struct st_lsm6dsr_hw * hw)669 int st_lsm6dsr_suspend_fifo(struct st_lsm6dsr_hw *hw)
670 {
671 int err;
672
673 mutex_lock(&hw->fifo_lock);
674 st_lsm6dsr_read_fifo(hw);
675 err = st_lsm6dsr_set_fifo_mode(hw, ST_LSM6DSR_FIFO_BYPASS);
676 mutex_unlock(&hw->fifo_lock);
677
678 return err;
679 }
680
681 /**
682 * Update ODR batching in FIFO and Timestamp
683 *
684 * @param iio_dev: Linux IIO device
685 * @param enable: enable/disable batcing in FIFO
686 * @return < 0 if error, 0 otherwise
687 */
st_lsm6dsr_update_batching(struct iio_dev * iio_dev,bool enable)688 int st_lsm6dsr_update_batching(struct iio_dev *iio_dev, bool enable)
689 {
690 struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
691 struct st_lsm6dsr_hw *hw = sensor->hw;
692 int err;
693
694 disable_irq(hw->irq);
695
696 err = st_lsm6dsr_set_sensor_batching_odr(sensor, enable);
697 if (err < 0)
698 goto out;
699
700 /* Calc TS ODR */
701 hw->odr = st_lsm6dsr_ts_odr(hw);
702
703 out:
704 enable_irq(hw->irq);
705
706 return err;
707 }
708
709 /**
710 * Update FIFO watermark value based to the enabled sensors
711 *
712 * @param iio_dev: Linux IIO device
713 * @param enable: enable/disable batcing in FIFO
714 * @return < 0 if error, 0 otherwise
715 */
st_lsm6dsr_update_fifo(struct iio_dev * iio_dev,bool enable)716 static int st_lsm6dsr_update_fifo(struct iio_dev *iio_dev, bool enable)
717 {
718 struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
719 struct st_lsm6dsr_hw *hw = sensor->hw;
720 int err;
721 int podr, puodr;
722
723 disable_irq(hw->irq);
724
725 if (sensor->id == ST_LSM6DSR_ID_EXT0 ||
726 sensor->id == ST_LSM6DSR_ID_EXT1) {
727 err = st_lsm6dsr_shub_set_enable(sensor, enable);
728 if (err < 0)
729 goto out;
730 } else {
731 if (sensor->id == ST_LSM6DSR_ID_STEP_COUNTER) {
732 err = st_lsm6dsr_step_counter_set_enable(sensor,
733 enable);
734 if (err < 0)
735 goto out;
736 } else {
737 err = st_lsm6dsr_sensor_set_enable(sensor, enable);
738 if (err < 0)
739 goto out;
740
741 err = st_lsm6dsr_set_sensor_batching_odr(sensor,
742 enable);
743 if (err < 0)
744 goto out;
745 }
746 }
747
748 /*
749 * this is an auxiliary sensor, it need to get batched
750 * together at least with a primary sensor (Acc/Gyro)
751 */
752 if (sensor->id == ST_LSM6DSR_ID_TEMP) {
753 if (!(hw->enable_mask & (BIT(ST_LSM6DSR_ID_ACC) |
754 BIT(ST_LSM6DSR_ID_GYRO)))) {
755 struct st_lsm6dsr_sensor *acc_sensor;
756 u8 data = 0;
757
758 acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSR_ID_ACC]);
759 if (enable) {
760 err = st_lsm6dsr_get_odr_val(ST_LSM6DSR_ID_ACC,
761 sensor->odr, sensor->uodr,
762 &podr, &puodr, &data);
763 if (err < 0)
764 goto out;
765 }
766
767 err = st_lsm6dsr_write_with_mask(hw,
768 acc_sensor->batch_reg.addr,
769 acc_sensor->batch_reg.mask,
770 data);
771 if (err < 0)
772 goto out;
773 }
774 }
775
776 err = st_lsm6dsr_update_watermark(sensor, sensor->watermark);
777 if (err < 0)
778 goto out;
779
780 /* Calc TS ODR */
781 hw->odr = st_lsm6dsr_ts_odr(hw);
782
783 if (enable && hw->fifo_mode == ST_LSM6DSR_FIFO_BYPASS) {
784 st_lsm6dsr_reset_hwts(hw);
785 err = st_lsm6dsr_set_fifo_mode(hw, ST_LSM6DSR_FIFO_CONT);
786 } else if (!hw->enable_mask) {
787 err = st_lsm6dsr_set_fifo_mode(hw, ST_LSM6DSR_FIFO_BYPASS);
788 }
789
790 out:
791 enable_irq(hw->irq);
792
793 return err;
794 }
795
796 /**
797 * Bottom handler for FSM Orientation sensor event generation
798 *
799 * @param irq: IIO trigger irq number
800 * @param p: iio poll function environment
801 * @return IRQ_HANDLED or < 0 for error
802 */
st_lsm6dsr_buffer_handler_thread(int irq,void * p)803 static irqreturn_t st_lsm6dsr_buffer_handler_thread(int irq, void *p)
804 {
805 struct iio_poll_func *pf = p;
806 struct iio_dev *iio_dev = pf->indio_dev;
807 struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
808 u8 buffer[sizeof(u8) + sizeof(s64)];
809 int err;
810
811 err = st_lsm6dsr_fsm_get_orientation(sensor->hw, buffer);
812 if (err < 0)
813 goto out;
814
815 iio_push_to_buffers_with_timestamp(iio_dev, buffer,
816 st_lsm6dsr_get_time_ns());
817 out:
818 iio_trigger_notify_done(sensor->trig);
819
820 return IRQ_HANDLED;
821 }
822
823 /**
824 * Top handler for sensor event generation + FIFO management
825 *
826 * @param irq: IIO trigger irq number
827 * @param private: iio poll function environment
828 * @return IRQ_HANDLED or < 0 for error
829 */
st_lsm6dsr_handler_irq(int irq,void * private)830 static irqreturn_t st_lsm6dsr_handler_irq(int irq, void *private)
831 {
832 struct st_lsm6dsr_hw *hw = (struct st_lsm6dsr_hw *)private;
833 s64 ts = st_lsm6dsr_get_time_ns();
834
835 hw->delta_ts = ts - hw->ts;
836 hw->ts = ts;
837
838 return IRQ_WAKE_THREAD;
839 }
840
841 /**
842 * Bottom handler for sensor event generation + FIFO management
843 *
844 * @param irq: irq line number
845 * @param private: device private environment pointer
846 * @return IRQ_HANDLED or < 0 for error
847 */
st_lsm6dsr_handler_thread(int irq,void * private)848 static irqreturn_t st_lsm6dsr_handler_thread(int irq, void *private)
849 {
850 struct st_lsm6dsr_hw *hw = (struct st_lsm6dsr_hw *)private;
851
852 mutex_lock(&hw->fifo_lock);
853 st_lsm6dsr_read_fifo(hw);
854 clear_bit(ST_LSM6DSR_HW_FLUSH, &hw->state);
855 mutex_unlock(&hw->fifo_lock);
856
857 st_lsm6dsr_report_events(hw);
858
859 return IRQ_HANDLED;
860 }
861
862 /**
863 * IIO fifo pre enabled callback function
864 *
865 * @param iio_dev: IIO device
866 * @return < 0 if error, 0 otherwise
867 */
st_lsm6dsr_fifo_preenable(struct iio_dev * iio_dev)868 static int st_lsm6dsr_fifo_preenable(struct iio_dev *iio_dev)
869 {
870 return st_lsm6dsr_update_fifo(iio_dev, true);
871 }
872
873 /**
874 * IIO fifo post disable callback function
875 *
876 * @param iio_dev: IIO device
877 * @return < 0 if error, 0 otherwise
878 */
st_lsm6dsr_fifo_postdisable(struct iio_dev * iio_dev)879 static int st_lsm6dsr_fifo_postdisable(struct iio_dev *iio_dev)
880 {
881 return st_lsm6dsr_update_fifo(iio_dev, false);
882 }
883
884 /**
885 * IIO fifo callback registruction structure
886 */
887 static const struct iio_buffer_setup_ops st_lsm6dsr_fifo_ops = {
888 .preenable = st_lsm6dsr_fifo_preenable,
889 .postdisable = st_lsm6dsr_fifo_postdisable,
890 };
891
892 /**
893 * Enable HW FIFO
894 *
895 * @param hw: ST IMU MEMS hw instance
896 * @return < 0 if error, 0 otherwise
897 */
st_lsm6dsr_fifo_init(struct st_lsm6dsr_hw * hw)898 static int st_lsm6dsr_fifo_init(struct st_lsm6dsr_hw *hw)
899 {
900 return st_lsm6dsr_write_with_mask(hw,
901 ST_LSM6DSR_REG_FIFO_CTRL4_ADDR,
902 ST_LSM6DSR_REG_DEC_TS_MASK, 1);
903 }
904
905 static const struct iio_trigger_ops st_lsm6dsr_trigger_ops;
906
st_lsm6dsr_buffer_preenable(struct iio_dev * iio_dev)907 static int st_lsm6dsr_buffer_preenable(struct iio_dev *iio_dev)
908 {
909 return st_lsm6dsr_embfunc_sensor_set_enable(iio_priv(iio_dev), true);
910 }
911
st_lsm6dsr_buffer_postdisable(struct iio_dev * iio_dev)912 static int st_lsm6dsr_buffer_postdisable(struct iio_dev *iio_dev)
913 {
914 return st_lsm6dsr_embfunc_sensor_set_enable(iio_priv(iio_dev), false);
915 }
916
917 static const struct iio_buffer_setup_ops st_lsm6dsr_buffer_ops = {
918 .preenable = st_lsm6dsr_buffer_preenable,
919 .postenable = NULL,
920 .predisable = NULL,
921 .postdisable = st_lsm6dsr_buffer_postdisable,
922 };
923
924
925 /**
926 * Init IRQ
927 *
928 * @param hw: ST IMU MEMS hw instance
929 * @return < 0 if error, 0 otherwise
930 */
st_lsm6dsr_irq_setup(struct st_lsm6dsr_hw * hw)931 int st_lsm6dsr_irq_setup(struct st_lsm6dsr_hw *hw)
932 {
933 unsigned long irq_type;
934 bool irq_active_low;
935 int err;
936
937 irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));
938 if (irq_type == IRQF_TRIGGER_NONE)
939 irq_type = IRQF_TRIGGER_HIGH;
940
941 switch (irq_type) {
942 case IRQF_TRIGGER_HIGH:
943 case IRQF_TRIGGER_RISING:
944 irq_active_low = false;
945 break;
946 case IRQF_TRIGGER_LOW:
947 case IRQF_TRIGGER_FALLING:
948 irq_active_low = true;
949 break;
950 default:
951 dev_info(hw->dev, "mode %lx unsupported\n", irq_type);
952 return -EINVAL;
953 }
954
955 err = st_lsm6dsr_write_with_mask(hw, ST_LSM6DSR_REG_CTRL3_C_ADDR,
956 ST_LSM6DSR_REG_H_LACTIVE_MASK,
957 irq_active_low);
958 if (err < 0)
959 return err;
960
961 err = st_lsm6dsr_write_with_mask(hw, ST_LSM6DSR_REG_INT1_CTRL_ADDR,
962 ST_LSM6DSR_REG_INT_FIFO_TH_MASK, 1);
963 if (err < 0)
964 return err;
965
966 return 0;
967 }
968
969 /**
970 * Init IIO buffers and triggers
971 *
972 * @param hw: ST IMU MEMS hw instance
973 * @return < 0 if error, 0 otherwise
974 */
st_lsm6dsr_buffers_setup(struct st_lsm6dsr_hw * hw)975 int st_lsm6dsr_buffers_setup(struct st_lsm6dsr_hw *hw)
976 {
977 struct device_node *np = hw->dev->of_node;
978 struct st_lsm6dsr_sensor *sensor;
979 struct iio_buffer *buffer;
980 struct iio_dev *iio_dev;
981 unsigned long irq_type;
982 int i, err;
983
984 err = st_lsm6dsr_irq_setup(hw);
985 if (err < 0)
986 return err;
987
988 irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));
989 if (irq_type == IRQF_TRIGGER_NONE)
990 irq_type = IRQF_TRIGGER_HIGH;
991
992 if (np && of_property_read_bool(np, "drive-open-drain")) {
993 err = st_lsm6dsr_write_with_mask(hw,
994 ST_LSM6DSR_REG_CTRL3_C_ADDR,
995 ST_LSM6DSR_REG_PP_OD_MASK, 1);
996 if (err < 0)
997 return err;
998
999 irq_type |= IRQF_SHARED;
1000 }
1001
1002 err = devm_request_threaded_irq(hw->dev, hw->irq,
1003 st_lsm6dsr_handler_irq,
1004 st_lsm6dsr_handler_thread,
1005 irq_type | IRQF_ONESHOT,
1006 "lsm6dsr", hw);
1007 if (err) {
1008 dev_err(hw->dev, "failed to request trigger irq %d\n",
1009 hw->irq);
1010 return err;
1011 }
1012
1013 for (i = ST_LSM6DSR_ID_GYRO; i <= ST_LSM6DSR_ID_SIGN_MOTION; i++) {
1014 if (!hw->iio_devs[i])
1015 continue;
1016
1017 buffer = devm_iio_kfifo_allocate(hw->dev);
1018 if (!buffer)
1019 return -ENOMEM;
1020
1021 iio_device_attach_buffer(hw->iio_devs[i], buffer);
1022 hw->iio_devs[i]->modes |= INDIO_BUFFER_SOFTWARE;
1023 hw->iio_devs[i]->setup_ops = &st_lsm6dsr_fifo_ops;
1024 }
1025
1026 err = st_lsm6dsr_fifo_init(hw);
1027 if (err < 0)
1028 return err;
1029
1030 iio_dev = hw->iio_devs[ST_LSM6DSR_ID_ORIENTATION];
1031 sensor = iio_priv(iio_dev);
1032
1033 err = iio_triggered_buffer_setup(iio_dev,
1034 NULL, st_lsm6dsr_buffer_handler_thread,
1035 &st_lsm6dsr_buffer_ops);
1036 if (err < 0)
1037 return err;
1038
1039 sensor->trig = devm_iio_trigger_alloc(hw->dev, "%s-trigger",
1040 iio_dev->name);
1041 if (!sensor->trig)
1042 return -ENOMEM;
1043
1044 iio_trigger_set_drvdata(sensor->trig, iio_dev);
1045 sensor->trig->ops = &st_lsm6dsr_trigger_ops;
1046 sensor->trig->dev.parent = hw->dev;
1047 sensor->trig->owner = THIS_MODULE;
1048 iio_dev->trig = iio_trigger_get(sensor->trig);
1049
1050 err = iio_trigger_register(sensor->trig);
1051 if (err < 0)
1052 iio_triggered_buffer_cleanup(iio_dev);
1053
1054 return err;
1055 }
1056
st_lsm6dsr_deallocate_buffers(struct st_lsm6dsr_hw * hw)1057 int st_lsm6dsr_deallocate_buffers(struct st_lsm6dsr_hw *hw)
1058 {
1059 struct iio_dev *iio_dev = hw->iio_devs[ST_LSM6DSR_ID_ORIENTATION];
1060 struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
1061
1062 iio_trigger_unregister(sensor->trig);
1063 iio_triggered_buffer_cleanup(iio_dev);
1064
1065 return 0;
1066 }
1067
1068