emulator/phy.c

   1 // SPDX-License-Identifier: LGPL-2.1-or-later
   2 /*
   3  *
   4  *  BlueZ - Bluetooth protocol stack for Linux
   5  *
   6  *  Copyright (C) 2011-2012  Intel Corporation
   7  *  Copyright (C) 2004-2010  Marcel Holtmann <marcel@holtmann.org>
   8  *
   9  *
  10  */
  11
  12 #ifdef HAVE_CONFIG_H
  13 #include <config.h>
  14 #endif
  15
  16 #include <fcntl.h>
  17 #include <unistd.h>
  18 #include <stdlib.h>
  19 #include <string.h>
  20 #include <sys/socket.h>
  21 #include <sys/random.h>
  22 #include <netinet/in.h>
  23 #include <netinet/ip.h>
  24 #include <time.h>
  25
  26 #include "src/shared/util.h"
  27 #include "src/shared/mainloop.h"
  28
  29 #include "phy.h"
  30
  31 #define BT_PHY_PORT 45023
  32
  33 struct bt_phy {
  34         volatile int ref_count;
  35         int rx_fd;
  36         int tx_fd;
  37         uint64_t id;
  38         bt_phy_callback_func_t callback;
  39         void *user_data;
  40 };
  41
  42 struct bt_phy_hdr {
  43         uint64_t id;
  44         uint32_t flags;
  45         uint16_t type;
  46         uint16_t len;
  47 } __attribute__ ((packed));
  48
  49 static bool get_random_bytes(void *buf, size_t num_bytes)
  50 {
  51         ssize_t len;
  52         int fd;
  53
  54         fd = open("/dev/urandom", O_RDONLY);
  55         if (fd < 0)
  56                 return false;
  57
  58         len = read(fd, buf, num_bytes);
  59
  60         close(fd);
  61
  62         if (len < 0)
  63                 return false;
  64
  65         return true;
  66 }
  67
  68 static void phy_rx_callback(int fd, uint32_t events, void *user_data)
  69 {
  70         struct bt_phy *phy = user_data;
  71         struct msghdr msg;
  72         struct iovec iov[2];
  73         struct bt_phy_hdr hdr;
  74         unsigned char buf[4096];
  75         ssize_t len;
  76
  77         if (events & (EPOLLERR | EPOLLHUP)) {
  78                 mainloop_remove_fd(fd);
  79                 return;
  80         }
  81
  82         iov[0].iov_base = &hdr;
  83         iov[0].iov_len = sizeof(hdr);
  84         iov[1].iov_base = buf;
  85         iov[1].iov_len = sizeof(buf);
  86
  87         memset(&msg, 0, sizeof(msg));
  88         msg.msg_iov = iov;
  89         msg.msg_iovlen = 2;
  90
  91         len = recvmsg(phy->rx_fd, &msg, MSG_DONTWAIT);
  92         if (len < 0)
  93                 return;
  94
  95         if ((size_t) len < sizeof(hdr))
  96                 return;
  97
  98         if (le64_to_cpu(hdr.id) == phy->id)
  99                 return;
 100
 101         if (len - sizeof(hdr) != le16_to_cpu(hdr.len))
 102                 return;
 103
 104         if (phy->callback)
 105                 phy->callback(le16_to_cpu(hdr.type),
 106                                 buf, len - sizeof(hdr), phy->user_data);
 107 }
 108
 109 static int create_rx_socket(void)
 110 {
 111         struct sockaddr_in addr;
 112         int fd, opt = 1;
 113
 114         fd = socket(PF_INET, SOCK_DGRAM | SOCK_CLOEXEC, 0);
 115         if (fd < 0)
 116                 return -1;
 117
 118         if (setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0) {
 119                 close(fd);
 120                 return -1;
 121         }
 122
 123         memset(&addr, 0, sizeof(addr));
 124         addr.sin_family = AF_INET;
 125         addr.sin_port = htons(BT_PHY_PORT);
 126         addr.sin_addr.s_addr = INADDR_BROADCAST;
 127
 128         if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 129                 close(fd);
 130                 return -1;
 131         }
 132
 133         return fd;
 134 }
 135
 136 static int create_tx_socket(void)
 137 {
 138         int fd, opt = 1;
 139
 140         fd = socket(PF_INET, SOCK_DGRAM | SOCK_CLOEXEC, 0);
 141         if (fd < 0)
 142                 return -1;
 143
 144         if (setsockopt(fd, SOL_SOCKET, SO_BROADCAST, &opt, sizeof(opt)) < 0) {
 145                 close(fd);
 146                 return -1;
 147         }
 148
 149         return fd;
 150 }
 151
 152 struct bt_phy *bt_phy_new(void)
 153 {
 154         struct bt_phy *phy;
 155
 156         phy = calloc(1, sizeof(*phy));
 157         if (!phy)
 158                 return NULL;
 159
 160         phy->rx_fd = create_rx_socket();
 161         if (phy->rx_fd < 0) {
 162                 free(phy);
 163                 return NULL;
 164         }
 165
 166         phy->tx_fd = create_tx_socket();
 167         if (phy->tx_fd < 0) {
 168                 close(phy->rx_fd);
 169                 free(phy);
 170                 return NULL;
 171         }
 172
 173         mainloop_add_fd(phy->rx_fd, EPOLLIN, phy_rx_callback, phy, NULL);
 174
 175         if (!get_random_bytes(&phy->id, sizeof(phy->id))) {
 176                 if (getrandom(&phy->id, sizeof(phy->id), 0) < 0) {
 177                         mainloop_remove_fd(phy->rx_fd);
 178                         close(phy->tx_fd);
 179                         close(phy->rx_fd);
 180                         free(phy);
 181                         return NULL;
 182                 }
 183         }
 184
 185         bt_phy_send(phy, BT_PHY_PKT_NULL, NULL, 0);
 186
 187         return bt_phy_ref(phy);
 188 }
 189
 190 struct bt_phy *bt_phy_ref(struct bt_phy *phy)
 191 {
 192         if (!phy)
 193                 return NULL;
 194
 195         __sync_fetch_and_add(&phy->ref_count, 1);
 196
 197         return phy;
 198 }
 199
 200 void bt_phy_unref(struct bt_phy *phy)
 201 {
 202         if (!phy)
 203                 return;
 204
 205         if (__sync_sub_and_fetch(&phy->ref_count, 1))
 206                 return;
 207
 208         mainloop_remove_fd(phy->rx_fd);
 209
 210         close(phy->tx_fd);
 211         close(phy->rx_fd);
 212
 213         free(phy);
 214 }
 215
 216 bool bt_phy_send(struct bt_phy *phy, uint16_t type,
 217                                         const void *data, size_t size)
 218 {
 219         return bt_phy_send_vector(phy, type, data, size, NULL, 0, NULL, 0);
 220 }
 221
 222 bool bt_phy_send_vector(struct bt_phy *phy, uint16_t type,
 223                                         const void *data1, size_t size1,
 224                                         const void *data2, size_t size2,
 225                                         const void *data3, size_t size3)
 226 {
 227         struct bt_phy_hdr hdr;
 228         struct sockaddr_in addr;
 229         struct msghdr msg;
 230         struct iovec iov[4];
 231         ssize_t len;
 232
 233         if (!phy)
 234                 return false;
 235
 236         memset(&addr, 0, sizeof(addr));
 237         addr.sin_family = AF_INET;
 238         addr.sin_port = htons(BT_PHY_PORT);
 239         addr.sin_addr.s_addr = INADDR_BROADCAST;
 240
 241         memset(&msg, 0, sizeof(msg));
 242         msg.msg_name = &addr;
 243         msg.msg_namelen = sizeof(addr);
 244         msg.msg_iov = iov;
 245         msg.msg_iovlen = 0;
 246
 247         memset(&hdr, 0, sizeof(hdr));
 248         hdr.id = cpu_to_le64(phy->id);
 249         hdr.flags = cpu_to_le32(0);
 250         hdr.type = cpu_to_le16(type);
 251         hdr.len = cpu_to_le16(size1 + size2 + size3);
 252
 253         iov[msg.msg_iovlen].iov_base = &hdr;
 254         iov[msg.msg_iovlen].iov_len = sizeof(hdr);
 255         msg.msg_iovlen++;
 256
 257         if (data1 && size1 > 0) {
 258                 iov[msg.msg_iovlen].iov_base = (void *) data1;
 259                 iov[msg.msg_iovlen].iov_len = size1;
 260                 msg.msg_iovlen++;
 261         }
 262
 263         if (data2 && size2 > 0) {
 264                 iov[msg.msg_iovlen].iov_base = (void *) data2;
 265                 iov[msg.msg_iovlen].iov_len = size2;
 266                 msg.msg_iovlen++;
 267         }
 268
 269         if (data3 && size3 > 0) {
 270                 iov[msg.msg_iovlen].iov_base = (void *) data3;
 271                 iov[msg.msg_iovlen].iov_len = size3;
 272                 msg.msg_iovlen++;
 273         }
 274
 275         memset(&addr, 0, sizeof(addr));
 276         addr.sin_family = AF_INET;
 277         addr.sin_port = htons(BT_PHY_PORT);
 278         addr.sin_addr.s_addr = INADDR_BROADCAST;
 279
 280         len = sendmsg(phy->tx_fd, &msg, MSG_DONTWAIT);
 281         if (len < 0)
 282                 return false;
 283
 284         return true;
 285 }
 286
 287 bool bt_phy_register(struct bt_phy *phy, bt_phy_callback_func_t callback,
 288                                                         void *user_data)
 289 {
 290         if (!phy)
 291                 return false;
 292
 293         phy->callback = callback;
 294         phy->user_data = user_data;
 295
 296         return true;
 297 }