1 /*
2 * Copyright (c) 2022 Intel Corporation
3 * Copyright (c) 2022 Esco Medical ApS
4 * Copyright (c) 2020 TDK Invensense
5 *
6 * SPDX-License-Identifier: Apache-2.0
7 */
8
9 #define DT_DRV_COMPAT invensense_icm42688
10
11 #include <zephyr/drivers/sensor.h>
12 #include <zephyr/drivers/spi.h>
13 #include <zephyr/sys/byteorder.h>
14
15 #include "icm42688.h"
16 #include "icm42688_decoder.h"
17 #include "icm42688_reg.h"
18 #include "icm42688_rtio.h"
19 #include "icm42688_spi.h"
20 #include "icm42688_trigger.h"
21
22 #include <zephyr/logging/log.h>
23 LOG_MODULE_REGISTER(ICM42688, CONFIG_SENSOR_LOG_LEVEL);
24
icm42688_convert_accel(struct sensor_value * val,int16_t raw_val,struct icm42688_cfg * cfg)25 static void icm42688_convert_accel(struct sensor_value *val, int16_t raw_val,
26 struct icm42688_cfg *cfg)
27 {
28 icm42688_accel_ms(cfg, (int32_t)raw_val, &val->val1, &val->val2);
29 }
30
icm42688_convert_gyro(struct sensor_value * val,int16_t raw_val,struct icm42688_cfg * cfg)31 static void icm42688_convert_gyro(struct sensor_value *val, int16_t raw_val,
32 struct icm42688_cfg *cfg)
33 {
34 icm42688_gyro_rads(cfg, (int32_t)raw_val, &val->val1, &val->val2);
35 }
36
icm42688_convert_temp(struct sensor_value * val,int16_t raw_val)37 static inline void icm42688_convert_temp(struct sensor_value *val, int16_t raw_val)
38 {
39 icm42688_temp_c((int32_t)raw_val, &val->val1, &val->val2);
40 }
41
icm42688_channel_parse_readings(enum sensor_channel chan,int16_t readings[7],struct icm42688_cfg * cfg,struct sensor_value * val)42 int icm42688_channel_parse_readings(enum sensor_channel chan, int16_t readings[7],
43 struct icm42688_cfg *cfg, struct sensor_value *val)
44 {
45 switch (chan) {
46 case SENSOR_CHAN_ACCEL_XYZ:
47 icm42688_convert_accel(&val[0], readings[1], cfg);
48 icm42688_convert_accel(&val[1], readings[2], cfg);
49 icm42688_convert_accel(&val[2], readings[3], cfg);
50 break;
51 case SENSOR_CHAN_ACCEL_X:
52 icm42688_convert_accel(val, readings[1], cfg);
53 break;
54 case SENSOR_CHAN_ACCEL_Y:
55 icm42688_convert_accel(val, readings[2], cfg);
56 break;
57 case SENSOR_CHAN_ACCEL_Z:
58 icm42688_convert_accel(val, readings[3], cfg);
59 break;
60 case SENSOR_CHAN_GYRO_XYZ:
61 icm42688_convert_gyro(&val[0], readings[4], cfg);
62 icm42688_convert_gyro(&val[1], readings[5], cfg);
63 icm42688_convert_gyro(&val[2], readings[6], cfg);
64 break;
65 case SENSOR_CHAN_GYRO_X:
66 icm42688_convert_gyro(val, readings[4], cfg);
67 break;
68 case SENSOR_CHAN_GYRO_Y:
69 icm42688_convert_gyro(val, readings[5], cfg);
70 break;
71 case SENSOR_CHAN_GYRO_Z:
72 icm42688_convert_gyro(val, readings[6], cfg);
73 break;
74 case SENSOR_CHAN_DIE_TEMP:
75 icm42688_convert_temp(val, readings[0]);
76 break;
77 default:
78 return -ENOTSUP;
79 }
80
81 return 0;
82 }
83
icm42688_channel_get(const struct device * dev,enum sensor_channel chan,struct sensor_value * val)84 static int icm42688_channel_get(const struct device *dev, enum sensor_channel chan,
85 struct sensor_value *val)
86 {
87 struct icm42688_dev_data *data = dev->data;
88
89 return icm42688_channel_parse_readings(chan, data->readings, &data->cfg, val);
90 }
91
icm42688_sample_fetch(const struct device * dev,enum sensor_channel chan)92 static int icm42688_sample_fetch(const struct device *dev, enum sensor_channel chan)
93 {
94 uint8_t status;
95 struct icm42688_dev_data *data = dev->data;
96 const struct icm42688_dev_cfg *cfg = dev->config;
97
98 int res = icm42688_spi_read(&cfg->spi, REG_INT_STATUS, &status, 1);
99
100 if (res) {
101 return res;
102 }
103
104 if (!FIELD_GET(BIT_INT_STATUS_DATA_RDY, status)) {
105 return -EBUSY;
106 }
107
108 uint8_t readings[14];
109
110 res = icm42688_read_all(dev, readings);
111
112 if (res) {
113 return res;
114 }
115
116 for (int i = 0; i < 7; i++) {
117 data->readings[i] = sys_le16_to_cpu((readings[i * 2] << 8) | readings[i * 2 + 1]);
118 }
119
120 return 0;
121 }
122
icm42688_attr_set(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,const struct sensor_value * val)123 static int icm42688_attr_set(const struct device *dev, enum sensor_channel chan,
124 enum sensor_attribute attr, const struct sensor_value *val)
125 {
126 const struct icm42688_dev_data *data = dev->data;
127 struct icm42688_cfg new_config = data->cfg;
128 int res = 0;
129
130 __ASSERT_NO_MSG(val != NULL);
131
132 switch (chan) {
133 case SENSOR_CHAN_ACCEL_X:
134 case SENSOR_CHAN_ACCEL_Y:
135 case SENSOR_CHAN_ACCEL_Z:
136 case SENSOR_CHAN_ACCEL_XYZ:
137 if (attr == SENSOR_ATTR_SAMPLING_FREQUENCY) {
138 new_config.accel_odr = icm42688_accel_hz_to_reg(val->val1);
139 } else if (attr == SENSOR_ATTR_FULL_SCALE) {
140 new_config.accel_fs = icm42688_accel_fs_to_reg(sensor_ms2_to_g(val));
141 } else {
142 LOG_ERR("Unsupported attribute");
143 res = -ENOTSUP;
144 }
145 break;
146 case SENSOR_CHAN_GYRO_X:
147 case SENSOR_CHAN_GYRO_Y:
148 case SENSOR_CHAN_GYRO_Z:
149 case SENSOR_CHAN_GYRO_XYZ:
150 if (attr == SENSOR_ATTR_SAMPLING_FREQUENCY) {
151 new_config.gyro_odr = icm42688_gyro_odr_to_reg(val->val1);
152 } else if (attr == SENSOR_ATTR_FULL_SCALE) {
153 new_config.gyro_fs = icm42688_gyro_fs_to_reg(sensor_rad_to_degrees(val));
154 } else {
155 LOG_ERR("Unsupported attribute");
156 res = -EINVAL;
157 }
158 break;
159 case SENSOR_CHAN_ALL:
160 if (attr == SENSOR_ATTR_BATCH_DURATION) {
161 if (val->val1 < 0) {
162 return -EINVAL;
163 }
164 new_config.batch_ticks = val->val1;
165 } else {
166 LOG_ERR("Unsupported attribute");
167 res = -EINVAL;
168 }
169 break;
170 default:
171 LOG_ERR("Unsupported channel");
172 res = -EINVAL;
173 break;
174 }
175
176 if (res) {
177 return res;
178 }
179 return icm42688_safely_configure(dev, &new_config);
180 }
181
icm42688_attr_get(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,struct sensor_value * val)182 static int icm42688_attr_get(const struct device *dev, enum sensor_channel chan,
183 enum sensor_attribute attr, struct sensor_value *val)
184 {
185 const struct icm42688_dev_data *data = dev->data;
186 const struct icm42688_cfg *cfg = &data->cfg;
187 int res = 0;
188
189 __ASSERT_NO_MSG(val != NULL);
190
191 switch (chan) {
192 case SENSOR_CHAN_ACCEL_X:
193 case SENSOR_CHAN_ACCEL_Y:
194 case SENSOR_CHAN_ACCEL_Z:
195 case SENSOR_CHAN_ACCEL_XYZ:
196 if (attr == SENSOR_ATTR_SAMPLING_FREQUENCY) {
197 icm42688_accel_reg_to_hz(cfg->accel_odr, val);
198 } else if (attr == SENSOR_ATTR_FULL_SCALE) {
199 icm42688_accel_reg_to_fs(cfg->accel_fs, val);
200 } else {
201 LOG_ERR("Unsupported attribute");
202 res = -EINVAL;
203 }
204 break;
205 case SENSOR_CHAN_GYRO_X:
206 case SENSOR_CHAN_GYRO_Y:
207 case SENSOR_CHAN_GYRO_Z:
208 case SENSOR_CHAN_GYRO_XYZ:
209 if (attr == SENSOR_ATTR_SAMPLING_FREQUENCY) {
210 icm42688_gyro_reg_to_odr(cfg->gyro_odr, val);
211 } else if (attr == SENSOR_ATTR_FULL_SCALE) {
212 icm42688_gyro_reg_to_fs(cfg->gyro_fs, val);
213 } else {
214 LOG_ERR("Unsupported attribute");
215 res = -EINVAL;
216 }
217 break;
218 case SENSOR_CHAN_ALL:
219 if (attr == SENSOR_ATTR_BATCH_DURATION) {
220 val->val1 = cfg->batch_ticks;
221 val->val2 = 0;
222 } else {
223 LOG_ERR("Unsupported attribute");
224 res = -EINVAL;
225 }
226 break;
227 default:
228 LOG_ERR("Unsupported channel");
229 res = -EINVAL;
230 break;
231 }
232
233 return res;
234 }
235
236 static const struct sensor_driver_api icm42688_driver_api = {
237 .sample_fetch = icm42688_sample_fetch,
238 .channel_get = icm42688_channel_get,
239 .attr_set = icm42688_attr_set,
240 .attr_get = icm42688_attr_get,
241 #ifdef CONFIG_ICM42688_TRIGGER
242 .trigger_set = icm42688_trigger_set,
243 #endif
244 .get_decoder = icm42688_get_decoder,
245 #ifdef CONFIG_SENSOR_ASYNC_API
246 .submit = icm42688_submit,
247 #endif
248 };
249
icm42688_init(const struct device * dev)250 int icm42688_init(const struct device *dev)
251 {
252 struct icm42688_dev_data *data = dev->data;
253 const struct icm42688_dev_cfg *cfg = dev->config;
254 int res;
255
256 if (!spi_is_ready_dt(&cfg->spi)) {
257 LOG_ERR("SPI bus is not ready");
258 return -ENODEV;
259 }
260
261 if (icm42688_reset(dev)) {
262 LOG_ERR("could not initialize sensor");
263 return -EIO;
264 }
265
266 #ifdef CONFIG_ICM42688_TRIGGER
267 res = icm42688_trigger_init(dev);
268 if (res != 0) {
269 LOG_ERR("Failed to initialize triggers");
270 return res;
271 }
272 #endif
273
274 data->cfg.accel_mode = ICM42688_ACCEL_LN;
275 data->cfg.accel_fs = ICM42688_ACCEL_FS_2G;
276 data->cfg.accel_odr = ICM42688_ACCEL_ODR_1000;
277 data->cfg.gyro_mode = ICM42688_GYRO_LN;
278 data->cfg.gyro_fs = ICM42688_GYRO_FS_125;
279 data->cfg.gyro_odr = ICM42688_GYRO_ODR_1000;
280 data->cfg.temp_dis = false;
281 data->cfg.fifo_en = IS_ENABLED(CONFIG_ICM42688_STREAM);
282 data->cfg.batch_ticks = 0;
283 data->cfg.fifo_hires = 0;
284 data->cfg.interrupt1_drdy = 0;
285 data->cfg.interrupt1_fifo_ths = 0;
286 data->cfg.interrupt1_fifo_full = 0;
287
288 res = icm42688_configure(dev, &data->cfg);
289 if (res != 0) {
290 LOG_ERR("Failed to configure");
291 return res;
292 }
293
294 return 0;
295 }
296
297 #ifndef CONFIG_ICM42688_TRIGGER
icm42688_lock(const struct device * dev)298 void icm42688_lock(const struct device *dev)
299 {
300 ARG_UNUSED(dev);
301 }
icm42688_unlock(const struct device * dev)302 void icm42688_unlock(const struct device *dev)
303 {
304 ARG_UNUSED(dev);
305 }
306 #endif
307
308 /* device defaults to spi mode 0/3 support */
309 #define ICM42688_SPI_CFG \
310 SPI_OP_MODE_MASTER | SPI_MODE_CPOL | SPI_MODE_CPHA | SPI_WORD_SET(8) | SPI_TRANSFER_MSB
311
312 #define ICM42688_RTIO_DEFINE(inst) \
313 SPI_DT_IODEV_DEFINE(icm42688_spi_iodev_##inst, DT_DRV_INST(inst), ICM42688_SPI_CFG, 0U); \
314 RTIO_DEFINE(icm42688_rtio_##inst, 8, 4);
315
316 #define ICM42688_DEFINE_DATA(inst) \
317 IF_ENABLED(CONFIG_ICM42688_STREAM, (ICM42688_RTIO_DEFINE(inst))); \
318 static struct icm42688_dev_data icm42688_driver_##inst = { \
319 IF_ENABLED(CONFIG_ICM42688_STREAM, (.r = &icm42688_rtio_##inst, \
320 .spi_iodev = &icm42688_spi_iodev_##inst,))};
321
322 #define ICM42688_INIT(inst) \
323 ICM42688_DEFINE_DATA(inst); \
324 \
325 static const struct icm42688_dev_cfg icm42688_cfg_##inst = { \
326 .spi = SPI_DT_SPEC_INST_GET(inst, ICM42688_SPI_CFG, 0U), \
327 .gpio_int1 = GPIO_DT_SPEC_INST_GET_OR(inst, int_gpios, {0}), \
328 }; \
329 \
330 SENSOR_DEVICE_DT_INST_DEFINE(inst, icm42688_init, NULL, &icm42688_driver_##inst, \
331 &icm42688_cfg_##inst, POST_KERNEL, \
332 CONFIG_SENSOR_INIT_PRIORITY, &icm42688_driver_api);
333
334 DT_INST_FOREACH_STATUS_OKAY(ICM42688_INIT)
335
