1 /******************************************************************************
2 *
3 * Copyright (C) 2009-2018 Realtek Corporation.
4 *
5 * Licensed under the Apache License, Version 2.0 (the "License");
6 * you may not use this file except in compliance with the License.
7 * You may obtain a copy of the License at:
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 *
17 ******************************************************************************/
18 #define LOG_TAG "rtk_btsnoop_net"
19 #include <unistd.h>
20 #include "rtk_btsnoop_net.h"
21 #include "bt_vendor_rtk.h"
22
23 #define RTK_NO_INTR(fn) \
24 do \
25 { \
26 } while ((fn) == -1 && errno == EINTR)
27
28 #define DATA_DIRECT_2_ELLISY 1
29
30 #define HCI_COMMAND 0x01
31 #define HCI_ACL_DATA_H2C 0x02
32 #define HCI_ACL_DATA_C2H 0x82
33 #define HCI_SCO_DATA_H2C 0x03
34 #define HCI_SCO_DATA_C2H 0x83
35 #define HCI_EVENT 0x84
36
37 #define HCI_COMMAND_PKT 0x01
38 #define HCI_ACLDATA_PKT 0x02
39 #define HCI_SCODATA_PKT 0x03
40 #define HCI_EVENT_PKT 0x04
41
42 unsigned int rtkbt_h5logfilter = 0x01;
43 bool rtk_btsnoop_dump = false;
44 bool rtk_btsnoop_net_dump = false;
45 bool rtk_btsnoop_save_log = false;
46 char rtk_btsnoop_path[1024] = {'\0'};
47 static pthread_mutex_t btsnoop_log_lock;
48
49 static void rtk_safe_close_(int *fd);
50 static void *rtk_listen_fn_(void *context);
51
52 static const char *RTK_LISTEN_THREAD_NAME_ = "rtk_btsnoop_net";
53 static const int RTK_LOCALHOST_ = 0xC0A80AE2; // 192.168.10.226
54 static const int RTK_LISTEN_PORT_ = 8872;
55
56 static const int RTK_REMOTEHOST_ = 0xC0A80A03; // 192.168.10.21
57 static const int RTK_REMOTE_PORT_ = 24352;
58
59 static pthread_t rtk_listen_thread_;
60 static bool rtk_listen_thread_valid_ = false;
61 static pthread_mutex_t rtk_client_socket_lock_ = PTHREAD_MUTEX_INITIALIZER;
62 static int rtk_listen_socket_ = -1;
63
64 // File descriptor for btsnoop file.
65 static int hci_btsnoop_fd = -1;
66 // Epoch in microseconds since 01/01/0000.
67 static const uint64_t BTSNOOP_EPOCH_DELTA = 0x00dcddb30f2f8000ULL;
68
rtk_btsnoop_timestamp(void)69 static uint64_t rtk_btsnoop_timestamp(void)
70 {
71 struct timeval tv;
72 gettimeofday(&tv, NULL);
73
74 // Timestamp is in microseconds.
75 uint64_t timestamp = tv.tv_sec * 1000LL * 1000LL;
76 timestamp += tv.tv_usec;
77 timestamp += BTSNOOP_EPOCH_DELTA;
78 return timestamp;
79 }
80
rtk_btsnoop_open()81 void rtk_btsnoop_open()
82 {
83 pthread_mutex_init(&btsnoop_log_lock, NULL);
84 char last_log_path[PATH_MAX];
85 uint64_t timestamp;
86 uint32_t usec;
87 uint8_t sec, hour, minus, day;
88
89 if (hci_btsnoop_fd != -1)
90 {
91 HILOGE("%s btsnoop log file is already open.", __func__);
92 return;
93 }
94
95 if (rtk_btsnoop_save_log)
96 {
97 timestamp = rtk_btsnoop_timestamp() - BTSNOOP_EPOCH_DELTA;
98 usec = (uint32_t)(timestamp % 1000000LL);
99 timestamp /= 1000000LL;
100 sec = (uint8_t)(timestamp % 60LL);
101 timestamp /= 60LL;
102 minus = (uint8_t)(timestamp % 60LL);
103 timestamp /= 60LL;
104 hour = (uint8_t)(timestamp % 24LL);
105 timestamp /= 24LL;
106 day = (uint8_t)(timestamp % 30LL);
107 timestamp /= 30LL;
108 // snprintf(last_log_path, PATH_MAX, "%s.%llu", rtk_btsnoop_path, rtk_btsnoop_timestamp());
109 snprintf(last_log_path, PATH_MAX, "%s.%uY-%dD-%dH-%dM-%dS-%dUS", rtk_btsnoop_path,
110 (uint32_t)timestamp, day, hour, minus, sec, usec);
111 if (!rename(rtk_btsnoop_path, last_log_path) && errno != ENOENT)
112 HILOGE("%s unable to rename '%s' to '%s': %s", __func__, rtk_btsnoop_path, last_log_path, strerror(errno));
113 }
114 else
115 {
116 snprintf(last_log_path, PATH_MAX, "%s.last", rtk_btsnoop_path);
117 if (!rename(rtk_btsnoop_path, last_log_path) && errno != ENOENT)
118 HILOGE("%s unable to rename '%s' to '%s': %s", __func__, rtk_btsnoop_path, last_log_path, strerror(errno));
119 }
120
121 hci_btsnoop_fd = open(rtk_btsnoop_path,
122 O_WRONLY | O_CREAT | O_TRUNC,
123 S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH);
124
125 if (hci_btsnoop_fd == -1)
126 {
127 HILOGE("%s unable to open '%s': %s", __func__, rtk_btsnoop_path, strerror(errno));
128 return;
129 }
130
131 write(hci_btsnoop_fd, "btsnoop\0\0\0\0\1\0\0\x3\xea", 16);
132 }
133
rtk_btsnoop_close(void)134 void rtk_btsnoop_close(void)
135 {
136 pthread_mutex_destroy(&btsnoop_log_lock);
137 if (hci_btsnoop_fd != -1)
138 close(hci_btsnoop_fd);
139 hci_btsnoop_fd = -1;
140 }
141
rtk_btsnoop_write(const void * data,size_t length)142 static void rtk_btsnoop_write(const void *data, size_t length)
143 {
144 if (hci_btsnoop_fd != -1)
145 write(hci_btsnoop_fd, data, length);
146 }
147
rtk_btsnoop_write_packet(serial_data_type_t type,const uint8_t * packet,bool is_received)148 static void rtk_btsnoop_write_packet(serial_data_type_t type, const uint8_t *packet, bool is_received)
149 {
150 int length_he = 0;
151 int length;
152 int flags;
153 int drops = 0;
154 pthread_mutex_lock(&btsnoop_log_lock);
155 switch (type)
156 {
157 case HCI_COMMAND_PKT:
158 length_he = packet[2] + 4;
159 flags = 2;
160 break;
161 case HCI_ACLDATA_PKT:
162 length_he = (packet[3] << 8) + packet[2] + 5;
163 flags = is_received;
164 break;
165 case HCI_SCODATA_PKT:
166 length_he = packet[2] + 4;
167 flags = is_received;
168 break;
169 case HCI_EVENT_PKT:
170 length_he = packet[1] + 3;
171 flags = 3;
172 break;
173 default:
174 break;
175 }
176
177 uint64_t timestamp = rtk_btsnoop_timestamp();
178 uint32_t time_hi = timestamp >> 32;
179 uint32_t time_lo = timestamp & 0xFFFFFFFF;
180
181 length = htonl(length_he);
182 flags = htonl(flags);
183 drops = htonl(drops);
184 time_hi = htonl(time_hi);
185 time_lo = htonl(time_lo);
186
187 rtk_btsnoop_write(&length, 4);
188 rtk_btsnoop_write(&length, 4);
189 rtk_btsnoop_write(&flags, 4);
190 rtk_btsnoop_write(&drops, 4);
191 rtk_btsnoop_write(&time_hi, 4);
192 rtk_btsnoop_write(&time_lo, 4);
193 rtk_btsnoop_write(&type, 1);
194 rtk_btsnoop_write(packet, length_he - 1);
195 pthread_mutex_unlock(&btsnoop_log_lock);
196 }
197
rtk_btsnoop_capture(const HC_BT_HDR * p_buf,bool is_rcvd)198 void rtk_btsnoop_capture(const HC_BT_HDR *p_buf, bool is_rcvd)
199 {
200 const uint8_t *p = (const uint8_t *)(p_buf + 1) + p_buf->offset;
201
202 if (hci_btsnoop_fd == -1)
203 return;
204
205 switch (p_buf->event & MSG_EVT_MASK)
206 {
207 case MSG_HC_TO_STACK_HCI_EVT:
208 if ((*(p + 3) == 0x94) && (*(p + 4) == 0xfc) && (*(p + 5) == 0x00) && (rtkbt_h5logfilter & 1))
209 {
210 }
211 else
212 {
213 rtk_btsnoop_write_packet(HCI_EVENT_PKT, p, false);
214 }
215 break;
216 case MSG_HC_TO_STACK_HCI_ACL:
217 case MSG_STACK_TO_HC_HCI_ACL:
218 rtk_btsnoop_write_packet(HCI_ACLDATA_PKT, p, is_rcvd);
219 break;
220 case MSG_HC_TO_STACK_HCI_SCO:
221 case MSG_STACK_TO_HC_HCI_SCO:
222 rtk_btsnoop_write_packet(HCI_SCODATA_PKT, p, is_rcvd);
223 break;
224 case MSG_STACK_TO_HC_HCI_CMD:
225 if (((rtkbt_h5logfilter & 1) == 0) || (*p != 0x94) || (*(p + 1) != 0xfc))
226 rtk_btsnoop_write_packet(HCI_COMMAND_PKT, p, true);
227 break;
228 }
229 }
230
rtk_btsnoop_net_open()231 void rtk_btsnoop_net_open()
232 {
233 rtk_listen_thread_valid_ = (pthread_create(&rtk_listen_thread_, NULL, rtk_listen_fn_, NULL) == 0);
234 if (!rtk_listen_thread_valid_)
235 {
236 HILOGE("%s pthread_create failed: %s", __func__, strerror(errno));
237 }
238 else
239 {
240 HILOGD("initialized");
241 }
242 }
243
rtk_btsnoop_net_close()244 void rtk_btsnoop_net_close()
245 {
246 if (rtk_listen_thread_valid_)
247 {
248 shutdown(rtk_listen_socket_, SHUT_RDWR);
249 pthread_join(rtk_listen_thread_, NULL);
250 rtk_listen_thread_valid_ = false;
251 }
252 }
253
rtk_btsnoop_net_write(serial_data_type_t type,uint8_t * data,bool is_received)254 void rtk_btsnoop_net_write(serial_data_type_t type, uint8_t *data, bool is_received)
255 {
256 if (rtk_listen_socket_ == -1)
257 {
258 return;
259 }
260 int length = 0;
261 uint8_t *p = data;
262
263 switch (type)
264 {
265 case HCI_COMMAND_PKT:
266 if (((rtkbt_h5logfilter & 1) == 0) || (*p != 0x94) || (*(p + 1) != 0xfc))
267 length = data[2] + 3;
268 else
269 return;
270 break;
271 case HCI_ACLDATA_PKT:
272 length = (data[3] << 8) + data[2] + 4;
273 break;
274 case HCI_SCODATA_PKT:
275 length = data[2] + 3;
276 break;
277 case HCI_EVENT_PKT:
278 if ((*(p + 3) == 0x94) && (*(p + 4) == 0xfc) && (*(p + 5) == 0x00) && (rtkbt_h5logfilter & 1))
279 {
280 return;
281 }
282 else
283 length = data[1] + 2;
284 break;
285 default:
286 break;
287 }
288
289 uint8_t buffer[4126] = {0};
290 // uint8_t test_buffer[] = {0x03, 0x00, 0x00, 0x00, 0x01, 0x01, 0x10, 0x00};
291 // uint8_t test_buffer2[] = {0x01, 0x10, 0x00};
292 struct sockaddr_in client_addr;
293 int i = 0;
294
295 #if DATA_DIRECT_2_ELLISY
296 uint8_t bit_rate[4] = {0x00, 0x1b, 0x37, 0x4b};
297 struct tm *t;
298 time_t tt;
299 time(&tt);
300 t = localtime(&tt);
301
302 struct timeval tv;
303 gettimeofday(&tv, NULL);
304
305 uint64_t nano_time = (t->tm_hour * 3600 + t->tm_min * 60 + t->tm_sec) * 1000 * 1000LL * 1000 + tv.tv_usec * 1000;
306 uint16_t year = (t->tm_year + 1900) & 0xFFFF;
307 uint8_t month = (t->tm_mon + 1) & 0xFF;
308 uint8_t day =
309 buffer[0] = 0x02;
310 buffer[1] = 0x00;
311 buffer[2] = 0x01;
312 buffer[3] = 0x02;
313 // time
314 memcpy(&buffer[4], &year, 2);
315 buffer[6] = month;
316 buffer[7] = day;
317 memcpy(&buffer[8], &nano_time, 6);
318 // bit rate
319 buffer[14] = 0x80;
320 memcpy(&buffer[15], bit_rate, 4);
321 // type
322 buffer[19] = 0x81;
323 i = 20;
324 #else
325 memcpy(&buffer[i], &length, sizeof(int));
326 i = 4;
327 #endif
328 switch (type)
329 {
330 case HCI_COMMAND_PKT:
331 buffer[i] = HCI_COMMAND;
332 break;
333
334 case HCI_ACLDATA_PKT:
335 if (is_received)
336 {
337 buffer[i] = HCI_ACL_DATA_C2H;
338 }
339 else
340 {
341 buffer[i] = HCI_ACL_DATA_H2C;
342 }
343 break;
344
345 case HCI_SCODATA_PKT:
346 if (is_received)
347 {
348 buffer[i] = HCI_SCO_DATA_C2H;
349 }
350 else
351 {
352 buffer[i] = HCI_SCO_DATA_H2C;
353 }
354 break;
355
356 case HCI_EVENT_PKT:
357 buffer[i] = HCI_EVENT;
358 break;
359
360 default:
361 buffer[i] = 0;
362 break;
363 }
364 #if DATA_DIRECT_2_ELLISY
365 // buffer[i] = HCI_COMMAND;
366 buffer[21] = 0x82;
367 i = 22;
368 #else
369 i = 5;
370 #endif
371 memcpy(&buffer[i], data, length);
372 // memcpy(&buffer[i], test_buffer2, 3);
373 memset(&client_addr, 0, sizeof(client_addr));
374 client_addr.sin_family = AF_INET;
375 client_addr.sin_addr.s_addr = htonl(RTK_REMOTEHOST_);
376 client_addr.sin_port = htons(RTK_REMOTE_PORT_);
377 pthread_mutex_lock(&rtk_client_socket_lock_);
378 int ret;
379 RTK_NO_INTR(ret = sendto(rtk_listen_socket_, buffer, (length + i), 0, (struct sockaddr *)&client_addr, sizeof(struct sockaddr_in)));
380 // sendto(rtk_listen_socket_, buffer, 25, 0,(struct sockaddr*)&client_addr, sizeof(struct sockaddr_in));
381 pthread_mutex_unlock(&rtk_client_socket_lock_);
382 }
383
rtk_listen_fn_(void * context)384 static void *rtk_listen_fn_(void *context)
385 {
386 RTK_UNUSED(context);
387 prctl(PR_SET_NAME, (unsigned long)RTK_LISTEN_THREAD_NAME_, 0, 0, 0);
388
389 rtk_listen_socket_ = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
390 if (rtk_listen_socket_ == -1)
391 {
392 HILOGE("%s socket creation failed: %s", __func__, strerror(errno));
393 goto cleanup;
394 }
395
396 struct sockaddr_in addr;
397 memset(&addr, 0, sizeof(addr));
398 addr.sin_family = AF_INET;
399 addr.sin_addr.s_addr = htonl(RTK_LOCALHOST_);
400 addr.sin_port = htons(RTK_LISTEN_PORT_);
401
402 struct sockaddr_in client_addr;
403 memset(&client_addr, 0, sizeof(client_addr));
404 client_addr.sin_family = AF_INET;
405 client_addr.sin_addr.s_addr = htonl(RTK_REMOTEHOST_);
406 client_addr.sin_port = htons(RTK_REMOTE_PORT_);
407
408 if (bind(rtk_listen_socket_, (struct sockaddr *)&addr, sizeof(addr)) == -1)
409 {
410 HILOGE("%s unable to bind listen socket: %s", __func__, strerror(errno));
411 goto cleanup;
412 }
413
414 return NULL;
415 cleanup:
416 rtk_safe_close_(&rtk_listen_socket_);
417 return NULL;
418 }
419
rtk_safe_close_(int * fd)420 static void rtk_safe_close_(int *fd)
421 {
422 assert(fd != NULL);
423 if (*fd != -1)
424 {
425 close(*fd);
426 *fd = -1;
427 }
428 }
429