1 /* 2 * 3 * BlueZ - Bluetooth protocol stack for Linux 4 * 5 * Copyright (C) 2000-2002 Maxim Krasnyansky <maxk (at) qualcomm.com> 6 * Copyright (C) 2003-2010 Marcel Holtmann <marcel (at) holtmann.org> 7 * 8 * 9 * This program is free software; you can redistribute it and/or modify 10 * it under the terms of the GNU General Public License as published by 11 * the Free Software Foundation; either version 2 of the License, or 12 * (at your option) any later version. 13 * 14 * This program is distributed in the hope that it will be useful, 15 * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 * GNU General Public License for more details. 18 * 19 * You should have received a copy of the GNU General Public License 20 * along with this program; if not, write to the Free Software 21 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 22 * 23 */ 24 25 #ifdef HAVE_CONFIG_H 26 #include <config.h> 27 #endif 28 29 #include <netinet/in.h> 30 #include <stdio.h> 31 #include <errno.h> 32 #include <ctype.h> 33 #include <fcntl.h> 34 #include <unistd.h> 35 #include <stdlib.h> 36 #include <stdint.h> 37 #include <string.h> 38 #include <signal.h> 39 #include <getopt.h> 40 #include <syslog.h> 41 #include <sys/time.h> 42 #include <sys/stat.h> 43 #include <sys/poll.h> 44 #include <sys/ioctl.h> 45 #include <sys/socket.h> 46 #include <sys/resource.h> 47 48 #include <bluetooth/bluetooth.h> 49 #include <bluetooth/hci.h> 50 #include <bluetooth/hci_lib.h> 51 52 #include <netdb.h> 53 54 #include <glib.h> 55 56 #if __BYTE_ORDER == __LITTLE_ENDIAN 57 static inline uint64_t ntoh64(uint64_t n) 58 { 59 uint64_t h; 60 uint64_t tmp = ntohl(n & 0x00000000ffffffff); 61 h = ntohl(n >> 32); 62 h |= tmp << 32; 63 return h; 64 } 65 #elif __BYTE_ORDER == __BIG_ENDIAN 66 #define ntoh64(x) (x) 67 #else 68 #error "Unknown byte order" 69 #endif 70 #define hton64(x) ntoh64(x) 71 72 #define GHCI_DEV "/dev/ghci" 73 74 #define VHCI_DEV "/dev/vhci" 75 #define VHCI_UDEV "/dev/hci_vhci" 76 77 #define VHCI_MAX_CONN 12 78 79 #define VHCI_ACL_MTU 192 80 #define VHCI_ACL_MAX_PKT 8 81 82 struct vhci_device { 83 uint8_t features[8]; 84 uint8_t name[248]; 85 uint8_t dev_class[3]; 86 uint8_t inq_mode; 87 uint8_t eir_fec; 88 uint8_t eir_data[240]; 89 uint16_t acl_cnt; 90 bdaddr_t bdaddr; 91 int fd; 92 int dd; 93 GIOChannel *scan; 94 }; 95 96 struct vhci_conn { 97 bdaddr_t dest; 98 uint16_t handle; 99 GIOChannel *chan; 100 }; 101 102 struct vhci_link_info { 103 bdaddr_t bdaddr; 104 uint8_t dev_class[3]; 105 uint8_t link_type; 106 uint8_t role; 107 } __attribute__ ((packed)); 108 109 static struct vhci_device vdev; 110 static struct vhci_conn *vconn[VHCI_MAX_CONN]; 111 112 struct btsnoop_hdr { 113 uint8_t id[8]; /* Identification Pattern */ 114 uint32_t version; /* Version Number = 1 */ 115 uint32_t type; /* Datalink Type */ 116 } __attribute__ ((packed)); 117 #define BTSNOOP_HDR_SIZE (sizeof(struct btsnoop_hdr)) 118 119 struct btsnoop_pkt { 120 uint32_t size; /* Original Length */ 121 uint32_t len; /* Included Length */ 122 uint32_t flags; /* Packet Flags */ 123 uint32_t drops; /* Cumulative Drops */ 124 uint64_t ts; /* Timestamp microseconds */ 125 uint8_t data[0]; /* Packet Data */ 126 } __attribute__ ((packed)); 127 #define BTSNOOP_PKT_SIZE (sizeof(struct btsnoop_pkt)) 128 129 static uint8_t btsnoop_id[] = { 0x62, 0x74, 0x73, 0x6e, 0x6f, 0x6f, 0x70, 0x00 }; 130 131 static GMainLoop *event_loop; 132 133 static volatile sig_atomic_t __io_canceled; 134 135 static inline void io_init(void) 136 { 137 __io_canceled = 0; 138 } 139 140 static inline void io_cancel(void) 141 { 142 __io_canceled = 1; 143 } 144 145 static void sig_term(int sig) 146 { 147 io_cancel(); 148 g_main_loop_quit(event_loop); 149 } 150 151 static gboolean io_acl_data(GIOChannel *chan, GIOCondition cond, gpointer data); 152 static gboolean io_conn_ind(GIOChannel *chan, GIOCondition cond, gpointer data); 153 static gboolean io_hci_data(GIOChannel *chan, GIOCondition cond, gpointer data); 154 155 static inline int read_n(int fd, void *buf, int len) 156 { 157 register int w, t = 0; 158 159 while (!__io_canceled && len > 0) { 160 if ((w = read(fd, buf, len)) < 0 ){ 161 if( errno == EINTR || errno == EAGAIN ) 162 continue; 163 return -1; 164 } 165 if (!w) 166 return 0; 167 len -= w; buf += w; t += w; 168 } 169 return t; 170 } 171 172 /* Write exactly len bytes (Signal safe)*/ 173 static inline int write_n(int fd, void *buf, int len) 174 { 175 register int w, t = 0; 176 177 while (!__io_canceled && len > 0) { 178 if ((w = write(fd, buf, len)) < 0 ){ 179 if( errno == EINTR || errno == EAGAIN ) 180 continue; 181 return -1; 182 } 183 if (!w) 184 return 0; 185 len -= w; buf += w; t += w; 186 } 187 return t; 188 } 189 190 static int create_snoop(char *file) 191 { 192 struct btsnoop_hdr hdr; 193 int fd, len; 194 195 fd = open(file, O_WRONLY | O_CREAT, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH); 196 if (fd < 0) 197 return fd; 198 199 memcpy(hdr.id, btsnoop_id, sizeof(btsnoop_id)); 200 hdr.version = htonl(1); 201 hdr.type = htonl(1002); 202 203 len = write(fd, &hdr, BTSNOOP_HDR_SIZE); 204 if (len < 0) { 205 close(fd); 206 return -EIO; 207 } 208 209 if (len != BTSNOOP_HDR_SIZE) { 210 close(fd); 211 return -1; 212 } 213 214 return fd; 215 } 216 217 static int write_snoop(int fd, int type, int incoming, unsigned char *buf, int len) 218 { 219 struct btsnoop_pkt pkt; 220 struct timeval tv; 221 uint32_t size = len; 222 uint64_t ts; 223 int err; 224 225 if (fd < 0) 226 return -1; 227 228 memset(&tv, 0, sizeof(tv)); 229 gettimeofday(&tv, NULL); 230 ts = (tv.tv_sec - 946684800ll) * 1000000ll + tv.tv_usec; 231 232 pkt.size = htonl(size); 233 pkt.len = pkt.size; 234 pkt.flags = ntohl(incoming & 0x01); 235 pkt.drops = htonl(0); 236 pkt.ts = hton64(ts + 0x00E03AB44A676000ll); 237 238 if (type == HCI_COMMAND_PKT || type == HCI_EVENT_PKT) 239 pkt.flags |= ntohl(0x02); 240 241 err = write(fd, &pkt, BTSNOOP_PKT_SIZE); 242 err = write(fd, buf, size); 243 244 return 0; 245 } 246 247 static struct vhci_conn *conn_get_by_bdaddr(bdaddr_t *ba) 248 { 249 register int i; 250 251 for (i = 0; i < VHCI_MAX_CONN; i++) 252 if (!bacmp(&vconn[i]->dest, ba)) 253 return vconn[i]; 254 255 return NULL; 256 } 257 258 static void command_status(uint16_t ogf, uint16_t ocf, uint8_t status) 259 { 260 uint8_t buf[HCI_MAX_FRAME_SIZE], *ptr = buf; 261 evt_cmd_status *cs; 262 hci_event_hdr *he; 263 264 /* Packet type */ 265 *ptr++ = HCI_EVENT_PKT; 266 267 /* Event header */ 268 he = (void *) ptr; ptr += HCI_EVENT_HDR_SIZE; 269 270 he->evt = EVT_CMD_STATUS; 271 he->plen = EVT_CMD_STATUS_SIZE; 272 273 cs = (void *) ptr; ptr += EVT_CMD_STATUS_SIZE; 274 275 cs->status = status; 276 cs->ncmd = 1; 277 cs->opcode = htobs(cmd_opcode_pack(ogf, ocf)); 278 279 write_snoop(vdev.dd, HCI_EVENT_PKT, 1, buf, ptr - buf); 280 281 if (write(vdev.fd, buf, ptr - buf) < 0) 282 syslog(LOG_ERR, "Can't send event: %s(%d)", 283 strerror(errno), errno); 284 } 285 286 static void command_complete(uint16_t ogf, uint16_t ocf, int plen, void *data) 287 { 288 uint8_t buf[HCI_MAX_FRAME_SIZE], *ptr = buf; 289 evt_cmd_complete *cc; 290 hci_event_hdr *he; 291 292 /* Packet type */ 293 *ptr++ = HCI_EVENT_PKT; 294 295 /* Event header */ 296 he = (void *) ptr; ptr += HCI_EVENT_HDR_SIZE; 297 298 he->evt = EVT_CMD_COMPLETE; 299 he->plen = EVT_CMD_COMPLETE_SIZE + plen; 300 301 cc = (void *) ptr; ptr += EVT_CMD_COMPLETE_SIZE; 302 303 cc->ncmd = 1; 304 cc->opcode = htobs(cmd_opcode_pack(ogf, ocf)); 305 306 if (plen) { 307 memcpy(ptr, data, plen); 308 ptr += plen; 309 } 310 311 write_snoop(vdev.dd, HCI_EVENT_PKT, 1, buf, ptr - buf); 312 313 if (write(vdev.fd, buf, ptr - buf) < 0) 314 syslog(LOG_ERR, "Can't send event: %s(%d)", 315 strerror(errno), errno); 316 } 317 318 static void connect_request(struct vhci_conn *conn) 319 { 320 uint8_t buf[HCI_MAX_FRAME_SIZE], *ptr = buf; 321 evt_conn_request *cr; 322 hci_event_hdr *he; 323 324 /* Packet type */ 325 *ptr++ = HCI_EVENT_PKT; 326 327 /* Event header */ 328 he = (void *) ptr; ptr += HCI_EVENT_HDR_SIZE; 329 330 he->evt = EVT_CONN_REQUEST; 331 he->plen = EVT_CONN_REQUEST_SIZE; 332 333 cr = (void *) ptr; ptr += EVT_CONN_REQUEST_SIZE; 334 335 bacpy(&cr->bdaddr, &conn->dest); 336 memset(&cr->dev_class, 0, sizeof(cr->dev_class)); 337 cr->link_type = ACL_LINK; 338 339 write_snoop(vdev.dd, HCI_EVENT_PKT, 1, buf, ptr - buf); 340 341 if (write(vdev.fd, buf, ptr - buf) < 0) 342 syslog(LOG_ERR, "Can't send event: %s (%d)", 343 strerror(errno), errno); 344 } 345 346 static void connect_complete(struct vhci_conn *conn) 347 { 348 uint8_t buf[HCI_MAX_FRAME_SIZE], *ptr = buf; 349 evt_conn_complete *cc; 350 hci_event_hdr *he; 351 352 /* Packet type */ 353 *ptr++ = HCI_EVENT_PKT; 354 355 /* Event header */ 356 he = (void *) ptr; ptr += HCI_EVENT_HDR_SIZE; 357 358 he->evt = EVT_CONN_COMPLETE; 359 he->plen = EVT_CONN_COMPLETE_SIZE; 360 361 cc = (void *) ptr; ptr += EVT_CONN_COMPLETE_SIZE; 362 363 bacpy(&cc->bdaddr, &conn->dest); 364 cc->status = 0x00; 365 cc->handle = htobs(conn->handle); 366 cc->link_type = ACL_LINK; 367 cc->encr_mode = 0x00; 368 369 write_snoop(vdev.dd, HCI_EVENT_PKT, 1, buf, ptr - buf); 370 371 if (write(vdev.fd, buf, ptr - buf) < 0) 372 syslog(LOG_ERR, "Can't send event: %s (%d)", 373 strerror(errno), errno); 374 } 375 376 static void disconn_complete(struct vhci_conn *conn) 377 { 378 uint8_t buf[HCI_MAX_FRAME_SIZE], *ptr = buf; 379 evt_disconn_complete *dc; 380 hci_event_hdr *he; 381 382 /* Packet type */ 383 *ptr++ = HCI_EVENT_PKT; 384 385 /* Event header */ 386 he = (void *) ptr; ptr += HCI_EVENT_HDR_SIZE; 387 388 he->evt = EVT_DISCONN_COMPLETE; 389 he->plen = EVT_DISCONN_COMPLETE_SIZE; 390 391 dc = (void *) ptr; ptr += EVT_DISCONN_COMPLETE_SIZE; 392 393 dc->status = 0x00; 394 dc->handle = htobs(conn->handle); 395 dc->reason = 0x00; 396 397 write_snoop(vdev.dd, HCI_EVENT_PKT, 1, buf, ptr - buf); 398 399 if (write(vdev.fd, buf, ptr - buf) < 0) 400 syslog(LOG_ERR, "Can't send event: %s (%d)", 401 strerror(errno), errno); 402 403 vdev.acl_cnt = 0; 404 } 405 406 static void num_completed_pkts(struct vhci_conn *conn) 407 { 408 uint8_t buf[HCI_MAX_FRAME_SIZE], *ptr = buf; 409 evt_num_comp_pkts *np; 410 hci_event_hdr *he; 411 412 /* Packet type */ 413 *ptr++ = HCI_EVENT_PKT; 414 415 /* Event header */ 416 he = (void *) ptr; ptr += HCI_EVENT_HDR_SIZE; 417 418 he->evt = EVT_NUM_COMP_PKTS; 419 he->plen = EVT_NUM_COMP_PKTS_SIZE; 420 421 np = (void *) ptr; ptr += EVT_NUM_COMP_PKTS_SIZE; 422 np->num_hndl = 1; 423 424 *((uint16_t *) ptr) = htobs(conn->handle); ptr += 2; 425 *((uint16_t *) ptr) = htobs(vdev.acl_cnt); ptr += 2; 426 427 write_snoop(vdev.dd, HCI_EVENT_PKT, 1, buf, ptr - buf); 428 429 if (write(vdev.fd, buf, ptr - buf) < 0) 430 syslog(LOG_ERR, "Can't send event: %s (%d)", 431 strerror(errno), errno); 432 } 433 434 static int scan_enable(uint8_t *data) 435 { 436 struct sockaddr_in sa; 437 GIOChannel *sk_io; 438 bdaddr_t ba; 439 int sk, opt; 440 441 if (!(*data & SCAN_PAGE)) { 442 if (vdev.scan) { 443 g_io_channel_close(vdev.scan); 444 vdev.scan = NULL; 445 } 446 return 0; 447 } 448 449 if (vdev.scan) 450 return 0; 451 452 if ((sk = socket(AF_INET, SOCK_STREAM, 0)) < 0) { 453 syslog(LOG_ERR, "Can't create socket: %s (%d)", 454 strerror(errno), errno); 455 return 1; 456 } 457 458 opt = 1; 459 setsockopt(sk, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)); 460 461 baswap(&ba, &vdev.bdaddr); 462 sa.sin_family = AF_INET; 463 memcpy(&sa.sin_addr.s_addr, &ba, sizeof(sa.sin_addr.s_addr)); 464 sa.sin_port = *(uint16_t *) &ba.b[4]; 465 if (bind(sk, (struct sockaddr *) &sa, sizeof(sa))) { 466 syslog(LOG_ERR, "Can't bind socket: %s (%d)", 467 strerror(errno), errno); 468 goto failed; 469 } 470 471 if (listen(sk, 10)) { 472 syslog(LOG_ERR, "Can't listen on socket: %s (%d)", 473 strerror(errno), errno); 474 goto failed; 475 } 476 477 sk_io = g_io_channel_unix_new(sk); 478 g_io_add_watch(sk_io, G_IO_IN | G_IO_NVAL, io_conn_ind, NULL); 479 vdev.scan = sk_io; 480 return 0; 481 482 failed: 483 close(sk); 484 return 1; 485 } 486 487 static void accept_connection(uint8_t *data) 488 { 489 accept_conn_req_cp *cp = (void *) data; 490 struct vhci_conn *conn; 491 492 if (!(conn = conn_get_by_bdaddr(&cp->bdaddr))) 493 return; 494 495 connect_complete(conn); 496 497 g_io_add_watch(conn->chan, G_IO_IN | G_IO_NVAL | G_IO_HUP, 498 io_acl_data, (gpointer) conn); 499 } 500 501 static void close_connection(struct vhci_conn *conn) 502 { 503 syslog(LOG_INFO, "Closing connection %s handle %d", 504 batostr(&conn->dest), conn->handle); 505 506 g_io_channel_close(conn->chan); 507 g_io_channel_unref(conn->chan); 508 509 vconn[conn->handle - 1] = NULL; 510 disconn_complete(conn); 511 free(conn); 512 } 513 514 static void disconnect(uint8_t *data) 515 { 516 disconnect_cp *cp = (void *) data; 517 struct vhci_conn *conn; 518 uint16_t handle; 519 520 handle = btohs(cp->handle); 521 522 if (handle > VHCI_MAX_CONN) 523 return; 524 525 if (!(conn = vconn[handle-1])) 526 return; 527 528 close_connection(conn); 529 } 530 531 static void create_connection(uint8_t *data) 532 { 533 create_conn_cp *cp = (void *) data; 534 struct vhci_link_info info; 535 struct vhci_conn *conn; 536 struct sockaddr_in sa; 537 int h, sk, opt; 538 bdaddr_t ba; 539 540 for (h = 0; h < VHCI_MAX_CONN; h++) 541 if (!vconn[h]) 542 goto do_connect; 543 544 syslog(LOG_ERR, "Too many connections"); 545 return; 546 547 do_connect: 548 if ((sk = socket(AF_INET, SOCK_STREAM, 0)) < 0) { 549 syslog(LOG_ERR, "Can't create socket: %s (%d)", 550 strerror(errno), errno); 551 return; 552 } 553 554 opt = 1; 555 setsockopt(sk, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)); 556 557 baswap(&ba, &vdev.bdaddr); 558 sa.sin_family = AF_INET; 559 sa.sin_addr.s_addr = INADDR_ANY; // *(uint32_t *) &ba; 560 sa.sin_port = 0; // *(uint16_t *) &ba.b[4]; 561 if (bind(sk, (struct sockaddr *) &sa, sizeof(sa))) { 562 syslog(LOG_ERR, "Can't bind socket: %s (%d)", 563 strerror(errno), errno); 564 close(sk); 565 return; 566 } 567 568 baswap(&ba, &cp->bdaddr); 569 sa.sin_family = AF_INET; 570 memcpy(&sa.sin_addr.s_addr, &ba, sizeof(sa.sin_addr.s_addr)); 571 sa.sin_port = *(uint16_t *) &ba.b[4]; 572 if (connect(sk, (struct sockaddr *) &sa, sizeof(sa)) < 0) { 573 syslog(LOG_ERR, "Can't connect: %s (%d)", 574 strerror(errno), errno); 575 close(sk); 576 return; 577 } 578 579 /* Send info */ 580 memset(&info, 0, sizeof(info)); 581 bacpy(&info.bdaddr, &vdev.bdaddr); 582 info.link_type = ACL_LINK; 583 info.role = 1; 584 write_n(sk, (void *) &info, sizeof(info)); 585 586 if (!(conn = malloc(sizeof(*conn)))) { 587 syslog(LOG_ERR, "Can't alloc new connection: %s (%d)", 588 strerror(errno), errno); 589 close(sk); 590 return; 591 } 592 593 memcpy((uint8_t *) &ba, (uint8_t *) &sa.sin_addr, 4); 594 memcpy((uint8_t *) &ba.b[4], (uint8_t *) &sa.sin_port, 2); 595 baswap(&conn->dest, &ba); 596 597 vconn[h] = conn; 598 conn->handle = h + 1; 599 conn->chan = g_io_channel_unix_new(sk); 600 601 connect_complete(conn); 602 g_io_add_watch(conn->chan, G_IO_IN | G_IO_NVAL | G_IO_HUP, 603 io_acl_data, (gpointer) conn); 604 return; 605 } 606 607 static void hci_link_control(uint16_t ocf, int plen, uint8_t *data) 608 { 609 uint8_t status; 610 611 const uint16_t ogf = OGF_LINK_CTL; 612 613 switch (ocf) { 614 case OCF_CREATE_CONN: 615 command_status(ogf, ocf, 0x00); 616 create_connection(data); 617 break; 618 619 case OCF_ACCEPT_CONN_REQ: 620 command_status(ogf, ocf, 0x00); 621 accept_connection(data); 622 break; 623 624 case OCF_DISCONNECT: 625 command_status(ogf, ocf, 0x00); 626 disconnect(data); 627 break; 628 629 default: 630 status = 0x01; 631 command_complete(ogf, ocf, 1, &status); 632 break; 633 } 634 } 635 636 static void hci_link_policy(uint16_t ocf, int plen, uint8_t *data) 637 { 638 uint8_t status; 639 640 const uint16_t ogf = OGF_INFO_PARAM; 641 642 switch (ocf) { 643 default: 644 status = 0x01; 645 command_complete(ogf, ocf, 1, &status); 646 break; 647 } 648 } 649 650 static void hci_host_control(uint16_t ocf, int plen, uint8_t *data) 651 { 652 read_local_name_rp ln; 653 read_class_of_dev_rp cd; 654 read_inquiry_mode_rp im; 655 read_ext_inquiry_response_rp ir; 656 uint8_t status; 657 658 const uint16_t ogf = OGF_HOST_CTL; 659 660 switch (ocf) { 661 case OCF_RESET: 662 status = 0x00; 663 command_complete(ogf, ocf, 1, &status); 664 break; 665 666 case OCF_SET_EVENT_FLT: 667 status = 0x00; 668 command_complete(ogf, ocf, 1, &status); 669 break; 670 671 case OCF_CHANGE_LOCAL_NAME: 672 status = 0x00; 673 memcpy(vdev.name, data, sizeof(vdev.name)); 674 command_complete(ogf, ocf, 1, &status); 675 break; 676 677 case OCF_READ_LOCAL_NAME: 678 ln.status = 0x00; 679 memcpy(ln.name, vdev.name, sizeof(ln.name)); 680 command_complete(ogf, ocf, sizeof(ln), &ln); 681 break; 682 683 case OCF_WRITE_CONN_ACCEPT_TIMEOUT: 684 case OCF_WRITE_PAGE_TIMEOUT: 685 status = 0x00; 686 command_complete(ogf, ocf, 1, &status); 687 break; 688 689 case OCF_WRITE_SCAN_ENABLE: 690 status = scan_enable(data); 691 command_complete(ogf, ocf, 1, &status); 692 break; 693 694 case OCF_WRITE_AUTH_ENABLE: 695 status = 0x00; 696 command_complete(ogf, ocf, 1, &status); 697 break; 698 699 case OCF_WRITE_ENCRYPT_MODE: 700 status = 0x00; 701 command_complete(ogf, ocf, 1, &status); 702 break; 703 704 case OCF_READ_CLASS_OF_DEV: 705 cd.status = 0x00; 706 memcpy(cd.dev_class, vdev.dev_class, 3); 707 command_complete(ogf, ocf, sizeof(cd), &cd); 708 break; 709 710 case OCF_WRITE_CLASS_OF_DEV: 711 status = 0x00; 712 memcpy(vdev.dev_class, data, 3); 713 command_complete(ogf, ocf, 1, &status); 714 break; 715 716 case OCF_READ_INQUIRY_MODE: 717 im.status = 0x00; 718 im.mode = vdev.inq_mode; 719 command_complete(ogf, ocf, sizeof(im), &im); 720 break; 721 722 case OCF_WRITE_INQUIRY_MODE: 723 status = 0x00; 724 vdev.inq_mode = data[0]; 725 command_complete(ogf, ocf, 1, &status); 726 break; 727 728 case OCF_READ_EXT_INQUIRY_RESPONSE: 729 ir.status = 0x00; 730 ir.fec = vdev.eir_fec; 731 memcpy(ir.data, vdev.eir_data, 240); 732 command_complete(ogf, ocf, sizeof(ir), &ir); 733 break; 734 735 case OCF_WRITE_EXT_INQUIRY_RESPONSE: 736 status = 0x00; 737 vdev.eir_fec = data[0]; 738 memcpy(vdev.eir_data, data + 1, 240); 739 command_complete(ogf, ocf, 1, &status); 740 break; 741 742 default: 743 status = 0x01; 744 command_complete(ogf, ocf, 1, &status); 745 break; 746 } 747 } 748 749 static void hci_info_param(uint16_t ocf, int plen, uint8_t *data) 750 { 751 read_local_version_rp lv; 752 read_local_features_rp lf; 753 read_local_ext_features_rp ef; 754 read_buffer_size_rp bs; 755 read_bd_addr_rp ba; 756 uint8_t status; 757 758 const uint16_t ogf = OGF_INFO_PARAM; 759 760 switch (ocf) { 761 case OCF_READ_LOCAL_VERSION: 762 lv.status = 0x00; 763 lv.hci_ver = 0x03; 764 lv.hci_rev = htobs(0x0000); 765 lv.lmp_ver = 0x03; 766 lv.manufacturer = htobs(29); 767 lv.lmp_subver = htobs(0x0000); 768 command_complete(ogf, ocf, sizeof(lv), &lv); 769 break; 770 771 case OCF_READ_LOCAL_FEATURES: 772 lf.status = 0x00; 773 memcpy(lf.features, vdev.features, 8); 774 command_complete(ogf, ocf, sizeof(lf), &lf); 775 break; 776 777 case OCF_READ_LOCAL_EXT_FEATURES: 778 ef.status = 0x00; 779 if (*data == 0) { 780 ef.page_num = 0; 781 ef.max_page_num = 0; 782 memcpy(ef.features, vdev.features, 8); 783 } else { 784 ef.page_num = *data; 785 ef.max_page_num = 0; 786 memset(ef.features, 0, 8); 787 } 788 command_complete(ogf, ocf, sizeof(ef), &ef); 789 break; 790 791 case OCF_READ_BUFFER_SIZE: 792 bs.status = 0x00; 793 bs.acl_mtu = htobs(VHCI_ACL_MTU); 794 bs.sco_mtu = 0; 795 bs.acl_max_pkt = htobs(VHCI_ACL_MAX_PKT); 796 bs.sco_max_pkt = htobs(0); 797 command_complete(ogf, ocf, sizeof(bs), &bs); 798 break; 799 800 case OCF_READ_BD_ADDR: 801 ba.status = 0x00; 802 bacpy(&ba.bdaddr, &vdev.bdaddr); 803 command_complete(ogf, ocf, sizeof(ba), &ba); 804 break; 805 806 default: 807 status = 0x01; 808 command_complete(ogf, ocf, 1, &status); 809 break; 810 } 811 } 812 813 static void hci_command(uint8_t *data) 814 { 815 hci_command_hdr *ch; 816 uint8_t *ptr = data; 817 uint16_t ogf, ocf; 818 819 ch = (hci_command_hdr *) ptr; 820 ptr += HCI_COMMAND_HDR_SIZE; 821 822 ch->opcode = btohs(ch->opcode); 823 ogf = cmd_opcode_ogf(ch->opcode); 824 ocf = cmd_opcode_ocf(ch->opcode); 825 826 switch (ogf) { 827 case OGF_LINK_CTL: 828 hci_link_control(ocf, ch->plen, ptr); 829 break; 830 831 case OGF_LINK_POLICY: 832 hci_link_policy(ocf, ch->plen, ptr); 833 break; 834 835 case OGF_HOST_CTL: 836 hci_host_control(ocf, ch->plen, ptr); 837 break; 838 839 case OGF_INFO_PARAM: 840 hci_info_param(ocf, ch->plen, ptr); 841 break; 842 } 843 } 844 845 static void hci_acl_data(uint8_t *data) 846 { 847 hci_acl_hdr *ah = (void *) data; 848 struct vhci_conn *conn; 849 uint16_t handle; 850 int fd; 851 852 handle = acl_handle(btohs(ah->handle)); 853 854 if (handle > VHCI_MAX_CONN || !(conn = vconn[handle - 1])) { 855 syslog(LOG_ERR, "Bad connection handle %d", handle); 856 return; 857 } 858 859 fd = g_io_channel_unix_get_fd(conn->chan); 860 if (write_n(fd, data, btohs(ah->dlen) + HCI_ACL_HDR_SIZE) < 0) { 861 close_connection(conn); 862 return; 863 } 864 865 if (++vdev.acl_cnt > VHCI_ACL_MAX_PKT - 1) { 866 /* Send num of complete packets event */ 867 num_completed_pkts(conn); 868 vdev.acl_cnt = 0; 869 } 870 } 871 872 static gboolean io_acl_data(GIOChannel *chan, GIOCondition cond, gpointer data) 873 { 874 struct vhci_conn *conn = (struct vhci_conn *) data; 875 unsigned char buf[HCI_MAX_FRAME_SIZE], *ptr; 876 hci_acl_hdr *ah; 877 uint16_t flags; 878 int fd, err, len; 879 880 if (cond & G_IO_NVAL) { 881 g_io_channel_unref(chan); 882 return FALSE; 883 } 884 885 if (cond & G_IO_HUP) { 886 close_connection(conn); 887 return FALSE; 888 } 889 890 fd = g_io_channel_unix_get_fd(chan); 891 892 ptr = buf + 1; 893 if (read_n(fd, ptr, HCI_ACL_HDR_SIZE) <= 0) { 894 close_connection(conn); 895 return FALSE; 896 } 897 898 ah = (void *) ptr; 899 ptr += HCI_ACL_HDR_SIZE; 900 901 len = btohs(ah->dlen); 902 if (read_n(fd, ptr, len) <= 0) { 903 close_connection(conn); 904 return FALSE; 905 } 906 907 buf[0] = HCI_ACLDATA_PKT; 908 909 flags = acl_flags(btohs(ah->handle)); 910 ah->handle = htobs(acl_handle_pack(conn->handle, flags)); 911 len += HCI_ACL_HDR_SIZE + 1; 912 913 write_snoop(vdev.dd, HCI_ACLDATA_PKT, 1, buf, len); 914 915 err = write(vdev.fd, buf, len); 916 917 return TRUE; 918 } 919 920 static gboolean io_conn_ind(GIOChannel *chan, GIOCondition cond, gpointer data) 921 { 922 struct vhci_link_info info; 923 struct vhci_conn *conn; 924 struct sockaddr_in sa; 925 socklen_t len; 926 int sk, nsk, h; 927 928 if (cond & G_IO_NVAL) 929 return FALSE; 930 931 sk = g_io_channel_unix_get_fd(chan); 932 933 len = sizeof(sa); 934 if ((nsk = accept(sk, (struct sockaddr *) &sa, &len)) < 0) 935 return TRUE; 936 937 if (read_n(nsk, &info, sizeof(info)) < 0) { 938 syslog(LOG_ERR, "Can't read link info"); 939 return TRUE; 940 } 941 942 if (!(conn = malloc(sizeof(*conn)))) { 943 syslog(LOG_ERR, "Can't alloc new connection"); 944 close(nsk); 945 return TRUE; 946 } 947 948 bacpy(&conn->dest, &info.bdaddr); 949 950 for (h = 0; h < VHCI_MAX_CONN; h++) 951 if (!vconn[h]) 952 goto accepted; 953 954 syslog(LOG_ERR, "Too many connections"); 955 free(conn); 956 close(nsk); 957 return TRUE; 958 959 accepted: 960 vconn[h] = conn; 961 conn->handle = h + 1; 962 conn->chan = g_io_channel_unix_new(nsk); 963 connect_request(conn); 964 965 return TRUE; 966 } 967 968 static gboolean io_hci_data(GIOChannel *chan, GIOCondition cond, gpointer data) 969 { 970 unsigned char buf[HCI_MAX_FRAME_SIZE], *ptr; 971 int type; 972 gsize len; 973 GIOError err; 974 975 ptr = buf; 976 977 if ((err = g_io_channel_read(chan, (gchar *) buf, sizeof(buf), &len))) { 978 if (err == G_IO_ERROR_AGAIN) 979 return TRUE; 980 981 syslog(LOG_ERR, "Read failed: %s (%d)", strerror(errno), errno); 982 g_io_channel_unref(chan); 983 g_main_loop_quit(event_loop); 984 return FALSE; 985 } 986 987 type = *ptr++; 988 989 write_snoop(vdev.dd, type, 0, buf, len); 990 991 switch (type) { 992 case HCI_COMMAND_PKT: 993 hci_command(ptr); 994 break; 995 996 case HCI_ACLDATA_PKT: 997 hci_acl_data(ptr); 998 break; 999 1000 default: 1001 syslog(LOG_ERR, "Unknown packet type 0x%2.2x", type); 1002 break; 1003 } 1004 1005 return TRUE; 1006 } 1007 1008 static int getbdaddrbyname(char *str, bdaddr_t *ba) 1009 { 1010 int i, n, len; 1011 1012 len = strlen(str); 1013 1014 /* Check address format */ 1015 for (i = 0, n = 0; i < len; i++) 1016 if (str[i] == ':') 1017 n++; 1018 1019 if (n == 5) { 1020 /* BD address */ 1021 baswap(ba, strtoba(str)); 1022 return 0; 1023 } 1024 1025 if (n == 1) { 1026 /* IP address + port */ 1027 struct hostent *hent; 1028 bdaddr_t b; 1029 char *ptr; 1030 1031 ptr = strchr(str, ':'); 1032 *ptr++ = 0; 1033 1034 if (!(hent = gethostbyname(str))) { 1035 fprintf(stderr, "Can't resolve %s\n", str); 1036 return -2; 1037 } 1038 1039 memcpy(&b, hent->h_addr, 4); 1040 *(uint16_t *) (&b.b[4]) = htons(atoi(ptr)); 1041 baswap(ba, &b); 1042 1043 return 0; 1044 } 1045 1046 fprintf(stderr, "Invalid address format\n"); 1047 1048 return -1; 1049 } 1050 1051 static void rewrite_bdaddr(unsigned char *buf, int len, bdaddr_t *bdaddr) 1052 { 1053 hci_event_hdr *eh; 1054 unsigned char *ptr = buf; 1055 int type; 1056 1057 if (!bdaddr) 1058 return; 1059 1060 if (!bacmp(bdaddr, BDADDR_ANY)) 1061 return; 1062 1063 type = *ptr++; 1064 1065 switch (type) { 1066 case HCI_EVENT_PKT: 1067 eh = (hci_event_hdr *) ptr; 1068 ptr += HCI_EVENT_HDR_SIZE; 1069 1070 if (eh->evt == EVT_CMD_COMPLETE) { 1071 evt_cmd_complete *cc = (void *) ptr; 1072 1073 ptr += EVT_CMD_COMPLETE_SIZE; 1074 1075 if (cc->opcode == htobs(cmd_opcode_pack(OGF_INFO_PARAM, 1076 OCF_READ_BD_ADDR))) { 1077 bacpy((bdaddr_t *) (ptr + 1), bdaddr); 1078 } 1079 } 1080 break; 1081 } 1082 } 1083 1084 static int run_proxy(int fd, int dev, bdaddr_t *bdaddr) 1085 { 1086 unsigned char buf[HCI_MAX_FRAME_SIZE + 1]; 1087 struct hci_dev_info di; 1088 struct hci_filter flt; 1089 struct pollfd p[2]; 1090 int dd, err, len, need_raw; 1091 1092 dd = hci_open_dev(dev); 1093 if (dd < 0) { 1094 syslog(LOG_ERR, "Can't open device hci%d: %s (%d)", 1095 dev, strerror(errno), errno); 1096 return 1; 1097 } 1098 1099 if (hci_devinfo(dev, &di) < 0) { 1100 syslog(LOG_ERR, "Can't get device info for hci%d: %s (%d)", 1101 dev, strerror(errno), errno); 1102 hci_close_dev(dd); 1103 return 1; 1104 } 1105 1106 need_raw = !hci_test_bit(HCI_RAW, &di.flags); 1107 1108 hci_filter_clear(&flt); 1109 hci_filter_all_ptypes(&flt); 1110 hci_filter_all_events(&flt); 1111 1112 if (setsockopt(dd, SOL_HCI, HCI_FILTER, &flt, sizeof(flt)) < 0) { 1113 syslog(LOG_ERR, "Can't set filter for hci%d: %s (%d)", 1114 dev, strerror(errno), errno); 1115 hci_close_dev(dd); 1116 return 1; 1117 } 1118 1119 if (need_raw) { 1120 if (ioctl(dd, HCISETRAW, 1) < 0) { 1121 syslog(LOG_ERR, "Can't set raw mode on hci%d: %s (%d)", 1122 dev, strerror(errno), errno); 1123 hci_close_dev(dd); 1124 return 1; 1125 } 1126 } 1127 1128 p[0].fd = fd; 1129 p[0].events = POLLIN; 1130 p[1].fd = dd; 1131 p[1].events = POLLIN; 1132 1133 while (!__io_canceled) { 1134 p[0].revents = 0; 1135 p[1].revents = 0; 1136 err = poll(p, 2, 500); 1137 if (err < 0) 1138 break; 1139 if (!err) 1140 continue; 1141 1142 if (p[0].revents & POLLIN) { 1143 len = read(fd, buf, sizeof(buf)); 1144 if (len > 0) { 1145 rewrite_bdaddr(buf, len, bdaddr); 1146 err = write(dd, buf, len); 1147 } 1148 } 1149 1150 if (p[1].revents & POLLIN) { 1151 len = read(dd, buf, sizeof(buf)); 1152 if (len > 0) { 1153 rewrite_bdaddr(buf, len, bdaddr); 1154 err = write(fd, buf, len); 1155 } 1156 } 1157 } 1158 1159 if (need_raw) { 1160 if (ioctl(dd, HCISETRAW, 0) < 0) 1161 syslog(LOG_ERR, "Can't clear raw mode on hci%d: %s (%d)", 1162 dev, strerror(errno), errno); 1163 } 1164 1165 hci_close_dev(dd); 1166 1167 syslog(LOG_INFO, "Exit"); 1168 1169 return 0; 1170 } 1171 1172 static void usage(void) 1173 { 1174 printf("hciemu - HCI emulator ver %s\n", VERSION); 1175 printf("Usage: \n"); 1176 printf("\thciemu [options] local_address\n" 1177 "Options:\n" 1178 "\t[-d device] use specified device\n" 1179 "\t[-b bdaddr] emulate specified address\n" 1180 "\t[-s file] create snoop file\n" 1181 "\t[-n] do not detach\n" 1182 "\t[-h] help, you are looking at it\n"); 1183 } 1184 1185 static struct option main_options[] = { 1186 { "device", 1, 0, 'd' }, 1187 { "bdaddr", 1, 0, 'b' }, 1188 { "snoop", 1, 0, 's' }, 1189 { "nodetach", 0, 0, 'n' }, 1190 { "help", 0, 0, 'h' }, 1191 { 0 } 1192 }; 1193 1194 int main(int argc, char *argv[]) 1195 { 1196 struct sigaction sa; 1197 GIOChannel *dev_io; 1198 char *device = NULL, *snoop = NULL; 1199 bdaddr_t bdaddr; 1200 int fd, dd, opt, detach = 1, dev = -1; 1201 1202 bacpy(&bdaddr, BDADDR_ANY); 1203 1204 while ((opt=getopt_long(argc, argv, "d:b:s:nh", main_options, NULL)) != EOF) { 1205 switch(opt) { 1206 case 'd': 1207 device = strdup(optarg); 1208 break; 1209 1210 case 'b': 1211 str2ba(optarg, &bdaddr); 1212 break; 1213 1214 case 's': 1215 snoop = strdup(optarg); 1216 break; 1217 1218 case 'n': 1219 detach = 0; 1220 break; 1221 1222 case 'h': 1223 default: 1224 usage(); 1225 exit(0); 1226 } 1227 } 1228 1229 argc -= optind; 1230 argv += optind; 1231 optind = 0; 1232 1233 if (argc < 1) { 1234 usage(); 1235 exit(1); 1236 } 1237 1238 if (strlen(argv[0]) > 3 && !strncasecmp(argv[0], "hci", 3)) { 1239 dev = hci_devid(argv[0]); 1240 if (dev < 0) { 1241 perror("Invalid device"); 1242 exit(1); 1243 } 1244 } else { 1245 if (getbdaddrbyname(argv[0], &vdev.bdaddr) < 0) 1246 exit(1); 1247 } 1248 1249 if (detach) { 1250 if (daemon(0, 0)) { 1251 perror("Can't start daemon"); 1252 exit(1); 1253 } 1254 } 1255 1256 /* Start logging to syslog and stderr */ 1257 openlog("hciemu", LOG_PID | LOG_NDELAY | LOG_PERROR, LOG_DAEMON); 1258 syslog(LOG_INFO, "HCI emulation daemon ver %s started", VERSION); 1259 1260 memset(&sa, 0, sizeof(sa)); 1261 sa.sa_flags = SA_NOCLDSTOP; 1262 sa.sa_handler = SIG_IGN; 1263 sigaction(SIGCHLD, &sa, NULL); 1264 sigaction(SIGPIPE, &sa, NULL); 1265 1266 sa.sa_handler = sig_term; 1267 sigaction(SIGTERM, &sa, NULL); 1268 sigaction(SIGINT, &sa, NULL); 1269 1270 io_init(); 1271 1272 if (!device && dev >= 0) 1273 device = strdup(GHCI_DEV); 1274 1275 /* Open and create virtual HCI device */ 1276 if (device) { 1277 fd = open(device, O_RDWR); 1278 if (fd < 0) { 1279 syslog(LOG_ERR, "Can't open device %s: %s (%d)", 1280 device, strerror(errno), errno); 1281 free(device); 1282 exit(1); 1283 } 1284 free(device); 1285 } else { 1286 fd = open(VHCI_DEV, O_RDWR); 1287 if (fd < 0) { 1288 fd = open(VHCI_UDEV, O_RDWR); 1289 if (fd < 0) { 1290 syslog(LOG_ERR, "Can't open device %s: %s (%d)", 1291 VHCI_DEV, strerror(errno), errno); 1292 exit(1); 1293 } 1294 } 1295 } 1296 1297 /* Create snoop file */ 1298 if (snoop) { 1299 dd = create_snoop(snoop); 1300 if (dd < 0) 1301 syslog(LOG_ERR, "Can't create snoop file %s: %s (%d)", 1302 snoop, strerror(errno), errno); 1303 free(snoop); 1304 } else 1305 dd = -1; 1306 1307 /* Create event loop */ 1308 event_loop = g_main_loop_new(NULL, FALSE); 1309 1310 if (dev >= 0) 1311 return run_proxy(fd, dev, &bdaddr); 1312 1313 /* Device settings */ 1314 vdev.features[0] = 0xff; 1315 vdev.features[1] = 0xff; 1316 vdev.features[2] = 0x8f; 1317 vdev.features[3] = 0xfe; 1318 vdev.features[4] = 0x9b; 1319 vdev.features[5] = 0xf9; 1320 vdev.features[6] = 0x01; 1321 vdev.features[7] = 0x80; 1322 1323 memset(vdev.name, 0, sizeof(vdev.name)); 1324 strncpy((char *) vdev.name, "BlueZ (Virtual HCI)", 1325 sizeof(vdev.name) - 1); 1326 1327 vdev.dev_class[0] = 0x00; 1328 vdev.dev_class[1] = 0x00; 1329 vdev.dev_class[2] = 0x00; 1330 1331 vdev.inq_mode = 0x00; 1332 vdev.eir_fec = 0x00; 1333 memset(vdev.eir_data, 0, sizeof(vdev.eir_data)); 1334 1335 vdev.fd = fd; 1336 vdev.dd = dd; 1337 1338 dev_io = g_io_channel_unix_new(fd); 1339 g_io_add_watch(dev_io, G_IO_IN, io_hci_data, NULL); 1340 1341 setpriority(PRIO_PROCESS, 0, -19); 1342 1343 /* Start event processor */ 1344 g_main_loop_run(event_loop); 1345 1346 close(fd); 1347 1348 if (dd >= 0) 1349 close(dd); 1350 1351 syslog(LOG_INFO, "Exit"); 1352 1353 return 0; 1354 } 1355
