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