1 /*
2 * Copyright (c) 2024 Cienet
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #define DT_DRV_COMPAT bosch_bma4xx
8
9 #include "bma4xx_decoder.h"
10 #include "bma4xx_defs.h"
11 #include "bma4xx.h"
12 #include <errno.h>
13 #include <zephyr/sys/byteorder.h>
14
15 #include <zephyr/logging/log.h>
16 LOG_MODULE_DECLARE(bma4xx, CONFIG_SENSOR_LOG_LEVEL);
17
18 #define IS_ACCEL(chan) \
19 ((chan) == SENSOR_CHAN_ACCEL_X || (chan) == SENSOR_CHAN_ACCEL_Y || \
20 (chan) == SENSOR_CHAN_ACCEL_Z || (chan) == SENSOR_CHAN_ACCEL_XYZ)
21
22 #ifdef CONFIG_BMA4XX_STREAM
23
24 static const uint64_t accel_period_ns[] = {
25 [BMA4XX_ODR_0_78125] = UINT64_C(100000000000000) / 78125,
26 [BMA4XX_ODR_1_5625] = UINT64_C(10000000000000) / 15625,
27 [BMA4XX_ODR_3_125] = UINT64_C(10000000000000) / 31250,
28 [BMA4XX_ODR_6_25] = UINT64_C(10000000000000) / 62500,
29 [BMA4XX_ODR_12_5] = UINT64_C(1000000000000) / 12500,
30 [BMA4XX_ODR_25] = UINT64_C(1000000000) / 25,
31 [BMA4XX_ODR_50] = UINT64_C(1000000000) / 50,
32 [BMA4XX_ODR_100] = UINT64_C(1000000000) / 100,
33 [BMA4XX_ODR_200] = UINT64_C(1000000000) / 200,
34 [BMA4XX_ODR_400] = UINT64_C(1000000000) / 400,
35 [BMA4XX_ODR_800] = UINT64_C(1000000000) / 800,
36 [BMA4XX_ODR_1600] = UINT64_C(10000000) / 16,
37 [BMA4XX_ODR_3200] = UINT64_C(10000000) / 32,
38 [BMA4XX_ODR_6400] = UINT64_C(10000000) / 64,
39 [BMA4XX_ODR_12800] = UINT64_C(10000000) / 128,
40 };
41
42 #endif /* CONFIG_BMA4XX_STREAM */
43
44 /*
45 * RTIO decoder
46 */
47
bma4xx_decoder_get_frame_count(const uint8_t * buffer,struct sensor_chan_spec ch,uint16_t * frame_count)48 static int bma4xx_decoder_get_frame_count(const uint8_t *buffer, struct sensor_chan_spec ch,
49 uint16_t *frame_count)
50 {
51 const struct bma4xx_fifo_data *edata = (const struct bma4xx_fifo_data *)buffer;
52 const struct bma4xx_decoder_header *header = &edata->header;
53
54 if (ch.chan_idx != 0) {
55 return -ENOTSUP;
56 }
57
58 if (!header->is_fifo) {
59 switch (ch.chan_type) {
60 case SENSOR_CHAN_ACCEL_X:
61 case SENSOR_CHAN_ACCEL_Y:
62 case SENSOR_CHAN_ACCEL_Z:
63 case SENSOR_CHAN_ACCEL_XYZ:
64 case SENSOR_CHAN_DIE_TEMP:
65 *frame_count = 1;
66 return 0;
67 default:
68 return -ENOTSUP;
69 }
70 }
71
72 if (!IS_ACCEL(ch.chan_type)) {
73 return -ENOTSUP;
74 }
75 /* Skip the header */
76 buffer += sizeof(struct bma4xx_fifo_data);
77
78 uint16_t count = 0;
79 const uint8_t *end = buffer + edata->fifo_count;
80
81 while (buffer < end) {
82 uint8_t size = BMA4XX_FIFO_HEADER_LENGTH;
83 const bool has_accel = FIELD_GET(BMA4XX_BIT_FIFO_HEADER_ACCEL, buffer[0]) == 1;
84 const bool has_aux = FIELD_GET(BMA4XX_BIT_FIFO_HEADER_AUX, buffer[0]) == 1;
85
86 if (FIELD_GET(BMA4XX_BIT_FIFO_HEADER_REGULAR, buffer[0])) {
87 if (has_accel && has_aux) {
88 size += BMA4XX_FIFO_MA_LENGTH;
89 } else if (has_accel) {
90 size += BMA4XX_FIFO_A_LENGTH;
91 } else if (has_aux) {
92 size += BMA4XX_FIFO_M_LENGTH;
93 }
94 ++count;
95 } else if (FIELD_GET(BMA4XX_BIT_FIFO_HEADER_CONTROL, buffer[0])) {
96 if (FIELD_GET(BMA4XX_BIT_FIFO_HEADER_SENSORTIME, buffer[0])) {
97 size += BMA4XX_FIFO_ST_LENGTH;
98 } else if (FIELD_GET(BMA4XX_BIT_FIFO_HEAD_OVER_READ_MSB, buffer[0])) {
99 size = *end;
100 } else {
101 size += BMA4XX_FIFO_CF_LENGTH;
102 }
103 }
104
105 buffer += size;
106 }
107
108 *frame_count = count;
109
110 return 0;
111 }
112
bma4xx_decoder_get_size_info(struct sensor_chan_spec ch,size_t * base_size,size_t * frame_size)113 static int bma4xx_decoder_get_size_info(struct sensor_chan_spec ch, size_t *base_size,
114 size_t *frame_size)
115 {
116 switch (ch.chan_type) {
117 case SENSOR_CHAN_ACCEL_X:
118 case SENSOR_CHAN_ACCEL_Y:
119 case SENSOR_CHAN_ACCEL_Z:
120 case SENSOR_CHAN_ACCEL_XYZ:
121 *base_size = sizeof(struct sensor_three_axis_data);
122 *frame_size = sizeof(struct sensor_three_axis_sample_data);
123 return 0;
124 case SENSOR_CHAN_DIE_TEMP:
125 *base_size = sizeof(struct sensor_q31_data);
126 *frame_size = sizeof(struct sensor_q31_sample_data);
127 return 0;
128 default:
129 return -ENOTSUP;
130 }
131 }
132
bma4xx_get_shift(struct sensor_chan_spec ch,uint8_t accel_fs,int8_t * shift)133 static int bma4xx_get_shift(struct sensor_chan_spec ch, uint8_t accel_fs, int8_t *shift)
134 {
135 switch (ch.chan_type) {
136 case SENSOR_CHAN_ACCEL_X:
137 case SENSOR_CHAN_ACCEL_Y:
138 case SENSOR_CHAN_ACCEL_Z:
139 case SENSOR_CHAN_ACCEL_XYZ:
140 switch (accel_fs) {
141 case BMA4XX_RANGE_2G:
142 /* 2 G's = 19.62 m/s^2. Use shift of 5 (+/-32) */
143 *shift = 5;
144 return 0;
145 case BMA4XX_RANGE_4G:
146 *shift = 6;
147 return 0;
148 case BMA4XX_RANGE_8G:
149 *shift = 7;
150 return 0;
151 case BMA4XX_RANGE_16G:
152 *shift = 8;
153 return 0;
154 default:
155 return -EINVAL;
156 }
157 case SENSOR_CHAN_DIE_TEMP:
158 *shift = BMA4XX_TEMP_SHIFT;
159 return 0;
160 default:
161 return -EINVAL;
162 }
163 }
164
bma4xx_convert_raw_accel_to_q31(int16_t raw_val,q31_t * out)165 static void bma4xx_convert_raw_accel_to_q31(int16_t raw_val, q31_t *out)
166 {
167 /* The full calculation is (assuming floating math):
168 * value_ms2 = raw_value * range * 9.80665 / BIT(11)
169 * We can treat 'range * 9.80665' as a scale, the scale is calculated by first getting 1g
170 * represented as a q31 value with the same shift as our result:
171 * 1g = (9.80665 * BIT(31)) >> shift
172 * Next, we need to multiply it by our range in g, which for this driver is one of
173 * [2, 4, 8, 16] and maps to a left shift of [1, 2, 3, 4]:
174 * 1g <<= log2(range)
175 * Note we used a right shift by 'shift' and left shift by log2(range). 'shift' is
176 * [5, 6, 7, 8] for range values [2, 4, 8, 16] since it's the final shift in m/s2. It is
177 * calculated via:
178 * shift = ceil(log2(range * 9.80665))
179 * This means that we can shorten the above 1g alterations to:
180 * 1g = (1g >> ceil(log2(range * 9.80665))) << log2(range)
181 * For the range values [2, 4, 8, 16], the following is true:
182 * (x >> ceil(log2(range * 9.80665))) << log2(range)
183 * = x >> 4
184 * Since the range cancels out in the right and left shift, we've now reduced the following:
185 * range * 9.80665 = 9.80665 * BIT(31 - 4)
186 * All that's left is to divide by the bma4xx's maximum range BIT(11).
187 */
188
189 int16_t value = (int16_t)sys_le16_to_cpu(raw_val << 4) >> 4;
190
191 const int64_t scale = (int64_t)(9.80665 * BIT64(31 - 4));
192
193 *out = CLAMP(((int64_t)value * scale) >> 11, INT32_MIN, INT32_MAX);
194 }
195
196 #ifdef CONFIG_BMA4XX_STREAM
197
bma4xx_unpack_accel_data(const uint8_t * pkt,uint8_t data_start_index,q31_t * out)198 static void bma4xx_unpack_accel_data(const uint8_t *pkt, uint8_t data_start_index, q31_t *out)
199 {
200 uint8_t offset = BMA4XX_FIFO_HEADER_LENGTH + (data_start_index * 2);
201 const bool has_aux = FIELD_GET(BMA4XX_BIT_FIFO_HEADER_AUX, pkt[0]) == 1;
202
203 if (has_aux) {
204 offset += BMA4XX_FIFO_M_LENGTH;
205 }
206
207 int16_t value = (pkt[offset + 1] << 4) | FIELD_GET(GENMASK(7, 4), pkt[offset]);
208
209 bma4xx_convert_raw_accel_to_q31((int16_t)value, out);
210 }
211
212 #endif /* CONFIG_BMA4XX_STREAM */
213
214 #ifdef CONFIG_BMA4XX_TEMPERATURE
215 /**
216 * @brief Convert the 8-bit temp register value into a Q31 celsius value
217 */
bma4xx_convert_raw_temp_to_q31(int8_t raw_val,q31_t * out)218 static void bma4xx_convert_raw_temp_to_q31(int8_t raw_val, q31_t *out)
219 {
220 /* Value of 0 equals 23 degrees C. Each bit count equals 1 degree C */
221
222 int64_t intermediate =
223 ((int64_t)raw_val + 23) * ((int64_t)INT32_MAX + 1) / (1 << BMA4XX_TEMP_SHIFT);
224
225 *out = CLAMP(intermediate, INT32_MIN, INT32_MAX);
226 }
227 #endif /* CONFIG_BMA4XX_TEMPERATURE */
228
bma4xx_one_shot_decode(const uint8_t * buffer,struct sensor_chan_spec ch,uint32_t * fit,uint16_t max_count,void * data_out)229 static int bma4xx_one_shot_decode(const uint8_t *buffer, struct sensor_chan_spec ch, uint32_t *fit,
230 uint16_t max_count, void *data_out)
231 {
232 const struct bma4xx_encoded_data *edata = (const struct bma4xx_encoded_data *)buffer;
233 const struct bma4xx_decoder_header *header = &edata->header;
234 int16_t raw_x, raw_y, raw_z;
235 int rc;
236
237 if (*fit != 0) {
238 return 0;
239 }
240 if (max_count == 0 || ch.chan_idx != 0) {
241 return -EINVAL;
242 }
243
244 switch (ch.chan_type) {
245 case SENSOR_CHAN_ACCEL_X:
246 case SENSOR_CHAN_ACCEL_Y:
247 case SENSOR_CHAN_ACCEL_Z:
248 case SENSOR_CHAN_ACCEL_XYZ: {
249 struct sensor_three_axis_data *out = (struct sensor_three_axis_data *)data_out;
250
251 out->header.base_timestamp_ns = edata->header.timestamp;
252 out->header.reading_count = 1;
253 rc = bma4xx_get_shift((struct sensor_chan_spec){.chan_type = SENSOR_CHAN_ACCEL_XYZ,
254 .chan_idx = 3},
255 header->accel_fs, &out->shift);
256 if (rc != 0) {
257 return -EINVAL;
258 }
259
260 raw_x = (((int16_t)edata->accel_xyz_raw_data[1]) << 4) |
261 FIELD_GET(GENMASK(7, 4), edata->accel_xyz_raw_data[0]);
262 raw_y = (((int16_t)edata->accel_xyz_raw_data[3]) << 4) |
263 FIELD_GET(GENMASK(7, 4), edata->accel_xyz_raw_data[2]);
264 raw_z = (((int16_t)edata->accel_xyz_raw_data[5]) << 4) |
265 FIELD_GET(GENMASK(7, 4), edata->accel_xyz_raw_data[4]);
266
267 bma4xx_convert_raw_accel_to_q31(raw_x, &out->readings[0].x);
268 bma4xx_convert_raw_accel_to_q31(raw_y, &out->readings[0].y);
269 bma4xx_convert_raw_accel_to_q31(raw_z, &out->readings[0].z);
270
271 *fit = 1;
272 return 1;
273 }
274 #ifdef CONFIG_BMA4XX_TEMPERATURE
275 case SENSOR_CHAN_DIE_TEMP: {
276 struct sensor_q31_data *out = (struct sensor_q31_data *)data_out;
277
278 out->header.base_timestamp_ns = edata->header.timestamp;
279 out->header.reading_count = 1;
280 rc = bma4xx_get_shift((struct sensor_chan_spec){.chan_type = SENSOR_CHAN_DIE_TEMP,
281 .chan_idx = 12},
282 0, &out->shift);
283
284 if (rc != 0) {
285 return -EINVAL;
286 }
287
288 bma4xx_convert_raw_temp_to_q31(edata->temp, &out->readings[0].temperature);
289
290 *fit = 1;
291 return 1;
292 }
293 #endif /* CONFIG_BMA4XX_TEMPERATURE */
294 default:
295 return -EINVAL;
296 }
297 }
298
299 #ifdef CONFIG_BMA4XX_STREAM
300
bma4xx_fifo_decode(const uint8_t * buffer,struct sensor_chan_spec ch,uint32_t * fit,uint16_t max_count,void * data_out)301 static int bma4xx_fifo_decode(const uint8_t *buffer, struct sensor_chan_spec ch, uint32_t *fit,
302 uint16_t max_count, void *data_out)
303 {
304 const struct bma4xx_fifo_data *edata = (const struct bma4xx_fifo_data *)buffer;
305 const uint8_t *buffer_end = buffer + sizeof(struct bma4xx_fifo_data) + edata->fifo_count;
306 int accel_frame_count = 0;
307 int count = 0;
308
309 if ((uintptr_t)buffer_end <= *fit || ch.chan_idx != 0) {
310 return 0;
311 }
312
313 if (!IS_ACCEL(ch.chan_type)) {
314 return -ENOTSUP;
315 }
316
317 ((struct sensor_data_header *)data_out)->base_timestamp_ns = edata->header.timestamp;
318
319 buffer += sizeof(struct bma4xx_fifo_data);
320
321 while (count < max_count && buffer < buffer_end) {
322 const bool has_accel = FIELD_GET(BMA4XX_BIT_FIFO_HEADER_ACCEL, buffer[0]) == 1;
323 const bool has_aux = FIELD_GET(BMA4XX_BIT_FIFO_HEADER_AUX, buffer[0]) == 1;
324 const uint8_t *frame_end = buffer;
325
326 if (FIELD_GET(BMA4XX_BIT_FIFO_HEADER_REGULAR, buffer[0])) {
327 if (has_accel && has_aux) {
328 frame_end += BMA4XX_FIFO_MA_LENGTH + BMA4XX_FIFO_HEADER_LENGTH;
329 accel_frame_count++;
330 } else if (has_accel) {
331 frame_end += BMA4XX_FIFO_A_LENGTH + BMA4XX_FIFO_HEADER_LENGTH;
332 accel_frame_count++;
333 } else if (has_aux) {
334 frame_end += BMA4XX_FIFO_M_LENGTH + BMA4XX_FIFO_HEADER_LENGTH;
335 }
336 } else if (FIELD_GET(BMA4XX_BIT_FIFO_HEADER_CONTROL, buffer[0])) {
337 if (FIELD_GET(BMA4XX_BIT_FIFO_HEADER_SENSORTIME, buffer[0])) {
338 frame_end += BMA4XX_FIFO_ST_LENGTH + BMA4XX_FIFO_HEADER_LENGTH;
339 } else if (FIELD_GET(BMA4XX_BIT_FIFO_HEAD_OVER_READ_MSB, buffer[0])) {
340 frame_end = buffer_end;
341 } else {
342 frame_end += BMA4XX_FIFO_CF_LENGTH + BMA4XX_FIFO_HEADER_LENGTH;
343 }
344 }
345
346 if ((uintptr_t)buffer < *fit) {
347 /* This frame was already decoded, move on to the next frame */
348 buffer = frame_end;
349 continue;
350 }
351
352 if (has_accel) {
353 struct sensor_three_axis_data *data =
354 (struct sensor_three_axis_data *)data_out;
355
356 uint64_t period_ns = accel_period_ns[edata->accel_odr];
357
358 bma4xx_get_shift(
359 (struct sensor_chan_spec){.chan_type = SENSOR_CHAN_ACCEL_XYZ,
360 .chan_idx = 3},
361 edata->header.accel_fs, &data->shift);
362 data->readings[count].timestamp_delta = (accel_frame_count - 1) * period_ns;
363 bma4xx_unpack_accel_data(buffer, 0, &data->readings[count].x);
364 bma4xx_unpack_accel_data(buffer, 1, &data->readings[count].y);
365 bma4xx_unpack_accel_data(buffer, 2, &data->readings[count].z);
366 }
367
368 buffer = frame_end;
369 *fit = (uintptr_t)frame_end;
370 count++;
371 }
372
373 return count;
374 }
375
376 #endif /* CONFIG_BMA4XX_STREAM */
377
bma4xx_decoder_decode(const uint8_t * buffer,struct sensor_chan_spec ch,uint32_t * fit,uint16_t max_count,void * data_out)378 static int bma4xx_decoder_decode(const uint8_t *buffer, struct sensor_chan_spec ch, uint32_t *fit,
379 uint16_t max_count, void *data_out)
380 {
381 #ifdef CONFIG_BMA4XX_STREAM
382
383 const struct bma4xx_decoder_header *header = (const struct bma4xx_decoder_header *)buffer;
384
385 if (header->is_fifo) {
386 return bma4xx_fifo_decode(buffer, ch, fit, max_count, data_out);
387 }
388 #endif
389
390 return bma4xx_one_shot_decode(buffer, ch, fit, max_count, data_out);
391 }
392
bma4xx_decoder_has_trigger(const uint8_t * buffer,enum sensor_trigger_type trigger)393 static bool bma4xx_decoder_has_trigger(const uint8_t *buffer, enum sensor_trigger_type trigger)
394 {
395 return false;
396 }
397
398 SENSOR_DECODER_API_DT_DEFINE() = {
399 .get_frame_count = bma4xx_decoder_get_frame_count,
400 .get_size_info = bma4xx_decoder_get_size_info,
401 .decode = bma4xx_decoder_decode,
402 .has_trigger = bma4xx_decoder_has_trigger,
403 };
404
bma4xx_get_decoder(const struct device * dev,const struct sensor_decoder_api ** decoder)405 int bma4xx_get_decoder(const struct device *dev, const struct sensor_decoder_api **decoder)
406 {
407 ARG_UNUSED(dev);
408 *decoder = &SENSOR_DECODER_NAME();
409
410 return 0;
411 }
412