emulator/serial.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2011-2014  Intel Corporation
   6  *  Copyright (C) 2004-2010  Marcel Holtmann <marcel@holtmann.org>
   7  *
   8  *
   9  *  This library is free software; you can redistribute it and/or
  10  *  modify it under the terms of the GNU Lesser General Public
  11  *  License as published by the Free Software Foundation; either
  12  *  version 2.1 of the License, or (at your option) any later version.
  13  *
  14  *  This library 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 GNU
  17  *  Lesser General Public License for more details.
  18  *
  19  *  You should have received a copy of the GNU Lesser General Public
  20  *  License along with this library; if not, write to the Free Software
  21  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  22  *
  23  */
  24
  25 #ifdef HAVE_CONFIG_H
  26 #include <config.h>
  27 #endif
  28
  29 #include <stdio.h>
  30 #include <errno.h>
  31 #include <ctype.h>
  32 #include <unistd.h>
  33 #include <stdlib.h>
  34 #include <string.h>
  35 #include <fcntl.h>
  36 #include <sys/param.h>
  37 #include <sys/epoll.h>
  38 #include <sys/uio.h>
  39
  40 #include "lib/bluetooth.h"
  41 #include "lib/hci.h"
  42
  43 #include "src/shared/mainloop.h"
  44 #include "btdev.h"
  45 #include "serial.h"
  46
  47 #define uninitialized_var(x) x = x
  48
  49 struct serial {
  50         enum serial_type type;
  51         uint16_t id;
  52         int fd;
  53         char path[PATH_MAX];
  54         struct btdev *btdev;
  55         uint8_t *pkt_data;
  56         uint8_t pkt_type;
  57         uint16_t pkt_expect;
  58         uint16_t pkt_len;
  59         uint16_t pkt_offset;
  60 };
  61
  62 static void open_pty(struct serial *serial);
  63
  64 static void serial_destroy(void *user_data)
  65 {
  66         struct serial *serial = user_data;
  67
  68         btdev_destroy(serial->btdev);
  69         serial->btdev = NULL;
  70
  71         close(serial->fd);
  72         serial->fd = -1;
  73 }
  74
  75 static void serial_write_callback(const struct iovec *iov, int iovlen,
  76                                                         void *user_data)
  77 {
  78         struct serial *serial = user_data;
  79         ssize_t written;
  80
  81         written = writev(serial->fd, iov, iovlen);
  82         if (written < 0)
  83                 return;
  84 }
  85
  86 static void serial_read_callback(int fd, uint32_t events, void *user_data)
  87 {
  88         struct serial *serial = user_data;
  89         static uint8_t buf[4096];
  90         uint8_t *ptr = buf;
  91         ssize_t len;
  92         uint16_t count;
  93
  94         if (events & (EPOLLERR | EPOLLHUP)) {
  95                 mainloop_remove_fd(serial->fd);
  96                 open_pty(serial);
  97                 return;
  98         }
  99
 100 again:
 101         len = read(serial->fd, buf + serial->pkt_offset,
 102                         sizeof(buf) - serial->pkt_offset);
 103         if (len < 0) {
 104                 if (errno == EAGAIN)
 105                         goto again;
 106                 return;
 107         }
 108
 109         if (!serial->btdev)
 110                 return;
 111
 112         count = serial->pkt_offset + len;
 113
 114         while (count > 0) {
 115                 hci_command_hdr *cmd_hdr;
 116
 117                 if (!serial->pkt_data) {
 118                         serial->pkt_type = ptr[0];
 119
 120                         switch (serial->pkt_type) {
 121                         case HCI_COMMAND_PKT:
 122                                 if (count < HCI_COMMAND_HDR_SIZE + 1) {
 123                                         serial->pkt_offset += len;
 124                                         return;
 125                                 }
 126                                 cmd_hdr = (hci_command_hdr *) (ptr + 1);
 127                                 serial->pkt_expect = HCI_COMMAND_HDR_SIZE +
 128                                                         cmd_hdr->plen + 1;
 129                                 serial->pkt_data = malloc(serial->pkt_expect);
 130                                 serial->pkt_len = 0;
 131                                 break;
 132                         default:
 133                                 printf("packet error\n");
 134                                 return;
 135                         }
 136
 137                         serial->pkt_offset = 0;
 138                 }
 139
 140                 if (count >= serial->pkt_expect) {
 141                         memcpy(serial->pkt_data + serial->pkt_len,
 142                                                 ptr, serial->pkt_expect);
 143                         ptr += serial->pkt_expect;
 144                         count -= serial->pkt_expect;
 145
 146                         btdev_receive_h4(serial->btdev, serial->pkt_data,
 147                                         serial->pkt_len + serial->pkt_expect);
 148
 149                         free(serial->pkt_data);
 150                         serial->pkt_data = NULL;
 151                 } else {
 152                         memcpy(serial->pkt_data + serial->pkt_len, ptr, count);
 153                         serial->pkt_len += count;
 154                         serial->pkt_expect -= count;
 155                         count = 0;
 156                 }
 157         }
 158 }
 159
 160 static void open_pty(struct serial *serial)
 161 {
 162         enum btdev_type uninitialized_var(type);
 163
 164         serial->fd = posix_openpt(O_RDWR | O_NOCTTY);
 165         if (serial->fd < 0) {
 166                 perror("Failed to get master pseudo terminal");
 167                 return;
 168         }
 169
 170         if (grantpt(serial->fd) < 0) {
 171                 perror("Failed to grant slave pseudo terminal");
 172                 close(serial->fd);
 173                 serial->fd = -1;
 174                 return;
 175         }
 176
 177         if (unlockpt(serial->fd) < 0) {
 178                 perror("Failed to unlock slave pseudo terminal");
 179                 close(serial->fd);
 180                 serial->fd = -1;
 181                 return;
 182         }
 183
 184         ptsname_r(serial->fd, serial->path, sizeof(serial->path));
 185
 186         printf("Pseudo terminal at %s\n", serial->path);
 187
 188         switch (serial->type) {
 189         case SERIAL_TYPE_BREDRLE:
 190                 type = BTDEV_TYPE_BREDRLE;
 191                 break;
 192         case SERIAL_TYPE_BREDR:
 193                 type = BTDEV_TYPE_BREDR;
 194                 break;
 195         case SERIAL_TYPE_LE:
 196                 type = BTDEV_TYPE_LE;
 197                 break;
 198         case SERIAL_TYPE_AMP:
 199                 type = BTDEV_TYPE_AMP;
 200                 break;
 201         }
 202
 203         serial->btdev = btdev_create(type, serial->id);
 204         if (!serial->btdev) {
 205                 close(serial->fd);
 206                 serial->fd = -1;
 207                 return;
 208         }
 209
 210         btdev_set_send_handler(serial->btdev, serial_write_callback, serial);
 211
 212         if (mainloop_add_fd(serial->fd, EPOLLIN, serial_read_callback,
 213                                                 serial, serial_destroy) < 0) {
 214                 btdev_destroy(serial->btdev);
 215                 serial->btdev = NULL;
 216                 close(serial->fd);
 217                 serial->fd = -1;
 218                 return;
 219         }
 220 }
 221
 222 struct serial *serial_open(enum serial_type type)
 223 {
 224         struct serial *serial;
 225         enum btdev_type uninitialized_var(dev_type);
 226
 227         serial = malloc(sizeof(*serial));
 228         if (!serial)
 229                 return NULL;
 230
 231         memset(serial, 0, sizeof(*serial));
 232         serial->type = type;
 233         serial->id = 0x42;
 234
 235         open_pty(serial);
 236
 237         return serial;
 238 }
 239
 240 void serial_close(struct serial *serial)
 241 {
 242         if (!serial)
 243                 return;
 244
 245         mainloop_remove_fd(serial->fd);
 246
 247         free(serial);
 248 }