1 /*
2 * Copyright (c) 2024 Analog Devices Inc.
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include "adxl345.h"
8
9 #ifdef CONFIG_ADXL345_STREAM
10
11 #define SENSOR_SCALING_FACTOR (SENSOR_G / (16 * 1000 / 100))
12
13 static const uint32_t accel_period_ns[] = {
14 [ADXL345_ODR_12HZ] = UINT32_C(1000000000) / 12,
15 [ADXL345_ODR_25HZ] = UINT32_C(1000000000) / 25,
16 [ADXL345_ODR_50HZ] = UINT32_C(1000000000) / 50,
17 [ADXL345_ODR_100HZ] = UINT32_C(1000000000) / 100,
18 [ADXL345_ODR_200HZ] = UINT32_C(1000000000) / 200,
19 [ADXL345_ODR_400HZ] = UINT32_C(1000000000) / 400,
20 };
21
22 static const uint32_t range_to_shift[] = {
23 [ADXL345_RANGE_2G] = 5,
24 [ADXL345_RANGE_4G] = 6,
25 [ADXL345_RANGE_8G] = 7,
26 [ADXL345_RANGE_16G] = 8,
27 };
28
29 /* (1 / sensitivity) * (pow(2,31) / pow(2,shift)) * (unit_scaler) */
30 static const uint32_t qscale_factor_no_full_res[] = {
31 /* (1.0 / ADXL362_ACCEL_2G_LSB_PER_G) * (2^31 / 2^5) * SENSOR_G / 1000000 */
32 [ADXL345_RANGE_2G] = UINT32_C(2569011),
33 /* (1.0 / ADXL362_ACCEL_4G_LSB_PER_G) * (2^31 / 2^6) * SENSOR_G / 1000000 */
34 [ADXL345_RANGE_4G] = UINT32_C(642253),
35 /* (1.0 / ADXL362_ACCEL_8G_LSB_PER_G) * (2^31 / 2^7) ) * SENSOR_G / 1000000 */
36 [ADXL345_RANGE_8G] = UINT32_C(160563),
37 /* (1.0 / ADXL362_ACCEL_8G_LSB_PER_G) * (2^31 / 2^8) ) * SENSOR_G / 1000000 */
38 [ADXL345_RANGE_16G] = UINT32_C(40141),
39 };
40
41 /* (1 / sensitivity) * (pow(2,31) / pow(2,shift)) * (unit_scaler) */
42 static const uint32_t qscale_factor_full_res[] = {
43 /* (1.0 / ADXL362_ACCEL_2G_LSB_PER_G) * (2^31 / 2^5) * SENSOR_G / 1000000 */
44 [ADXL345_RANGE_2G] = UINT32_C(2569011),
45 /* (1.0 / ADXL362_ACCEL_4G_LSB_PER_G) * (2^31 / 2^6) * SENSOR_G / 1000000 */
46 [ADXL345_RANGE_4G] = UINT32_C(1284506),
47 /* (1.0 / ADXL362_ACCEL_8G_LSB_PER_G) * (2^31 / 2^7) ) * SENSOR_G / 1000000 */
48 [ADXL345_RANGE_8G] = UINT32_C(642253),
49 /* (1.0 / ADXL362_ACCEL_8G_LSB_PER_G) * (2^31 / 2^8) ) * SENSOR_G / 1000000 */
50 [ADXL345_RANGE_16G] = UINT32_C(321126),
51 };
52
adxl345_accel_convert_q31(q31_t * out,int16_t sample,int32_t range,uint8_t is_full_res)53 static inline void adxl345_accel_convert_q31(q31_t *out, int16_t sample, int32_t range,
54 uint8_t is_full_res)
55 {
56 if (is_full_res) {
57 switch (range) {
58 case ADXL345_RANGE_2G:
59 if (sample & BIT(9)) {
60 sample |= ADXL345_COMPLEMENT_MASK(10);
61 }
62 break;
63 case ADXL345_RANGE_4G:
64 if (sample & BIT(10)) {
65 sample |= ADXL345_COMPLEMENT_MASK(11);
66 }
67 break;
68 case ADXL345_RANGE_8G:
69 if (sample & BIT(11)) {
70 sample |= ADXL345_COMPLEMENT_MASK(12);
71 }
72 break;
73 case ADXL345_RANGE_16G:
74 if (sample & BIT(12)) {
75 sample |= ADXL345_COMPLEMENT_MASK(13);
76 }
77 break;
78 }
79 } else {
80 if (sample & BIT(9)) {
81 sample |= ADXL345_COMPLEMENT;
82 }
83 }
84
85 *out = sample * qscale_factor_no_full_res[range];
86 }
87
adxl345_decode_stream(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)88 static int adxl345_decode_stream(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
89 uint32_t *fit, uint16_t max_count, void *data_out)
90 {
91 const struct adxl345_fifo_data *enc_data = (const struct adxl345_fifo_data *)buffer;
92 const uint8_t *buffer_end =
93 buffer + sizeof(struct adxl345_fifo_data) + enc_data->fifo_byte_count;
94 int count = 0;
95 uint8_t sample_num = 0;
96
97 if ((uintptr_t)buffer_end <= *fit || chan_spec.chan_idx != 0) {
98 return 0;
99 }
100
101 struct sensor_three_axis_data *data = (struct sensor_three_axis_data *)data_out;
102
103 memset(data, 0, sizeof(struct sensor_three_axis_data));
104 data->header.base_timestamp_ns = enc_data->timestamp;
105 data->header.reading_count = 1;
106 data->shift = range_to_shift[enc_data->selected_range];
107
108 buffer += sizeof(struct adxl345_fifo_data);
109
110 uint8_t sample_set_size = enc_data->sample_set_size;
111 uint64_t period_ns = accel_period_ns[enc_data->accel_odr];
112 uint8_t is_full_res = enc_data->is_full_res;
113
114 /* Calculate which sample is decoded. */
115 if ((uint8_t *)*fit >= buffer) {
116 sample_num = ((uint8_t *)*fit - buffer) / sample_set_size;
117 }
118
119 while (count < max_count && buffer < buffer_end) {
120 const uint8_t *sample_end = buffer;
121
122 sample_end += sample_set_size;
123
124 if ((uintptr_t)buffer < *fit) {
125 /* This frame was already decoded, move on to the next frame */
126 buffer = sample_end;
127 continue;
128 }
129
130 switch (chan_spec.chan_type) {
131 case SENSOR_CHAN_ACCEL_XYZ:
132 data->readings[count].timestamp_delta = sample_num * period_ns;
133 uint8_t buff_offset = 0;
134
135 adxl345_accel_convert_q31(&data->readings[count].x, *(int16_t *)buffer,
136 enc_data->selected_range, is_full_res);
137 buff_offset = 2;
138 adxl345_accel_convert_q31(&data->readings[count].y,
139 *(int16_t *)(buffer + buff_offset),
140 enc_data->selected_range, is_full_res);
141 buff_offset += 2;
142 adxl345_accel_convert_q31(&data->readings[count].z,
143 *(int16_t *)(buffer + buff_offset),
144 enc_data->selected_range, is_full_res);
145 break;
146 default:
147 return -ENOTSUP;
148 }
149 buffer = sample_end;
150 *fit = (uintptr_t)sample_end;
151 count++;
152 }
153 return count;
154 }
155
156 #endif /* CONFIG_ADXL345_STREAM */
157
adxl345_decoder_get_frame_count(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint16_t * frame_count)158 static int adxl345_decoder_get_frame_count(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
159 uint16_t *frame_count)
160 {
161 int32_t ret = -ENOTSUP;
162
163 if (chan_spec.chan_idx != 0) {
164 return ret;
165 }
166
167 #ifdef CONFIG_ADXL345_STREAM
168 const struct adxl345_fifo_data *data = (const struct adxl345_fifo_data *)buffer;
169
170 if (!data->is_fifo) {
171 #endif /* CONFIG_ADXL345_STREAM */
172 switch (chan_spec.chan_type) {
173 case SENSOR_CHAN_ACCEL_X:
174 case SENSOR_CHAN_ACCEL_Y:
175 case SENSOR_CHAN_ACCEL_Z:
176 case SENSOR_CHAN_ACCEL_XYZ:
177 *frame_count = 1;
178 ret = 0;
179 break;
180
181 default:
182 break;
183 }
184 #ifdef CONFIG_ADXL345_STREAM
185 } else {
186 if (data->fifo_byte_count == 0) {
187 *frame_count = 0;
188 ret = 0;
189 } else {
190 switch (chan_spec.chan_type) {
191 case SENSOR_CHAN_ACCEL_XYZ:
192 *frame_count =
193 data->fifo_byte_count / data->sample_set_size;
194 ret = 0;
195 break;
196
197 default:
198 break;
199 }
200 }
201 }
202 #endif /* CONFIG_ADXL345_STREAM */
203
204 return ret;
205 }
206
adxl345_decode_sample(const struct adxl345_sample * data,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)207 static int adxl345_decode_sample(const struct adxl345_sample *data,
208 struct sensor_chan_spec chan_spec, uint32_t *fit,
209 uint16_t max_count, void *data_out)
210 {
211 struct sensor_value *out = (struct sensor_value *)data_out;
212
213 if (*fit > 0) {
214 return -ENOTSUP;
215 }
216
217 switch (chan_spec.chan_type) {
218 case SENSOR_CHAN_ACCEL_XYZ:
219 adxl345_accel_convert(out++, data->x);
220 adxl345_accel_convert(out++, data->y);
221 adxl345_accel_convert(out, data->z);
222 break;
223 default:
224 return -ENOTSUP;
225 }
226
227 *fit = 1;
228
229 return 0;
230 }
231
adxl345_decoder_decode(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)232 static int adxl345_decoder_decode(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
233 uint32_t *fit, uint16_t max_count, void *data_out)
234 {
235 const struct adxl345_sample *data = (const struct adxl345_sample *)buffer;
236
237 #ifdef CONFIG_ADXL345_STREAM
238 if (data->is_fifo) {
239 return adxl345_decode_stream(buffer, chan_spec, fit, max_count, data_out);
240 }
241 #endif /* CONFIG_ADXL345_STREAM */
242
243 return adxl345_decode_sample(data, chan_spec, fit, max_count, data_out);
244 }
245
adxl345_decoder_has_trigger(const uint8_t * buffer,enum sensor_trigger_type trigger)246 static bool adxl345_decoder_has_trigger(const uint8_t *buffer, enum sensor_trigger_type trigger)
247 {
248 const struct adxl345_fifo_data *data = (const struct adxl345_fifo_data *)buffer;
249
250 if (!data->is_fifo) {
251 return false;
252 }
253
254 switch (trigger) {
255 case SENSOR_TRIG_FIFO_WATERMARK:
256 return FIELD_GET(ADXL345_INT_MAP_WATERMARK_MSK, data->int_status);
257 default:
258 return false;
259 }
260 }
261
262 SENSOR_DECODER_API_DT_DEFINE() = {
263 .get_frame_count = adxl345_decoder_get_frame_count,
264 .decode = adxl345_decoder_decode,
265 .has_trigger = adxl345_decoder_has_trigger,
266 };
267
adxl345_get_decoder(const struct device * dev,const struct sensor_decoder_api ** decoder)268 int adxl345_get_decoder(const struct device *dev, const struct sensor_decoder_api **decoder)
269 {
270 ARG_UNUSED(dev);
271 *decoder = &SENSOR_DECODER_NAME();
272
273 return 0;
274 }
275
