1 /* Bosch BMA4xx 3-axis accelerometer driver
2 *
3 * Copyright (c) 2023 Google LLC
4 * Copyright (c) 2024 Croxel Inc.
5 *
6 * SPDX-License-Identifier: Apache-2.0
7 */
8
9 #define DT_DRV_COMPAT bosch_bma4xx
10
11 #include <zephyr/init.h>
12 #include <zephyr/sys/byteorder.h>
13 #include <zephyr/sys/__assert.h>
14 #include <zephyr/logging/log.h>
15 #include <zephyr/pm/device.h>
16 #include <zephyr/rtio/work.h>
17 #include <zephyr/drivers/sensor_clock.h>
18
19 LOG_MODULE_REGISTER(bma4xx, CONFIG_SENSOR_LOG_LEVEL);
20 #include "bma4xx.h"
21
22 /**
23 * @brief Helper for converting m/s^2 offset values into register values
24 */
bma4xx_offset_to_reg_val(const struct sensor_value * val,uint8_t * reg_val)25 static int bma4xx_offset_to_reg_val(const struct sensor_value *val, uint8_t *reg_val)
26 {
27 int32_t ug = sensor_ms2_to_ug(val);
28
29 if (ug < BMA4XX_OFFSET_MICROG_MIN || ug > BMA4XX_OFFSET_MICROG_MAX) {
30 return -ERANGE;
31 }
32
33 *reg_val = ug / BMA4XX_OFFSET_MICROG_PER_BIT;
34 return 0;
35 }
36
37 /**
38 * @brief Set the X, Y, or Z axis offsets.
39 */
bma4xx_attr_set_offset(const struct device * dev,enum sensor_channel chan,const struct sensor_value * val)40 static int bma4xx_attr_set_offset(const struct device *dev, enum sensor_channel chan,
41 const struct sensor_value *val)
42 {
43 struct bma4xx_data *bma4xx = dev->data;
44 uint8_t reg_addr;
45 uint8_t reg_val[3];
46 int rc;
47
48 switch (chan) {
49 case SENSOR_CHAN_ACCEL_X:
50 case SENSOR_CHAN_ACCEL_Y:
51 case SENSOR_CHAN_ACCEL_Z:
52 reg_addr = BMA4XX_REG_OFFSET_0 + (chan - SENSOR_CHAN_ACCEL_X);
53 rc = bma4xx_offset_to_reg_val(val, ®_val[0]);
54 if (rc) {
55 return rc;
56 }
57 return bma4xx->hw_ops->write_reg(dev, reg_addr, reg_val[0]);
58 case SENSOR_CHAN_ACCEL_XYZ:
59 /* Expect val to point to an array of three sensor_values */
60 reg_addr = BMA4XX_REG_OFFSET_0;
61 for (int i = 0; i < 3; i++) {
62 rc = bma4xx_offset_to_reg_val(&val[i], ®_val[i]);
63 if (rc) {
64 return rc;
65 }
66 }
67 return bma4xx->hw_ops->write_data(dev, reg_addr, (uint8_t *)reg_val,
68 sizeof(reg_val));
69 default:
70 return -ENOTSUP;
71 }
72 }
73
74 static const uint32_t odr_to_reg_map[] = {
75 0, /* Invalid */
76 781250, /* 0.78125 Hz (25/32) => 0x1 */
77 1562500, /* 1.5625 Hz (25/16) => 0x2 */
78 3125000, /* 3.125 Hz (25/8) => 0x3 */
79 6250000, /* 6.25 Hz (25/4) => 0x4 */
80 12500000, /* 12.5 Hz (25/2) => 0x5 */
81 25000000, /* 25 Hz => 0x6 */
82 50000000, /* 50 Hz => 0x7*/
83 100000000, /* 100 Hz => 0x8*/
84 200000000, /* 200 Hz => 0x9*/
85 400000000, /* 400 Hz => 0xa*/
86 800000000, /* 800 Hz => 0xb*/
87 1600000000, /* 1600 Hz => 0xc*/
88 };
89
90 /**
91 * @brief Convert an ODR rate in Hz to a register value
92 */
bma4xx_odr_to_reg(uint32_t microhertz,uint8_t * reg_val)93 static int bma4xx_odr_to_reg(uint32_t microhertz, uint8_t *reg_val)
94 {
95 if (microhertz == 0) {
96 /* Illegal ODR value */
97 return -ERANGE;
98 }
99
100 for (uint8_t i = 0; i < ARRAY_SIZE(odr_to_reg_map); i++) {
101 if (microhertz <= odr_to_reg_map[i]) {
102 *reg_val = i;
103 return 0;
104 }
105 }
106
107 /* Requested ODR too high */
108 return -ERANGE;
109 }
110
111 /**
112 * Set the sensor's acceleration offset (per axis). Use bma4xx_commit_nvm() to save these
113 * offsets to nonvolatile memory so they are automatically set during power-on-reset.
114 */
bma4xx_attr_set_odr(const struct device * dev,const struct sensor_value * val)115 static int bma4xx_attr_set_odr(const struct device *dev, const struct sensor_value *val)
116 {
117 struct bma4xx_data *bma4xx = dev->data;
118 int status;
119 uint8_t reg_val;
120
121 /* Convert ODR Hz value to microhertz and round up to closest register setting */
122 status = bma4xx_odr_to_reg(val->val1 * 1000000 + val->val2, ®_val);
123 if (status < 0) {
124 return status;
125 }
126
127 status = bma4xx->hw_ops->update_reg(dev, BMA4XX_REG_ACCEL_CONFIG, BMA4XX_MASK_ACC_CONF_ODR,
128 reg_val);
129 if (status < 0) {
130 return status;
131 }
132
133 bma4xx->accel_odr = reg_val;
134 return 0;
135 }
136
137 static const uint32_t fs_to_reg_map[] = {
138 2000000, /* +/-2G => 0x0 */
139 4000000, /* +/-4G => 0x1 */
140 8000000, /* +/-8G => 0x2 */
141 16000000, /* +/-16G => 0x3 */
142 };
143
bma4xx_fs_to_reg(int32_t range_ug,uint8_t * reg_val)144 static int bma4xx_fs_to_reg(int32_t range_ug, uint8_t *reg_val)
145 {
146 if (range_ug == 0) {
147 /* Illegal value */
148 return -ERANGE;
149 }
150
151 range_ug = abs(range_ug);
152
153 for (uint8_t i = 0; i < 4; i++) {
154 if (range_ug <= fs_to_reg_map[i]) {
155 *reg_val = i;
156 return 0;
157 }
158 }
159
160 /* Requested range too high */
161 return -ERANGE;
162 }
163
164 /**
165 * Set the sensor's full-scale range
166 */
bma4xx_attr_set_range(const struct device * dev,const struct sensor_value * val)167 static int bma4xx_attr_set_range(const struct device *dev, const struct sensor_value *val)
168 {
169 struct bma4xx_data *bma4xx = dev->data;
170 int status;
171 uint8_t reg_val;
172
173 /* Convert m/s^2 to micro-G's and find closest register setting */
174 status = bma4xx_fs_to_reg(sensor_ms2_to_ug(val), ®_val);
175 if (status < 0) {
176 return status;
177 }
178
179 status = bma4xx->hw_ops->update_reg(dev, BMA4XX_REG_ACCEL_RANGE, BMA4XX_MASK_ACC_RANGE,
180 reg_val);
181 if (status < 0) {
182 return status;
183 }
184
185 bma4xx->accel_fs_range = reg_val;
186 return 0;
187 }
188
189 /**
190 * Set the sensor's bandwidth parameter (one of BMA4XX_BWP_*)
191 */
bma4xx_attr_set_bwp(const struct device * dev,const struct sensor_value * val)192 static int bma4xx_attr_set_bwp(const struct device *dev, const struct sensor_value *val)
193 {
194 /* Require that `val2` is unused, and that `val1` is in range of a valid BWP */
195 if (val->val2 || val->val1 < BMA4XX_BWP_OSR4_AVG1 || val->val1 > BMA4XX_BWP_RES_AVG128) {
196 return -EINVAL;
197 }
198
199 struct bma4xx_data *bma4xx = dev->data;
200
201 return bma4xx->hw_ops->update_reg(dev, BMA4XX_REG_ACCEL_CONFIG, BMA4XX_MASK_ACC_CONF_BWP,
202 (((uint8_t)val->val1) << BMA4XX_SHIFT_ACC_CONF_BWP));
203 }
204
205 /**
206 * @brief Implement the sensor API attribute set method.
207 */
bma4xx_attr_set(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,const struct sensor_value * val)208 static int bma4xx_attr_set(const struct device *dev, enum sensor_channel chan,
209 enum sensor_attribute attr, const struct sensor_value *val)
210 {
211 switch (attr) {
212 case SENSOR_ATTR_SAMPLING_FREQUENCY:
213 return bma4xx_attr_set_odr(dev, val);
214 case SENSOR_ATTR_FULL_SCALE:
215 return bma4xx_attr_set_range(dev, val);
216 case SENSOR_ATTR_OFFSET:
217 return bma4xx_attr_set_offset(dev, chan, val);
218 case SENSOR_ATTR_CONFIGURATION:
219 /* Use for setting the bandwidth parameter (BWP) */
220 return bma4xx_attr_set_bwp(dev, val);
221 default:
222 return -ENOTSUP;
223 }
224 }
225
226 /**
227 * Internal device initialization function for both bus types.
228 */
bma4xx_chip_init(const struct device * dev)229 static int bma4xx_chip_init(const struct device *dev)
230 {
231 struct bma4xx_data *bma4xx = dev->data;
232 const struct bma4xx_config *cfg = dev->config;
233 int status;
234
235 /* Sensor bus-specific initialization */
236 status = cfg->bus_init(dev);
237 if (status) {
238 LOG_ERR("bus_init failed: %d", status);
239 return status;
240 }
241
242 /* Read Chip ID */
243 status = bma4xx->hw_ops->read_reg(dev, BMA4XX_REG_CHIP_ID, &bma4xx->chip_id);
244 if (status) {
245 LOG_ERR("could not read chip_id: %d", status);
246 return status;
247 }
248 LOG_DBG("chip_id is 0x%02x", bma4xx->chip_id);
249
250 if (bma4xx->chip_id != BMA4XX_CHIP_ID_BMA422) {
251 LOG_WRN("Driver tested for BMA422. Check for unintended operation.");
252 }
253
254 /* Issue soft reset command */
255 status = bma4xx->hw_ops->write_reg(dev, BMA4XX_REG_CMD, BMA4XX_CMD_SOFT_RESET);
256 if (status) {
257 LOG_ERR("Could not soft-reset chip: %d", status);
258 return status;
259 }
260
261 k_sleep(K_USEC(1000));
262
263 /* Default is: range = +/-4G, ODR = 100 Hz, BWP = "NORM_AVG4" */
264 bma4xx->accel_fs_range = BMA4XX_RANGE_4G;
265 bma4xx->accel_bwp = BMA4XX_BWP_NORM_AVG4;
266 bma4xx->accel_odr = BMA4XX_ODR_100;
267
268 /* Switch to performance power mode */
269 status = bma4xx->hw_ops->update_reg(dev, BMA4XX_REG_ACCEL_CONFIG, BMA4XX_BIT_ACC_PERF_MODE,
270 BMA4XX_BIT_ACC_PERF_MODE);
271 if (status) {
272 LOG_ERR("Could not enable performance power save mode: %d", status);
273 return status;
274 }
275
276 /* Enable accelerometer */
277 status = bma4xx->hw_ops->update_reg(dev, BMA4XX_REG_POWER_CTRL, BMA4XX_BIT_ACC_EN,
278 BMA4XX_BIT_ACC_EN);
279 if (status) {
280 LOG_ERR("Could not enable accel: %d", status);
281 return status;
282 }
283
284 return status;
285 }
286
287 /*
288 * Sample fetch and conversion
289 */
290
291 /**
292 * @brief Read accelerometer data from the BMA4xx
293 */
bma4xx_sample_fetch(const struct device * dev,int16_t * x,int16_t * y,int16_t * z)294 static int bma4xx_sample_fetch(const struct device *dev, int16_t *x, int16_t *y, int16_t *z)
295 {
296 struct bma4xx_data *bma4xx = dev->data;
297 uint8_t read_data[6];
298 int status;
299
300 /* Burst read regs DATA_8 through DATA_13, which holds the accel readings */
301 status = bma4xx->hw_ops->read_data(dev, BMA4XX_REG_DATA_8, (uint8_t *)&read_data,
302 BMA4XX_REG_DATA_13 - BMA4XX_REG_DATA_8 + 1);
303 if (status < 0) {
304 LOG_ERR("Cannot read accel data: %d", status);
305 return status;
306 }
307
308 LOG_DBG("Raw values [0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x]", read_data[0],
309 read_data[1], read_data[2], read_data[3], read_data[4], read_data[5]);
310 /* Values in accel_data[N] are left-aligned and will read 16x actual */
311 *x = ((int16_t)read_data[1] << 4) | FIELD_GET(GENMASK(7, 4), read_data[0]);
312 *y = ((int16_t)read_data[3] << 4) | FIELD_GET(GENMASK(7, 4), read_data[2]);
313 *z = ((int16_t)read_data[5] << 4) | FIELD_GET(GENMASK(7, 4), read_data[4]);
314
315 LOG_DBG("XYZ reg vals are %d, %d, %d", *x, *y, *z);
316
317 return 0;
318 }
319
320 #ifdef CONFIG_BMA4XX_TEMPERATURE
321 /**
322 * @brief Read temperature register on BMA4xx
323 */
bma4xx_temp_fetch(const struct device * dev,int8_t * temp)324 static int bma4xx_temp_fetch(const struct device *dev, int8_t *temp)
325 {
326 struct bma4xx_data *bma4xx = dev->data;
327 int status;
328
329 status = bma4xx->hw_ops->read_reg(dev, BMA4XX_REG_TEMPERATURE, temp);
330 if (status) {
331 LOG_ERR("could not read temp reg: %d", status);
332 return status;
333 }
334
335 LOG_DBG("temp reg val is %d", *temp);
336 return 0;
337 }
338 #endif
339
340 /*
341 * RTIO submit and encoding
342 */
343
bma4xx_submit_one_shot(const struct device * dev,struct rtio_iodev_sqe * iodev_sqe)344 static void bma4xx_submit_one_shot(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe)
345 {
346 struct bma4xx_data *bma4xx = dev->data;
347
348 const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data;
349 const struct sensor_chan_spec *const channels = cfg->channels;
350 const size_t num_channels = cfg->count;
351
352 uint32_t min_buf_len = sizeof(struct bma4xx_encoded_data);
353 struct bma4xx_encoded_data *edata;
354 uint8_t *buf;
355 uint32_t buf_len;
356 uint64_t cycles;
357 int rc;
358
359 /* Get the buffer for the frame, it may be allocated dynamically by the rtio context */
360 rc = rtio_sqe_rx_buf(iodev_sqe, min_buf_len, min_buf_len, &buf, &buf_len);
361 if (rc != 0) {
362 LOG_ERR("Failed to get a read buffer of size %u bytes", min_buf_len);
363 rtio_iodev_sqe_err(iodev_sqe, rc);
364 return;
365 }
366
367 rc = sensor_clock_get_cycles(&cycles);
368 if (rc != 0) {
369 LOG_ERR("Failed to get sensor clock cycles");
370 rtio_iodev_sqe_err(iodev_sqe, rc);
371 return;
372 }
373
374 /* Prepare response */
375 edata = (struct bma4xx_encoded_data *)buf;
376 edata->header.is_fifo = false;
377 edata->header.accel_fs = bma4xx->accel_fs_range;
378 edata->header.timestamp = sensor_clock_cycles_to_ns(cycles);
379 edata->has_accel = 0;
380 edata->has_temp = 0;
381
382 /* Determine what channels we need to fetch */
383 for (int i = 0; i < num_channels; i++) {
384 if (channels[i].chan_idx != 0) {
385 LOG_ERR("Only channel index 0 supported");
386 rtio_iodev_sqe_err(iodev_sqe, -ENOTSUP);
387 return;
388 }
389 switch (channels[i].chan_type) {
390 case SENSOR_CHAN_ALL:
391 edata->has_accel = 1;
392 #ifdef CONFIG_BMA4XX_TEMPERATURE
393 edata->has_temp = 1;
394 #endif /* CONFIG_BMA4XX_TEMPERATURE */
395 break;
396 case SENSOR_CHAN_ACCEL_X:
397 case SENSOR_CHAN_ACCEL_Y:
398 case SENSOR_CHAN_ACCEL_Z:
399 case SENSOR_CHAN_ACCEL_XYZ:
400 edata->has_accel = 1;
401 break;
402 #ifdef CONFIG_BMA4XX_TEMPERATURE
403 case SENSOR_CHAN_DIE_TEMP:
404 edata->has_temp = 1;
405 break;
406 #endif /* CONFIG_BMA4XX_TEMPERATURE */
407 default:
408 LOG_ERR("Requested unsupported channel type %d, idx %d",
409 channels[i].chan_type, channels[i].chan_idx);
410 rtio_iodev_sqe_err(iodev_sqe, -ENOTSUP);
411 return;
412 }
413 }
414
415 if (edata->has_accel) {
416 rc = bma4xx_sample_fetch(dev, &edata->accel_xyz[0], &edata->accel_xyz[1],
417 &edata->accel_xyz[2]);
418 if (rc != 0) {
419 LOG_ERR("Failed to fetch accel samples");
420 rtio_iodev_sqe_err(iodev_sqe, rc);
421 return;
422 }
423 }
424
425 #ifdef CONFIG_BMA4XX_TEMPERATURE
426 if (edata->has_temp) {
427 rc = bma4xx_temp_fetch(dev, &edata->temp);
428 if (rc != 0) {
429 LOG_ERR("Failed to fetch temp sample");
430 rtio_iodev_sqe_err(iodev_sqe, rc);
431 return;
432 }
433 }
434 #endif /* CONFIG_BMA4XX_TEMPERATURE */
435
436 rtio_iodev_sqe_ok(iodev_sqe, 0);
437 }
438
bma4xx_submit_sync(struct rtio_iodev_sqe * iodev_sqe)439 static void bma4xx_submit_sync(struct rtio_iodev_sqe *iodev_sqe)
440 {
441 const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data;
442 const struct device *dev = cfg->sensor;
443
444 /* TODO: Add streaming support */
445 if (!cfg->is_streaming) {
446 bma4xx_submit_one_shot(dev, iodev_sqe);
447 } else {
448 rtio_iodev_sqe_err(iodev_sqe, -ENOTSUP);
449 }
450 }
451
bma4xx_submit(const struct device * dev,struct rtio_iodev_sqe * iodev_sqe)452 static void bma4xx_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe)
453 {
454 struct rtio_work_req *req = rtio_work_req_alloc();
455
456 if (req == NULL) {
457 LOG_ERR("RTIO work item allocation failed. Consider to increase "
458 "CONFIG_RTIO_WORKQ_POOL_ITEMS.");
459 rtio_iodev_sqe_err(iodev_sqe, -ENOMEM);
460 return;
461 }
462
463 rtio_work_req_submit(req, iodev_sqe, bma4xx_submit_sync);
464 }
465
466 /*
467 * RTIO decoder
468 */
469
bma4xx_decoder_get_frame_count(const uint8_t * buffer,struct sensor_chan_spec ch,uint16_t * frame_count)470 static int bma4xx_decoder_get_frame_count(const uint8_t *buffer, struct sensor_chan_spec ch,
471 uint16_t *frame_count)
472 {
473 const struct bma4xx_encoded_data *edata = (const struct bma4xx_encoded_data *)buffer;
474 const struct bma4xx_decoder_header *header = &edata->header;
475
476 if (ch.chan_idx != 0) {
477 return -ENOTSUP;
478 }
479
480 if (!header->is_fifo) {
481 switch (ch.chan_type) {
482 case SENSOR_CHAN_ACCEL_X:
483 case SENSOR_CHAN_ACCEL_Y:
484 case SENSOR_CHAN_ACCEL_Z:
485 case SENSOR_CHAN_ACCEL_XYZ:
486 *frame_count = edata->has_accel ? 1 : 0;
487 return 0;
488 case SENSOR_CHAN_DIE_TEMP:
489 *frame_count = edata->has_temp ? 1 : 0;
490 return 0;
491 default:
492 return -ENOTSUP;
493 }
494 return 0;
495 }
496
497 /* FIFO (streaming) mode operation is not yet supported */
498 return -ENOTSUP;
499 }
500
bma4xx_decoder_get_size_info(struct sensor_chan_spec ch,size_t * base_size,size_t * frame_size)501 static int bma4xx_decoder_get_size_info(struct sensor_chan_spec ch, size_t *base_size,
502 size_t *frame_size)
503 {
504 switch (ch.chan_type) {
505 case SENSOR_CHAN_ACCEL_X:
506 case SENSOR_CHAN_ACCEL_Y:
507 case SENSOR_CHAN_ACCEL_Z:
508 case SENSOR_CHAN_ACCEL_XYZ:
509 *base_size = sizeof(struct sensor_three_axis_data);
510 *frame_size = sizeof(struct sensor_three_axis_sample_data);
511 return 0;
512 case SENSOR_CHAN_DIE_TEMP:
513 *base_size = sizeof(struct sensor_q31_data);
514 *frame_size = sizeof(struct sensor_q31_sample_data);
515 return 0;
516 default:
517 return -ENOTSUP;
518 }
519 }
520
bma4xx_get_shift(struct sensor_chan_spec ch,uint8_t accel_fs,int8_t * shift)521 static int bma4xx_get_shift(struct sensor_chan_spec ch, uint8_t accel_fs, int8_t *shift)
522 {
523 switch (ch.chan_type) {
524 case SENSOR_CHAN_ACCEL_X:
525 case SENSOR_CHAN_ACCEL_Y:
526 case SENSOR_CHAN_ACCEL_Z:
527 case SENSOR_CHAN_ACCEL_XYZ:
528 switch (accel_fs) {
529 case BMA4XX_RANGE_2G:
530 /* 2 G's = 19.62 m/s^2. Use shift of 5 (+/-32) */
531 *shift = 5;
532 return 0;
533 case BMA4XX_RANGE_4G:
534 *shift = 6;
535 return 0;
536 case BMA4XX_RANGE_8G:
537 *shift = 7;
538 return 0;
539 case BMA4XX_RANGE_16G:
540 *shift = 8;
541 return 0;
542 default:
543 return -EINVAL;
544 }
545 case SENSOR_CHAN_DIE_TEMP:
546 *shift = BMA4XX_TEMP_SHIFT;
547 return 0;
548 default:
549 return -EINVAL;
550 }
551 }
552
bma4xx_convert_raw_accel_to_q31(int16_t raw_val,q31_t * out)553 static void bma4xx_convert_raw_accel_to_q31(int16_t raw_val, q31_t *out)
554 {
555 /* The full calculation is (assuming floating math):
556 * value_ms2 = raw_value * range * 9.8065 / BIT(11)
557 * We can treat 'range * 9.8065' as a scale, the scale is calculated by first getting 1g
558 * represented as a q31 value with the same shift as our result:
559 * 1g = (9.8065 * BIT(31)) >> shift
560 * Next, we need to multiply it by our range in g, which for this driver is one of
561 * [2, 4, 8, 16] and maps to a left shift of [1, 2, 3, 4]:
562 * 1g <<= log2(range)
563 * Note we used a right shift by 'shift' and left shift by log2(range). 'shift' is
564 * [5, 6, 7, 8] for range values [2, 4, 8, 16] since it's the final shift in m/s2. It is
565 * calculated via:
566 * shift = ceil(log2(range * 9.8065))
567 * This means that we can shorten the above 1g alterations to:
568 * 1g = (1g >> ceil(log2(range * 9.8065))) << log2(range)
569 * For the range values [2, 4, 8, 16], the following is true:
570 * (x >> ceil(log2(range * 9.8065))) << log2(range)
571 * = x >> 4
572 * Since the range cancels out in the right and left shift, we've now reduced the following:
573 * range * 9.8065 = 9.8065 * BIT(31 - 4)
574 * All that's left is to divide by the bma4xx's maximum range BIT(11).
575 */
576 const int64_t scale = (int64_t)(9.8065 * BIT64(31 - 4));
577
578 *out = CLAMP(((int64_t)raw_val * scale) >> 11, INT32_MIN, INT32_MAX);
579 }
580
581 #ifdef CONFIG_BMA4XX_TEMPERATURE
582 /**
583 * @brief Convert the 8-bit temp register value into a Q31 celsius value
584 */
bma4xx_convert_raw_temp_to_q31(int8_t raw_val,q31_t * out)585 static void bma4xx_convert_raw_temp_to_q31(int8_t raw_val, q31_t *out)
586 {
587 /* Value of 0 equals 23 degrees C. Each bit count equals 1 degree C */
588
589 int64_t intermediate =
590 ((int64_t)raw_val + 23) * ((int64_t)INT32_MAX + 1) / (1 << BMA4XX_TEMP_SHIFT);
591
592 *out = CLAMP(intermediate, INT32_MIN, INT32_MAX);
593 }
594 #endif /* CONFIG_BMA4XX_TEMPERATURE */
595
bma4xx_one_shot_decode(const uint8_t * buffer,struct sensor_chan_spec ch,uint32_t * fit,uint16_t max_count,void * data_out)596 static int bma4xx_one_shot_decode(const uint8_t *buffer, struct sensor_chan_spec ch,
597 uint32_t *fit, uint16_t max_count, void *data_out)
598 {
599 const struct bma4xx_encoded_data *edata = (const struct bma4xx_encoded_data *)buffer;
600 const struct bma4xx_decoder_header *header = &edata->header;
601 int rc;
602
603 if (*fit != 0) {
604 return 0;
605 }
606 if (max_count == 0 || ch.chan_idx != 0) {
607 return -EINVAL;
608 }
609
610 switch (ch.chan_type) {
611 case SENSOR_CHAN_ACCEL_X:
612 case SENSOR_CHAN_ACCEL_Y:
613 case SENSOR_CHAN_ACCEL_Z:
614 case SENSOR_CHAN_ACCEL_XYZ: {
615 if (!edata->has_accel) {
616 return -ENODATA;
617 }
618
619 struct sensor_three_axis_data *out = (struct sensor_three_axis_data *)data_out;
620
621 out->header.base_timestamp_ns = edata->header.timestamp;
622 out->header.reading_count = 1;
623 rc = bma4xx_get_shift((struct sensor_chan_spec){.chan_type = SENSOR_CHAN_ACCEL_XYZ,
624 .chan_idx = 0},
625 header->accel_fs, &out->shift);
626 if (rc != 0) {
627 return -EINVAL;
628 }
629
630 bma4xx_convert_raw_accel_to_q31(edata->accel_xyz[0], &out->readings[0].x);
631 bma4xx_convert_raw_accel_to_q31(edata->accel_xyz[1], &out->readings[0].y);
632 bma4xx_convert_raw_accel_to_q31(edata->accel_xyz[2], &out->readings[0].z);
633
634 *fit = 1;
635 return 1;
636 }
637 #ifdef CONFIG_BMA4XX_TEMPERATURE
638 case SENSOR_CHAN_DIE_TEMP: {
639 if (!edata->has_temp) {
640 return -ENODATA;
641 }
642
643 struct sensor_q31_data *out = (struct sensor_q31_data *)data_out;
644
645 out->header.base_timestamp_ns = edata->header.timestamp;
646 out->header.reading_count = 1;
647 rc = bma4xx_get_shift(SENSOR_CHAN_DIE_TEMP, 0, &out->shift);
648 if (rc != 0) {
649 return -EINVAL;
650 }
651
652 bma4xx_convert_raw_temp_to_q31(edata->temp, &out->readings[0].temperature);
653
654 *fit = 1;
655 return 1;
656 }
657 #endif /* CONFIG_BMA4XX_TEMPERATURE */
658 default:
659 return -EINVAL;
660 }
661 }
662
bma4xx_decoder_decode(const uint8_t * buffer,struct sensor_chan_spec ch,uint32_t * fit,uint16_t max_count,void * data_out)663 static int bma4xx_decoder_decode(const uint8_t *buffer, struct sensor_chan_spec ch,
664 uint32_t *fit, uint16_t max_count,
665 void *data_out)
666 {
667 const struct bma4xx_decoder_header *header = (const struct bma4xx_decoder_header *)buffer;
668
669 if (header->is_fifo) {
670 /* FIFO (streaming) mode operation is not yet supported */
671 return -ENOTSUP;
672 }
673
674 return bma4xx_one_shot_decode(buffer, ch, fit, max_count, data_out);
675 }
676
677 SENSOR_DECODER_API_DT_DEFINE() = {
678 .get_frame_count = bma4xx_decoder_get_frame_count,
679 .get_size_info = bma4xx_decoder_get_size_info,
680 .decode = bma4xx_decoder_decode,
681 };
682
bma4xx_get_decoder(const struct device * dev,const struct sensor_decoder_api ** decoder)683 static int bma4xx_get_decoder(const struct device *dev, const struct sensor_decoder_api **decoder)
684 {
685 ARG_UNUSED(dev);
686 *decoder = &SENSOR_DECODER_NAME();
687
688 return 0;
689 }
690
691 /*
692 * Sensor driver API
693 */
694
695 static DEVICE_API(sensor, bma4xx_driver_api) = {
696 .attr_set = bma4xx_attr_set,
697 .submit = bma4xx_submit,
698 .get_decoder = bma4xx_get_decoder,
699 };
700
701 /*
702 * Device instantiation macros
703 */
704
705 /* Initializes a struct bma4xx_config for an instance on a SPI bus.
706 * SPI operation is not currently supported.
707 */
708
709 #define BMA4XX_CONFIG_SPI(inst) \
710 { \
711 .bus_cfg.spi = SPI_DT_SPEC_INST_GET(inst, 0, 0), .bus_init = &bma_spi_init, \
712 }
713
714 /* Initializes a struct bma4xx_config for an instance on an I2C bus. */
715 #define BMA4XX_CONFIG_I2C(inst) \
716 { \
717 .bus_cfg.i2c = I2C_DT_SPEC_INST_GET(inst), .bus_init = &bma4xx_i2c_init, \
718 }
719
720 /*
721 * Main instantiation macro, which selects the correct bus-specific
722 * instantiation macros for the instance.
723 */
724 #define BMA4XX_DEFINE(inst) \
725 static struct bma4xx_data bma4xx_data_##inst; \
726 static const struct bma4xx_config bma4xx_config_##inst = COND_CODE_1( \
727 DT_INST_ON_BUS(inst, spi), (BMA4XX_CONFIG_SPI(inst)), (BMA4XX_CONFIG_I2C(inst))); \
728 \
729 SENSOR_DEVICE_DT_INST_DEFINE(inst, bma4xx_chip_init, NULL, &bma4xx_data_##inst, \
730 &bma4xx_config_##inst, POST_KERNEL, \
731 CONFIG_SENSOR_INIT_PRIORITY, &bma4xx_driver_api);
732
733 DT_INST_FOREACH_STATUS_OKAY(BMA4XX_DEFINE)
734
