1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3 * STMicroelectronics st_lsm6dsr sensor driver
4 *
5 * Copyright 2020 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 #include <linux/iio/sysfs.h>
14 #include <linux/interrupt.h>
15 #include <linux/pm.h>
16 #include<linux/input.h>
17 #include<linux/i2c.h>
18
19 #include <linux/platform_data/st_sensors_pdata.h>
20
21 #include "st_lsm6dsr.h"
22 #include "flat_roll_inward_x_y.h"
23
24 #define ST_LSM6DSR_PM_IMPLEMENTED 1
25 #define ST_LSM6DSR_PM_REGULATOR_CTL 1
26
27 struct st_lsm6dsr_hw *hw_input;
28 struct delayed_work data_work;
29 struct input_dev *acc_input;
30 struct input_dev *gyro_input;
31 struct st_lsm6dsr_sensor *sensor[16];
32 static int lsm6dsr_enable;
33
34 /** Configuration array generated from Unico Tool **/
35 const struct ucf_line_t flat_roll_inward_x_y[] = {
36 {.address = 0x10, .data = 0x00,},
37 {.address = 0x11, .data = 0x00,},
38 {.address = 0x01, .data = 0x80,},
39 {.address = 0x04, .data = 0x00,},
40 {.address = 0x05, .data = 0x00,},
41 {.address = 0x5F, .data = 0x4B,},
42 {.address = 0x46, .data = 0x07,},
43 {.address = 0x47, .data = 0x00,},
44 {.address = 0x0A, .data = 0x00,},
45 {.address = 0x0B, .data = 0x07,},
46 {.address = 0x0C, .data = 0x00,},
47 {.address = 0x0E, .data = 0x00,},
48 {.address = 0x0F, .data = 0x00,},
49 {.address = 0x10, .data = 0x00,},
50 {.address = 0x17, .data = 0x40,},
51 {.address = 0x02, .data = 0x11,},
52 {.address = 0x08, .data = 0x7A,},
53 {.address = 0x09, .data = 0x00,},
54 {.address = 0x09, .data = 0x00,},
55 {.address = 0x09, .data = 0x03,},
56 {.address = 0x09, .data = 0x03,},
57 {.address = 0x09, .data = 0x00,},
58 {.address = 0x09, .data = 0x04,},
59 {.address = 0x02, .data = 0x41,},
60 {.address = 0x08, .data = 0x00,},
61 {.address = 0x09, .data = 0xFA,},
62 {.address = 0x09, .data = 0x00,},
63 {.address = 0x09, .data = 0x46,},
64 {.address = 0x09, .data = 0x00,},
65 {.address = 0x09, .data = 0x1A,},
66 {.address = 0x09, .data = 0x00,},
67 {.address = 0x09, .data = 0xCD,},
68 {.address = 0x09, .data = 0x38,},
69 {.address = 0x09, .data = 0x33,},
70 {.address = 0x09, .data = 0x3C,},
71 {.address = 0x09, .data = 0x00,},
72 {.address = 0x09, .data = 0x00,},
73 {.address = 0x09, .data = 0x02,},
74 {.address = 0x09, .data = 0x00,},
75 {.address = 0x09, .data = 0xA0,},
76 {.address = 0x09, .data = 0x00,},
77 {.address = 0x09, .data = 0x08,},
78 {.address = 0x09, .data = 0x00,},
79 {.address = 0x09, .data = 0x00,},
80 {.address = 0x09, .data = 0x00,},
81 {.address = 0x09, .data = 0x06,},
82 {.address = 0x09, .data = 0x00,},
83 {.address = 0x09, .data = 0x05,},
84 {.address = 0x09, .data = 0x00,},
85 {.address = 0x09, .data = 0x02,},
86 {.address = 0x09, .data = 0x10,},
87 {.address = 0x09, .data = 0x44,},
88 {.address = 0x09, .data = 0xCC,},
89 {.address = 0x09, .data = 0x12,},
90 {.address = 0x09, .data = 0x66,},
91 {.address = 0x09, .data = 0x08,},
92 {.address = 0x09, .data = 0x61,},
93 {.address = 0x09, .data = 0x77,},
94 {.address = 0x09, .data = 0xAA,},
95 {.address = 0x09, .data = 0x33,},
96 {.address = 0x09, .data = 0x37,},
97 {.address = 0x09, .data = 0x52,},
98 {.address = 0x09, .data = 0x88,},
99 {.address = 0x09, .data = 0xFE,},
100 {.address = 0x09, .data = 0x08,},
101 {.address = 0x09, .data = 0x13,},
102 {.address = 0x09, .data = 0xAA,},
103 {.address = 0x09, .data = 0xCD,},
104 {.address = 0x09, .data = 0x38,},
105 {.address = 0x09, .data = 0x72,},
106 {.address = 0x09, .data = 0x33,},
107 {.address = 0x09, .data = 0x66,},
108 {.address = 0x09, .data = 0x06,},
109 {.address = 0x09, .data = 0x88,},
110 {.address = 0x09, .data = 0xFE,},
111 {.address = 0x09, .data = 0x20,},
112 {.address = 0x09, .data = 0xAA,},
113 {.address = 0x09, .data = 0x9A,},
114 {.address = 0x09, .data = 0x35,},
115 {.address = 0x09, .data = 0x45,},
116 {.address = 0x09, .data = 0x41,},
117 {.address = 0x09, .data = 0x73,},
118 {.address = 0x09, .data = 0x1A,},
119 {.address = 0x09, .data = 0x3B,},
120 {.address = 0x09, .data = 0xFE,},
121 {.address = 0x09, .data = 0x08,},
122 {.address = 0x09, .data = 0xAA,},
123 {.address = 0x09, .data = 0x00,},
124 {.address = 0x09, .data = 0x00,},
125 {.address = 0x09, .data = 0x41,},
126 {.address = 0x09, .data = 0x75,},
127 {.address = 0x09, .data = 0x1A,},
128 {.address = 0x09, .data = 0x44,},
129 {.address = 0x09, .data = 0x44,},
130 {.address = 0x09, .data = 0x22,},
131 {.address = 0x09, .data = 0xFA,},
132 {.address = 0x09, .data = 0x00,},
133 {.address = 0x09, .data = 0x46,},
134 {.address = 0x09, .data = 0x00,},
135 {.address = 0x09, .data = 0x1A,},
136 {.address = 0x09, .data = 0x00,},
137 {.address = 0x09, .data = 0xCD,},
138 {.address = 0x09, .data = 0x38,},
139 {.address = 0x09, .data = 0x33,},
140 {.address = 0x09, .data = 0x3C,},
141 {.address = 0x09, .data = 0x00,},
142 {.address = 0x09, .data = 0x00,},
143 {.address = 0x09, .data = 0x02,},
144 {.address = 0x09, .data = 0x00,},
145 {.address = 0x09, .data = 0xA0,},
146 {.address = 0x09, .data = 0x00,},
147 {.address = 0x09, .data = 0x08,},
148 {.address = 0x09, .data = 0x00,},
149 {.address = 0x09, .data = 0x00,},
150 {.address = 0x09, .data = 0x00,},
151 {.address = 0x09, .data = 0x06,},
152 {.address = 0x09, .data = 0x00,},
153 {.address = 0x09, .data = 0x05,},
154 {.address = 0x09, .data = 0x00,},
155 {.address = 0x09, .data = 0x02,},
156 {.address = 0x09, .data = 0x10,},
157 {.address = 0x09, .data = 0x44,},
158 {.address = 0x09, .data = 0xCC,},
159 {.address = 0x09, .data = 0x12,},
160 {.address = 0x09, .data = 0x66,},
161 {.address = 0x09, .data = 0x08,},
162 {.address = 0x09, .data = 0x61,},
163 {.address = 0x09, .data = 0x77,},
164 {.address = 0x09, .data = 0xAA,},
165 {.address = 0x09, .data = 0x33,},
166 {.address = 0x09, .data = 0x37,},
167 {.address = 0x09, .data = 0x52,},
168 {.address = 0x09, .data = 0x88,},
169 {.address = 0x09, .data = 0xFE,},
170 {.address = 0x09, .data = 0x08,},
171 {.address = 0x09, .data = 0x13,},
172 {.address = 0x09, .data = 0xAA,},
173 {.address = 0x09, .data = 0xCD,},
174 {.address = 0x09, .data = 0x38,},
175 {.address = 0x09, .data = 0x72,},
176 {.address = 0x09, .data = 0x33,},
177 {.address = 0x09, .data = 0x66,},
178 {.address = 0x09, .data = 0x06,},
179 {.address = 0x09, .data = 0x88,},
180 {.address = 0x09, .data = 0xFE,},
181 {.address = 0x09, .data = 0x80,},
182 {.address = 0x09, .data = 0xAA,},
183 {.address = 0x09, .data = 0x66,},
184 {.address = 0x09, .data = 0x36,},
185 {.address = 0x09, .data = 0x45,},
186 {.address = 0x09, .data = 0x41,},
187 {.address = 0x09, .data = 0x73,},
188 {.address = 0x09, .data = 0x1A,},
189 {.address = 0x09, .data = 0x3B,},
190 {.address = 0x09, .data = 0xFE,},
191 {.address = 0x09, .data = 0x08,},
192 {.address = 0x09, .data = 0xAA,},
193 {.address = 0x09, .data = 0x00,},
194 {.address = 0x09, .data = 0x00,},
195 {.address = 0x09, .data = 0x41,},
196 {.address = 0x09, .data = 0x75,},
197 {.address = 0x09, .data = 0x1A,},
198 {.address = 0x09, .data = 0x44,},
199 {.address = 0x09, .data = 0x44,},
200 {.address = 0x09, .data = 0x22,},
201 {.address = 0x09, .data = 0xFA,},
202 {.address = 0x09, .data = 0x00,},
203 {.address = 0x09, .data = 0x46,},
204 {.address = 0x09, .data = 0x00,},
205 {.address = 0x09, .data = 0x1A,},
206 {.address = 0x09, .data = 0x00,},
207 {.address = 0x09, .data = 0x66,},
208 {.address = 0x09, .data = 0x32,},
209 {.address = 0x09, .data = 0x33,},
210 {.address = 0x09, .data = 0x3C,},
211 {.address = 0x09, .data = 0x00,},
212 {.address = 0x09, .data = 0x00,},
213 {.address = 0x09, .data = 0x02,},
214 {.address = 0x09, .data = 0x00,},
215 {.address = 0x09, .data = 0xA0,},
216 {.address = 0x09, .data = 0x00,},
217 {.address = 0x09, .data = 0x08,},
218 {.address = 0x09, .data = 0x00,},
219 {.address = 0x09, .data = 0x00,},
220 {.address = 0x09, .data = 0x00,},
221 {.address = 0x09, .data = 0x06,},
222 {.address = 0x09, .data = 0x00,},
223 {.address = 0x09, .data = 0x05,},
224 {.address = 0x09, .data = 0x00,},
225 {.address = 0x09, .data = 0x02,},
226 {.address = 0x09, .data = 0x10,},
227 {.address = 0x09, .data = 0x44,},
228 {.address = 0x09, .data = 0xCC,},
229 {.address = 0x09, .data = 0x12,},
230 {.address = 0x09, .data = 0x66,},
231 {.address = 0x09, .data = 0x08,},
232 {.address = 0x09, .data = 0x61,},
233 {.address = 0x09, .data = 0x77,},
234 {.address = 0x09, .data = 0xAA,},
235 {.address = 0x09, .data = 0x33,},
236 {.address = 0x09, .data = 0x37,},
237 {.address = 0x09, .data = 0x52,},
238 {.address = 0x09, .data = 0x88,},
239 {.address = 0x09, .data = 0xFE,},
240 {.address = 0x09, .data = 0x08,},
241 {.address = 0x09, .data = 0x13,},
242 {.address = 0x09, .data = 0xAA,},
243 {.address = 0x09, .data = 0x66,},
244 {.address = 0x09, .data = 0x32,},
245 {.address = 0x09, .data = 0x52,},
246 {.address = 0x09, .data = 0x33,},
247 {.address = 0x09, .data = 0x66,},
248 {.address = 0x09, .data = 0x06,},
249 {.address = 0x09, .data = 0x88,},
250 {.address = 0x09, .data = 0xFE,},
251 {.address = 0x09, .data = 0x08,},
252 {.address = 0x09, .data = 0xAA,},
253 {.address = 0x09, .data = 0x66,},
254 {.address = 0x09, .data = 0x36,},
255 {.address = 0x09, .data = 0x45,},
256 {.address = 0x09, .data = 0x41,},
257 {.address = 0x09, .data = 0x73,},
258 {.address = 0x09, .data = 0x1A,},
259 {.address = 0x09, .data = 0x3B,},
260 {.address = 0x09, .data = 0xFE,},
261 {.address = 0x09, .data = 0x08,},
262 {.address = 0x09, .data = 0xAA,},
263 {.address = 0x09, .data = 0x00,},
264 {.address = 0x09, .data = 0x00,},
265 {.address = 0x09, .data = 0x41,},
266 {.address = 0x09, .data = 0x75,},
267 {.address = 0x09, .data = 0x1A,},
268 {.address = 0x09, .data = 0x44,},
269 {.address = 0x09, .data = 0x44,},
270 {.address = 0x09, .data = 0x22,},
271 {.address = 0x04, .data = 0x00,},
272 {.address = 0x05, .data = 0x01,},
273 {.address = 0x17, .data = 0x00,},
274 {.address = 0x01, .data = 0x00,},
275 {.address = 0x01, .data = 0x00,},
276 {.address = 0x02, .data = 0x3F,},
277 {.address = 0x04, .data = 0x00,},
278 {.address = 0x05, .data = 0x00,},
279 {.address = 0x06, .data = 0x00,},
280 {.address = 0x07, .data = 0x00,},
281 {.address = 0x08, .data = 0x00,},
282 {.address = 0x09, .data = 0x00,},
283 {.address = 0x0A, .data = 0x00,},
284 {.address = 0x0B, .data = 0x00,},
285 {.address = 0x0E, .data = 0x00,},
286 {.address = 0x10, .data = 0x52,},
287 {.address = 0x11, .data = 0x50,},
288 {.address = 0x13, .data = 0x02,},
289 {.address = 0x14, .data = 0x00,},
290 {.address = 0x15, .data = 0x06,},
291 {.address = 0x16, .data = 0x00,},
292 {.address = 0x17, .data = 0x60,},
293 {.address = 0x5E, .data = 0x02,},
294 {.address = 0x5F, .data = 0x02,},
295 };
296
297 static struct st_lsm6dsr_suspend_resume_entry
298 st_lsm6dsr_suspend_resume[ST_LSM6DSR_SUSPEND_RESUME_REGS] = {
299 [ST_LSM6DSR_CTRL1_XL_REG] = {
300 .addr = ST_LSM6DSR_CTRL1_XL_ADDR,
301 .mask = GENMASK(3, 2),
302 },
303 [ST_LSM6DSR_CTRL2_G_REG] = {
304 .addr = ST_LSM6DSR_CTRL2_G_ADDR,
305 .mask = GENMASK(3, 2),
306 },
307 [ST_LSM6DSR_REG_CTRL3_C_REG] = {
308 .addr = ST_LSM6DSR_REG_CTRL3_C_ADDR,
309 .mask = ST_LSM6DSR_REG_BDU_MASK |
310 ST_LSM6DSR_REG_PP_OD_MASK |
311 ST_LSM6DSR_REG_H_LACTIVE_MASK,
312 },
313 [ST_LSM6DSR_REG_CTRL4_C_REG] = {
314 .addr = ST_LSM6DSR_REG_CTRL4_C_ADDR,
315 .mask = ST_LSM6DSR_REG_DRDY_MASK,
316 },
317 [ST_LSM6DSR_REG_CTRL5_C_REG] = {
318 .addr = ST_LSM6DSR_REG_CTRL5_C_ADDR,
319 .mask = ST_LSM6DSR_REG_ROUNDING_MASK,
320 },
321 [ST_LSM6DSR_REG_CTRL10_C_REG] = {
322 .addr = ST_LSM6DSR_REG_CTRL10_C_ADDR,
323 .mask = ST_LSM6DSR_REG_TIMESTAMP_EN_MASK,
324 },
325 [ST_LSM6DSR_REG_TAP_CFG0_REG] = {
326 .addr = ST_LSM6DSR_REG_TAP_CFG0_ADDR,
327 .mask = ST_LSM6DSR_REG_LIR_MASK,
328 },
329 [ST_LSM6DSR_REG_INT1_CTRL_REG] = {
330 .addr = ST_LSM6DSR_REG_INT1_CTRL_ADDR,
331 .mask = ST_LSM6DSR_REG_INT_FIFO_TH_MASK,
332 },
333 [ST_LSM6DSR_REG_INT2_CTRL_REG] = {
334 .addr = ST_LSM6DSR_REG_INT2_CTRL_ADDR,
335 .mask = ST_LSM6DSR_REG_INT_FIFO_TH_MASK,
336 },
337 [ST_LSM6DSR_REG_FIFO_CTRL1_REG] = {
338 .addr = ST_LSM6DSR_REG_FIFO_CTRL1_ADDR,
339 .mask = GENMASK(7, 0),
340 },
341 [ST_LSM6DSR_REG_FIFO_CTRL2_REG] = {
342 .addr = ST_LSM6DSR_REG_FIFO_CTRL2_ADDR,
343 .mask = ST_LSM6DSR_REG_FIFO_WTM8_MASK,
344 },
345 [ST_LSM6DSR_REG_FIFO_CTRL3_REG] = {
346 .addr = ST_LSM6DSR_REG_FIFO_CTRL3_ADDR,
347 .mask = ST_LSM6DSR_REG_BDR_XL_MASK |
348 ST_LSM6DSR_REG_BDR_GY_MASK,
349 },
350 [ST_LSM6DSR_REG_FIFO_CTRL4_REG] = {
351 .addr = ST_LSM6DSR_REG_FIFO_CTRL4_ADDR,
352 .mask = ST_LSM6DSR_REG_DEC_TS_MASK |
353 ST_LSM6DSR_REG_ODR_T_BATCH_MASK,
354 },
355 };
356
357 /**
358 * List of supported ODR
359 *
360 * The following table is complete list of supported ODR by Acc, Gyro and Temp
361 * sensors. ODR value can be also decimal (i.e 12.5 Hz)
362 */
363 static const struct st_lsm6dsr_odr_table_entry st_lsm6dsr_odr_table[] = {
364 [ST_LSM6DSR_ID_ACC] = {
365 .odr_size = 8,
366 .reg = {
367 .addr = ST_LSM6DSR_CTRL1_XL_ADDR,
368 .mask = GENMASK(7, 4),
369 },
370 .odr_avl[0] = { 0, 0, 0x00 },
371 .odr_avl[1] = { 12, 500000, 0x01 },
372 .odr_avl[2] = { 26, 0, 0x02 },
373 .odr_avl[3] = { 52, 0, 0x03 },
374 .odr_avl[4] = { 104, 0, 0x04 },
375 .odr_avl[5] = { 208, 0, 0x05 },
376 .odr_avl[6] = { 416, 0, 0x06 },
377 .odr_avl[7] = { 833, 0, 0x07 }
378 },
379 [ST_LSM6DSR_ID_GYRO] = {
380 .odr_size = 8,
381 .reg = {
382 .addr = ST_LSM6DSR_CTRL2_G_ADDR,
383 .mask = GENMASK(7, 4),
384 },
385 .odr_avl[0] = { 0, 0, 0x00 },
386 .odr_avl[1] = { 12, 500000, 0x01 },
387 .odr_avl[2] = { 26, 0, 0x02 },
388 .odr_avl[3] = { 52, 0, 0x03 },
389 .odr_avl[4] = { 104, 0, 0x04 },
390 .odr_avl[5] = { 208, 0, 0x05 },
391 .odr_avl[6] = { 416, 0, 0x06 },
392 .odr_avl[7] = { 833, 0, 0x07 }
393 },
394 [ST_LSM6DSR_ID_TEMP] = {
395 .odr_size = 2,
396 .odr_avl[0] = { 0, 0, 0x00 },
397 .odr_avl[1] = { 12, 500000, 0x02 },
398 },
399 };
400
401 /**
402 * List of supported Full Scale Value
403 *
404 * The following table is complete list of supported Full Scale by Acc, Gyro
405 * and Temp sensors.
406 */
407 static const struct st_lsm6dsr_fs_table_entry st_lsm6dsr_fs_table[] = {
408 [ST_LSM6DSR_ID_ACC] = {
409 .size = ST_LSM6DSR_FS_ACC_LIST_SIZE,
410 .fs_avl[0] = {
411 .reg = {
412 .addr = ST_LSM6DSR_CTRL1_XL_ADDR,
413 .mask = GENMASK(3, 2),
414 },
415 .gain = ST_LSM6DSR_ACC_FS_2G_GAIN,
416 .val = 0x0,
417 },
418 .fs_avl[1] = {
419 .reg = {
420 .addr = ST_LSM6DSR_CTRL1_XL_ADDR,
421 .mask = GENMASK(3, 2),
422 },
423 .gain = ST_LSM6DSR_ACC_FS_4G_GAIN,
424 .val = 0x2,
425 },
426 .fs_avl[2] = {
427 .reg = {
428 .addr = ST_LSM6DSR_CTRL1_XL_ADDR,
429 .mask = GENMASK(3, 2),
430 },
431 .gain = ST_LSM6DSR_ACC_FS_8G_GAIN,
432 .val = 0x3,
433 },
434 .fs_avl[3] = {
435 .reg = {
436 .addr = ST_LSM6DSR_CTRL1_XL_ADDR,
437 .mask = GENMASK(3, 2),
438 },
439 .gain = ST_LSM6DSR_ACC_FS_16G_GAIN,
440 .val = 0x1,
441 },
442 },
443 [ST_LSM6DSR_ID_GYRO] = {
444 .size = ST_LSM6DSR_FS_GYRO_LIST_SIZE,
445 .fs_avl[0] = {
446 .reg = {
447 .addr = ST_LSM6DSR_CTRL2_G_ADDR,
448 .mask = GENMASK(3, 0),
449 },
450 .gain = ST_LSM6DSR_GYRO_FS_250_GAIN,
451 .val = 0x0,
452 },
453 .fs_avl[1] = {
454 .reg = {
455 .addr = ST_LSM6DSR_CTRL2_G_ADDR,
456 .mask = GENMASK(3, 0),
457 },
458 .gain = ST_LSM6DSR_GYRO_FS_500_GAIN,
459 .val = 0x4,
460 },
461 .fs_avl[2] = {
462 .reg = {
463 .addr = ST_LSM6DSR_CTRL2_G_ADDR,
464 .mask = GENMASK(3, 0),
465 },
466 .gain = ST_LSM6DSR_GYRO_FS_1000_GAIN,
467 .val = 0x8,
468 },
469 .fs_avl[3] = {
470 .reg = {
471 .addr = ST_LSM6DSR_CTRL2_G_ADDR,
472 .mask = GENMASK(3, 0),
473 },
474 .gain = ST_LSM6DSR_GYRO_FS_2000_GAIN,
475 .val = 0x0C,
476 },
477 .fs_avl[4] = {
478 .reg = {
479 .addr = ST_LSM6DSR_CTRL2_G_ADDR,
480 .mask = GENMASK(3, 0),
481 },
482 .gain = ST_LSM6DSR_GYRO_FS_4000_GAIN,
483 .val = 0x1,
484 },
485 },
486 [ST_LSM6DSR_ID_TEMP] = {
487 .size = ST_LSM6DSR_FS_TEMP_LIST_SIZE,
488 .fs_avl[0] = {
489 .reg = { 0 },
490 .gain = ST_LSM6DSR_TEMP_FS_GAIN,
491 .val = 0x0
492 },
493 },
494 };
495
496 /**
497 * Accelerometer IIO channels description
498 *
499 * Accelerometer exports to IIO framework the following data channels:
500 * X Axis (16 bit signed in little endian)
501 * Y Axis (16 bit signed in little endian)
502 * Z Axis (16 bit signed in little endian)
503 * Timestamp (64 bit signed in little endian)
504 * Accelerometer exports to IIO framework the following event channels:
505 * Flush event done
506 */
507 static const struct iio_chan_spec st_lsm6dsr_acc_channels[] = {
508 ST_LSM6DSR_DATA_CHANNEL(IIO_ACCEL, ST_LSM6DSR_REG_OUTX_L_A_ADDR,
509 1, IIO_MOD_X, 0, 16, 16, 's'),
510 ST_LSM6DSR_DATA_CHANNEL(IIO_ACCEL, ST_LSM6DSR_REG_OUTY_L_A_ADDR,
511 1, IIO_MOD_Y, 1, 16, 16, 's'),
512 ST_LSM6DSR_DATA_CHANNEL(IIO_ACCEL, ST_LSM6DSR_REG_OUTZ_L_A_ADDR,
513 1, IIO_MOD_Z, 2, 16, 16, 's'),
514 ST_LSM6DSR_EVENT_CHANNEL(IIO_ACCEL, flush),
515 IIO_CHAN_SOFT_TIMESTAMP(3),
516 };
517
518 /**
519 * Gyro IIO channels description
520 *
521 * Gyro exports to IIO framework the following data channels:
522 * X Axis (16 bit signed in little endian)
523 * Y Axis (16 bit signed in little endian)
524 * Z Axis (16 bit signed in little endian)
525 * Timestamp (64 bit signed in little endian)
526 * Gyro exports to IIO framework the following event channels:
527 * Flush event done
528 */
529 static const struct iio_chan_spec st_lsm6dsr_gyro_channels[] = {
530 ST_LSM6DSR_DATA_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSR_REG_OUTX_L_G_ADDR,
531 1, IIO_MOD_X, 0, 16, 16, 's'),
532 ST_LSM6DSR_DATA_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSR_REG_OUTY_L_G_ADDR,
533 1, IIO_MOD_Y, 1, 16, 16, 's'),
534 ST_LSM6DSR_DATA_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSR_REG_OUTZ_L_G_ADDR,
535 1, IIO_MOD_Z, 2, 16, 16, 's'),
536 ST_LSM6DSR_EVENT_CHANNEL(IIO_ANGL_VEL, flush),
537 IIO_CHAN_SOFT_TIMESTAMP(3),
538 };
539
540 /**
541 * Step Counter IIO channels description
542 *
543 * Step Counter exports to IIO framework the following data channels:
544 * Step Counters (16 bit unsigned in little endian)
545 * Timestamp (64 bit signed in little endian)
546 * Step Counter exports to IIO framework the following event channels:
547 * Flush event done
548 */
549 static const struct iio_chan_spec st_lsm6dsr_step_counter_channels[] = {
550 {
551 .type = IIO_STEP_COUNTER,
552 .scan_index = 0,
553 .scan_type = {
554 .sign = 'u',
555 .realbits = 16,
556 .storagebits = 16,
557 .endianness = IIO_LE,
558 },
559 },
560 ST_LSM6DSR_EVENT_CHANNEL(IIO_STEP_COUNTER, flush),
561 IIO_CHAN_SOFT_TIMESTAMP(1),
562 };
563
564 /**
565 * @brief Step Detector IIO channels description
566 *
567 * Step Detector exports to IIO framework the following event channels:
568 * Step detection event detection
569 */
570 static const struct iio_chan_spec st_lsm6dsr_step_detector_channels[] = {
571 ST_LSM6DSR_EVENT_CHANNEL(IIO_STEP_DETECTOR, thr),
572 };
573
574 /**
575 * Significant Motion IIO channels description
576 *
577 * Significant Motion exports to IIO framework the following event channels:
578 * Significant Motion event detection
579 */
580 static const struct iio_chan_spec st_lsm6dsr_sign_motion_channels[] = {
581 ST_LSM6DSR_EVENT_CHANNEL(IIO_SIGN_MOTION, thr),
582 };
583
584 /**
585 * Tilt IIO channels description
586 *
587 * Tilt exports to IIO framework the following event channels:
588 * Tilt event detection
589 */
590 static const struct iio_chan_spec st_lsm6dsr_tilt_channels[] = {
591 ST_LSM6DSR_EVENT_CHANNEL(IIO_TILT, thr),
592 };
593
594 /**
595 * Temperature IIO channels description
596 *
597 * Temperature exports to IIO framework the following data channels:
598 * Temperature (16 bit signed in little endian)
599 * Temperature exports to IIO framework the following event channels:
600 * Temperature event threshold
601 */
602 static const struct iio_chan_spec st_lsm6dsr_temp_channels[] = {
603 {
604 .type = IIO_TEMP,
605 .address = ST_LSM6DSR_REG_OUT_TEMP_L_ADDR,
606 .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
607 BIT(IIO_CHAN_INFO_OFFSET) |
608 BIT(IIO_CHAN_INFO_SCALE),
609 .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ),
610 .scan_index = 0,
611 .scan_type = {
612 .sign = 's',
613 .realbits = 16,
614 .storagebits = 16,
615 .endianness = IIO_LE,
616 }
617 },
618 ST_LSM6DSR_EVENT_CHANNEL(IIO_TEMP, flush),
619 IIO_CHAN_SOFT_TIMESTAMP(1),
620 };
621
622 /**
623 * Glance IIO channels description
624 *
625 * Glance exports to IIO framework the following event channels:
626 * Glance event detection
627 */
628 static const struct iio_chan_spec st_lsm6dsr_glance_channels[] = {
629 ST_LSM6DSR_EVENT_CHANNEL(IIO_GESTURE, thr),
630 };
631
632 /**
633 * Motion IIO channels description
634 *
635 * Motion exports to IIO framework the following event channels:
636 * Motion event detection
637 */
638 static const struct iio_chan_spec st_lsm6dsr_motion_channels[] = {
639 ST_LSM6DSR_EVENT_CHANNEL(IIO_GESTURE, thr),
640 };
641
642 /**
643 * No Motion IIO channels description
644 *
645 * No Motion exports to IIO framework the following event channels:
646 * No Motion event detection
647 */
648 static const struct iio_chan_spec st_lsm6dsr_no_motion_channels[] = {
649 ST_LSM6DSR_EVENT_CHANNEL(IIO_GESTURE, thr),
650 };
651
652 /**
653 * Wakeup IIO channels description
654 *
655 * Wakeup exports to IIO framework the following event channels:
656 * Wakeup event detection
657 */
658 static const struct iio_chan_spec st_lsm6dsr_wakeup_channels[] = {
659 ST_LSM6DSR_EVENT_CHANNEL(IIO_GESTURE, thr),
660 };
661
662 /**
663 * Pickup IIO channels description
664 *
665 * Pickup exports to IIO framework the following event channels:
666 * Pickup event detection
667 */
668 static const struct iio_chan_spec st_lsm6dsr_pickup_channels[] = {
669 ST_LSM6DSR_EVENT_CHANNEL(IIO_GESTURE, thr),
670 };
671
672 /**
673 * Orientation IIO channels description
674 *
675 * Orientation exports to IIO framework the following data channels:
676 * Orientation (8 bit unsigned in little endian)
677 * Timestamp (64 bit signed in little endian)
678 */
679 static const struct iio_chan_spec st_lsm6dsr_orientation_channels[] = {
680 {
681 .type = IIO_GESTURE,
682 .scan_index = 0,
683 .scan_type = {
684 .sign = 'u',
685 .realbits = 8,
686 .storagebits = 8,
687 },
688 },
689 IIO_CHAN_SOFT_TIMESTAMP(1),
690 };
691
692 /**
693 * Wrist IIO channels description
694 *
695 * Wrist exports to IIO framework the following event channels:
696 * Wrist event detection
697 */
698 static const struct iio_chan_spec st_lsm6dsr_wrist_channels[] = {
699 ST_LSM6DSR_EVENT_CHANNEL(IIO_GESTURE, thr),
700 };
701
__st_lsm6dsr_write_with_mask(struct st_lsm6dsr_hw * hw,u8 addr,u8 mask,u8 val)702 int __st_lsm6dsr_write_with_mask(struct st_lsm6dsr_hw *hw, u8 addr, u8 mask,
703 u8 val)
704 {
705 u8 data;
706 int err;
707
708 mutex_lock(&hw->lock);
709
710 err = hw->tf->read(hw->dev, addr, sizeof(data), &data);
711 if (err < 0) {
712 dev_err(hw->dev, "failed to read %02x register\n", addr);
713 goto out;
714 }
715
716 data = (data & ~mask) | ((val << __ffs(mask)) & mask);
717
718 err = hw->tf->write(hw->dev, addr, sizeof(data), &data);
719 if (err < 0)
720 dev_err(hw->dev, "failed to write %02x register\n", addr);
721
722 out:
723 mutex_unlock(&hw->lock);
724
725 return err;
726 }
727
728 /**
729 * Detect device ID
730 *
731 * Check the value of the Device ID if valid
732 *
733 * @param hw: ST IMU MEMS hw instance
734 * @return 0 if OK, negative value for ERROR
735 */
st_lsm6dsr_check_whoami(struct st_lsm6dsr_hw * hw)736 static int st_lsm6dsr_check_whoami(struct st_lsm6dsr_hw *hw)
737 {
738 int err;
739 u8 data;
740
741 err = hw->tf->read(hw->dev, ST_LSM6DSR_REG_WHOAMI_ADDR, sizeof(data),
742 &data);
743 if (err < 0) {
744 dev_err(hw->dev, "failed to read whoami register\n");
745 return err;
746 }
747
748 if (data != ST_LSM6DSR_WHOAMI_VAL) {
749 dev_err(hw->dev, "unsupported whoami [%02x]\n", data);
750 return -ENODEV;
751 }
752
753 return 0;
754 }
755
756 /**
757 * Get timestamp calibration
758 *
759 * Read timestamp calibration data and trim delta time
760 *
761 * @param hw: ST IMU MEMS hw instance
762 * @return 0 if OK, negative value for ERROR
763 */
st_lsm6dsr_get_odr_calibration(struct st_lsm6dsr_hw * hw)764 static int st_lsm6dsr_get_odr_calibration(struct st_lsm6dsr_hw *hw)
765 {
766 int err;
767 s8 data;
768 s64 odr_calib;
769
770 err = hw->tf->read(hw->dev, ST_LSM6DSR_INTERNAL_FREQ_FINE, sizeof(data),
771 (u8 *)&data);
772 if (err < 0) {
773 dev_err(hw->dev, "failed to read %d register\n",
774 ST_LSM6DSR_INTERNAL_FREQ_FINE);
775 return err;
776 }
777
778 odr_calib = (data * 37500) / 1000;
779 hw->ts_delta_ns = ST_LSM6DSR_TS_DELTA_NS - odr_calib;
780
781 dev_info(hw->dev, "Freq Fine %lld (ts %lld)\n",
782 odr_calib, hw->ts_delta_ns);
783
784 return 0;
785 }
786
787 /**
788 * Set sensor Full Scale
789 *
790 * Set new Full Scale value for a specific sensor
791 *
792 * @param sensor: ST IMU sensor instance
793 * @param gain: New gain value
794 * @return 0 if OK, negative value for ERROR
795 */
st_lsm6dsr_set_full_scale(struct st_lsm6dsr_sensor * sensor,u32 gain)796 static int st_lsm6dsr_set_full_scale(struct st_lsm6dsr_sensor *sensor, u32 gain)
797 {
798 enum st_lsm6dsr_sensor_id id = sensor->id;
799 int i, err;
800 u8 val;
801
802 for (i = 0; i < st_lsm6dsr_fs_table[id].size; i++)
803 if (st_lsm6dsr_fs_table[id].fs_avl[i].gain == gain)
804 break;
805
806 if (i == st_lsm6dsr_fs_table[id].size)
807 return -EINVAL;
808
809 val = st_lsm6dsr_fs_table[id].fs_avl[i].val;
810 err = st_lsm6dsr_write_with_mask(sensor->hw,
811 st_lsm6dsr_fs_table[id].fs_avl[i].reg.addr,
812 st_lsm6dsr_fs_table[id].fs_avl[i].reg.mask,
813 val);
814 if (err < 0)
815 return err;
816
817 sensor->gain = gain;
818 return 0;
819 }
820
821 /**
822 * Get a valid ODR
823 *
824 * Check a valid ODR closest to the passed value
825 *
826 * @param id: Sensor Identifier
827 * @param odr: Most significant part of ODR value (in Hz).
828 * @param uodr: Least significant part of ODR value (in micro Hz).
829 * @param podr: User data pointer.
830 * @param puodr: User data pointer.
831 * @param val: ODR register value data pointer.
832 * @return 0 if OK, negative value for ERROR
833 */
st_lsm6dsr_get_odr_val(enum st_lsm6dsr_sensor_id id,int odr,int uodr,int * podr,int * puodr,u8 * val)834 int st_lsm6dsr_get_odr_val(enum st_lsm6dsr_sensor_id id, int odr, int uodr,
835 int *podr, int *puodr, u8 *val)
836 {
837 int i;
838 int sensor_odr;
839 int all_odr = ST_LSM6DSR_ODR_EXPAND(odr, uodr);
840
841 for (i = 0; i < st_lsm6dsr_odr_table[id].odr_size; i++) {
842 sensor_odr =
843 ST_LSM6DSR_ODR_EXPAND(st_lsm6dsr_odr_table[id].odr_avl[i].hz,
844 st_lsm6dsr_odr_table[id].odr_avl[i].uhz);
845 if (sensor_odr >= all_odr)
846 break;
847 }
848
849 if (i == st_lsm6dsr_odr_table[id].odr_size)
850 return -EINVAL;
851
852 *val = st_lsm6dsr_odr_table[id].odr_avl[i].val;
853 *podr = st_lsm6dsr_odr_table[id].odr_avl[i].hz;
854 *puodr = st_lsm6dsr_odr_table[id].odr_avl[i].uhz;
855
856 return 0;
857 }
858
st_lsm6dsr_check_odr_dependency(struct st_lsm6dsr_hw * hw,int odr,int uodr,enum st_lsm6dsr_sensor_id ref_id)859 static u16 st_lsm6dsr_check_odr_dependency(struct st_lsm6dsr_hw *hw,
860 int odr, int uodr,
861 enum st_lsm6dsr_sensor_id ref_id)
862 {
863 struct st_lsm6dsr_sensor *ref = iio_priv(hw->iio_devs[ref_id]);
864 bool enable = odr > 0;
865 u16 ret;
866
867 if (enable) {
868 /* uodr not used */
869 if (hw->enable_mask & BIT(ref_id))
870 ret = max_t(int, ref->odr, odr);
871 else
872 ret = odr;
873 } else {
874 ret = (hw->enable_mask & BIT(ref_id)) ? ref->odr : 0;
875 }
876
877 return ret;
878 }
879
880 /**
881 * Set new ODR to sensor
882 * Set a valid ODR closest to the passed value
883 *
884 * @param sensor: ST IMU sensor instance
885 * @param req_odr: Most significant part of ODR value (in Hz).
886 * @param req_uodr: Least significant part of ODR value (in micro Hz).
887 * @return 0 if OK, negative value for ERROR
888 */
st_lsm6dsr_set_odr(struct st_lsm6dsr_sensor * sensor,int req_odr,int req_uodr)889 static int st_lsm6dsr_set_odr(struct st_lsm6dsr_sensor *sensor, int req_odr,
890 int req_uodr)
891 {
892 struct st_lsm6dsr_hw *hw = sensor->hw;
893 enum st_lsm6dsr_sensor_id id = sensor->id;
894 int err;
895 u8 val;
896
897 switch (id) {
898 case ST_LSM6DSR_ID_STEP_COUNTER:
899 case ST_LSM6DSR_ID_STEP_DETECTOR:
900 case ST_LSM6DSR_ID_SIGN_MOTION:
901 case ST_LSM6DSR_ID_TILT:
902 case ST_LSM6DSR_ID_NO_MOTION:
903 case ST_LSM6DSR_ID_MOTION:
904 case ST_LSM6DSR_ID_GLANCE:
905 case ST_LSM6DSR_ID_WAKEUP:
906 case ST_LSM6DSR_ID_PICKUP:
907 case ST_LSM6DSR_ID_ORIENTATION:
908 case ST_LSM6DSR_ID_WRIST_TILT:
909 case ST_LSM6DSR_ID_TEMP:
910 case ST_LSM6DSR_ID_EXT0:
911 case ST_LSM6DSR_ID_EXT1:
912 case ST_LSM6DSR_ID_ACC: {
913 int odr;
914 int i;
915
916 id = ST_LSM6DSR_ID_ACC;
917 for (i = ST_LSM6DSR_ID_ACC; i <= ST_LSM6DSR_ID_TILT; i++) {
918 if (!hw->iio_devs[i])
919 continue;
920
921 if (i == sensor->id)
922 continue;
923
924 /* req_uodr not used */
925 odr = st_lsm6dsr_check_odr_dependency(hw, req_odr,
926 req_uodr, i);
927 if (odr != req_odr)
928 /* device already configured */
929 return 0;
930 }
931 break;
932 }
933 default:
934 break;
935 }
936
937 err = st_lsm6dsr_get_odr_val(id, req_odr, req_uodr, &req_odr,
938 &req_uodr, &val);
939 if (err < 0)
940 return err;
941
942 return st_lsm6dsr_write_with_mask(hw, st_lsm6dsr_odr_table[id].reg.addr,
943 st_lsm6dsr_odr_table[id].reg.mask,
944 val);
945 }
946
947 /**
948 * Enable or Disable sensor
949 *
950 * @param sensor: ST IMU sensor instance
951 * @param enable: Enable or disable the sensor [true,false].
952 * @return 0 if OK, negative value for ERROR
953 */
st_lsm6dsr_sensor_set_enable(struct st_lsm6dsr_sensor * sensor,bool enable)954 int st_lsm6dsr_sensor_set_enable(struct st_lsm6dsr_sensor *sensor,
955 bool enable)
956 {
957 int uodr = 0;
958 int odr = 0;
959 int err;
960
961 if (enable) {
962 odr = sensor->odr;
963 uodr = sensor->uodr;
964 }
965
966 err = st_lsm6dsr_set_odr(sensor, odr, uodr);
967 if (err < 0)
968 return err;
969
970 if (enable)
971 sensor->hw->enable_mask |= BIT(sensor->id);
972 else
973 sensor->hw->enable_mask &= ~BIT(sensor->id);
974
975 return 0;
976 }
977
978 /**
979 * Single sensor read operation
980 *
981 * @param sensor: ST IMU sensor instance
982 * @param addr: Output data register value.
983 * @param val: Output data buffer.
984 * @return IIO_VAL_INT if OK, negative value for ERROR
985 */
st_lsm6dsr_read_oneshot(struct st_lsm6dsr_sensor * sensor,u8 addr,int * val)986 static int st_lsm6dsr_read_oneshot(struct st_lsm6dsr_sensor *sensor,
987 u8 addr, int *val)
988 {
989 int err, delay;
990 __le16 data;
991
992 err = st_lsm6dsr_sensor_set_enable(sensor, true);
993 if (err < 0)
994 return err;
995
996 delay = 1000000 / sensor->odr;
997 usleep_range(delay, 2 * delay);
998
999 err = st_lsm6dsr_read_atomic(sensor->hw, addr, sizeof(data),
1000 (u8 *)&data);
1001 if (err < 0)
1002 return err;
1003
1004 //st_lsm6dsr_sensor_set_enable(sensor, false);
1005
1006 *val = (s16)le16_to_cpu(data);
1007
1008 return IIO_VAL_INT;
1009 }
1010
1011 /**
1012 * Read Sensor data configuration
1013 *
1014 * @param iio_dev: IIO Device.
1015 * @param ch: IIO Channel.
1016 * @param val: Data Buffer (MSB).
1017 * @param val2: Data Buffer (LSB).
1018 * @param mask: Data Mask.
1019 * @return 0 if OK, -EINVAL value for ERROR
1020 */
st_lsm6dsr_read_raw(struct iio_dev * iio_dev,struct iio_chan_spec const * ch,int * val,int * val2,long mask)1021 static int st_lsm6dsr_read_raw(struct iio_dev *iio_dev,
1022 struct iio_chan_spec const *ch,
1023 int *val, int *val2, long mask)
1024 {
1025 struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
1026 int ret;
1027
1028 switch (mask) {
1029 case IIO_CHAN_INFO_RAW:
1030 mutex_lock(&iio_dev->mlock);
1031 if (iio_buffer_enabled(iio_dev)) {
1032 ret = -EBUSY;
1033 mutex_unlock(&iio_dev->mlock);
1034 break;
1035 }
1036 ret = st_lsm6dsr_read_oneshot(sensor, ch->address, val);
1037 mutex_unlock(&iio_dev->mlock);
1038 break;
1039 case IIO_CHAN_INFO_OFFSET:
1040 switch (ch->type) {
1041 case IIO_TEMP:
1042 *val = sensor->offset;
1043 ret = IIO_VAL_INT;
1044 break;
1045 default:
1046 return -EINVAL;
1047 }
1048 break;
1049 case IIO_CHAN_INFO_SAMP_FREQ:
1050 *val = (int)sensor->odr;
1051 *val2 = (int)sensor->uodr;
1052 ret = IIO_VAL_INT_PLUS_MICRO;
1053 break;
1054 case IIO_CHAN_INFO_SCALE:
1055 switch (ch->type) {
1056 case IIO_TEMP:
1057 *val = 1;
1058 *val2 = ST_LSM6DSR_TEMP_GAIN;
1059 ret = IIO_VAL_FRACTIONAL;
1060 break;
1061 case IIO_ACCEL:
1062 case IIO_ANGL_VEL: {
1063 *val = 0;
1064 *val2 = sensor->gain;
1065 ret = IIO_VAL_INT_PLUS_MICRO;
1066 }
1067 break;
1068 default:
1069 return -EINVAL;
1070 }
1071 break;
1072 default:
1073 ret = -EINVAL;
1074 break;
1075 }
1076
1077 return ret;
1078 }
1079
1080 /**
1081 * Write Sensor data configuration
1082 *
1083 * @param iio_dev: IIO Device.
1084 * @param chan: IIO Channel.
1085 * @param val: Data Buffer (MSB).
1086 * @param val2: Data Buffer (LSB).
1087 * @param mask: Data Mask.
1088 * @return 0 if OK, -EINVAL value for ERROR
1089 */
st_lsm6dsr_write_raw(struct iio_dev * iio_dev,struct iio_chan_spec const * chan,int val,int val2,long mask)1090 static int st_lsm6dsr_write_raw(struct iio_dev *iio_dev,
1091 struct iio_chan_spec const *chan,
1092 int val, int val2, long mask)
1093 {
1094 struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
1095 int err;
1096
1097 mutex_lock(&iio_dev->mlock);
1098
1099 switch (mask) {
1100 case IIO_CHAN_INFO_SCALE:
1101 err = st_lsm6dsr_set_full_scale(sensor, val2);
1102 break;
1103 case IIO_CHAN_INFO_SAMP_FREQ: {
1104 u8 data;
1105 int todr, tuodr;
1106
1107 err = st_lsm6dsr_get_odr_val(sensor->id, val, val2, &todr,
1108 &tuodr, &data);
1109 if (!err) {
1110 sensor->odr = todr;
1111 sensor->uodr = tuodr;
1112 }
1113
1114 /*
1115 * VTS test testSamplingRateHotSwitchOperation not toggle the
1116 * enable status of sensor after changing the ODR -> force it
1117 */
1118 if (sensor->hw->enable_mask & BIT(sensor->id)) {
1119 switch (sensor->id) {
1120 case ST_LSM6DSR_ID_GYRO:
1121 case ST_LSM6DSR_ID_ACC:
1122 err = st_lsm6dsr_set_odr(sensor, sensor->odr,
1123 sensor->uodr);
1124 /* I2C interface err can be positive */
1125 if (err < 0)
1126 break;
1127
1128 err = st_lsm6dsr_update_batching(iio_dev, 1);
1129 default:
1130 break;
1131 }
1132 }
1133 break;
1134 }
1135 default:
1136 err = -EINVAL;
1137 break;
1138 }
1139
1140 mutex_unlock(&iio_dev->mlock);
1141
1142 return err;
1143 }
1144
1145 #ifdef CONFIG_DEBUG_FS
st_lsm6dsr_reg_access(struct iio_dev * iio_dev,unsigned int reg,unsigned int writeval,unsigned int * readval)1146 static int st_lsm6dsr_reg_access(struct iio_dev *iio_dev, unsigned int reg,
1147 unsigned int writeval, unsigned int *readval)
1148 {
1149 struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
1150 int ret;
1151
1152 mutex_lock(&iio_dev->mlock);
1153 if (readval == NULL) {
1154 ret = sensor->hw->tf->write(sensor->hw->dev, reg, 1,
1155 (u8 *)&writeval);
1156 } else {
1157 sensor->hw->tf->read(sensor->hw->dev, reg, 1,
1158 (u8 *)readval);
1159 ret = 0;
1160 }
1161 mutex_unlock(&iio_dev->mlock);
1162
1163 return ret;
1164 }
1165 #endif
1166
1167 /**
1168 * Read sensor event configuration
1169 *
1170 * @param iio_dev: IIO Device.
1171 * @param chan: IIO Channel.
1172 * @param type: Event Type.
1173 * @param dir: Event Direction.
1174 * @return 1 if Enabled, 0 Disabled
1175 */
st_lsm6dsr_read_event_config(struct iio_dev * iio_dev,const struct iio_chan_spec * chan,enum iio_event_type type,enum iio_event_direction dir)1176 static int st_lsm6dsr_read_event_config(struct iio_dev *iio_dev,
1177 const struct iio_chan_spec *chan,
1178 enum iio_event_type type,
1179 enum iio_event_direction dir)
1180 {
1181 struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
1182 struct st_lsm6dsr_hw *hw = sensor->hw;
1183
1184 return !!(hw->enable_mask & BIT(sensor->id));
1185 }
1186
1187 /**
1188 * Write sensor event configuration
1189 *
1190 * @param iio_dev: IIO Device.
1191 * @param chan: IIO Channel.
1192 * @param type: Event Type.
1193 * @param dir: Event Direction.
1194 * @param state: New event state.
1195 * @return 0 if OK, negative for ERROR
1196 */
st_lsm6dsr_write_event_config(struct iio_dev * iio_dev,const struct iio_chan_spec * chan,enum iio_event_type type,enum iio_event_direction dir,int state)1197 static int st_lsm6dsr_write_event_config(struct iio_dev *iio_dev,
1198 const struct iio_chan_spec *chan,
1199 enum iio_event_type type,
1200 enum iio_event_direction dir,
1201 int state)
1202 {
1203 struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
1204 int err;
1205
1206 mutex_lock(&iio_dev->mlock);
1207 err = st_lsm6dsr_embfunc_sensor_set_enable(sensor, state);
1208 mutex_unlock(&iio_dev->mlock);
1209
1210 return err;
1211 }
1212
1213 /**
1214 * Get a list of available sensor ODR
1215 *
1216 * List of available ODR returned separated by commas
1217 *
1218 * @param dev: IIO Device.
1219 * @param attr: IIO Channel attribute.
1220 * @param buf: User buffer.
1221 * @return buffer len
1222 */
1223 static ssize_t
st_lsm6dsr_sysfs_sampling_frequency_avail(struct device * dev,struct device_attribute * attr,char * buf)1224 st_lsm6dsr_sysfs_sampling_frequency_avail(struct device *dev,
1225 struct device_attribute *attr,
1226 char *buf)
1227 {
1228 struct st_lsm6dsr_sensor *sensor = iio_priv(dev_get_drvdata(dev));
1229 enum st_lsm6dsr_sensor_id id = sensor->id;
1230 int i, len = 0;
1231
1232 for (i = 0; i < ST_LSM6DSR_ODR_LIST_SIZE; i++) {
1233 if (!st_lsm6dsr_odr_table[id].odr_avl[i].hz)
1234 continue;
1235
1236 if (st_lsm6dsr_odr_table[id].odr_avl[i].uhz == 0)
1237 len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
1238 st_lsm6dsr_odr_table[id].odr_avl[i].hz);
1239 else
1240 len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%d ",
1241 st_lsm6dsr_odr_table[id].odr_avl[i].hz,
1242 st_lsm6dsr_odr_table[id].odr_avl[i].uhz);
1243 }
1244
1245 buf[len - 1] = '\n';
1246
1247 return len;
1248 }
1249
1250 /**
1251 * Get a list of available sensor Full Scale
1252 *
1253 * List of available Full Scale returned separated by commas
1254 *
1255 * @param dev: IIO Device.
1256 * @param attr: IIO Channel attribute.
1257 * @param buf: User buffer.
1258 * @return buffer len
1259 */
st_lsm6dsr_sysfs_scale_avail(struct device * dev,struct device_attribute * attr,char * buf)1260 static ssize_t st_lsm6dsr_sysfs_scale_avail(struct device *dev,
1261 struct device_attribute *attr,
1262 char *buf)
1263 {
1264 struct st_lsm6dsr_sensor *sensor = iio_priv(dev_get_drvdata(dev));
1265 enum st_lsm6dsr_sensor_id id = sensor->id;
1266 int i, len = 0;
1267
1268 for (i = 0; i < st_lsm6dsr_fs_table[id].size; i++)
1269 len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
1270 st_lsm6dsr_fs_table[id].fs_avl[i].gain);
1271 buf[len - 1] = '\n';
1272
1273 return len;
1274 }
1275
1276 /**
1277 * Reset step counter value
1278 *
1279 * @param dev: IIO Device.
1280 * @param attr: IIO Channel attribute.
1281 * @param buf: User buffer.
1282 * @param size: User buffer size.
1283 * @return buffer len, negative for ERROR
1284 */
1285 static ssize_t
st_lsm6dsr_sysfs_reset_step_counter(struct device * dev,struct device_attribute * attr,const char * buf,size_t size)1286 st_lsm6dsr_sysfs_reset_step_counter(struct device *dev,
1287 struct device_attribute *attr,
1288 const char *buf, size_t size)
1289 {
1290 struct iio_dev *iio_dev = dev_get_drvdata(dev);
1291 int err;
1292
1293 err = st_lsm6dsr_reset_step_counter(iio_dev);
1294
1295 return err < 0 ? err : size;
1296 }
1297
1298 static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsr_sysfs_sampling_frequency_avail);
1299 static IIO_DEVICE_ATTR(in_accel_scale_available, 0444,
1300 st_lsm6dsr_sysfs_scale_avail, NULL, 0);
1301 static IIO_DEVICE_ATTR(in_anglvel_scale_available, 0444,
1302 st_lsm6dsr_sysfs_scale_avail, NULL, 0);
1303 static IIO_DEVICE_ATTR(in_temp_scale_available, 0444,
1304 st_lsm6dsr_sysfs_scale_avail, NULL, 0);
1305 static IIO_DEVICE_ATTR(hwfifo_watermark_max, 0444,
1306 st_lsm6dsr_get_max_watermark, NULL, 0);
1307 static IIO_DEVICE_ATTR(hwfifo_flush, 0200, NULL, st_lsm6dsr_flush_fifo, 0);
1308 static IIO_DEVICE_ATTR(hwfifo_watermark, 0644, st_lsm6dsr_get_watermark,
1309 st_lsm6dsr_set_watermark, 0);
1310 static IIO_DEVICE_ATTR(reset_counter, 0200, NULL,
1311 st_lsm6dsr_sysfs_reset_step_counter, 0);
1312
1313 static struct attribute *st_lsm6dsr_acc_attributes[] = {
1314 &iio_dev_attr_sampling_frequency_available.dev_attr.attr,
1315 &iio_dev_attr_in_accel_scale_available.dev_attr.attr,
1316 &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr,
1317 &iio_dev_attr_hwfifo_watermark.dev_attr.attr,
1318 &iio_dev_attr_hwfifo_flush.dev_attr.attr,
1319 NULL,
1320 };
1321
1322 static const struct attribute_group st_lsm6dsr_acc_attribute_group = {
1323 .attrs = st_lsm6dsr_acc_attributes,
1324 };
1325
1326 static const struct iio_info st_lsm6dsr_acc_info = {
1327 .attrs = &st_lsm6dsr_acc_attribute_group,
1328 .read_raw = st_lsm6dsr_read_raw,
1329 .write_raw = st_lsm6dsr_write_raw,
1330 #ifdef CONFIG_DEBUG_FS
1331 /* connect debug info to first device */
1332 .debugfs_reg_access = st_lsm6dsr_reg_access,
1333 #endif /* CONFIG_DEBUG_FS */
1334 };
1335
1336 static struct attribute *st_lsm6dsr_gyro_attributes[] = {
1337 &iio_dev_attr_sampling_frequency_available.dev_attr.attr,
1338 &iio_dev_attr_in_anglvel_scale_available.dev_attr.attr,
1339 &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr,
1340 &iio_dev_attr_hwfifo_watermark.dev_attr.attr,
1341 &iio_dev_attr_hwfifo_flush.dev_attr.attr,
1342 NULL,
1343 };
1344
1345 static const struct attribute_group st_lsm6dsr_gyro_attribute_group = {
1346 .attrs = st_lsm6dsr_gyro_attributes,
1347 };
1348
1349 static const struct iio_info st_lsm6dsr_gyro_info = {
1350 .attrs = &st_lsm6dsr_gyro_attribute_group,
1351 .read_raw = st_lsm6dsr_read_raw,
1352 .write_raw = st_lsm6dsr_write_raw,
1353 };
1354
1355 static struct attribute *st_lsm6dsr_temp_attributes[] = {
1356 &iio_dev_attr_sampling_frequency_available.dev_attr.attr,
1357 &iio_dev_attr_in_temp_scale_available.dev_attr.attr,
1358 &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr,
1359 &iio_dev_attr_hwfifo_watermark.dev_attr.attr,
1360 &iio_dev_attr_hwfifo_flush.dev_attr.attr,
1361 NULL,
1362 };
1363
1364 static const struct attribute_group st_lsm6dsr_temp_attribute_group = {
1365 .attrs = st_lsm6dsr_temp_attributes,
1366 };
1367
1368 static const struct iio_info st_lsm6dsr_temp_info = {
1369 .attrs = &st_lsm6dsr_temp_attribute_group,
1370 .read_raw = st_lsm6dsr_read_raw,
1371 .write_raw = st_lsm6dsr_write_raw,
1372 };
1373
1374 static struct attribute *st_lsm6dsr_step_counter_attributes[] = {
1375 &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr,
1376 &iio_dev_attr_hwfifo_watermark.dev_attr.attr,
1377 &iio_dev_attr_reset_counter.dev_attr.attr,
1378 &iio_dev_attr_hwfifo_flush.dev_attr.attr,
1379 NULL,
1380 };
1381
1382 static const struct attribute_group st_lsm6dsr_step_counter_attribute_group = {
1383 .attrs = st_lsm6dsr_step_counter_attributes,
1384 };
1385
1386 static const struct iio_info st_lsm6dsr_step_counter_info = {
1387 .attrs = &st_lsm6dsr_step_counter_attribute_group,
1388 };
1389
1390 static struct attribute *st_lsm6dsr_step_detector_attributes[] = {
1391 NULL,
1392 };
1393
1394 static const struct attribute_group st_lsm6dsr_step_detector_attribute_group = {
1395 .attrs = st_lsm6dsr_step_detector_attributes,
1396 };
1397
1398 static const struct iio_info st_lsm6dsr_step_detector_info = {
1399 .attrs = &st_lsm6dsr_step_detector_attribute_group,
1400 .read_event_config = st_lsm6dsr_read_event_config,
1401 .write_event_config = st_lsm6dsr_write_event_config,
1402 };
1403
1404 static struct attribute *st_lsm6dsr_sign_motion_attributes[] = {
1405 NULL,
1406 };
1407
1408 static const struct attribute_group st_lsm6dsr_sign_motion_attribute_group = {
1409 .attrs = st_lsm6dsr_sign_motion_attributes,
1410 };
1411
1412 static const struct iio_info st_lsm6dsr_sign_motion_info = {
1413 .attrs = &st_lsm6dsr_sign_motion_attribute_group,
1414 .read_event_config = st_lsm6dsr_read_event_config,
1415 .write_event_config = st_lsm6dsr_write_event_config,
1416 };
1417
1418 static struct attribute *st_lsm6dsr_tilt_attributes[] = {
1419 NULL,
1420 };
1421
1422 static const struct attribute_group st_lsm6dsr_tilt_attribute_group = {
1423 .attrs = st_lsm6dsr_tilt_attributes,
1424 };
1425
1426 static const struct iio_info st_lsm6dsr_tilt_info = {
1427 .attrs = &st_lsm6dsr_tilt_attribute_group,
1428 .read_event_config = st_lsm6dsr_read_event_config,
1429 .write_event_config = st_lsm6dsr_write_event_config,
1430 };
1431
1432 static struct attribute *st_lsm6dsr_glance_attributes[] = {
1433 NULL,
1434 };
1435
1436 static const struct attribute_group st_lsm6dsr_glance_attribute_group = {
1437 .attrs = st_lsm6dsr_glance_attributes,
1438 };
1439
1440 static const struct iio_info st_lsm6dsr_glance_info = {
1441 .attrs = &st_lsm6dsr_glance_attribute_group,
1442 .read_event_config = st_lsm6dsr_read_event_config,
1443 .write_event_config = st_lsm6dsr_write_event_config,
1444 };
1445
1446 static struct attribute *st_lsm6dsr_motion_attributes[] = {
1447 NULL,
1448 };
1449
1450 static const struct attribute_group st_lsm6dsr_motion_attribute_group = {
1451 .attrs = st_lsm6dsr_motion_attributes,
1452 };
1453
1454 static const struct iio_info st_lsm6dsr_motion_info = {
1455 .attrs = &st_lsm6dsr_motion_attribute_group,
1456 .read_event_config = st_lsm6dsr_read_event_config,
1457 .write_event_config = st_lsm6dsr_write_event_config,
1458 };
1459
1460 static struct attribute *st_lsm6dsr_no_motion_attributes[] = {
1461 NULL,
1462 };
1463
1464 static const struct attribute_group st_lsm6dsr_no_motion_attribute_group = {
1465 .attrs = st_lsm6dsr_no_motion_attributes,
1466 };
1467
1468 static const struct iio_info st_lsm6dsr_no_motion_info = {
1469 .attrs = &st_lsm6dsr_no_motion_attribute_group,
1470 .read_event_config = st_lsm6dsr_read_event_config,
1471 .write_event_config = st_lsm6dsr_write_event_config,
1472 };
1473
1474 static struct attribute *st_lsm6dsr_wakeup_attributes[] = {
1475 NULL,
1476 };
1477
1478 static const struct attribute_group st_lsm6dsr_wakeup_attribute_group = {
1479 .attrs = st_lsm6dsr_wakeup_attributes,
1480 };
1481
1482 static const struct iio_info st_lsm6dsr_wakeup_info = {
1483 .attrs = &st_lsm6dsr_wakeup_attribute_group,
1484 .read_event_config = st_lsm6dsr_read_event_config,
1485 .write_event_config = st_lsm6dsr_write_event_config,
1486 };
1487
1488 static struct attribute *st_lsm6dsr_pickup_attributes[] = {
1489 NULL,
1490 };
1491
1492 static const struct attribute_group st_lsm6dsr_pickup_attribute_group = {
1493 .attrs = st_lsm6dsr_pickup_attributes,
1494 };
1495
1496 static const struct iio_info st_lsm6dsr_pickup_info = {
1497 .attrs = &st_lsm6dsr_pickup_attribute_group,
1498 .read_event_config = st_lsm6dsr_read_event_config,
1499 .write_event_config = st_lsm6dsr_write_event_config,
1500 };
1501
1502 static struct attribute *st_lsm6dsr_orientation_attributes[] = {
1503 NULL,
1504 };
1505
1506 static const struct attribute_group st_lsm6dsr_orientation_attribute_group = {
1507 .attrs = st_lsm6dsr_orientation_attributes,
1508 };
1509
1510 static const struct iio_info st_lsm6dsr_orientation_info = {
1511 .attrs = &st_lsm6dsr_orientation_attribute_group,
1512 };
1513
1514 static struct attribute *st_lsm6dsr_wrist_attributes[] = {
1515 NULL,
1516 };
1517
1518 static const struct attribute_group st_lsm6dsr_wrist_attribute_group = {
1519 .attrs = st_lsm6dsr_wrist_attributes,
1520 };
1521
1522 static const struct iio_info st_lsm6dsr_wrist_info = {
1523 .attrs = &st_lsm6dsr_wrist_attribute_group,
1524 .read_event_config = st_lsm6dsr_read_event_config,
1525 .write_event_config = st_lsm6dsr_write_event_config,
1526 };
1527
1528 static const unsigned long st_lsm6dsr_available_scan_masks[] = { 0x7, 0x0 };
1529 static const unsigned long st_lsm6dsr_sc_available_scan_masks[] = { 0x1, 0x0 };
1530
st_lsm6dsr_of_get_pin(struct st_lsm6dsr_hw * hw,int * pin)1531 static int st_lsm6dsr_of_get_pin(struct st_lsm6dsr_hw *hw, int *pin)
1532 {
1533 struct device_node *np = hw->dev->of_node;
1534
1535 if (!np)
1536 return -EINVAL;
1537
1538 return of_property_read_u32(np, "st,int-pin", pin);
1539 }
1540
st_lsm6dsr_get_int_reg(struct st_lsm6dsr_hw * hw,u8 * drdy_reg,u8 * ef_irq_reg)1541 static int st_lsm6dsr_get_int_reg(struct st_lsm6dsr_hw *hw, u8 *drdy_reg,
1542 u8 *ef_irq_reg)
1543 {
1544 int err = 0, int_pin;
1545
1546 if (st_lsm6dsr_of_get_pin(hw, &int_pin) < 0) {
1547 struct st_sensors_platform_data *pdata;
1548 struct device *dev = hw->dev;
1549
1550 pdata = (struct st_sensors_platform_data *)dev->platform_data;
1551 int_pin = pdata ? pdata->drdy_int_pin : 1;
1552 }
1553
1554 switch (int_pin) {
1555 case 1:
1556 hw->embfunc_pg0_irq_reg = ST_LSM6DSR_REG_MD1_CFG_ADDR;
1557 hw->embfunc_irq_reg = ST_LSM6DSR_REG_EMB_FUNC_INT1_ADDR;
1558 *ef_irq_reg = ST_LSM6DSR_REG_MD1_CFG_ADDR;
1559 *drdy_reg = ST_LSM6DSR_REG_INT1_CTRL_ADDR;
1560 break;
1561 case 2:
1562 hw->embfunc_pg0_irq_reg = ST_LSM6DSR_REG_MD2_CFG_ADDR;
1563 hw->embfunc_irq_reg = ST_LSM6DSR_REG_EMB_FUNC_INT2_ADDR;
1564 *ef_irq_reg = ST_LSM6DSR_REG_MD2_CFG_ADDR;
1565 *drdy_reg = ST_LSM6DSR_REG_INT2_CTRL_ADDR;
1566 break;
1567 default:
1568 dev_err(hw->dev, "unsupported interrupt pin\n");
1569 err = -EINVAL;
1570 break;
1571 }
1572
1573 return err;
1574 }
1575
st_lsm6dsr_reset_device(struct st_lsm6dsr_hw * hw)1576 static int st_lsm6dsr_reset_device(struct st_lsm6dsr_hw *hw)
1577 {
1578 int err;
1579
1580 /* disable I3C */
1581 err = st_lsm6dsr_write_with_mask(hw, ST_LSM6DSR_REG_CTRL9_XL_ADDR,
1582 ST_LSM6DSR_REG_I3C_DISABLE_MASK, 1);
1583 if (err < 0)
1584 return err;
1585
1586 /* sw reset */
1587 err = st_lsm6dsr_write_with_mask(hw,
1588 ST_LSM6DSR_REG_CTRL3_C_ADDR,
1589 ST_LSM6DSR_REG_SW_RESET_MASK, 1);
1590 if (err < 0)
1591 return err;
1592
1593 usleep_range(15, 20);
1594
1595 /* boot */
1596 err = st_lsm6dsr_write_with_mask(hw,
1597 ST_LSM6DSR_REG_CTRL3_C_ADDR,
1598 ST_LSM6DSR_REG_BOOT_MASK, 1);
1599 /* init L_LACTIVE */
1600 err = st_lsm6dsr_write_with_mask(hw,
1601 ST_LSM6DSR_REG_CTRL3_C_ADDR,
1602 ST_LSM6DSR_REG_H_LACTIVE_MASK, 1);
1603
1604 msleep(20);
1605
1606 return err;
1607 }
1608
st_lsm6dsr_init_device(struct st_lsm6dsr_hw * hw)1609 static int st_lsm6dsr_init_device(struct st_lsm6dsr_hw *hw)
1610 {
1611 u8 drdy_reg, ef_irq_reg;
1612 int err;
1613
1614 /* configure latch interrupts enabled */
1615 err = st_lsm6dsr_write_with_mask(hw,
1616 ST_LSM6DSR_REG_TAP_CFG0_ADDR,
1617 ST_LSM6DSR_REG_LIR_MASK, 1);
1618 if (err < 0)
1619 return err;
1620
1621 /* enable Block Data Update */
1622 err = st_lsm6dsr_write_with_mask(hw,
1623 ST_LSM6DSR_REG_CTRL3_C_ADDR,
1624 ST_LSM6DSR_REG_BDU_MASK, 1);
1625 if (err < 0)
1626 return err;
1627
1628 /* enable rounding for fast FIFO reading */
1629 err = st_lsm6dsr_write_with_mask(hw,
1630 ST_LSM6DSR_REG_CTRL5_C_ADDR,
1631 ST_LSM6DSR_REG_ROUNDING_MASK, 3);
1632 if (err < 0)
1633 return err;
1634
1635 /* init timestamp engine */
1636 err = st_lsm6dsr_write_with_mask(hw,
1637 ST_LSM6DSR_REG_CTRL10_C_ADDR,
1638 ST_LSM6DSR_REG_TIMESTAMP_EN_MASK, 1);
1639 if (err < 0)
1640 return err;
1641
1642 /* configure interrupt registers */
1643 err = st_lsm6dsr_get_int_reg(hw, &drdy_reg, &ef_irq_reg);
1644 if (err < 0)
1645 return err;
1646
1647 /* Enable DRDY MASK for filters settling time */
1648 err = st_lsm6dsr_write_with_mask(hw, ST_LSM6DSR_REG_CTRL4_C_ADDR,
1649 ST_LSM6DSR_REG_DRDY_MASK, 1);
1650 if (err < 0)
1651 return err;
1652
1653 /* enable FIFO watermak interrupt */
1654 err = st_lsm6dsr_write_with_mask(hw, drdy_reg,
1655 ST_LSM6DSR_REG_INT_FIFO_TH_MASK, 1);
1656 if (err < 0)
1657 return err;
1658
1659 /* enable enbedded function interrupts */
1660 err = st_lsm6dsr_write_with_mask(hw, ef_irq_reg,
1661 ST_LSM6DSR_REG_INT_EMB_FUNC_MASK, 1);
1662 if (err < 0)
1663 return err;
1664
1665 /* init finite state machine */
1666 return st_lsm6dsr_fsm_init(hw);
1667 }
1668
st_lsm6dsr_init_fsm(struct st_lsm6dsr_hw * hw)1669 static int st_lsm6dsr_init_fsm(struct st_lsm6dsr_hw *hw)
1670 {
1671 int i;
1672 int err;
1673
1674 /* yxsui init config fsm */
1675 mutex_lock(&hw->page_lock);
1676 for (i = 0; i < ARRAY_SIZE(flat_roll_inward_x_y); i++) {
1677 //err = hw->tf->write(hw->dev, addr, sizeof(data), &data);
1678 err = hw->tf->write(hw->dev, flat_roll_inward_x_y[i].address,
1679 sizeof(flat_roll_inward_x_y[i].data),
1680 &(flat_roll_inward_x_y[i].data));
1681 if (err < 0)
1682 dev_info(hw->dev, "yxsui init config fsm fail\n");
1683 }
1684 mutex_unlock(&hw->page_lock);
1685
1686 return 0;
1687 }
1688
1689 /**
1690 * Allocate IIO device
1691 *
1692 * @param hw: ST IMU MEMS hw instance.
1693 * @param id: Sensor Identifier.
1694 * @retval struct iio_dev *, NULL if ERROR
1695 */
st_lsm6dsr_alloc_iiodev(struct st_lsm6dsr_hw * hw,enum st_lsm6dsr_sensor_id id)1696 static struct iio_dev *st_lsm6dsr_alloc_iiodev(struct st_lsm6dsr_hw *hw,
1697 enum st_lsm6dsr_sensor_id id)
1698 {
1699 struct st_lsm6dsr_sensor *sensor;
1700 struct iio_dev *iio_dev;
1701
1702 iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor));
1703 if (!iio_dev)
1704 return NULL;
1705
1706 iio_dev->modes = INDIO_DIRECT_MODE;
1707 iio_dev->dev.parent = hw->dev;
1708
1709 sensor = iio_priv(iio_dev);
1710 sensor->id = id;
1711 sensor->hw = hw;
1712 sensor->watermark = 1;
1713
1714 sensor->decimator = 0;
1715 sensor->dec_counter = 0;
1716
1717 switch (id) {
1718 case ST_LSM6DSR_ID_ACC:
1719 iio_dev->channels = st_lsm6dsr_acc_channels;
1720 iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsr_acc_channels);
1721 iio_dev->name = "lsm6dsr_accel";
1722 iio_dev->info = &st_lsm6dsr_acc_info;
1723 iio_dev->available_scan_masks = st_lsm6dsr_available_scan_masks;
1724
1725 sensor->batch_reg.addr = ST_LSM6DSR_REG_FIFO_CTRL3_ADDR;
1726 sensor->batch_reg.mask = ST_LSM6DSR_REG_BDR_XL_MASK;
1727 sensor->max_watermark = ST_LSM6DSR_MAX_FIFO_DEPTH;
1728 sensor->odr = st_lsm6dsr_odr_table[id].odr_avl[1].hz;
1729 sensor->uodr = st_lsm6dsr_odr_table[id].odr_avl[1].uhz;
1730 st_lsm6dsr_set_full_scale(sensor,
1731 st_lsm6dsr_fs_table[id].fs_avl[1].gain);
1732 break;
1733 case ST_LSM6DSR_ID_GYRO:
1734 iio_dev->channels = st_lsm6dsr_gyro_channels;
1735 iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsr_gyro_channels);
1736 iio_dev->name = "lsm6dsr_gyro";
1737 iio_dev->info = &st_lsm6dsr_gyro_info;
1738 iio_dev->available_scan_masks = st_lsm6dsr_available_scan_masks;
1739
1740 sensor->batch_reg.addr = ST_LSM6DSR_REG_FIFO_CTRL3_ADDR;
1741 sensor->batch_reg.mask = ST_LSM6DSR_REG_BDR_GY_MASK;
1742 sensor->max_watermark = ST_LSM6DSR_MAX_FIFO_DEPTH;
1743 sensor->odr = st_lsm6dsr_odr_table[id].odr_avl[1].hz;
1744 sensor->uodr = st_lsm6dsr_odr_table[id].odr_avl[1].uhz;
1745 st_lsm6dsr_set_full_scale(sensor,
1746 st_lsm6dsr_fs_table[id].fs_avl[2].gain);
1747 break;
1748 case ST_LSM6DSR_ID_TEMP:
1749 iio_dev->channels = st_lsm6dsr_temp_channels;
1750 iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsr_temp_channels);
1751 iio_dev->name = "lsm6dsr_temp";
1752 iio_dev->info = &st_lsm6dsr_temp_info;
1753
1754 sensor->batch_reg.addr = ST_LSM6DSR_REG_FIFO_CTRL4_ADDR;
1755 sensor->batch_reg.mask = ST_LSM6DSR_REG_ODR_T_BATCH_MASK;
1756 sensor->max_watermark = ST_LSM6DSR_MAX_FIFO_DEPTH;
1757 sensor->odr = st_lsm6dsr_odr_table[id].odr_avl[1].hz;
1758 sensor->uodr = st_lsm6dsr_odr_table[id].odr_avl[1].uhz;
1759 sensor->gain = st_lsm6dsr_fs_table[id].fs_avl[0].gain;
1760 sensor->offset = ST_LSM6DSR_TEMP_OFFSET;
1761 break;
1762 case ST_LSM6DSR_ID_STEP_COUNTER:
1763 iio_dev->channels = st_lsm6dsr_step_counter_channels;
1764 iio_dev->num_channels =
1765 ARRAY_SIZE(st_lsm6dsr_step_counter_channels);
1766 iio_dev->name = "lsm6dsr_step_c";
1767 iio_dev->info = &st_lsm6dsr_step_counter_info;
1768 iio_dev->available_scan_masks =
1769 st_lsm6dsr_sc_available_scan_masks;
1770
1771 sensor->max_watermark = 1;
1772 sensor->odr =
1773 st_lsm6dsr_odr_table[ST_LSM6DSR_ID_ACC].odr_avl[2].hz;
1774 sensor->uodr =
1775 st_lsm6dsr_odr_table[ST_LSM6DSR_ID_ACC].odr_avl[2].uhz;
1776 break;
1777 case ST_LSM6DSR_ID_STEP_DETECTOR:
1778 iio_dev->channels = st_lsm6dsr_step_detector_channels;
1779 iio_dev->num_channels =
1780 ARRAY_SIZE(st_lsm6dsr_step_detector_channels);
1781 iio_dev->name = "lsm6dsr_step_d";
1782 iio_dev->info = &st_lsm6dsr_step_detector_info;
1783 iio_dev->available_scan_masks =
1784 st_lsm6dsr_sc_available_scan_masks;
1785
1786 sensor->odr =
1787 st_lsm6dsr_odr_table[ST_LSM6DSR_ID_ACC].odr_avl[2].hz;
1788 sensor->uodr =
1789 st_lsm6dsr_odr_table[ST_LSM6DSR_ID_ACC].odr_avl[2].uhz;
1790 break;
1791 case ST_LSM6DSR_ID_SIGN_MOTION:
1792 iio_dev->channels = st_lsm6dsr_sign_motion_channels;
1793 iio_dev->num_channels =
1794 ARRAY_SIZE(st_lsm6dsr_sign_motion_channels);
1795 iio_dev->name = "lsm6dsr_sign_motion";
1796 iio_dev->info = &st_lsm6dsr_sign_motion_info;
1797 iio_dev->available_scan_masks =
1798 st_lsm6dsr_sc_available_scan_masks;
1799
1800 sensor->odr =
1801 st_lsm6dsr_odr_table[ST_LSM6DSR_ID_ACC].odr_avl[2].hz;
1802 sensor->uodr =
1803 st_lsm6dsr_odr_table[ST_LSM6DSR_ID_ACC].odr_avl[2].uhz;
1804 break;
1805 case ST_LSM6DSR_ID_TILT:
1806 iio_dev->channels = st_lsm6dsr_tilt_channels;
1807 iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsr_tilt_channels);
1808 iio_dev->name = "lsm6dsr_tilt";
1809 iio_dev->info = &st_lsm6dsr_tilt_info;
1810 iio_dev->available_scan_masks =
1811 st_lsm6dsr_sc_available_scan_masks;
1812
1813 sensor->odr =
1814 st_lsm6dsr_odr_table[ST_LSM6DSR_ID_ACC].odr_avl[2].hz;
1815 sensor->uodr =
1816 st_lsm6dsr_odr_table[ST_LSM6DSR_ID_ACC].odr_avl[2].uhz;
1817 break;
1818 case ST_LSM6DSR_ID_GLANCE:
1819 iio_dev->channels = st_lsm6dsr_glance_channels;
1820 iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsr_glance_channels);
1821 iio_dev->name = "lsm6dsr_glance";
1822 iio_dev->info = &st_lsm6dsr_glance_info;
1823 iio_dev->available_scan_masks =
1824 st_lsm6dsr_sc_available_scan_masks;
1825
1826 sensor->odr =
1827 st_lsm6dsr_odr_table[ST_LSM6DSR_ID_ACC].odr_avl[2].hz;
1828 sensor->uodr =
1829 st_lsm6dsr_odr_table[ST_LSM6DSR_ID_ACC].odr_avl[2].uhz;
1830 break;
1831 case ST_LSM6DSR_ID_MOTION:
1832 iio_dev->channels = st_lsm6dsr_motion_channels;
1833 iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsr_motion_channels);
1834 iio_dev->name = "lsm6dsr_motion";
1835 iio_dev->info = &st_lsm6dsr_motion_info;
1836 iio_dev->available_scan_masks =
1837 st_lsm6dsr_sc_available_scan_masks;
1838
1839 sensor->odr =
1840 st_lsm6dsr_odr_table[ST_LSM6DSR_ID_ACC].odr_avl[2].hz;
1841 sensor->uodr =
1842 st_lsm6dsr_odr_table[ST_LSM6DSR_ID_ACC].odr_avl[2].uhz;
1843 break;
1844 case ST_LSM6DSR_ID_NO_MOTION:
1845 iio_dev->channels = st_lsm6dsr_no_motion_channels;
1846 iio_dev->num_channels =
1847 ARRAY_SIZE(st_lsm6dsr_no_motion_channels);
1848 iio_dev->name = "lsm6dsr_no_motion";
1849 iio_dev->info = &st_lsm6dsr_no_motion_info;
1850 iio_dev->available_scan_masks =
1851 st_lsm6dsr_sc_available_scan_masks;
1852
1853 sensor->odr =
1854 st_lsm6dsr_odr_table[ST_LSM6DSR_ID_ACC].odr_avl[2].hz;
1855 sensor->uodr =
1856 st_lsm6dsr_odr_table[ST_LSM6DSR_ID_ACC].odr_avl[2].uhz;
1857 break;
1858 case ST_LSM6DSR_ID_WAKEUP:
1859 iio_dev->channels = st_lsm6dsr_wakeup_channels;
1860 iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsr_wakeup_channels);
1861 iio_dev->name = "lsm6dsr_wk";
1862 iio_dev->info = &st_lsm6dsr_wakeup_info;
1863 iio_dev->available_scan_masks =
1864 st_lsm6dsr_sc_available_scan_masks;
1865
1866 sensor->odr =
1867 st_lsm6dsr_odr_table[ST_LSM6DSR_ID_ACC].odr_avl[2].hz;
1868 sensor->uodr =
1869 st_lsm6dsr_odr_table[ST_LSM6DSR_ID_ACC].odr_avl[2].uhz;
1870 break;
1871 case ST_LSM6DSR_ID_PICKUP:
1872 iio_dev->channels = st_lsm6dsr_pickup_channels;
1873 iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsr_pickup_channels);
1874 iio_dev->name = "lsm6dsr_pickup";
1875 iio_dev->info = &st_lsm6dsr_pickup_info;
1876 iio_dev->available_scan_masks =
1877 st_lsm6dsr_sc_available_scan_masks;
1878
1879 sensor->odr =
1880 st_lsm6dsr_odr_table[ST_LSM6DSR_ID_ACC].odr_avl[2].hz;
1881 sensor->uodr =
1882 st_lsm6dsr_odr_table[ST_LSM6DSR_ID_ACC].odr_avl[2].uhz;
1883 break;
1884 case ST_LSM6DSR_ID_ORIENTATION:
1885 iio_dev->channels = st_lsm6dsr_orientation_channels;
1886 iio_dev->num_channels =
1887 ARRAY_SIZE(st_lsm6dsr_orientation_channels);
1888 iio_dev->name = "lsm6dsr_dev_orientation";
1889 iio_dev->info = &st_lsm6dsr_orientation_info;
1890 iio_dev->available_scan_masks =
1891 st_lsm6dsr_sc_available_scan_masks;
1892
1893 sensor->odr =
1894 st_lsm6dsr_odr_table[ST_LSM6DSR_ID_ACC].odr_avl[2].hz;
1895 sensor->uodr =
1896 st_lsm6dsr_odr_table[ST_LSM6DSR_ID_ACC].odr_avl[2].uhz;
1897 break;
1898 case ST_LSM6DSR_ID_WRIST_TILT:
1899 iio_dev->channels = st_lsm6dsr_wrist_channels;
1900 iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsr_wrist_channels);
1901 iio_dev->name = "lsm6dsr_wrist";
1902 iio_dev->info = &st_lsm6dsr_wrist_info;
1903 iio_dev->available_scan_masks =
1904 st_lsm6dsr_sc_available_scan_masks;
1905
1906 sensor->odr =
1907 st_lsm6dsr_odr_table[ST_LSM6DSR_ID_ACC].odr_avl[2].hz;
1908 sensor->uodr =
1909 st_lsm6dsr_odr_table[ST_LSM6DSR_ID_ACC].odr_avl[2].uhz;
1910 break;
1911 default:
1912 return NULL;
1913 }
1914
1915 return iio_dev;
1916 }
1917
1918 #ifdef ST_LSM6DSR_INPUT_TS
st_lsm6dsr_get_time_ns(void)1919 static inline s64 st_lsm6dsr_get_time_ns(void)
1920 {
1921 struct timespec ts;
1922
1923 get_monotonic_boottime(&ts);
1924 return timespec_to_ns(&ts);
1925 }
1926 #endif
1927
st_lsm6dsr_input_init(struct st_lsm6dsr_hw * hw)1928 static int st_lsm6dsr_input_init(struct st_lsm6dsr_hw *hw)
1929 {
1930 struct input_dev *dev = NULL;
1931 int err = 0;
1932
1933 // acc
1934 dev = input_allocate_device();
1935 if (!dev) {
1936 dev_err(hw->dev, "can't allocate device!\n");
1937 return -ENOMEM;
1938 }
1939 dev->name = "st_lsm6dsr_acc";
1940 dev->id.bustype = BUS_I2C;
1941 input_set_capability(dev, EV_ABS, ABS_X);
1942 input_set_capability(dev, EV_ABS, ABS_Y);
1943 input_set_capability(dev, EV_ABS, ABS_Z);
1944 #ifdef ST_LSM6DSR_INPUT_TS
1945 input_set_capability(dev, EV_ABS, MSC_TIMESTAMP);
1946 #endif
1947 input_set_capability(dev, EV_ABS, ABS_THROTTLE);
1948 input_set_abs_params(dev, ABS_X, (-32*1000), (32*1000), 0, 0);
1949 input_set_abs_params(dev, ABS_Y, (-32*1000), (32*1000), 0, 0);
1950 input_set_abs_params(dev, ABS_Z, (-32*1000), (32*1000), 0, 0);
1951 input_set_abs_params(dev, ABS_THROTTLE, 0, 1, 0, 0);
1952 err = input_register_device(dev);
1953 if (err < 0) {
1954 dev_err(hw->dev, "can't register device!\n");
1955 input_free_device(dev);
1956 return err;
1957 }
1958 acc_input = dev;
1959
1960 // gyro
1961 dev = input_allocate_device();
1962 if (!dev) {
1963 dev_err(hw->dev, "can't allocate device!\n");
1964 return -ENOMEM;
1965 }
1966 dev->name = "st_lsm6dsr_gyro";
1967 dev->id.bustype = BUS_I2C;
1968 input_set_capability(dev, EV_ABS, ABS_RX);
1969 input_set_capability(dev, EV_ABS, ABS_RY);
1970 input_set_capability(dev, EV_ABS, ABS_RZ);
1971 #ifdef ST_LSM6DSR_INPUT_TS
1972 input_set_capability(dev, EV_ABS, MSC_TIMESTAMP);
1973 #endif
1974 input_set_capability(dev, EV_ABS, ABS_THROTTLE);
1975 input_set_abs_params(dev, ABS_RX, (-32*1000), (32*1000), 0, 0);
1976 input_set_abs_params(dev, ABS_RY, (-32*1000), (32*1000), 0, 0);
1977 input_set_abs_params(dev, ABS_RZ, (-32*1000), (32*1000), 0, 0);
1978 input_set_abs_params(dev, ABS_THROTTLE, 0, 1, 0, 0);
1979 err = input_register_device(dev);
1980 if (err < 0) {
1981 dev_err(hw->dev, "can't register device!\n");
1982 input_free_device(dev);
1983 return err;
1984 }
1985 gyro_input = dev;
1986
1987 return 0;
1988 }
1989
data_work_func(struct work_struct * work)1990 static void data_work_func(struct work_struct *work)
1991 {
1992 int err;
1993 unsigned char data[12];
1994 int acc_out[3];
1995 int gyro_out[3];
1996 #ifdef ST_LSM6DSR_INPUT_TS
1997 s64 ts;
1998 #endif
1999
2000 if (lsm6dsr_enable == 1) {
2001 err = hw_input->tf->read(hw_input->dev,
2002 ST_LSM6DSR_REG_OUTX_L_G_ADDR,
2003 sizeof(data), data);
2004 #ifdef ST_LSM6DSR_INPUT_TS
2005 ts = st_lsm6dsr_get_time_ns();
2006 #endif
2007 gyro_out[0] = (short)((data[1]<<8) | (data[0]));
2008 gyro_out[1] = (short)((data[3]<<8) | (data[2]));
2009 gyro_out[2] = (short)((data[5]<<8) | (data[4]));
2010 acc_out[0] = (short)((data[7]<<8) | (data[6]));
2011 acc_out[1] = (short)((data[9]<<8) | (data[8]));
2012 acc_out[2] = (short)((data[11]<<8) | (data[10]));
2013
2014 input_report_abs(acc_input, ABS_X, acc_out[0]);
2015 input_report_abs(acc_input, ABS_Y, acc_out[1]);
2016 input_report_abs(acc_input, ABS_Z, acc_out[2]);
2017 #ifdef ST_LSM6DSR_INPUT_TS
2018 input_report_abs(acc_input, MSC_TIMESTAMP, ts);
2019 #endif
2020 input_report_abs(acc_input, ABS_THROTTLE, 1); // acc
2021 input_sync(acc_input);
2022
2023 input_report_abs(gyro_input, ABS_RX, gyro_out[0]);
2024 input_report_abs(gyro_input, ABS_RY, gyro_out[1]);
2025 input_report_abs(gyro_input, ABS_RZ, gyro_out[2]);
2026 #ifdef ST_LSM6DSR_INPUT_TS
2027 input_report_abs(gyro_input, MSC_TIMESTAMP, ts);
2028 #endif
2029 input_report_abs(gyro_input, ABS_THROTTLE, 2); // gyro
2030 input_sync(gyro_input);
2031
2032 }
2033 schedule_delayed_work(&data_work, 10); // ms
2034 }
2035
imuenable_store(struct device * dev,struct device_attribute * attr,const char * buf,size_t count)2036 static ssize_t imuenable_store(struct device *dev,
2037 struct device_attribute *attr, const char *buf,
2038 size_t count)
2039 {
2040 int i, err = 0;
2041 uint32_t databuf[1] = { 0 };
2042
2043 err = kstrtou32(buf, 16, &databuf[0]);
2044 if (!err) {
2045 if (databuf[0] == 0) {
2046 lsm6dsr_enable = 0;
2047 for (i = 0; i < 2; i++) {
2048 // power off enabled sensors
2049 if (i == 1)
2050 st_lsm6dsr_set_odr(sensor[i], 26, 0);
2051 else
2052 st_lsm6dsr_sensor_set_enable(sensor[i], false);
2053 }
2054 } else if (databuf[0] == 1) {
2055 lsm6dsr_enable = 1;
2056 for (i = 0; i < 2; i++) {
2057 // power off enabled sensors
2058 st_lsm6dsr_sensor_set_enable(sensor[i], true);
2059 }
2060 }
2061 }
2062
2063 return count;
2064 }
2065
imuenable_show(struct device * dev,struct device_attribute * attr,char * buf)2066 static ssize_t imuenable_show(struct device *dev,
2067 struct device_attribute *attr, char *buf)
2068 {
2069 int err;
2070 unsigned char data[1];
2071
2072 err = hw_input->tf->read(hw_input->dev, 0x10, sizeof(data), data);
2073
2074 err = hw_input->tf->read(hw_input->dev, 0x11, sizeof(data), data);
2075
2076 err = hw_input->tf->read(hw_input->dev, 0x12, sizeof(data), data);
2077
2078 err = hw_input->tf->read(hw_input->dev, 0x13, sizeof(data), data);
2079
2080 err = hw_input->tf->read(hw_input->dev, 0x15, sizeof(data), data);
2081
2082 err = hw_input->tf->read(hw_input->dev, 0x17, sizeof(data), data);
2083
2084 return 0;
2085 }
2086
2087 static DEVICE_ATTR_RW(imuenable);
2088
2089 static struct attribute *lsm6dsr_attributes[] = {
2090 &dev_attr_imuenable.attr,
2091 NULL
2092 };
2093
2094 static struct attribute_group lsm6dsr_attribute_group = {
2095 .attrs = lsm6dsr_attributes
2096 };
2097
2098 /**
2099 * Probe device function
2100 * Implements [MODULE] feature for Power Management
2101 *
2102 * @param dev: Device pointer.
2103 * @param irq: I2C/SPI client irq.
2104 * @param tf_ops: Bus Transfer Function pointer.
2105 * @retval struct iio_dev *, NULL if ERROR
2106 */
st_lsm6dsr_probe(struct device * dev,int irq,const struct st_lsm6dsr_transfer_function * tf_ops)2107 int st_lsm6dsr_probe(struct device *dev, int irq,
2108 const struct st_lsm6dsr_transfer_function *tf_ops)
2109 {
2110 struct st_lsm6dsr_hw *hw;
2111 int i, err;
2112
2113 hw = devm_kzalloc(dev, sizeof(*hw), GFP_KERNEL);
2114 if (!hw)
2115 return -ENOMEM;
2116
2117 dev_set_drvdata(dev, (void *)hw);
2118
2119 mutex_init(&hw->lock);
2120 mutex_init(&hw->fifo_lock);
2121 mutex_init(&hw->page_lock);
2122
2123 hw->dev = dev;
2124 hw->irq = irq;
2125 hw->tf = tf_ops;
2126
2127 /* attribute */
2128 err = sysfs_create_group(&hw->dev->kobj, &lsm6dsr_attribute_group);
2129 if (err < 0)
2130 return err;
2131
2132 err = st_lsm6dsr_check_whoami(hw);
2133 if (err < 0)
2134 return err;
2135
2136 err = st_lsm6dsr_get_odr_calibration(hw);
2137 if (err < 0)
2138 return err;
2139
2140 err = st_lsm6dsr_reset_device(hw);
2141 if (err < 0)
2142 return err;
2143
2144 err = st_lsm6dsr_init_device(hw);
2145 if (err < 0)
2146 return err;
2147
2148 for (i = 0; i < ARRAY_SIZE(st_lsm6dsr_main_sensor_list); i++) {
2149 enum st_lsm6dsr_sensor_id id = st_lsm6dsr_main_sensor_list[i];
2150
2151 hw->iio_devs[id] = st_lsm6dsr_alloc_iiodev(hw, id);
2152 if (!hw->iio_devs[id])
2153 return -ENOMEM;
2154 }
2155
2156 err = st_lsm6dsr_shub_probe(hw);
2157 if (err < 0)
2158 return err;
2159
2160 if (hw->irq > 0) {
2161 err = st_lsm6dsr_buffers_setup(hw);
2162 if (err < 0)
2163 return err;
2164 }
2165
2166 for (i = 0; i < ST_LSM6DSR_ID_MAX; i++) {
2167 if (!hw->iio_devs[i])
2168 continue;
2169
2170 sensor[i] = iio_priv(hw->iio_devs[i]);
2171
2172 err = devm_iio_device_register(hw->dev, hw->iio_devs[i]);
2173 if (err)
2174 return err;
2175 }
2176
2177 #if defined(CONFIG_PM) && defined(CONFIG_IIO_ST_LSM6DSR_MAY_WAKEUP)
2178 err = device_init_wakeup(dev, 1);
2179 if (err)
2180 return err;
2181 #endif /* CONFIG_PM && CONFIG_IIO_ST_LSM6DSR_MAY_WAKEUP */
2182
2183 err = st_lsm6dsr_input_init(hw);
2184 if (err < 0)
2185 return err;
2186
2187 INIT_DELAYED_WORK(&data_work, data_work_func);
2188 schedule_delayed_work(&data_work, 10);
2189
2190 err = st_lsm6dsr_init_fsm(hw);
2191 if (err < 0)
2192 return err;
2193
2194 hw_input = hw;
2195 dev_info(dev, "Device probed\n");
2196
2197 return 0;
2198 }
2199 EXPORT_SYMBOL(st_lsm6dsr_probe);
2200
2201
st_lsm6dsr_remove(struct device * dev)2202 int st_lsm6dsr_remove(struct device *dev)
2203 {
2204 struct st_lsm6dsr_hw *hw = dev_get_drvdata(dev);
2205
2206 sysfs_remove_group(&hw->dev->kobj, &lsm6dsr_attribute_group);
2207
2208 return st_lsm6dsr_deallocate_buffers(hw);
2209 }
2210 EXPORT_SYMBOL(st_lsm6dsr_remove);
2211
st_lsm6dsr_bk_regs(struct st_lsm6dsr_hw * hw)2212 static int __maybe_unused st_lsm6dsr_bk_regs(struct st_lsm6dsr_hw *hw)
2213 {
2214 int i, err = 0;
2215 u8 data, addr;
2216
2217 mutex_lock(&hw->page_lock);
2218 for (i = 0; i < ST_LSM6DSR_SUSPEND_RESUME_REGS; i++) {
2219 addr = st_lsm6dsr_suspend_resume[i].addr;
2220 err = hw->tf->read(hw->dev, addr, sizeof(data), &data);
2221 if (err < 0) {
2222 dev_err(hw->dev, "failed to read whoami register\n");
2223 goto out_lock;
2224 }
2225
2226 st_lsm6dsr_suspend_resume[i].val = data;
2227 }
2228
2229 out_lock:
2230 mutex_unlock(&hw->page_lock);
2231
2232 return err;
2233 }
2234
st_lsm6dsr_restore_regs(struct st_lsm6dsr_hw * hw)2235 static int __maybe_unused st_lsm6dsr_restore_regs(struct st_lsm6dsr_hw *hw)
2236 {
2237 int i, err = 0;
2238 u8 data, addr;
2239
2240 mutex_lock(&hw->page_lock);
2241 for (i = 0; i < ST_LSM6DSR_SUSPEND_RESUME_REGS; i++) {
2242 addr = st_lsm6dsr_suspend_resume[i].addr;
2243 err = hw->tf->read(hw->dev, addr, sizeof(data), &data);
2244 if (err < 0) {
2245 dev_err(hw->dev, "failed to read %02x reg\n", addr);
2246 goto out_lock;
2247 }
2248
2249 data &= ~st_lsm6dsr_suspend_resume[i].mask;
2250 data |= (st_lsm6dsr_suspend_resume[i].val &
2251 st_lsm6dsr_suspend_resume[i].mask);
2252
2253 err = hw->tf->write(hw->dev, addr, sizeof(data), &data);
2254 if (err < 0) {
2255 dev_err(hw->dev, "failed to write %02x reg\n", addr);
2256 goto out_lock;
2257 }
2258 }
2259
2260 out_lock:
2261 mutex_unlock(&hw->page_lock);
2262
2263 return err;
2264 }
2265
2266 /**
2267 * Power Management suspend callback [MODULE]
2268 * Implements [MODULE] feature for Power Management
2269 *
2270 * @param dev: Device pointer.
2271 * @retval 0 is OK, negative value if ERROR
2272 */
st_lsm6dsr_suspend(struct device * dev)2273 static int __maybe_unused st_lsm6dsr_suspend(struct device *dev)
2274 {
2275 #ifdef ST_LSM6DSR_PM_IMPLEMENTED
2276 struct st_lsm6dsr_hw *hw = dev_get_drvdata(dev);
2277 struct st_lsm6dsr_sensor *sensor;
2278 int i, err = 0;
2279
2280 dev_info(dev, "Suspending device\n");
2281
2282 for (i = 0; i < ST_LSM6DSR_ID_MAX; i++) {
2283 if (!hw->iio_devs[i])
2284 continue;
2285
2286 sensor = iio_priv(hw->iio_devs[i]);
2287 if (!(hw->enable_mask & BIT(sensor->id)))
2288 continue;
2289
2290 if (hw->enable_mask & (BIT(ST_LSM6DSR_ID_STEP_COUNTER) |
2291 BIT(ST_LSM6DSR_ID_GYRO) |
2292 BIT(ST_LSM6DSR_ID_ACC) |
2293 BIT(ST_LSM6DSR_ID_EXT0) |
2294 BIT(ST_LSM6DSR_ID_EXT1))) {
2295 err = st_lsm6dsr_suspend_fifo(hw);
2296 if (err < 0)
2297 return err;
2298 }
2299
2300 // power off enabled sensors
2301 if (sensor->id == ST_LSM6DSR_ID_EXT0 ||
2302 sensor->id == ST_LSM6DSR_ID_EXT1)
2303 err = st_lsm6dsr_shub_set_enable(sensor, false);
2304 else
2305 err = st_lsm6dsr_sensor_set_enable(sensor, false);
2306 if (err < 0)
2307 return err;
2308
2309 hw->suspend_mask |= BIT(sensor->id);
2310 }
2311
2312 #ifndef ST_LSM6DSR_PM_REGULATOR_CTL
2313 err = st_lsm6dsr_bk_regs(hw);
2314 #endif
2315
2316 #ifdef CONFIG_IIO_ST_LSM6DSR_MAY_WAKEUP
2317 if (device_may_wakeup(dev))
2318 enable_irq_wake(hw->irq);
2319 #endif
2320
2321 return err < 0 ? err : 0;
2322 #endif
2323
2324 return 0;
2325 }
2326
2327 /**
2328 * Power Management resume callback [MODULE]
2329 * Implements [MODULE] feature for Power Management
2330 *
2331 * @param dev: Device pointer.
2332 * @retval 0 is OK, negative value if ERROR
2333 */
st_lsm6dsr_resume(struct device * dev)2334 static int __maybe_unused st_lsm6dsr_resume(struct device *dev)
2335 {
2336 #ifdef ST_LSM6DSR_PM_IMPLEMENTED
2337 struct st_lsm6dsr_hw *hw = dev_get_drvdata(dev);
2338 struct st_lsm6dsr_sensor *sensor;
2339 int i, err = 0;
2340
2341 dev_info(dev, "Resuming device\n");
2342
2343 #ifdef CONFIG_IIO_ST_LSM6DSR_MAY_WAKEUP
2344 if (device_may_wakeup(dev))
2345 disable_irq_wake(hw->irq);
2346 #endif
2347
2348 #ifdef ST_LSM6DSR_PM_REGULATOR_CTL
2349 err = st_lsm6dsr_reset_device(hw);
2350 if (err < 0)
2351 return err;
2352
2353 err = st_lsm6dsr_init_device(hw);
2354 if (err < 0)
2355 return err;
2356
2357 err = st_lsm6dsr_irq_setup(hw);
2358 if (err < 0)
2359 return err;
2360
2361 err = st_lsm6dsr_init_fsm(hw);
2362 if (err < 0)
2363 return err;
2364 #else
2365 err = st_lsm6dsr_restore_regs(hw);
2366 if (err < 0)
2367 return err;
2368 #endif
2369
2370 // power on suspened sensors
2371 for (i = 0; i < ST_LSM6DSR_ID_MAX; i++) {
2372 if (!hw->iio_devs[i])
2373 continue;
2374
2375 sensor = iio_priv(hw->iio_devs[i]);
2376 if (!(hw->suspend_mask & BIT(sensor->id)))
2377 continue;
2378
2379 if (sensor->id == ST_LSM6DSR_ID_EXT0 ||
2380 sensor->id == ST_LSM6DSR_ID_EXT1)
2381 err = st_lsm6dsr_shub_set_enable(sensor, true);
2382 else
2383 err = st_lsm6dsr_sensor_set_enable(sensor, true);
2384
2385 if (err < 0)
2386 return err;
2387
2388 if (sensor->id == ST_LSM6DSR_ID_STEP_COUNTER ||
2389 sensor->id == ST_LSM6DSR_ID_GYRO ||
2390 sensor->id == ST_LSM6DSR_ID_ACC ||
2391 sensor->id == ST_LSM6DSR_ID_EXT0 ||
2392 sensor->id == ST_LSM6DSR_ID_EXT1) {
2393 err = st_lsm6dsr_set_fifo_mode(hw,
2394 ST_LSM6DSR_FIFO_CONT);
2395 if (err < 0)
2396 return err;
2397 }
2398
2399 hw->suspend_mask &= ~BIT(sensor->id);
2400 }
2401
2402 err = st_lsm6dsr_reset_hwts(hw);
2403 if (err < 0)
2404 return err;
2405 #endif
2406
2407 return 0;
2408 }
2409
2410 const struct dev_pm_ops st_lsm6dsr_pm_ops = {
2411 SET_SYSTEM_SLEEP_PM_OPS(st_lsm6dsr_suspend, st_lsm6dsr_resume)
2412 };
2413 EXPORT_SYMBOL(st_lsm6dsr_pm_ops);
2414
2415 MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi@st.com>");
2416 MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsr driver");
2417 MODULE_LICENSE("GPL");
2418