1 /*
2 * Copyright (c) 2023 Google LLC
3 * SPDX-License-Identifier: Apache-2.0
4 */
5
6 #include <zephyr/device.h>
7 #include <zephyr/drivers/emul_sensor.h>
8 #include <zephyr/drivers/emul.h>
9 #include <zephyr/ztest.h>
10 #include <zephyr/drivers/sensor.h>
11 #include <zephyr/drivers/sensor_data_types.h>
12 #include <zephyr/rtio/rtio.h>
13 #include <zephyr/sys/bitarray.h>
14
15 #include <zephyr/logging/log.h>
16 LOG_MODULE_REGISTER(generic_test);
17
18 /**
19 * A union of all sensor data types.
20 */
21 union sensor_data_union {
22 struct sensor_three_axis_data three_axis;
23 struct sensor_occurrence_data occurrence;
24 struct sensor_q31_data q31;
25 struct sensor_byte_data byte;
26 struct sensor_uint64_data uint64;
27 };
28
29 /*
30 * Set up an RTIO context that can be shared for all sensors
31 */
32
33 static struct sensor_chan_spec iodev_all_channels[SENSOR_CHAN_ALL];
34 static struct sensor_read_config iodev_read_config = {
35 .channels = iodev_all_channels,
36 .max = SENSOR_CHAN_ALL,
37 };
38 RTIO_IODEV_DEFINE(iodev_read, &__sensor_iodev_api, &iodev_read_config);
39
40 /* Create the RTIO context to service the reading */
41 RTIO_DEFINE_WITH_MEMPOOL(sensor_read_rtio_ctx, 1, 1, 1, 64, 4);
42
43 /**
44 * @brief Prepare the RTIO context for next test
45 */
before(void * args)46 static void before(void *args)
47 {
48 ARG_UNUSED(args);
49
50 /* Clear the array of requested channels */
51 memset(iodev_all_channels, 0, sizeof(iodev_all_channels));
52
53 /* Reset the channel count */
54 iodev_read_config.count = 0;
55
56 /* Remove previous sensor pointer */
57 iodev_read_config.sensor = NULL;
58
59 /* Wipe the mempool by marking every block free */
60 zassert_ok(sys_bitarray_clear_region(sensor_read_rtio_ctx.block_pool->bitmap,
61 sensor_read_rtio_ctx.block_pool->info.num_blocks,
62 0));
63
64 /* Flush the SQ and CQ */
65 rtio_sqe_drop_all(&sensor_read_rtio_ctx);
66 while (rtio_cqe_consume(&sensor_read_rtio_ctx)) {
67 ;
68 }
69 }
70
71 /**
72 * @brief Helper function the carries out the generic sensor test for a given sensor device.
73 * Verifies that the device has a suitable emulator that implements the backend API and
74 * skips the test gracefully if not.
75 */
run_generic_test(const struct device * dev)76 static void run_generic_test(const struct device *dev)
77 {
78 zassert_not_null(dev, "Cannot get device pointer. Is this driver properly instantiated?");
79
80 const struct emul *emul = emul_get_binding(dev->name);
81
82 /* Skip this sensor if there is no emulator loaded. */
83 if (!emul) {
84 ztest_test_skip();
85 }
86
87 /* Also skip if this emulator does not implement the backend API. */
88 if (!emul_sensor_backend_is_supported(emul)) {
89 ztest_test_skip();
90 }
91
92 /*
93 * Begin the actual test sequence
94 */
95
96 static struct {
97 bool supported;
98 bool received;
99 q31_t expected_values[CONFIG_GENERIC_SENSOR_TEST_NUM_EXPECTED_VALS];
100 q31_t epsilon;
101 int8_t expected_value_shift;
102 } channel_table[SENSOR_CHAN_ALL];
103
104 memset(channel_table, 0, sizeof(channel_table));
105
106 /* Discover supported channels on this device and fill out our sensor read request */
107 for (enum sensor_channel ch = 0; ch < ARRAY_SIZE(channel_table); ch++) {
108 if (SENSOR_CHANNEL_3_AXIS(ch)) {
109 continue;
110 }
111
112 q31_t lower, upper;
113 int8_t shift;
114 struct sensor_chan_spec ch_spec = {.chan_type = ch, .chan_idx = 0};
115
116 if (emul_sensor_backend_get_sample_range(emul, ch_spec, &lower, &upper,
117 &channel_table[ch].epsilon, &shift) == 0) {
118 /* This channel is supported */
119 channel_table[ch].supported = true;
120
121 LOG_INF("CH %d: lower=%d, upper=%d, eps=%d, shift=%d", ch, lower, upper,
122 channel_table[ch].epsilon, shift);
123
124 /* Add to the list of channels to read */
125 iodev_all_channels[iodev_read_config.count++].chan_type = ch;
126
127 /* Generate a set of CONFIG_GENERIC_SENSOR_TEST_NUM_EXPECTED_VALS test
128 * values.
129 */
130
131 channel_table[ch].expected_value_shift = shift;
132 for (size_t i = 0; i < CONFIG_GENERIC_SENSOR_TEST_NUM_EXPECTED_VALS; i++) {
133 channel_table[ch].expected_values[i] =
134 lower +
135 (i * ((int64_t)upper - lower) /
136 (CONFIG_GENERIC_SENSOR_TEST_NUM_EXPECTED_VALS - 1));
137 LOG_INF("CH %d: Expected value %d/%d: %d", ch, i + 1,
138 CONFIG_GENERIC_SENSOR_TEST_NUM_EXPECTED_VALS,
139 channel_table[ch].expected_values[i]);
140 }
141 }
142 }
143 iodev_read_config.sensor = dev;
144
145 /* Do a read of all channels for quantity CONFIG_GENERIC_SENSOR_TEST_NUM_EXPECTED_VALS
146 * iterations.
147 */
148
149 for (size_t iteration = 0; iteration < CONFIG_GENERIC_SENSOR_TEST_NUM_EXPECTED_VALS;
150 iteration++) {
151 int rv;
152
153 /* Reset received flag */
154 for (size_t i = 0; i < ARRAY_SIZE(channel_table); i++) {
155 channel_table[i].received = false;
156 }
157
158 /* Set this iteration's expected values in emul for every supported channel */
159 for (size_t i = 0; i < iodev_read_config.count; i++) {
160 struct sensor_chan_spec ch_spec = iodev_all_channels[i];
161
162 rv = emul_sensor_backend_set_channel(
163 emul, ch_spec,
164 &channel_table[ch_spec.chan_type].expected_values[iteration],
165 channel_table[ch_spec.chan_type].expected_value_shift);
166 zassert_ok(rv,
167 "Cannot set value 0x%08x on channel (type: %d, index: %d) "
168 "(error %d, iteration %d/%d)",
169 channel_table[i].expected_values[iteration], ch_spec.chan_type,
170 ch_spec.chan_idx, rv, iteration + 1,
171 CONFIG_GENERIC_SENSOR_TEST_NUM_EXPECTED_VALS);
172 }
173
174 /* Perform the actual sensor read */
175 rv = sensor_read_async_mempool(&iodev_read, &sensor_read_rtio_ctx, NULL);
176 zassert_ok(rv, "Could not read sensor (error %d, iteration %d/%d)", rv,
177 iteration + 1, CONFIG_GENERIC_SENSOR_TEST_NUM_EXPECTED_VALS);
178
179 /* Wait for a CQE */
180 struct rtio_cqe *cqe = rtio_cqe_consume_block(&sensor_read_rtio_ctx);
181
182 zassert_ok(cqe->result, "CQE has failed status (error %d, iteration %d/%d)",
183 cqe->result, iteration + 1,
184 CONFIG_GENERIC_SENSOR_TEST_NUM_EXPECTED_VALS);
185
186 uint8_t *buf = NULL;
187 uint32_t buf_len = 0;
188
189 /* Cache the data from the CQE */
190 rtio_cqe_get_mempool_buffer(&sensor_read_rtio_ctx, cqe, &buf, &buf_len);
191
192 /* Release the CQE */
193 rtio_cqe_release(&sensor_read_rtio_ctx, cqe);
194
195 const struct sensor_decoder_api *decoder;
196 union sensor_data_union decoded_data;
197
198 zassert_ok(sensor_get_decoder(dev, &decoder));
199
200 /* Loop through each channel */
201 for (int ch = 0; ch < ARRAY_SIZE(channel_table); ch++) {
202 if (!channel_table[ch].supported) {
203 continue;
204 }
205
206 struct sensor_decode_context ctx =
207 SENSOR_DECODE_CONTEXT_INIT(decoder, buf, ch, 0);
208
209 rv = sensor_decode(&ctx, &decoded_data, 1);
210 zassert_equal(1, rv, "Could not decode (error %d, ch %d, iteration %d/%d)",
211 rv, ch, iteration + 1,
212 CONFIG_GENERIC_SENSOR_TEST_NUM_EXPECTED_VALS);
213
214 channel_table[ch].received = true;
215
216 /* Retrieve the actual value */
217 q31_t q;
218 int8_t shift;
219
220 switch (ch) {
221 /* Special handling to break out triplet samples. */
222 case SENSOR_CHAN_MAGN_X:
223 case SENSOR_CHAN_ACCEL_X:
224 case SENSOR_CHAN_GYRO_X:
225 q = decoded_data.three_axis.readings[0].x;
226 shift = decoded_data.three_axis.shift;
227 break;
228 case SENSOR_CHAN_MAGN_Y:
229 case SENSOR_CHAN_ACCEL_Y:
230 case SENSOR_CHAN_GYRO_Y:
231 q = decoded_data.three_axis.readings[0].y;
232 shift = decoded_data.three_axis.shift;
233 break;
234 case SENSOR_CHAN_MAGN_Z:
235 case SENSOR_CHAN_ACCEL_Z:
236 case SENSOR_CHAN_GYRO_Z:
237 q = decoded_data.three_axis.readings[0].z;
238 shift = decoded_data.three_axis.shift;
239 break;
240
241 /* Default case for single Q31 samples */
242 default:
243 q = decoded_data.q31.readings[0].value;
244 shift = decoded_data.q31.shift;
245 break;
246 }
247
248 /* Align everything to be a 64-bit Q32.32 number for comparison */
249 int64_t expected_shifted =
250 (int64_t)channel_table[ch].expected_values[iteration]
251 << channel_table[ch].expected_value_shift;
252 int64_t actual_shifted = (int64_t)q << shift;
253 int64_t epsilon_shifted = (int64_t)channel_table[ch].epsilon
254 << channel_table[ch].expected_value_shift;
255
256 zassert_within(expected_shifted, actual_shifted, epsilon_shifted,
257 "Expected %lld, got %lld (shift %d, ch %d, iteration %d/%d, "
258 "Error %lld, Epsilon %lld)",
259 expected_shifted, actual_shifted, shift, ch, iteration + 1,
260 CONFIG_GENERIC_SENSOR_TEST_NUM_EXPECTED_VALS,
261 expected_shifted - actual_shifted, epsilon_shifted);
262 }
263
264 /* Release the memory */
265 rtio_release_buffer(&sensor_read_rtio_ctx, buf, buf_len);
266
267 /* Ensure all supported channels were received */
268 int missing_channel_count = 0;
269
270 for (size_t i = 0; i < ARRAY_SIZE(channel_table); i++) {
271 if (channel_table[i].supported && !channel_table[i].received) {
272 missing_channel_count++;
273 }
274 }
275
276 zassert_equal(0, missing_channel_count, "%d channel(s) not received",
277 missing_channel_count);
278 }
279 }
280
281 #define DECLARE_ZTEST_PER_DEVICE(n) \
282 ZTEST(generic, test_##n) \
283 { \
284 run_generic_test(DEVICE_DT_GET(n)); \
285 }
286
287 /* Iterate through each of the emulated buses and create a test for each device. */
288 DT_FOREACH_CHILD_STATUS_OKAY(DT_NODELABEL(test_i2c), DECLARE_ZTEST_PER_DEVICE)
289 DT_FOREACH_CHILD_STATUS_OKAY(DT_NODELABEL(test_i3c), DECLARE_ZTEST_PER_DEVICE)
290 DT_FOREACH_CHILD_STATUS_OKAY(DT_NODELABEL(test_spi), DECLARE_ZTEST_PER_DEVICE)
291
292 ZTEST_SUITE(generic, NULL, NULL, before, NULL, NULL);
293
