emulator/b1ee.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2011-2012  Intel Corporation
   6  *  Copyright (C) 2004-2010  Marcel Holtmann <marcel@holtmann.org>
   7  *
   8  *
   9  *  This program is free software; you can redistribute it and/or modify
  10  *  it under the terms of the GNU General Public License as published by
  11  *  the Free Software Foundation; either version 2 of the License, or
  12  *  (at your option) any later version.
  13  *
  14  *  This program is distributed in the hope that it will be useful,
  15  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  16  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  17  *  GNU General Public License for more details.
  18  *
  19  *  You should have received a copy of the GNU General Public License
  20  *  along with this program; if not, write to the Free Software
  21  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  22  *
  23  */
  24
  25 #ifdef HAVE_CONFIG_H
  26 #include <config.h>
  27 #endif
  28
  29 #include <stdio.h>
  30 #include <errno.h>
  31 #include <fcntl.h>
  32 #include <unistd.h>
  33 #include <stdlib.h>
  34 #include <string.h>
  35
  36 #include <netdb.h>
  37 #include <arpa/inet.h>
  38
  39 #include "lib/bluetooth.h"
  40 #include "lib/hci.h"
  41
  42 #include "src/shared/mainloop.h"
  43
  44 #define DEFAULT_SERVER          "b1ee.com"
  45 #define DEFAULT_HOST_PORT       "45550"         /* 0xb1ee */
  46 #define DEFAULT_SNIFFER_PORT    "45551"         /* 0xb1ef */
  47
  48 static int sniffer_fd;
  49 static int server_fd;
  50 static int vhci_fd;
  51
  52 static void sniffer_read_callback(int fd, uint32_t events, void *user_data)
  53 {
  54         static uint8_t buf[4096];
  55         ssize_t len;
  56
  57         if (events & (EPOLLERR | EPOLLHUP))
  58                 return;
  59
  60 again:
  61         len = recv(fd, buf, sizeof(buf), MSG_DONTWAIT);
  62         if (len < 0) {
  63                 if (errno == EAGAIN)
  64                         goto again;
  65                 return;
  66         }
  67
  68         printf("Sniffer received: %zi bytes\n", len);
  69 }
  70
  71 static uint8_t *server_pkt_data;
  72 static uint8_t server_pkt_type;
  73 static uint16_t server_pkt_expect;
  74 static uint16_t server_pkt_len;
  75 static uint16_t server_pkt_offset;
  76
  77 static void server_read_callback(int fd, uint32_t events, void *user_data)
  78 {
  79         static uint8_t buf[4096];
  80         uint8_t *ptr = buf;
  81         ssize_t len;
  82         uint16_t count;
  83
  84         if (events & (EPOLLERR | EPOLLHUP))
  85                 return;
  86
  87 again:
  88         len = recv(fd, buf + server_pkt_offset,
  89                         sizeof(buf) - server_pkt_offset, MSG_DONTWAIT);
  90         if (len < 0) {
  91                 if (errno == EAGAIN)
  92                         goto again;
  93                 return;
  94         }
  95
  96         count = server_pkt_offset + len;
  97
  98         while (count > 0) {
  99                 hci_event_hdr *evt_hdr;
 100
 101                 if (!server_pkt_data) {
 102                         server_pkt_type = ptr[0];
 103
 104                         switch (server_pkt_type) {
 105                         case HCI_EVENT_PKT:
 106                                 if (count < HCI_EVENT_HDR_SIZE + 1) {
 107                                         server_pkt_offset += len;
 108                                         return;
 109                                 }
 110                                 evt_hdr = (hci_event_hdr *) (ptr + 1);
 111                                 server_pkt_expect = HCI_EVENT_HDR_SIZE +
 112                                                         evt_hdr->plen + 1;
 113                                 server_pkt_data = malloc(server_pkt_expect);
 114                                 server_pkt_len = 0;
 115                                 break;
 116                         default:
 117                                 fprintf(stderr, "Unknown packet from server\n");
 118                                 return;
 119                         }
 120
 121                         server_pkt_offset = 0;
 122                 }
 123
 124                 if (count >= server_pkt_expect) {
 125                         ssize_t written;
 126
 127                         memcpy(server_pkt_data + server_pkt_len,
 128                                                 ptr, server_pkt_expect);
 129                         ptr += server_pkt_expect;
 130                         count -= server_pkt_expect;
 131
 132                         written = write(vhci_fd, server_pkt_data,
 133                                         server_pkt_len + server_pkt_expect);
 134                         if (written != server_pkt_len + server_pkt_expect)
 135                                 fprintf(stderr, "Write to /dev/vhci failed\n");
 136
 137                         free(server_pkt_data);
 138                         server_pkt_data = NULL;
 139                 } else {
 140                         memcpy(server_pkt_data + server_pkt_len, ptr, count);
 141                         server_pkt_len += count;
 142                         server_pkt_expect -= count;
 143                         count = 0;
 144                 }
 145         }
 146 }
 147
 148 static void vhci_read_callback(int fd, uint32_t events, void *user_data)
 149 {
 150         unsigned char buf[4096];
 151         ssize_t len, written;
 152
 153         if (events & (EPOLLERR | EPOLLHUP))
 154                 return;
 155
 156         len = read(fd, buf, sizeof(buf));
 157         if (len < 0)
 158                 return;
 159
 160         written = write(server_fd, buf, len);
 161         if (written != len)
 162                 fprintf(stderr, "Write to server failed\n");
 163 }
 164
 165 static void signal_callback(int signum, void *user_data)
 166 {
 167         switch (signum) {
 168         case SIGINT:
 169         case SIGTERM:
 170                 mainloop_quit();
 171                 break;
 172         }
 173 }
 174
 175 static int do_connect(const char *node, const char *service)
 176 {
 177         struct addrinfo hints;
 178         struct addrinfo *info, *res;
 179         int err, fd = -1;
 180
 181         memset(&hints, 0, sizeof(hints));
 182         hints.ai_family = PF_UNSPEC;
 183         hints.ai_socktype = SOCK_STREAM;
 184
 185         err = getaddrinfo(DEFAULT_SERVER, DEFAULT_HOST_PORT, &hints, &res);
 186         if (err) {
 187                 perror(gai_strerror(err));
 188                 exit(1);
 189         }
 190
 191         for (info = res; info; info = info->ai_next) {
 192                 char str[INET6_ADDRSTRLEN];
 193
 194                 inet_ntop(info->ai_family, info->ai_addr->sa_data,
 195                                                         str, sizeof(str));
 196
 197                 fd = socket(info->ai_family, info->ai_socktype,
 198                                                 info->ai_protocol);
 199                 if (fd < 0)
 200                         continue;
 201
 202                 printf("Trying to connect to %s on port %s\n", str, service);
 203
 204                 if (connect(fd, res->ai_addr, res->ai_addrlen) < 0) {
 205                         perror("Failed to connect");
 206                         close(fd);
 207                         continue;
 208                 }
 209
 210                 printf("Successfully connected to %s on port %s\n",
 211                                                         str, service);
 212                 break;
 213         }
 214
 215         freeaddrinfo(res);
 216
 217         if (res == NULL)
 218                 exit(1);
 219
 220         return fd;
 221 }
 222
 223 int main(int argc, char *argv[])
 224 {
 225         const char sniff_cmd[] = { 0x01, 0x00,
 226                                         0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
 227         ssize_t written;
 228         sigset_t mask;
 229
 230         server_fd = do_connect(DEFAULT_SERVER, DEFAULT_HOST_PORT);
 231         sniffer_fd = do_connect(DEFAULT_SERVER, DEFAULT_SNIFFER_PORT);
 232
 233         written = write(sniffer_fd, sniff_cmd, sizeof(sniff_cmd));
 234         if (written < 0)
 235                 perror("Failed to enable sniffer");
 236
 237         vhci_fd = open("/dev/vhci", O_RDWR | O_NONBLOCK);
 238         if (vhci_fd < 0) {
 239                 perror("Failed to /dev/vhci");
 240                 close(server_fd);
 241                 exit(1);
 242         }
 243
 244         mainloop_init();
 245
 246         sigemptyset(&mask);
 247         sigaddset(&mask, SIGINT);
 248         sigaddset(&mask, SIGTERM);
 249
 250         mainloop_set_signal(&mask, signal_callback, NULL, NULL);
 251
 252         mainloop_add_fd(sniffer_fd, EPOLLIN, sniffer_read_callback, NULL, NULL);
 253         mainloop_add_fd(server_fd, EPOLLIN, server_read_callback, NULL, NULL);
 254         mainloop_add_fd(vhci_fd, EPOLLIN, vhci_read_callback, NULL, NULL);
 255
 256         return mainloop_run();
 257 }