1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3 * STMicroelectronics st_lsm6dsr embedded function sensor driver
4 *
5 * Copyright 2019 STMicroelectronics Inc.
6 *
7 * Lorenzo Bianconi <lorenzo.bianconi@st.com>
8 */
9
10 #include <linux/kernel.h>
11 #include <linux/module.h>
12 #include <linux/iio/iio.h>
13
14 #include "st_lsm6dsr.h"
15
16 #define ST_LSM6DSR_REG_PAGE_SEL_ADDR 0x02
17 #define ST_LSM6DSR_REG_PAGE_SEL_RST_MASK BIT(0)
18
19 #define ST_LSM6DSR_REG_EMB_FUNC_EN_A_ADDR 0x04
20 #define ST_LSM6DSR_REG_PAGE_ADDRESS 0x08
21 #define ST_LSM6DSR_REG_PAGE_VALUE 0x09
22 #define ST_LSM6DSR_FSM_BASE_ADDRESS 0x2da
23
24 #define ST_LSM6DSR_REG_PEDO_EN_MASK BIT(3)
25 #define ST_LSM6DSR_REG_TILT_EN_MASK BIT(4)
26 #define ST_LSM6DSR_REG_SIGN_MOTION_EN_MASK BIT(5)
27
28 #define ST_LSM6DSR_REG_INT_DTAP_MASK BIT(3)
29 #define ST_LSM6DSR_REG_INT_STAP_MASK BIT(6)
30
31 #define ST_LSM6DSR_REG_EMB_FUNC_EN_B_ADDR 0x05
32 #define ST_LSM6DSR_REG_FSM_EN_MASK BIT(0)
33 #define ST_LSM6DSR_REG_INT_STEP_DET_MASK BIT(3)
34 #define ST_LSM6DSR_REG_INT_TILT_MASK BIT(4)
35 #define ST_LSM6DSR_REG_INT_SIGMOT_MASK BIT(5)
36
37 #define ST_LSM6DSR_PAGE_RW_ADDR 0x17
38 #define ST_LSM6DSR_REG_WR_MASK GENMASK(6, 5)
39 #define ST_LSM6DSR_REG_EMB_FUNC_LIR_MASK BIT(7)
40
41 #define ST_LSM6DSR_REG_EMB_FUNC_FIFO_CFG_ADDR 0x44
42 #define ST_LSM6DSR_REG_PEDO_FIFO_EN_MASK BIT(6)
43
44 #define ST_LSM6DSR_REG_FSM_ENABLE_A_ADDR 0x46
45 #define ST_LSM6DSR_REG_FSM_OUTS6_ADDR 0x51
46 #define ST_LSM6DSR_REG_ORIENTATION_0_MASK BIT(5)
47 #define ST_LSM6DSR_REG_ORIENTATION_90_MASK BIT(7)
48 #define ST_LSM6DSR_REG_ORIENTATION_180_MASK BIT(4)
49 #define ST_LSM6DSR_REG_ORIENTATION_270_MASK BIT(6)
50
51 /* Finite State Machine ODR configuration */
52 #define ST_LSM6DSR_REG_EMB_FUNC_ODR_CFG_B_ADDR 0x5f
53 #define ST_LSM6DSR_REG_FSM_ODR_MASK GENMASK(5, 3)
54 #define ST_LSM6DSR_FSM_ODR_12_5 0
55 #define ST_LSM6DSR_FSM_ODR_26 1
56 #define ST_LSM6DSR_FSM_ODR_52 2
57 #define ST_LSM6DSR_FSM_ODR_104 3
58 #define ST_LSM6DSR_FSM_ODR_208 4
59 #define ST_LSM6DSR_FSM_ODR_416 5
60
61 #define ST_LSM6DSR_REG_STEP_COUNTER_L_ADDR 0x62
62 #define ST_LSM6DSR_REG_EMB_FUNC_SRC_ADDR 0x64
63 #define ST_LSM6DSR_REG_PEDO_RST_STEP_MASK BIT(7)
64
65 #define ST_LSM6DSR_FSM_MAX_SIZE 255
66
67 /**
68 * @struct st_lsm6dsr_fsm_sensor
69 * @brief Single FSM description entry
70 *
71 * Implements #595543 Feature
72 *
73 * The following FSM state machine LSM6DSR features listed in EX_FUN_FSM_SENSOR:
74 *
75 * SENSOR_TYPE_GLANCE_GESTURE
76 * SENSOR_TYPE_MOTION_DETECT
77 * SENSOR_TYPE_STATIONARY_DETECT
78 * SENSOR_TYPE_WAKE_GESTURE
79 * SENSOR_TYPE_PICK_UP_GESTURE
80 * SENSOR_TYPE_WRIST_TILT_GESTURE
81 *
82 * will be managed as event sensors
83 *
84 * data: FSM binary data block.
85 * id: Sensor Identifier.
86 * FSM binary data block len.
87 */
88 struct st_lsm6dsr_fsm_sensor {
89 u8 data[ST_LSM6DSR_FSM_MAX_SIZE];
90 enum st_lsm6dsr_sensor_id id;
91 u16 len;
92 };
93
94 static const struct st_lsm6dsr_fsm_sensor st_lsm6dsr_fsm_sensor_list[] = {
95 /* glance */
96 {
97 .id = ST_LSM6DSR_ID_GLANCE,
98 .data = {
99 0xb2, 0x10, 0x24, 0x20, 0x17, 0x17, 0x66, 0x32,
100 0x66, 0x3c, 0x20, 0x20, 0x02, 0x02, 0x08, 0x08,
101 0x00, 0x04, 0x0c, 0x00, 0xc7, 0x66, 0x33, 0x73,
102 0x77, 0x64, 0x88, 0x75, 0x99, 0x66, 0x33, 0x53,
103 0x44, 0xf5, 0x22, 0x00,
104 },
105 .len = 36,
106 },
107 /* motion */
108 {
109 .id = ST_LSM6DSR_ID_MOTION,
110 .data = {
111 0x51, 0x10, 0x16, 0x00, 0x00, 0x00, 0x66, 0x3c,
112 0x02, 0x00, 0x00, 0x7d, 0x00, 0xc7, 0x05, 0x99,
113 0x33, 0x53, 0x44, 0xf5, 0x22, 0x00,
114 },
115 .len = 22,
116 },
117 /* no motion */
118 {
119 .id = ST_LSM6DSR_ID_NO_MOTION,
120 .data = {
121 0x51, 0x00, 0x10, 0x00, 0x00, 0x00, 0x66, 0x3c,
122 0x02, 0x00, 0x00, 0x7d, 0xff, 0x53, 0x99, 0x50,
123 },
124 .len = 16,
125 },
126 /* wakeup */
127 {
128 .id = ST_LSM6DSR_ID_WAKEUP,
129 .data = {
130 0xe2, 0x00, 0x1e, 0x20, 0x13, 0x15, 0x66, 0x3e,
131 0x66, 0xbe, 0xcd, 0x3c, 0xc0, 0xc0, 0x02, 0x02,
132 0x0b, 0x10, 0x05, 0x66, 0xcc, 0x35, 0x38, 0x35,
133 0x77, 0xdd, 0x03, 0x54, 0x22, 0x00,
134 },
135 .len = 30,
136 },
137 /* pickup */
138 {
139 .id = ST_LSM6DSR_ID_PICKUP,
140 .data = {
141 0x51, 0x00, 0x10, 0x00, 0x00, 0x00, 0x33, 0x3c,
142 0x02, 0x00, 0x00, 0x05, 0x05, 0x99, 0x30, 0x00,
143 },
144 .len = 16,
145 },
146 /* orientation */
147 {
148 .id = ST_LSM6DSR_ID_ORIENTATION,
149 .data = {
150 0x91, 0x10, 0x16, 0x00, 0x00, 0x00, 0x66, 0x3a,
151 0x66, 0x32, 0xf0, 0x00, 0x00, 0x0d, 0x00, 0xc7,
152 0x05, 0x73, 0x99, 0x08, 0xf5, 0x22,
153 },
154 .len = 22,
155 },
156 /* wrist tilt */
157 {
158 .id = ST_LSM6DSR_ID_WRIST_TILT,
159 .data = {
160 0x52, 0x00, 0x14, 0x00, 0x00, 0x00, 0xae, 0xb7,
161 0x80, 0x00, 0x00, 0x06, 0x0f, 0x05, 0x73, 0x33,
162 0x07, 0x54, 0x44, 0x22,
163 },
164 .len = 20,
165 },
166 };
167
168 struct st_lsm6dsr_fsm_fs {
169 u32 gain;
170 __le16 val;
171 };
172
173 static const struct st_lsm6dsr_fsm_fs st_lsm6dsr_fsm_fs_table[] = {
174 { ST_LSM6DSR_ACC_FS_2G_GAIN, 0x03ff },
175 { ST_LSM6DSR_ACC_FS_4G_GAIN, 0x07fe },
176 { ST_LSM6DSR_ACC_FS_8G_GAIN, 0x0bfe },
177 { ST_LSM6DSR_ACC_FS_16G_GAIN, 0x0ffe },
178 };
179
180 static inline
st_lsm6dsr_fsm_set_access(struct st_lsm6dsr_hw * hw,bool enable)181 int st_lsm6dsr_fsm_set_access(struct st_lsm6dsr_hw *hw, bool enable)
182 {
183 u8 val = enable ? 2 : 0;
184
185 return __st_lsm6dsr_write_with_mask(hw,
186 ST_LSM6DSR_PAGE_RW_ADDR,
187 ST_LSM6DSR_REG_WR_MASK,
188 val);
189 }
190
st_lsm6dsr_fsm_write(struct st_lsm6dsr_hw * hw,u16 base_addr,int len,const u8 * data)191 static int st_lsm6dsr_fsm_write(struct st_lsm6dsr_hw *hw, u16 base_addr,
192 int len, const u8 *data)
193 {
194 u8 msb, lsb;
195 int i, err;
196
197 msb = (((base_addr >> 8) & 0xf) << 4) | 1;
198 lsb = base_addr & 0xff;
199
200 err = hw->tf->write(hw->dev,
201 ST_LSM6DSR_REG_PAGE_ADDRESS,
202 sizeof(lsb),
203 &lsb);
204 if (err < 0)
205 return err;
206
207 err = hw->tf->write(hw->dev,
208 ST_LSM6DSR_REG_PAGE_SEL_ADDR,
209 sizeof(msb),
210 &msb);
211 if (err < 0)
212 return err;
213
214 for (i = 0; i < len; i++) {
215 err = hw->tf->write(hw->dev,
216 ST_LSM6DSR_REG_PAGE_VALUE,
217 sizeof(u8),
218 &data[i]);
219 if (err < 0)
220 return err;
221
222 if (++lsb == 0) {
223 msb += (1 << 4);
224 err = hw->tf->write(hw->dev,
225 ST_LSM6DSR_REG_PAGE_SEL_ADDR,
226 sizeof(msb),
227 &msb);
228 if (err < 0)
229 return err;
230 }
231 }
232
233 return err;
234 }
235
st_lsm6dsr_ef_pg1_sensor_set_enable(struct st_lsm6dsr_sensor * sensor,u8 mask,u8 irq_mask,bool enable)236 static int st_lsm6dsr_ef_pg1_sensor_set_enable(struct st_lsm6dsr_sensor *sensor,
237 u8 mask, u8 irq_mask,
238 bool enable)
239 {
240 struct st_lsm6dsr_hw *hw = sensor->hw;
241 int err;
242
243 err = st_lsm6dsr_sensor_set_enable(sensor, enable);
244 if (err < 0)
245 return err;
246
247 mutex_lock(&hw->page_lock);
248 err = st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK,
249 true);
250 if (err < 0)
251 goto unlock;
252
253 err = __st_lsm6dsr_write_with_mask(hw,
254 ST_LSM6DSR_REG_EMB_FUNC_EN_A_ADDR,
255 mask, enable);
256 if (err < 0)
257 goto reset_page;
258
259 err = __st_lsm6dsr_write_with_mask(hw, hw->embfunc_irq_reg, irq_mask,
260 enable);
261 reset_page:
262 st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, false);
263 unlock:
264 mutex_unlock(&hw->page_lock);
265
266 return err;
267 }
268
269 /**
270 * FSM Function sensor [FSM_FUN]
271 *
272 * @param sensor: ST IMU sensor instance
273 * @param enable: Enable/Disable sensor
274 * @return < 0 if error, 0 otherwise
275 */
st_lsm6dsr_fsm_set_enable(struct st_lsm6dsr_sensor * sensor,bool enable)276 static int st_lsm6dsr_fsm_set_enable(struct st_lsm6dsr_sensor *sensor,
277 bool enable)
278 {
279 struct st_lsm6dsr_hw *hw = sensor->hw;
280 u16 enable_mask = hw->fsm_enable_mask;
281 int err, i;
282
283 for (i = 0; i < ARRAY_SIZE(st_lsm6dsr_fsm_sensor_list); i++)
284 if (st_lsm6dsr_fsm_sensor_list[i].id == sensor->id)
285 break;
286
287 if (i == ARRAY_SIZE(st_lsm6dsr_fsm_sensor_list))
288 return -EINVAL;
289
290 err = st_lsm6dsr_sensor_set_enable(sensor, enable);
291 if (err < 0)
292 return err;
293
294 mutex_lock(&hw->page_lock);
295 err = st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK,
296 true);
297 if (err < 0)
298 goto unlock;
299
300 if (enable)
301 enable_mask |= BIT(i);
302 else
303 enable_mask &= ~BIT(i);
304
305 err = hw->tf->write(hw->dev, ST_LSM6DSR_REG_FSM_ENABLE_A_ADDR,
306 sizeof(enable_mask), (u8 *)&enable_mask);
307 if (err < 0)
308 goto reset_page;
309
310 hw->fsm_enable_mask = enable_mask;
311
312 reset_page:
313 st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, false);
314 unlock:
315 mutex_unlock(&hw->page_lock);
316
317 return err;
318 }
319
320 /**
321 * Enable Embedded Function sensor [EMB_FUN]
322 *
323 * @param sensor: ST IMU sensor instance
324 * @param enable: Enable/Disable sensor
325 * @return < 0 if error, 0 otherwise
326 */
st_lsm6dsr_embfunc_sensor_set_enable(struct st_lsm6dsr_sensor * sensor,bool enable)327 int st_lsm6dsr_embfunc_sensor_set_enable(struct st_lsm6dsr_sensor *sensor,
328 bool enable)
329 {
330 int err;
331
332 switch (sensor->id) {
333 case ST_LSM6DSR_ID_STEP_DETECTOR:
334 err = st_lsm6dsr_ef_pg1_sensor_set_enable(sensor,
335 ST_LSM6DSR_REG_PEDO_EN_MASK,
336 ST_LSM6DSR_REG_INT_STEP_DET_MASK,
337 enable);
338 break;
339 case ST_LSM6DSR_ID_SIGN_MOTION:
340 err = st_lsm6dsr_ef_pg1_sensor_set_enable(sensor,
341 ST_LSM6DSR_REG_SIGN_MOTION_EN_MASK,
342 ST_LSM6DSR_REG_INT_SIGMOT_MASK,
343 enable);
344 break;
345 case ST_LSM6DSR_ID_TILT:
346 err = st_lsm6dsr_ef_pg1_sensor_set_enable(sensor,
347 ST_LSM6DSR_REG_TILT_EN_MASK,
348 ST_LSM6DSR_REG_TILT_EN_MASK,
349 enable);
350 break;
351 case ST_LSM6DSR_ID_NO_MOTION:
352 case ST_LSM6DSR_ID_MOTION:
353 case ST_LSM6DSR_ID_WAKEUP:
354 case ST_LSM6DSR_ID_PICKUP:
355 case ST_LSM6DSR_ID_ORIENTATION:
356 case ST_LSM6DSR_ID_WRIST_TILT:
357 case ST_LSM6DSR_ID_GLANCE:
358 err = st_lsm6dsr_fsm_set_enable(sensor, enable);
359 break;
360 default:
361 err = -EINVAL;
362 break;
363 }
364
365 return err;
366 }
367
368 /**
369 * Enable Step Counter Sensor [EMB_FUN]
370 *
371 * @param sensor: ST IMU sensor instance
372 * @param enable: Enable/Disable sensor
373 * @return < 0 if error, 0 otherwise
374 */
st_lsm6dsr_step_counter_set_enable(struct st_lsm6dsr_sensor * sensor,bool enable)375 int st_lsm6dsr_step_counter_set_enable(struct st_lsm6dsr_sensor *sensor,
376 bool enable)
377 {
378 struct st_lsm6dsr_hw *hw = sensor->hw;
379 int err;
380
381 err = st_lsm6dsr_sensor_set_enable(sensor, enable);
382 if (err < 0)
383 return err;
384
385 mutex_lock(&hw->page_lock);
386 err = st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK,
387 true);
388 if (err < 0)
389 goto unlock;
390
391 err = __st_lsm6dsr_write_with_mask(hw,
392 ST_LSM6DSR_REG_EMB_FUNC_EN_A_ADDR,
393 ST_LSM6DSR_REG_PEDO_EN_MASK,
394 enable);
395 if (err < 0)
396 goto reset_page;
397
398 err = __st_lsm6dsr_write_with_mask(hw,
399 ST_LSM6DSR_REG_EMB_FUNC_FIFO_CFG_ADDR,
400 ST_LSM6DSR_REG_PEDO_FIFO_EN_MASK,
401 enable);
402
403 reset_page:
404 st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, false);
405 unlock:
406 mutex_unlock(&hw->page_lock);
407
408 return err;
409 }
410
411 /**
412 * Reset Step Counter value [EMB_FUN]
413 *
414 * @param iio_dev: IIO device
415 * @return < 0 if error, 0 otherwise
416 */
st_lsm6dsr_reset_step_counter(struct iio_dev * iio_dev)417 int st_lsm6dsr_reset_step_counter(struct iio_dev *iio_dev)
418 {
419 struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
420 struct st_lsm6dsr_hw *hw = sensor->hw;
421 u16 prev_val, val = 0;
422 __le16 data;
423 int err;
424
425 mutex_lock(&iio_dev->mlock);
426 if (iio_buffer_enabled(iio_dev)) {
427 err = -EBUSY;
428 goto unlock_iio_dev;
429 }
430
431 err = st_lsm6dsr_step_counter_set_enable(sensor, true);
432 if (err < 0)
433 goto unlock_iio_dev;
434
435 mutex_lock(&hw->page_lock);
436 err = st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK,
437 true);
438 if (err < 0)
439 goto unlock_page;
440
441 do {
442 prev_val = val;
443 err = __st_lsm6dsr_write_with_mask(hw,
444 ST_LSM6DSR_REG_EMB_FUNC_SRC_ADDR,
445 ST_LSM6DSR_REG_PEDO_RST_STEP_MASK, 1);
446 if (err < 0)
447 goto reset_page;
448
449 msleep(100);
450
451 err = hw->tf->read(hw->dev,
452 ST_LSM6DSR_REG_STEP_COUNTER_L_ADDR,
453 sizeof(data), (u8 *)&data);
454 if (err < 0)
455 goto reset_page;
456
457 val = le16_to_cpu(data);
458 } while (val && val >= prev_val);
459
460 reset_page:
461 st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, false);
462 unlock_page:
463 mutex_unlock(&hw->page_lock);
464
465 err = st_lsm6dsr_step_counter_set_enable(sensor, false);
466 unlock_iio_dev:
467 mutex_unlock(&iio_dev->mlock);
468
469 return err;
470 }
471
472 /**
473 * Read Orientation data sensor [EMB_FUN]
474 *
475 * @param hw: ST IMU MEMS hw instance.
476 * @param out: Out data buffer.
477 * @return < 0 if error, 0 otherwise
478 */
st_lsm6dsr_fsm_get_orientation(struct st_lsm6dsr_hw * hw,u8 * out)479 int st_lsm6dsr_fsm_get_orientation(struct st_lsm6dsr_hw *hw, u8 *out)
480 {
481 int err;
482 u8 data;
483
484 mutex_lock(&hw->page_lock);
485 err = st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK,
486 true);
487 if (err < 0)
488 goto unlock;
489
490 err = hw->tf->read(hw->dev, ST_LSM6DSR_REG_FSM_OUTS6_ADDR,
491 sizeof(data), &data);
492 if (err < 0)
493 goto reset_page;
494
495 switch (data) {
496 case ST_LSM6DSR_REG_ORIENTATION_0_MASK:
497 *out = 0;
498 break;
499 case ST_LSM6DSR_REG_ORIENTATION_90_MASK:
500 *out = 1;
501 break;
502 case ST_LSM6DSR_REG_ORIENTATION_180_MASK:
503 *out = 2;
504 break;
505 case ST_LSM6DSR_REG_ORIENTATION_270_MASK:
506 *out = 3;
507 break;
508 default:
509 err = -EINVAL;
510 break;
511 }
512
513 reset_page:
514 st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, false);
515 unlock:
516 mutex_unlock(&hw->page_lock);
517
518 return err;
519 }
520
521
522 /**
523 * Initialize Finite State Machine HW block [FSM_FUN]
524 *
525 * @param hw: ST IMU MEMS hw instance
526 * @return < 0 if error, 0 otherwise
527 */
st_lsm6dsr_fsm_init(struct st_lsm6dsr_hw * hw)528 int st_lsm6dsr_fsm_init(struct st_lsm6dsr_hw *hw)
529 {
530 u8 nfsm[] = {
531 ARRAY_SIZE(st_lsm6dsr_fsm_sensor_list),
532 ARRAY_SIZE(st_lsm6dsr_fsm_sensor_list)
533 };
534 __le16 irq_mask, fsm_addr = ST_LSM6DSR_FSM_BASE_ADDRESS;
535 u8 val[2] = {};
536 int i, err;
537
538 mutex_lock(&hw->page_lock);
539 err = st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK,
540 true);
541 if (err < 0)
542 goto unlock;
543
544 /* enable gesture rec */
545 err = __st_lsm6dsr_write_with_mask(hw,
546 ST_LSM6DSR_REG_EMB_FUNC_EN_B_ADDR,
547 ST_LSM6DSR_REG_FSM_EN_MASK,
548 1);
549 if (err < 0)
550 goto reset_page;
551
552 /* gest rec ODR 52Hz */
553 err = __st_lsm6dsr_write_with_mask(hw,
554 ST_LSM6DSR_REG_EMB_FUNC_ODR_CFG_B_ADDR,
555 ST_LSM6DSR_REG_FSM_ODR_MASK,
556 ST_LSM6DSR_FSM_ODR_52);
557 if (err < 0)
558 goto reset_page;
559
560 /* disable all fsm sensors */
561 err = hw->tf->write(hw->dev, ST_LSM6DSR_REG_FSM_ENABLE_A_ADDR,
562 sizeof(val), val);
563 if (err < 0)
564 goto reset_page;
565
566 /* enable fsm interrupt */
567 irq_mask = (1 << ARRAY_SIZE(st_lsm6dsr_fsm_sensor_list)) - 1;
568 err = hw->tf->write(hw->dev, hw->embfunc_irq_reg + 1,
569 sizeof(irq_mask),
570 (u8 *)&irq_mask);
571 if (err < 0)
572 goto reset_page;
573
574 /* enable latched interrupts */
575 err = __st_lsm6dsr_write_with_mask(hw,
576 ST_LSM6DSR_PAGE_RW_ADDR,
577 ST_LSM6DSR_REG_EMB_FUNC_LIR_MASK,
578 1);
579 if (err < 0)
580 goto reset_page;
581
582 /* enable access */
583 err = st_lsm6dsr_fsm_set_access(hw, true);
584 if (err < 0)
585 goto reset_page;
586
587 /* # of configured fsm */
588 err = st_lsm6dsr_fsm_write(hw, 0x17c, sizeof(nfsm), nfsm);
589 if (err < 0)
590 goto reset_access;
591
592 err = st_lsm6dsr_fsm_write(hw, 0x17e, sizeof(fsm_addr), (u8 *)
593 &fsm_addr);
594 if (err < 0)
595 goto reset_access;
596
597 /* configure fsm */
598 for (i = 0; i < ARRAY_SIZE(st_lsm6dsr_fsm_sensor_list); i++) {
599 err = st_lsm6dsr_fsm_write(hw, fsm_addr,
600 st_lsm6dsr_fsm_sensor_list[i].len,
601 st_lsm6dsr_fsm_sensor_list[i].data);
602 if (err < 0)
603 goto reset_access;
604
605 fsm_addr += st_lsm6dsr_fsm_sensor_list[i].len;
606 }
607
608 reset_access:
609 st_lsm6dsr_fsm_set_access(hw, false);
610
611 __st_lsm6dsr_write_with_mask(hw,
612 ST_LSM6DSR_REG_PAGE_SEL_ADDR,
613 ST_LSM6DSR_REG_PAGE_SEL_RST_MASK, 1);
614 reset_page:
615 st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, false);
616 unlock:
617 mutex_unlock(&hw->page_lock);
618
619 return err;
620 }
621