1 /*
2 * Copyright (c) 2018 STMicroelectronics
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/device.h>
8 #include <zephyr/drivers/i2c.h>
9 #include <zephyr/sys/__assert.h>
10 #include <zephyr/sys/util.h>
11 #include <zephyr/kernel.h>
12 #include <zephyr/drivers/sensor.h>
13 #include <zephyr/logging/log.h>
14
15 #include "lsm6dsl.h"
16
17 LOG_MODULE_DECLARE(LSM6DSL, CONFIG_SENSOR_LOG_LEVEL);
18
19 #define LSM6DSL_EMBEDDED_SLV0_ADDR 0x02
20 #define LSM6DSL_EMBEDDED_SLV0_SUBADDR 0x03
21 #define LSM6DSL_EMBEDDED_SLV0_CONFIG 0x04
22 #define LSM6DSL_EMBEDDED_SLV1_ADDR 0x05
23 #define LSM6DSL_EMBEDDED_SLV1_SUBADDR 0x06
24 #define LSM6DSL_EMBEDDED_SLV1_CONFIG 0x07
25 #define LSM6DSL_EMBEDDED_SLV2_ADDR 0x08
26 #define LSM6DSL_EMBEDDED_SLV2_SUBADDR 0x09
27 #define LSM6DSL_EMBEDDED_SLV2_CONFIG 0x0A
28 #define LSM6DSL_EMBEDDED_SLV3_ADDR 0x0B
29 #define LSM6DSL_EMBEDDED_SLV3_SUBADDR 0x0C
30 #define LSM6DSL_EMBEDDED_SLV3_CONFIG 0x0D
31 #define LSM6DSL_EMBEDDED_SLV0_DATAWRITE 0x0E
32
33 #define LSM6DSL_EMBEDDED_SLVX_READ 0x1
34 #define LSM6DSL_EMBEDDED_SLVX_THREE_SENS 0x20
35 #define LSM6DSL_EMBEDDED_SLV0_WRITE_IDLE 0x07
36
37 static int lsm6dsl_shub_write_slave_reg(const struct device *dev,
38 uint8_t slv_addr, uint8_t slv_reg,
39 uint8_t *value, uint16_t len);
40
41 /*
42 * LIS2MDL magn device specific part
43 */
44 #ifdef CONFIG_LSM6DSL_EXT0_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_OFF_CANC 0x02
54 #define LIS2MDL_SENSITIVITY 1500
55
lsm6dsl_lis2mdl_init(const struct device * dev,uint8_t i2c_addr)56 static int lsm6dsl_lis2mdl_init(const struct device *dev, uint8_t i2c_addr)
57 {
58 struct lsm6dsl_data *data = dev->data;
59 uint8_t mag_cfg[2];
60
61 data->magn_sensitivity = LIS2MDL_SENSITIVITY;
62
63 /* sw reset device */
64 mag_cfg[0] = LIS2MDL_SW_RESET;
65 lsm6dsl_shub_write_slave_reg(dev, i2c_addr,
66 LIS2MDL_CFG_REG_A, mag_cfg, 1);
67
68 k_sleep(K_MSEC(10)); /* turn-on time in ms */
69
70 /* configure mag */
71 mag_cfg[0] = LIS2MDL_ODR_10HZ;
72 mag_cfg[1] = LIS2MDL_OFF_CANC;
73 lsm6dsl_shub_write_slave_reg(dev, i2c_addr,
74 LIS2MDL_CFG_REG_A, mag_cfg, 2);
75
76 return 0;
77 }
78 #endif /* CONFIG_LSM6DSL_EXT0_LIS2MDL */
79
80 /*
81 * LIS3MDL magn device specific part
82 */
83 #ifdef CONFIG_LSM6DSL_EXT0_LIS3MDL
84
85 #define LIS3MDL_REG_CTRL1 0x20
86 #define LIS3MDL_REG_CTRL2 0x21
87 #define LIS3MDL_REG_CTRL3 0x22
88 #define LIS3MDL_REG_CTRL4 0x23
89 #define LIS3MDL_REG_CTRL5 0x24
90
91 #define LIS3MDL_REG_SAMPLE_START 0x28
92
93 #define LIS3MDL_REG_INT_CFG 0x30
94 #define LIS3MDL_INT_X_EN BIT(7)
95 #define LIS3MDL_INT_Y_EN BIT(6)
96 #define LIS3MDL_INT_Z_EN BIT(5)
97 #define LIS3MDL_INT_XYZ_EN \
98 (LIS3MDL_INT_X_EN | LIS3MDL_INT_Y_EN | LIS3MDL_INT_Z_EN)
99
100 #define LIS3MDL_STATUS_REG 0x27
101
102 /* REG_CTRL2 */
103 #define LIS3MDL_REBOOT_MASK BIT(3)
104 #define LIS3MDL_SOFT_RST_MASK BIT(2)
105
106 /* REG_CTRL1 */
107 #define LIS3MDL_OM_SHIFT 5
108 #define LIS3MDL_DO_SHIFT 2
109 #define LIS3MDL_FAST_ODR_SHIFT 1
110 #define LIS3MDL_ODR_BITS(om_bits, do_bits, fast_odr) \
111 (((om_bits) << LIS3MDL_OM_SHIFT) | \
112 ((do_bits) << LIS3MDL_DO_SHIFT) | \
113 ((fast_odr) << LIS3MDL_FAST_ODR_SHIFT))
114 static const uint8_t lis3mdl_odr_bits[] = {
115 LIS3MDL_ODR_BITS(0, 0, 0), /* 0.625 Hz */
116 LIS3MDL_ODR_BITS(0, 1, 0), /* 1.25 Hz */
117 LIS3MDL_ODR_BITS(0, 2, 0), /* 2.5 Hz */
118 LIS3MDL_ODR_BITS(0, 3, 0), /* 5 Hz */
119 LIS3MDL_ODR_BITS(0, 4, 0), /* 10 Hz */
120 LIS3MDL_ODR_BITS(0, 5, 0), /* 20 Hz */
121 LIS3MDL_ODR_BITS(0, 6, 0), /* 40 Hz */
122 LIS3MDL_ODR_BITS(0, 7, 0), /* 80 Hz */
123 LIS3MDL_ODR_BITS(3, 0, 1), /* 155 Hz */
124 LIS3MDL_ODR_BITS(2, 0, 1), /* 300 Hz */
125 LIS3MDL_ODR_BITS(1, 0, 1), /* 560 Hz */
126 LIS3MDL_ODR_BITS(0, 0, 1) /* 1000 Hz */
127 };
128 #define LIS3MDL_ODR lis3mdl_odr_bits[4]
129
130 /* REG_CTRL3 */
131 #define LIS3MDL_MD_CONTINUOUS 0x00
132
133 /* Others */
134 #define LIS3MDL_SENSITIVITY 6842
135
lsm6dsl_lis3mdl_init(const struct device * dev,uint8_t i2c_addr)136 static int lsm6dsl_lis3mdl_init(const struct device *dev, uint8_t i2c_addr)
137 {
138 struct lsm6dsl_data *data = dev->data;
139 uint8_t mag_cfg[2];
140
141 data->magn_sensitivity = LIS3MDL_SENSITIVITY;
142
143 /* sw reset device */
144 mag_cfg[0] = LIS3MDL_REBOOT_MASK | LIS3MDL_SOFT_RST_MASK;
145 lsm6dsl_shub_write_slave_reg(dev, i2c_addr,
146 LIS3MDL_REG_CTRL2, mag_cfg, 1);
147
148 k_sleep(K_MSEC(10)); /* turn-on time in ms */
149
150 /* configure mag */
151 mag_cfg[0] = LIS3MDL_ODR;
152 lsm6dsl_shub_write_slave_reg(dev, i2c_addr,
153 LIS3MDL_REG_CTRL1, mag_cfg, 1);
154
155 mag_cfg[0] = LIS3MDL_MD_CONTINUOUS;
156 lsm6dsl_shub_write_slave_reg(dev, i2c_addr,
157 LIS3MDL_REG_CTRL3, mag_cfg, 1);
158 return 0;
159 }
160 #endif /* CONFIG_LSM6DSL_EXT0_LIS3MDL */
161
162 /*
163 * LPS22HB baro/temp device specific part
164 */
165 #ifdef CONFIG_LSM6DSL_EXT0_LPS22HB
166
167 #define LPS22HB_CTRL_REG1 0x10
168 #define LPS22HB_CTRL_REG2 0x11
169
170 #define LPS22HB_SW_RESET 0x04
171 #define LPS22HB_ODR_10HZ 0x20
172 #define LPS22HB_LPF_EN 0x08
173 #define LPS22HB_BDU_EN 0x02
174
lsm6dsl_lps22hb_init(const struct device * dev,uint8_t i2c_addr)175 static int lsm6dsl_lps22hb_init(const struct device *dev, uint8_t i2c_addr)
176 {
177 uint8_t baro_cfg[2];
178
179 /* sw reset device */
180 baro_cfg[0] = LPS22HB_SW_RESET;
181 lsm6dsl_shub_write_slave_reg(dev, i2c_addr,
182 LPS22HB_CTRL_REG2, baro_cfg, 1);
183
184 k_sleep(K_MSEC(1)); /* turn-on time in ms */
185
186 /* configure device */
187 baro_cfg[0] = LPS22HB_ODR_10HZ | LPS22HB_LPF_EN | LPS22HB_BDU_EN;
188 lsm6dsl_shub_write_slave_reg(dev, i2c_addr,
189 LPS22HB_CTRL_REG1, baro_cfg, 1);
190
191 return 0;
192 }
193 #endif /* CONFIG_LSM6DSL_EXT0_LPS22HB */
194
195 /* List of supported external sensors */
196 static struct lsm6dsl_shub_sens_list {
197 uint8_t i2c_addr[2];
198 uint8_t wai_addr;
199 uint8_t wai_val;
200 uint8_t out_data_addr;
201 uint8_t out_data_len;
202 int (*dev_init)(const struct device *dev, uint8_t i2c_addr);
203 } lsm6dsl_shub_sens_list[] = {
204 #ifdef CONFIG_LSM6DSL_EXT0_LIS2MDL
205 {
206 /* LIS2MDL */
207 .i2c_addr = { 0x1E },
208 .wai_addr = 0x4F,
209 .wai_val = 0x40,
210 .out_data_addr = 0x68,
211 .out_data_len = 0x06,
212 .dev_init = (lsm6dsl_lis2mdl_init),
213 },
214 #endif /* CONFIG_LSM6DSL_EXT0_LIS2MDL */
215
216 #ifdef CONFIG_LSM6DSL_EXT0_LIS3MDL
217 {
218 /* LIS3MDL */
219 .i2c_addr = {0x1C, 0x1E},
220 .wai_addr = 0x0F,
221 .wai_val = 0x3D,
222 .out_data_addr = 0x28,
223 .out_data_len = 0x06,
224 .dev_init = (lsm6dsl_lis3mdl_init),
225 },
226 #endif /* CONFIG_LSM6DSL_EXT0_LIS3MDL */
227
228 #ifdef CONFIG_LSM6DSL_EXT0_LPS22HB
229 {
230 /* LPS22HB */
231 .i2c_addr = { 0x5C, 0x5D },
232 .wai_addr = 0x0F,
233 .wai_val = 0xB1,
234 .out_data_addr = 0x28,
235 .out_data_len = 0x05,
236 .dev_init = (lsm6dsl_lps22hb_init),
237 },
238 #endif /* CONFIG_LSM6DSL_EXT0_LPS22HB */
239 };
240
241 static uint8_t ext_i2c_addr;
242
lsm6dsl_shub_wait_completed(const struct device * dev)243 static inline void lsm6dsl_shub_wait_completed(const struct device *dev)
244 {
245 struct lsm6dsl_data *data = dev->data;
246 uint16_t freq;
247
248 freq = (data->accel_freq == 0U) ? 26 : data->accel_freq;
249 k_msleep((2000U / freq) + 1);
250 }
251
lsm6dsl_shub_embedded_en(const struct device * dev,bool on)252 static inline void lsm6dsl_shub_embedded_en(const struct device *dev, bool on)
253 {
254 struct lsm6dsl_data *data = dev->data;
255 uint8_t func_en = (on) ? 0x1 : 0x0;
256
257 data->hw_tf->update_reg(dev, LSM6DSL_REG_FUNC_CFG_ACCESS,
258 LSM6DSL_MASK_FUNC_CFG_EN,
259 func_en << LSM6DSL_SHIFT_FUNC_CFG_EN);
260
261 k_sleep(K_MSEC(1));
262 }
263
264 #ifdef LSM6DSL_DEBUG
lsm6dsl_read_embedded_reg(const struct device * dev,uint8_t reg_addr,uint8_t * value,int len)265 static int lsm6dsl_read_embedded_reg(const struct device *dev,
266 uint8_t reg_addr, uint8_t *value, int len)
267 {
268 struct lsm6dsl_data *data = dev->data;
269 lsm6dsl_shub_embedded_en(dev, true);
270
271 if (data->hw_tf->read_data(dev, reg_addr, value, len) < 0) {
272 LOG_DBG("failed to read external reg: %02x", reg_addr);
273 lsm6dsl_shub_embedded_en(dev, false);
274 return -EIO;
275 }
276
277 lsm6dsl_shub_embedded_en(dev, false);
278
279 return 0;
280 }
281 #endif
282
lsm6dsl_shub_write_embedded_regs(const struct device * dev,uint8_t reg_addr,uint8_t * value,uint8_t len)283 static int lsm6dsl_shub_write_embedded_regs(const struct device *dev,
284 uint8_t reg_addr,
285 uint8_t *value, uint8_t len)
286 {
287 struct lsm6dsl_data *data = dev->data;
288 lsm6dsl_shub_embedded_en(dev, true);
289
290 if (data->hw_tf->write_data(dev, reg_addr, value, len) < 0) {
291 LOG_DBG("failed to write external reg: %02x", reg_addr);
292 lsm6dsl_shub_embedded_en(dev, false);
293 return -EIO;
294 }
295
296 lsm6dsl_shub_embedded_en(dev, false);
297
298 return 0;
299 }
300
lsm6dsl_shub_enable(const struct device * dev)301 static void lsm6dsl_shub_enable(const struct device *dev)
302 {
303 struct lsm6dsl_data *data = dev->data;
304
305 /* Enable Digital Func */
306 data->hw_tf->update_reg(dev, LSM6DSL_REG_CTRL10_C,
307 LSM6DSL_MASK_CTRL10_C_FUNC_EN,
308 1 << LSM6DSL_SHIFT_CTRL10_C_FUNC_EN);
309
310 /* Enable Accel @26hz */
311 if (!data->accel_freq) {
312 data->hw_tf->update_reg(dev,
313 LSM6DSL_REG_CTRL1_XL,
314 LSM6DSL_MASK_CTRL1_XL_ODR_XL,
315 2 << LSM6DSL_SHIFT_CTRL1_XL_ODR_XL);
316 }
317
318 /* Enable Sensor Hub */
319 data->hw_tf->update_reg(dev, LSM6DSL_REG_MASTER_CONFIG,
320 LSM6DSL_MASK_MASTER_CONFIG_MASTER_ON,
321 1 << LSM6DSL_SHIFT_MASTER_CONFIG_MASTER_ON);
322 }
323
lsm6dsl_shub_disable(const struct device * dev)324 static void lsm6dsl_shub_disable(const struct device *dev)
325 {
326 struct lsm6dsl_data *data = dev->data;
327
328 /* Disable Sensor Hub */
329 data->hw_tf->update_reg(dev, LSM6DSL_REG_MASTER_CONFIG,
330 LSM6DSL_MASK_MASTER_CONFIG_MASTER_ON,
331 0 << LSM6DSL_SHIFT_MASTER_CONFIG_MASTER_ON);
332
333 /* Disable Accel */
334 if (!data->accel_freq) {
335 data->hw_tf->update_reg(dev,
336 LSM6DSL_REG_CTRL1_XL,
337 LSM6DSL_MASK_CTRL1_XL_ODR_XL,
338 0 << LSM6DSL_SHIFT_CTRL1_XL_ODR_XL);
339 }
340
341 /* Disable Digital Func */
342 data->hw_tf->update_reg(dev, LSM6DSL_REG_CTRL10_C,
343 LSM6DSL_MASK_CTRL10_C_FUNC_EN,
344 0 << LSM6DSL_SHIFT_CTRL10_C_FUNC_EN);
345 }
346
347 /*
348 * use SLV0 for generic read to slave device
349 */
lsm6dsl_shub_read_slave_reg(const struct device * dev,uint8_t slv_addr,uint8_t slv_reg,uint8_t * value,uint16_t len)350 static int lsm6dsl_shub_read_slave_reg(const struct device *dev,
351 uint8_t slv_addr, uint8_t slv_reg,
352 uint8_t *value, uint16_t len)
353 {
354 struct lsm6dsl_data *data = dev->data;
355 uint8_t slave[3];
356
357 slave[0] = (slv_addr << 1) | LSM6DSL_EMBEDDED_SLVX_READ;
358 slave[1] = slv_reg;
359 slave[2] = (len & 0x7);
360
361 if (lsm6dsl_shub_write_embedded_regs(dev, LSM6DSL_EMBEDDED_SLV0_ADDR,
362 slave, 3) < 0) {
363 LOG_DBG("error writing embedded reg");
364 return -EIO;
365 }
366
367 /* turn SH on */
368 lsm6dsl_shub_enable(dev);
369 lsm6dsl_shub_wait_completed(dev);
370 data->hw_tf->read_data(dev, LSM6DSL_REG_SENSORHUB1, value, len);
371
372 lsm6dsl_shub_disable(dev);
373 return 0;
374 }
375
376 /*
377 * use SLV0 to configure slave device
378 */
lsm6dsl_shub_write_slave_reg(const struct device * dev,uint8_t slv_addr,uint8_t slv_reg,uint8_t * value,uint16_t len)379 static int lsm6dsl_shub_write_slave_reg(const struct device *dev,
380 uint8_t slv_addr, uint8_t slv_reg,
381 uint8_t *value, uint16_t len)
382 {
383 uint8_t slv_cfg[3];
384 uint8_t cnt = 0U;
385
386 while (cnt < len) {
387 slv_cfg[0] = (slv_addr << 1) & ~LSM6DSL_EMBEDDED_SLVX_READ;
388 slv_cfg[1] = slv_reg + cnt;
389
390 if (lsm6dsl_shub_write_embedded_regs(dev,
391 LSM6DSL_EMBEDDED_SLV0_ADDR,
392 slv_cfg, 2) < 0) {
393 LOG_DBG("error writing embedded reg");
394 return -EIO;
395 }
396
397 slv_cfg[0] = value[cnt];
398 if (lsm6dsl_shub_write_embedded_regs(dev,
399 LSM6DSL_EMBEDDED_SLV0_DATAWRITE,
400 slv_cfg, 1) < 0) {
401 LOG_DBG("error writing embedded reg");
402 return -EIO;
403 }
404
405 /* turn SH on */
406 lsm6dsl_shub_enable(dev);
407 lsm6dsl_shub_wait_completed(dev);
408 lsm6dsl_shub_disable(dev);
409
410 cnt++;
411 }
412
413 /* Put master in IDLE mode */
414 slv_cfg[0] = LSM6DSL_EMBEDDED_SLV0_WRITE_IDLE;
415 slv_cfg[1] = lsm6dsl_shub_sens_list[0].wai_addr;
416 slv_cfg[2] = LSM6DSL_EMBEDDED_SLVX_THREE_SENS;
417 if (lsm6dsl_shub_write_embedded_regs(dev,
418 LSM6DSL_EMBEDDED_SLV0_ADDR,
419 slv_cfg, 3) < 0) {
420 LOG_DBG("error writing embedded reg");
421 return -EIO;
422 }
423
424 return 0;
425 }
426
427 /*
428 * SLAVEs configurations:
429 *
430 * - SLAVE 0: used for configuring the slave device
431 * - SLAVE 1: used as data read channel to slave device
432 * - SLAVE 2: used for generic reads while data channel is enabled
433 */
lsm6dsl_shub_set_data_channel(const struct device * dev)434 static int lsm6dsl_shub_set_data_channel(const struct device *dev)
435 {
436 uint8_t slv_cfg[3];
437 uint8_t slv_i2c_addr = lsm6dsl_shub_sens_list[0].i2c_addr[ext_i2c_addr];
438
439 /* SLV0 is used for generic write */
440 slv_cfg[0] = LSM6DSL_EMBEDDED_SLV0_WRITE_IDLE;
441 slv_cfg[1] = lsm6dsl_shub_sens_list[0].wai_addr;
442 slv_cfg[2] = LSM6DSL_EMBEDDED_SLVX_THREE_SENS;
443 if (lsm6dsl_shub_write_embedded_regs(dev,
444 LSM6DSL_EMBEDDED_SLV0_ADDR,
445 slv_cfg, 3) < 0) {
446 LOG_DBG("error writing embedded reg");
447 return -EIO;
448 }
449
450 /* Set data channel for slave device */
451 slv_cfg[0] = (slv_i2c_addr << 1) | LSM6DSL_EMBEDDED_SLVX_READ;
452 slv_cfg[1] = lsm6dsl_shub_sens_list[0].out_data_addr;
453 slv_cfg[2] = lsm6dsl_shub_sens_list[0].out_data_len;
454 if (lsm6dsl_shub_write_embedded_regs(dev,
455 LSM6DSL_EMBEDDED_SLV1_ADDR,
456 slv_cfg, 3) < 0) {
457 LOG_DBG("error writing embedded reg");
458 return -EIO;
459 }
460
461 /* turn SH on */
462 lsm6dsl_shub_enable(dev);
463 lsm6dsl_shub_wait_completed(dev);
464
465 return 0;
466 }
467
lsm6dsl_shub_read_external_chip(const struct device * dev,uint8_t * buf,uint8_t len)468 int lsm6dsl_shub_read_external_chip(const struct device *dev, uint8_t *buf,
469 uint8_t len)
470 {
471 struct lsm6dsl_data *data = dev->data;
472
473 data->hw_tf->read_data(dev, LSM6DSL_REG_SENSORHUB1, buf, len);
474
475 return 0;
476 }
477
lsm6dsl_shub_init_external_chip(const struct device * dev)478 int lsm6dsl_shub_init_external_chip(const struct device *dev)
479 {
480 uint8_t i;
481 uint8_t chip_id = 0U;
482 uint8_t slv_i2c_addr;
483 uint8_t slv_wai_addr = lsm6dsl_shub_sens_list[0].wai_addr;
484
485 /*
486 * The external sensor may have different I2C address.
487 * So, try them one by one until we read the correct
488 * chip ID.
489 */
490 for (i = 0U; i < ARRAY_SIZE(lsm6dsl_shub_sens_list[0].i2c_addr); i++) {
491 slv_i2c_addr = lsm6dsl_shub_sens_list[0].i2c_addr[i];
492
493 if (slv_i2c_addr == 0U) {
494 continue;
495 }
496
497 if (lsm6dsl_shub_read_slave_reg(dev, slv_i2c_addr,
498 slv_wai_addr,
499 &chip_id, 1) < 0) {
500 LOG_DBG("failed reading external chip id");
501 return -EIO;
502 }
503 if (chip_id == lsm6dsl_shub_sens_list[0].wai_val) {
504 break;
505 }
506 }
507
508 if (i >= ARRAY_SIZE(lsm6dsl_shub_sens_list[0].i2c_addr)) {
509 LOG_DBG("invalid chip id 0x%x", chip_id);
510 return -EIO;
511 }
512 LOG_DBG("Ext Device Chip Id: %02x", chip_id);
513 ext_i2c_addr = i;
514
515 /* init external device */
516 lsm6dsl_shub_sens_list[0].dev_init(dev, slv_i2c_addr);
517
518 lsm6dsl_shub_set_data_channel(dev);
519
520 return 0;
521 }
522
