test/hciemu.c

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