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