1 /* ST Microelectronics LSM6DSO 6-axis IMU sensor driver
2 *
3 * Copyright (c) 2019 STMicroelectronics
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 *
7 * Datasheet:
8 * https://www.st.com/resource/en/datasheet/lsm6dso.pdf
9 */
10
11 #define DT_DRV_COMPAT st_lsm6dso
12
13 #include <zephyr/kernel.h>
14 #include <zephyr/drivers/sensor.h>
15 #include <zephyr/drivers/gpio.h>
16 #include <zephyr/logging/log.h>
17
18 #include "lsm6dso.h"
19
20 LOG_MODULE_DECLARE(LSM6DSO, CONFIG_SENSOR_LOG_LEVEL);
21
22 #if defined(CONFIG_LSM6DSO_ENABLE_TEMP)
23 /**
24 * lsm6dso_enable_t_int - TEMP enable selected int pin to generate interrupt
25 */
lsm6dso_enable_t_int(const struct device * dev,int enable)26 static int lsm6dso_enable_t_int(const struct device *dev, int enable)
27 {
28 const struct lsm6dso_config *cfg = dev->config;
29 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
30 lsm6dso_int2_ctrl_t int2_ctrl;
31
32 if (enable) {
33 int16_t buf;
34
35 /* dummy read: re-trigger interrupt */
36 lsm6dso_temperature_raw_get(ctx, &buf);
37 }
38
39 /* set interrupt (TEMP DRDY interrupt is only on INT2) */
40 if (cfg->int_pin == 1) {
41 return -EIO;
42 }
43
44 lsm6dso_read_reg(ctx, LSM6DSO_INT2_CTRL, (uint8_t *)&int2_ctrl, 1);
45 int2_ctrl.int2_drdy_temp = enable;
46 return lsm6dso_write_reg(ctx, LSM6DSO_INT2_CTRL,
47 (uint8_t *)&int2_ctrl, 1);
48 }
49 #endif
50
51 /**
52 * lsm6dso_enable_xl_int - XL enable selected int pin to generate interrupt
53 */
lsm6dso_enable_xl_int(const struct device * dev,int enable)54 static int lsm6dso_enable_xl_int(const struct device *dev, int enable)
55 {
56 const struct lsm6dso_config *cfg = dev->config;
57 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
58
59 if (enable) {
60 int16_t buf[3];
61
62 /* dummy read: re-trigger interrupt */
63 lsm6dso_acceleration_raw_get(ctx, buf);
64 }
65
66 /* set interrupt */
67 if (cfg->int_pin == 1) {
68 lsm6dso_int1_ctrl_t int1_ctrl;
69
70 lsm6dso_read_reg(ctx, LSM6DSO_INT1_CTRL,
71 (uint8_t *)&int1_ctrl, 1);
72
73 int1_ctrl.int1_drdy_xl = enable;
74 return lsm6dso_write_reg(ctx, LSM6DSO_INT1_CTRL,
75 (uint8_t *)&int1_ctrl, 1);
76 } else {
77 lsm6dso_int2_ctrl_t int2_ctrl;
78
79 lsm6dso_read_reg(ctx, LSM6DSO_INT2_CTRL,
80 (uint8_t *)&int2_ctrl, 1);
81 int2_ctrl.int2_drdy_xl = enable;
82 return lsm6dso_write_reg(ctx, LSM6DSO_INT2_CTRL,
83 (uint8_t *)&int2_ctrl, 1);
84 }
85 }
86
87 /**
88 * lsm6dso_enable_g_int - Gyro enable selected int pin to generate interrupt
89 */
lsm6dso_enable_g_int(const struct device * dev,int enable)90 static int lsm6dso_enable_g_int(const struct device *dev, int enable)
91 {
92 const struct lsm6dso_config *cfg = dev->config;
93 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
94
95 if (enable) {
96 int16_t buf[3];
97
98 /* dummy read: re-trigger interrupt */
99 lsm6dso_angular_rate_raw_get(ctx, buf);
100 }
101
102 /* set interrupt */
103 if (cfg->int_pin == 1) {
104 lsm6dso_int1_ctrl_t int1_ctrl;
105
106 lsm6dso_read_reg(ctx, LSM6DSO_INT1_CTRL,
107 (uint8_t *)&int1_ctrl, 1);
108 int1_ctrl.int1_drdy_g = enable;
109 return lsm6dso_write_reg(ctx, LSM6DSO_INT1_CTRL,
110 (uint8_t *)&int1_ctrl, 1);
111 } else {
112 lsm6dso_int2_ctrl_t int2_ctrl;
113
114 lsm6dso_read_reg(ctx, LSM6DSO_INT2_CTRL,
115 (uint8_t *)&int2_ctrl, 1);
116 int2_ctrl.int2_drdy_g = enable;
117 return lsm6dso_write_reg(ctx, LSM6DSO_INT2_CTRL,
118 (uint8_t *)&int2_ctrl, 1);
119 }
120 }
121
122 /**
123 * lsm6dso_trigger_set - link external trigger to event data ready
124 */
lsm6dso_trigger_set(const struct device * dev,const struct sensor_trigger * trig,sensor_trigger_handler_t handler)125 int lsm6dso_trigger_set(const struct device *dev,
126 const struct sensor_trigger *trig,
127 sensor_trigger_handler_t handler)
128 {
129 const struct lsm6dso_config *cfg = dev->config;
130 struct lsm6dso_data *lsm6dso = dev->data;
131
132 if (!cfg->trig_enabled) {
133 LOG_ERR("trigger_set op not supported");
134 return -ENOTSUP;
135 }
136
137 if (trig->chan == SENSOR_CHAN_ACCEL_XYZ) {
138 lsm6dso->handler_drdy_acc = handler;
139 lsm6dso->trig_drdy_acc = trig;
140 if (handler) {
141 return lsm6dso_enable_xl_int(dev, LSM6DSO_EN_BIT);
142 } else {
143 return lsm6dso_enable_xl_int(dev, LSM6DSO_DIS_BIT);
144 }
145 } else if (trig->chan == SENSOR_CHAN_GYRO_XYZ) {
146 lsm6dso->handler_drdy_gyr = handler;
147 lsm6dso->trig_drdy_gyr = trig;
148 if (handler) {
149 return lsm6dso_enable_g_int(dev, LSM6DSO_EN_BIT);
150 } else {
151 return lsm6dso_enable_g_int(dev, LSM6DSO_DIS_BIT);
152 }
153 }
154 #if defined(CONFIG_LSM6DSO_ENABLE_TEMP)
155 else if (trig->chan == SENSOR_CHAN_DIE_TEMP) {
156 lsm6dso->handler_drdy_temp = handler;
157 lsm6dso->trig_drdy_temp = trig;
158 if (handler) {
159 return lsm6dso_enable_t_int(dev, LSM6DSO_EN_BIT);
160 } else {
161 return lsm6dso_enable_t_int(dev, LSM6DSO_DIS_BIT);
162 }
163 }
164 #endif
165
166 return -ENOTSUP;
167 }
168
169 /**
170 * lsm6dso_handle_interrupt - handle the drdy event
171 * read data and call handler if registered any
172 */
lsm6dso_handle_interrupt(const struct device * dev)173 static void lsm6dso_handle_interrupt(const struct device *dev)
174 {
175 struct lsm6dso_data *lsm6dso = dev->data;
176 const struct lsm6dso_config *cfg = dev->config;
177 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
178 lsm6dso_status_reg_t status;
179
180 while (1) {
181 if (lsm6dso_status_reg_get(ctx, &status) < 0) {
182 LOG_DBG("failed reading status reg");
183 return;
184 }
185
186 if ((status.xlda == 0) && (status.gda == 0)
187 #if defined(CONFIG_LSM6DSO_ENABLE_TEMP)
188 && (status.tda == 0)
189 #endif
190 ) {
191 break;
192 }
193
194 if ((status.xlda) && (lsm6dso->handler_drdy_acc != NULL)) {
195 lsm6dso->handler_drdy_acc(dev, lsm6dso->trig_drdy_acc);
196 }
197
198 if ((status.gda) && (lsm6dso->handler_drdy_gyr != NULL)) {
199 lsm6dso->handler_drdy_gyr(dev, lsm6dso->trig_drdy_gyr);
200 }
201
202 #if defined(CONFIG_LSM6DSO_ENABLE_TEMP)
203 if ((status.tda) && (lsm6dso->handler_drdy_temp != NULL)) {
204 lsm6dso->handler_drdy_temp(dev, lsm6dso->trig_drdy_temp);
205 }
206 #endif
207 }
208
209 gpio_pin_interrupt_configure_dt(&cfg->gpio_drdy,
210 GPIO_INT_EDGE_TO_ACTIVE);
211 }
212
lsm6dso_gpio_callback(const struct device * dev,struct gpio_callback * cb,uint32_t pins)213 static void lsm6dso_gpio_callback(const struct device *dev,
214 struct gpio_callback *cb, uint32_t pins)
215 {
216 struct lsm6dso_data *lsm6dso =
217 CONTAINER_OF(cb, struct lsm6dso_data, gpio_cb);
218 const struct lsm6dso_config *cfg = lsm6dso->dev->config;
219
220 ARG_UNUSED(pins);
221
222 gpio_pin_interrupt_configure_dt(&cfg->gpio_drdy, GPIO_INT_DISABLE);
223
224 #if defined(CONFIG_LSM6DSO_TRIGGER_OWN_THREAD)
225 k_sem_give(&lsm6dso->gpio_sem);
226 #elif defined(CONFIG_LSM6DSO_TRIGGER_GLOBAL_THREAD)
227 k_work_submit(&lsm6dso->work);
228 #endif /* CONFIG_LSM6DSO_TRIGGER_OWN_THREAD */
229 }
230
231 #ifdef CONFIG_LSM6DSO_TRIGGER_OWN_THREAD
lsm6dso_thread(void * p1,void * p2,void * p3)232 static void lsm6dso_thread(void *p1, void *p2, void *p3)
233 {
234 ARG_UNUSED(p2);
235 ARG_UNUSED(p3);
236
237 struct lsm6dso_data *lsm6dso = p1;
238
239 while (1) {
240 k_sem_take(&lsm6dso->gpio_sem, K_FOREVER);
241 lsm6dso_handle_interrupt(lsm6dso->dev);
242 }
243 }
244 #endif /* CONFIG_LSM6DSO_TRIGGER_OWN_THREAD */
245
246 #ifdef CONFIG_LSM6DSO_TRIGGER_GLOBAL_THREAD
lsm6dso_work_cb(struct k_work * work)247 static void lsm6dso_work_cb(struct k_work *work)
248 {
249 struct lsm6dso_data *lsm6dso =
250 CONTAINER_OF(work, struct lsm6dso_data, work);
251
252 lsm6dso_handle_interrupt(lsm6dso->dev);
253 }
254 #endif /* CONFIG_LSM6DSO_TRIGGER_GLOBAL_THREAD */
255
lsm6dso_init_interrupt(const struct device * dev)256 int lsm6dso_init_interrupt(const struct device *dev)
257 {
258 struct lsm6dso_data *lsm6dso = dev->data;
259 const struct lsm6dso_config *cfg = dev->config;
260 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
261 int ret;
262
263 /* setup data ready gpio interrupt (INT1 or INT2) */
264 if (!gpio_is_ready_dt(&cfg->gpio_drdy)) {
265 LOG_ERR("Cannot get pointer to drdy_gpio device");
266 return -EINVAL;
267 }
268
269 #if defined(CONFIG_LSM6DSO_TRIGGER_OWN_THREAD)
270 k_sem_init(&lsm6dso->gpio_sem, 0, K_SEM_MAX_LIMIT);
271
272 k_thread_create(&lsm6dso->thread, lsm6dso->thread_stack,
273 CONFIG_LSM6DSO_THREAD_STACK_SIZE,
274 lsm6dso_thread, lsm6dso,
275 NULL, NULL, K_PRIO_COOP(CONFIG_LSM6DSO_THREAD_PRIORITY),
276 0, K_NO_WAIT);
277 k_thread_name_set(&lsm6dso->thread, "lsm6dso");
278 #elif defined(CONFIG_LSM6DSO_TRIGGER_GLOBAL_THREAD)
279 lsm6dso->work.handler = lsm6dso_work_cb;
280 #endif /* CONFIG_LSM6DSO_TRIGGER_OWN_THREAD */
281
282 ret = gpio_pin_configure_dt(&cfg->gpio_drdy, GPIO_INPUT);
283 if (ret < 0) {
284 LOG_DBG("Could not configure gpio");
285 return ret;
286 }
287
288 gpio_init_callback(&lsm6dso->gpio_cb,
289 lsm6dso_gpio_callback,
290 BIT(cfg->gpio_drdy.pin));
291
292 if (gpio_add_callback(cfg->gpio_drdy.port, &lsm6dso->gpio_cb) < 0) {
293 LOG_DBG("Could not set gpio callback");
294 return -EIO;
295 }
296
297
298 /* set data ready mode on int1/int2 */
299 LOG_DBG("drdy_pulsed is %d", (int)cfg->drdy_pulsed);
300 lsm6dso_dataready_pulsed_t mode = cfg->drdy_pulsed ? LSM6DSO_DRDY_PULSED :
301 LSM6DSO_DRDY_LATCHED;
302
303 ret = lsm6dso_data_ready_mode_set(ctx, mode);
304 if (ret < 0) {
305 LOG_ERR("drdy_pulsed config error %d", (int)cfg->drdy_pulsed);
306 return ret;
307 }
308
309 return gpio_pin_interrupt_configure_dt(&cfg->gpio_drdy,
310 GPIO_INT_EDGE_TO_ACTIVE);
311 }
312
