diff options
author | Marcel Holtmann <marcel@holtmann.org> | 2014-12-14 05:05:19 +0100 |
---|---|---|
committer | Marcel Holtmann <marcel@holtmann.org> | 2014-12-14 05:14:37 +0100 |
commit | 03336eed5a314a01187243ecd3598a620f13b8a4 (patch) | |
tree | c5054b45b5cb040cbcf95ff7b8abc026d32e211c /emulator | |
parent | 8470d254636888f0b1fee893b1c5192b240e5c95 (diff) | |
download | bluez-03336eed5a314a01187243ecd3598a620f13b8a4.tar.gz |
emulator: Add support for simple PHY simulation
Diffstat (limited to 'emulator')
-rw-r--r-- | emulator/phy.c | 228 | ||||
-rw-r--r-- | emulator/phy.h | 29 |
2 files changed, 257 insertions, 0 deletions
diff --git a/emulator/phy.c b/emulator/phy.c index 30808df6d..9965ccaa8 100644 --- a/emulator/phy.c +++ b/emulator/phy.c @@ -26,14 +26,132 @@ #include <config.h> #endif +#include <fcntl.h> +#include <unistd.h> #include <stdlib.h> +#include <string.h> +#include <sys/socket.h> +#include <netinet/in.h> +#include <netinet/ip.h> +#include <time.h> + +#include "src/shared/util.h" +#include "monitor/mainloop.h" #include "phy.h" +#define BT_PHY_PORT 45023 + struct bt_phy { volatile int ref_count; + int rx_fd; + int tx_fd; + uint64_t id; + bt_phy_callback_func_t callback; + void *user_data; }; +struct bt_phy_hdr { + uint64_t id; + uint32_t flags; + uint16_t type; + uint16_t len; +} __attribute__ ((packed)); + +static bool get_random_bytes(void *buf, size_t num_bytes) +{ + ssize_t len; + int fd; + + fd = open("/dev/urandom", O_RDONLY); + if (fd < 0) + return false; + + len = read(fd, buf, num_bytes); + if (len < 0) + return false; + + return true; +} + +static void phy_rx_callback(int fd, uint32_t events, void *user_data) +{ + struct bt_phy *phy = user_data; + struct msghdr msg; + struct iovec iov[2]; + struct bt_phy_hdr hdr; + unsigned char buf[4096]; + ssize_t len; + + if (events & (EPOLLERR | EPOLLHUP)) { + mainloop_remove_fd(fd); + return; + } + + iov[0].iov_base = &hdr; + iov[0].iov_len = sizeof(hdr); + iov[1].iov_base = buf; + iov[1].iov_len = sizeof(buf); + + memset(&msg, 0, sizeof(msg)); + msg.msg_iov = iov; + msg.msg_iovlen = 2; + + len = recvmsg(phy->rx_fd, &msg, MSG_DONTWAIT); + if (len < 0) + return; + + if ((size_t) len < sizeof(hdr)) + return; + + if (le64_to_cpu(hdr.id) == phy->id) + return; + + if (len - sizeof(hdr) != le16_to_cpu(hdr.len)) + return; + + if (phy->callback) + phy->callback(le16_to_cpu(hdr.type), + buf, len - sizeof(hdr), phy->user_data); +} + +static int create_rx_socket(void) +{ + struct sockaddr_in addr; + int fd, opt = 1; + + fd = socket(PF_INET, SOCK_DGRAM | SOCK_CLOEXEC, 0); + if (fd < 0) + return -1; + + setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)); + + memset(&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + addr.sin_port = htons(BT_PHY_PORT); + addr.sin_addr.s_addr = INADDR_BROADCAST; + + if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + close(fd); + return -1; + } + + return fd; +} + +static int create_tx_socket(void) +{ + int fd, opt = 1; + + fd = socket(PF_INET, SOCK_DGRAM | SOCK_CLOEXEC, 0); + if (fd < 0) + return -1; + + setsockopt(fd, SOL_SOCKET, SO_BROADCAST, &opt, sizeof(opt)); + + return fd; +} + struct bt_phy *bt_phy_new(void) { struct bt_phy *phy; @@ -42,6 +160,28 @@ struct bt_phy *bt_phy_new(void) if (!phy) return NULL; + phy->rx_fd = create_rx_socket(); + if (phy->rx_fd < 0) { + free(phy); + return NULL; + } + + phy->tx_fd = create_tx_socket(); + if (phy->tx_fd < 0) { + close(phy->rx_fd); + free(phy); + return NULL; + } + + mainloop_add_fd(phy->rx_fd, EPOLLIN, phy_rx_callback, phy, NULL); + + if (!get_random_bytes(&phy->id, sizeof(phy->id))) { + srandom(time(NULL)); + phy->id = random(); + } + + bt_phy_send(phy, BT_PHY_PKT_NULL, NULL, 0); + return bt_phy_ref(phy); } @@ -63,5 +203,93 @@ void bt_phy_unref(struct bt_phy *phy) if (__sync_sub_and_fetch(&phy->ref_count, 1)) return; + mainloop_remove_fd(phy->rx_fd); + + close(phy->tx_fd); + close(phy->rx_fd); + free(phy); } + +bool bt_phy_send(struct bt_phy *phy, uint16_t type, + const void *data, size_t size) +{ + return bt_phy_send_vector(phy, type, data, size, NULL, 0, NULL, 0); +} + +bool bt_phy_send_vector(struct bt_phy *phy, uint16_t type, + const void *data1, size_t size1, + const void *data2, size_t size2, + const void *data3, size_t size3) +{ + struct bt_phy_hdr hdr; + struct sockaddr_in addr; + struct msghdr msg; + struct iovec iov[4]; + ssize_t len; + + if (!phy) + return false; + + memset(&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + addr.sin_port = htons(BT_PHY_PORT); + addr.sin_addr.s_addr = INADDR_BROADCAST; + + memset(&msg, 0, sizeof(msg)); + msg.msg_name = &addr; + msg.msg_namelen = sizeof(addr); + msg.msg_iov = iov; + msg.msg_iovlen = 0; + + memset(&hdr, 0, sizeof(hdr)); + hdr.id = cpu_to_le64(phy->id); + hdr.flags = cpu_to_le32(0); + hdr.type = cpu_to_le16(type); + hdr.len = cpu_to_le16(size1 + size2 + size3); + + iov[msg.msg_iovlen].iov_base = &hdr; + iov[msg.msg_iovlen].iov_len = sizeof(hdr); + msg.msg_iovlen++; + + if (data1 && size1 > 0) { + iov[msg.msg_iovlen].iov_base = (void *) data1; + iov[msg.msg_iovlen].iov_len = size1; + msg.msg_iovlen++; + } + + if (data2 && size2 > 0) { + iov[msg.msg_iovlen].iov_base = (void *) data2; + iov[msg.msg_iovlen].iov_len = size2; + msg.msg_iovlen++; + } + + if (data3 && size3 > 0) { + iov[msg.msg_iovlen].iov_base = (void *) data3; + iov[msg.msg_iovlen].iov_len = size3; + msg.msg_iovlen++; + } + + memset(&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + addr.sin_port = htons(BT_PHY_PORT); + addr.sin_addr.s_addr = INADDR_BROADCAST; + + len = sendmsg(phy->tx_fd, &msg, MSG_DONTWAIT); + if (len < 0) + return false; + + return true; +} + +bool bt_phy_register(struct bt_phy *phy, bt_phy_callback_func_t callback, + void *user_data) +{ + if (!phy) + return false; + + phy->callback = callback; + phy->user_data = user_data; + + return true; +} diff --git a/emulator/phy.h b/emulator/phy.h index a070e0353..6225bfb3a 100644 --- a/emulator/phy.h +++ b/emulator/phy.h @@ -22,9 +22,38 @@ * */ +#include <stdbool.h> +#include <stdint.h> + struct bt_phy; struct bt_phy *bt_phy_new(void); struct bt_phy *bt_phy_ref(struct bt_phy *phy); void bt_phy_unref(struct bt_phy *phy); + +bool bt_phy_send(struct bt_phy *phy, uint16_t type, + const void *data, size_t size); +bool bt_phy_send_vector(struct bt_phy *phy, uint16_t type, + const void *data1, size_t size1, + const void *data2, size_t size2, + const void *data3, size_t size3); + +typedef void (*bt_phy_callback_func_t)(uint16_t type, const void *data, + size_t size, void *user_data); + +bool bt_phy_register(struct bt_phy *phy, bt_phy_callback_func_t callback, + void *user_data); + +#define BT_PHY_PKT_NULL 0x0000 + +#define BT_PHY_PKT_ADV 0x0001 +struct bt_phy_pkt_adv { + uint8_t pdu_type; + uint8_t tx_addr_type; + uint8_t tx_addr[6]; + uint8_t rx_addr_type; + uint8_t rx_addr[6]; + uint8_t adv_data_len; + uint8_t scan_rsp_len; +} __attribute__ ((packed)); |