1 /*
2 * Copyright (c) 2024 Analog Devices Inc.
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include "adxl372.h"
8
9 #ifdef CONFIG_ADXL372_STREAM
10
11 /* (1.0 / 10 (sensor sensitivity)) * (2^31 / 2^11 (sensor shift) ) * SENSOR_G */
12 #define SENSOR_QSCALE_FACTOR UINT32_C(1027604)
13
14 #define ADXL372_COMPLEMENT 0xf000
15
16 static const uint32_t accel_period_ns[] = {
17 [ADXL372_ODR_400HZ] = UINT32_C(1000000000) / 400,
18 [ADXL372_ODR_800HZ] = UINT32_C(1000000000) / 800,
19 [ADXL372_ODR_1600HZ] = UINT32_C(1000000000) / 1600,
20 [ADXL372_ODR_3200HZ] = UINT32_C(1000000000) / 3200,
21 [ADXL372_ODR_6400HZ] = UINT32_C(1000000000) / 6400,
22 };
23
adxl372_accel_convert_q31(q31_t * out,const uint8_t * buff)24 static inline void adxl372_accel_convert_q31(q31_t *out, const uint8_t *buff)
25 {
26 int16_t data_in = ((int16_t)*buff << 4) | (((int16_t)*(buff + 1) & 0xF0) >> 4);
27
28 if (data_in & BIT(11)) {
29 data_in |= ADXL372_COMPLEMENT;
30 }
31
32 *out = data_in * SENSOR_QSCALE_FACTOR;
33 }
34
adxl372_decode_stream(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)35 static int adxl372_decode_stream(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
36 uint32_t *fit, uint16_t max_count, void *data_out)
37 {
38 const struct adxl372_fifo_data *enc_data = (const struct adxl372_fifo_data *)buffer;
39 const uint8_t *buffer_end =
40 buffer + sizeof(struct adxl372_fifo_data) + enc_data->fifo_byte_count;
41 int count = 0;
42 uint8_t sample_num = 0;
43
44 if ((uintptr_t)buffer_end <= *fit || chan_spec.chan_idx != 0) {
45 return 0;
46 }
47
48 struct sensor_three_axis_data *data = (struct sensor_three_axis_data *)data_out;
49
50 memset(data, 0, sizeof(struct sensor_three_axis_data));
51 data->header.base_timestamp_ns = enc_data->timestamp;
52 data->header.reading_count = 1;
53 data->header.shift = 11; /* Sensor shift */
54
55 buffer += sizeof(struct adxl372_fifo_data);
56
57 uint8_t sample_set_size = enc_data->sample_set_size;
58 uint64_t period_ns = accel_period_ns[enc_data->accel_odr];
59
60 /* Calculate which sample is decoded. */
61 if ((uint8_t *)*fit >= buffer) {
62 sample_num = ((uint8_t *)*fit - buffer) / sample_set_size;
63 }
64
65 while (count < max_count && buffer < buffer_end) {
66 const uint8_t *sample_end = buffer;
67
68 sample_end += sample_set_size;
69
70 if ((uintptr_t)buffer < *fit) {
71 /* This frame was already decoded, move on to the next frame */
72 buffer = sample_end;
73 continue;
74 }
75
76 switch (chan_spec.chan_type) {
77 case SENSOR_CHAN_ACCEL_X:
78 if (enc_data->has_x) {
79 data->readings[count].timestamp_delta = sample_num * period_ns;
80 adxl372_accel_convert_q31(&data->readings[count].x, buffer);
81 }
82 break;
83 case SENSOR_CHAN_ACCEL_Y:
84 if (enc_data->has_y) {
85 uint8_t buff_offset = 0;
86
87 /* If packet has X channel, then Y channel has offset. */
88 if (enc_data->has_x) {
89 buff_offset = 2;
90 }
91 data->readings[count].timestamp_delta = sample_num * period_ns;
92 adxl372_accel_convert_q31(&data->readings[count].y,
93 (buffer + buff_offset));
94 }
95 break;
96 case SENSOR_CHAN_ACCEL_Z:
97 if (enc_data->has_z) {
98 uint8_t buff_offset = 0;
99
100 /* If packet has X channel and/or Y channel,
101 * then Z channel has offset.
102 */
103 if (enc_data->has_x) {
104 buff_offset = 2;
105 }
106
107 if (enc_data->has_y) {
108 buff_offset += 2;
109 }
110 data->readings[count].timestamp_delta = sample_num * period_ns;
111 adxl372_accel_convert_q31(&data->readings[count].z,
112 (buffer + buff_offset));
113 }
114 break;
115 case SENSOR_CHAN_ACCEL_XYZ:
116 data->readings[count].timestamp_delta = sample_num * period_ns;
117 uint8_t buff_offset = 0;
118
119 if (enc_data->has_x) {
120 adxl372_accel_convert_q31(&data->readings[count].x, buffer);
121 buff_offset = 2;
122 }
123
124 if (enc_data->has_y) {
125 adxl372_accel_convert_q31(&data->readings[count].y,
126 (buffer + buff_offset));
127
128 buff_offset += 2;
129 }
130
131 if (enc_data->has_z) {
132 adxl372_accel_convert_q31(&data->readings[count].z,
133 (buffer + buff_offset));
134 }
135 break;
136 default:
137 return -ENOTSUP;
138 }
139
140 buffer = sample_end;
141 *fit = (uintptr_t)sample_end;
142 count++;
143 }
144 return count;
145 }
146
147 #endif /* CONFIG_ADXL372_STREAM */
148
adxl372_decoder_get_frame_count(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint16_t * frame_count)149 static int adxl372_decoder_get_frame_count(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
150 uint16_t *frame_count)
151 {
152 int32_t ret = -ENOTSUP;
153
154 if (chan_spec.chan_idx != 0) {
155 return ret;
156 }
157
158 #ifdef CONFIG_ADXL372_STREAM
159 const struct adxl372_fifo_data *data = (const struct adxl372_fifo_data *)buffer;
160
161 if (!data->is_fifo) {
162 #endif /* CONFIG_ADXL372_STREAM */
163 switch (chan_spec.chan_type) {
164 case SENSOR_CHAN_ACCEL_X:
165 case SENSOR_CHAN_ACCEL_Y:
166 case SENSOR_CHAN_ACCEL_Z:
167 case SENSOR_CHAN_ACCEL_XYZ:
168 *frame_count = 1;
169 ret = 0;
170 break;
171
172 default:
173 break;
174 }
175 #ifdef CONFIG_ADXL372_STREAM
176 } else {
177 if (data->fifo_byte_count == 0) {
178 *frame_count = 0;
179 ret = 0;
180 } else {
181 switch (chan_spec.chan_type) {
182 case SENSOR_CHAN_ACCEL_X:
183 if (data->has_x) {
184 *frame_count =
185 data->fifo_byte_count / data->sample_set_size;
186 ret = 0;
187 }
188 break;
189 case SENSOR_CHAN_ACCEL_Y:
190 if (data->has_y) {
191 *frame_count =
192 data->fifo_byte_count / data->sample_set_size;
193 ret = 0;
194 }
195 break;
196 case SENSOR_CHAN_ACCEL_Z:
197 if (data->has_z) {
198 *frame_count =
199 data->fifo_byte_count / data->sample_set_size;
200 ret = 0;
201 }
202 break;
203 case SENSOR_CHAN_ACCEL_XYZ:
204 if (data->has_x || data->has_y || data->has_z) {
205 *frame_count =
206 data->fifo_byte_count / data->sample_set_size;
207 ret = 0;
208 }
209 break;
210
211 default:
212 break;
213 }
214 }
215 }
216 #endif /* CONFIG_ADXL372_STREAM */
217
218 return ret;
219 }
220
adxl372_decode_sample(const struct adxl372_xyz_accel_data * data,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)221 static int adxl372_decode_sample(const struct adxl372_xyz_accel_data *data,
222 struct sensor_chan_spec chan_spec, uint32_t *fit,
223 uint16_t max_count, void *data_out)
224 {
225 struct sensor_value *out = (struct sensor_value *)data_out;
226
227 if (*fit > 0) {
228 return -ENOTSUP;
229 }
230
231 switch (chan_spec.chan_type) {
232 case SENSOR_CHAN_ACCEL_X:
233 adxl372_accel_convert(out, data->x);
234 break;
235 case SENSOR_CHAN_ACCEL_Y:
236 adxl372_accel_convert(out, data->y);
237 break;
238 case SENSOR_CHAN_ACCEL_Z:
239 adxl372_accel_convert(out, data->z);
240 break;
241 case SENSOR_CHAN_ACCEL_XYZ:
242 adxl372_accel_convert(out++, data->x);
243 adxl372_accel_convert(out++, data->y);
244 adxl372_accel_convert(out, data->z);
245 break;
246 default:
247 return -ENOTSUP;
248 }
249
250 *fit = 1;
251
252 return 0;
253 }
254
adxl372_decoder_decode(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)255 static int adxl372_decoder_decode(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
256 uint32_t *fit, uint16_t max_count, void *data_out)
257 {
258 const struct adxl372_xyz_accel_data *data = (const struct adxl372_xyz_accel_data *)buffer;
259
260 #ifdef CONFIG_ADXL372_STREAM
261 if (data->is_fifo) {
262 return adxl372_decode_stream(buffer, chan_spec, fit, max_count, data_out);
263 }
264 #endif /* CONFIG_ADXL372_STREAM */
265
266 return adxl372_decode_sample(data, chan_spec, fit, max_count, data_out);
267 }
268
adxl372_decoder_has_trigger(const uint8_t * buffer,enum sensor_trigger_type trigger)269 static bool adxl372_decoder_has_trigger(const uint8_t *buffer, enum sensor_trigger_type trigger)
270 {
271 const struct adxl372_fifo_data *data = (const struct adxl372_fifo_data *)buffer;
272
273 if (!data->is_fifo) {
274 return false;
275 }
276
277 switch (trigger) {
278 case SENSOR_TRIG_DATA_READY:
279 return FIELD_GET(ADXL372_INT1_MAP_DATA_RDY_MSK, data->int_status);
280 case SENSOR_TRIG_FIFO_WATERMARK:
281 case SENSOR_TRIG_FIFO_FULL:
282 return FIELD_GET(ADXL372_INT1_MAP_FIFO_FULL_MSK, data->int_status);
283 default:
284 return false;
285 }
286 }
287
288 SENSOR_DECODER_API_DT_DEFINE() = {
289 .get_frame_count = adxl372_decoder_get_frame_count,
290 .decode = adxl372_decoder_decode,
291 .has_trigger = adxl372_decoder_has_trigger,
292 };
293
adxl372_get_decoder(const struct device * dev,const struct sensor_decoder_api ** decoder)294 int adxl372_get_decoder(const struct device *dev, const struct sensor_decoder_api **decoder)
295 {
296 ARG_UNUSED(dev);
297 *decoder = &SENSOR_DECODER_NAME();
298
299 return 0;
300 }
301
