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