1 /* lsm6dsl.c - Driver for LSM6DSL accelerometer, gyroscope and
2 * temperature sensor
3 */
4
5 /*
6 * Copyright (c) 2017 Linaro Limited
7 *
8 * SPDX-License-Identifier: Apache-2.0
9 */
10
11 #define DT_DRV_COMPAT st_lsm6dsl
12
13 #include <zephyr/drivers/sensor.h>
14 #include <zephyr/kernel.h>
15 #include <zephyr/device.h>
16 #include <zephyr/init.h>
17 #include <string.h>
18 #include <zephyr/sys/byteorder.h>
19 #include <zephyr/sys/__assert.h>
20 #include <zephyr/logging/log.h>
21 #include <zephyr/pm/device.h>
22
23 #include "lsm6dsl.h"
24
25 LOG_MODULE_REGISTER(LSM6DSL, CONFIG_SENSOR_LOG_LEVEL);
26
27 static const uint16_t lsm6dsl_odr_map[] = {0, 12, 26, 52, 104, 208, 416, 833,
28 1666, 3332, 6664, 1};
29
30 #if defined(LSM6DSL_ACCEL_ODR_RUNTIME) || defined(LSM6DSL_GYRO_ODR_RUNTIME) ||\
31 defined(CONFIG_PM_DEVICE)
lsm6dsl_freq_to_odr_val(uint16_t freq)32 static int lsm6dsl_freq_to_odr_val(uint16_t freq)
33 {
34 size_t i;
35
36 for (i = 0; i < ARRAY_SIZE(lsm6dsl_odr_map); i++) {
37 if (freq == lsm6dsl_odr_map[i]) {
38 return i;
39 }
40 }
41
42 return -EINVAL;
43 }
44 #endif
45
lsm6dsl_odr_to_freq_val(uint16_t odr)46 static int lsm6dsl_odr_to_freq_val(uint16_t odr)
47 {
48 /* for valid index, return value from map */
49 if (odr < ARRAY_SIZE(lsm6dsl_odr_map)) {
50 return lsm6dsl_odr_map[odr];
51 }
52
53 /* invalid index, return the fastest entry (6.66kHz) */
54 BUILD_ASSERT(ARRAY_SIZE(lsm6dsl_odr_map) > 10);
55 return lsm6dsl_odr_map[10];
56 }
57
58 #ifdef LSM6DSL_ACCEL_FS_RUNTIME
59 static const uint16_t lsm6dsl_accel_fs_map[] = {2, 16, 4, 8};
60 static const uint16_t lsm6dsl_accel_fs_sens[] = {1, 8, 2, 4};
61
lsm6dsl_accel_range_to_fs_val(int32_t range)62 static int lsm6dsl_accel_range_to_fs_val(int32_t range)
63 {
64 size_t i;
65
66 for (i = 0; i < ARRAY_SIZE(lsm6dsl_accel_fs_map); i++) {
67 if (range == lsm6dsl_accel_fs_map[i]) {
68 return i;
69 }
70 }
71
72 return -EINVAL;
73 }
74 #endif
75
76 #ifdef LSM6DSL_GYRO_FS_RUNTIME
77 static const uint16_t lsm6dsl_gyro_fs_map[] = {250, 500, 1000, 2000, 125};
78 static const uint16_t lsm6dsl_gyro_fs_sens[] = {2, 4, 8, 16, 1};
79
lsm6dsl_gyro_range_to_fs_val(int32_t range)80 static int lsm6dsl_gyro_range_to_fs_val(int32_t range)
81 {
82 size_t i;
83
84 for (i = 0; i < ARRAY_SIZE(lsm6dsl_gyro_fs_map); i++) {
85 if (range == lsm6dsl_gyro_fs_map[i]) {
86 return i;
87 }
88 }
89
90 return -EINVAL;
91 }
92 #endif
93
lsm6dsl_reboot(const struct device * dev)94 static inline int lsm6dsl_reboot(const struct device *dev)
95 {
96 struct lsm6dsl_data *data = dev->data;
97
98 if (data->hw_tf->update_reg(dev, LSM6DSL_REG_CTRL3_C,
99 LSM6DSL_MASK_CTRL3_C_BOOT,
100 1 << LSM6DSL_SHIFT_CTRL3_C_BOOT) < 0) {
101 return -EIO;
102 }
103
104 /* Wait sensor turn-on time as per datasheet */
105 k_busy_wait(USEC_PER_MSEC * 35U);
106
107 return 0;
108 }
109
lsm6dsl_accel_set_fs_raw(const struct device * dev,uint8_t fs)110 static int lsm6dsl_accel_set_fs_raw(const struct device *dev, uint8_t fs)
111 {
112 struct lsm6dsl_data *data = dev->data;
113
114 if (data->hw_tf->update_reg(dev,
115 LSM6DSL_REG_CTRL1_XL,
116 LSM6DSL_MASK_CTRL1_XL_FS_XL,
117 fs << LSM6DSL_SHIFT_CTRL1_XL_FS_XL) < 0) {
118 return -EIO;
119 }
120
121 return 0;
122 }
123
lsm6dsl_accel_set_odr_raw(const struct device * dev,uint8_t odr)124 static int lsm6dsl_accel_set_odr_raw(const struct device *dev, uint8_t odr)
125 {
126 struct lsm6dsl_data *data = dev->data;
127
128 if (data->hw_tf->update_reg(dev,
129 LSM6DSL_REG_CTRL1_XL,
130 LSM6DSL_MASK_CTRL1_XL_ODR_XL,
131 odr << LSM6DSL_SHIFT_CTRL1_XL_ODR_XL) < 0) {
132 return -EIO;
133 }
134
135 data->accel_freq = lsm6dsl_odr_to_freq_val(odr);
136
137 return 0;
138 }
139
lsm6dsl_gyro_set_fs_raw(const struct device * dev,uint8_t fs)140 static int lsm6dsl_gyro_set_fs_raw(const struct device *dev, uint8_t fs)
141 {
142 struct lsm6dsl_data *data = dev->data;
143
144 if (fs == GYRO_FULLSCALE_125) {
145 if (data->hw_tf->update_reg(dev,
146 LSM6DSL_REG_CTRL2_G,
147 LSM6DSL_MASK_CTRL2_FS125 | LSM6DSL_MASK_CTRL2_G_FS_G,
148 1 << LSM6DSL_SHIFT_CTRL2_FS125) < 0) {
149 return -EIO;
150 }
151 } else {
152 if (data->hw_tf->update_reg(dev,
153 LSM6DSL_REG_CTRL2_G,
154 LSM6DSL_MASK_CTRL2_FS125 | LSM6DSL_MASK_CTRL2_G_FS_G,
155 fs << LSM6DSL_SHIFT_CTRL2_G_FS_G) < 0) {
156 return -EIO;
157 }
158 }
159
160 return 0;
161 }
162
lsm6dsl_gyro_set_odr_raw(const struct device * dev,uint8_t odr)163 static int lsm6dsl_gyro_set_odr_raw(const struct device *dev, uint8_t odr)
164 {
165 struct lsm6dsl_data *data = dev->data;
166
167 if (data->hw_tf->update_reg(dev,
168 LSM6DSL_REG_CTRL2_G,
169 LSM6DSL_MASK_CTRL2_G_ODR_G,
170 odr << LSM6DSL_SHIFT_CTRL2_G_ODR_G) < 0) {
171 return -EIO;
172 }
173
174 data->gyro_freq = lsm6dsl_odr_to_freq_val(odr);
175
176 return 0;
177 }
178
179 #ifdef LSM6DSL_ACCEL_ODR_RUNTIME
lsm6dsl_accel_odr_set(const struct device * dev,uint16_t freq)180 static int lsm6dsl_accel_odr_set(const struct device *dev, uint16_t freq)
181 {
182 int odr;
183
184 odr = lsm6dsl_freq_to_odr_val(freq);
185 if (odr < 0) {
186 return odr;
187 }
188
189 if (lsm6dsl_accel_set_odr_raw(dev, odr) < 0) {
190 LOG_DBG("failed to set accelerometer sampling rate");
191 return -EIO;
192 }
193
194 return 0;
195 }
196 #endif
197
198 #ifdef LSM6DSL_ACCEL_FS_RUNTIME
lsm6dsl_accel_range_set(const struct device * dev,int32_t range)199 static int lsm6dsl_accel_range_set(const struct device *dev, int32_t range)
200 {
201 int fs;
202 struct lsm6dsl_data *data = dev->data;
203
204 fs = lsm6dsl_accel_range_to_fs_val(range);
205 if (fs < 0) {
206 return fs;
207 }
208
209 if (lsm6dsl_accel_set_fs_raw(dev, fs) < 0) {
210 LOG_DBG("failed to set accelerometer full-scale");
211 return -EIO;
212 }
213
214 data->accel_sensitivity = (float)(lsm6dsl_accel_fs_sens[fs]
215 * SENSI_GRAIN_XL);
216 return 0;
217 }
218 #endif
219
lsm6dsl_accel_config(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,const struct sensor_value * val)220 static int lsm6dsl_accel_config(const struct device *dev,
221 enum sensor_channel chan,
222 enum sensor_attribute attr,
223 const struct sensor_value *val)
224 {
225 switch (attr) {
226 #ifdef LSM6DSL_ACCEL_FS_RUNTIME
227 case SENSOR_ATTR_FULL_SCALE:
228 return lsm6dsl_accel_range_set(dev, sensor_ms2_to_g(val));
229 #endif
230 #ifdef LSM6DSL_ACCEL_ODR_RUNTIME
231 case SENSOR_ATTR_SAMPLING_FREQUENCY:
232 return lsm6dsl_accel_odr_set(dev, val->val1);
233 #endif
234 default:
235 LOG_DBG("Accel attribute not supported.");
236 return -ENOTSUP;
237 }
238
239 return 0;
240 }
241
242 #ifdef LSM6DSL_GYRO_ODR_RUNTIME
lsm6dsl_gyro_odr_set(const struct device * dev,uint16_t freq)243 static int lsm6dsl_gyro_odr_set(const struct device *dev, uint16_t freq)
244 {
245 int odr;
246
247 odr = lsm6dsl_freq_to_odr_val(freq);
248 if (odr < 0) {
249 return odr;
250 }
251
252 if (lsm6dsl_gyro_set_odr_raw(dev, odr) < 0) {
253 LOG_DBG("failed to set gyroscope sampling rate");
254 return -EIO;
255 }
256
257 return 0;
258 }
259 #endif
260
261 #ifdef LSM6DSL_GYRO_FS_RUNTIME
lsm6dsl_gyro_range_set(const struct device * dev,int32_t range)262 static int lsm6dsl_gyro_range_set(const struct device *dev, int32_t range)
263 {
264 int fs;
265 struct lsm6dsl_data *data = dev->data;
266
267 fs = lsm6dsl_gyro_range_to_fs_val(range);
268 if (fs < 0) {
269 return fs;
270 }
271
272 if (lsm6dsl_gyro_set_fs_raw(dev, fs) < 0) {
273 LOG_DBG("failed to set gyroscope full-scale");
274 return -EIO;
275 }
276
277 data->gyro_sensitivity = (float)(lsm6dsl_gyro_fs_sens[fs]
278 * SENSI_GRAIN_G);
279 return 0;
280 }
281 #endif
282
lsm6dsl_gyro_config(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,const struct sensor_value * val)283 static int lsm6dsl_gyro_config(const struct device *dev,
284 enum sensor_channel chan,
285 enum sensor_attribute attr,
286 const struct sensor_value *val)
287 {
288 switch (attr) {
289 #ifdef LSM6DSL_GYRO_FS_RUNTIME
290 case SENSOR_ATTR_FULL_SCALE:
291 return lsm6dsl_gyro_range_set(dev, sensor_rad_to_degrees(val));
292 #endif
293 #ifdef LSM6DSL_GYRO_ODR_RUNTIME
294 case SENSOR_ATTR_SAMPLING_FREQUENCY:
295 return lsm6dsl_gyro_odr_set(dev, val->val1);
296 #endif
297 default:
298 LOG_DBG("Gyro attribute not supported.");
299 return -ENOTSUP;
300 }
301
302 return 0;
303 }
304
lsm6dsl_attr_set(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,const struct sensor_value * val)305 static int lsm6dsl_attr_set(const struct device *dev,
306 enum sensor_channel chan,
307 enum sensor_attribute attr,
308 const struct sensor_value *val)
309 {
310 switch (chan) {
311 case SENSOR_CHAN_ACCEL_XYZ:
312 return lsm6dsl_accel_config(dev, chan, attr, val);
313 case SENSOR_CHAN_GYRO_XYZ:
314 return lsm6dsl_gyro_config(dev, chan, attr, val);
315 default:
316 LOG_WRN("attr_set() not supported on this channel.");
317 return -ENOTSUP;
318 }
319
320 return 0;
321 }
322
lsm6dsl_sample_fetch_accel(const struct device * dev)323 static int lsm6dsl_sample_fetch_accel(const struct device *dev)
324 {
325 struct lsm6dsl_data *data = dev->data;
326 uint8_t buf[6];
327
328 if (data->hw_tf->read_data(dev, LSM6DSL_REG_OUTX_L_XL,
329 buf, sizeof(buf)) < 0) {
330 LOG_DBG("failed to read sample");
331 return -EIO;
332 }
333
334 data->accel_sample_x = (int16_t)((uint16_t)(buf[0]) |
335 ((uint16_t)(buf[1]) << 8));
336 data->accel_sample_y = (int16_t)((uint16_t)(buf[2]) |
337 ((uint16_t)(buf[3]) << 8));
338 data->accel_sample_z = (int16_t)((uint16_t)(buf[4]) |
339 ((uint16_t)(buf[5]) << 8));
340
341 return 0;
342 }
343
lsm6dsl_sample_fetch_gyro(const struct device * dev)344 static int lsm6dsl_sample_fetch_gyro(const struct device *dev)
345 {
346 struct lsm6dsl_data *data = dev->data;
347 uint8_t buf[6];
348
349 if (data->hw_tf->read_data(dev, LSM6DSL_REG_OUTX_L_G,
350 buf, sizeof(buf)) < 0) {
351 LOG_DBG("failed to read sample");
352 return -EIO;
353 }
354
355 data->gyro_sample_x = (int16_t)((uint16_t)(buf[0]) |
356 ((uint16_t)(buf[1]) << 8));
357 data->gyro_sample_y = (int16_t)((uint16_t)(buf[2]) |
358 ((uint16_t)(buf[3]) << 8));
359 data->gyro_sample_z = (int16_t)((uint16_t)(buf[4]) |
360 ((uint16_t)(buf[5]) << 8));
361
362 return 0;
363 }
364
365 #if defined(CONFIG_LSM6DSL_ENABLE_TEMP)
lsm6dsl_sample_fetch_temp(const struct device * dev)366 static int lsm6dsl_sample_fetch_temp(const struct device *dev)
367 {
368 struct lsm6dsl_data *data = dev->data;
369 uint8_t buf[2];
370
371 if (data->hw_tf->read_data(dev, LSM6DSL_REG_OUT_TEMP_L,
372 buf, sizeof(buf)) < 0) {
373 LOG_DBG("failed to read sample");
374 return -EIO;
375 }
376
377 data->temp_sample = (int16_t)((uint16_t)(buf[0]) |
378 ((uint16_t)(buf[1]) << 8));
379
380 return 0;
381 }
382 #endif
383
384 #if defined(CONFIG_LSM6DSL_EXT0_LIS2MDL) || defined(CONFIG_LSM6DSL_EXT0_LIS3MDL)
lsm6dsl_sample_fetch_magn(const struct device * dev)385 static int lsm6dsl_sample_fetch_magn(const struct device *dev)
386 {
387 struct lsm6dsl_data *data = dev->data;
388 uint8_t buf[6];
389
390 if (lsm6dsl_shub_read_external_chip(dev, buf, sizeof(buf)) < 0) {
391 LOG_DBG("failed to read ext mag sample");
392 return -EIO;
393 }
394
395 data->magn_sample_x = (int16_t)((uint16_t)(buf[0]) |
396 ((uint16_t)(buf[1]) << 8));
397 data->magn_sample_y = (int16_t)((uint16_t)(buf[2]) |
398 ((uint16_t)(buf[3]) << 8));
399 data->magn_sample_z = (int16_t)((uint16_t)(buf[4]) |
400 ((uint16_t)(buf[5]) << 8));
401
402 return 0;
403 }
404 #endif
405 #if defined(CONFIG_LSM6DSL_EXT0_LPS22HB)
lsm6dsl_sample_fetch_press(const struct device * dev)406 static int lsm6dsl_sample_fetch_press(const struct device *dev)
407 {
408 struct lsm6dsl_data *data = dev->data;
409 uint8_t buf[5];
410
411 if (lsm6dsl_shub_read_external_chip(dev, buf, sizeof(buf)) < 0) {
412 LOG_DBG("failed to read ext press sample");
413 return -EIO;
414 }
415
416 data->sample_press = (int32_t)((uint32_t)(buf[0]) |
417 ((uint32_t)(buf[1]) << 8) |
418 ((uint32_t)(buf[2]) << 16));
419 data->sample_temp = (int16_t)((uint16_t)(buf[3]) |
420 ((uint16_t)(buf[4]) << 8));
421
422 return 0;
423 }
424 #endif
425
lsm6dsl_sample_fetch(const struct device * dev,enum sensor_channel chan)426 static int lsm6dsl_sample_fetch(const struct device *dev,
427 enum sensor_channel chan)
428 {
429 switch (chan) {
430 case SENSOR_CHAN_ACCEL_XYZ:
431 lsm6dsl_sample_fetch_accel(dev);
432 break;
433 case SENSOR_CHAN_GYRO_XYZ:
434 lsm6dsl_sample_fetch_gyro(dev);
435 break;
436 #if defined(CONFIG_LSM6DSL_ENABLE_TEMP)
437 case SENSOR_CHAN_DIE_TEMP:
438 lsm6dsl_sample_fetch_temp(dev);
439 break;
440 #endif
441 #if defined(CONFIG_LSM6DSL_EXT0_LIS2MDL) || defined(CONFIG_LSM6DSL_EXT0_LIS3MDL)
442 case SENSOR_CHAN_MAGN_XYZ:
443 lsm6dsl_sample_fetch_magn(dev);
444 break;
445 #endif
446 #if defined(CONFIG_LSM6DSL_EXT0_LPS22HB)
447 case SENSOR_CHAN_AMBIENT_TEMP:
448 case SENSOR_CHAN_PRESS:
449 lsm6dsl_sample_fetch_press(dev);
450 break;
451 #endif
452 case SENSOR_CHAN_ALL:
453 lsm6dsl_sample_fetch_accel(dev);
454 lsm6dsl_sample_fetch_gyro(dev);
455 #if defined(CONFIG_LSM6DSL_ENABLE_TEMP)
456 lsm6dsl_sample_fetch_temp(dev);
457 #endif
458 #if defined(CONFIG_LSM6DSL_EXT0_LIS2MDL) || defined(CONFIG_LSM6DSL_EXT0_LIS3MDL)
459 lsm6dsl_sample_fetch_magn(dev);
460 #endif
461 #if defined(CONFIG_LSM6DSL_EXT0_LPS22HB)
462 lsm6dsl_sample_fetch_press(dev);
463 #endif
464 break;
465 default:
466 return -ENOTSUP;
467 }
468
469 return 0;
470 }
471
lsm6dsl_accel_convert(struct sensor_value * val,int raw_val,float sensitivity)472 static inline void lsm6dsl_accel_convert(struct sensor_value *val, int raw_val,
473 float sensitivity)
474 {
475 int64_t dval;
476
477 /* Sensitivity is exposed in ug/LSB */
478 /* Convert to m/s^2 */
479 dval = (int64_t)raw_val * sensitivity;
480 sensor_ug_to_ms2(dval, val);
481 }
482
lsm6dsl_accel_get_channel(enum sensor_channel chan,struct sensor_value * val,struct lsm6dsl_data * data,float sensitivity)483 static inline int lsm6dsl_accel_get_channel(enum sensor_channel chan,
484 struct sensor_value *val,
485 struct lsm6dsl_data *data,
486 float sensitivity)
487 {
488 switch (chan) {
489 case SENSOR_CHAN_ACCEL_X:
490 lsm6dsl_accel_convert(val, data->accel_sample_x, sensitivity);
491 break;
492 case SENSOR_CHAN_ACCEL_Y:
493 lsm6dsl_accel_convert(val, data->accel_sample_y, sensitivity);
494 break;
495 case SENSOR_CHAN_ACCEL_Z:
496 lsm6dsl_accel_convert(val, data->accel_sample_z, sensitivity);
497 break;
498 case SENSOR_CHAN_ACCEL_XYZ:
499 lsm6dsl_accel_convert(val, data->accel_sample_x, sensitivity);
500 lsm6dsl_accel_convert(val + 1, data->accel_sample_y,
501 sensitivity);
502 lsm6dsl_accel_convert(val + 2, data->accel_sample_z,
503 sensitivity);
504 break;
505 default:
506 return -ENOTSUP;
507 }
508
509 return 0;
510 }
511
lsm6dsl_accel_channel_get(enum sensor_channel chan,struct sensor_value * val,struct lsm6dsl_data * data)512 static int lsm6dsl_accel_channel_get(enum sensor_channel chan,
513 struct sensor_value *val,
514 struct lsm6dsl_data *data)
515 {
516 return lsm6dsl_accel_get_channel(chan, val, data,
517 data->accel_sensitivity);
518 }
519
lsm6dsl_gyro_convert(struct sensor_value * val,int raw_val,float sensitivity)520 static inline void lsm6dsl_gyro_convert(struct sensor_value *val, int raw_val,
521 float sensitivity)
522 {
523 int64_t dval;
524
525 /* Sensitivity is exposed in udps/LSB */
526 /* So, calculate value in 10 udps unit and then to rad/s */
527 dval = (int64_t)raw_val * sensitivity / 10;
528 sensor_10udegrees_to_rad(dval, val);
529 }
530
lsm6dsl_gyro_get_channel(enum sensor_channel chan,struct sensor_value * val,struct lsm6dsl_data * data,float sensitivity)531 static inline int lsm6dsl_gyro_get_channel(enum sensor_channel chan,
532 struct sensor_value *val,
533 struct lsm6dsl_data *data,
534 float sensitivity)
535 {
536 switch (chan) {
537 case SENSOR_CHAN_GYRO_X:
538 lsm6dsl_gyro_convert(val, data->gyro_sample_x, sensitivity);
539 break;
540 case SENSOR_CHAN_GYRO_Y:
541 lsm6dsl_gyro_convert(val, data->gyro_sample_y, sensitivity);
542 break;
543 case SENSOR_CHAN_GYRO_Z:
544 lsm6dsl_gyro_convert(val, data->gyro_sample_z, sensitivity);
545 break;
546 case SENSOR_CHAN_GYRO_XYZ:
547 lsm6dsl_gyro_convert(val, data->gyro_sample_x, sensitivity);
548 lsm6dsl_gyro_convert(val + 1, data->gyro_sample_y, sensitivity);
549 lsm6dsl_gyro_convert(val + 2, data->gyro_sample_z, sensitivity);
550 break;
551 default:
552 return -ENOTSUP;
553 }
554
555 return 0;
556 }
557
lsm6dsl_gyro_channel_get(enum sensor_channel chan,struct sensor_value * val,struct lsm6dsl_data * data)558 static int lsm6dsl_gyro_channel_get(enum sensor_channel chan,
559 struct sensor_value *val,
560 struct lsm6dsl_data *data)
561 {
562 return lsm6dsl_gyro_get_channel(chan, val, data,
563 data->gyro_sensitivity);
564 }
565
566 #if defined(CONFIG_LSM6DSL_ENABLE_TEMP)
lsm6dsl_gyro_channel_get_temp(struct sensor_value * val,struct lsm6dsl_data * data)567 static void lsm6dsl_gyro_channel_get_temp(struct sensor_value *val,
568 struct lsm6dsl_data *data)
569 {
570 /* val = temp_sample / 256 + 25 */
571 val->val1 = data->temp_sample / 256 + 25;
572 val->val2 = (data->temp_sample % 256) * (1000000 / 256);
573 }
574 #endif
575
576 #if defined(CONFIG_LSM6DSL_EXT0_LIS2MDL) || defined(CONFIG_LSM6DSL_EXT0_LIS3MDL)
lsm6dsl_magn_convert(struct sensor_value * val,int raw_val,float sensitivity)577 static inline void lsm6dsl_magn_convert(struct sensor_value *val, int raw_val,
578 float sensitivity)
579 {
580 double dval;
581
582 /* Sensitivity is exposed in mgauss/LSB */
583 dval = (double)(raw_val * sensitivity);
584 val->val1 = (int32_t)dval / 1000000;
585 val->val2 = (int32_t)dval % 1000000;
586 }
587
lsm6dsl_magn_get_channel(enum sensor_channel chan,struct sensor_value * val,struct lsm6dsl_data * data)588 static inline int lsm6dsl_magn_get_channel(enum sensor_channel chan,
589 struct sensor_value *val,
590 struct lsm6dsl_data *data)
591 {
592 switch (chan) {
593 case SENSOR_CHAN_MAGN_X:
594 lsm6dsl_magn_convert(val,
595 data->magn_sample_x,
596 data->magn_sensitivity);
597 break;
598 case SENSOR_CHAN_MAGN_Y:
599 lsm6dsl_magn_convert(val,
600 data->magn_sample_y,
601 data->magn_sensitivity);
602 break;
603 case SENSOR_CHAN_MAGN_Z:
604 lsm6dsl_magn_convert(val,
605 data->magn_sample_z,
606 data->magn_sensitivity);
607 break;
608 case SENSOR_CHAN_MAGN_XYZ:
609 lsm6dsl_magn_convert(val,
610 data->magn_sample_x,
611 data->magn_sensitivity);
612 lsm6dsl_magn_convert(val + 1,
613 data->magn_sample_y,
614 data->magn_sensitivity);
615 lsm6dsl_magn_convert(val + 2,
616 data->magn_sample_z,
617 data->magn_sensitivity);
618 break;
619 default:
620 return -ENOTSUP;
621 }
622
623 return 0;
624 }
625
lsm6dsl_magn_channel_get(enum sensor_channel chan,struct sensor_value * val,struct lsm6dsl_data * data)626 static int lsm6dsl_magn_channel_get(enum sensor_channel chan,
627 struct sensor_value *val,
628 struct lsm6dsl_data *data)
629 {
630 return lsm6dsl_magn_get_channel(chan, val, data);
631 }
632 #endif
633
634 #if defined(CONFIG_LSM6DSL_EXT0_LPS22HB)
lps22hb_press_convert(struct sensor_value * val,int32_t raw_val)635 static inline void lps22hb_press_convert(struct sensor_value *val,
636 int32_t raw_val)
637 {
638 /* Pressure sensitivity is 4096 LSB/hPa */
639 /* Convert raw_val to val in kPa */
640 val->val1 = (raw_val >> 12) / 10;
641 val->val2 = (raw_val >> 12) % 10 * 100000 +
642 (((int32_t)((raw_val) & 0x0FFF) * 100000L) >> 12);
643 }
644
lps22hb_temp_convert(struct sensor_value * val,int16_t raw_val)645 static inline void lps22hb_temp_convert(struct sensor_value *val,
646 int16_t raw_val)
647 {
648 /* Temperature sensitivity is 100 LSB/deg C */
649 val->val1 = raw_val / 100;
650 val->val2 = (int32_t)raw_val % 100 * (10000);
651 }
652 #endif
653
lsm6dsl_channel_get(const struct device * dev,enum sensor_channel chan,struct sensor_value * val)654 static int lsm6dsl_channel_get(const struct device *dev,
655 enum sensor_channel chan,
656 struct sensor_value *val)
657 {
658 struct lsm6dsl_data *data = dev->data;
659
660 switch (chan) {
661 case SENSOR_CHAN_ACCEL_X:
662 case SENSOR_CHAN_ACCEL_Y:
663 case SENSOR_CHAN_ACCEL_Z:
664 case SENSOR_CHAN_ACCEL_XYZ:
665 lsm6dsl_accel_channel_get(chan, val, data);
666 break;
667 case SENSOR_CHAN_GYRO_X:
668 case SENSOR_CHAN_GYRO_Y:
669 case SENSOR_CHAN_GYRO_Z:
670 case SENSOR_CHAN_GYRO_XYZ:
671 lsm6dsl_gyro_channel_get(chan, val, data);
672 break;
673 #if defined(CONFIG_LSM6DSL_ENABLE_TEMP)
674 case SENSOR_CHAN_DIE_TEMP:
675 lsm6dsl_gyro_channel_get_temp(val, data);
676 break;
677 #endif
678 #if defined(CONFIG_LSM6DSL_EXT0_LIS2MDL) || defined(CONFIG_LSM6DSL_EXT0_LIS3MDL)
679 case SENSOR_CHAN_MAGN_X:
680 case SENSOR_CHAN_MAGN_Y:
681 case SENSOR_CHAN_MAGN_Z:
682 case SENSOR_CHAN_MAGN_XYZ:
683 lsm6dsl_magn_channel_get(chan, val, data);
684 break;
685 #endif
686 #if defined(CONFIG_LSM6DSL_EXT0_LPS22HB)
687 case SENSOR_CHAN_PRESS:
688 lps22hb_press_convert(val, data->sample_press);
689 break;
690
691 case SENSOR_CHAN_AMBIENT_TEMP:
692 lps22hb_temp_convert(val, data->sample_temp);
693 break;
694 #endif
695 default:
696 return -ENOTSUP;
697 }
698
699 return 0;
700 }
701
702 static const struct sensor_driver_api lsm6dsl_driver_api = {
703 .attr_set = lsm6dsl_attr_set,
704 #if CONFIG_LSM6DSL_TRIGGER
705 .trigger_set = lsm6dsl_trigger_set,
706 #endif
707 .sample_fetch = lsm6dsl_sample_fetch,
708 .channel_get = lsm6dsl_channel_get,
709 };
710
lsm6dsl_init_chip(const struct device * dev)711 static int lsm6dsl_init_chip(const struct device *dev)
712 {
713 struct lsm6dsl_data *data = dev->data;
714 uint8_t chip_id;
715
716 if (lsm6dsl_reboot(dev) < 0) {
717 LOG_DBG("failed to reboot device");
718 return -EIO;
719 }
720
721 if (data->hw_tf->read_reg(dev, LSM6DSL_REG_WHO_AM_I, &chip_id) < 0) {
722 LOG_DBG("failed reading chip id");
723 return -EIO;
724 }
725 if (chip_id != LSM6DSL_VAL_WHO_AM_I) {
726 LOG_DBG("invalid chip id 0x%x", chip_id);
727 return -EIO;
728 }
729
730 LOG_DBG("chip id 0x%x", chip_id);
731
732 if (lsm6dsl_accel_set_fs_raw(dev,
733 LSM6DSL_DEFAULT_ACCEL_FULLSCALE) < 0) {
734 LOG_DBG("failed to set accelerometer full-scale");
735 return -EIO;
736 }
737 data->accel_sensitivity = LSM6DSL_DEFAULT_ACCEL_SENSITIVITY;
738
739 if (lsm6dsl_accel_set_odr_raw(dev, CONFIG_LSM6DSL_ACCEL_ODR) < 0) {
740 LOG_DBG("failed to set accelerometer sampling rate");
741 return -EIO;
742 }
743
744 if (lsm6dsl_gyro_set_fs_raw(dev, LSM6DSL_DEFAULT_GYRO_FULLSCALE) < 0) {
745 LOG_DBG("failed to set gyroscope full-scale");
746 return -EIO;
747 }
748 data->gyro_sensitivity = LSM6DSL_DEFAULT_GYRO_SENSITIVITY;
749
750 if (lsm6dsl_gyro_set_odr_raw(dev, CONFIG_LSM6DSL_GYRO_ODR) < 0) {
751 LOG_DBG("failed to set gyroscope sampling rate");
752 return -EIO;
753 }
754
755 if (data->hw_tf->update_reg(dev,
756 LSM6DSL_REG_FIFO_CTRL5,
757 LSM6DSL_MASK_FIFO_CTRL5_FIFO_MODE,
758 0 << LSM6DSL_SHIFT_FIFO_CTRL5_FIFO_MODE) < 0) {
759 LOG_DBG("failed to set FIFO mode");
760 return -EIO;
761 }
762
763 if (data->hw_tf->update_reg(dev,
764 LSM6DSL_REG_CTRL3_C,
765 LSM6DSL_MASK_CTRL3_C_BDU |
766 LSM6DSL_MASK_CTRL3_C_BLE |
767 LSM6DSL_MASK_CTRL3_C_IF_INC,
768 (1 << LSM6DSL_SHIFT_CTRL3_C_BDU) |
769 (0 << LSM6DSL_SHIFT_CTRL3_C_BLE) |
770 (1 << LSM6DSL_SHIFT_CTRL3_C_IF_INC)) < 0) {
771 LOG_DBG("failed to set BDU, BLE and burst");
772 return -EIO;
773 }
774
775 if (data->hw_tf->update_reg(dev,
776 LSM6DSL_REG_CTRL6_C,
777 LSM6DSL_MASK_CTRL6_C_XL_HM_MODE,
778 (1 << LSM6DSL_SHIFT_CTRL6_C_XL_HM_MODE)) < 0) {
779 LOG_DBG("failed to disable accelerometer high performance mode");
780 return -EIO;
781 }
782
783 if (data->hw_tf->update_reg(dev,
784 LSM6DSL_REG_CTRL7_G,
785 LSM6DSL_MASK_CTRL7_G_HM_MODE,
786 (1 << LSM6DSL_SHIFT_CTRL7_G_HM_MODE)) < 0) {
787 LOG_DBG("failed to disable gyroscope high performance mode");
788 return -EIO;
789 }
790
791 return 0;
792 }
793
lsm6dsl_init(const struct device * dev)794 static int lsm6dsl_init(const struct device *dev)
795 {
796 int ret;
797 const struct lsm6dsl_config * const config = dev->config;
798
799 ret = config->bus_init(dev);
800 if (ret < 0) {
801 LOG_ERR("Failed to initialize sensor bus");
802 return ret;
803 }
804
805 ret = lsm6dsl_init_chip(dev);
806 if (ret < 0) {
807 LOG_ERR("Failed to initialize chip");
808 return ret;
809 }
810
811 #ifdef CONFIG_LSM6DSL_TRIGGER
812 ret = lsm6dsl_init_interrupt(dev);
813 if (ret < 0) {
814 LOG_ERR("Failed to initialize interrupt.");
815 return ret;
816 }
817 #endif
818
819 #ifdef CONFIG_LSM6DSL_SENSORHUB
820 ret = lsm6dsl_shub_init_external_chip(dev);
821 if (ret < 0) {
822 LOG_ERR("Failed to initialize external chip");
823 return ret;
824 }
825 #endif
826
827 return 0;
828 }
829
830 #ifdef CONFIG_PM_DEVICE
lsm6dsl_pm_action(const struct device * dev,enum pm_device_action action)831 static int lsm6dsl_pm_action(const struct device *dev,
832 enum pm_device_action action)
833 {
834 struct lsm6dsl_data *data = dev->data;
835 int ret = -EIO;
836 uint8_t accel_odr = 0;
837 uint8_t gyro_odr = 0;
838
839 switch (action) {
840 case PM_DEVICE_ACTION_RESUME:
841 /* Restore saved ODR values */
842 accel_odr = lsm6dsl_freq_to_odr_val(data->accel_freq);
843 ret = lsm6dsl_accel_set_odr_raw(dev, accel_odr);
844 if (ret < 0) {
845 LOG_ERR("Failed to resume accelerometer");
846 break;
847 }
848 gyro_odr = lsm6dsl_freq_to_odr_val(data->gyro_freq);
849 ret = lsm6dsl_gyro_set_odr_raw(dev, gyro_odr);
850 if (ret < 0) {
851 LOG_ERR("Failed to resume gyro");
852 break;
853 }
854 break;
855 case PM_DEVICE_ACTION_SUSPEND:
856 /*
857 * Set accelerometer ODR to power-down. Don't use the direct
858 * function to not overwrite the saved value
859 */
860 ret = data->hw_tf->update_reg(dev, LSM6DSL_REG_CTRL1_XL,
861 LSM6DSL_MASK_CTRL1_XL_ODR_XL, 0);
862 if (ret < 0) {
863 LOG_ERR("Failed to suspend accelerometer");
864 break;
865 }
866 /* Set gyro ODR to power-down */
867 ret = data->hw_tf->update_reg(dev, LSM6DSL_REG_CTRL2_G,
868 LSM6DSL_MASK_CTRL2_G_ODR_G, 0);
869 if (ret < 0) {
870 LOG_ERR("Failed to suspend gyro");
871 break;
872 }
873
874 break;
875 default:
876 return -ENOTSUP;
877 }
878
879 return ret;
880 }
881 #endif
882
883
884 #if DT_NUM_INST_STATUS_OKAY(DT_DRV_COMPAT) == 0
885 #warning "LSM6DSL driver enabled without any devices"
886 #endif
887
888 /*
889 * Device creation macro, shared by LSM6DSL_DEFINE_SPI() and
890 * LSM6DSL_DEFINE_I2C().
891 */
892
893 #define LSM6DSL_DEVICE_INIT(inst) \
894 PM_DEVICE_DT_INST_DEFINE(inst, lsm6dsl_pm_action); \
895 SENSOR_DEVICE_DT_INST_DEFINE(inst, \
896 lsm6dsl_init, \
897 PM_DEVICE_DT_INST_GET(inst), \
898 &lsm6dsl_data_##inst, \
899 &lsm6dsl_config_##inst, \
900 POST_KERNEL, \
901 CONFIG_SENSOR_INIT_PRIORITY, \
902 &lsm6dsl_driver_api);
903
904 /*
905 * Instantiation macros used when a device is on a SPI bus.
906 */
907
908 #ifdef CONFIG_LSM6DSL_TRIGGER
909 #define LSM6DSL_CFG_IRQ(inst) \
910 .int_gpio = GPIO_DT_SPEC_INST_GET(inst, irq_gpios),
911 #else
912 #define LSM6DSL_CFG_IRQ(inst)
913 #endif /* CONFIG_LSM6DSL_TRIGGER */
914
915 #define LSM6DSL_CONFIG_SPI(inst) \
916 { \
917 .bus_init = lsm6dsl_spi_init, \
918 .bus_cfg.spi = SPI_DT_SPEC_INST_GET(inst, SPI_WORD_SET(8) | \
919 SPI_OP_MODE_MASTER | \
920 SPI_MODE_CPOL | \
921 SPI_MODE_CPHA, 0), \
922 COND_CODE_1(DT_INST_NODE_HAS_PROP(inst, irq_gpios), \
923 (LSM6DSL_CFG_IRQ(inst)), ()) \
924 }
925
926 #define LSM6DSL_DEFINE_SPI(inst) \
927 static struct lsm6dsl_data lsm6dsl_data_##inst; \
928 static const struct lsm6dsl_config lsm6dsl_config_##inst = \
929 LSM6DSL_CONFIG_SPI(inst); \
930 LSM6DSL_DEVICE_INIT(inst)
931
932 /*
933 * Instantiation macros used when a device is on an I2C bus.
934 */
935
936 #define LSM6DSL_CONFIG_I2C(inst) \
937 { \
938 .bus_init = lsm6dsl_i2c_init, \
939 .bus_cfg.i2c = I2C_DT_SPEC_INST_GET(inst), \
940 COND_CODE_1(DT_INST_NODE_HAS_PROP(inst, irq_gpios), \
941 (LSM6DSL_CFG_IRQ(inst)), ()) \
942 }
943
944 #define LSM6DSL_DEFINE_I2C(inst) \
945 static struct lsm6dsl_data lsm6dsl_data_##inst; \
946 static const struct lsm6dsl_config lsm6dsl_config_##inst = \
947 LSM6DSL_CONFIG_I2C(inst); \
948 LSM6DSL_DEVICE_INIT(inst)
949 /*
950 * Main instantiation macro. Use of COND_CODE_1() selects the right
951 * bus-specific macro at preprocessor time.
952 */
953
954 #define LSM6DSL_DEFINE(inst) \
955 COND_CODE_1(DT_INST_ON_BUS(inst, spi), \
956 (LSM6DSL_DEFINE_SPI(inst)), \
957 (LSM6DSL_DEFINE_I2C(inst)))
958
959 DT_INST_FOREACH_STATUS_OKAY(LSM6DSL_DEFINE)
960
