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 #include <zephyr/drivers/sensor.h>
10 #include <zephyr/drivers/spi.h>
11 #include <zephyr/sys/byteorder.h>
12 #include "icm42688.h"
13 #include "icm42688_reg.h"
14 #include "icm42688_spi.h"
15 #include "icm42688_trigger.h"
16
17 #include <zephyr/logging/log.h>
18 LOG_MODULE_REGISTER(ICM42688_LL, CONFIG_SENSOR_LOG_LEVEL);
19
icm42688_reset(const struct device * dev)20 int icm42688_reset(const struct device *dev)
21 {
22 int res;
23 uint8_t value;
24 const struct icm42688_dev_cfg *dev_cfg = dev->config;
25
26 /* start up time for register read/write after POR is 1ms and supply ramp time is 3ms */
27 k_msleep(3);
28
29 /* perform a soft reset to ensure a clean slate, reset bit will auto-clear */
30 res = icm42688_spi_single_write(&dev_cfg->spi, REG_DEVICE_CONFIG, BIT_SOFT_RESET);
31
32 if (res) {
33 LOG_ERR("write REG_SIGNAL_PATH_RESET failed");
34 return res;
35 }
36
37 /* wait for soft reset to take effect */
38 k_msleep(SOFT_RESET_TIME_MS);
39
40 /* clear reset done int flag */
41 res = icm42688_spi_read(&dev_cfg->spi, REG_INT_STATUS, &value, 1);
42
43 if (res) {
44 return res;
45 }
46
47 if (FIELD_GET(BIT_INT_STATUS_RESET_DONE, value) != 1) {
48 LOG_ERR("unexpected RESET_DONE value, %i", value);
49 return -EINVAL;
50 }
51
52 res = icm42688_spi_read(&dev_cfg->spi, REG_WHO_AM_I, &value, 1);
53 if (res) {
54 return res;
55 }
56
57 if (value != WHO_AM_I_ICM42688) {
58 LOG_ERR("invalid WHO_AM_I value, was %i but expected %i", value, WHO_AM_I_ICM42688);
59 return -EINVAL;
60 }
61
62 return 0;
63 }
64
icm42688_compute_fifo_wm(const struct icm42688_cfg * cfg)65 static uint16_t icm42688_compute_fifo_wm(const struct icm42688_cfg *cfg)
66 {
67 const bool accel_enabled = cfg->accel_mode != ICM42688_ACCEL_OFF;
68 const bool gyro_enabled = cfg->gyro_mode != ICM42688_GYRO_OFF;
69 const int pkt_size = cfg->fifo_hires ? 20 : (accel_enabled && gyro_enabled ? 16 : 8);
70 int accel_modr = 0;
71 int gyro_modr = 0;
72 int64_t modr;
73
74 if (cfg->batch_ticks == 0 || (!accel_enabled && !gyro_enabled)) {
75 return 0;
76 }
77
78 if (accel_enabled) {
79 struct sensor_value val = {0};
80
81 icm42688_accel_reg_to_hz(cfg->accel_odr, &val);
82 accel_modr = sensor_value_to_micro(&val) / 1000;
83 }
84 if (gyro_enabled) {
85 struct sensor_value val = {0};
86
87 icm42688_gyro_reg_to_odr(cfg->gyro_odr, &val);
88 gyro_modr = sensor_value_to_micro(&val) / 1000;
89 }
90
91 if (accel_modr == 0) {
92 modr = gyro_modr;
93 } else if (gyro_modr == 0) {
94 modr = accel_modr;
95 } else {
96 /* Need to find the least common multiplier (LCM) */
97 int n1 = accel_modr;
98 int n2 = gyro_modr;
99
100 while (n1 != n2) {
101 if (n1 > n2) {
102 n1 -= n2;
103 } else {
104 n2 -= n1;
105 }
106 }
107 LOG_DBG("GCD=%d", n1);
108 modr = ((int64_t)accel_modr * (int64_t)gyro_modr) / n1;
109 }
110 /* At this point we have 'modr' as mHz which is 1 / msec. */
111
112 /* Convert 'modr' to bytes * batch_ticks / msec */
113 modr *= (int64_t)cfg->batch_ticks * pkt_size;
114
115 /* 'modr' = byte_ticks_per_msec / kticks_per_sec */
116 modr = DIV_ROUND_UP(modr, CONFIG_SYS_CLOCK_TICKS_PER_SEC * INT64_C(1000));
117
118 return (uint16_t)MIN(modr, 0x7ff);
119 }
120
icm42688_configure(const struct device * dev,struct icm42688_cfg * cfg)121 int icm42688_configure(const struct device *dev, struct icm42688_cfg *cfg)
122 {
123 struct icm42688_dev_data *dev_data = dev->data;
124 const struct icm42688_dev_cfg *dev_cfg = dev->config;
125 int res;
126
127 /* Disable interrupts, reconfigured at end */
128 res = icm42688_spi_single_write(&dev_cfg->spi, REG_INT_SOURCE0, 0);
129
130 /* if fifo is enabled right now, disable and flush */
131 if (dev_data->cfg.fifo_en) {
132 res = icm42688_spi_single_write(&dev_cfg->spi, REG_FIFO_CONFIG,
133 FIELD_PREP(MASK_FIFO_MODE, BIT_FIFO_MODE_BYPASS));
134
135 if (res != 0) {
136 LOG_ERR("Error writing FIFO_CONFIG");
137 return -EINVAL;
138 }
139
140 res = icm42688_spi_single_write(&dev_cfg->spi, REG_SIGNAL_PATH_RESET,
141 FIELD_PREP(BIT_FIFO_FLUSH, 1));
142
143 if (res != 0) {
144 LOG_ERR("Error flushing fifo");
145 return -EINVAL;
146 }
147 }
148
149 /* TODO maybe do the next few steps intelligently by checking current config */
150
151 /* Power management to set gyro/accel modes */
152 uint8_t pwr_mgmt0 = FIELD_PREP(MASK_GYRO_MODE, cfg->gyro_mode) |
153 FIELD_PREP(MASK_ACCEL_MODE, cfg->accel_mode) |
154 FIELD_PREP(BIT_TEMP_DIS, cfg->temp_dis);
155
156 LOG_DBG("PWR_MGMT0 (0x%x) 0x%x", REG_PWR_MGMT0, pwr_mgmt0);
157 res = icm42688_spi_single_write(&dev_cfg->spi, REG_PWR_MGMT0, pwr_mgmt0);
158
159 if (res != 0) {
160 LOG_ERR("Error writing PWR_MGMT0");
161 return -EINVAL;
162 }
163
164 /* Need to wait at least 200us before updating more registers
165 * see datasheet 14.36
166 */
167 k_busy_wait(250);
168
169 uint8_t accel_config0 = FIELD_PREP(MASK_ACCEL_ODR, cfg->accel_odr) |
170 FIELD_PREP(MASK_ACCEL_UI_FS_SEL, cfg->accel_fs);
171
172 LOG_DBG("ACCEL_CONFIG0 (0x%x) 0x%x", REG_ACCEL_CONFIG0, accel_config0);
173 res = icm42688_spi_single_write(&dev_cfg->spi, REG_ACCEL_CONFIG0, accel_config0);
174 if (res != 0) {
175 LOG_ERR("Error writing ACCEL_CONFIG0");
176 return -EINVAL;
177 }
178
179 uint8_t gyro_config0 = FIELD_PREP(MASK_GYRO_ODR, cfg->gyro_odr) |
180 FIELD_PREP(MASK_GYRO_UI_FS_SEL, cfg->gyro_fs);
181
182 LOG_DBG("GYRO_CONFIG0 (0x%x) 0x%x", REG_GYRO_CONFIG0, gyro_config0);
183 res = icm42688_spi_single_write(&dev_cfg->spi, REG_GYRO_CONFIG0, gyro_config0);
184 if (res != 0) {
185 LOG_ERR("Error writing GYRO_CONFIG0");
186 return -EINVAL;
187 }
188
189 /*
190 * Accelerometer sensor need at least 10ms startup time
191 * Gyroscope sensor need at least 30ms startup time
192 */
193 k_msleep(50);
194
195 /* Ensure FIFO is in bypass mode */
196 uint8_t fifo_config_bypass = FIELD_PREP(MASK_FIFO_MODE, BIT_FIFO_MODE_BYPASS);
197
198 LOG_DBG("FIFO_CONFIG (0x%x) 0x%x", REG_FIFO_CONFIG, fifo_config_bypass);
199 res = icm42688_spi_single_write(&dev_cfg->spi, REG_FIFO_CONFIG, fifo_config_bypass);
200 if (res != 0) {
201 LOG_ERR("Error writing FIFO_CONFIG");
202 return -EINVAL;
203 }
204
205 /* Disable FSYNC */
206 uint8_t tmst_config;
207
208 res = icm42688_spi_single_write(&dev_cfg->spi, REG_FSYNC_CONFIG, 0);
209 if (res != 0) {
210 LOG_ERR("Error writing FSYNC_CONFIG");
211 return -EINVAL;
212 }
213 res = icm42688_spi_read(&dev_cfg->spi, REG_TMST_CONFIG, &tmst_config, 1);
214 if (res != 0) {
215 LOG_ERR("Error reading TMST_CONFIG");
216 return -EINVAL;
217 }
218 res = icm42688_spi_single_write(&dev_cfg->spi, REG_TMST_CONFIG, tmst_config & ~BIT(1));
219 if (res != 0) {
220 LOG_ERR("Error writing TMST_CONFIG");
221 return -EINVAL;
222 }
223
224 /* Pulse mode with async reset (resets interrupt line on int status read) */
225 if (IS_ENABLED(CONFIG_ICM42688_TRIGGER)) {
226 res = icm42688_trigger_enable_interrupt(dev, cfg);
227 } else {
228 res = icm42688_spi_single_write(&dev_cfg->spi, REG_INT_CONFIG,
229 BIT_INT1_DRIVE_CIRCUIT | BIT_INT1_POLARITY);
230 }
231 if (res) {
232 LOG_ERR("Error writing to INT_CONFIG");
233 return res;
234 }
235
236 uint8_t int_config1 = 0;
237
238 if ((cfg->accel_odr <= ICM42688_ACCEL_ODR_4000 ||
239 cfg->gyro_odr <= ICM42688_GYRO_ODR_4000)) {
240 int_config1 = FIELD_PREP(BIT_INT_TPULSE_DURATION, 1) |
241 FIELD_PREP(BIT_INT_TDEASSERT_DISABLE, 1);
242 }
243
244 res = icm42688_spi_single_write(&dev_cfg->spi, REG_INT_CONFIG1, int_config1);
245 if (res) {
246 LOG_ERR("Error writing to INT_CONFIG1");
247 return res;
248 }
249
250 /* fifo configuration steps if desired */
251 if (cfg->fifo_en) {
252 LOG_INF("FIFO ENABLED");
253
254 /* Setup desired FIFO packet fields, maybe should base this on the other
255 * temp/accel/gyro en fields in cfg
256 */
257 uint8_t fifo_cfg1 =
258 FIELD_PREP(BIT_FIFO_TEMP_EN, 1) | FIELD_PREP(BIT_FIFO_GYRO_EN, 1) |
259 FIELD_PREP(BIT_FIFO_ACCEL_EN, 1) | FIELD_PREP(BIT_FIFO_TMST_FSYNC_EN, 1);
260
261 LOG_DBG("FIFO_CONFIG1 (0x%x) 0x%x", REG_FIFO_CONFIG1, fifo_cfg1);
262 res = icm42688_spi_single_write(&dev_cfg->spi, REG_FIFO_CONFIG1, fifo_cfg1);
263 if (res != 0) {
264 LOG_ERR("Error writing FIFO_CONFIG1");
265 return -EINVAL;
266 }
267
268 /* Set watermark and interrupt handling first */
269 uint16_t fifo_wm = icm42688_compute_fifo_wm(cfg);
270 uint8_t fifo_wml = fifo_wm & 0xFF;
271
272 LOG_DBG("FIFO_CONFIG2( (0x%x)) (WM Low) 0x%x", REG_FIFO_CONFIG2, fifo_wml);
273 res = icm42688_spi_single_write(&dev_cfg->spi, REG_FIFO_CONFIG2, fifo_wml);
274 if (res != 0) {
275 LOG_ERR("Error writing FIFO_CONFIG2");
276 return -EINVAL;
277 }
278
279 uint8_t fifo_wmh = (fifo_wm >> 8) & 0x0F;
280
281 LOG_DBG("FIFO_CONFIG3 (0x%x) (WM High) 0x%x", REG_FIFO_CONFIG3, fifo_wmh);
282 res = icm42688_spi_single_write(&dev_cfg->spi, REG_FIFO_CONFIG3, fifo_wmh);
283 if (res != 0) {
284 LOG_ERR("Error writing FIFO_CONFIG3");
285 return -EINVAL;
286 }
287
288 /* Begin streaming */
289 uint8_t fifo_config = FIELD_PREP(MASK_FIFO_MODE, BIT_FIFO_MODE_STREAM);
290
291 LOG_DBG("FIFO_CONFIG (0x%x) 0x%x", REG_FIFO_CONFIG, 1 << 6);
292 res = icm42688_spi_single_write(&dev_cfg->spi, REG_FIFO_CONFIG, fifo_config);
293
294 /* Config interrupt source to only be fifo wm/full */
295 uint8_t int_source0 = BIT_FIFO_FULL_INT1_EN | BIT_FIFO_THS_INT1_EN;
296
297 LOG_DBG("INT_SOURCE0 (0x%x) 0x%x", REG_INT_SOURCE0, int_source0);
298 res = icm42688_spi_single_write(&dev_cfg->spi, REG_INT_SOURCE0, int_source0);
299 if (res) {
300 return res;
301 }
302 } else {
303 LOG_INF("FIFO DISABLED");
304
305 /* No fifo mode so set data ready as interrupt source */
306 uint8_t int_source0 = BIT_UI_DRDY_INT1_EN;
307
308 LOG_DBG("INT_SOURCE0 (0x%x) 0x%x", REG_INT_SOURCE0, int_source0);
309 res = icm42688_spi_single_write(&dev_cfg->spi, REG_INT_SOURCE0, int_source0);
310 if (res) {
311 return res;
312 }
313 }
314
315 return res;
316 }
317
icm42688_safely_configure(const struct device * dev,struct icm42688_cfg * cfg)318 int icm42688_safely_configure(const struct device *dev, struct icm42688_cfg *cfg)
319 {
320 struct icm42688_dev_data *drv_data = dev->data;
321 int ret = icm42688_configure(dev, cfg);
322
323 if (ret == 0) {
324 drv_data->cfg = *cfg;
325 } else {
326 ret = icm42688_configure(dev, &drv_data->cfg);
327 }
328
329 return ret;
330 }
331
icm42688_read_all(const struct device * dev,uint8_t data[14])332 int icm42688_read_all(const struct device *dev, uint8_t data[14])
333 {
334 const struct icm42688_dev_cfg *dev_cfg = dev->config;
335 int res;
336
337 res = icm42688_spi_read(&dev_cfg->spi, REG_TEMP_DATA1, data, 14);
338
339 return res;
340 }
341
