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