1 /*
2 * Copyright (C) 2018 Knowles Electronics
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 #include <stdio.h>
18 #include <stdlib.h>
19 #include <string.h>
20 #include <signal.h>
21 #include <stdbool.h>
22 #include <errno.h>
23 #include <sys/ioctl.h>
24 #include <unistd.h>
25
26 #define LOG_TAG "ia_tunneling_sensor_test"
27
28 #include <cutils/log.h>
29 #include <linux/mfd/adnc/iaxxx-system-identifiers.h>
30 #include <linux/mfd/adnc/iaxxx-tunnel-intf.h>
31 #include <linux/mfd/adnc/iaxxx-sensor-tunnel.h>
32
33 #define MAX_TUNNELS 32
34 #define BUF_SIZE 8192
35 #define OUTPUT_FILE "/data/data/tnl_op"
36 #define UNPARSED_OUTPUT_FILE "/data/data/unparsed_output"
37
38 #define TUNNEL_DEVICE "/dev/sensor_tunnel"
39 #define SENSOR_TUNNEL_SOURCE_ID IAXXX_SYSID_CHANNEL_RX_15_EP_0
40 #define SENSOR_TUNNEL_MODE TNL_MODE_ASYNC
41 #define SENSOR_TUNNEL_ENCODE TNL_ENC_Q15
42 #define VSYNC_SENSOR_SOURCE_ID IAXXX_SYSID_SENSOR_OUTPUT_1
43 #define VSYNC_SENSOR_MODE TNL_MODE_ASYNC
44 #define VSYNC_SENSOR_ENCODE TNL_ENC_OPAQUE
45
46 struct raf_format_type {
47 uint16_t frameSizeInBytes; // Frame length in bytes
48 uint8_t encoding; // Encoding
49 uint8_t sampleRate; // Sample rate
50 };
51
52 struct raf_frame_type {
53 uint64_t timeStamp; // Timestamp of the frame
54 uint32_t seqNo; // Optional sequence number of the frame
55
56 struct raf_format_type format; // Format information for the frame
57 uint32_t data[0]; /* Start of the variable size payload. It must start at
58 128 bit aligned address for all the frames */
59 };
60
61 volatile int capturing = 1;
62
sigint_handler(int sig __unused)63 void sigint_handler(int sig __unused) {
64 ALOGE("Interrupted, setting the exit condition");
65 capturing = 0;
66 }
67
main(int argc,char * argv[])68 int main(int argc, char *argv[]) {
69 int err = 0;
70 FILE *tun_dev = NULL;
71 FILE *out_fp[MAX_TUNNELS] = { NULL };
72 FILE *unp_out_fp = NULL;
73 int bytes_avail = 0, bytes_rem = 0;
74 int bytes_read = 0;
75 void *buf = NULL;
76 // The magic number is ROME in ASCII reversed.
77 // So we are looking for EMOR in the byte stream
78 const unsigned char magic_num[4] = {0x45, 0x4D, 0x4F, 0x52};
79 int i = 0;
80 bool valid_frame = true;
81 int timer_signal = 0;
82 int lastSeqNum[MAX_TUNNELS] = { 0 };
83 int notFirstFrame[MAX_TUNNELS] = { 0 };
84 int frameDropCount[MAX_TUNNELS] = { 0 };
85 uint64_t tunnel_time_stamps[MAX_TUNNELS] = { 0 };
86 unsigned char *frame_start, *buf_itr;
87 // Minimum bytes required is the magic number + tunnel id + reserved and
88 // crc + raf struct
89 int min_bytes_req = 4 + 2 + 6 + sizeof(struct raf_frame_type);
90 struct tunlMsg tm;
91
92 if (argc < 2) {
93 ALOGE("USAGE: %s <Time in seconds> ", argv[0]);
94 return -EINVAL;
95 }
96
97 timer_signal= strtol(argv[1], NULL, 0);
98 ALOGD("tunnel out timer based req %d", timer_signal);
99
100 tun_dev = fopen(TUNNEL_DEVICE, "r");
101 if (NULL == tun_dev) {
102 ALOGE("Failed to open the tunnel device node");
103 goto exit;
104 }
105
106 err = ioctl(fileno(tun_dev), FLICKER_ROUTE_SETUP, NULL);
107 if (err == -1) {
108 ALOGE("%s: ERROR Tunnel setup failed for VSYNC sensor %s", __func__, strerror(errno));
109 goto exit;
110 }
111
112 tm.tunlSrc = VSYNC_SENSOR_SOURCE_ID;
113 tm.tunlMode = VSYNC_SENSOR_MODE;
114 tm.tunlEncode = VSYNC_SENSOR_ENCODE;
115 err = ioctl(fileno(tun_dev), FLICKER_TUNNEL_SETUP, &tm);
116 if (err == -1) {
117 ALOGE("%s: ERROR Tunnel setup failed for VSYNC sensor %s", __func__, strerror(errno));
118 goto exit;
119 }
120
121 tm.tunlSrc = SENSOR_TUNNEL_SOURCE_ID;
122 tm.tunlMode = SENSOR_TUNNEL_MODE;
123 tm.tunlEncode = SENSOR_TUNNEL_ENCODE;
124 err = ioctl(fileno(tun_dev), FLICKER_TUNNEL_SETUP, &tm);
125 if (err == -1) {
126 ALOGE("%s: ERROR Tunnel setup failed %s", __func__, strerror(errno));
127 goto exit;
128 }
129
130 buf = malloc(BUF_SIZE * 2);
131 if (NULL == buf) {
132 ALOGE("Failed to allocate memory to read buffer");
133 goto exit;
134 }
135
136 unp_out_fp = fopen(UNPARSED_OUTPUT_FILE, "wb");
137 if (NULL == unp_out_fp) {
138 ALOGE("Failed to open the file %s", UNPARSED_OUTPUT_FILE);
139 goto exit;
140 }
141
142 signal(SIGINT, sigint_handler);
143
144 if (timer_signal) {
145 signal(SIGALRM, sigint_handler);
146 alarm(timer_signal);
147 }
148
149 unsigned short int tunnel_id;
150 unsigned short int tunl_src;
151 while (1) {
152 read_again:
153 if (0 == capturing) {
154 ALOGE("Time to bail from here");
155 break;
156 }
157
158 if (0 != bytes_avail) {
159 if (bytes_avail < 0) {
160 bytes_rem = 0;
161 } else {
162 bytes_rem = bytes_avail;
163 ALOGD("bytes_avail is %d", bytes_rem);
164 memcpy(buf, buf_itr, bytes_rem);
165 }
166 } else {
167 bytes_rem = 0;
168 }
169
170 // Ensure that we read BUF_SIZE always otherwise kernel read will hang
171 bytes_avail = fread((void *)((unsigned char *)buf + bytes_rem),
172 1, BUF_SIZE, tun_dev);
173 if (bytes_avail <= 0) {
174 ALOGE("Failed to read data from the tunnel");
175 break;
176 }
177
178 fwrite((void *)((unsigned char *)buf + bytes_rem),
179 bytes_avail, 1, unp_out_fp);
180 fflush(unp_out_fp);
181
182 // update the available bytes with the previous reminder if any
183 bytes_avail += bytes_rem;
184 ALOGD("bytes_avail is after read %d", bytes_avail);
185 buf_itr = (unsigned char *)buf;
186
187 do {
188 // Check for MagicNumber 0x454D4F52
189 if (buf_itr[0] != magic_num[0] || buf_itr[1] != magic_num[1] ||
190 buf_itr[2] != magic_num[2] || buf_itr[3] != magic_num[3]) {
191 ALOGE("Could not find the magic number, reading again");
192 ALOGE("buf_itr[0] %x buf_itr[1] %x buf_itr[2] %x "
193 "buf_itr[3] %x ", buf_itr[0], buf_itr[1], buf_itr[2],
194 buf_itr[3]);
195 goto exit;
196 }
197 ALOGD("bytes_avail is after magic %d: prev :%d",
198 bytes_avail, bytes_avail + 540);
199 // Bookmark the start of the frame
200 frame_start = buf_itr;
201
202 // Skip the magic number
203 buf_itr += 4;
204 bytes_avail -= 4;
205
206 // Read the tunnelID
207 tunnel_id = ((unsigned char) (buf_itr[0]) |
208 (unsigned char) (buf_itr[1]) << 8);
209
210 // Skip tunnelID
211 buf_itr += 2;
212 bytes_avail -= 2;
213
214 tunl_src = ((unsigned char) (buf_itr[0]) |
215 (unsigned char) (buf_itr[1]) << 8);
216
217 // Skip src id field and CRC - 6 bytes in total
218 buf_itr += 6;
219 bytes_avail -= 6;
220
221 valid_frame = true;
222 if (tunnel_id > MAX_TUNNELS) {
223 ALOGE("Invalid tunnel id %d", tunnel_id);
224 valid_frame = false;
225 }
226
227 struct raf_frame_type rft;
228 memcpy(&rft, buf_itr, sizeof(struct raf_frame_type));
229 if (true == valid_frame) {
230 if (NULL == out_fp[tunnel_id]) {
231 char filename[256];
232 snprintf(filename, 256,
233 "%sid%d-src0x%x-enc0x%x.raw",
234 OUTPUT_FILE, tunnel_id,
235 tunl_src, rft.format.encoding);
236 // Open the file to dump
237 out_fp[tunnel_id] = fopen(filename, "wb");
238 if (NULL == out_fp[tunnel_id]) {
239 ALOGE("ERROR: Failed to open the file %s", filename);
240 goto exit;
241 }
242 }
243 }
244
245 ALOGD("Tunnel id %d timestamp %llu", tunnel_id, rft.timeStamp);
246 tunnel_time_stamps[tunnel_id] = rft.timeStamp;
247
248 // Skip the raf_frame_type
249 buf_itr += sizeof(struct raf_frame_type);
250 bytes_avail -= sizeof(struct raf_frame_type);
251
252 if (bytes_avail < rft.format.frameSizeInBytes) {
253 ALOGD("Incomplete frame received bytes_avail %d framesize %d",
254 bytes_avail, rft.format.frameSizeInBytes);
255 buf_itr = frame_start;
256 bytes_avail += min_bytes_req;
257 goto read_again;
258 }
259
260 if (true == valid_frame) {
261 ALOGD("@@@Tunnel id %d encoding %d",
262 tunnel_id, rft.format.encoding);
263 fwrite(buf_itr, rft.format.frameSizeInBytes, 1, out_fp[tunnel_id]);
264 }
265
266 /* Calculate the frame drop count */
267 if (notFirstFrame[tunnel_id]) {
268 frameDropCount[tunnel_id] +=
269 (rft.seqNo - lastSeqNum[tunnel_id] - 1);
270 }
271 lastSeqNum[tunnel_id] = rft.seqNo;
272 notFirstFrame[tunnel_id] = 1;
273 // Skip the data
274 buf_itr += rft.format.frameSizeInBytes;
275 bytes_avail -= rft.format.frameSizeInBytes;
276 bytes_read += rft.format.frameSizeInBytes + min_bytes_req;
277 } while (bytes_avail > min_bytes_req);
278 }
279
280 exit:
281 for (i = 0; i < MAX_TUNNELS; i++) {
282 if (notFirstFrame[i]) {
283 ALOGD("drop count tunnel id %u: %u", i, frameDropCount[i]);
284 }
285 }
286 ALOGD("bytes_read so far %d", bytes_read);
287 if (buf) {
288 free(buf);
289 buf = NULL;
290 }
291
292 if (unp_out_fp) {
293 fflush(unp_out_fp);
294 fclose(unp_out_fp);
295 }
296
297 for (i = 0; i < MAX_TUNNELS; i++) {
298 if (out_fp[i]) {
299 fflush(out_fp[i]);
300 fclose(out_fp[i]);
301 }
302 }
303
304 tm.tunlSrc = SENSOR_TUNNEL_SOURCE_ID;
305 tm.tunlMode = SENSOR_TUNNEL_MODE;
306 tm.tunlEncode = SENSOR_TUNNEL_ENCODE;
307 err = ioctl(fileno(tun_dev), FLICKER_TUNNEL_TERMINATE, &tm);
308 if (err == -1) {
309 ALOGE("%s: ERROR Tunnel terminate failed %s",
310 __func__, strerror(errno));
311 }
312
313 tm.tunlSrc = VSYNC_SENSOR_SOURCE_ID;
314 tm.tunlMode = VSYNC_SENSOR_MODE;
315 tm.tunlEncode = VSYNC_SENSOR_ENCODE;
316 err = ioctl(fileno(tun_dev), FLICKER_TUNNEL_TERMINATE, &tm);
317 if (err == -1) {
318 ALOGE("%s: ERROR Tunnel terminate failed %s",
319 __func__, strerror(errno));
320 }
321
322 err = ioctl(fileno(tun_dev), FLICKER_ROUTE_TERMINATE, NULL);
323 if (err == -1) {
324 ALOGE("%s: ERROR Tunnel terminate failed %s",
325 __func__, strerror(errno));
326 }
327
328 if (tun_dev)
329 fclose(tun_dev);
330
331 return 0;
332 }
333