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