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