| // SPDX-License-Identifier: LGPL-2.1-or-later |
| /* |
| * |
| * BlueZ - Bluetooth protocol stack for Linux |
| * |
| * Copyright (C) 2011-2012 Intel Corporation |
| * Copyright (C) 2004-2010 Marcel Holtmann <marcel@holtmann.org> |
| * |
| * |
| */ |
| |
| #ifdef HAVE_CONFIG_H |
| #include <config.h> |
| #endif |
| |
| #define _GNU_SOURCE |
| #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 "src/shared/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); |
| |
| close(fd); |
| |
| 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; |
| |
| if (setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0) { |
| close(fd); |
| return -1; |
| } |
| |
| 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; |
| |
| if (setsockopt(fd, SOL_SOCKET, SO_BROADCAST, &opt, sizeof(opt)) < 0) { |
| close(fd); |
| return -1; |
| } |
| |
| return fd; |
| } |
| |
| struct bt_phy *bt_phy_new(void) |
| { |
| struct bt_phy *phy; |
| |
| phy = calloc(1, sizeof(*phy)); |
| 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))) { |
| if (util_getrandom(&phy->id, sizeof(phy->id), 0) < 0) { |
| mainloop_remove_fd(phy->rx_fd); |
| close(phy->tx_fd); |
| close(phy->rx_fd); |
| free(phy); |
| return NULL; |
| } |
| } |
| |
| bt_phy_send(phy, BT_PHY_PKT_NULL, NULL, 0); |
| |
| return bt_phy_ref(phy); |
| } |
| |
| struct bt_phy *bt_phy_ref(struct bt_phy *phy) |
| { |
| if (!phy) |
| return NULL; |
| |
| __sync_fetch_and_add(&phy->ref_count, 1); |
| |
| return phy; |
| } |
| |
| void bt_phy_unref(struct bt_phy *phy) |
| { |
| if (!phy) |
| return; |
| |
| 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; |
| } |