1 /*
2 * Copyright (c) 2024 Analog Devices Inc.
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include "adxl362.h"
8 #include <zephyr/sys/byteorder.h>
9
10 #ifdef CONFIG_ADXL362_STREAM
11
12 /* (2^31 / 2^8(shift) */
13 #define ADXL362_TEMP_QSCALE 8388608
14 #define ADXL362_TEMP_LSB_PER_C 15
15
16 #define ADXL362_COMPLEMENT 0xf000
17
18 static const uint32_t accel_period_ns[] = {
19 [ADXL362_ODR_12_5_HZ] = UINT32_C(10000000000) / 125,
20 [ADXL362_ODR_25_HZ] = UINT32_C(1000000000) / 25,
21 [ADXL362_ODR_50_HZ] = UINT32_C(1000000000) / 50,
22 [ADXL362_ODR_100_HZ] = UINT32_C(1000000000) / 100,
23 [ADXL362_ODR_200_HZ] = UINT32_C(1000000000) / 200,
24 [ADXL362_ODR_400_HZ] = UINT32_C(1000000000) / 400,
25 };
26
27 static const uint32_t range_to_shift[] = {
28 [ADXL362_RANGE_2G] = 5,
29 [ADXL362_RANGE_4G] = 6,
30 [ADXL362_RANGE_8G] = 7,
31 };
32
33 /* (1 / sensitivity) * (pow(2,31) / pow(2,shift)) * (unit_scaler) */
34 static const uint32_t qscale_factor[] = {
35 /* (1.0 / ADXL362_ACCEL_2G_LSB_PER_G) * (2^31 / 2^5) * SENSOR_G / 1000000 */
36 [ADXL362_RANGE_2G] = UINT32_C(658338),
37 /* (1.0 / ADXL362_ACCEL_4G_LSB_PER_G) * (2^31 / 2^6) * SENSOR_G / 1000000 */
38 [ADXL362_RANGE_4G] = UINT32_C(658338),
39 /* (1.0 / ADXL362_ACCEL_8G_LSB_PER_G) * (2^31 / 2^7) ) * SENSOR_G / 1000000 */
40 [ADXL362_RANGE_8G] = UINT32_C(700360),
41 };
42
adxl362_temp_convert_q31(q31_t * out,int16_t data_in)43 static inline void adxl362_temp_convert_q31(q31_t *out, int16_t data_in)
44 {
45 /* See sensitivity and bias specifications in table 1 of datasheet */
46 data_in &= 0xFFF;
47
48 if (data_in & BIT(11)) {
49 data_in |= ADXL362_COMPLEMENT;
50 }
51
52 *out = ((data_in - ADXL362_TEMP_BIAS_LSB) / ADXL362_TEMP_LSB_PER_C
53 + ADXL362_TEMP_BIAS_TEST_CONDITION) * ADXL362_TEMP_QSCALE;
54 }
55
adxl362_accel_convert_q31(q31_t * out,int16_t data_in,int32_t range)56 static inline void adxl362_accel_convert_q31(q31_t *out, int16_t data_in, int32_t range)
57 {
58 data_in &= 0xFFF;
59
60 if (data_in & BIT(11)) {
61 data_in |= ADXL362_COMPLEMENT;
62 }
63
64 *out = data_in * qscale_factor[range];
65 }
66
adxl362_decode_stream(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)67 static int adxl362_decode_stream(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
68 uint32_t *fit, uint16_t max_count, void *data_out)
69 {
70 const struct adxl362_fifo_data *enc_data = (const struct adxl362_fifo_data *)buffer;
71 const uint8_t *buffer_end =
72 buffer + sizeof(struct adxl362_fifo_data) + enc_data->fifo_byte_count;
73 int count = 0;
74 uint8_t sample_num = 0;
75 int16_t data_in;
76
77 if ((uintptr_t)buffer_end <= *fit || chan_spec.chan_idx != 0) {
78 return 0;
79 }
80
81 buffer += sizeof(struct adxl362_fifo_data);
82
83 uint8_t sample_set_size = 6;
84
85 if (enc_data->has_tmp) {
86 sample_set_size = 8;
87 }
88
89 uint64_t period_ns = accel_period_ns[enc_data->accel_odr];
90
91 /* Calculate which sample is decoded. */
92 if ((uint8_t *)*fit >= buffer) {
93 sample_num = ((uint8_t *)*fit - buffer) / sample_set_size;
94 }
95
96 while (count < max_count && buffer < buffer_end) {
97 const uint8_t *sample_end = buffer;
98
99 sample_end += sample_set_size;
100
101 if ((uintptr_t)buffer < *fit) {
102 /* This frame was already decoded, move on to the next frame */
103 buffer = sample_end;
104 continue;
105 }
106
107 if (chan_spec.chan_type == SENSOR_CHAN_DIE_TEMP) {
108 if (enc_data->has_tmp) {
109 struct sensor_q31_data *data = (struct sensor_q31_data *)data_out;
110
111 memset(data, 0, sizeof(struct sensor_three_axis_data));
112 data->header.base_timestamp_ns = enc_data->timestamp;
113 data->header.reading_count = 1;
114 data->shift = 8;
115
116 data->readings[count].timestamp_delta =
117 period_ns * sample_num;
118
119 data_in = sys_le16_to_cpu(*((int16_t *)(buffer + 6)));
120
121 /* Check if this sample contains temperature value. */
122 if (ADXL362_FIFO_HDR_CHECK_TEMP(data_in)) {
123 adxl362_temp_convert_q31(&data->readings[count].temperature,
124 data_in);
125 }
126 }
127 } else {
128 struct sensor_three_axis_data *data =
129 (struct sensor_three_axis_data *)data_out;
130
131 memset(data, 0, sizeof(struct sensor_three_axis_data));
132 data->header.base_timestamp_ns = enc_data->timestamp;
133 data->header.reading_count = 1;
134 data->shift = range_to_shift[enc_data->selected_range];
135
136 switch (chan_spec.chan_type) {
137 case SENSOR_CHAN_ACCEL_X:
138 data->readings[count].timestamp_delta = sample_num
139 * period_ns;
140
141 /* Convert received data into signeg integer. */
142 data_in = sys_le16_to_cpu(*((int16_t *)buffer));
143
144 /* Check if this sample contains X value. */
145 if (ADXL362_FIFO_HDR_CHECK_ACCEL_X(data_in)) {
146 adxl362_accel_convert_q31(&data->readings[count].x,
147 data_in, enc_data->selected_range);
148 }
149 break;
150 case SENSOR_CHAN_ACCEL_Y:
151 data->readings[count].timestamp_delta = sample_num
152 * period_ns;
153
154 /* Convert received data into signeg integer. */
155 data_in = sys_le16_to_cpu(*((int16_t *)(buffer + 2)));
156
157 /* Check if this sample contains Y value. */
158 if (ADXL362_FIFO_HDR_CHECK_ACCEL_Y(data_in)) {
159 adxl362_accel_convert_q31(&data->readings[count].y,
160 data_in, enc_data->selected_range);
161 }
162 break;
163 case SENSOR_CHAN_ACCEL_Z:
164 data->readings[count].timestamp_delta = sample_num
165 * period_ns;
166 /* Convert received data into signeg integer. */
167 data_in = sys_le16_to_cpu(*((int16_t *)(buffer + 4)));
168
169 /* Check if this sample contains Y value. */
170 if (ADXL362_FIFO_HDR_CHECK_ACCEL_Z(data_in)) {
171 adxl362_accel_convert_q31(&data->readings[count].z,
172 data_in, enc_data->selected_range);
173 }
174 break;
175 case SENSOR_CHAN_ACCEL_XYZ:
176 data->readings[count].timestamp_delta = sample_num * period_ns;
177
178 /* Convert received data into signeg integer. */
179 data_in = sys_le16_to_cpu(*((int16_t *)buffer));
180
181 /* Check if this sample contains X value. */
182 if (ADXL362_FIFO_HDR_CHECK_ACCEL_X(data_in)) {
183 adxl362_accel_convert_q31(&data->readings[count].x,
184 data_in, enc_data->selected_range);
185 }
186
187 /* Convert received data into signeg integer. */
188 data_in = sys_le16_to_cpu(*((int16_t *)(buffer + 2)));
189
190 /* Check if this sample contains Y value. */
191 if (ADXL362_FIFO_HDR_CHECK_ACCEL_Y(data_in)) {
192 adxl362_accel_convert_q31(&data->readings[count].y,
193 data_in, enc_data->selected_range);
194 }
195
196 /* Convert received data into signeg integer. */
197 data_in = sys_le16_to_cpu(*((int16_t *)(buffer + 4)));
198
199 /* Check if this sample contains Z value. */
200 if (ADXL362_FIFO_HDR_CHECK_ACCEL_Z(data_in)) {
201 adxl362_accel_convert_q31(&data->readings[count].z,
202 data_in, enc_data->selected_range);
203 }
204 break;
205 default:
206 return -ENOTSUP;
207 }
208 }
209
210 buffer = sample_end;
211 *fit = (uintptr_t)sample_end;
212 count++;
213 }
214 return count;
215 }
216
217 #endif /* CONFIG_ADXL362_STREAM */
218
adxl362_decoder_get_frame_count(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint16_t * frame_count)219 static int adxl362_decoder_get_frame_count(const uint8_t *buffer,
220 struct sensor_chan_spec chan_spec,
221 uint16_t *frame_count)
222 {
223 int32_t ret = -ENOTSUP;
224
225 if (chan_spec.chan_idx != 0) {
226 return ret;
227 }
228
229 #ifdef CONFIG_ADXL362_STREAM
230 const struct adxl362_fifo_data *data = (const struct adxl362_fifo_data *)buffer;
231
232 if (!data->is_fifo) {
233 #endif /* CONFIG_ADXL362_STREAM */
234 switch (chan_spec.chan_type) {
235 case SENSOR_CHAN_ACCEL_X:
236 case SENSOR_CHAN_ACCEL_Y:
237 case SENSOR_CHAN_ACCEL_Z:
238 case SENSOR_CHAN_ACCEL_XYZ:
239 *frame_count = 1;
240 ret = 0;
241 break;
242
243 default:
244 break;
245 }
246 #ifdef CONFIG_ADXL362_STREAM
247 } else {
248 if (data->fifo_byte_count == 0) {
249 *frame_count = 0;
250 ret = 0;
251 } else {
252 switch (chan_spec.chan_type) {
253 case SENSOR_CHAN_ACCEL_X:
254 case SENSOR_CHAN_ACCEL_Y:
255 case SENSOR_CHAN_ACCEL_Z:
256 case SENSOR_CHAN_ACCEL_XYZ:
257 if (data->has_tmp) {
258 /*6 bytes for XYZ and 2 bytes for TEMP*/
259 *frame_count = data->fifo_byte_count / 8;
260 ret = 0;
261 } else {
262 *frame_count = data->fifo_byte_count / 6;
263 ret = 0;
264 }
265 break;
266
267 case SENSOR_CHAN_DIE_TEMP:
268 if (data->has_tmp) {
269 /*6 bytes for XYZ and 2 bytes for TEMP*/
270 *frame_count = data->fifo_byte_count / 8;
271 ret = 0;
272 }
273 break;
274
275 default:
276 break;
277 }
278 }
279 }
280 #endif /* CONFIG_ADXL362_STREAM */
281
282 return ret;
283 }
284
adxl362_decode_sample(const struct adxl362_sample_data * data,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)285 static int adxl362_decode_sample(const struct adxl362_sample_data *data,
286 struct sensor_chan_spec chan_spec, uint32_t *fit, uint16_t max_count, void *data_out)
287 {
288 struct sensor_value *out = (struct sensor_value *) data_out;
289
290 if (*fit > 0) {
291 return -ENOTSUP;
292 }
293
294 switch (chan_spec.chan_type) {
295 case SENSOR_CHAN_ACCEL_X: /* Acceleration on the X axis, in m/s^2. */
296 adxl362_accel_convert(out, data->acc_x, data->selected_range);
297 break;
298 case SENSOR_CHAN_ACCEL_Y: /* Acceleration on the Y axis, in m/s^2. */
299 adxl362_accel_convert(out, data->acc_y, data->selected_range);
300 break;
301 case SENSOR_CHAN_ACCEL_Z: /* Acceleration on the Z axis, in m/s^2. */
302 adxl362_accel_convert(out, data->acc_z, data->selected_range);
303 break;
304 case SENSOR_CHAN_ACCEL_XYZ: /* Acceleration on the XYZ axis, in m/s^2. */
305 adxl362_accel_convert(out++, data->acc_x, data->selected_range);
306 adxl362_accel_convert(out++, data->acc_y, data->selected_range);
307 adxl362_accel_convert(out, data->acc_z, data->selected_range);
308 break;
309 case SENSOR_CHAN_DIE_TEMP: /* Temperature in degrees Celsius. */
310 adxl362_temp_convert(out, data->temp);
311 break;
312 default:
313 return -ENOTSUP;
314 }
315
316 *fit = 1;
317
318 return 0;
319 }
320
adxl362_decoder_decode(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)321 static int adxl362_decoder_decode(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
322 uint32_t *fit, uint16_t max_count, void *data_out)
323 {
324 const struct adxl362_sample_data *data = (const struct adxl362_sample_data *)buffer;
325
326 #ifdef CONFIG_ADXL362_STREAM
327 if (data->is_fifo) {
328 return adxl362_decode_stream(buffer, chan_spec, fit, max_count, data_out);
329 }
330 #endif /* CONFIG_ADXL362_STREAM */
331
332 return adxl362_decode_sample(data, chan_spec, fit, max_count, data_out);
333 }
334
adxl362_decoder_has_trigger(const uint8_t * buffer,enum sensor_trigger_type trigger)335 static bool adxl362_decoder_has_trigger(const uint8_t *buffer, enum sensor_trigger_type trigger)
336 {
337 const struct adxl362_fifo_data *data = (const struct adxl362_fifo_data *)buffer;
338
339 if (!data->is_fifo) {
340 return false;
341 }
342
343 switch (trigger) {
344 case SENSOR_TRIG_DATA_READY:
345 return ADXL362_STATUS_CHECK_DATA_READY(data->int_status);
346 case SENSOR_TRIG_FIFO_WATERMARK:
347 return ADXL362_STATUS_CHECK_FIFO_WTR(data->int_status);
348 case SENSOR_TRIG_FIFO_FULL:
349 return ADXL362_STATUS_CHECK_FIFO_OVR(data->int_status);
350 default:
351 return false;
352 }
353 }
354
355 SENSOR_DECODER_API_DT_DEFINE() = {
356 .get_frame_count = adxl362_decoder_get_frame_count,
357 .decode = adxl362_decoder_decode,
358 .has_trigger = adxl362_decoder_has_trigger,
359 };
360
adxl362_get_decoder(const struct device * dev,const struct sensor_decoder_api ** decoder)361 int adxl362_get_decoder(const struct device *dev, const struct sensor_decoder_api **decoder)
362 {
363 ARG_UNUSED(dev);
364 *decoder = &SENSOR_DECODER_NAME();
365
366 return 0;
367 }
368
