tools/btproxy.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2011-2012  Intel Corporation
   6  *  Copyright (C) 2004-2010  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 <string.h>
  36 #include <alloca.h>
  37 #include <getopt.h>
  38 #include <stdbool.h>
  39 #include <termios.h>
  40 #include <sys/stat.h>
  41 #include <sys/socket.h>
  42 #include <sys/un.h>
  43
  44 #include <netdb.h>
  45 #include <arpa/inet.h>
  46
  47 #include "src/shared/util.h"
  48 #include "src/shared/mainloop.h"
  49 #include "src/shared/ecc.h"
  50 #include "monitor/bt.h"
  51
  52 #define HCI_BREDR       0x00
  53 #define HCI_AMP         0x01
  54
  55 #define BTPROTO_HCI     1
  56 struct sockaddr_hci {
  57         sa_family_t     hci_family;
  58         unsigned short  hci_dev;
  59         unsigned short  hci_channel;
  60 };
  61 #define HCI_CHANNEL_USER        1
  62
  63 static uint16_t hci_index = 0;
  64 static bool client_active = false;
  65 static bool debug_enabled = false;
  66 static bool emulate_ecc = false;
  67
  68 static void hexdump_print(const char *str, void *user_data)
  69 {
  70         printf("%s%s\n", (char *) user_data, str);
  71 }
  72
  73 struct proxy {
  74         /* Receive commands, ACL and SCO data */
  75         int host_fd;
  76         uint8_t host_buf[4096];
  77         uint16_t host_len;
  78         bool host_shutdown;
  79
  80         /* Receive events, ACL and SCO data */
  81         int dev_fd;
  82         uint8_t dev_buf[4096];
  83         uint16_t dev_len;
  84         bool dev_shutdown;
  85
  86         /* ECC emulation */
  87         uint8_t event_mask[8];
  88         uint8_t local_sk256[32];
  89 };
  90
  91 static bool write_packet(int fd, const void *data, size_t size,
  92                                                         void *user_data)
  93 {
  94         while (size > 0) {
  95                 ssize_t written;
  96
  97                 written = write(fd, data, size);
  98                 if (written < 0) {
  99                         if (errno == EAGAIN || errno == EINTR)
 100                                 continue;
 101                         return false;
 102                 }
 103
 104                 if (debug_enabled)
 105                         util_hexdump('<', data, written, hexdump_print,
 106                                                                 user_data);
 107
 108                 data += written;
 109                 size -= written;
 110         }
 111
 112         return true;
 113 }
 114
 115 static void host_write_packet(struct proxy *proxy, void *buf, uint16_t len)
 116 {
 117         if (!write_packet(proxy->dev_fd, buf, len, "D: ")) {
 118                 fprintf(stderr, "Write to device descriptor failed\n");
 119                 mainloop_remove_fd(proxy->dev_fd);
 120         }
 121 }
 122
 123 static void dev_write_packet(struct proxy *proxy, void *buf, uint16_t len)
 124 {
 125         if (!write_packet(proxy->host_fd, buf, len, "H: ")) {
 126                 fprintf(stderr, "Write to host descriptor failed\n");
 127                 mainloop_remove_fd(proxy->host_fd);
 128         }
 129 }
 130
 131 static void cmd_status(struct proxy *proxy, uint8_t status, uint16_t opcode)
 132 {
 133         size_t buf_size = 1 + sizeof(struct bt_hci_evt_hdr) +
 134                                         sizeof(struct bt_hci_evt_cmd_status);
 135         void *buf = alloca(buf_size);
 136         struct bt_hci_evt_hdr *hdr = buf + 1;
 137         struct bt_hci_evt_cmd_status *cs = buf + 1 + sizeof(*hdr);
 138
 139         *((uint8_t *) buf) = BT_H4_EVT_PKT;
 140
 141         hdr->evt = BT_HCI_EVT_CMD_STATUS;
 142         hdr->plen = sizeof(*cs);
 143
 144         cs->status = status;
 145         cs->ncmd = 0x01;
 146         cs->opcode = cpu_to_le16(opcode);
 147
 148         dev_write_packet(proxy, buf, buf_size);
 149 }
 150
 151 static void le_meta_event(struct proxy *proxy, uint8_t event,
 152                                                 void *data, uint8_t len)
 153 {
 154         size_t buf_size = 1 + sizeof(struct bt_hci_evt_hdr) + 1 + len;
 155         void *buf = alloca(buf_size);
 156         struct bt_hci_evt_hdr *hdr = buf + 1;
 157
 158         *((uint8_t *) buf) = BT_H4_EVT_PKT;
 159
 160         hdr->evt = BT_HCI_EVT_LE_META_EVENT;
 161         hdr->plen = 1 + len;
 162
 163         *((uint8_t *) (buf + 1 + sizeof(*hdr))) = event;
 164
 165         if (len > 0)
 166                 memcpy(buf + 1 + sizeof(*hdr) + 1, data, len);
 167
 168         dev_write_packet(proxy, buf, buf_size);
 169 }
 170
 171 static void host_emulate_ecc(struct proxy *proxy, void *buf, uint16_t len)
 172 {
 173         uint8_t pkt_type = *((uint8_t *) buf);
 174         struct bt_hci_cmd_hdr *hdr = buf + 1;
 175         struct bt_hci_cmd_le_set_event_mask *lsem;
 176         struct bt_hci_cmd_le_generate_dhkey *lgd;
 177         struct bt_hci_evt_le_read_local_pk256_complete lrlpkc;
 178         struct bt_hci_evt_le_generate_dhkey_complete lgdc;
 179
 180         if (pkt_type != BT_H4_CMD_PKT) {
 181                 host_write_packet(proxy, buf, len);
 182                 return;
 183         }
 184
 185         switch (le16_to_cpu(hdr->opcode)) {
 186         case BT_HCI_CMD_LE_SET_EVENT_MASK:
 187                 lsem = buf + 1 + sizeof(*hdr);
 188                 memcpy(proxy->event_mask, lsem->mask, 8);
 189
 190                 lsem->mask[0] &= ~0x80;         /* P-256 Public Key Complete */
 191                 lsem->mask[1] &= ~0x01;         /* Generate DHKey Complete */
 192
 193                 host_write_packet(proxy, buf, len);
 194                 break;
 195
 196         case BT_HCI_CMD_LE_READ_LOCAL_PK256:
 197                 if (!ecc_make_key(lrlpkc.local_pk256, proxy->local_sk256)) {
 198                         cmd_status(proxy, BT_HCI_ERR_COMMAND_DISALLOWED,
 199                                         BT_HCI_CMD_LE_READ_LOCAL_PK256);
 200                         break;
 201                 }
 202                 cmd_status(proxy, BT_HCI_ERR_SUCCESS,
 203                                         BT_HCI_CMD_LE_READ_LOCAL_PK256);
 204
 205                 if (!(proxy->event_mask[0] & 0x80))
 206                         break;
 207
 208                 lrlpkc.status = BT_HCI_ERR_SUCCESS;
 209                 le_meta_event(proxy, BT_HCI_EVT_LE_READ_LOCAL_PK256_COMPLETE,
 210                                                 &lrlpkc, sizeof(lrlpkc));
 211                 break;
 212
 213         case BT_HCI_CMD_LE_GENERATE_DHKEY:
 214                 lgd = buf + 1 + sizeof(*hdr);
 215                 if (!ecdh_shared_secret(lgd->remote_pk256, proxy->local_sk256,
 216                                                                 lgdc.dhkey)) {
 217                         cmd_status(proxy, BT_HCI_ERR_COMMAND_DISALLOWED,
 218                                                 BT_HCI_CMD_LE_GENERATE_DHKEY);
 219                         break;
 220                 }
 221                 cmd_status(proxy, BT_HCI_ERR_SUCCESS,
 222                                         BT_HCI_CMD_LE_GENERATE_DHKEY);
 223
 224                 if (!(proxy->event_mask[1] & 0x01))
 225                         break;
 226
 227                 lgdc.status = BT_HCI_ERR_SUCCESS;
 228                 le_meta_event(proxy, BT_HCI_EVT_LE_GENERATE_DHKEY_COMPLETE,
 229                                                         &lgdc, sizeof(lgdc));
 230                 break;
 231
 232         default:
 233                 host_write_packet(proxy, buf, len);
 234                 break;
 235         }
 236 }
 237
 238 static void dev_emulate_ecc(struct proxy *proxy, void *buf, uint16_t len)
 239 {
 240         uint8_t pkt_type = *((uint8_t *) buf);
 241         struct bt_hci_evt_hdr *hdr = buf + 1;
 242         struct bt_hci_evt_cmd_complete *cc;
 243         struct bt_hci_rsp_read_local_commands *rlc;
 244
 245         if (pkt_type != BT_H4_EVT_PKT) {
 246                 dev_write_packet(proxy, buf, len);
 247                 return;
 248         }
 249
 250         switch (hdr->evt) {
 251         case BT_HCI_EVT_CMD_COMPLETE:
 252                 cc = buf + 1 + sizeof(*hdr);
 253
 254                 switch (le16_to_cpu(cc->opcode)) {
 255                 case BT_HCI_CMD_READ_LOCAL_COMMANDS:
 256                         rlc = buf + 1 + sizeof(*hdr) + sizeof(*cc);
 257                         rlc->commands[34] |= 0x02;      /* P-256 Public Key */
 258                         rlc->commands[34] |= 0x04;      /* Generate DHKey */
 259                         break;
 260                 }
 261
 262                 dev_write_packet(proxy, buf, len);
 263                 break;
 264
 265         default:
 266                 dev_write_packet(proxy, buf, len);
 267                 break;
 268         }
 269 }
 270
 271 static void host_read_destroy(void *user_data)
 272 {
 273         struct proxy *proxy = user_data;
 274
 275         printf("Closing host descriptor\n");
 276
 277         if (proxy->host_shutdown)
 278                 shutdown(proxy->host_fd, SHUT_RDWR);
 279
 280         close(proxy->host_fd);
 281         proxy->host_fd = -1;
 282
 283         if (proxy->dev_fd < 0) {
 284                 client_active = false;
 285                 free(proxy);
 286         } else
 287                 mainloop_remove_fd(proxy->dev_fd);
 288 }
 289
 290 static void host_read_callback(int fd, uint32_t events, void *user_data)
 291 {
 292         struct proxy *proxy = user_data;
 293         struct bt_hci_cmd_hdr *cmd_hdr;
 294         struct bt_hci_acl_hdr *acl_hdr;
 295         struct bt_hci_sco_hdr *sco_hdr;
 296         ssize_t len;
 297         uint16_t pktlen;
 298
 299         if (events & (EPOLLERR | EPOLLHUP)) {
 300                 fprintf(stderr, "Error from host descriptor\n");
 301                 mainloop_remove_fd(proxy->host_fd);
 302                 return;
 303         }
 304
 305         if (events & EPOLLRDHUP) {
 306                 fprintf(stderr, "Remote hangup of host descriptor\n");
 307                 mainloop_remove_fd(proxy->host_fd);
 308                 return;
 309         }
 310
 311         len = read(proxy->host_fd, proxy->host_buf + proxy->host_len,
 312                                 sizeof(proxy->host_buf) - proxy->host_len);
 313         if (len < 0) {
 314                 if (errno == EAGAIN || errno == EINTR)
 315                         return;
 316
 317                 fprintf(stderr, "Read from host descriptor failed\n");
 318                 mainloop_remove_fd(proxy->host_fd);
 319                 return;
 320         }
 321
 322         if (debug_enabled)
 323                 util_hexdump('>', proxy->host_buf + proxy->host_len, len,
 324                                                 hexdump_print, "H: ");
 325
 326         proxy->host_len += len;
 327
 328 process_packet:
 329         if (proxy->host_len < 1)
 330                 return;
 331
 332         switch (proxy->host_buf[0]) {
 333         case BT_H4_CMD_PKT:
 334                 if (proxy->host_len < 1 + sizeof(*cmd_hdr))
 335                         return;
 336
 337                 cmd_hdr = (void *) (proxy->host_buf + 1);
 338                 pktlen = 1 + sizeof(*cmd_hdr) + cmd_hdr->plen;
 339                 break;
 340         case BT_H4_ACL_PKT:
 341                 if (proxy->host_len < 1 + sizeof(*acl_hdr))
 342                         return;
 343
 344                 acl_hdr = (void *) (proxy->host_buf + 1);
 345                 pktlen = 1 + sizeof(*acl_hdr) + cpu_to_le16(acl_hdr->dlen);
 346                 break;
 347         case BT_H4_SCO_PKT:
 348                 if (proxy->host_len < 1 + sizeof(*sco_hdr))
 349                         return;
 350
 351                 sco_hdr = (void *) (proxy->host_buf + 1);
 352                 pktlen = 1 + sizeof(*sco_hdr) + sco_hdr->dlen;
 353                 break;
 354         case 0xff:
 355                 /* Notification packet from /dev/vhci - ignore */
 356                 proxy->host_len = 0;
 357                 return;
 358         default:
 359                 fprintf(stderr, "Received unknown host packet type 0x%02x\n",
 360                                                         proxy->host_buf[0]);
 361                 mainloop_remove_fd(proxy->host_fd);
 362                 return;
 363         }
 364
 365         if (proxy->host_len < pktlen)
 366                 return;
 367
 368         if (emulate_ecc)
 369                 host_emulate_ecc(proxy, proxy->host_buf, pktlen);
 370         else
 371                 host_write_packet(proxy, proxy->host_buf, pktlen);
 372
 373         if (proxy->host_len > pktlen) {
 374                 memmove(proxy->host_buf, proxy->host_buf + pktlen,
 375                                                 proxy->host_len - pktlen);
 376                 proxy->host_len -= pktlen;
 377                 goto process_packet;
 378         }
 379
 380         proxy->host_len = 0;
 381 }
 382
 383 static void dev_read_destroy(void *user_data)
 384 {
 385         struct proxy *proxy = user_data;
 386
 387         printf("Closing device descriptor\n");
 388
 389         if (proxy->dev_shutdown)
 390                 shutdown(proxy->dev_fd, SHUT_RDWR);
 391
 392         close(proxy->dev_fd);
 393         proxy->dev_fd = -1;
 394
 395         if (proxy->host_fd < 0) {
 396                 client_active = false;
 397                 free(proxy);
 398         } else
 399                 mainloop_remove_fd(proxy->host_fd);
 400 }
 401
 402 static void dev_read_callback(int fd, uint32_t events, void *user_data)
 403 {
 404         struct proxy *proxy = user_data;
 405         struct bt_hci_evt_hdr *evt_hdr;
 406         struct bt_hci_acl_hdr *acl_hdr;
 407         struct bt_hci_sco_hdr *sco_hdr;
 408         ssize_t len;
 409         uint16_t pktlen;
 410
 411         if (events & (EPOLLERR | EPOLLHUP)) {
 412                 fprintf(stderr, "Error from device descriptor\n");
 413                 mainloop_remove_fd(proxy->dev_fd);
 414                 return;
 415         }
 416
 417         if (events & EPOLLRDHUP) {
 418                 fprintf(stderr, "Remote hangup of device descriptor\n");
 419                 mainloop_remove_fd(proxy->host_fd);
 420                 return;
 421         }
 422
 423         len = read(proxy->dev_fd, proxy->dev_buf + proxy->dev_len,
 424                                 sizeof(proxy->dev_buf) - proxy->dev_len);
 425         if (len < 0) {
 426                 if (errno == EAGAIN || errno == EINTR)
 427                         return;
 428
 429                 fprintf(stderr, "Read from device descriptor failed\n");
 430                 mainloop_remove_fd(proxy->dev_fd);
 431                 return;
 432         }
 433
 434         if (debug_enabled)
 435                 util_hexdump('>', proxy->dev_buf + proxy->dev_len, len,
 436                                                 hexdump_print, "D: ");
 437
 438         proxy->dev_len += len;
 439
 440 process_packet:
 441         if (proxy->dev_len < 1)
 442                 return;
 443
 444         switch (proxy->dev_buf[0]) {
 445         case BT_H4_EVT_PKT:
 446                 if (proxy->dev_len < 1 + sizeof(*evt_hdr))
 447                         return;
 448
 449                 evt_hdr = (void *) (proxy->dev_buf + 1);
 450                 pktlen = 1 + sizeof(*evt_hdr) + evt_hdr->plen;
 451                 break;
 452         case BT_H4_ACL_PKT:
 453                 if (proxy->dev_len < 1 + sizeof(*acl_hdr))
 454                         return;
 455
 456                 acl_hdr = (void *) (proxy->dev_buf + 1);
 457                 pktlen = 1 + sizeof(*acl_hdr) + cpu_to_le16(acl_hdr->dlen);
 458                 break;
 459         case BT_H4_SCO_PKT:
 460                 if (proxy->dev_len < 1 + sizeof(*sco_hdr))
 461                         return;
 462
 463                 sco_hdr = (void *) (proxy->dev_buf + 1);
 464                 pktlen = 1 + sizeof(*sco_hdr) + sco_hdr->dlen;
 465                 break;
 466         default:
 467                 fprintf(stderr, "Received unknown device packet type 0x%02x\n",
 468                                                         proxy->dev_buf[0]);
 469                 mainloop_remove_fd(proxy->dev_fd);
 470                 return;
 471         }
 472
 473         if (proxy->dev_len < pktlen)
 474                 return;
 475
 476         if (emulate_ecc)
 477                 dev_emulate_ecc(proxy, proxy->dev_buf, pktlen);
 478         else
 479                 dev_write_packet(proxy, proxy->dev_buf, pktlen);
 480
 481         if (proxy->dev_len > pktlen) {
 482                 memmove(proxy->dev_buf, proxy->dev_buf + pktlen,
 483                                                 proxy->dev_len - pktlen);
 484                 proxy->dev_len -= pktlen;
 485                 goto process_packet;
 486         }
 487
 488         proxy->dev_len = 0;
 489 }
 490
 491 static bool setup_proxy(int host_fd, bool host_shutdown,
 492                                                 int dev_fd, bool dev_shutdown)
 493 {
 494         struct proxy *proxy;
 495
 496         proxy = new0(struct proxy, 1);
 497         if (!proxy)
 498                 return false;
 499
 500         if (emulate_ecc)
 501                 printf("Enabling ECC emulation\n");
 502
 503         proxy->host_fd = host_fd;
 504         proxy->host_shutdown = host_shutdown;
 505
 506         proxy->dev_fd = dev_fd;
 507         proxy->dev_shutdown = dev_shutdown;
 508
 509         mainloop_add_fd(proxy->host_fd, EPOLLIN | EPOLLRDHUP,
 510                                 host_read_callback, proxy, host_read_destroy);
 511
 512         mainloop_add_fd(proxy->dev_fd, EPOLLIN | EPOLLRDHUP,
 513                                 dev_read_callback, proxy, dev_read_destroy);
 514
 515         return true;
 516 }
 517
 518 static int open_channel(uint16_t index)
 519 {
 520         struct sockaddr_hci addr;
 521         int fd;
 522
 523         printf("Opening user channel for hci%u\n", hci_index);
 524
 525         fd = socket(PF_BLUETOOTH, SOCK_RAW | SOCK_CLOEXEC, BTPROTO_HCI);
 526         if (fd < 0) {
 527                 perror("Failed to open Bluetooth socket");
 528                 return -1;
 529         }
 530
 531         memset(&addr, 0, sizeof(addr));
 532         addr.hci_family = AF_BLUETOOTH;
 533         addr.hci_dev = index;
 534         addr.hci_channel = HCI_CHANNEL_USER;
 535
 536         if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 537                 close(fd);
 538                 perror("Failed to bind Bluetooth socket");
 539                 return -1;
 540         }
 541
 542         return fd;
 543 }
 544
 545 static void server_callback(int fd, uint32_t events, void *user_data)
 546 {
 547         union {
 548                 struct sockaddr common;
 549                 struct sockaddr_un sun;
 550                 struct sockaddr_in sin;
 551         } addr;
 552         socklen_t len;
 553         int host_fd, dev_fd;
 554
 555         if (events & (EPOLLERR | EPOLLHUP)) {
 556                 mainloop_quit();
 557                 return;
 558         }
 559
 560         memset(&addr, 0, sizeof(addr));
 561         len = sizeof(addr);
 562
 563         if (getsockname(fd, &addr.common, &len) < 0) {
 564                 perror("Failed to get socket name");
 565                 return;
 566         }
 567
 568         host_fd = accept(fd, &addr.common, &len);
 569         if (host_fd < 0) {
 570                 perror("Failed to accept client socket");
 571                 return;
 572         }
 573
 574         if (client_active) {
 575                 fprintf(stderr, "Active client already present\n");
 576                 close(host_fd);
 577                 return;
 578         }
 579
 580         dev_fd = open_channel(hci_index);
 581         if (dev_fd < 0) {
 582                 close(host_fd);
 583                 return;
 584         }
 585
 586         printf("New client connected\n");
 587
 588         if (!setup_proxy(host_fd, true, dev_fd, false)) {
 589                 close(dev_fd);
 590                 close(host_fd);
 591                 return;
 592         }
 593
 594         client_active = true;
 595 }
 596
 597 static int open_unix(const char *path)
 598 {
 599         struct sockaddr_un addr;
 600         int fd;
 601
 602         unlink(path);
 603
 604         fd = socket(PF_UNIX, SOCK_STREAM | SOCK_CLOEXEC, 0);
 605         if (fd < 0) {
 606                 perror("Failed to open Unix server socket");
 607                 return -1;
 608         }
 609
 610         memset(&addr, 0, sizeof(addr));
 611         addr.sun_family = AF_UNIX;
 612         strcpy(addr.sun_path, path);
 613
 614         if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 615                 perror("Failed to bind Unix server socket");
 616                 close(fd);
 617                 return -1;
 618         }
 619
 620         if (listen(fd, 1) < 0) {
 621                 perror("Failed to listen Unix server socket");
 622                 close(fd);
 623                 return -1;
 624         }
 625
 626         if (chmod(path, 0666) < 0)
 627                 perror("Failed to change mode");
 628
 629         return fd;
 630 }
 631
 632 static int open_tcp(const char *address, unsigned int port)
 633 {
 634         struct sockaddr_in addr;
 635         int fd, opt = 1;
 636
 637         fd = socket(PF_INET, SOCK_STREAM | SOCK_CLOEXEC, 0);
 638         if (fd < 0) {
 639                 perror("Failed to open TCP server socket");
 640                 return -1;
 641         }
 642
 643         setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
 644
 645         memset(&addr, 0, sizeof(addr));
 646         addr.sin_family = AF_INET;
 647         addr.sin_addr.s_addr = inet_addr(address);
 648         addr.sin_port = htons(port);
 649
 650         if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 651                 perror("Failed to bind TCP server socket");
 652                 close(fd);
 653                 return -1;
 654         }
 655
 656         if (listen(fd, 1) < 0) {
 657                 perror("Failed to listen TCP server socket");
 658                 close(fd);
 659                 return -1;
 660         }
 661
 662         return fd;
 663 }
 664
 665 static int connect_tcp(const char *address, unsigned int port)
 666 {
 667         struct sockaddr_in addr;
 668         int fd;
 669
 670         fd = socket(PF_INET, SOCK_STREAM | SOCK_CLOEXEC, 0);
 671         if (fd < 0) {
 672                 perror("Failed to open TCP client socket");
 673                 return -1;
 674         }
 675
 676         memset(&addr, 0, sizeof(addr));
 677         addr.sin_family = AF_INET;
 678         addr.sin_addr.s_addr = inet_addr(address);
 679         addr.sin_port = htons(port);
 680
 681         if (connect(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 682                 perror("Failed to connect TCP client socket");
 683                 close(fd);
 684                 return -1;
 685         }
 686
 687         return fd;
 688 }
 689
 690 static int open_vhci(uint8_t type)
 691 {
 692         uint8_t create_req[2] = { 0xff, type };
 693         ssize_t written;
 694         int fd;
 695
 696         fd = open("/dev/vhci", O_RDWR | O_CLOEXEC);
 697         if (fd < 0) {
 698                 perror("Failed to open /dev/vhci device");
 699                 return -1;
 700         }
 701
 702         written = write(fd, create_req, sizeof(create_req));
 703         if (written < 0) {
 704                 perror("Failed to set device type");
 705                 close(fd);
 706                 return -1;
 707         }
 708
 709         return fd;
 710 }
 711
 712 static void signal_callback(int signum, void *user_data)
 713 {
 714         switch (signum) {
 715         case SIGINT:
 716         case SIGTERM:
 717                 mainloop_quit();
 718                 break;
 719         }
 720 }
 721
 722 static void usage(void)
 723 {
 724         printf("btproxy - Bluetooth controller proxy\n"
 725                 "Usage:\n");
 726         printf("\tbtproxy [options]\n");
 727         printf("Options:\n"
 728                 "\t-c, --connect <address>     Connect to server\n"
 729                 "\t-l, --listen [address]      Use TCP server\n"
 730                 "\t-u, --unix [path]           Use Unix server\n"
 731                 "\t-p, --port <port>           Use specified TCP port\n"
 732                 "\t-i, --index <num>           Use specified controller\n"
 733                 "\t-a, --amp                   Create AMP controller\n"
 734                 "\t-e, --ecc                   Emulate ECC support\n"
 735                 "\t-d, --debug                 Enable debugging output\n"
 736                 "\t-h, --help                  Show help options\n");
 737 }
 738
 739 static const struct option main_options[] = {
 740         { "redirect", no_argument,       NULL, 'r' },
 741         { "connect",  required_argument, NULL, 'c' },
 742         { "listen",   optional_argument, NULL, 'l' },
 743         { "unix",     optional_argument, NULL, 'u' },
 744         { "port",     required_argument, NULL, 'p' },
 745         { "index",    required_argument, NULL, 'i' },
 746         { "amp",      no_argument,       NULL, 'a' },
 747         { "ecc",      no_argument,       NULL, 'e' },
 748         { "debug",    no_argument,       NULL, 'd' },
 749         { "version",  no_argument,       NULL, 'v' },
 750         { "help",     no_argument,       NULL, 'h' },
 751         { }
 752 };
 753
 754 int main(int argc, char *argv[])
 755 {
 756         const char *connect_address = NULL;
 757         const char *server_address = NULL;
 758         const char *unix_path = NULL;
 759         unsigned short tcp_port = 0xb1ee;       /* 45550 */
 760         bool use_redirect = false;
 761         uint8_t type = HCI_BREDR;
 762         const char *str;
 763         sigset_t mask;
 764
 765         for (;;) {
 766                 int opt;
 767
 768                 opt = getopt_long(argc, argv, "rc:l::u::p:i:aedvh",
 769                                                 main_options, NULL);
 770                 if (opt < 0)
 771                         break;
 772
 773                 switch (opt) {
 774                 case 'r':
 775                         use_redirect = true;
 776                         break;
 777                 case 'c':
 778                         connect_address = optarg;
 779                         break;
 780                 case 'l':
 781                         if (optarg)
 782                                 server_address = optarg;
 783                         else
 784                                 server_address = "0.0.0.0";
 785                         break;
 786                 case 'u':
 787                         if (optarg)
 788                                 unix_path = optarg;
 789                         else
 790                                 unix_path = "/tmp/bt-server-bredr";
 791                         break;
 792                 case 'p':
 793                         tcp_port = atoi(optarg);
 794                         break;
 795                 case 'i':
 796                         if (strlen(optarg) > 3 && !strncmp(optarg, "hci", 3))
 797                                 str = optarg + 3;
 798                         else
 799                                 str = optarg;
 800                         if (!isdigit(*str)) {
 801                                 usage();
 802                                 return EXIT_FAILURE;
 803                         }
 804                         hci_index = atoi(str);
 805                         break;
 806                 case 'a':
 807                         type = HCI_AMP;
 808                         break;
 809                 case 'e':
 810                         emulate_ecc = true;
 811                         break;
 812                 case 'd':
 813                         debug_enabled = true;
 814                         break;
 815                 case 'v':
 816                         printf("%s\n", VERSION);
 817                         return EXIT_SUCCESS;
 818                 case 'h':
 819                         usage();
 820                         return EXIT_SUCCESS;
 821                 default:
 822                         return EXIT_FAILURE;
 823                 }
 824         }
 825
 826         if (argc - optind > 0) {
 827                 fprintf(stderr, "Invalid command line parameters\n");
 828                 return EXIT_FAILURE;
 829         }
 830
 831         if (unix_path && (server_address || use_redirect)) {
 832                 fprintf(stderr, "Invalid to specify TCP and Unix servers\n");
 833                 return EXIT_FAILURE;
 834         }
 835
 836         if (connect_address && (unix_path || server_address || use_redirect)) {
 837                 fprintf(stderr, "Invalid to specify client and server mode\n");
 838                 return EXIT_FAILURE;
 839         }
 840
 841         mainloop_init();
 842
 843         sigemptyset(&mask);
 844         sigaddset(&mask, SIGINT);
 845         sigaddset(&mask, SIGTERM);
 846
 847         mainloop_set_signal(&mask, signal_callback, NULL, NULL);
 848
 849         if (connect_address || use_redirect) {
 850                 int host_fd, dev_fd;
 851
 852                 if (use_redirect) {
 853                         printf("Creating local redirect\n");
 854
 855                         dev_fd = open_channel(hci_index);
 856                 } else {
 857                         printf("Connecting to %s:%u\n", connect_address,
 858                                                                 tcp_port);
 859
 860                         dev_fd = connect_tcp(connect_address, tcp_port);
 861                 }
 862
 863                 if (dev_fd < 0)
 864                         return EXIT_FAILURE;
 865
 866                 printf("Opening virtual device\n");
 867
 868                 host_fd = open_vhci(type);
 869                 if (host_fd < 0) {
 870                         close(dev_fd);
 871                         return EXIT_FAILURE;
 872                 }
 873
 874                 if (!setup_proxy(host_fd, false, dev_fd, true)) {
 875                         close(dev_fd);
 876                         close(host_fd);
 877                         return EXIT_FAILURE;
 878                 }
 879         } else {
 880                 int server_fd;
 881
 882                 if (unix_path) {
 883                         printf("Listening on %s\n", unix_path);
 884
 885                         server_fd = open_unix(unix_path);
 886                 } else if (server_address) {
 887                         printf("Listening on %s:%u\n", server_address,
 888                                                                 tcp_port);
 889
 890                         server_fd = open_tcp(server_address, tcp_port);
 891                 } else {
 892                         fprintf(stderr, "Missing emulator device\n");
 893                         return EXIT_FAILURE;
 894                 }
 895
 896                 if (server_fd < 0)
 897                         return EXIT_FAILURE;
 898
 899                 mainloop_add_fd(server_fd, EPOLLIN, server_callback,
 900                                                         NULL, NULL);
 901         }
 902
 903         return mainloop_run();
 904 }