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/device.h>
14 #include <zephyr/drivers/i2c.h>
15 #include <zephyr/sys/__assert.h>
16 #include <zephyr/sys/util.h>
17 #include <zephyr/kernel.h>
18 #include <zephyr/drivers/sensor.h>
19 #include <zephyr/logging/log.h>
20
21 #include "lsm6dsv16x.h"
22
23 LOG_MODULE_DECLARE(LSM6DSV16X, CONFIG_SENSOR_LOG_LEVEL);
24
25 static int lsm6dsv16x_shub_write_target_reg(const struct device *dev,
26 uint8_t trgt_addr, uint8_t trgt_reg,
27 uint8_t *value, uint16_t len);
28 static int lsm6dsv16x_shub_read_target_reg(const struct device *dev,
29 uint8_t trgt_addr, uint8_t trgt_reg,
30 uint8_t *value, uint16_t len);
31 static void lsm6dsv16x_shub_enable(const struct device *dev, uint8_t enable);
32
33
34 /* ST HAL skips this register, only supports it via the slower lsm6dsv16x_sh_status_get() */
lsm6dsv16x_sh_status_mainpage_get(stmdev_ctx_t * ctx,lsm6dsv16x_status_master_t * val)35 static int32_t lsm6dsv16x_sh_status_mainpage_get(stmdev_ctx_t *ctx,
36 lsm6dsv16x_status_master_t *val)
37 {
38 return lsm6dsv16x_read_reg(ctx, LSM6DSV16X_STATUS_MASTER_MAINPAGE, (uint8_t *)val, 1);
39 }
40
41 /*
42 * LIS2MDL magn device specific part
43 */
44 #ifdef CONFIG_LSM6DSV16X_EXT_LIS2MDL
45
46 #define LIS2MDL_CFG_REG_A 0x60
47 #define LIS2MDL_CFG_REG_B 0x61
48 #define LIS2MDL_CFG_REG_C 0x62
49 #define LIS2MDL_STATUS_REG 0x67
50
51 #define LIS2MDL_SW_RESET 0x20
52 #define LIS2MDL_ODR_10HZ 0x00
53 #define LIS2MDL_ODR_100HZ 0x0C
54 #define LIS2MDL_OFF_CANC 0x02
55 #define LIS2MDL_SENSITIVITY 1500
56
lsm6dsv16x_lis2mdl_init(const struct device * dev,uint8_t i2c_addr)57 static int lsm6dsv16x_lis2mdl_init(const struct device *dev, uint8_t i2c_addr)
58 {
59 struct lsm6dsv16x_data *data = dev->data;
60 uint8_t mag_cfg[2];
61
62 data->magn_gain = LIS2MDL_SENSITIVITY;
63
64 /* sw reset device */
65 mag_cfg[0] = LIS2MDL_SW_RESET;
66 lsm6dsv16x_shub_write_target_reg(dev, i2c_addr,
67 LIS2MDL_CFG_REG_A, mag_cfg, 1);
68
69 k_sleep(K_MSEC(10)); /* turn-on time in ms */
70
71 /* configure mag */
72 mag_cfg[0] = LIS2MDL_ODR_10HZ;
73 mag_cfg[1] = LIS2MDL_OFF_CANC;
74 lsm6dsv16x_shub_write_target_reg(dev, i2c_addr,
75 LIS2MDL_CFG_REG_A, mag_cfg, 2);
76
77 return 0;
78 }
79
80 static const uint16_t lis2mdl_map[] = {10, 20, 50, 100};
81
lsm6dsv16x_lis2mdl_odr_set(const struct device * dev,uint8_t i2c_addr,uint16_t freq)82 static int lsm6dsv16x_lis2mdl_odr_set(const struct device *dev,
83 uint8_t i2c_addr, uint16_t freq)
84 {
85 uint8_t odr, cfg;
86
87 for (odr = 0; odr < ARRAY_SIZE(lis2mdl_map); odr++) {
88 if (freq <= lis2mdl_map[odr]) {
89 break;
90 }
91 }
92
93 if (odr == ARRAY_SIZE(lis2mdl_map)) {
94 LOG_DBG("shub: LIS2MDL freq val %d not supported.", freq);
95 return -ENOTSUP;
96 }
97
98 cfg = (odr << 2);
99 lsm6dsv16x_shub_write_target_reg(dev, i2c_addr,
100 LIS2MDL_CFG_REG_A, &cfg, 1);
101
102 lsm6dsv16x_shub_enable(dev, 1);
103 return 0;
104 }
105
lsm6dsv16x_lis2mdl_conf(const struct device * dev,uint8_t i2c_addr,enum sensor_channel chan,enum sensor_attribute attr,const struct sensor_value * val)106 static int lsm6dsv16x_lis2mdl_conf(const struct device *dev, uint8_t i2c_addr,
107 enum sensor_channel chan,
108 enum sensor_attribute attr,
109 const struct sensor_value *val)
110 {
111 switch (attr) {
112 case SENSOR_ATTR_SAMPLING_FREQUENCY:
113 return lsm6dsv16x_lis2mdl_odr_set(dev, i2c_addr, val->val1);
114 default:
115 LOG_DBG("shub: LIS2MDL attribute not supported.");
116 return -ENOTSUP;
117 }
118
119 return 0;
120 }
121 #endif /* CONFIG_LSM6DSV16X_EXT_LIS2MDL */
122
123 /*
124 * HTS221 humidity device specific part
125 */
126 #ifdef CONFIG_LSM6DSV16X_EXT_HTS221
127
128 #define HTS221_AUTOINCREMENT BIT(7)
129
130 #define HTS221_REG_CTRL1 0x20
131 #define HTS221_ODR_1HZ 0x01
132 #define HTS221_BDU 0x04
133 #define HTS221_PD 0x80
134
135 #define HTS221_REG_CONV_START 0x30
136
lsm6dsv16x_hts221_read_conv_data(const struct device * dev,uint8_t i2c_addr)137 static int lsm6dsv16x_hts221_read_conv_data(const struct device *dev,
138 uint8_t i2c_addr)
139 {
140 struct lsm6dsv16x_data *data = dev->data;
141 uint8_t buf[16], i;
142 struct hts221_data *ht = &data->hts221;
143
144 for (i = 0; i < sizeof(buf); i += 7) {
145 unsigned char len = MIN(7, sizeof(buf) - i);
146
147 if (lsm6dsv16x_shub_read_target_reg(dev, i2c_addr,
148 (HTS221_REG_CONV_START + i) |
149 HTS221_AUTOINCREMENT,
150 &buf[i], len) < 0) {
151 LOG_DBG("shub: failed to read hts221 conv data");
152 return -EIO;
153 }
154 }
155
156 ht->y0 = buf[0] / 2;
157 ht->y1 = buf[1] / 2;
158 ht->x0 = buf[6] | (buf[7] << 8);
159 ht->x1 = buf[10] | (buf[11] << 8);
160
161 return 0;
162 }
163
lsm6dsv16x_hts221_init(const struct device * dev,uint8_t i2c_addr)164 static int lsm6dsv16x_hts221_init(const struct device *dev, uint8_t i2c_addr)
165 {
166 uint8_t hum_cfg;
167
168 /* configure ODR and BDU */
169 hum_cfg = HTS221_ODR_1HZ | HTS221_BDU | HTS221_PD;
170 lsm6dsv16x_shub_write_target_reg(dev, i2c_addr,
171 HTS221_REG_CTRL1, &hum_cfg, 1);
172
173 return lsm6dsv16x_hts221_read_conv_data(dev, i2c_addr);
174 }
175
176 static const uint16_t hts221_map[] = {0, 1, 7, 12};
177
lsm6dsv16x_hts221_odr_set(const struct device * dev,uint8_t i2c_addr,uint16_t freq)178 static int lsm6dsv16x_hts221_odr_set(const struct device *dev,
179 uint8_t i2c_addr, uint16_t freq)
180 {
181 uint8_t odr, cfg;
182
183 for (odr = 0; odr < ARRAY_SIZE(hts221_map); odr++) {
184 if (freq <= hts221_map[odr]) {
185 break;
186 }
187 }
188
189 if (odr == ARRAY_SIZE(hts221_map)) {
190 LOG_DBG("shub: HTS221 freq val %d not supported.", freq);
191 return -ENOTSUP;
192 }
193
194 cfg = odr | HTS221_BDU | HTS221_PD;
195 lsm6dsv16x_shub_write_target_reg(dev, i2c_addr,
196 HTS221_REG_CTRL1, &cfg, 1);
197
198 lsm6dsv16x_shub_enable(dev, 1);
199 return 0;
200 }
201
lsm6dsv16x_hts221_conf(const struct device * dev,uint8_t i2c_addr,enum sensor_channel chan,enum sensor_attribute attr,const struct sensor_value * val)202 static int lsm6dsv16x_hts221_conf(const struct device *dev, uint8_t i2c_addr,
203 enum sensor_channel chan,
204 enum sensor_attribute attr,
205 const struct sensor_value *val)
206 {
207 switch (attr) {
208 case SENSOR_ATTR_SAMPLING_FREQUENCY:
209 return lsm6dsv16x_hts221_odr_set(dev, i2c_addr, val->val1);
210 default:
211 LOG_DBG("shub: HTS221 attribute not supported.");
212 return -ENOTSUP;
213 }
214
215 return 0;
216 }
217 #endif /* CONFIG_LSM6DSV16X_EXT_HTS221 */
218
219 /*
220 * LPS22HB baro/temp device specific part
221 */
222 #ifdef CONFIG_LSM6DSV16X_EXT_LPS22HB
223
224 #define LPS22HB_CTRL_REG1 0x10
225 #define LPS22HB_CTRL_REG2 0x11
226
227 #define LPS22HB_SW_RESET 0x04
228 #define LPS22HB_ODR_10HZ 0x20
229 #define LPS22HB_LPF_EN 0x08
230 #define LPS22HB_BDU_EN 0x02
231
lsm6dsv16x_lps22hb_init(const struct device * dev,uint8_t i2c_addr)232 static int lsm6dsv16x_lps22hb_init(const struct device *dev, uint8_t i2c_addr)
233 {
234 uint8_t baro_cfg[2];
235
236 /* sw reset device */
237 baro_cfg[0] = LPS22HB_SW_RESET;
238 lsm6dsv16x_shub_write_target_reg(dev, i2c_addr,
239 LPS22HB_CTRL_REG2, baro_cfg, 1);
240
241 k_sleep(K_MSEC(1)); /* turn-on time in ms */
242
243 /* configure device */
244 baro_cfg[0] = LPS22HB_ODR_10HZ | LPS22HB_LPF_EN | LPS22HB_BDU_EN;
245 lsm6dsv16x_shub_write_target_reg(dev, i2c_addr,
246 LPS22HB_CTRL_REG1, baro_cfg, 1);
247
248 return 0;
249 }
250 #endif /* CONFIG_LSM6DSV16X_EXT_LPS22HB */
251
252 /*
253 * LPS22HH baro/temp device specific part
254 */
255 #ifdef CONFIG_LSM6DSV16X_EXT_LPS22HH
256
257 #define LPS22HH_CTRL_REG1 0x10
258 #define LPS22HH_CTRL_REG2 0x11
259
260 #define LPS22HH_SW_RESET 0x04
261 #define LPS22HH_IF_ADD_INC 0x10
262 #define LPS22HH_ODR_10HZ 0x20
263 #define LPS22HH_LPF_EN 0x08
264 #define LPS22HH_BDU_EN 0x02
265
lsm6dsv16x_lps22hh_init(const struct device * dev,uint8_t i2c_addr)266 static int lsm6dsv16x_lps22hh_init(const struct device *dev, uint8_t i2c_addr)
267 {
268 uint8_t baro_cfg[2];
269
270 /* sw reset device */
271 baro_cfg[0] = LPS22HH_SW_RESET;
272 lsm6dsv16x_shub_write_target_reg(dev, i2c_addr,
273 LPS22HH_CTRL_REG2, baro_cfg, 1);
274
275 k_sleep(K_MSEC(100)); /* turn-on time in ms */
276
277 /* configure device */
278 baro_cfg[0] = LPS22HH_IF_ADD_INC;
279 lsm6dsv16x_shub_write_target_reg(dev, i2c_addr,
280 LPS22HH_CTRL_REG2, baro_cfg, 1);
281
282 baro_cfg[0] = LPS22HH_ODR_10HZ | LPS22HH_LPF_EN | LPS22HH_BDU_EN;
283 lsm6dsv16x_shub_write_target_reg(dev, i2c_addr,
284 LPS22HH_CTRL_REG1, baro_cfg, 1);
285
286 return 0;
287 }
288
289 static const uint16_t lps22hh_map[] = {0, 1, 10, 25, 50, 75, 100, 200};
290
lsm6dsv16x_lps22hh_odr_set(const struct device * dev,uint8_t i2c_addr,uint16_t freq)291 static int lsm6dsv16x_lps22hh_odr_set(const struct device *dev,
292 uint8_t i2c_addr, uint16_t freq)
293 {
294 uint8_t odr, cfg;
295
296 for (odr = 0; odr < ARRAY_SIZE(lps22hh_map); odr++) {
297 if (freq <= lps22hh_map[odr]) {
298 break;
299 }
300 }
301
302 if (odr == ARRAY_SIZE(lps22hh_map)) {
303 LOG_DBG("shub: LPS22HH freq val %d not supported.", freq);
304 return -ENOTSUP;
305 }
306
307 cfg = (odr << 4) | LPS22HH_LPF_EN | LPS22HH_BDU_EN;
308 lsm6dsv16x_shub_write_target_reg(dev, i2c_addr,
309 LPS22HH_CTRL_REG1, &cfg, 1);
310
311 lsm6dsv16x_shub_enable(dev, 1);
312 return 0;
313 }
314
lsm6dsv16x_lps22hh_conf(const struct device * dev,uint8_t i2c_addr,enum sensor_channel chan,enum sensor_attribute attr,const struct sensor_value * val)315 static int lsm6dsv16x_lps22hh_conf(const struct device *dev, uint8_t i2c_addr,
316 enum sensor_channel chan,
317 enum sensor_attribute attr,
318 const struct sensor_value *val)
319 {
320 switch (attr) {
321 case SENSOR_ATTR_SAMPLING_FREQUENCY:
322 return lsm6dsv16x_lps22hh_odr_set(dev, i2c_addr, val->val1);
323 default:
324 LOG_DBG("shub: LPS22HH attribute not supported.");
325 return -ENOTSUP;
326 }
327
328 return 0;
329 }
330 #endif /* CONFIG_LSM6DSV16X_EXT_LPS22HH */
331
332 /*
333 * LPS22DF baro/temp device specific part
334 */
335 #ifdef CONFIG_LSM6DSV16X_EXT_LPS22DF
336
337 #define LPS22DF_CTRL_REG1 0x10
338 #define LPS22DF_CTRL_REG2 0x11
339
340 #define LPS22DF_SW_RESET 0x04
341 #define LPS22DF_BDU_EN 0x08
342 #define LPS22DF_EN_LPFP 0x10
343 #define LPS22DF_ODR_10HZ 0x18
344 #define LPS22DF_AVG_16 0x02
345
lsm6dsv16x_lps22df_init(const struct device * dev,uint8_t i2c_addr)346 static int lsm6dsv16x_lps22df_init(const struct device *dev, uint8_t i2c_addr)
347 {
348 uint8_t baro_cfg[2];
349
350 /* sw reset device */
351 baro_cfg[0] = LPS22DF_SW_RESET;
352 lsm6dsv16x_shub_write_target_reg(dev, i2c_addr,
353 LPS22DF_CTRL_REG2, baro_cfg, 1);
354
355 k_busy_wait(50); /* turn-on time in us */
356
357 /* configure device */
358 baro_cfg[0] = LPS22DF_BDU_EN | LPS22DF_EN_LPFP;
359 lsm6dsv16x_shub_write_target_reg(dev, i2c_addr,
360 LPS22DF_CTRL_REG2, baro_cfg, 1);
361
362 baro_cfg[0] = LPS22DF_ODR_10HZ | LPS22DF_AVG_16;
363 lsm6dsv16x_shub_write_target_reg(dev, i2c_addr,
364 LPS22DF_CTRL_REG1, baro_cfg, 1);
365
366 return 0;
367 }
368
369 static const uint16_t lps22df_map[] = {0, 1, 4, 10, 25, 50, 75, 100, 200};
370
lsm6dsv16x_lps22df_odr_set(const struct device * dev,uint8_t i2c_addr,uint16_t freq)371 static int lsm6dsv16x_lps22df_odr_set(const struct device *dev,
372 uint8_t i2c_addr, uint16_t freq)
373 {
374 uint8_t odr, cfg;
375
376 for (odr = 0; odr < ARRAY_SIZE(lps22df_map); odr++) {
377 if (freq <= lps22df_map[odr]) {
378 break;
379 }
380 }
381
382 if (odr == ARRAY_SIZE(lps22df_map)) {
383 LOG_DBG("shub: LPS22DF freq val %d not supported.", freq);
384 return -ENOTSUP;
385 }
386
387 cfg = (odr << 3) | LPS22DF_AVG_16;
388 lsm6dsv16x_shub_write_target_reg(dev, i2c_addr,
389 LPS22DF_CTRL_REG1, &cfg, 1);
390
391 lsm6dsv16x_shub_enable(dev, 1);
392 return 0;
393 }
394
lsm6dsv16x_lps22df_conf(const struct device * dev,uint8_t i2c_addr,enum sensor_channel chan,enum sensor_attribute attr,const struct sensor_value * val)395 static int lsm6dsv16x_lps22df_conf(const struct device *dev, uint8_t i2c_addr,
396 enum sensor_channel chan,
397 enum sensor_attribute attr,
398 const struct sensor_value *val)
399 {
400 switch (attr) {
401 case SENSOR_ATTR_SAMPLING_FREQUENCY:
402 return lsm6dsv16x_lps22df_odr_set(dev, i2c_addr, val->val1);
403 default:
404 LOG_DBG("shub: LPS22DF attribute not supported.");
405 return -ENOTSUP;
406 }
407
408 return 0;
409 }
410 #endif /* CONFIG_LSM6DSV16X_EXT_LPS22DF */
411
412 /* List of supported external sensors */
413 static struct lsm6dsv16x_shub_slist {
414 enum sensor_channel type;
415 uint8_t i2c_addr[2];
416 uint8_t ext_i2c_addr;
417 uint8_t wai_addr;
418 uint8_t wai_val;
419 uint8_t out_data_addr;
420 uint8_t out_data_len;
421 uint8_t sh_out_reg;
422 int (*dev_init)(const struct device *dev, uint8_t i2c_addr);
423 int (*dev_conf)(const struct device *dev, uint8_t i2c_addr,
424 enum sensor_channel chan, enum sensor_attribute attr,
425 const struct sensor_value *val);
426 } lsm6dsv16x_shub_slist[] = {
427 #ifdef CONFIG_LSM6DSV16X_EXT_LIS2MDL
428 {
429 /* LIS2MDL */
430 .type = SENSOR_CHAN_MAGN_XYZ,
431 .i2c_addr = { 0x1E },
432 .wai_addr = 0x4F,
433 .wai_val = 0x40,
434 .out_data_addr = 0x68,
435 .out_data_len = 0x06,
436 .dev_init = (lsm6dsv16x_lis2mdl_init),
437 .dev_conf = (lsm6dsv16x_lis2mdl_conf),
438 },
439 #endif /* CONFIG_LSM6DSV16X_EXT_LIS2MDL */
440
441 #ifdef CONFIG_LSM6DSV16X_EXT_HTS221
442 {
443 /* HTS221 */
444 .type = SENSOR_CHAN_HUMIDITY,
445 .i2c_addr = { 0x5F },
446 .wai_addr = 0x0F,
447 .wai_val = 0xBC,
448 .out_data_addr = 0x28 | HTS221_AUTOINCREMENT,
449 .out_data_len = 0x02,
450 .dev_init = (lsm6dsv16x_hts221_init),
451 .dev_conf = (lsm6dsv16x_hts221_conf),
452 },
453 #endif /* CONFIG_LSM6DSV16X_EXT_HTS221 */
454
455 #ifdef CONFIG_LSM6DSV16X_EXT_LPS22HB
456 {
457 /* LPS22HB */
458 .type = SENSOR_CHAN_PRESS,
459 .i2c_addr = { 0x5C, 0x5D },
460 .wai_addr = 0x0F,
461 .wai_val = 0xB1,
462 .out_data_addr = 0x28,
463 .out_data_len = 0x05,
464 .dev_init = (lsm6dsv16x_lps22hb_init),
465 },
466 #endif /* CONFIG_LSM6DSV16X_EXT_LPS22HB */
467
468 #ifdef CONFIG_LSM6DSV16X_EXT_LPS22HH
469 {
470 /* LPS22HH */
471 .type = SENSOR_CHAN_PRESS,
472 .i2c_addr = { 0x5C, 0x5D },
473 .wai_addr = 0x0F,
474 .wai_val = 0xB3,
475 .out_data_addr = 0x28,
476 .out_data_len = 0x05,
477 .dev_init = (lsm6dsv16x_lps22hh_init),
478 .dev_conf = (lsm6dsv16x_lps22hh_conf),
479 },
480 #endif /* CONFIG_LSM6DSV16X_EXT_LPS22HH */
481
482 #ifdef CONFIG_LSM6DSV16X_EXT_LPS22DF
483 {
484 /* LPS22DF */
485 .type = SENSOR_CHAN_PRESS,
486 .i2c_addr = { 0x5C, 0x5D },
487 .wai_addr = 0x0F,
488 .wai_val = 0xB4,
489 .out_data_addr = 0x28,
490 .out_data_len = 0x05,
491 .dev_init = (lsm6dsv16x_lps22df_init),
492 .dev_conf = (lsm6dsv16x_lps22df_conf),
493 },
494 #endif /* CONFIG_LSM6DSV16X_EXT_LPS22DF */
495 };
496
lsm6dsv16x_shub_wait_completed(stmdev_ctx_t * ctx)497 static int lsm6dsv16x_shub_wait_completed(stmdev_ctx_t *ctx)
498 {
499 lsm6dsv16x_status_master_t status;
500 int tries = 200; /* Should be max ~160 ms, from 2 cycles at slowest ODR 12.5 Hz */
501
502 do {
503 if (!--tries) {
504 LOG_DBG("shub: Timeout waiting for operation to complete");
505 return -ETIMEDOUT;
506 }
507 k_msleep(1);
508 lsm6dsv16x_sh_status_mainpage_get(ctx, &status);
509 } while (status.sens_hub_endop == 0);
510
511 return 1;
512 }
513
lsm6dsv16x_shub_enable(const struct device * dev,uint8_t enable)514 static void lsm6dsv16x_shub_enable(const struct device *dev, uint8_t enable)
515 {
516 const struct lsm6dsv16x_config *cfg = dev->config;
517 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
518 struct lsm6dsv16x_data *data = dev->data;
519
520 /* Enable Accel @26hz */
521 if (!data->accel_freq) {
522 uint8_t odr = (enable) ? 2 : 0;
523
524 if (lsm6dsv16x_xl_data_rate_set(ctx, odr) < 0) {
525 LOG_DBG("shub: failed to set XL sampling rate");
526 return;
527 }
528 }
529
530 if (enable) {
531 lsm6dsv16x_status_master_t status;
532
533 /* Clear any pending status flags */
534 lsm6dsv16x_sh_status_mainpage_get(ctx, &status);
535 }
536
537 if (lsm6dsv16x_sh_master_set(ctx, enable) < 0) {
538 LOG_DBG("shub: failed to set master on");
539 lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK);
540 return;
541 }
542
543 if (!enable) {
544 /* wait 300us (necessary per AN5763 §7.2.1) */
545 k_busy_wait(300);
546 }
547 }
548
549 /* must be called with master on */
lsm6dsv16x_shub_check_slv0_nack(stmdev_ctx_t * ctx)550 static int lsm6dsv16x_shub_check_slv0_nack(stmdev_ctx_t *ctx)
551 {
552 lsm6dsv16x_all_sources_t status;
553
554 if (lsm6dsv16x_all_sources_get(ctx, &status) < 0) {
555 LOG_DBG("shub: error reading embedded reg");
556 return -EIO;
557 }
558
559 if (status.sh_slave0_nack) {
560 LOG_DBG("shub: TRGT 0 nacked");
561 return -EIO;
562 }
563
564 return 0;
565 }
566
567 /*
568 * use TRGT 0 for generic read to target device
569 */
lsm6dsv16x_shub_read_target_reg(const struct device * dev,uint8_t trgt_addr,uint8_t trgt_reg,uint8_t * value,uint16_t len)570 static int lsm6dsv16x_shub_read_target_reg(const struct device *dev,
571 uint8_t trgt_addr, uint8_t trgt_reg,
572 uint8_t *value, uint16_t len)
573 {
574 const struct lsm6dsv16x_config *cfg = dev->config;
575 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
576 lsm6dsv16x_sh_cfg_read_t trgt_cfg;
577
578 trgt_cfg.slv_add = trgt_addr;
579 trgt_cfg.slv_subadd = trgt_reg;
580 trgt_cfg.slv_len = len;
581
582 lsm6dsv16x_sh_slv_cfg_read(ctx, 0, &trgt_cfg);
583
584 /* turn SH on, wait for shub i2c read to finish */
585 lsm6dsv16x_shub_enable(dev, 1);
586 lsm6dsv16x_shub_wait_completed(ctx);
587
588 /* read data from external target */
589 if (lsm6dsv16x_sh_read_data_raw_get(ctx, value, len) < 0) {
590 LOG_DBG("shub: error reading sensor data");
591 return -EIO;
592 }
593
594 if (lsm6dsv16x_shub_check_slv0_nack(ctx) < 0) {
595 lsm6dsv16x_shub_enable(dev, 0);
596 return -EIO;
597 }
598
599 lsm6dsv16x_shub_enable(dev, 0);
600 return 0;
601 }
602
603 /*
604 * use TRGT 0 to configure target device
605 */
lsm6dsv16x_shub_write_target_reg(const struct device * dev,uint8_t trgt_addr,uint8_t trgt_reg,uint8_t * value,uint16_t len)606 static int lsm6dsv16x_shub_write_target_reg(const struct device *dev,
607 uint8_t trgt_addr, uint8_t trgt_reg,
608 uint8_t *value, uint16_t len)
609 {
610 const struct lsm6dsv16x_config *cfg = dev->config;
611 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
612 lsm6dsv16x_sh_cfg_write_t trgt_cfg;
613 uint8_t cnt = 0U;
614
615 lsm6dsv16x_shub_enable(dev, 0);
616
617 while (cnt < len) {
618 trgt_cfg.slv0_add = trgt_addr;
619 trgt_cfg.slv0_subadd = trgt_reg + cnt;
620 trgt_cfg.slv0_data = value[cnt];
621
622 lsm6dsv16x_sh_cfg_write(ctx, &trgt_cfg);
623
624 /* turn SH on, wait for shub i2c write to finish */
625 lsm6dsv16x_shub_enable(dev, 1);
626 lsm6dsv16x_shub_wait_completed(ctx);
627
628 if (lsm6dsv16x_shub_check_slv0_nack(ctx) < 0) {
629 lsm6dsv16x_shub_enable(dev, 0);
630 return -EIO;
631 }
632
633 lsm6dsv16x_shub_enable(dev, 0);
634
635 cnt++;
636 }
637
638 /* Put TRGT 0 in IDLE mode */
639 trgt_cfg.slv0_add = 0x7;
640 trgt_cfg.slv0_subadd = 0x0;
641 trgt_cfg.slv0_data = 0x0;
642 lsm6dsv16x_sh_cfg_write(ctx, &trgt_cfg);
643
644 return 0;
645 }
646
647 /*
648 * TARGETs configurations:
649 *
650 * - TARGET 0: used for configuring all target devices
651 * - TARGET 1: used as data read channel for external target device #1
652 * - TARGET 2: used as data read channel for external target device #2
653 * - TARGET 3: used for generic reads while data channel is enabled
654 */
lsm6dsv16x_shub_set_data_channel(const struct device * dev)655 static int lsm6dsv16x_shub_set_data_channel(const struct device *dev)
656 {
657 struct lsm6dsv16x_data *data = dev->data;
658 const struct lsm6dsv16x_config *cfg = dev->config;
659 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
660 uint8_t n;
661 struct lsm6dsv16x_shub_slist *sp;
662 lsm6dsv16x_sh_cfg_read_t trgt_cfg;
663
664 /* Configure shub data channels to access external targets */
665 for (n = 0; n < data->num_ext_dev; n++) {
666 sp = &lsm6dsv16x_shub_slist[data->shub_ext[n]];
667
668 trgt_cfg.slv_add = sp->ext_i2c_addr;
669 trgt_cfg.slv_subadd = sp->out_data_addr;
670 trgt_cfg.slv_len = sp->out_data_len;
671
672 if (lsm6dsv16x_sh_slv_cfg_read(ctx, n + 1, &trgt_cfg) < 0) {
673 LOG_DBG("shub: error configuring shub for ext targets");
674 return -EIO;
675 }
676 }
677
678 /* Configure the master */
679 lsm6dsv16x_sh_slave_connected_t aux = LSM6DSV16X_SLV_0_1_2;
680
681 if (lsm6dsv16x_sh_slave_connected_set(ctx, aux) < 0) {
682 LOG_DBG("shub: error setting aux sensors");
683 return -EIO;
684 }
685
686
687 /* turn SH on, no need to wait for 1st shub i2c read, if any, to complete */
688 lsm6dsv16x_shub_enable(dev, 1);
689
690 return 0;
691 }
692
lsm6dsv16x_shub_get_idx(const struct device * dev,enum sensor_channel type)693 int lsm6dsv16x_shub_get_idx(const struct device *dev, enum sensor_channel type)
694 {
695 uint8_t n;
696 struct lsm6dsv16x_data *data = dev->data;
697 struct lsm6dsv16x_shub_slist *sp;
698
699 for (n = 0; n < data->num_ext_dev; n++) {
700 sp = &lsm6dsv16x_shub_slist[data->shub_ext[n]];
701
702 if (sp->type == type) {
703 return n;
704 }
705 }
706
707 LOG_ERR("shub: dev %s type %d not supported", dev->name, type);
708 return -ENOTSUP;
709 }
710
lsm6dsv16x_shub_fetch_external_devs(const struct device * dev)711 int lsm6dsv16x_shub_fetch_external_devs(const struct device *dev)
712 {
713 uint8_t n;
714 const struct lsm6dsv16x_config *cfg = dev->config;
715 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
716 struct lsm6dsv16x_data *data = dev->data;
717 struct lsm6dsv16x_shub_slist *sp;
718
719 /* read data from external target */
720 if (lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK) < 0) {
721 LOG_DBG("failed to enter SENSOR_HUB bank");
722 return -EIO;
723 }
724
725 for (n = 0; n < data->num_ext_dev; n++) {
726 sp = &lsm6dsv16x_shub_slist[data->shub_ext[n]];
727
728 if (lsm6dsv16x_read_reg(ctx, sp->sh_out_reg,
729 data->ext_data[n], sp->out_data_len) < 0) {
730 LOG_DBG("shub: failed to read sample");
731 (void) lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK);
732 return -EIO;
733 }
734 }
735
736 return lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK);
737 }
738
lsm6dsv16x_shub_config(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,const struct sensor_value * val)739 int lsm6dsv16x_shub_config(const struct device *dev, enum sensor_channel chan,
740 enum sensor_attribute attr,
741 const struct sensor_value *val)
742 {
743 struct lsm6dsv16x_data *data = dev->data;
744 struct lsm6dsv16x_shub_slist *sp = NULL;
745 uint8_t n;
746
747 for (n = 0; n < data->num_ext_dev; n++) {
748 sp = &lsm6dsv16x_shub_slist[data->shub_ext[n]];
749
750 if (sp->type == chan) {
751 break;
752 }
753 }
754
755 if (n == data->num_ext_dev) {
756 LOG_DBG("shub: %s chan %d not supported", dev->name, chan);
757 return -ENOTSUP;
758 }
759
760 if (sp == NULL || sp->dev_conf == NULL) {
761 LOG_DBG("shub: chan not configurable");
762 return -ENOTSUP;
763 }
764
765 return sp->dev_conf(dev, sp->ext_i2c_addr, chan, attr, val);
766 }
767
lsm6dsv16x_shub_init(const struct device * dev)768 int lsm6dsv16x_shub_init(const struct device *dev)
769 {
770 struct lsm6dsv16x_data *data = dev->data;
771 const struct lsm6dsv16x_config *cfg = dev->config;
772 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
773 uint8_t i, n = 0, regn;
774 uint8_t chip_id;
775 struct lsm6dsv16x_shub_slist *sp;
776
777 LOG_INF("shub: start sensorhub for %s", dev->name);
778
779 /*
780 * This must be set or lsm6dsv16x_shub_write_target_reg() will
781 * repeatedly write the same regi
782 */
783 if (lsm6dsv16x_sh_write_mode_set(ctx, LSM6DSV16X_ONLY_FIRST_CYCLE) < 0) {
784 LOG_DBG("shub: error setting write once");
785 return -EIO;
786 }
787
788 for (n = 0; n < ARRAY_SIZE(lsm6dsv16x_shub_slist); n++) {
789 if (data->num_ext_dev >= LSM6DSV16X_SHUB_MAX_NUM_TARGETS) {
790 break;
791 }
792
793 chip_id = 0;
794 sp = &lsm6dsv16x_shub_slist[n];
795
796 /*
797 * The external sensor may have different I2C address.
798 * So, try them one by one until we read the correct
799 * chip ID.
800 */
801 for (i = 0U; i < ARRAY_SIZE(sp->i2c_addr); i++) {
802 if (lsm6dsv16x_shub_read_target_reg(dev,
803 sp->i2c_addr[i],
804 sp->wai_addr,
805 &chip_id, 1) < 0) {
806 LOG_DBG("shub: failed reading chip id");
807 continue;
808 }
809 if (chip_id == sp->wai_val) {
810 break;
811 }
812 }
813
814 if (i >= ARRAY_SIZE(sp->i2c_addr)) {
815 LOG_DBG("shub: invalid chip id 0x%x", chip_id);
816 continue;
817 }
818 LOG_INF("shub: Ext Device Chip Id: %02x", chip_id);
819 sp->ext_i2c_addr = sp->i2c_addr[i];
820
821 data->shub_ext[data->num_ext_dev++] = n;
822 }
823
824 LOG_DBG("shub: dev %s - num_ext_dev %d", dev->name, data->num_ext_dev);
825 if (data->num_ext_dev == 0) {
826 LOG_ERR("shub: no target devices found");
827 return -EINVAL;
828 }
829
830 /* init external devices */
831 for (n = 0, regn = 0; n < data->num_ext_dev; n++) {
832 sp = &lsm6dsv16x_shub_slist[data->shub_ext[n]];
833 sp->sh_out_reg = LSM6DSV16X_SENSOR_HUB_1 + regn;
834 regn += sp->out_data_len;
835 sp->dev_init(dev, sp->ext_i2c_addr);
836 }
837
838 lsm6dsv16x_shub_set_data_channel(dev);
839
840 return 0;
841 }
842
