1 /* ST Microelectronics LSM6DSV16X 6-axis IMU sensor driver
2 *
3 * Copyright (c) 2023 STMicroelectronics
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 *
7 * Datasheet:
8 * https://www.st.com/resource/en/datasheet/lsm6dsv16x.pdf
9 */
10
11 #define DT_DRV_COMPAT st_lsm6dsv16x
12
13 #include <zephyr/drivers/sensor.h>
14 #include <zephyr/kernel.h>
15 #include <zephyr/device.h>
16 #include <zephyr/init.h>
17 #include <zephyr/pm/device.h>
18 #include <string.h>
19 #include <zephyr/sys/__assert.h>
20 #include <zephyr/logging/log.h>
21
22 #include <zephyr/dt-bindings/sensor/lsm6dsv16x.h>
23 #include "lsm6dsv16x.h"
24 #include "lsm6dsv16x_decoder.h"
25 #include "lsm6dsv16x_rtio.h"
26
27 LOG_MODULE_REGISTER(LSM6DSV16X, CONFIG_SENSOR_LOG_LEVEL);
28
lsm6dsv16x_is_active(const struct device * dev)29 bool lsm6dsv16x_is_active(const struct device *dev)
30 {
31 #if defined(CONFIG_PM_DEVICE)
32 enum pm_device_state state;
33 (void)pm_device_state_get(dev, &state);
34 return (state == PM_DEVICE_STATE_ACTIVE);
35 #else
36 return true;
37 #endif /* CONFIG_PM_DEVICE*/
38 }
39
40 /*
41 * values taken from lsm6dsv16x_data_rate_t in hal/st module. The mode/accuracy
42 * should be selected through accel-odr property in DT
43 */
44 static const float lsm6dsv16x_odr_map[3][13] = {
45 /* High Accuracy off */
46 {0.0f, 1.875f, 7.5f, 15.0f, 30.0f, 60.0f,
47 120.0f, 240.0f, 480.0f, 960.0f, 1920.0f,
48 3840.0f, 7680.0f},
49
50 /* High Accuracy 1 */
51 {0.0f, 1.875f, 7.5f, 15.625f, 31.25f, 62.5f,
52 125.0f, 250.0f, 500.0f, 1000.0f, 2000.0f,
53 4000.0f, 8000.0f},
54
55 /* High Accuracy 2 */
56 {0.0f, 1.875f, 7.5f, 12.5f, 25.0f, 50.0f,
57 100.0f, 200.0f, 400.0f, 800.0f, 1600.0f,
58 3200.0f, 6400.0f},
59 };
60
lsm6dsv16x_freq_to_odr_val(const struct device * dev,uint16_t freq)61 static int lsm6dsv16x_freq_to_odr_val(const struct device *dev, uint16_t freq)
62 {
63 const struct lsm6dsv16x_config *cfg = dev->config;
64 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
65 lsm6dsv16x_data_rate_t odr;
66 int8_t mode;
67 size_t i;
68
69 if (lsm6dsv16x_xl_data_rate_get(ctx, &odr) < 0) {
70 return -EINVAL;
71 }
72
73 mode = (odr >> 4) & 0xf;
74
75 for (i = 0; i < ARRAY_SIZE(lsm6dsv16x_odr_map[mode]); i++) {
76 if (freq <= lsm6dsv16x_odr_map[mode][i]) {
77 LOG_DBG("mode: %d - odr: %d", mode, i);
78 return i;
79 }
80 }
81
82 return -EINVAL;
83 }
84
85 static const uint16_t lsm6dsv16x_accel_fs_map[] = {2, 4, 8, 16};
86
lsm6dsv16x_accel_range_to_fs_val(int32_t range)87 static int lsm6dsv16x_accel_range_to_fs_val(int32_t range)
88 {
89 size_t i;
90
91 for (i = 0; i < ARRAY_SIZE(lsm6dsv16x_accel_fs_map); i++) {
92 if (range == lsm6dsv16x_accel_fs_map[i]) {
93 return i;
94 }
95 }
96
97 return -EINVAL;
98 }
99
100 static const uint16_t lsm6dsv16x_gyro_fs_map[] = {125, 250, 500, 1000, 2000, 0, 0,
101 0, 0, 0, 0, 0, 4000};
102 static const uint16_t lsm6dsv16x_gyro_fs_sens[] = {1, 2, 4, 8, 16, 0, 0, 0, 0, 0, 0, 0, 32};
103
lsm6dsv16x_calc_accel_gain(uint8_t fs)104 int lsm6dsv16x_calc_accel_gain(uint8_t fs)
105 {
106 return lsm6dsv16x_accel_fs_map[fs] * GAIN_UNIT_XL / 2;
107 }
108
lsm6dsv16x_calc_gyro_gain(uint8_t fs)109 int lsm6dsv16x_calc_gyro_gain(uint8_t fs)
110 {
111 return lsm6dsv16x_gyro_fs_sens[fs] * GAIN_UNIT_G;
112 }
113
lsm6dsv16x_gyro_range_to_fs_val(int32_t range)114 static int lsm6dsv16x_gyro_range_to_fs_val(int32_t range)
115 {
116 size_t i;
117
118 for (i = 0; i < ARRAY_SIZE(lsm6dsv16x_gyro_fs_map); i++) {
119 if (range == lsm6dsv16x_gyro_fs_map[i]) {
120 return i;
121 }
122 }
123
124 return -EINVAL;
125 }
126
lsm6dsv16x_accel_set_fs_raw(const struct device * dev,uint8_t fs)127 static int lsm6dsv16x_accel_set_fs_raw(const struct device *dev, uint8_t fs)
128 {
129 const struct lsm6dsv16x_config *cfg = dev->config;
130 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
131 struct lsm6dsv16x_data *data = dev->data;
132 lsm6dsv16x_xl_full_scale_t val;
133
134 switch (fs) {
135 case 0:
136 val = LSM6DSV16X_2g;
137 break;
138 case 1:
139 val = LSM6DSV16X_4g;
140 break;
141 case 2:
142 val = LSM6DSV16X_8g;
143 break;
144 case 3:
145 val = LSM6DSV16X_16g;
146 break;
147 default:
148 return -EIO;
149 }
150
151 if (lsm6dsv16x_xl_full_scale_set(ctx, val) < 0) {
152 return -EIO;
153 }
154
155 data->accel_fs = fs;
156
157 return 0;
158 }
159
lsm6dsv16x_accel_set_odr_raw(const struct device * dev,uint8_t odr)160 static int lsm6dsv16x_accel_set_odr_raw(const struct device *dev, uint8_t odr)
161 {
162 const struct lsm6dsv16x_config *cfg = dev->config;
163 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
164 struct lsm6dsv16x_data *data = dev->data;
165
166 if (lsm6dsv16x_xl_data_rate_set(ctx, odr) < 0) {
167 return -EIO;
168 }
169
170 data->accel_freq = odr;
171
172 return 0;
173 }
174
lsm6dsv16x_gyro_set_fs_raw(const struct device * dev,uint8_t fs)175 static int lsm6dsv16x_gyro_set_fs_raw(const struct device *dev, uint8_t fs)
176 {
177 const struct lsm6dsv16x_config *cfg = dev->config;
178 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
179 struct lsm6dsv16x_data *data = dev->data;
180
181 if (lsm6dsv16x_gy_full_scale_set(ctx, fs) < 0) {
182 return -EIO;
183 }
184
185 data->gyro_fs = fs;
186 return 0;
187 }
188
lsm6dsv16x_gyro_set_odr_raw(const struct device * dev,uint8_t odr)189 static int lsm6dsv16x_gyro_set_odr_raw(const struct device *dev, uint8_t odr)
190 {
191 const struct lsm6dsv16x_config *cfg = dev->config;
192 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
193
194 if (lsm6dsv16x_gy_data_rate_set(ctx, odr) < 0) {
195 return -EIO;
196 }
197
198 return 0;
199 }
200
lsm6dsv16x_accel_odr_set(const struct device * dev,uint16_t freq)201 static int lsm6dsv16x_accel_odr_set(const struct device *dev, uint16_t freq)
202 {
203 int odr;
204
205 odr = lsm6dsv16x_freq_to_odr_val(dev, freq);
206 if (odr < 0) {
207 return odr;
208 }
209
210 if (lsm6dsv16x_accel_set_odr_raw(dev, odr) < 0) {
211 LOG_DBG("failed to set accelerometer sampling rate");
212 return -EIO;
213 }
214
215 return 0;
216 }
217
lsm6dsv16x_accel_range_set(const struct device * dev,int32_t range)218 static int lsm6dsv16x_accel_range_set(const struct device *dev, int32_t range)
219 {
220 int fs;
221 struct lsm6dsv16x_data *data = dev->data;
222
223 fs = lsm6dsv16x_accel_range_to_fs_val(range);
224 if (fs < 0) {
225 return fs;
226 }
227
228 if (lsm6dsv16x_accel_set_fs_raw(dev, fs) < 0) {
229 LOG_DBG("failed to set accelerometer full-scale");
230 return -EIO;
231 }
232
233 data->acc_gain = lsm6dsv16x_calc_accel_gain(fs);
234 return 0;
235 }
236
237 #define LSM6DSV16X_WU_INACT_THS_W_MAX 5
238 #define LSM6DSV16X_WAKE_UP_THS_MAX 0x3FU
239 static const float wu_inact_ths_w_lsb[] = {7.8125f, 15.625f, 31.25f, 62.5f, 125.0f, 250.0f};
240
lsm6dsv16x_accel_wake_threshold_set(const struct device * dev,const struct sensor_value * val)241 static int lsm6dsv16x_accel_wake_threshold_set(const struct device *dev,
242 const struct sensor_value *val)
243 {
244 const struct lsm6dsv16x_config *cfg = dev->config;
245 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
246 lsm6dsv16x_act_thresholds_t thresholds;
247
248 if (lsm6dsv16x_act_thresholds_get(ctx, &thresholds) < 0) {
249 LOG_DBG("failed to get thresholds");
250 return -EIO;
251 }
252
253 float val_mg = sensor_ms2_to_ug(val) / 1000.0f;
254
255 thresholds.inactivity_cfg.wu_inact_ths_w = LSM6DSV16X_WU_INACT_THS_W_MAX;
256 thresholds.threshold = LSM6DSV16X_WAKE_UP_THS_MAX;
257
258 for (uint8_t i = 0; i <= LSM6DSV16X_WU_INACT_THS_W_MAX; i++) {
259 if (val_mg < (wu_inact_ths_w_lsb[i] * (float)LSM6DSV16X_WAKE_UP_THS_MAX)) {
260 thresholds.inactivity_cfg.wu_inact_ths_w = i;
261 thresholds.threshold = (uint8_t)(val_mg / wu_inact_ths_w_lsb[i]);
262 break;
263 }
264 }
265
266 return lsm6dsv16x_act_thresholds_set(ctx, &thresholds);
267 }
268
lsm6dsv16x_accel_wake_duration_set(const struct device * dev,const struct sensor_value * val)269 static int lsm6dsv16x_accel_wake_duration_set(const struct device *dev,
270 const struct sensor_value *val)
271 {
272 const struct lsm6dsv16x_config *cfg = dev->config;
273 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
274 lsm6dsv16x_act_thresholds_t thresholds;
275
276 if (lsm6dsv16x_act_thresholds_get(ctx, &thresholds) < 0) {
277 LOG_DBG("failed to get thresholds");
278 return -EIO;
279 }
280
281 thresholds.duration = MIN(val->val1, 3);
282
283 return lsm6dsv16x_act_thresholds_set(ctx, &thresholds);
284 }
285
lsm6dsv16x_accel_config(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,const struct sensor_value * val)286 static int lsm6dsv16x_accel_config(const struct device *dev,
287 enum sensor_channel chan,
288 enum sensor_attribute attr,
289 const struct sensor_value *val)
290 {
291 const struct lsm6dsv16x_config *cfg = dev->config;
292 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
293 lsm6dsv16x_xl_mode_t mode;
294
295 switch (attr) {
296 case SENSOR_ATTR_FULL_SCALE:
297 return lsm6dsv16x_accel_range_set(dev, sensor_ms2_to_g(val));
298 case SENSOR_ATTR_SAMPLING_FREQUENCY:
299 return lsm6dsv16x_accel_odr_set(dev, val->val1);
300 case SENSOR_ATTR_SLOPE_TH:
301 return lsm6dsv16x_accel_wake_threshold_set(dev, val);
302 case SENSOR_ATTR_SLOPE_DUR:
303 return lsm6dsv16x_accel_wake_duration_set(dev, val);
304 case SENSOR_ATTR_CONFIGURATION:
305 switch (val->val1) {
306 case 0: /* High Performance */
307 mode = LSM6DSV16X_XL_HIGH_PERFORMANCE_MD;
308 break;
309 case 1: /* High Accuracy */
310 mode = LSM6DSV16X_XL_HIGH_ACCURACY_ODR_MD;
311 break;
312 case 3: /* ODR triggered */
313 mode = LSM6DSV16X_XL_ODR_TRIGGERED_MD;
314 break;
315 case 4: /* Low Power 2 */
316 mode = LSM6DSV16X_XL_LOW_POWER_2_AVG_MD;
317 break;
318 case 5: /* Low Power 4 */
319 mode = LSM6DSV16X_XL_LOW_POWER_4_AVG_MD;
320 break;
321 case 6: /* Low Power 8 */
322 mode = LSM6DSV16X_XL_LOW_POWER_8_AVG_MD;
323 break;
324 case 7: /* Normal */
325 mode = LSM6DSV16X_XL_NORMAL_MD;
326 break;
327 default:
328 return -EIO;
329 }
330
331 return lsm6dsv16x_xl_mode_set(ctx, mode);
332 default:
333 LOG_DBG("Accel attribute not supported.");
334 return -ENOTSUP;
335 }
336
337 return 0;
338 }
339
lsm6dsv16x_gyro_odr_set(const struct device * dev,uint16_t freq)340 static int lsm6dsv16x_gyro_odr_set(const struct device *dev, uint16_t freq)
341 {
342 int odr;
343
344 if (freq < 8) {
345 return -EIO;
346 }
347
348 odr = lsm6dsv16x_freq_to_odr_val(dev, freq);
349 if (odr < 0) {
350 return odr;
351 }
352
353 if (lsm6dsv16x_gyro_set_odr_raw(dev, odr) < 0) {
354 LOG_DBG("failed to set gyroscope sampling rate");
355 return -EIO;
356 }
357
358 return 0;
359 }
360
lsm6dsv16x_gyro_range_set(const struct device * dev,int32_t range)361 static int lsm6dsv16x_gyro_range_set(const struct device *dev, int32_t range)
362 {
363 int fs;
364 struct lsm6dsv16x_data *data = dev->data;
365
366 fs = lsm6dsv16x_gyro_range_to_fs_val(range);
367 if (fs < 0) {
368 return fs;
369 }
370
371 if (lsm6dsv16x_gyro_set_fs_raw(dev, fs) < 0) {
372 LOG_DBG("failed to set gyroscope full-scale");
373 return -EIO;
374 }
375
376 data->gyro_gain = lsm6dsv16x_calc_gyro_gain(fs);
377 return 0;
378 }
379
lsm6dsv16x_gyro_config(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,const struct sensor_value * val)380 static int lsm6dsv16x_gyro_config(const struct device *dev,
381 enum sensor_channel chan,
382 enum sensor_attribute attr,
383 const struct sensor_value *val)
384 {
385 const struct lsm6dsv16x_config *cfg = dev->config;
386 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
387 lsm6dsv16x_gy_mode_t mode;
388
389 switch (attr) {
390 case SENSOR_ATTR_FULL_SCALE:
391 return lsm6dsv16x_gyro_range_set(dev, sensor_rad_to_degrees(val));
392 case SENSOR_ATTR_SAMPLING_FREQUENCY:
393 return lsm6dsv16x_gyro_odr_set(dev, val->val1);
394 case SENSOR_ATTR_CONFIGURATION:
395 switch (val->val1) {
396 case 0: /* High Performance */
397 mode = LSM6DSV16X_GY_HIGH_PERFORMANCE_MD;
398 break;
399 case 1: /* High Accuracy */
400 mode = LSM6DSV16X_GY_HIGH_ACCURACY_ODR_MD;
401 break;
402 case 4: /* Sleep */
403 mode = LSM6DSV16X_GY_SLEEP_MD;
404 break;
405 case 5: /* Low Power */
406 mode = LSM6DSV16X_GY_LOW_POWER_MD;
407 break;
408 default:
409 return -EIO;
410 }
411
412 return lsm6dsv16x_gy_mode_set(ctx, mode);
413 default:
414 LOG_DBG("Gyro attribute not supported.");
415 return -ENOTSUP;
416 }
417
418 return 0;
419 }
420
lsm6dsv16x_attr_set(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,const struct sensor_value * val)421 static int lsm6dsv16x_attr_set(const struct device *dev,
422 enum sensor_channel chan,
423 enum sensor_attribute attr,
424 const struct sensor_value *val)
425 {
426 #if defined(CONFIG_LSM6DSV16X_SENSORHUB)
427 struct lsm6dsv16x_data *data = dev->data;
428 #endif /* CONFIG_LSM6DSV16X_SENSORHUB */
429
430 if (!lsm6dsv16x_is_active(dev)) {
431 return -EBUSY;
432 }
433
434 switch (chan) {
435 case SENSOR_CHAN_ACCEL_XYZ:
436 return lsm6dsv16x_accel_config(dev, chan, attr, val);
437 case SENSOR_CHAN_GYRO_XYZ:
438 return lsm6dsv16x_gyro_config(dev, chan, attr, val);
439 #if defined(CONFIG_LSM6DSV16X_SENSORHUB)
440 case SENSOR_CHAN_MAGN_XYZ:
441 case SENSOR_CHAN_PRESS:
442 case SENSOR_CHAN_HUMIDITY:
443 if (!data->shub_inited) {
444 LOG_ERR("shub not inited.");
445 return -ENOTSUP;
446 }
447
448 return lsm6dsv16x_shub_config(dev, chan, attr, val);
449 #endif /* CONFIG_LSM6DSV16X_SENSORHUB */
450 default:
451 LOG_WRN("attr_set() not supported on this channel.");
452 return -ENOTSUP;
453 }
454
455 return 0;
456 }
457
lsm6dsv16x_accel_wake_threshold_get(const struct device * dev,struct sensor_value * val)458 static int lsm6dsv16x_accel_wake_threshold_get(const struct device *dev, struct sensor_value *val)
459 {
460 const struct lsm6dsv16x_config *cfg = dev->config;
461 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
462 lsm6dsv16x_act_thresholds_t thresholds;
463 float val_mg;
464
465 if (lsm6dsv16x_act_thresholds_get(ctx, &thresholds) < 0) {
466 LOG_DBG("failed to get thresholds");
467 return -EIO;
468 }
469
470 val_mg = wu_inact_ths_w_lsb[thresholds.inactivity_cfg.wu_inact_ths_w];
471 val_mg *= (float)thresholds.threshold;
472
473 sensor_ug_to_ms2(1000.0f * val_mg, val);
474
475 return 0;
476 }
477
lsm6dsv16x_accel_wake_duration_get(const struct device * dev,struct sensor_value * val)478 static int lsm6dsv16x_accel_wake_duration_get(const struct device *dev, struct sensor_value *val)
479 {
480 const struct lsm6dsv16x_config *cfg = dev->config;
481 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
482 lsm6dsv16x_act_thresholds_t thresholds;
483
484 if (lsm6dsv16x_act_thresholds_get(ctx, &thresholds) < 0) {
485 LOG_DBG("failed to get thresholds");
486 return -EIO;
487 }
488
489 val->val1 = thresholds.duration;
490 val->val2 = 0;
491
492 return 0;
493 }
494
lsm6dsv16x_accel_get_config(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,struct sensor_value * val)495 static int lsm6dsv16x_accel_get_config(const struct device *dev,
496 enum sensor_channel chan,
497 enum sensor_attribute attr,
498 struct sensor_value *val)
499 {
500 const struct lsm6dsv16x_config *cfg = dev->config;
501 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
502 struct lsm6dsv16x_data *data = dev->data;
503
504 switch (attr) {
505 case SENSOR_ATTR_FULL_SCALE:
506 sensor_g_to_ms2(lsm6dsv16x_accel_fs_map[data->accel_fs], val);
507 break;
508 case SENSOR_ATTR_SAMPLING_FREQUENCY: {
509 lsm6dsv16x_data_rate_t odr;
510 int8_t mode;
511
512 if (lsm6dsv16x_xl_data_rate_get(ctx, &odr) < 0) {
513 return -EINVAL;
514 }
515
516 mode = (odr >> 4) & 0xf;
517
518 val->val1 = lsm6dsv16x_odr_map[mode][data->accel_freq];
519 val->val2 = 0;
520 break;
521 }
522 case SENSOR_ATTR_SLOPE_TH:
523 return lsm6dsv16x_accel_wake_threshold_get(dev, val);
524 case SENSOR_ATTR_SLOPE_DUR:
525 return lsm6dsv16x_accel_wake_duration_get(dev, val);
526 case SENSOR_ATTR_CONFIGURATION: {
527 lsm6dsv16x_xl_mode_t mode;
528
529 lsm6dsv16x_xl_mode_get(ctx, &mode);
530
531 switch (mode) {
532 case LSM6DSV16X_XL_HIGH_PERFORMANCE_MD:
533 val->val1 = 0;
534 break;
535 case LSM6DSV16X_XL_HIGH_ACCURACY_ODR_MD:
536 val->val1 = 1;
537 break;
538 case LSM6DSV16X_XL_ODR_TRIGGERED_MD:
539 val->val1 = 3;
540 break;
541 case LSM6DSV16X_XL_LOW_POWER_2_AVG_MD:
542 val->val1 = 4;
543 break;
544 case LSM6DSV16X_XL_LOW_POWER_4_AVG_MD:
545 val->val1 = 5;
546 break;
547 case LSM6DSV16X_XL_LOW_POWER_8_AVG_MD:
548 val->val1 = 6;
549 break;
550 case LSM6DSV16X_XL_NORMAL_MD:
551 val->val1 = 7;
552 break;
553 default:
554 return -EIO;
555 }
556
557 break;
558 }
559 default:
560 LOG_DBG("Attr attribute not supported.");
561 return -ENOTSUP;
562 }
563
564 return 0;
565 }
566
lsm6dsv16x_gyro_get_config(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,struct sensor_value * val)567 static int lsm6dsv16x_gyro_get_config(const struct device *dev,
568 enum sensor_channel chan,
569 enum sensor_attribute attr,
570 struct sensor_value *val)
571 {
572 const struct lsm6dsv16x_config *cfg = dev->config;
573 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
574 struct lsm6dsv16x_data *data = dev->data;
575
576 switch (attr) {
577 case SENSOR_ATTR_FULL_SCALE:
578 sensor_degrees_to_rad(lsm6dsv16x_gyro_fs_map[data->gyro_fs], val);
579 break;
580 case SENSOR_ATTR_SAMPLING_FREQUENCY: {
581 lsm6dsv16x_data_rate_t odr;
582 int8_t mode;
583
584 if (lsm6dsv16x_gy_data_rate_get(ctx, &odr) < 0) {
585 return -EINVAL;
586 }
587
588 mode = (odr >> 4) & 0xf;
589
590 val->val1 = lsm6dsv16x_odr_map[mode][data->gyro_freq];
591 val->val2 = 0;
592 break;
593 }
594 case SENSOR_ATTR_CONFIGURATION: {
595 lsm6dsv16x_gy_mode_t mode;
596
597 lsm6dsv16x_gy_mode_get(ctx, &mode);
598
599 switch (mode) {
600 case LSM6DSV16X_GY_HIGH_PERFORMANCE_MD:
601 val->val1 = 0;
602 break;
603 case LSM6DSV16X_GY_HIGH_ACCURACY_ODR_MD:
604 val->val1 = 1;
605 break;
606 case LSM6DSV16X_GY_SLEEP_MD:
607 val->val1 = 4;
608 break;
609 case LSM6DSV16X_GY_LOW_POWER_MD:
610 val->val1 = 5;
611 break;
612 default:
613 return -EIO;
614 }
615
616 break;
617 }
618 default:
619 LOG_DBG("Gyro attribute not supported.");
620 return -ENOTSUP;
621 }
622
623 return 0;
624 }
625
lsm6dsv16x_attr_get(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,struct sensor_value * val)626 static int lsm6dsv16x_attr_get(const struct device *dev, enum sensor_channel chan,
627 enum sensor_attribute attr, struct sensor_value *val)
628 {
629 if (!lsm6dsv16x_is_active(dev)) {
630 return -EBUSY;
631 }
632
633 switch (chan) {
634 case SENSOR_CHAN_ACCEL_XYZ:
635 return lsm6dsv16x_accel_get_config(dev, chan, attr, val);
636 case SENSOR_CHAN_GYRO_XYZ:
637 return lsm6dsv16x_gyro_get_config(dev, chan, attr, val);
638 default:
639 LOG_WRN("attr_get() not supported on this channel.");
640 return -ENOTSUP;
641 }
642
643 return 0;
644 }
645
lsm6dsv16x_sample_fetch_accel(const struct device * dev)646 static int lsm6dsv16x_sample_fetch_accel(const struct device *dev)
647 {
648 const struct lsm6dsv16x_config *cfg = dev->config;
649 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
650 struct lsm6dsv16x_data *data = dev->data;
651
652 if (lsm6dsv16x_acceleration_raw_get(ctx, data->acc) < 0) {
653 LOG_DBG("Failed to read sample");
654 return -EIO;
655 }
656
657 return 0;
658 }
659
lsm6dsv16x_sample_fetch_gyro(const struct device * dev)660 static int lsm6dsv16x_sample_fetch_gyro(const struct device *dev)
661 {
662 const struct lsm6dsv16x_config *cfg = dev->config;
663 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
664 struct lsm6dsv16x_data *data = dev->data;
665
666 if (lsm6dsv16x_angular_rate_raw_get(ctx, data->gyro) < 0) {
667 LOG_DBG("Failed to read sample");
668 return -EIO;
669 }
670
671 return 0;
672 }
673
674 #if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
lsm6dsv16x_sample_fetch_temp(const struct device * dev)675 static int lsm6dsv16x_sample_fetch_temp(const struct device *dev)
676 {
677 const struct lsm6dsv16x_config *cfg = dev->config;
678 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
679 struct lsm6dsv16x_data *data = dev->data;
680
681 if (lsm6dsv16x_temperature_raw_get(ctx, &data->temp_sample) < 0) {
682 LOG_DBG("Failed to read sample");
683 return -EIO;
684 }
685
686 return 0;
687 }
688 #endif
689
690 #if defined(CONFIG_LSM6DSV16X_SENSORHUB)
lsm6dsv16x_sample_fetch_shub(const struct device * dev)691 static int lsm6dsv16x_sample_fetch_shub(const struct device *dev)
692 {
693 if (lsm6dsv16x_shub_fetch_external_devs(dev) < 0) {
694 LOG_DBG("failed to read ext shub devices");
695 return -EIO;
696 }
697
698 return 0;
699 }
700 #endif /* CONFIG_LSM6DSV16X_SENSORHUB */
701
lsm6dsv16x_sample_fetch(const struct device * dev,enum sensor_channel chan)702 static int lsm6dsv16x_sample_fetch(const struct device *dev,
703 enum sensor_channel chan)
704 {
705 #if defined(CONFIG_LSM6DSV16X_SENSORHUB)
706 struct lsm6dsv16x_data *data = dev->data;
707 #endif /* CONFIG_LSM6DSV16X_SENSORHUB */
708
709 if (!lsm6dsv16x_is_active(dev)) {
710 return -EBUSY;
711 }
712
713 switch (chan) {
714 case SENSOR_CHAN_ACCEL_XYZ:
715 lsm6dsv16x_sample_fetch_accel(dev);
716 break;
717 case SENSOR_CHAN_GYRO_XYZ:
718 lsm6dsv16x_sample_fetch_gyro(dev);
719 break;
720 #if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
721 case SENSOR_CHAN_DIE_TEMP:
722 lsm6dsv16x_sample_fetch_temp(dev);
723 break;
724 #endif
725 case SENSOR_CHAN_ALL:
726 lsm6dsv16x_sample_fetch_accel(dev);
727 lsm6dsv16x_sample_fetch_gyro(dev);
728 #if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
729 lsm6dsv16x_sample_fetch_temp(dev);
730 #endif
731 #if defined(CONFIG_LSM6DSV16X_SENSORHUB)
732 if (data->shub_inited) {
733 lsm6dsv16x_sample_fetch_shub(dev);
734 }
735 #endif
736 break;
737 default:
738 return -ENOTSUP;
739 }
740
741 return 0;
742 }
743
lsm6dsv16x_accel_convert(struct sensor_value * val,int raw_val,uint32_t sensitivity)744 static inline void lsm6dsv16x_accel_convert(struct sensor_value *val, int raw_val,
745 uint32_t sensitivity)
746 {
747 int64_t dval;
748
749 /* Sensitivity is exposed in ug/LSB */
750 /* Convert to m/s^2 */
751 dval = (int64_t)(raw_val) * sensitivity;
752 sensor_ug_to_ms2(dval, val);
753 }
754
lsm6dsv16x_accel_get_channel(enum sensor_channel chan,struct sensor_value * val,struct lsm6dsv16x_data * data,uint32_t sensitivity)755 static inline int lsm6dsv16x_accel_get_channel(enum sensor_channel chan,
756 struct sensor_value *val,
757 struct lsm6dsv16x_data *data,
758 uint32_t sensitivity)
759 {
760 uint8_t i;
761
762 switch (chan) {
763 case SENSOR_CHAN_ACCEL_X:
764 lsm6dsv16x_accel_convert(val, data->acc[0], sensitivity);
765 break;
766 case SENSOR_CHAN_ACCEL_Y:
767 lsm6dsv16x_accel_convert(val, data->acc[1], sensitivity);
768 break;
769 case SENSOR_CHAN_ACCEL_Z:
770 lsm6dsv16x_accel_convert(val, data->acc[2], sensitivity);
771 break;
772 case SENSOR_CHAN_ACCEL_XYZ:
773 for (i = 0; i < 3; i++) {
774 lsm6dsv16x_accel_convert(val++, data->acc[i], sensitivity);
775 }
776 break;
777 default:
778 return -ENOTSUP;
779 }
780
781 return 0;
782 }
783
lsm6dsv16x_accel_channel_get(enum sensor_channel chan,struct sensor_value * val,struct lsm6dsv16x_data * data)784 static int lsm6dsv16x_accel_channel_get(enum sensor_channel chan,
785 struct sensor_value *val,
786 struct lsm6dsv16x_data *data)
787 {
788 return lsm6dsv16x_accel_get_channel(chan, val, data, data->acc_gain);
789 }
790
lsm6dsv16x_gyro_convert(struct sensor_value * val,int raw_val,uint32_t sensitivity)791 static inline void lsm6dsv16x_gyro_convert(struct sensor_value *val, int raw_val,
792 uint32_t sensitivity)
793 {
794 int64_t dval;
795
796 /* Sensitivity is exposed in udps/LSB */
797 /* So, calculate value in 10 udps unit and then to rad/s */
798 dval = (int64_t)(raw_val) * sensitivity / 10;
799 sensor_10udegrees_to_rad(dval, val);
800 }
801
lsm6dsv16x_gyro_get_channel(enum sensor_channel chan,struct sensor_value * val,struct lsm6dsv16x_data * data,uint32_t sensitivity)802 static inline int lsm6dsv16x_gyro_get_channel(enum sensor_channel chan,
803 struct sensor_value *val,
804 struct lsm6dsv16x_data *data,
805 uint32_t sensitivity)
806 {
807 uint8_t i;
808
809 switch (chan) {
810 case SENSOR_CHAN_GYRO_X:
811 lsm6dsv16x_gyro_convert(val, data->gyro[0], sensitivity);
812 break;
813 case SENSOR_CHAN_GYRO_Y:
814 lsm6dsv16x_gyro_convert(val, data->gyro[1], sensitivity);
815 break;
816 case SENSOR_CHAN_GYRO_Z:
817 lsm6dsv16x_gyro_convert(val, data->gyro[2], sensitivity);
818 break;
819 case SENSOR_CHAN_GYRO_XYZ:
820 for (i = 0; i < 3; i++) {
821 lsm6dsv16x_gyro_convert(val++, data->gyro[i], sensitivity);
822 }
823 break;
824 default:
825 return -ENOTSUP;
826 }
827
828 return 0;
829 }
830
lsm6dsv16x_gyro_channel_get(enum sensor_channel chan,struct sensor_value * val,struct lsm6dsv16x_data * data)831 static int lsm6dsv16x_gyro_channel_get(enum sensor_channel chan,
832 struct sensor_value *val,
833 struct lsm6dsv16x_data *data)
834 {
835 return lsm6dsv16x_gyro_get_channel(chan, val, data, data->gyro_gain);
836 }
837
838 #if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
lsm6dsv16x_gyro_channel_get_temp(struct sensor_value * val,struct lsm6dsv16x_data * data)839 static void lsm6dsv16x_gyro_channel_get_temp(struct sensor_value *val,
840 struct lsm6dsv16x_data *data)
841 {
842 /* convert units to micro Celsius. Raw temperature samples are
843 * expressed in 256 LSB/deg_C units. And LSB output is 0 at 25 C.
844 */
845 int64_t temp_sample = data->temp_sample;
846 int64_t micro_c = (temp_sample * 1000000LL) / 256;
847
848 val->val1 = (int32_t)(micro_c / 1000000) + 25;
849 val->val2 = (int32_t)(micro_c % 1000000);
850 }
851 #endif
852
853 #if defined(CONFIG_LSM6DSV16X_SENSORHUB)
lsm6dsv16x_magn_convert(struct sensor_value * val,int raw_val,uint16_t sensitivity)854 static inline void lsm6dsv16x_magn_convert(struct sensor_value *val, int raw_val,
855 uint16_t sensitivity)
856 {
857 double dval;
858
859 /* Sensitivity is exposed in ugauss/LSB */
860 dval = (double)(raw_val * sensitivity);
861 val->val1 = (int32_t)dval / 1000000;
862 val->val2 = (int32_t)dval % 1000000;
863 }
864
lsm6dsv16x_magn_get_channel(enum sensor_channel chan,struct sensor_value * val,struct lsm6dsv16x_data * data)865 static inline int lsm6dsv16x_magn_get_channel(enum sensor_channel chan,
866 struct sensor_value *val,
867 struct lsm6dsv16x_data *data)
868 {
869 int16_t sample[3];
870 int idx;
871
872 idx = lsm6dsv16x_shub_get_idx(data->dev, SENSOR_CHAN_MAGN_XYZ);
873 if (idx < 0) {
874 LOG_DBG("external magn not supported");
875 return -ENOTSUP;
876 }
877
878
879 sample[0] = (int16_t)(data->ext_data[idx][0] |
880 (data->ext_data[idx][1] << 8));
881 sample[1] = (int16_t)(data->ext_data[idx][2] |
882 (data->ext_data[idx][3] << 8));
883 sample[2] = (int16_t)(data->ext_data[idx][4] |
884 (data->ext_data[idx][5] << 8));
885
886 switch (chan) {
887 case SENSOR_CHAN_MAGN_X:
888 lsm6dsv16x_magn_convert(val, sample[0], data->magn_gain);
889 break;
890 case SENSOR_CHAN_MAGN_Y:
891 lsm6dsv16x_magn_convert(val, sample[1], data->magn_gain);
892 break;
893 case SENSOR_CHAN_MAGN_Z:
894 lsm6dsv16x_magn_convert(val, sample[2], data->magn_gain);
895 break;
896 case SENSOR_CHAN_MAGN_XYZ:
897 lsm6dsv16x_magn_convert(val, sample[0], data->magn_gain);
898 lsm6dsv16x_magn_convert(val + 1, sample[1], data->magn_gain);
899 lsm6dsv16x_magn_convert(val + 2, sample[2], data->magn_gain);
900 break;
901 default:
902 return -ENOTSUP;
903 }
904
905 return 0;
906 }
907
lsm6dsv16x_hum_convert(struct sensor_value * val,struct lsm6dsv16x_data * data)908 static inline void lsm6dsv16x_hum_convert(struct sensor_value *val,
909 struct lsm6dsv16x_data *data)
910 {
911 float rh;
912 int16_t raw_val;
913 struct hts221_data *ht = &data->hts221;
914 int idx;
915
916 idx = lsm6dsv16x_shub_get_idx(data->dev, SENSOR_CHAN_HUMIDITY);
917 if (idx < 0) {
918 LOG_DBG("external press/temp not supported");
919 return;
920 }
921
922 raw_val = (int16_t)(data->ext_data[idx][0] |
923 (data->ext_data[idx][1] << 8));
924
925 /* find relative humidty by linear interpolation */
926 rh = (ht->y1 - ht->y0) * raw_val + ht->x1 * ht->y0 - ht->x0 * ht->y1;
927 rh /= (ht->x1 - ht->x0);
928
929 /* convert humidity to integer and fractional part */
930 val->val1 = rh;
931 val->val2 = rh * 1000000;
932 }
933
lsm6dsv16x_press_convert(struct sensor_value * val,struct lsm6dsv16x_data * data)934 static inline void lsm6dsv16x_press_convert(struct sensor_value *val,
935 struct lsm6dsv16x_data *data)
936 {
937 int32_t raw_val;
938 int idx;
939
940 idx = lsm6dsv16x_shub_get_idx(data->dev, SENSOR_CHAN_PRESS);
941 if (idx < 0) {
942 LOG_DBG("external press/temp not supported");
943 return;
944 }
945
946 raw_val = (int32_t)(data->ext_data[idx][0] |
947 (data->ext_data[idx][1] << 8) |
948 (data->ext_data[idx][2] << 16));
949
950 /* Pressure sensitivity is 4096 LSB/hPa */
951 /* Convert raw_val to val in kPa */
952 val->val1 = (raw_val >> 12) / 10;
953 val->val2 = (raw_val >> 12) % 10 * 100000 +
954 (((int32_t)((raw_val) & 0x0FFF) * 100000L) >> 12);
955 }
956
lsm6dsv16x_temp_convert(struct sensor_value * val,struct lsm6dsv16x_data * data)957 static inline void lsm6dsv16x_temp_convert(struct sensor_value *val,
958 struct lsm6dsv16x_data *data)
959 {
960 int16_t raw_val;
961 int idx;
962
963 idx = lsm6dsv16x_shub_get_idx(data->dev, SENSOR_CHAN_PRESS);
964 if (idx < 0) {
965 LOG_DBG("external press/temp not supported");
966 return;
967 }
968
969 raw_val = (int16_t)(data->ext_data[idx][3] |
970 (data->ext_data[idx][4] << 8));
971
972 /* Temperature sensitivity is 100 LSB/deg C */
973 val->val1 = raw_val / 100;
974 val->val2 = (int32_t)raw_val % 100 * (10000);
975 }
976 #endif
977
lsm6dsv16x_channel_get(const struct device * dev,enum sensor_channel chan,struct sensor_value * val)978 static int lsm6dsv16x_channel_get(const struct device *dev,
979 enum sensor_channel chan,
980 struct sensor_value *val)
981 {
982 struct lsm6dsv16x_data *data = dev->data;
983
984 if (!lsm6dsv16x_is_active(dev)) {
985 return -EBUSY;
986 }
987
988 switch (chan) {
989 case SENSOR_CHAN_ACCEL_X:
990 case SENSOR_CHAN_ACCEL_Y:
991 case SENSOR_CHAN_ACCEL_Z:
992 case SENSOR_CHAN_ACCEL_XYZ:
993 lsm6dsv16x_accel_channel_get(chan, val, data);
994 break;
995 case SENSOR_CHAN_GYRO_X:
996 case SENSOR_CHAN_GYRO_Y:
997 case SENSOR_CHAN_GYRO_Z:
998 case SENSOR_CHAN_GYRO_XYZ:
999 lsm6dsv16x_gyro_channel_get(chan, val, data);
1000 break;
1001 #if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
1002 case SENSOR_CHAN_DIE_TEMP:
1003 lsm6dsv16x_gyro_channel_get_temp(val, data);
1004 break;
1005 #endif
1006 #if defined(CONFIG_LSM6DSV16X_SENSORHUB)
1007 case SENSOR_CHAN_MAGN_X:
1008 case SENSOR_CHAN_MAGN_Y:
1009 case SENSOR_CHAN_MAGN_Z:
1010 case SENSOR_CHAN_MAGN_XYZ:
1011 if (!data->shub_inited) {
1012 LOG_ERR("attr_set() shub not inited.");
1013 return -ENOTSUP;
1014 }
1015
1016 lsm6dsv16x_magn_get_channel(chan, val, data);
1017 break;
1018
1019 case SENSOR_CHAN_HUMIDITY:
1020 if (!data->shub_inited) {
1021 LOG_ERR("attr_set() shub not inited.");
1022 return -ENOTSUP;
1023 }
1024
1025 lsm6dsv16x_hum_convert(val, data);
1026 break;
1027
1028 case SENSOR_CHAN_PRESS:
1029 if (!data->shub_inited) {
1030 LOG_ERR("attr_set() shub not inited.");
1031 return -ENOTSUP;
1032 }
1033
1034 lsm6dsv16x_press_convert(val, data);
1035 break;
1036
1037 case SENSOR_CHAN_AMBIENT_TEMP:
1038 if (!data->shub_inited) {
1039 LOG_ERR("attr_set() shub not inited.");
1040 return -ENOTSUP;
1041 }
1042
1043 lsm6dsv16x_temp_convert(val, data);
1044 break;
1045 #endif
1046 default:
1047 return -ENOTSUP;
1048 }
1049
1050 return 0;
1051 }
1052
1053 static DEVICE_API(sensor, lsm6dsv16x_driver_api) = {
1054 .attr_set = lsm6dsv16x_attr_set,
1055 .attr_get = lsm6dsv16x_attr_get,
1056 #if CONFIG_LSM6DSV16X_TRIGGER
1057 .trigger_set = lsm6dsv16x_trigger_set,
1058 #endif
1059 .sample_fetch = lsm6dsv16x_sample_fetch,
1060 .channel_get = lsm6dsv16x_channel_get,
1061 #ifdef CONFIG_SENSOR_ASYNC_API
1062 .get_decoder = lsm6dsv16x_get_decoder,
1063 .submit = lsm6dsv16x_submit,
1064 #endif
1065 };
1066
lsm6dsv16x_init_chip(const struct device * dev)1067 static int lsm6dsv16x_init_chip(const struct device *dev)
1068 {
1069 const struct lsm6dsv16x_config *cfg = dev->config;
1070 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
1071 struct lsm6dsv16x_data *lsm6dsv16x = dev->data;
1072 uint8_t chip_id;
1073 uint8_t odr, fs;
1074
1075 #if DT_ANY_INST_ON_BUS_STATUS_OKAY(i3c)
1076 if (cfg->i3c.bus != NULL) {
1077 /*
1078 * Need to grab the pointer to the I3C device descriptor
1079 * before we can talk to the sensor.
1080 */
1081 lsm6dsv16x->i3c_dev = i3c_device_find(cfg->i3c.bus, &cfg->i3c.dev_id);
1082 if (lsm6dsv16x->i3c_dev == NULL) {
1083 LOG_ERR("Cannot find I3C device descriptor");
1084 return -ENODEV;
1085 }
1086 }
1087 #endif
1088
1089 /* All registers except 0x01 are different between banks, including the WHO_AM_I
1090 * register and the register used for a SW reset. If the lsm6dsv16x wasn't on the user
1091 * bank when it reset, then both the chip id check and the sw reset will fail unless we
1092 * set the bank now.
1093 */
1094 if (lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK) < 0) {
1095 LOG_DBG("Failed to set user bank");
1096 return -EIO;
1097 }
1098
1099 if (lsm6dsv16x_device_id_get(ctx, &chip_id) < 0) {
1100 LOG_DBG("Failed reading chip id");
1101 return -EIO;
1102 }
1103
1104 LOG_INF("chip id 0x%x", chip_id);
1105
1106 if (chip_id != LSM6DSV16X_ID) {
1107 LOG_DBG("Invalid chip id 0x%x", chip_id);
1108 return -EIO;
1109 }
1110
1111 /* Resetting the whole device while using I3C will also reset the DA, therefore perform
1112 * only a software reset if the bus is I3C. It should be assumed that the device was
1113 * already fully reset by the I3C CCC RSTACT (whole chip) done as apart of the I3C Bus
1114 * initialization.
1115 */
1116 if (ON_I3C_BUS(cfg)) {
1117 /* Restore default configuration */
1118 lsm6dsv16x_reset_set(ctx, LSM6DSV16X_RESTORE_CAL_PARAM);
1119
1120 /* wait 150us as reported in AN5763 */
1121 k_sleep(K_USEC(150));
1122 } else {
1123 /* reset device (sw_por) */
1124 if (lsm6dsv16x_reset_set(ctx, LSM6DSV16X_GLOBAL_RST) < 0) {
1125 return -EIO;
1126 }
1127
1128 /* wait 30ms as reported in AN5763 */
1129 k_sleep(K_MSEC(30));
1130 }
1131
1132 fs = cfg->accel_range;
1133 LOG_DBG("accel range is %d", fs);
1134 if (lsm6dsv16x_accel_set_fs_raw(dev, fs) < 0) {
1135 LOG_ERR("failed to set accelerometer range %d", fs);
1136 return -EIO;
1137 }
1138 lsm6dsv16x->acc_gain = lsm6dsv16x_calc_accel_gain(fs);
1139
1140 odr = cfg->accel_odr;
1141 LOG_DBG("accel odr is %d", odr);
1142 if (lsm6dsv16x_accel_set_odr_raw(dev, odr) < 0) {
1143 LOG_ERR("failed to set accelerometer odr %d", odr);
1144 return -EIO;
1145 }
1146
1147 fs = cfg->gyro_range;
1148 LOG_DBG("gyro range is %d", fs);
1149 if (lsm6dsv16x_gyro_set_fs_raw(dev, fs) < 0) {
1150 LOG_ERR("failed to set gyroscope range %d", fs);
1151 return -EIO;
1152 }
1153 lsm6dsv16x->gyro_gain = lsm6dsv16x_calc_gyro_gain(fs);
1154
1155 odr = cfg->gyro_odr;
1156 LOG_DBG("gyro odr is %d", odr);
1157 lsm6dsv16x->gyro_freq = odr;
1158 if (lsm6dsv16x_gyro_set_odr_raw(dev, odr) < 0) {
1159 LOG_ERR("failed to set gyroscope odr %d", odr);
1160 return -EIO;
1161 }
1162
1163 #if DT_ANY_INST_ON_BUS_STATUS_OKAY(i3c)
1164 if (IS_ENABLED(CONFIG_LSM6DSV16X_STREAM) && (ON_I3C_BUS(cfg))) {
1165 /*
1166 * Set MRL to the Max Size of the FIFO so the entire FIFO can be read
1167 * out at once
1168 */
1169 struct i3c_ccc_mrl setmrl = {
1170 .len = 0x0700,
1171 .ibi_len = lsm6dsv16x->i3c_dev->data_length.max_ibi,
1172 };
1173 if (i3c_ccc_do_setmrl(lsm6dsv16x->i3c_dev, &setmrl) < 0) {
1174 LOG_ERR("failed to set mrl");
1175 return -EIO;
1176 }
1177 }
1178 #endif
1179
1180 if (lsm6dsv16x_block_data_update_set(ctx, 1) < 0) {
1181 LOG_DBG("failed to set BDU mode");
1182 return -EIO;
1183 }
1184
1185 return 0;
1186 }
1187
lsm6dsv16x_init(const struct device * dev)1188 static int lsm6dsv16x_init(const struct device *dev)
1189 {
1190 #ifdef CONFIG_LSM6DSV16X_TRIGGER
1191 const struct lsm6dsv16x_config *cfg = dev->config;
1192 #endif
1193 struct lsm6dsv16x_data *data = dev->data;
1194
1195 LOG_INF("Initialize device %s", dev->name);
1196 data->dev = dev;
1197
1198 if (lsm6dsv16x_init_chip(dev) < 0) {
1199 LOG_DBG("failed to initialize chip");
1200 return -EIO;
1201 }
1202
1203 #ifdef CONFIG_LSM6DSV16X_TRIGGER
1204 if (cfg->trig_enabled) {
1205 if (lsm6dsv16x_init_interrupt(dev) < 0) {
1206 LOG_ERR("Failed to initialize interrupt.");
1207 return -EIO;
1208 }
1209 }
1210 #endif
1211
1212 #ifdef CONFIG_LSM6DSV16X_SENSORHUB
1213 data->shub_inited = true;
1214 if (lsm6dsv16x_shub_init(dev) < 0) {
1215 LOG_INF("shub: no external chips found");
1216 data->shub_inited = false;
1217 }
1218 #endif
1219
1220 return 0;
1221 }
1222
1223 #if defined(CONFIG_PM_DEVICE)
lsm6dsv16x_pm_action(const struct device * dev,enum pm_device_action action)1224 static int lsm6dsv16x_pm_action(const struct device *dev, enum pm_device_action action)
1225 {
1226 struct lsm6dsv16x_data *data = dev->data;
1227 const struct lsm6dsv16x_config *cfg = dev->config;
1228 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
1229 int ret = 0;
1230
1231 LOG_DBG("PM action: %d", (int)action);
1232
1233 switch (action) {
1234 case PM_DEVICE_ACTION_RESUME:
1235 if (lsm6dsv16x_xl_data_rate_set(ctx, data->accel_freq) < 0) {
1236 LOG_ERR("failed to set accelerometer odr %d", (int)data->accel_freq);
1237 ret = -EIO;
1238 }
1239 if (lsm6dsv16x_gy_data_rate_set(ctx, data->gyro_freq) < 0) {
1240 LOG_ERR("failed to set gyroscope odr %d", (int)data->gyro_freq);
1241 ret = -EIO;
1242 }
1243 break;
1244 case PM_DEVICE_ACTION_SUSPEND:
1245 if (lsm6dsv16x_xl_data_rate_set(ctx, LSM6DSV16X_DT_ODR_OFF) < 0) {
1246 LOG_ERR("failed to disable accelerometer");
1247 ret = -EIO;
1248 }
1249 if (lsm6dsv16x_gy_data_rate_set(ctx, LSM6DSV16X_DT_ODR_OFF) < 0) {
1250 LOG_ERR("failed to disable gyroscope");
1251 ret = -EIO;
1252 }
1253 break;
1254 default:
1255 ret = -ENOTSUP;
1256 break;
1257 }
1258
1259 return ret;
1260 }
1261 #endif /* CONFIG_PM_DEVICE */
1262
1263 #if DT_NUM_INST_STATUS_OKAY(DT_DRV_COMPAT) == 0
1264 #warning "LSM6DSV16X driver enabled without any devices"
1265 #endif
1266
1267 /*
1268 * Device creation macro, shared by LSM6DSV16X_DEFINE_SPI() and
1269 * LSM6DSV16X_DEFINE_I2C().
1270 */
1271
1272 #define LSM6DSV16X_DEVICE_INIT(inst) \
1273 PM_DEVICE_DT_INST_DEFINE(inst, lsm6dsv16x_pm_action); \
1274 SENSOR_DEVICE_DT_INST_DEFINE(inst, lsm6dsv16x_init, PM_DEVICE_DT_INST_GET(inst), \
1275 &lsm6dsv16x_data_##inst, &lsm6dsv16x_config_##inst, \
1276 POST_KERNEL, CONFIG_SENSOR_INIT_PRIORITY, \
1277 &lsm6dsv16x_driver_api);
1278
1279 #ifdef CONFIG_LSM6DSV16X_TRIGGER
1280 #define LSM6DSV16X_CFG_IRQ(inst) \
1281 .trig_enabled = true, \
1282 .int1_gpio = GPIO_DT_SPEC_INST_GET_OR(inst, int1_gpios, { 0 }), \
1283 .int2_gpio = GPIO_DT_SPEC_INST_GET_OR(inst, int2_gpios, { 0 }), \
1284 .drdy_pulsed = DT_INST_PROP(inst, drdy_pulsed), \
1285 .drdy_pin = DT_INST_PROP(inst, drdy_pin)
1286 #else
1287 #define LSM6DSV16X_CFG_IRQ(inst)
1288 #endif /* CONFIG_LSM6DSV16X_TRIGGER */
1289
1290 #define LSM6DSV16X_CONFIG_COMMON(inst) \
1291 .accel_odr = DT_INST_PROP(inst, accel_odr), \
1292 .accel_range = DT_INST_PROP(inst, accel_range), \
1293 .gyro_odr = DT_INST_PROP(inst, gyro_odr), \
1294 .gyro_range = DT_INST_PROP(inst, gyro_range), \
1295 IF_ENABLED(CONFIG_LSM6DSV16X_STREAM, \
1296 (.fifo_wtm = DT_INST_PROP(inst, fifo_watermark), \
1297 .accel_batch = DT_INST_PROP(inst, accel_fifo_batch_rate), \
1298 .gyro_batch = DT_INST_PROP(inst, gyro_fifo_batch_rate), \
1299 .sflp_odr = DT_INST_PROP(inst, sflp_odr), \
1300 .sflp_fifo_en = DT_INST_PROP(inst, sflp_fifo_enable), \
1301 .temp_batch = DT_INST_PROP(inst, temp_fifo_batch_rate),)) \
1302 IF_ENABLED(UTIL_OR(DT_INST_NODE_HAS_PROP(inst, int1_gpios), \
1303 DT_INST_NODE_HAS_PROP(inst, int2_gpios)), \
1304 (LSM6DSV16X_CFG_IRQ(inst)))
1305
1306 /*
1307 * Instantiation macros used when a device is on a SPI bus.
1308 */
1309
1310 #define LSM6DSV16X_SPI_OP (SPI_WORD_SET(8) | \
1311 SPI_OP_MODE_MASTER | \
1312 SPI_MODE_CPOL | \
1313 SPI_MODE_CPHA) \
1314
1315 #define LSM6DSV16X_SPI_RTIO_DEFINE(inst) \
1316 SPI_DT_IODEV_DEFINE(lsm6dsv16x_iodev_##inst, \
1317 DT_DRV_INST(inst), LSM6DSV16X_SPI_OP, 0U); \
1318 RTIO_DEFINE(lsm6dsv16x_rtio_ctx_##inst, 4, 4);
1319
1320 #define LSM6DSV16X_CONFIG_SPI(inst) \
1321 { \
1322 STMEMSC_CTX_SPI(&lsm6dsv16x_config_##inst.stmemsc_cfg), \
1323 .stmemsc_cfg = { \
1324 .spi = SPI_DT_SPEC_INST_GET(inst, \
1325 LSM6DSV16X_SPI_OP, \
1326 0), \
1327 }, \
1328 LSM6DSV16X_CONFIG_COMMON(inst) \
1329 }
1330
1331 #define LSM6DSV16X_DEFINE_SPI(inst) \
1332 IF_ENABLED(UTIL_AND(CONFIG_LSM6DSV16X_STREAM, \
1333 CONFIG_SPI_RTIO), \
1334 (LSM6DSV16X_SPI_RTIO_DEFINE(inst))); \
1335 static struct lsm6dsv16x_data lsm6dsv16x_data_##inst = { \
1336 IF_ENABLED(UTIL_AND(CONFIG_LSM6DSV16X_STREAM, \
1337 CONFIG_SPI_RTIO), \
1338 (.rtio_ctx = &lsm6dsv16x_rtio_ctx_##inst, \
1339 .iodev = &lsm6dsv16x_iodev_##inst, \
1340 .bus_type = BUS_SPI,)) \
1341 }; \
1342 static const struct lsm6dsv16x_config lsm6dsv16x_config_##inst = \
1343 LSM6DSV16X_CONFIG_SPI(inst); \
1344
1345 /*
1346 * Instantiation macros used when a device is on an I2C bus.
1347 */
1348
1349 #define LSM6DSV16X_I2C_RTIO_DEFINE(inst) \
1350 I2C_DT_IODEV_DEFINE(lsm6dsv16x_iodev_##inst, DT_DRV_INST(inst));\
1351 RTIO_DEFINE(lsm6dsv16x_rtio_ctx_##inst, 4, 4);
1352
1353 #define LSM6DSV16X_CONFIG_I2C(inst) \
1354 { \
1355 STMEMSC_CTX_I2C(&lsm6dsv16x_config_##inst.stmemsc_cfg), \
1356 .stmemsc_cfg = { \
1357 .i2c = I2C_DT_SPEC_INST_GET(inst), \
1358 }, \
1359 LSM6DSV16X_CONFIG_COMMON(inst) \
1360 }
1361
1362
1363 #define LSM6DSV16X_DEFINE_I2C(inst) \
1364 IF_ENABLED(UTIL_AND(CONFIG_LSM6DSV16X_STREAM, \
1365 CONFIG_I2C_RTIO), \
1366 (LSM6DSV16X_I2C_RTIO_DEFINE(inst))); \
1367 static struct lsm6dsv16x_data lsm6dsv16x_data_##inst = { \
1368 IF_ENABLED(UTIL_AND(CONFIG_LSM6DSV16X_STREAM, \
1369 CONFIG_I2C_RTIO), \
1370 (.rtio_ctx = &lsm6dsv16x_rtio_ctx_##inst, \
1371 .iodev = &lsm6dsv16x_iodev_##inst, \
1372 .bus_type = BUS_I2C,)) \
1373 }; \
1374 static const struct lsm6dsv16x_config lsm6dsv16x_config_##inst = \
1375 LSM6DSV16X_CONFIG_I2C(inst); \
1376
1377 /*
1378 * Instantiation macros used when a device is on an I3C bus.
1379 */
1380
1381 #define LSM6DSV16X_I3C_RTIO_DEFINE(inst) \
1382 I3C_DT_IODEV_DEFINE(lsm6dsv16x_i3c_iodev_##inst, DT_DRV_INST(inst)); \
1383 RTIO_DEFINE(lsm6dsv16x_rtio_ctx_##inst, 4, 4);
1384
1385 #define LSM6DSV16X_CONFIG_I3C(inst) \
1386 { \
1387 STMEMSC_CTX_I3C(&lsm6dsv16x_config_##inst.stmemsc_cfg), \
1388 .stmemsc_cfg = { \
1389 .i3c = &lsm6dsv16x_data_##inst.i3c_dev, \
1390 }, \
1391 .i3c.bus = DEVICE_DT_GET(DT_INST_BUS(inst)), \
1392 .i3c.dev_id = I3C_DEVICE_ID_DT_INST(inst), \
1393 IF_ENABLED(CONFIG_LSM6DSV16X_TRIGGER, \
1394 (.int_en_i3c = DT_INST_PROP(inst, int_en_i3c), \
1395 .bus_act_sel = DT_INST_ENUM_IDX(inst, bus_act_sel_us),)) \
1396 LSM6DSV16X_CONFIG_COMMON(inst) \
1397 }
1398
1399 #define LSM6DSV16X_DEFINE_I3C(inst) \
1400 IF_ENABLED(UTIL_AND(CONFIG_LSM6DSV16X_STREAM, \
1401 CONFIG_I3C_RTIO), \
1402 (LSM6DSV16X_I3C_RTIO_DEFINE(inst))); \
1403 static struct lsm6dsv16x_data lsm6dsv16x_data_##inst = { \
1404 IF_ENABLED(UNTIL_AND(CONFIG_LSM6DSV16X_STREAM, \
1405 CONFIG_I3C_RTIO), \
1406 (.rtio_ctx = &lsm6dsv16x_rtio_ctx_##inst, \
1407 .iodev = &lsm6dsv16x_i3c_iodev_##inst, \
1408 .bus_type = BUS_I3C,)) \
1409 }; \
1410 static const struct lsm6dsv16x_config lsm6dsv16x_config_##inst = \
1411 LSM6DSV16X_CONFIG_I3C(inst); \
1412
1413 #define LSM6DSV16X_DEFINE_I3C_OR_I2C(inst) \
1414 COND_CODE_0(DT_INST_PROP_BY_IDX(inst, reg, 1), \
1415 (LSM6DSV16X_DEFINE_I2C(inst)), \
1416 (LSM6DSV16X_DEFINE_I3C(inst)))
1417
1418 /*
1419 * Main instantiation macro. Use of COND_CODE_1() selects the right
1420 * bus-specific macro at preprocessor time.
1421 */
1422
1423 #define LSM6DSV16X_DEFINE(inst) \
1424 COND_CODE_1(DT_INST_ON_BUS(inst, spi), \
1425 (LSM6DSV16X_DEFINE_SPI(inst)), \
1426 (COND_CODE_1(DT_INST_ON_BUS(inst, i3c), \
1427 (LSM6DSV16X_DEFINE_I3C_OR_I2C(inst)), \
1428 (LSM6DSV16X_DEFINE_I2C(inst))))); \
1429 LSM6DSV16X_DEVICE_INIT(inst)
1430
1431 DT_INST_FOREACH_STATUS_OKAY(LSM6DSV16X_DEFINE)
1432
