1 /*
2 * Copyright (C) 2019 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #define LOG_TAG "oslo_data_injection_test"
18
19 #include <errno.h>
20 #include <getopt.h>
21 #include <inttypes.h>
22 #include <limits.h>
23 #include <log/log.h>
24 #include <math.h>
25 #include <pthread.h>
26 #include <stdbool.h>
27 #include <stdio.h>
28 #include <stdlib.h>
29 #include <string.h>
30 #include <sys/ioctl.h>
31 #include <sys/stat.h>
32 #include <sys/types.h>
33 #include <time.h>
34 #include <unistd.h>
35
36 #include "oslo_iaxxx_sensor_control.h"
37
38 #define FRAME_SIZE_MAX (16 * 1024)
39 #define FRAME_PERIOD_MS_MAX (1000)
40 #define INJECT_BYTES_PER_SEC_MAX (400 * 1024) // Reach: 12672 Bytes * 30 Hz = 380160 Bytes/Sec
41 #define CHANNEL_NUM 3
42 #define BYTES_PER_SAMPLE 2
43
44 enum oslo_header_index {
45 OSLO_HEADER_INDEX_SYNC_0 = 0,
46 OSLO_HEADER_INDEX_SYNC_1,
47 OSLO_HEADER_INDEX_FRAME_COUNT,
48 OSLO_HEADER_INDEX_SHAPE_GROUP_COUNT,
49 OSLO_HEADER_INDEX_CHIRP_LENGTH,
50 OSLO_HEADER_INDEX_SADC_VAL,
51 OSLO_HEADER_INDEX_NUM,
52 };
53
54 const uint16_t SYNC_WORD = 0x0000;
55
56 static struct frame_sync {
57 pthread_mutex_t mutex;
58 pthread_cond_t condition;
59 bool ready;
60 } f_sync = {PTHREAD_MUTEX_INITIALIZER, PTHREAD_COND_INITIALIZER, false};
61
frame_sync_signal(struct frame_sync * fs)62 static void frame_sync_signal(struct frame_sync *fs) {
63 pthread_mutex_lock(&fs->mutex);
64 fs->ready = true;
65 pthread_cond_signal(&fs->condition);
66 pthread_mutex_unlock(&fs->mutex);
67 }
68
frame_sync_wait(struct frame_sync * fs)69 static void frame_sync_wait(struct frame_sync *fs) {
70 int ret;
71 pthread_mutex_lock(&fs->mutex);
72 while (!fs->ready) {
73 ret = pthread_cond_wait(&fs->condition, &fs->mutex);
74 }
75 fs->ready = false;
76 pthread_mutex_unlock(&fs->mutex);
77 }
78
frame_sync_timer_handler(int signum __unused)79 static void frame_sync_timer_handler(int signum __unused) {
80 ALOGV("%s", __func__);
81 frame_sync_signal(&f_sync);
82 }
83
frame_sync_timer_enable(bool en,uint32_t period_ms)84 static void frame_sync_timer_enable(bool en, uint32_t period_ms) {
85 struct itimerval tv;
86 struct sigaction sa;
87
88 memset(&sa, 0, sizeof(sa));
89 memset(&tv, 0, sizeof(tv));
90
91 if (en) {
92 sa.sa_handler = frame_sync_timer_handler;
93 sigaction(SIGALRM, &sa, NULL);
94
95 tv.it_interval.tv_sec = period_ms / 1000;
96 tv.it_interval.tv_usec = period_ms * 1000;
97 tv.it_value.tv_sec = period_ms / 1000;
98 tv.it_value.tv_usec = period_ms * 1000;
99 setitimer(ITIMER_REAL, &tv, NULL);
100 } else {
101 setitimer(ITIMER_REAL, &tv, NULL);
102
103 sa.sa_handler = SIG_DFL;
104 sigaction(SIGALRM, &sa, NULL);
105 }
106 }
107
frame_data_validate(const uint8_t * data,size_t frame_size)108 static bool frame_data_validate(const uint8_t *data, size_t frame_size) {
109 uint16_t *header = (uint16_t *)data;
110 uint16_t frame_cnt;
111 uint16_t shape_group_count;
112 uint16_t chirp_len;
113 size_t chirp_size;
114 size_t num_chirps_per_frame;
115
116 // Validate 1st header of frame
117 if (header[OSLO_HEADER_INDEX_SYNC_0] != SYNC_WORD ||
118 header[OSLO_HEADER_INDEX_SYNC_1] != SYNC_WORD ||
119 header[OSLO_HEADER_INDEX_SHAPE_GROUP_COUNT] != 0) {
120 ALOGE("%s: Invalid frame header: %04x, %04x, %04x, %04x, %04x, %04x\n", __func__,
121 header[0], header[1], header[2], header[3], header[4], header[5]);
122 fprintf(stderr, "%s: Invalid frame header: %04x, %04x, %04x, %04x, %04x, %04x\n", __func__,
123 header[0], header[1], header[2], header[3], header[4], header[5]);
124 return false;
125 } else {
126 frame_cnt = header[OSLO_HEADER_INDEX_FRAME_COUNT];
127 shape_group_count = header[OSLO_HEADER_INDEX_SHAPE_GROUP_COUNT];
128 chirp_len = header[OSLO_HEADER_INDEX_CHIRP_LENGTH];
129 chirp_size = chirp_len * CHANNEL_NUM * BYTES_PER_SAMPLE;
130 chirp_size += OSLO_HEADER_INDEX_NUM * BYTES_PER_SAMPLE;
131 num_chirps_per_frame = frame_size / chirp_size;
132 ALOGD("%s: frame_cnt: 0x%04x, chirp_len: %d, chirp_size: %d, num_chirps_per_frame: %d\n",
133 __func__, frame_cnt, chirp_len, chirp_size, num_chirps_per_frame);
134 }
135
136 if (frame_size % chirp_size != 0) {
137 ALOGE("frame_size (%d) is not a multiple of the chirp_size (%d)!!!\n", frame_size,
138 chirp_size);
139 fprintf(stderr, "frame_size (%d) is not a multiple of the chirp_size (%d)!!!\n",
140 frame_size, chirp_size);
141 return false;
142 }
143
144 // Validate header of each chirp
145 for (int i = 0; i < num_chirps_per_frame; i++) {
146 if (header[OSLO_HEADER_INDEX_SYNC_0] != SYNC_WORD ||
147 header[OSLO_HEADER_INDEX_SYNC_1] != SYNC_WORD ||
148 header[OSLO_HEADER_INDEX_FRAME_COUNT] != frame_cnt ||
149 header[OSLO_HEADER_INDEX_SHAPE_GROUP_COUNT] != i ||
150 header[OSLO_HEADER_INDEX_CHIRP_LENGTH] != chirp_len) {
151 ALOGE("%s: Invalid %dth header: %04x, %04x, %04x, %04x, %04x, %04x\n", __func__, i,
152 header[0], header[1], header[2], header[3], header[4], header[5]);
153 fprintf(stderr, "%s: Invalid %dth chirp header: %04x, %04x, %04x, %04x, %04x, %04x\n",
154 __func__, i, header[0], header[1], header[2], header[3], header[4], header[5]);
155 return false;
156 }
157 data += chirp_size;
158 header = (uint16_t *)data;
159 }
160
161 return true;
162 }
163
show_help()164 static int show_help() {
165 fprintf(stdout,
166 "usage: oslo_data_injection_test <file_name> <frame_period_ms> <frame_size> [-s n] [-c n]\n"
167 "\n"
168 "-s n Skip n input frames\n"
169 "-c n Inject only n input frames\n");
170 exit(EXIT_FAILURE);
171 }
172
main(int argc,char * argv[])173 int main(int argc, char *argv[]) {
174 int ret = 0;
175 int frame_size = 0;
176 int frame_period_ms = 0;
177 char *file_path = NULL;
178 uint8_t frame_data_buf[FRAME_SIZE_MAX] = {0};
179 uint8_t *data_ptr;
180 uint16_t *header = (uint16_t *)frame_data_buf;
181 uint32_t frames_injected = 0;
182 uint32_t frames_processed_pre_inject = 0;
183 uint32_t frames_processed_post_inject = 0;
184 size_t bytes_written;
185 size_t frame_data_bytes_remaining;
186 struct ia_sensor_mgr *smd;
187 FILE *fid = NULL;
188 struct stat file_stat;
189 uint32_t file_size;
190 uint32_t inject_bytes_per_sec;
191 uint32_t skip_frames = 0;
192 uint32_t max_inject_frames = UINT32_MAX;
193 int c;
194
195 const struct option long_options[] = {{"skip", required_argument, NULL, 's'},
196 {"count", required_argument, NULL, 'c'},
197 {NULL, 0, NULL, 0}};
198
199 if (argc < 4) {
200 show_help();
201 } else {
202 file_path = argv[1];
203 frame_period_ms = strtol(argv[2], NULL, 0);
204 frame_size = strtol(argv[3], NULL, 0);
205 }
206
207 while ((c = getopt_long(argc, argv, "s:c:", long_options, NULL)) != -1) {
208 switch (c) {
209 case 's':
210 if (NULL == optarg) {
211 show_help();
212 } else {
213 skip_frames = strtoul(optarg, NULL, 0);
214 }
215 break;
216 case 'c':
217 if (NULL == optarg) {
218 show_help();
219 } else {
220 max_inject_frames = strtoul(optarg, NULL, 0);
221 }
222 break;
223 default:
224 show_help();
225 break;
226 }
227 }
228
229 if (frame_period_ms < 0 || frame_period_ms > FRAME_PERIOD_MS_MAX) {
230 fprintf(stderr, "Invalid frame_period_ms:%d\n", frame_period_ms);
231 ret = -EINVAL;
232 goto out;
233 }
234
235 if (frame_size < 0 || frame_size > FRAME_SIZE_MAX) {
236 fprintf(stderr, "Invalid frame_size:%d\n", frame_size);
237 ret = -EINVAL;
238 goto out;
239 }
240
241 inject_bytes_per_sec = frame_size * 1000 / frame_period_ms;
242 if (inject_bytes_per_sec > INJECT_BYTES_PER_SEC_MAX) {
243 fprintf(stderr, "Invalid bytes_per_sec:%d\n", inject_bytes_per_sec);
244 ret = -EINVAL;
245 goto out;
246 }
247
248 if (stat(file_path, &file_stat) == -1) {
249 fprintf(stderr, "Could not stat file: %s - %s\n", file_path, strerror(errno));
250 ret = -errno;
251 goto out;
252 }
253 file_size = file_stat.st_size;
254
255 smd = iaxxx_sensor_mgr_init();
256 if (NULL == smd) {
257 ALOGE("%s: ERROR Failed to init ia_sensor_mgr", __func__);
258 ret = -ENOMEM;
259 goto out;
260 }
261
262 frames_processed_pre_inject = oslo_driver_get_param(smd, SENSOR_PARAM_FRAMES_PROCESSED);
263
264 fid = fopen(file_path, "r");
265 if (fid == NULL) {
266 ALOGE("Cannot open '%s' (%s)\n", file_path, strerror(errno));
267 fprintf(stdout, "Cannot open '%s' (%s)\n", file_path, strerror(errno));
268 ret = -errno;
269 goto out_err_fopen;
270 }
271
272 frame_sync_timer_enable(true, frame_period_ms);
273
274 while (!feof(fid)) {
275 frame_data_bytes_remaining = fread(frame_data_buf, 1, frame_size, fid);
276 if (!frame_data_bytes_remaining) {
277 ALOGE("Zero bytes read\n");
278 break;
279 }
280 if (frame_data_bytes_remaining != frame_size ||
281 !frame_data_validate(frame_data_buf, frame_size)) {
282 ALOGE("Drop invalid frame, header: %04x, %04x, %04x, %04x, %04x, %04x\n", header[0],
283 header[1], header[2], header[3], header[4], header[5]);
284 fprintf(stdout, "Drop invalid frame, header: %04x, %04x, %04x, %04x, %04x, %04x\n",
285 header[0], header[1], header[2], header[3], header[4], header[5]);
286 continue;
287 }
288 if (skip_frames > 0) {
289 ALOGD("Num of frames to skip %u\n", skip_frames);
290 skip_frames--;
291 continue;
292 }
293 if (frames_injected >= max_inject_frames) {
294 ALOGD("Already injected %u frames, STOP\n", frames_injected);
295 break;
296 }
297 data_ptr = frame_data_buf;
298 while (frame_data_bytes_remaining) {
299 bytes_written = oslo_driver_set_param_blk(smd, PARAM_BLK_ID_FRAME_DATA_INJECTION,
300 data_ptr, frame_data_bytes_remaining);
301 if (bytes_written == 0) {
302 ALOGE("Failed to inject data!!!\n");
303 break;
304 }
305 frame_data_bytes_remaining -= bytes_written;
306 data_ptr += bytes_written;
307 }
308 frame_sync_wait(&f_sync);
309 if (frames_injected % 10 == 0)
310 ALOGD("Process injected frame %d, header: %04x, %04x, %04x, %04x, %04x, %04x\n",
311 frames_injected, header[0], header[1], header[2], header[3], header[4],
312 header[5]);
313 oslo_driver_set_param(smd, OSLO_CONTROL_INJECT_FRAME_READY, 1);
314 frames_injected++;
315 }
316
317 frame_sync_timer_enable(false, 0);
318
319 if (fid)
320 fclose(fid);
321
322 frames_processed_post_inject = oslo_driver_get_param(smd, SENSOR_PARAM_FRAMES_PROCESSED);
323
324 ALOGD("A total of %d frames were injected\n", frames_injected);
325 ALOGD("Frames processed pre:%d, post:%d\n", frames_processed_pre_inject,
326 frames_processed_post_inject);
327
328 out_err_fopen:
329 iaxxx_sensor_mgr_deinit(smd);
330 out:
331
332 return ret;
333 }
334