Add extracted tools: CitrineOS, OpenOCPP, ShapeShifter
- CitrineOS core extracted (CSMS OCPP 2.0.1) - OpenOCPP extracted (firmware OCPP 1.6J/2.0.1) - ShapeShifter library installed (pip install -e) - ShapeShifter specification extracted - EVerest extracted TODO updated with progress
This commit is contained in:
135
tools/EVerest-main/lib/everest/io/src/CMakeLists.txt
Normal file
135
tools/EVerest-main/lib/everest/io/src/CMakeLists.txt
Normal file
@@ -0,0 +1,135 @@
|
||||
|
||||
add_library(everest_io SHARED)
|
||||
add_library(everest::io ALIAS everest_io)
|
||||
|
||||
set_target_properties(everest_io PROPERTIES
|
||||
VERSION 0.1.0
|
||||
SOVERSION 0
|
||||
)
|
||||
|
||||
target_sources(everest_io
|
||||
PRIVATE
|
||||
event/fd_event_handler.cpp
|
||||
event/unique_fd.cpp
|
||||
event/event_fd.cpp
|
||||
event/timer_fd.cpp
|
||||
can/socket_can_handler.cpp
|
||||
can/can_recv_filter.cpp
|
||||
can/can_payload.cpp
|
||||
socket/socket.cpp
|
||||
utilities/generic_error_state.cpp
|
||||
event/fd_event_client.cpp
|
||||
udp/udp_socket.cpp
|
||||
udp/udp_unconnected_socket.cpp
|
||||
udp/udp_dualstack_server_socket.cpp
|
||||
udp/endpoint.cpp
|
||||
udp/udp_payload.cpp
|
||||
utilities/stop_watch.cpp
|
||||
serial/serial.cpp
|
||||
serial/pty_handler.cpp
|
||||
serial/event_pty.cpp
|
||||
mdns/mdns.cpp
|
||||
mdns/mdns_socket.cpp
|
||||
tun_tap/tap_handler.cpp
|
||||
tcp/tcp_socket.cpp
|
||||
netlink/vcan_netlink_manager.cpp
|
||||
raw/raw_socket.cpp
|
||||
)
|
||||
|
||||
target_include_directories(everest_io
|
||||
PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/../include>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
)
|
||||
|
||||
if(EVEREST_IO_WITH_MQTT)
|
||||
target_sources(everest_io
|
||||
PRIVATE
|
||||
mqtt/mosquitto_cpp.cpp
|
||||
mqtt/mqtt_client.cpp
|
||||
)
|
||||
target_include_directories(everest_io
|
||||
PUBLIC
|
||||
$<BUILD_INTERFACE:${mosquitto_SOURCE_DIR}/include>
|
||||
)
|
||||
endif()
|
||||
|
||||
target_link_libraries(everest_io
|
||||
PUBLIC
|
||||
$<BUILD_INTERFACE:everest_util>
|
||||
$<INSTALL_INTERFACE:everest::util>
|
||||
)
|
||||
|
||||
include(CheckIncludeFile)
|
||||
check_include_file(net/ethernet.h HAVE_NET_ETHERNET_H)
|
||||
if(HAVE_NET_ETHERNET_H AND USING_MUSL)
|
||||
target_compile_definitions(everest_io PRIVATE _NETINET_IF_ETHER_H)
|
||||
endif()
|
||||
|
||||
if(EVEREST_NO_PACKET_IGNORE_OUTGOING)
|
||||
target_compile_definitions(everest_io PRIVATE EVEREST_NO_PACKET_IGNORE_OUTGOING)
|
||||
endif()
|
||||
|
||||
target_compile_features(everest_io PUBLIC cxx_std_17)
|
||||
|
||||
target_compile_options(everest_io
|
||||
PRIVATE
|
||||
-Wall
|
||||
-Wextra
|
||||
-Wshadow
|
||||
-Wnon-virtual-dtor
|
||||
-Wunused
|
||||
-Wpedantic
|
||||
-Wformat=2
|
||||
-Werror
|
||||
)
|
||||
|
||||
include(CMakePackageConfigHelpers)
|
||||
|
||||
set_target_properties(everest_io
|
||||
PROPERTIES
|
||||
EXPORT_NAME io
|
||||
POSITION_INDEPENDENT_CODE ON
|
||||
)
|
||||
|
||||
if(EVEREST_IO_WITH_MQTT)
|
||||
if(DISABLE_EDM)
|
||||
target_link_libraries(everest_io
|
||||
PRIVATE
|
||||
mosquitto
|
||||
)
|
||||
else()
|
||||
target_link_libraries(everest_io
|
||||
PRIVATE
|
||||
libmosquitto
|
||||
)
|
||||
install(TARGETS libmosquitto
|
||||
EXPORT everest-core-targets
|
||||
ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
|
||||
LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
|
||||
RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
|
||||
INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}
|
||||
)
|
||||
endif()
|
||||
endif()
|
||||
|
||||
install(TARGETS everest_io
|
||||
EXPORT everest-core-targets
|
||||
ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
|
||||
LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
|
||||
RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
|
||||
INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}
|
||||
)
|
||||
|
||||
if(EVEREST_IO_WITH_MQTT)
|
||||
install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/everest
|
||||
DESTINATION include
|
||||
FILES_MATCHING PATTERN "*.hpp"
|
||||
)
|
||||
else()
|
||||
install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/everest
|
||||
DESTINATION include
|
||||
FILES_MATCHING PATTERN "*.hpp"
|
||||
PATTERN "mqtt" EXCLUDE
|
||||
)
|
||||
endif()
|
||||
65
tools/EVerest-main/lib/everest/io/src/can/can_payload.cpp
Normal file
65
tools/EVerest-main/lib/everest/io/src/can/can_payload.cpp
Normal file
@@ -0,0 +1,65 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2025 Pionix GmbH and Contributors to EVerest
|
||||
|
||||
#include <everest/io/can/can_payload.hpp>
|
||||
#include <linux/can.h>
|
||||
|
||||
namespace everest::lib::io::can {
|
||||
|
||||
can_dataset::can_dataset(uint32_t can_id_, uint8_t len8_dlc_, can_payload const& payload_) {
|
||||
payload = payload_;
|
||||
len8_dlc = len8_dlc_;
|
||||
set_can_id(can_id_);
|
||||
}
|
||||
|
||||
uint32_t can_dataset::get_can_id() const {
|
||||
if (eff) {
|
||||
return can_id & CAN_EFF_MASK;
|
||||
} else {
|
||||
return can_id & CAN_SFF_MASK;
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t can_dataset::get_can_id_with_flags() const {
|
||||
auto tmp = get_can_id();
|
||||
if (eff) {
|
||||
tmp |= CAN_EFF_FLAG;
|
||||
}
|
||||
if (rtr) {
|
||||
tmp |= CAN_RTR_FLAG;
|
||||
}
|
||||
if (err) {
|
||||
tmp |= CAN_ERR_FLAG;
|
||||
}
|
||||
return tmp;
|
||||
}
|
||||
|
||||
void can_dataset::set_can_id(uint32_t id) {
|
||||
can_id = id;
|
||||
}
|
||||
|
||||
void can_dataset::set_can_id_with_flags(uint32_t id) {
|
||||
eff = id & CAN_EFF_FLAG;
|
||||
rtr = id & CAN_RTR_FLAG;
|
||||
err = id & CAN_ERR_FLAG;
|
||||
set_can_id(id);
|
||||
}
|
||||
|
||||
void can_dataset::set_can_id_with_flags(uint32_t id, bool eff_, bool rtr_, bool err_) {
|
||||
eff = eff_;
|
||||
rtr = rtr_;
|
||||
err = err_;
|
||||
set_can_id(id);
|
||||
}
|
||||
|
||||
bool can_dataset::eff_flag() const {
|
||||
return eff;
|
||||
}
|
||||
bool can_dataset::rtr_flag() const {
|
||||
return rtr;
|
||||
}
|
||||
bool can_dataset::err_flag() const {
|
||||
return err;
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::can
|
||||
@@ -0,0 +1,12 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2026 Pionix GmbH and Contributors to EVerest
|
||||
|
||||
#include <everest/io/can/can_recv_filter.hpp>
|
||||
|
||||
namespace everest::lib::io::can {
|
||||
|
||||
can_recv_filter can_recv_filter::reject_match(uint32_t id, uint32_t mask, bool extended) {
|
||||
return can_recv_filter{id, mask, extended, true};
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::can
|
||||
214
tools/EVerest-main/lib/everest/io/src/can/socket_can_handler.cpp
Normal file
214
tools/EVerest-main/lib/everest/io/src/can/socket_can_handler.cpp
Normal file
@@ -0,0 +1,214 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2026 Pionix GmbH and Contributors to EVerest
|
||||
|
||||
#include <algorithm>
|
||||
#include <everest/io/can/can_recv_filter.hpp>
|
||||
#include <everest/io/can/socket_can_handler.hpp>
|
||||
#include <everest/io/event/fd_event_handler.hpp>
|
||||
#include <everest/io/event/unique_fd.hpp>
|
||||
#include <everest/io/socket/socket.hpp>
|
||||
#include <linux/can.h>
|
||||
#include <linux/can/raw.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/types.h>
|
||||
#include <unistd.h>
|
||||
|
||||
// has to be after sys/types.h for musl
|
||||
#include <net/if.h>
|
||||
|
||||
namespace everest::lib::io::can {
|
||||
|
||||
namespace {
|
||||
|
||||
canid_t to_linux_can_id(can_recv_filter const& filter) {
|
||||
auto id = filter.can_id & CAN_EFF_MASK;
|
||||
if (filter.extended) {
|
||||
id |= CAN_EFF_FLAG;
|
||||
}
|
||||
if (filter.invert) {
|
||||
id |= CAN_INV_FILTER;
|
||||
}
|
||||
return id;
|
||||
}
|
||||
|
||||
canid_t to_linux_can_mask(can_recv_filter const& filter) {
|
||||
auto mask = filter.can_mask & CAN_EFF_MASK;
|
||||
if (filter.extended) {
|
||||
mask |= CAN_EFF_FLAG;
|
||||
}
|
||||
return mask;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
bool socket_can_handler::data_valid(can_payload const& payload) {
|
||||
return payload.size() <= CAN_MAX_DLEN;
|
||||
}
|
||||
|
||||
int socket_can_handler::tx(uint32_t can_id, uint8_t len8_dlc, can_payload const& payload) {
|
||||
if (not is_open()) {
|
||||
return ENETDOWN;
|
||||
}
|
||||
if (not data_valid(payload)) {
|
||||
return EINVAL;
|
||||
}
|
||||
struct can_frame frame;
|
||||
|
||||
frame.can_id = can_id;
|
||||
auto const max_dlc_value = 15;
|
||||
frame.len8_dlc = std::min<uint8_t>(len8_dlc, max_dlc_value);
|
||||
frame.len = std::min<uint8_t>(CAN_MAX_DLEN, payload.size());
|
||||
memcpy(frame.data, payload.data(), frame.len);
|
||||
|
||||
auto bytes_written = write(m_owned_can_fd, &frame, sizeof(can_frame));
|
||||
if (bytes_written != sizeof(can_frame)) {
|
||||
if (bytes_written == -1) {
|
||||
return errno;
|
||||
} else {
|
||||
return EAGAIN;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int socket_can_handler::rx(uint32_t& can_id, uint8_t& len8_dlc, can_payload& payload) {
|
||||
if (is_open()) {
|
||||
static auto const framesize = sizeof(struct can_frame);
|
||||
can_frame frame;
|
||||
auto nbytes = read(m_owned_can_fd, &frame, framesize);
|
||||
|
||||
if (nbytes == framesize) {
|
||||
payload.clear();
|
||||
payload.assign(frame.data, frame.data + frame.can_dlc);
|
||||
can_id = frame.can_id;
|
||||
len8_dlc = frame.len8_dlc;
|
||||
return 0;
|
||||
} else if (nbytes == -1) {
|
||||
return errno;
|
||||
} else {
|
||||
return EINVAL;
|
||||
}
|
||||
}
|
||||
return ENETDOWN;
|
||||
}
|
||||
|
||||
bool socket_can_handler::tx(can_dataset const& data) {
|
||||
auto can_id = data.get_can_id_with_flags();
|
||||
auto status = tx(can_id, data.len8_dlc, data.payload);
|
||||
return status == 0;
|
||||
}
|
||||
|
||||
bool socket_can_handler::rx(can_dataset& data) {
|
||||
uint32_t can_id = 0;
|
||||
auto status = rx(can_id, data.len8_dlc, data.payload);
|
||||
if (status == 0)
|
||||
data.set_can_id_with_flags(can_id);
|
||||
return status == 0;
|
||||
}
|
||||
|
||||
bool socket_can_handler::open(std::string const& can_device, std::vector<can_recv_filter> const& recv_filters) {
|
||||
// IFNAMSIZ is the size of the buffer to write the name to.
|
||||
// This situation is special concerning null termination,
|
||||
// The name can occupy the fill buffer. If it does not, nulltermination is necessary
|
||||
// Since most Linux systems enforce 15 chars as limit plus 1 for nulltermination
|
||||
// we do the same thing here.
|
||||
if (can_device.size() >= IFNAMSIZ) {
|
||||
return false;
|
||||
}
|
||||
m_recv_filters = recv_filters;
|
||||
m_can_dev = can_device;
|
||||
return open_device() == 0;
|
||||
}
|
||||
|
||||
bool socket_can_handler::set_recv_filters(std::vector<can_recv_filter> const& recv_filters) {
|
||||
m_recv_filters = recv_filters;
|
||||
if (!is_open()) {
|
||||
return true;
|
||||
}
|
||||
return apply_recv_filters();
|
||||
}
|
||||
|
||||
std::vector<can_recv_filter> const& socket_can_handler::get_recv_filters() const {
|
||||
return m_recv_filters;
|
||||
}
|
||||
|
||||
bool socket_can_handler::apply_recv_filters() {
|
||||
if (!is_open()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (m_recv_filters.empty()) {
|
||||
can_filter accept_all{};
|
||||
return ::setsockopt(m_owned_can_fd, SOL_CAN_RAW, CAN_RAW_FILTER, &accept_all, sizeof(accept_all)) == 0;
|
||||
}
|
||||
|
||||
std::vector<can_filter> linux_filters;
|
||||
linux_filters.reserve(m_recv_filters.size());
|
||||
for (auto const& filter : m_recv_filters) {
|
||||
linux_filters.push_back({to_linux_can_id(filter), to_linux_can_mask(filter)});
|
||||
}
|
||||
|
||||
return ::setsockopt(m_owned_can_fd, SOL_CAN_RAW, CAN_RAW_FILTER, linux_filters.data(),
|
||||
linux_filters.size() * sizeof(can_filter)) == 0;
|
||||
}
|
||||
|
||||
int socket_can_handler::open_device() {
|
||||
auto can_fd = event::unique_fd(::socket(PF_CAN, SOCK_RAW, CAN_RAW));
|
||||
if (can_fd < 0) {
|
||||
perror("Socket");
|
||||
return errno;
|
||||
}
|
||||
struct ifreq ifr;
|
||||
memset(&ifr, 0, sizeof(ifr));
|
||||
// We know m_can_dev fits because of the check in open().
|
||||
// strncpy will copy the string and the null terminator.
|
||||
// The previous memset handles any trailing bytes in the 16-byte buffer.
|
||||
strncpy(ifr.ifr_name, m_can_dev.c_str(), IFNAMSIZ);
|
||||
if (ioctl(can_fd, SIOCGIFINDEX, &ifr) < 0) {
|
||||
perror(m_can_dev.c_str());
|
||||
return errno;
|
||||
}
|
||||
|
||||
struct sockaddr_can addr;
|
||||
memset(&addr, 0, sizeof(addr));
|
||||
addr.can_family = AF_CAN;
|
||||
addr.can_ifindex = ifr.ifr_ifindex;
|
||||
|
||||
if (bind(can_fd, (struct sockaddr*)&addr, sizeof(addr)) < 0) {
|
||||
perror("Bind");
|
||||
return errno;
|
||||
}
|
||||
|
||||
socket::set_non_blocking(can_fd);
|
||||
socket::set_socket_send_buffer_to_min(can_fd);
|
||||
m_owned_can_fd = std::move(can_fd);
|
||||
|
||||
if (!apply_recv_filters()) {
|
||||
m_owned_can_fd.close();
|
||||
return errno != 0 ? errno : EINVAL;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool socket_can_handler::is_open() const {
|
||||
return m_owned_can_fd.is_fd();
|
||||
}
|
||||
|
||||
void socket_can_handler::close() {
|
||||
m_owned_can_fd.close();
|
||||
}
|
||||
|
||||
int socket_can_handler::get_fd() const {
|
||||
return m_owned_can_fd;
|
||||
}
|
||||
|
||||
int socket_can_handler::get_error() const {
|
||||
return socket::get_pending_error(m_owned_can_fd);
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::can
|
||||
51
tools/EVerest-main/lib/everest/io/src/event/event_fd.cpp
Normal file
51
tools/EVerest-main/lib/everest/io/src/event/event_fd.cpp
Normal file
@@ -0,0 +1,51 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2025 Pionix GmbH and Contributors to EVerest
|
||||
|
||||
#include <everest/io/event/event_fd.hpp>
|
||||
#include <optional>
|
||||
#include <stdexcept>
|
||||
#include <sys/eventfd.h>
|
||||
|
||||
namespace everest::lib::io::event {
|
||||
|
||||
event_fd_base::event_fd_base(unsigned int initval, int flags) : m_fd(::eventfd(initval, flags)) {
|
||||
if (m_fd == -1) {
|
||||
throw std::runtime_error("failed to create an eventfd");
|
||||
}
|
||||
}
|
||||
|
||||
event_fd_base::operator int() const {
|
||||
return get_raw_fd();
|
||||
}
|
||||
|
||||
int event_fd_base::get_raw_fd() const {
|
||||
return m_fd;
|
||||
}
|
||||
|
||||
bool event_fd_base::valid() const {
|
||||
return m_fd != unique_fd::NO_DESCRIPTOR_SENTINEL;
|
||||
}
|
||||
|
||||
std::optional<uint64_t> event_fd_base::read() {
|
||||
eventfd_t eventfd_buffer{0};
|
||||
if (eventfd_read(m_fd, &eventfd_buffer) == 0) {
|
||||
return eventfd_buffer;
|
||||
}
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
bool event_fd_base::write(std::uint64_t data) {
|
||||
return eventfd_write(m_fd, data) == 0;
|
||||
}
|
||||
|
||||
bool event_fd_base::notify() {
|
||||
return write(1);
|
||||
}
|
||||
|
||||
event_fd::event_fd() : event_fd_base(0, 0) {
|
||||
}
|
||||
|
||||
semaphore_fd::semaphore_fd() : event_fd_base(0, EFD_SEMAPHORE) {
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::event
|
||||
164
tools/EVerest-main/lib/everest/io/src/event/fd_event_client.cpp
Normal file
164
tools/EVerest-main/lib/everest/io/src/event/fd_event_client.cpp
Normal file
@@ -0,0 +1,164 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2025 Pionix GmbH and Contributors to EVerest
|
||||
|
||||
#include <everest/io/event/fd_event_client.hpp>
|
||||
#include <everest/io/event/fd_event_handler.hpp>
|
||||
|
||||
namespace everest::lib::io::event {
|
||||
|
||||
generic_fd_event_client_impl::generic_fd_event_client_impl(action const& send_one, action const& receive_one,
|
||||
action const& reset_client, error_status const& get_error) :
|
||||
m_send_one(send_one), m_receive_one(receive_one), m_reset_client(reset_client), m_get_error(get_error) {
|
||||
m_event_handler = std::make_unique<event::fd_event_handler>();
|
||||
}
|
||||
|
||||
generic_fd_event_client_impl::~generic_fd_event_client_impl() = default;
|
||||
|
||||
int generic_fd_event_client_impl::get_poll_fd() {
|
||||
return m_event_handler->get_poll_fd();
|
||||
}
|
||||
|
||||
sync_status generic_fd_event_client_impl::sync() {
|
||||
return sync_impl(-1);
|
||||
}
|
||||
|
||||
sync_status generic_fd_event_client_impl::sync_impl(int timeout_ms) {
|
||||
auto result = m_event_handler->poll(std::chrono::milliseconds(timeout_ms));
|
||||
m_event_handler->run_actions();
|
||||
|
||||
// The error handler must be called after all event handlers have run.
|
||||
// Removing handlers during error processing is likely to result in
|
||||
// inconsistent state and and segmentations fault.
|
||||
return result ? sync_status::ok : sync_status::timeout;
|
||||
}
|
||||
|
||||
bool generic_fd_event_client_impl::setup_error_event_handler() {
|
||||
return m_event_handler->register_event_handler(&m_error_status_event_fd, [this](auto) {
|
||||
if (on_error()) {
|
||||
error_handler();
|
||||
call_error_handler(m_error);
|
||||
return sync_status::error;
|
||||
} else if (clear_error_pending()) {
|
||||
clear_error_handler(m_error);
|
||||
}
|
||||
return sync_status::ok;
|
||||
});
|
||||
}
|
||||
|
||||
void generic_fd_event_client_impl::setup_io_event_handler(int fd) {
|
||||
using namespace everest::lib::io::event;
|
||||
m_event_handler->register_event_handler(
|
||||
fd,
|
||||
[this, fd](auto events) {
|
||||
auto success = true;
|
||||
if (events.count(poll_events::error)) {
|
||||
auto error = m_get_error();
|
||||
if (error) {
|
||||
set_error_status_and_notify(error);
|
||||
}
|
||||
success = false;
|
||||
}
|
||||
if (events.count(poll_events::hungup)) {
|
||||
success = false;
|
||||
}
|
||||
if (success && events.count(poll_events::read)) {
|
||||
success = rx_handler();
|
||||
}
|
||||
if (success && events.count(poll_events::write)) {
|
||||
success = tx_handler(fd);
|
||||
}
|
||||
},
|
||||
poll_events::read);
|
||||
m_event_handler->register_event_handler(&m_io_event_fd, [this, fd](auto) {
|
||||
m_event_handler->modify_event_handler(fd, poll_events::write, event_modification::add);
|
||||
});
|
||||
}
|
||||
|
||||
void generic_fd_event_client_impl::set_error_handler(cb_error const& handler) {
|
||||
add_action([this, handler]() { m_error = handler; });
|
||||
}
|
||||
|
||||
bool generic_fd_event_client_impl::unregister_source(int fd) {
|
||||
return m_event_handler->remove_event_handler(fd);
|
||||
}
|
||||
|
||||
bool generic_fd_event_client_impl::set_error_status_and_notify(int error_code) {
|
||||
auto result = set_error_status(error_code);
|
||||
m_error_status_event_fd.notify();
|
||||
return result;
|
||||
}
|
||||
|
||||
bool generic_fd_event_client_impl::rx_handler() {
|
||||
auto status = m_receive_one();
|
||||
auto error_code = status == action_status::success ? 0 : m_get_error();
|
||||
auto result = set_error_status_and_notify(error_code);
|
||||
return result;
|
||||
}
|
||||
|
||||
bool generic_fd_event_client_impl::tx_handler(int fd) {
|
||||
// We send one message only, even if more data is queued.
|
||||
// This prevents the kernel buffer from filling up
|
||||
auto status = m_send_one();
|
||||
switch (status) {
|
||||
case action_status::empty: {
|
||||
// if there are no more message we no longer listen to writeable events
|
||||
// otherwise we wait for the socket to become writeable again.
|
||||
m_event_handler->modify_event_handler(fd, event::poll_events::write, event::event_modification::remove);
|
||||
return true;
|
||||
}
|
||||
case action_status::fail: {
|
||||
auto error_code = m_get_error();
|
||||
set_error_status_and_notify(error_code);
|
||||
return false;
|
||||
}
|
||||
case action_status::success: {
|
||||
set_error_status_and_notify(0);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void generic_fd_event_client_impl::error_handler() {
|
||||
add_action([this]() { m_reset_client(); });
|
||||
}
|
||||
|
||||
void generic_fd_event_client_impl::prepare_io_event_handler() {
|
||||
m_event_handler->register_event_handler(&m_connected_event_fd, [this](auto) {
|
||||
auto client_status = m_client_status.handle();
|
||||
if (client_status->ok) {
|
||||
auto error_code = m_get_error();
|
||||
set_error_status_and_notify(error_code);
|
||||
setup_io_event_handler(client_status->fd);
|
||||
if (m_on_ready_action) {
|
||||
m_event_handler->add_action(m_on_ready_action);
|
||||
}
|
||||
} else {
|
||||
auto error_code = m_get_error();
|
||||
set_error_status_and_notify(error_code);
|
||||
}
|
||||
|
||||
return sync_status::ok;
|
||||
});
|
||||
}
|
||||
|
||||
void generic_fd_event_client_impl::on_client_ready(bool ok, int fd) {
|
||||
auto client_status = m_client_status.handle();
|
||||
client_status->ok = ok;
|
||||
client_status->fd = fd;
|
||||
m_connected_event_fd.notify();
|
||||
}
|
||||
|
||||
void generic_fd_event_client_impl::add_action(fd_event_handler::task&& item) {
|
||||
m_event_handler->add_action(std::forward<fd_event_handler::task>(item));
|
||||
}
|
||||
|
||||
void generic_fd_event_client_impl::set_on_ready_action(ready_action&& item) {
|
||||
m_on_ready_action = std::move(item);
|
||||
auto client_status = m_client_status.handle();
|
||||
if (client_status->ok) {
|
||||
m_event_handler->add_action(m_on_ready_action);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::event
|
||||
379
tools/EVerest-main/lib/everest/io/src/event/fd_event_handler.cpp
Normal file
379
tools/EVerest-main/lib/everest/io/src/event/fd_event_handler.cpp
Normal file
@@ -0,0 +1,379 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2025 Pionix GmbH and Contributors to EVerest
|
||||
|
||||
#include "everest/io/event/fd_event_sync_interface.hpp"
|
||||
#include <everest/io/event/event_fd.hpp>
|
||||
#include <everest/io/event/fd_event_client.hpp>
|
||||
#include <everest/io/event/fd_event_handler.hpp>
|
||||
#include <everest/io/event/timer_fd.hpp>
|
||||
#include <everest/io/event/unique_fd.hpp>
|
||||
|
||||
#include <cstdio>
|
||||
#include <fcntl.h>
|
||||
#include <map>
|
||||
#include <vector>
|
||||
|
||||
#include <poll.h>
|
||||
#include <sys/epoll.h>
|
||||
|
||||
namespace everest::lib::io::event {
|
||||
|
||||
namespace {
|
||||
uint32_t poll_event_to_bitmask(poll_events e) {
|
||||
switch (e) {
|
||||
case poll_events::read:
|
||||
return EPOLLIN;
|
||||
case poll_events::priority:
|
||||
return EPOLLPRI;
|
||||
case poll_events::write:
|
||||
return EPOLLOUT;
|
||||
case poll_events::error:
|
||||
return EPOLLERR;
|
||||
case poll_events::hungup:
|
||||
return EPOLLHUP;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
std::set<poll_events> bitmask_to_poll_events(uint32_t bitmask) {
|
||||
std::set<poll_events> result;
|
||||
if (bitmask & EPOLLIN) {
|
||||
result.insert(poll_events::read);
|
||||
}
|
||||
if (bitmask & EPOLLPRI) {
|
||||
result.insert(poll_events::priority);
|
||||
}
|
||||
if (bitmask & EPOLLOUT) {
|
||||
result.insert(poll_events::write);
|
||||
}
|
||||
if (bitmask & EPOLLERR) {
|
||||
result.insert(poll_events::error);
|
||||
}
|
||||
if (bitmask & EPOLLHUP) {
|
||||
result.insert(poll_events::hungup);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
uint32_t sum_events(std::set<poll_events> const& events) {
|
||||
uint32_t result = 0;
|
||||
for (auto e : events) {
|
||||
result = result | poll_event_to_bitmask(e);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
} // namespace
|
||||
|
||||
std::set<poll_events> operator|(poll_events lhs, poll_events rhs) {
|
||||
return {lhs, rhs};
|
||||
}
|
||||
|
||||
std::set<poll_events>& operator|(std::set<poll_events>& lhs, poll_events rhs) {
|
||||
lhs.insert(rhs);
|
||||
return lhs;
|
||||
}
|
||||
|
||||
bool operator&(std::set<poll_events> const& lhs, poll_events rhs) {
|
||||
return lhs.count(rhs) == 1;
|
||||
}
|
||||
|
||||
class EventHandlerMap {
|
||||
public:
|
||||
EventHandlerMap() : m_epoll_fd(epoll_create1(0)) {
|
||||
if (not m_epoll_fd.is_fd()) {
|
||||
::perror("epoll_create");
|
||||
}
|
||||
}
|
||||
|
||||
bool add(int fd, fd_event_handler::event_handler_type handler, fd_event_handler::event_list const& events) {
|
||||
epoll_event event;
|
||||
event.events = sum_events(events);
|
||||
event.data.fd = fd;
|
||||
auto result = epoll_ctl(m_epoll_fd, EPOLL_CTL_ADD, fd, &event) == 0;
|
||||
if (result) {
|
||||
m_event_map[fd] = {std::move(handler), event};
|
||||
m_pollfds.resize(m_pollfds.size() + 1);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
bool remove(int fd) {
|
||||
auto epoll_result = epoll_ctl(m_epoll_fd, EPOLL_CTL_DEL, fd, nullptr);
|
||||
auto handler_result = m_event_map.count(fd);
|
||||
if (handler_result) {
|
||||
m_event_map.erase(fd);
|
||||
m_pollfds.resize(m_pollfds.size() - 1);
|
||||
}
|
||||
return epoll_result or handler_result;
|
||||
}
|
||||
bool modify_remove(int fd, fd_event_handler::event_list const& events) {
|
||||
auto action = [](uint32_t current, fd_event_handler::event_list const& change) {
|
||||
auto raw_change = sum_events(change);
|
||||
auto result = current & (~raw_change);
|
||||
return result;
|
||||
};
|
||||
return modify(fd, events, action);
|
||||
}
|
||||
|
||||
bool modify_add(int fd, fd_event_handler::event_list const& events) {
|
||||
auto action = [](uint32_t current, fd_event_handler::event_list const& change) {
|
||||
auto raw_change = sum_events(change);
|
||||
auto result = current | raw_change;
|
||||
return result;
|
||||
};
|
||||
return modify(fd, events, action);
|
||||
}
|
||||
|
||||
bool modify_replace(int fd, fd_event_handler::event_list const& events) {
|
||||
auto action = [](uint32_t, fd_event_handler::event_list const& change) {
|
||||
auto raw_change = sum_events(change);
|
||||
auto result = raw_change;
|
||||
return result;
|
||||
};
|
||||
return modify(fd, events, action);
|
||||
}
|
||||
|
||||
const auto& get(int fd) const {
|
||||
return std::get<fd_event_handler::event_handler_type>(m_event_map.at(fd));
|
||||
}
|
||||
|
||||
bool exists(int fd) const {
|
||||
return m_event_map.count(fd);
|
||||
}
|
||||
|
||||
auto& get_pollfds() {
|
||||
return m_pollfds;
|
||||
}
|
||||
|
||||
int get_epoll_fd() {
|
||||
return static_cast<int>(m_epoll_fd);
|
||||
}
|
||||
|
||||
private:
|
||||
using event_state = std::tuple<fd_event_handler::event_handler_type, epoll_event>;
|
||||
bool modify(int fd, fd_event_handler::event_list const& events,
|
||||
std::function<uint32_t(uint32_t, fd_event_handler::event_list const&)> const& event_action) {
|
||||
auto result = false;
|
||||
if (m_event_map.count(fd)) {
|
||||
auto& event = std::get<epoll_event>(m_event_map.at(fd));
|
||||
auto backup = event.events;
|
||||
event.events = event_action(backup, events);
|
||||
result = epoll_ctl(m_epoll_fd, EPOLL_CTL_MOD, fd, &event) == 0;
|
||||
if (not result) {
|
||||
event.events = backup;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
std::vector<epoll_event> m_pollfds;
|
||||
std::map<int, event_state> m_event_map;
|
||||
unique_fd m_epoll_fd;
|
||||
};
|
||||
|
||||
fd_event_handler::~fd_event_handler() = default;
|
||||
|
||||
fd_event_handler::fd_event_handler() {
|
||||
m_handlers = std::make_unique<EventHandlerMap>();
|
||||
register_event_handler(&m_action_event, [](auto&&) {});
|
||||
}
|
||||
|
||||
bool fd_event_handler::register_event_handler(int fd, event_handler_type const& handler, event_list const& events) {
|
||||
if (fd == -1 or not handler or m_handlers->exists(fd)) {
|
||||
return false;
|
||||
}
|
||||
m_handlers->add(fd, handler, events);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool fd_event_handler::register_event_handler(int fd, event_handler_type const& handler, poll_events event) {
|
||||
return register_event_handler(fd, handler, event_list{event});
|
||||
}
|
||||
|
||||
bool fd_event_handler::register_event_handler(event_fd* fd, event_handler_type const& handler) {
|
||||
if (not fd) {
|
||||
return false;
|
||||
}
|
||||
auto raw = fd->get_raw_fd();
|
||||
return register_event_handler(
|
||||
raw,
|
||||
[handler, fd](event_list const& e) {
|
||||
fd->read();
|
||||
handler(e);
|
||||
},
|
||||
poll_events::read);
|
||||
}
|
||||
|
||||
bool fd_event_handler::register_event_handler(event_fd* fd, event_handler_simple_type const& handler) {
|
||||
return register_event_handler(fd, [handler](event_list const&) { handler(); });
|
||||
}
|
||||
|
||||
bool fd_event_handler::register_event_handler(timer_fd* fd, event_handler_type const& handler) {
|
||||
if (not fd) {
|
||||
return false;
|
||||
}
|
||||
auto raw = fd->get_raw_fd();
|
||||
return register_event_handler(
|
||||
raw,
|
||||
[handler, fd](event_list const& e) {
|
||||
fd->read();
|
||||
handler(e);
|
||||
},
|
||||
poll_events::read);
|
||||
}
|
||||
|
||||
bool fd_event_handler::register_event_handler(timer_fd* fd, event_handler_simple_type const& handler) {
|
||||
return register_event_handler(fd, [handler](event_list const&) { handler(); });
|
||||
}
|
||||
|
||||
bool fd_event_handler::register_event_handler(fd_event_sync_interface* obj) {
|
||||
if (not obj) {
|
||||
return false;
|
||||
}
|
||||
auto raw = obj->get_poll_fd();
|
||||
return register_event_handler(
|
||||
raw, [obj](event_list const&) { obj->sync(); }, poll_events::read);
|
||||
}
|
||||
|
||||
bool fd_event_handler::register_event_handler(fd_event_register_interface* obj) {
|
||||
if (not obj) {
|
||||
return false;
|
||||
}
|
||||
return obj->register_events(*this);
|
||||
}
|
||||
|
||||
bool fd_event_handler::register_event_handler(fd_event_handler* obj) {
|
||||
if (not obj or obj == this) {
|
||||
return false;
|
||||
}
|
||||
auto raw = obj->get_poll_fd();
|
||||
return register_event_handler(
|
||||
raw,
|
||||
[obj](event_list const&) {
|
||||
obj->poll();
|
||||
obj->run_actions();
|
||||
},
|
||||
poll_events::read);
|
||||
}
|
||||
|
||||
bool fd_event_handler::unregister_event_handler(fd_event_register_interface* obj) {
|
||||
if (not obj) {
|
||||
return false;
|
||||
}
|
||||
return obj->unregister_events(*this);
|
||||
}
|
||||
|
||||
bool fd_event_handler::unregister_event_handler(fd_event_sync_interface* obj) {
|
||||
if (not obj) {
|
||||
return false;
|
||||
}
|
||||
return remove_event_handler(obj->get_poll_fd());
|
||||
}
|
||||
|
||||
bool fd_event_handler::unregister_event_handler(timer_fd* obj) {
|
||||
if (not obj) {
|
||||
return false;
|
||||
}
|
||||
return remove_event_handler(obj->get_raw_fd());
|
||||
}
|
||||
|
||||
bool fd_event_handler::unregister_event_handler(event_fd* obj) {
|
||||
if (not obj) {
|
||||
return false;
|
||||
}
|
||||
return remove_event_handler(obj->get_raw_fd());
|
||||
}
|
||||
|
||||
bool fd_event_handler::unregister_event_handler(int fd) {
|
||||
if (fd == -1) {
|
||||
return false;
|
||||
}
|
||||
return remove_event_handler(fd);
|
||||
}
|
||||
|
||||
bool fd_event_handler::modify_event_handler(int fd, event_list const& events, event_modification change) {
|
||||
if (fd == -1) {
|
||||
return false;
|
||||
}
|
||||
switch (change) {
|
||||
case event_modification::add:
|
||||
return m_handlers->modify_add(fd, events);
|
||||
case event_modification::remove:
|
||||
return m_handlers->modify_remove(fd, events);
|
||||
case event_modification::replace:
|
||||
return m_handlers->modify_replace(fd, events);
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool fd_event_handler::modify_event_handler(int fd, poll_events event, event_modification change) {
|
||||
return modify_event_handler(fd, event_list{event}, change);
|
||||
}
|
||||
|
||||
bool fd_event_handler::remove_event_handler(int fd) {
|
||||
if (fd == -1) {
|
||||
return false;
|
||||
}
|
||||
return m_handlers->remove(fd);
|
||||
}
|
||||
|
||||
void fd_event_handler::poll() {
|
||||
poll_impl(-1);
|
||||
}
|
||||
|
||||
bool fd_event_handler::poll_impl(int timeout_ms) {
|
||||
auto& pollfds = m_handlers->get_pollfds();
|
||||
auto status = ::epoll_wait(m_handlers->get_epoll_fd(), pollfds.data(), pollfds.size(), timeout_ms);
|
||||
|
||||
if (status > 0) {
|
||||
for (int i = 0; i < status; ++i) {
|
||||
auto& item = pollfds[i];
|
||||
m_handlers->get(item.data.fd)(bitmask_to_poll_events(item.events));
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
int fd_event_handler::get_poll_fd() {
|
||||
return m_handlers->get_epoll_fd();
|
||||
}
|
||||
|
||||
void fd_event_handler::add_action(task&& item) {
|
||||
task_pool.push(std::forward<task>(item));
|
||||
m_action_event.notify();
|
||||
}
|
||||
|
||||
void fd_event_handler::add_action(task const& item) {
|
||||
task_pool.push(std::move(item));
|
||||
m_action_event.notify();
|
||||
}
|
||||
|
||||
void fd_event_handler::run_actions() {
|
||||
while (true) {
|
||||
auto item = task_pool.try_pop();
|
||||
if (item.has_value()) {
|
||||
try {
|
||||
item.value()();
|
||||
} catch (...) {
|
||||
}
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void fd_event_handler::run_once() {
|
||||
poll();
|
||||
run_actions();
|
||||
}
|
||||
|
||||
void fd_event_handler::run(std::atomic_bool& online) {
|
||||
while (online.load()) {
|
||||
poll();
|
||||
run_actions();
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::event
|
||||
75
tools/EVerest-main/lib/everest/io/src/event/timer_fd.cpp
Normal file
75
tools/EVerest-main/lib/everest/io/src/event/timer_fd.cpp
Normal file
@@ -0,0 +1,75 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2025 Pionix GmbH and Contributors to EVerest
|
||||
|
||||
#include <ctime>
|
||||
#include <everest/io/event/timer_fd.hpp>
|
||||
#include <stdexcept>
|
||||
#include <sys/timerfd.h>
|
||||
#include <unistd.h>
|
||||
|
||||
namespace everest::lib::io::event {
|
||||
|
||||
timer_fd::timer_fd() : m_fd(::timerfd_create(CLOCK_MONOTONIC, TFD_NONBLOCK)) {
|
||||
if (m_fd == -1) {
|
||||
throw std::runtime_error("failed to create an timerfd");
|
||||
}
|
||||
}
|
||||
|
||||
timer_fd::operator int() const {
|
||||
return get_raw_fd();
|
||||
}
|
||||
|
||||
int timer_fd::get_raw_fd() const {
|
||||
return m_fd;
|
||||
}
|
||||
|
||||
bool timer_fd::valid() const {
|
||||
return m_fd != unique_fd::NO_DESCRIPTOR_SENTINEL;
|
||||
}
|
||||
|
||||
int timer_fd::read() {
|
||||
uint64_t buffer;
|
||||
return ::read(m_fd, &buffer, sizeof(buffer));
|
||||
}
|
||||
|
||||
bool timer_fd::reset() {
|
||||
return set_timeout_ns(m_to_ns);
|
||||
}
|
||||
|
||||
void timer_fd::set_single_shot(bool on) {
|
||||
m_single_shot = on;
|
||||
}
|
||||
|
||||
bool timer_fd::disarm() {
|
||||
struct itimerspec timer {};
|
||||
return ::timerfd_settime(m_fd, 0, &timer, nullptr) == 0;
|
||||
}
|
||||
|
||||
bool timer_fd::set_timeout_ms(long long to) {
|
||||
return set_timeout_ns(1000 * 1000 * to);
|
||||
}
|
||||
|
||||
bool timer_fd::set_timeout_us(long long to) {
|
||||
return set_timeout_ns(1000 * to);
|
||||
}
|
||||
|
||||
bool timer_fd::set_timeout_ns(long long to) {
|
||||
m_to_ns = to;
|
||||
struct itimerspec timer {};
|
||||
auto const sec = to / 1000000000LL;
|
||||
auto const nano = to % 1000000000LL;
|
||||
|
||||
timer.it_value.tv_sec = sec;
|
||||
timer.it_value.tv_nsec = nano;
|
||||
if (m_single_shot) {
|
||||
timer.it_interval.tv_sec = 0;
|
||||
timer.it_interval.tv_nsec = 0;
|
||||
} else {
|
||||
timer.it_interval.tv_sec = sec;
|
||||
timer.it_interval.tv_nsec = nano;
|
||||
}
|
||||
|
||||
return ::timerfd_settime(m_fd, 0, &timer, nullptr) == 0;
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::event
|
||||
53
tools/EVerest-main/lib/everest/io/src/event/unique_fd.cpp
Normal file
53
tools/EVerest-main/lib/everest/io/src/event/unique_fd.cpp
Normal file
@@ -0,0 +1,53 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2025 Pionix GmbH and Contributors to EVerest
|
||||
|
||||
#include <everest/io/event/unique_fd.hpp>
|
||||
#include <unistd.h>
|
||||
#include <utility>
|
||||
|
||||
namespace everest::lib::io::event {
|
||||
|
||||
namespace {
|
||||
void close_descriptor_if_valid(int fd) {
|
||||
if (fd != unique_fd::NO_DESCRIPTOR_SENTINEL) {
|
||||
// NOTE (aw): according to the close(2) man page, close might return an error but it should not be retried
|
||||
::close(fd);
|
||||
}
|
||||
}
|
||||
} // namespace
|
||||
|
||||
unique_fd::unique_fd(int fd) : m_fd(fd){};
|
||||
|
||||
unique_fd::operator int() const {
|
||||
return m_fd;
|
||||
}
|
||||
|
||||
bool unique_fd::is_fd() const {
|
||||
return m_fd != NO_DESCRIPTOR_SENTINEL;
|
||||
}
|
||||
|
||||
int unique_fd::release() {
|
||||
return std::exchange(m_fd, NO_DESCRIPTOR_SENTINEL);
|
||||
}
|
||||
|
||||
void unique_fd::close() {
|
||||
close_descriptor_if_valid(m_fd);
|
||||
std::exchange(m_fd, NO_DESCRIPTOR_SENTINEL);
|
||||
}
|
||||
|
||||
unique_fd::unique_fd(unique_fd&& other) : m_fd(std::exchange(other.m_fd, NO_DESCRIPTOR_SENTINEL)) {
|
||||
}
|
||||
|
||||
unique_fd& unique_fd::operator=(unique_fd&& other) {
|
||||
if (this != &other) {
|
||||
close_descriptor_if_valid(m_fd);
|
||||
m_fd = std::exchange(other.m_fd, NO_DESCRIPTOR_SENTINEL);
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
unique_fd::~unique_fd() {
|
||||
close_descriptor_if_valid(m_fd);
|
||||
}
|
||||
} // namespace everest::lib::io::event
|
||||
377
tools/EVerest-main/lib/everest/io/src/mdns/mdns.cpp
Normal file
377
tools/EVerest-main/lib/everest/io/src/mdns/mdns.cpp
Normal file
@@ -0,0 +1,377 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2026 Pionix GmbH and Contributors to EVerest
|
||||
|
||||
#include "everest/io/mdns/mdns.hpp"
|
||||
#include <arpa/inet.h>
|
||||
#include <cstring>
|
||||
#include <iostream>
|
||||
namespace everest::lib::io::mdns {
|
||||
|
||||
namespace {
|
||||
|
||||
std::string parse_name(const std::uint8_t* buffer, int size, int* offset) {
|
||||
std::string name = "";
|
||||
int curr = *offset;
|
||||
bool moved = false;
|
||||
int next_offset = -1;
|
||||
|
||||
while (curr < size && buffer[curr] != 0) {
|
||||
if ((buffer[curr] & 0xC0) == 0xC0) {
|
||||
int pointer_offset = ((buffer[curr] & 0x3F) << 8) | buffer[curr + 1];
|
||||
if (!moved) {
|
||||
next_offset = curr + 2;
|
||||
}
|
||||
curr = pointer_offset;
|
||||
moved = true;
|
||||
} else {
|
||||
int len = buffer[curr];
|
||||
curr++;
|
||||
for (int i = 0; i < len && curr < size; ++i) {
|
||||
name += static_cast<char>(buffer[curr]);
|
||||
curr++;
|
||||
}
|
||||
if (curr < size && buffer[curr] != 0) {
|
||||
name += ".";
|
||||
}
|
||||
}
|
||||
}
|
||||
*offset = moved ? next_offset : curr + 1;
|
||||
return name;
|
||||
}
|
||||
|
||||
void parse_mdns_A(const std::uint8_t* buffer, mDNS_discovery& mdns) {
|
||||
auto size_of_ip_string = 16;
|
||||
mdns.ip.resize(size_of_ip_string);
|
||||
std::snprintf(mdns.ip.data(), mdns.ip.size(), "%d.%d.%d.%d", buffer[0], buffer[1], buffer[2], buffer[3]);
|
||||
}
|
||||
|
||||
void parse_mdns_SRV(const std::uint8_t* base, int record_data_offset, mDNS_discovery& mdns, int size) {
|
||||
mdns.port = (base[record_data_offset + 4] << 8) | base[record_data_offset + 5];
|
||||
int name_offset = record_data_offset + 6;
|
||||
mdns.hostname = parse_name(base, size, &name_offset);
|
||||
}
|
||||
|
||||
void parse_mdns_TXT(const std::uint8_t* buffer, mDNS_discovery& mdns, int rdlen) {
|
||||
int txt_ptr = 0;
|
||||
int end_of_record = rdlen;
|
||||
|
||||
while (txt_ptr < end_of_record) {
|
||||
std::uint8_t txt_len = buffer[txt_ptr];
|
||||
txt_ptr++;
|
||||
if (txt_ptr + txt_len <= end_of_record) {
|
||||
const char* entry_cstr = reinterpret_cast<const char*>(&buffer[txt_ptr]);
|
||||
std::string entry(entry_cstr, txt_len);
|
||||
|
||||
std::size_t sep = entry.find('=');
|
||||
if (sep != std::string::npos) {
|
||||
std::string key = entry.substr(0, sep);
|
||||
std::string val = entry.substr(sep + 1);
|
||||
mdns.add_string(key, val);
|
||||
}
|
||||
|
||||
txt_ptr += txt_len;
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void parse_mdns_PTR(const std::uint8_t* base, int record_data_offset, mDNS_discovery& mdns, int size) {
|
||||
std::string service_instance = parse_name(base, size, &record_data_offset);
|
||||
mdns.service_instance = service_instance;
|
||||
}
|
||||
|
||||
void encode_dns_name(std::vector<std::uint8_t>& packet, std::string const& name) {
|
||||
std::size_t start = 0;
|
||||
std::size_t end;
|
||||
while ((end = name.find('.', start)) != std::string::npos) {
|
||||
auto label_len = end - start;
|
||||
if (label_len > 63) {
|
||||
label_len = 63;
|
||||
}
|
||||
packet.push_back(static_cast<std::uint8_t>(label_len));
|
||||
for (std::size_t i = start; i < start + label_len; ++i) {
|
||||
packet.push_back(static_cast<std::uint8_t>(name[i]));
|
||||
}
|
||||
start = end + 1;
|
||||
}
|
||||
if (start < name.length()) {
|
||||
auto label_len = name.length() - start;
|
||||
if (label_len > 63) {
|
||||
label_len = 63;
|
||||
}
|
||||
packet.push_back(static_cast<std::uint8_t>(label_len));
|
||||
for (std::size_t i = start; i < start + label_len; ++i) {
|
||||
packet.push_back(static_cast<std::uint8_t>(name[i]));
|
||||
}
|
||||
}
|
||||
packet.push_back(0);
|
||||
}
|
||||
|
||||
void append_uint16(std::vector<std::uint8_t>& packet, std::uint16_t value) {
|
||||
packet.push_back(static_cast<std::uint8_t>((value >> 8) & 0xFF));
|
||||
packet.push_back(static_cast<std::uint8_t>(value & 0xFF));
|
||||
}
|
||||
|
||||
void append_uint32(std::vector<std::uint8_t>& packet, std::uint32_t value) {
|
||||
packet.push_back(static_cast<std::uint8_t>((value >> 24) & 0xFF));
|
||||
packet.push_back(static_cast<std::uint8_t>((value >> 16) & 0xFF));
|
||||
packet.push_back(static_cast<std::uint8_t>((value >> 8) & 0xFF));
|
||||
packet.push_back(static_cast<std::uint8_t>(value & 0xFF));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
[[maybe_unused]] std::optional<mDNS_discovery> parse_mdns_packet(std::vector<std::uint8_t> const& packet) {
|
||||
int size = packet.size();
|
||||
const auto* buf = packet.data();
|
||||
auto const mdns_packet_min_size = 12;
|
||||
if (size < mdns_packet_min_size) {
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
if ((buf[2] & 0x80) == 0) {
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
mDNS_discovery result;
|
||||
std::size_t questions = (buf[4] << 8) | buf[5];
|
||||
std::size_t answers = (buf[6] << 8) | buf[7];
|
||||
std::size_t authority = (buf[8] << 8) | buf[9];
|
||||
std::size_t additional = (buf[10] << 8) | buf[11];
|
||||
int curr = mdns_packet_min_size;
|
||||
|
||||
for (size_t i = 0; i < questions && curr < size; ++i) {
|
||||
parse_name(buf, size, &curr);
|
||||
curr += 4;
|
||||
}
|
||||
|
||||
auto const mdns_record_header_size = 10;
|
||||
int total_records = static_cast<int>(answers + authority + additional);
|
||||
for (int i = 0; i < total_records && curr < size; ++i) {
|
||||
std::string name = parse_name(buf, size, &curr);
|
||||
if (curr + mdns_record_header_size > size) {
|
||||
break;
|
||||
}
|
||||
std::uint16_t type = (buf[curr] << 8) | buf[curr + 1];
|
||||
std::uint16_t rdlen = (buf[curr + 8] << 8) | buf[curr + 9];
|
||||
curr += mdns_record_header_size;
|
||||
|
||||
if (type == 0x01 && rdlen == 4) {
|
||||
parse_mdns_A(buf + curr, result);
|
||||
} else if (type == 0x21) {
|
||||
parse_mdns_SRV(buf, curr, result, size);
|
||||
} else if (type == 0x10) {
|
||||
parse_mdns_TXT(buf + curr, result, rdlen);
|
||||
} else if (type == 0x0C) {
|
||||
parse_mdns_PTR(buf, curr, result, size);
|
||||
}
|
||||
curr += rdlen;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
[[maybe_unused]] std::vector<std::uint8_t> create_mdns_query(std::string const& name) {
|
||||
std::vector<uint8_t> encoded;
|
||||
std::size_t start = 0, end;
|
||||
while ((end = name.find('.', start)) != std::string::npos) {
|
||||
encoded.push_back(static_cast<uint8_t>(end - start));
|
||||
for (std::size_t i = start; i < end; ++i) {
|
||||
encoded.push_back(name[i]);
|
||||
}
|
||||
start = end + 1;
|
||||
}
|
||||
encoded.push_back(static_cast<uint8_t>(name.length() - start));
|
||||
for (std::size_t i = start; i < name.length(); ++i) {
|
||||
encoded.push_back(name[i]);
|
||||
}
|
||||
encoded.push_back(0);
|
||||
|
||||
std::vector<std::uint8_t> packet = {0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
|
||||
#if defined(__GNUC__) && (__GNUC__ < 12)
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wstringop-overread"
|
||||
#endif
|
||||
packet.insert(packet.end(), encoded.begin(), encoded.end());
|
||||
#if defined(__GNUC__) && (__GNUC__ < 12)
|
||||
#pragma GCC diagnostic pop
|
||||
#endif
|
||||
packet.push_back(0x00);
|
||||
packet.push_back(0x0c);
|
||||
packet.push_back(0x00);
|
||||
packet.push_back(0x01);
|
||||
|
||||
return packet;
|
||||
}
|
||||
|
||||
bool apply_value(std::string const& src, std::string& tar) {
|
||||
if (src == tar) {
|
||||
return false;
|
||||
}
|
||||
if (src.empty()) {
|
||||
return false;
|
||||
}
|
||||
tar = src;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool apply_value(std::uint16_t const src, std::uint16_t& tar) {
|
||||
if (src == 0 or src == tar) {
|
||||
return false;
|
||||
}
|
||||
tar = src;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool mDNS_registry::update(const mDNS_discovery& update) {
|
||||
if (update.service_instance.empty()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
bool something_new = data.count(update.service_instance) == 0;
|
||||
|
||||
auto& item = data[update.service_instance];
|
||||
item.service_instance = update.service_instance;
|
||||
|
||||
something_new = apply_value(update.port, item.port) || something_new;
|
||||
something_new = apply_value(update.hostname, item.hostname) || something_new;
|
||||
something_new = apply_value(update.ip, item.ip) || something_new;
|
||||
|
||||
for (auto const& [key, val] : update.txt) {
|
||||
something_new = apply_value(val, item.txt[key]) || something_new;
|
||||
}
|
||||
return something_new;
|
||||
}
|
||||
|
||||
void mDNS_registry::remove(const std::string& instance) {
|
||||
data.erase(instance);
|
||||
}
|
||||
|
||||
void mDNS_registry::clear() {
|
||||
data.clear();
|
||||
}
|
||||
|
||||
mDNS_registry::registry const& mDNS_registry::get() {
|
||||
return data;
|
||||
}
|
||||
|
||||
[[maybe_unused]] std::vector<std::uint8_t> create_mdns_response(mDNS_discovery const& service,
|
||||
std::string const& service_type) {
|
||||
std::string service_fqdn = service_type + ".local";
|
||||
std::string instance_fqdn = service.service_instance;
|
||||
if (instance_fqdn.empty()) {
|
||||
instance_fqdn = service.hostname + "." + service_fqdn;
|
||||
}
|
||||
std::string host_fqdn = service.hostname + ".local";
|
||||
|
||||
std::vector<std::uint8_t> packet;
|
||||
append_uint16(packet, 0x0000); // Transaction ID
|
||||
append_uint16(packet, 0x8400); // Flags: response, authoritative
|
||||
append_uint16(packet, 0x0000); // Questions
|
||||
append_uint16(packet, 0x0004); // Answer RRs (PTR + SRV + TXT + A)
|
||||
append_uint16(packet, 0x0000); // Authority RRs
|
||||
append_uint16(packet, 0x0000); // Additional RRs
|
||||
|
||||
auto const default_ttl = static_cast<std::uint32_t>(120);
|
||||
|
||||
// PTR record: service_type.local -> instance_fqdn
|
||||
encode_dns_name(packet, service_fqdn);
|
||||
append_uint16(packet, 0x000C);
|
||||
append_uint16(packet, 0x0001);
|
||||
append_uint32(packet, default_ttl);
|
||||
std::vector<std::uint8_t> ptr_rdata;
|
||||
encode_dns_name(ptr_rdata, instance_fqdn);
|
||||
append_uint16(packet, static_cast<std::uint16_t>(ptr_rdata.size()));
|
||||
packet.insert(packet.end(), ptr_rdata.begin(), ptr_rdata.end());
|
||||
|
||||
// SRV record: instance_fqdn -> host:port
|
||||
encode_dns_name(packet, instance_fqdn);
|
||||
append_uint16(packet, 0x0021);
|
||||
append_uint16(packet, 0x8001);
|
||||
append_uint32(packet, default_ttl);
|
||||
std::vector<std::uint8_t> srv_rdata;
|
||||
append_uint16(srv_rdata, 0x0000); // Priority
|
||||
append_uint16(srv_rdata, 0x0000); // Weight
|
||||
append_uint16(srv_rdata, service.port);
|
||||
encode_dns_name(srv_rdata, host_fqdn);
|
||||
append_uint16(packet, static_cast<std::uint16_t>(srv_rdata.size()));
|
||||
packet.insert(packet.end(), srv_rdata.begin(), srv_rdata.end());
|
||||
|
||||
// TXT record: instance_fqdn -> key=value pairs
|
||||
encode_dns_name(packet, instance_fqdn);
|
||||
append_uint16(packet, 0x0010);
|
||||
append_uint16(packet, 0x8001);
|
||||
append_uint32(packet, default_ttl);
|
||||
std::vector<std::uint8_t> txt_rdata;
|
||||
for (auto const& [key, val] : service.txt) {
|
||||
std::string entry = key + "=" + val;
|
||||
txt_rdata.push_back(static_cast<std::uint8_t>(entry.size()));
|
||||
for (char ch : entry) {
|
||||
txt_rdata.push_back(static_cast<std::uint8_t>(ch));
|
||||
}
|
||||
}
|
||||
if (txt_rdata.empty()) {
|
||||
txt_rdata.push_back(0);
|
||||
}
|
||||
append_uint16(packet, static_cast<std::uint16_t>(txt_rdata.size()));
|
||||
packet.insert(packet.end(), txt_rdata.begin(), txt_rdata.end());
|
||||
|
||||
// A record: host_fqdn -> IP
|
||||
encode_dns_name(packet, host_fqdn);
|
||||
append_uint16(packet, 0x0001);
|
||||
append_uint16(packet, 0x8001);
|
||||
append_uint32(packet, default_ttl);
|
||||
append_uint16(packet, 0x0004);
|
||||
struct in_addr addr;
|
||||
if (inet_pton(AF_INET, service.ip.c_str(), &addr) == 1) {
|
||||
auto* bytes = reinterpret_cast<std::uint8_t*>(&addr.s_addr);
|
||||
packet.push_back(bytes[0]);
|
||||
packet.push_back(bytes[1]);
|
||||
packet.push_back(bytes[2]);
|
||||
packet.push_back(bytes[3]);
|
||||
} else {
|
||||
packet.push_back(0);
|
||||
packet.push_back(0);
|
||||
packet.push_back(0);
|
||||
packet.push_back(0);
|
||||
}
|
||||
|
||||
return packet;
|
||||
}
|
||||
|
||||
[[maybe_unused]] bool is_query_for(std::vector<std::uint8_t> const& packet, std::string const& service_type) {
|
||||
int size = static_cast<int>(packet.size());
|
||||
auto const* buf = packet.data();
|
||||
auto const mdns_packet_min_size = 12;
|
||||
if (size < mdns_packet_min_size) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if ((buf[2] & 0x80) != 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
std::size_t questions = (buf[4] << 8) | buf[5];
|
||||
if (questions == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
int curr = mdns_packet_min_size;
|
||||
std::string service_fqdn = service_type + ".local";
|
||||
|
||||
for (std::size_t i = 0; i < questions && curr < size; ++i) {
|
||||
std::string qname = parse_name(buf, size, &curr);
|
||||
if (curr + 4 > size) {
|
||||
break;
|
||||
}
|
||||
std::uint16_t qtype = (buf[curr] << 8) | buf[curr + 1];
|
||||
curr += 4;
|
||||
|
||||
if ((qtype == 0x0C || qtype == 0xFF) && qname == service_fqdn) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::mdns
|
||||
66
tools/EVerest-main/lib/everest/io/src/mdns/mdns_socket.cpp
Normal file
66
tools/EVerest-main/lib/everest/io/src/mdns/mdns_socket.cpp
Normal file
@@ -0,0 +1,66 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2026 Pionix GmbH and Contributors to EVerest
|
||||
|
||||
#include "everest/io/udp/udp_payload.hpp"
|
||||
#include "everest/io/udp/udp_socket.hpp"
|
||||
#include <arpa/inet.h>
|
||||
#include <everest/io/event/fd_event_handler.hpp>
|
||||
#include <everest/io/event/unique_fd.hpp>
|
||||
#include <everest/io/mdns/mdns.hpp>
|
||||
#include <everest/io/mdns/mdns_socket.hpp>
|
||||
#include <everest/io/socket/socket.hpp>
|
||||
#include <iostream>
|
||||
#include <net/if.h>
|
||||
#include <netinet/in.h>
|
||||
#include <optional>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/socket.h>
|
||||
#include <unistd.h>
|
||||
|
||||
namespace everest::lib::io::mdns {
|
||||
|
||||
/////////////////////////////////////////////////
|
||||
|
||||
bool mdns_socket::open(std::string const& interface) {
|
||||
auto const mdns_port = 5353;
|
||||
auto const* const mdns_ip = "224.0.0.251";
|
||||
|
||||
auto socket = socket::open_mdns_socket(interface);
|
||||
m_owned_udp_fd = std::move(socket);
|
||||
|
||||
target.addr = inet_addr(mdns_ip);
|
||||
target.port = htons(mdns_port);
|
||||
target.family = AF_INET;
|
||||
|
||||
return socket::get_pending_error(m_owned_udp_fd) == 0;
|
||||
}
|
||||
|
||||
bool mdns_socket::tx(PayloadT const& payload) {
|
||||
return tx_impl(payload.buffer.data(), payload.size(), target);
|
||||
}
|
||||
|
||||
bool mdns_socket::rx(PayloadT& payload) {
|
||||
ssize_t msg_size = 0;
|
||||
auto result = rx_impl(rx_buffer.data(), rx_buffer.size(), msg_size);
|
||||
if (result) {
|
||||
payload.set_message(rx_buffer.data(), msg_size);
|
||||
}
|
||||
return result.has_value();
|
||||
}
|
||||
|
||||
bool mdns_socket::query(std::string const& what) {
|
||||
PayloadT payload;
|
||||
payload.buffer = everest::lib::io::mdns::create_mdns_query(what);
|
||||
return tx(payload);
|
||||
}
|
||||
|
||||
bool mdns_socket::announce(mDNS_discovery const& service, std::string const& service_type) {
|
||||
PayloadT payload;
|
||||
payload.buffer = create_mdns_response(service, service_type);
|
||||
return tx(payload);
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::mdns
|
||||
521
tools/EVerest-main/lib/everest/io/src/mqtt/mosquitto_cpp.cpp
Normal file
521
tools/EVerest-main/lib/everest/io/src/mqtt/mosquitto_cpp.cpp
Normal file
@@ -0,0 +1,521 @@
|
||||
/*
|
||||
* Licensor: Pionix GmbH, 2025
|
||||
* License: BaseCamp - License Version 1.0
|
||||
*
|
||||
* Licensed under the terms and conditions of the BaseCamp License contained in the "LICENSE" file, also available
|
||||
* under: https://pionix.com/pionix-license-terms
|
||||
* You may not use this file/code except in compliance with said License.
|
||||
*/
|
||||
|
||||
#include <everest/io/mqtt/mosquitto_cpp.hpp>
|
||||
// #include <companion/utilities/String.hpp>
|
||||
// #include <utils/logging.hpp>
|
||||
|
||||
#include <cstdio>
|
||||
#include <iostream>
|
||||
#include <mosquitto.h>
|
||||
#include <stdexcept>
|
||||
/**
|
||||
* \file MQTT v5 client
|
||||
*/
|
||||
|
||||
namespace {
|
||||
using namespace everest::lib::io::mqtt;
|
||||
constexpr int loop_timeout_ms = -1; // use default
|
||||
|
||||
int password_callback(char* buf, int size, [[maybe_unused]] int rwflag, [[maybe_unused]] void* userdata) {
|
||||
if ((buf != nullptr) && (size > 0)) {
|
||||
*buf = '\0';
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
constexpr mosquitto_cpp::QoS convert_to_qos(int qos) {
|
||||
switch (qos) {
|
||||
case static_cast<int>(mosquitto_cpp::QoS::at_least_once):
|
||||
case static_cast<int>(mosquitto_cpp::QoS::at_most_once):
|
||||
case static_cast<int>(mosquitto_cpp::QoS::exactly_once):
|
||||
return static_cast<mosquitto_cpp::QoS>(qos);
|
||||
}
|
||||
|
||||
throw std::out_of_range("QoS from int: " + std::to_string(qos));
|
||||
}
|
||||
|
||||
// clang-format off
|
||||
#define FOR_ALL_ERROR_CODES(apply) \
|
||||
apply(SUCCESS, Success) \
|
||||
apply(INVAL, InvalidArgument) \
|
||||
apply(NOMEM, NoMemory) \
|
||||
apply(ERRNO, Errno) \
|
||||
apply(NO_CONN, NoConnection) \
|
||||
apply(CONN_LOST, ConnectionLost) \
|
||||
apply(CONN_REFUSED, ConnectionRefused) \
|
||||
apply(PROTOCOL, Protocol) \
|
||||
apply(PAYLOAD_SIZE, PayloadSize) \
|
||||
apply(MALFORMED_UTF8, MalformedUTF8) \
|
||||
apply(DUPLICATE_PROPERTY, DuplicateProperty) \
|
||||
apply(QOS_NOT_SUPPORTED, QoSNotSupported) \
|
||||
apply(OVERSIZE_PACKET, OversizePacket) \
|
||||
apply(NOT_SUPPORTED, NotSupported) \
|
||||
apply(EAI, HostnameLookup) \
|
||||
apply(TLS, Tls) \
|
||||
apply(TLS_HANDSHAKE, TlsHandshake)
|
||||
|
||||
// clang-format off
|
||||
#define VALUES(a, b) \
|
||||
case MOSQ_ERR_##a: return ErrorCode::b;
|
||||
// clang-format on
|
||||
|
||||
constexpr ErrorCode convertEC(int rc) {
|
||||
switch (rc) {
|
||||
FOR_ALL_ERROR_CODES(VALUES)
|
||||
default:
|
||||
// LogError << "Unrecognised mosquitto error: " << rc;
|
||||
return ErrorCode::Unknown;
|
||||
}
|
||||
}
|
||||
#undef VALUES
|
||||
|
||||
} // namespace
|
||||
|
||||
namespace everest::lib::io::mqtt {
|
||||
|
||||
PropertiesBase::~PropertiesBase() {
|
||||
if (!is_const) {
|
||||
mosquitto_property_free_all(&props);
|
||||
}
|
||||
}
|
||||
|
||||
void PropertiesBase::free_property(mqtt5__property** ptr) {
|
||||
mosquitto_property_free_all(ptr);
|
||||
}
|
||||
|
||||
const mqtt5__property* PropertiesBase::get_property(property_t prop) const {
|
||||
const mosquitto_property* result{nullptr};
|
||||
for (const auto* p = props; p != nullptr; p = mosquitto_property_next(p)) {
|
||||
if (mosquitto_property_identifier(p) == static_cast<int>(prop)) {
|
||||
result = p;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
std::string PropertiesBase::get_response_topic() const {
|
||||
std::string result;
|
||||
char* topic{nullptr};
|
||||
if (mosquitto_property_read_string(props, static_cast<int>(property_t::ResponseTopic), &topic, false) != nullptr) {
|
||||
result = topic;
|
||||
}
|
||||
::free(topic);
|
||||
// LogDebug << "response_topic (" << props << "): " << result;
|
||||
return result;
|
||||
}
|
||||
|
||||
std::string PropertiesBase::get_correlation_data() const {
|
||||
std::string result;
|
||||
void* data{nullptr};
|
||||
std::uint16_t len{0};
|
||||
if (mosquitto_property_read_binary(props, static_cast<int>(property_t::CorrelationData), &data, &len, false) !=
|
||||
nullptr) {
|
||||
result = std::string{static_cast<char*>(data), len};
|
||||
}
|
||||
::free(data);
|
||||
// LogDebug << "correlation_data (" << props << "): " << result;
|
||||
return result;
|
||||
}
|
||||
|
||||
ErrorCode Properties::set_response_topic(const std::string& topic) {
|
||||
const auto result =
|
||||
convertEC(mosquitto_property_add_string(&props, static_cast<int>(property_t::ResponseTopic), topic.data()));
|
||||
if (result != ErrorCode::Success) {
|
||||
// LogError << "set_response_topic: " << to_string(result);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
ErrorCode Properties::set_correlation_data(const std::string& data) {
|
||||
const auto result = convertEC(
|
||||
mosquitto_property_add_binary(&props, static_cast<int>(property_t::CorrelationData), data.data(), data.size()));
|
||||
if (result != ErrorCode::Success) {
|
||||
// LogError << "set_correlation_data: " << to_string(result);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
// clang-format off
|
||||
#define VALUES(a, b) \
|
||||
case ErrorCode::b: return {#b};
|
||||
// clang-format on
|
||||
|
||||
std::string_view to_string(ErrorCode ec) {
|
||||
switch (ec) {
|
||||
FOR_ALL_ERROR_CODES(VALUES)
|
||||
default:
|
||||
return {"<unknown>"};
|
||||
}
|
||||
}
|
||||
#undef VALUES
|
||||
|
||||
void mosquitto_cpp::cb_connect([[maybe_unused]] mosquitto* mosq, void* obj, int rc, int flags,
|
||||
const mqtt5__property* props) {
|
||||
if (obj != nullptr) {
|
||||
auto& client = *reinterpret_cast<mosquitto_cpp*>(obj);
|
||||
const PropertiesAccess p(props);
|
||||
const auto r = static_cast<ResponseCode>(rc);
|
||||
client.connect_ccb(r, flags, p);
|
||||
}
|
||||
}
|
||||
|
||||
void mosquitto_cpp::cb_disconnect([[maybe_unused]] mosquitto* mosq, void* obj, int rc, const mqtt5__property* props) {
|
||||
if (obj != nullptr) {
|
||||
auto& client = *reinterpret_cast<mosquitto_cpp*>(obj);
|
||||
const PropertiesAccess p(props);
|
||||
const auto ec = convertEC(rc);
|
||||
client.disconnect_ccb(ec, p);
|
||||
client.callbacks.clear();
|
||||
}
|
||||
}
|
||||
|
||||
void mosquitto_cpp::cb_log([[maybe_unused]] mosquitto* mosq, void* obj, int level, const char* str) {
|
||||
if (obj != nullptr) {
|
||||
auto& client = *reinterpret_cast<mosquitto_cpp*>(obj);
|
||||
LogLevel l;
|
||||
switch (level) {
|
||||
case MOSQ_LOG_INFO:
|
||||
l = LogLevel::info;
|
||||
break;
|
||||
case MOSQ_LOG_NOTICE:
|
||||
l = LogLevel::notice;
|
||||
break;
|
||||
case MOSQ_LOG_WARNING:
|
||||
l = LogLevel::warning;
|
||||
break;
|
||||
case MOSQ_LOG_ERR:
|
||||
l = LogLevel::error;
|
||||
break;
|
||||
case MOSQ_LOG_DEBUG:
|
||||
default:
|
||||
l = LogLevel::debug;
|
||||
break;
|
||||
}
|
||||
client.log(l, str);
|
||||
}
|
||||
}
|
||||
|
||||
void mosquitto_cpp::cb_message([[maybe_unused]] mosquitto* mosq, void* obj, const mosquitto_message* msg,
|
||||
const mqtt5__property* props) {
|
||||
if ((obj != nullptr) && (msg != nullptr)) {
|
||||
auto& client = *reinterpret_cast<mosquitto_cpp*>(obj);
|
||||
const PropertiesAccess p(props);
|
||||
const std::string_view topic(msg->topic);
|
||||
const std::string_view payload(static_cast<const char*>(msg->payload), msg->payloadlen);
|
||||
const QoS qos = convert_to_qos(msg->qos);
|
||||
client.message_ccb(topic, payload, qos, p);
|
||||
}
|
||||
}
|
||||
|
||||
void mosquitto_cpp::cb_publish([[maybe_unused]] mosquitto* mosq, void* obj, int mid, int rc,
|
||||
const mqtt5__property* props) {
|
||||
if (obj != nullptr) {
|
||||
auto& client = *reinterpret_cast<mosquitto_cpp*>(obj);
|
||||
const PropertiesAccess p(props);
|
||||
const auto r = static_cast<ResponseCode>(rc);
|
||||
client.publish_ccb(mid, r, p);
|
||||
}
|
||||
}
|
||||
|
||||
void mosquitto_cpp::connect_ccb(ResponseCode rc, int flags, const PropertiesAccess& props) {
|
||||
if (connect_cb) {
|
||||
connect_cb(*this, rc, flags, props);
|
||||
}
|
||||
}
|
||||
|
||||
void mosquitto_cpp::disconnect_ccb(ErrorCode ec, const PropertiesAccess& props) {
|
||||
if (disconnect_cb) {
|
||||
disconnect_cb(*this, ec, props);
|
||||
}
|
||||
}
|
||||
|
||||
void mosquitto_cpp::message_ccb(const std::string_view& topic, const std::string_view& payload, QoS qos,
|
||||
const PropertiesAccess& props) {
|
||||
// LogDebug << "MQTT msg " << topic << ' ' << bc_utils::hex_string(payload);
|
||||
for (const auto& [sub_topic, callback] : callbacks) {
|
||||
bool matches{false};
|
||||
if ((mosquitto_topic_matches_sub2(sub_topic.data(), sub_topic.size(), topic.data(), topic.size(), &matches) ==
|
||||
MOSQ_ERR_SUCCESS) &&
|
||||
matches) {
|
||||
callback(*this, topic, payload, qos, props);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void mosquitto_cpp::publish_ccb(int mid, ResponseCode rc, const PropertiesAccess& props) {
|
||||
if (publish_cb) {
|
||||
publish_cb(*this, mid, rc, props);
|
||||
}
|
||||
}
|
||||
|
||||
mosquitto_cpp::mosquitto_cpp() : mosquitto_cpp(nullptr) {
|
||||
}
|
||||
|
||||
mosquitto_cpp::mosquitto_cpp(const char* id) : client(nullptr, nullptr) {
|
||||
mosquitto* ptr = mosquitto_new(id, true, this);
|
||||
std::unique_ptr<mosquitto, decltype(&mosquitto_destroy)> tmp(ptr, &mosquitto_destroy);
|
||||
client.swap(tmp);
|
||||
// use v5
|
||||
mosquitto_int_option(client.get(), MOSQ_OPT_PROTOCOL_VERSION, MQTT_PROTOCOL_V5);
|
||||
|
||||
// TODO(james-ctc): select sensible defaults
|
||||
// mosquitto_int_option(client.get(), MOSQ_OPT_RECEIVE_MAXIMUM, 20);
|
||||
// mosquitto_int_option(client.get(), MOSQ_OPT_SEND_MAXIMUM, 20);
|
||||
|
||||
// Enable TCP no delay
|
||||
// mosquitto_int_option(client.get(), MOSQ_OPT_TCP_NODELAY, 1);
|
||||
|
||||
// multiple threads are likely
|
||||
mosquitto_threaded_set(client.get(), true);
|
||||
mosquitto_connect_v5_callback_set(client.get(), cb_connect);
|
||||
mosquitto_disconnect_v5_callback_set(client.get(), cb_disconnect);
|
||||
mosquitto_log_callback_set(client.get(), cb_log);
|
||||
mosquitto_message_v5_callback_set(client.get(), cb_message);
|
||||
mosquitto_publish_v5_callback_set(client.get(), cb_publish);
|
||||
// mosquitto_subscribe_v5_callback_set(client.get(), cb_subscribe);
|
||||
}
|
||||
|
||||
mosquitto_cpp::~mosquitto_cpp() {
|
||||
disconnect();
|
||||
}
|
||||
|
||||
void mosquitto_cpp::log(LogLevel level, [[maybe_unused]] const char* str) {
|
||||
switch (level) {
|
||||
case LogLevel::info:
|
||||
case LogLevel::notice:
|
||||
// LogInfo << str;
|
||||
break;
|
||||
case LogLevel::warning:
|
||||
// LogWarning << str;
|
||||
break;
|
||||
case LogLevel::error:
|
||||
// LogError << str;
|
||||
break;
|
||||
case LogLevel::debug:
|
||||
// LogDebug << str;
|
||||
break;
|
||||
default:
|
||||
// should not occur
|
||||
// LogCritical << str;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void mosquitto_cpp::set_callback_connect_impl(connect_callback cb) {
|
||||
connect_cb = std::move(cb);
|
||||
}
|
||||
|
||||
void mosquitto_cpp::set_callback_disconnect_impl(disconnect_callback cb) {
|
||||
disconnect_cb = std::move(cb);
|
||||
}
|
||||
|
||||
void mosquitto_cpp::set_callback_publish_impl(publish_callback cb) {
|
||||
publish_cb = std::move(cb);
|
||||
}
|
||||
|
||||
void mosquitto_cpp::set_callback_connect(connect_callback cb) {
|
||||
set_callback_connect_impl(std::move(cb));
|
||||
}
|
||||
|
||||
void mosquitto_cpp::set_callback_disconnect(disconnect_callback cb) {
|
||||
set_callback_disconnect_impl(std::move(cb));
|
||||
}
|
||||
|
||||
void mosquitto_cpp::set_callback_publish(publish_callback cb) {
|
||||
set_callback_publish_impl(std::move(cb));
|
||||
}
|
||||
|
||||
bool mosquitto_cpp::is_connect_callback_set() {
|
||||
return static_cast<bool>(connect_cb);
|
||||
}
|
||||
bool mosquitto_cpp::is_disconnect_callback_set() {
|
||||
return static_cast<bool>(disconnect_cb);
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::tls(const char* ca_file, const char* ca_path, const char* cert_file, const char* key_file) {
|
||||
return convertEC(mosquitto_tls_set(client.get(), ca_file, ca_path, cert_file, key_file, password_callback));
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::tls(const ::std::string& ca_file, const ::std::string& ca_path, const ::std::string& cert_file,
|
||||
const ::std::string& key_file) {
|
||||
const char* ca_file_ptr{nullptr};
|
||||
const char* ca_path_ptr{nullptr};
|
||||
if (!ca_file.empty()) {
|
||||
ca_file_ptr = ca_file.c_str();
|
||||
}
|
||||
if (!ca_path.empty()) {
|
||||
ca_path_ptr = ca_path.c_str();
|
||||
}
|
||||
return tls(ca_file_ptr, ca_path_ptr, cert_file.c_str(), key_file.c_str());
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::connect(const std::string_view& host, std::uint16_t port, std::uint16_t keepalive_seconds) {
|
||||
return connect_impl("", host, port, keepalive_seconds);
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::connect(const std::string_view& bind_address, const std::string_view& host, std::uint16_t port,
|
||||
std::uint16_t keepalive_seconds) {
|
||||
return connect_impl(bind_address, host, port, keepalive_seconds);
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::connect(const std::string_view& unix_domain_socket, std::uint16_t keepalive_seconds) {
|
||||
return connect_impl("", unix_domain_socket, 0, keepalive_seconds);
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::connect_impl(const std::string_view& bind_address, const std::string_view& host,
|
||||
std::uint16_t port, std::uint16_t keepalive_seconds) {
|
||||
const char* bind_to = bind_address.empty() ? nullptr : bind_address.data();
|
||||
return convertEC(mosquitto_connect_bind_async(client.get(), host.data(), port, keepalive_seconds, bind_to));
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::reconnect() {
|
||||
return convertEC(mosquitto_reconnect_async(client.get()));
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::disconnect() {
|
||||
return convertEC(mosquitto_disconnect(client.get()));
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::loop_forever() {
|
||||
return convertEC(mosquitto_loop_forever(client.get(), loop_timeout_ms, 1));
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::loop_read() {
|
||||
return convertEC(mosquitto_loop_read(client.get(), 1));
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::loop_write() {
|
||||
return convertEC(mosquitto_loop_write(client.get(), 1));
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::loop_misc() {
|
||||
return convertEC(mosquitto_loop_misc(client.get()));
|
||||
}
|
||||
|
||||
bool mosquitto_cpp::want_write() {
|
||||
return mosquitto_want_write(client.get());
|
||||
}
|
||||
|
||||
int mosquitto_cpp::socket() const {
|
||||
return mosquitto_socket(client.get());
|
||||
}
|
||||
|
||||
void mosquitto_cpp::set_option_threaded(bool val) {
|
||||
(void)mosquitto_threaded_set(client.get(), val);
|
||||
}
|
||||
|
||||
void mosquitto_cpp::set_option_tcpnodelay(bool val) {
|
||||
(void)mosquitto_int_option(client.get(), MOSQ_OPT_TCP_NODELAY, val);
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::set_will(const std::string_view& topic, const std::string_view& payload, QoS qos, bool retain,
|
||||
PropertiesBase&& props) {
|
||||
return set_will_impl(topic, payload, qos, retain, std::move(props));
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::set_will_impl(const std::string_view& topic, const std::string_view& payload, QoS qos,
|
||||
bool retain, PropertiesBase&& props) {
|
||||
// must be called before connect()
|
||||
|
||||
// On success only, the property list becomes the property of libmosquitto
|
||||
// once this function is called and will be freed by the library.
|
||||
// The property list must be freed by the application on error.
|
||||
|
||||
ErrorCode result;
|
||||
auto* prop_p = props.release();
|
||||
result = convertEC(mosquitto_will_set_v5(client.get(), topic.data(), payload.size(), payload.data(),
|
||||
static_cast<int>(qos), retain, prop_p));
|
||||
if (result != ErrorCode::Success) {
|
||||
PropertiesBase::free_property(&prop_p);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::publish(int* mid, const std::string_view& topic, const std::string_view& payload, QoS qos,
|
||||
bool retain, const PropertiesBase& props) {
|
||||
return publish_impl(mid, topic, payload, qos, retain, props);
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::publish(const std::string_view& topic, const std::string_view& payload, QoS qos, bool retain,
|
||||
const PropertiesBase& props) {
|
||||
return publish(nullptr, topic, payload, qos, retain, props);
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::publish(const std::string_view& topic, const std::string_view& payload) {
|
||||
return publish(nullptr, topic, payload, default_QoS, false, {});
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::publish(message const& data) {
|
||||
return publish(data.topic, data.payload, data.qos, false, {});
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::publish_impl(int* mid, const std::string_view& topic, const std::string_view& payload, QoS qos,
|
||||
bool retain, const PropertiesBase& props) {
|
||||
// LogDebug << "MQTT pub " << topic << ' ' << bc_utils::hex_string(payload);
|
||||
return convertEC(mosquitto_publish_v5(client.get(), mid, topic.data(), payload.size(), payload.data(),
|
||||
static_cast<int>(qos), retain, props));
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::subscribe(const std::string_view& topic, QoS qos, int options, const PropertiesBase& props,
|
||||
subscribe_callback cb) {
|
||||
return subscribe_impl(topic, qos, options, props, std::move(cb));
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::subscribe(const std::string_view& topic, subscribe_callback cb) {
|
||||
return subscribe(topic, default_QoS, 0, {}, std::move(cb));
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::subscribe(std::string_view const& topic, subscribe_message_callback const& cb, QoS qos) {
|
||||
return subscribe(topic, qos, 0, {},
|
||||
[cb = std::move(cb)](auto& cb_client, auto const& cb_topic, auto const& cb_payload, QoS cb_qos,
|
||||
[[maybe_unused]] auto const& props) {
|
||||
message data;
|
||||
data.topic = cb_topic;
|
||||
data.payload = cb_payload;
|
||||
data.qos = cb_qos;
|
||||
cb(cb_client, data);
|
||||
});
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::subscribe_impl(const std::string_view& topic, QoS qos, int options,
|
||||
const PropertiesBase& props, subscribe_callback cb) {
|
||||
// LogDebug << "MQTT sub " << topic;
|
||||
auto result =
|
||||
convertEC(mosquitto_subscribe_v5(client.get(), nullptr, topic.data(), static_cast<int>(qos), options, props));
|
||||
if (result == ErrorCode::Success) {
|
||||
const auto [it, success] = callbacks.insert({std::string{topic}, std::move(cb)});
|
||||
if (!success) {
|
||||
result = ErrorCode::MapInsert;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::unsubscribe(const std::string_view& topic, const PropertiesBase& props) {
|
||||
return unsubscribe_impl(topic, props);
|
||||
}
|
||||
|
||||
ErrorCode mosquitto_cpp::unsubscribe_impl(const std::string_view& topic, const PropertiesBase& props) {
|
||||
auto result = convertEC(mosquitto_unsubscribe_v5(client.get(), nullptr, topic.data(), props));
|
||||
if (result == ErrorCode::Success) {
|
||||
callbacks.erase(std::string{topic});
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
bool mosquitto_cpp::library_init() {
|
||||
return mosquitto_lib_init() == MOSQ_ERR_SUCCESS;
|
||||
}
|
||||
|
||||
void mosquitto_cpp::library_cleanup() {
|
||||
(void)mosquitto_lib_cleanup();
|
||||
}
|
||||
} // namespace everest::lib::io::mqtt
|
||||
235
tools/EVerest-main/lib/everest/io/src/mqtt/mqtt_client.cpp
Normal file
235
tools/EVerest-main/lib/everest/io/src/mqtt/mqtt_client.cpp
Normal file
@@ -0,0 +1,235 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2025 Pionix GmbH and Contributors to EVerest
|
||||
|
||||
/** \file */
|
||||
|
||||
#include "everest/io/event/fd_event_handler.hpp"
|
||||
#include "everest/io/event/fd_event_sync_interface.hpp"
|
||||
#include "everest/io/mqtt/mosquitto_cpp.hpp"
|
||||
#include <chrono>
|
||||
#include <cstring>
|
||||
#include <everest/io/mqtt/mqtt_client.hpp>
|
||||
#include <memory>
|
||||
|
||||
namespace {
|
||||
constexpr std::uint32_t sync_interval = 1000;
|
||||
} // namespace
|
||||
|
||||
namespace everest::lib::io::mqtt {
|
||||
|
||||
mqtt_client::mqtt_client(std::uint32_t reconnect_to_ms, std::string client_id) :
|
||||
mosquitto_cpp(client_id.empty() ? nullptr : client_id.c_str()), m_client_id(std::move(client_id)) {
|
||||
set_option_tcpnodelay(true);
|
||||
m_reconnect_timer.set_timeout_ms(reconnect_to_ms);
|
||||
m_sync_timer.set_timeout_ms(sync_interval);
|
||||
}
|
||||
|
||||
std::string mqtt_client::get_client_id() const {
|
||||
return m_client_id;
|
||||
}
|
||||
|
||||
mqtt_client::~mqtt_client() {
|
||||
// This fixes late adding actions to the event_handler
|
||||
mosquitto_cpp::set_callback_disconnect_impl([](auto&, auto, auto const&) {});
|
||||
}
|
||||
|
||||
void mqtt_client::handle_socket(event_list const& events) {
|
||||
ErrorCode ec{ErrorCode::Success};
|
||||
if (events.count(event::poll_events::read)) {
|
||||
ec = loop_read();
|
||||
}
|
||||
if (ec == ErrorCode::Success and events.count(event::poll_events::write)) {
|
||||
ec = loop_write();
|
||||
if (not want_write()) {
|
||||
listen_to_write_events(false);
|
||||
}
|
||||
}
|
||||
if (ec == ErrorCode::Success) {
|
||||
ec = loop_misc();
|
||||
}
|
||||
if (ec == ErrorCode::Success and want_write()) {
|
||||
listen_to_write_events(true);
|
||||
}
|
||||
if (ec not_eq ErrorCode::Success) {
|
||||
}
|
||||
}
|
||||
|
||||
void mqtt_client::handle_sync_timer() {
|
||||
loop_misc();
|
||||
if (want_write()) {
|
||||
listen_to_write_events(true);
|
||||
}
|
||||
}
|
||||
|
||||
void mqtt_client::handle_reconnect_timer() {
|
||||
m_handler.add_action([this] {
|
||||
if (m_last_socket not_eq -1) {
|
||||
m_handler.unregister_event_handler(m_last_socket);
|
||||
m_last_socket = -1;
|
||||
}
|
||||
auto result = reconnect();
|
||||
if (result == ErrorCode::Success) {
|
||||
m_last_socket = socket();
|
||||
if (m_last_socket not_eq -1) {
|
||||
m_handler.register_event_handler(
|
||||
socket(), [this](auto const& events) { handle_socket(events); }, event::poll_events::read);
|
||||
listen_to_write_events_if_wanted();
|
||||
listen_to_reconnect_timer(false);
|
||||
}
|
||||
}
|
||||
listen_to_write_events_if_wanted();
|
||||
handle_error(result);
|
||||
});
|
||||
}
|
||||
|
||||
void mqtt_client::listen_to_reconnect_timer(bool enable) {
|
||||
m_handler.add_action([this, enable]() {
|
||||
if (enable) {
|
||||
m_reconnect_timer.reset();
|
||||
m_handler.register_event_handler(&m_reconnect_timer, [this](auto const&) { handle_reconnect_timer(); });
|
||||
} else {
|
||||
m_handler.unregister_event_handler(&m_reconnect_timer);
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
void mqtt_client::listen_to_sync_timer(bool enable) {
|
||||
m_handler.add_action([this, enable]() {
|
||||
if (enable) {
|
||||
m_sync_timer.reset();
|
||||
m_handler.register_event_handler(&m_sync_timer, [this](auto const&) { handle_sync_timer(); });
|
||||
} else {
|
||||
m_handler.unregister_event_handler(&m_sync_timer);
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
ErrorCode mqtt_client::handle_error(ErrorCode error) {
|
||||
if (not m_error_handler) {
|
||||
return error;
|
||||
}
|
||||
if (m_last_error == error) {
|
||||
return error;
|
||||
}
|
||||
m_last_error = error;
|
||||
if (error == ErrorCode::Errno) {
|
||||
m_error_handler(errno, strerror(errno));
|
||||
} else {
|
||||
m_error_handler(static_cast<int>(error), static_cast<std::string>(to_string(error)));
|
||||
}
|
||||
return error;
|
||||
}
|
||||
|
||||
void mqtt_client::listen_to_write_events(bool enable) {
|
||||
m_handler.add_action([this, enable]() {
|
||||
auto action = enable ? event::event_modification::add : event::event_modification::remove;
|
||||
m_handler.modify_event_handler(socket(), event::poll_events::write, action);
|
||||
});
|
||||
}
|
||||
|
||||
void mqtt_client::listen_to_write_events_if_wanted() {
|
||||
if (want_write()) {
|
||||
listen_to_write_events(true);
|
||||
}
|
||||
}
|
||||
|
||||
void mqtt_client::set_error_handler(const cb_error& handler) {
|
||||
m_error_handler = handler;
|
||||
// setting dummy callbacks. Actual error_handling callbacks are integrated in the called functions
|
||||
// The check ensures that set_error_handler and set_callback_xxxxx can be called in any order
|
||||
if (not is_connect_callback_set()) {
|
||||
set_callback_connect([](auto&, auto, auto, auto const&) {});
|
||||
}
|
||||
if (not is_disconnect_callback_set()) {
|
||||
set_callback_disconnect([](auto&, auto, auto const&) {});
|
||||
}
|
||||
}
|
||||
|
||||
///// fd_event_sync_interface implementation
|
||||
|
||||
int mqtt_client::get_poll_fd() {
|
||||
return m_handler.get_poll_fd();
|
||||
}
|
||||
|
||||
everest::lib::io::event::sync_status mqtt_client::sync() {
|
||||
m_handler.run_once();
|
||||
return everest::lib::io::event::sync_status::ok;
|
||||
};
|
||||
|
||||
///// mosquitto_cpp override
|
||||
|
||||
ErrorCode mqtt_client::connect_impl(std::string_view const& bind_address, std::string_view const& host,
|
||||
std::uint16_t port, std::uint16_t keepalive_seconds) {
|
||||
auto result = mosquitto_cpp::connect_impl(bind_address, host, port, keepalive_seconds);
|
||||
if (result == ErrorCode::Success) {
|
||||
m_last_socket = socket();
|
||||
m_handler.register_event_handler(
|
||||
socket(), [this](auto const& events) { handle_socket(events); }, event::poll_events::read);
|
||||
listen_to_write_events_if_wanted();
|
||||
listen_to_sync_timer(true);
|
||||
} else {
|
||||
listen_to_reconnect_timer(true);
|
||||
listen_to_sync_timer(false);
|
||||
}
|
||||
return handle_error(result);
|
||||
}
|
||||
|
||||
ErrorCode mqtt_client::set_will_impl(const std::string_view& topic, const std::string_view& payload, QoS qos,
|
||||
bool retain, PropertiesBase&& props) {
|
||||
auto result = mosquitto_cpp::set_will_impl(topic, payload, qos, retain, std::move(props));
|
||||
listen_to_write_events_if_wanted();
|
||||
return handle_error(result);
|
||||
}
|
||||
|
||||
ErrorCode mqtt_client::publish_impl(int* mid, const std::string_view& topic, const std::string_view& payload, QoS qos,
|
||||
bool retain, const PropertiesBase& props) {
|
||||
auto result = ErrorCode::NoConnection;
|
||||
if (m_connected) {
|
||||
result = mosquitto_cpp::publish_impl(mid, topic, payload, qos, retain, props);
|
||||
listen_to_write_events_if_wanted();
|
||||
handle_error(result);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
ErrorCode mqtt_client::subscribe_impl(const std::string_view& topic, QoS qos, int options, const PropertiesBase& props,
|
||||
subscribe_callback cb) {
|
||||
auto result = mosquitto_cpp::subscribe_impl(topic, qos, options, props, std::move(cb));
|
||||
listen_to_write_events_if_wanted();
|
||||
return handle_error(result);
|
||||
}
|
||||
|
||||
ErrorCode mqtt_client::unsubscribe_impl(const std::string_view& topic, const PropertiesBase& props) {
|
||||
auto result = mosquitto_cpp::unsubscribe_impl(topic, props);
|
||||
listen_to_write_events_if_wanted();
|
||||
return handle_error(result);
|
||||
}
|
||||
|
||||
void mqtt_client::set_callback_connect_impl(connect_callback cb) {
|
||||
mosquitto_cpp::set_callback_connect_impl(
|
||||
[this, cb = std::move(cb)](auto& cb_client, auto rc, auto flags, auto const& props) {
|
||||
m_connected = true;
|
||||
if (cb) {
|
||||
cb(cb_client, rc, flags, props);
|
||||
}
|
||||
handle_error(ErrorCode::Success);
|
||||
});
|
||||
}
|
||||
|
||||
void mqtt_client::set_callback_disconnect_impl(disconnect_callback cb) {
|
||||
mosquitto_cpp::set_callback_disconnect_impl(
|
||||
[this, cb = std::move(cb)](auto& cb_client, auto ec, auto const& props) {
|
||||
m_connected = false;
|
||||
listen_to_reconnect_timer(true);
|
||||
if (cb) {
|
||||
cb(cb_client, ec, props);
|
||||
}
|
||||
handle_error(ec);
|
||||
});
|
||||
}
|
||||
|
||||
void mqtt_client::set_callback_publish_impl(publish_callback cb) {
|
||||
mosquitto_cpp::set_callback_publish_impl(cb);
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::mqtt
|
||||
@@ -0,0 +1,247 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2025 Pionix GmbH and Contributors to EVerest
|
||||
|
||||
// clang-format off
|
||||
#include <everest/io/netlink/vcan_netlink_manager.hpp>
|
||||
#include <linux/netlink.h>
|
||||
#include <cstring>
|
||||
#include <exception>
|
||||
#include <iostream>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <net/if.h>
|
||||
#include <linux/if_arp.h>
|
||||
#include <linux/rtnetlink.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/time.h>
|
||||
#include <unistd.h>
|
||||
// clang-format on
|
||||
|
||||
namespace everest::lib::io::netlink {
|
||||
|
||||
struct vcan_netlink_manager::NetlinkMessage {
|
||||
struct nlmsghdr header;
|
||||
char buffer[4096];
|
||||
};
|
||||
|
||||
void vcan_netlink_manager::add_attribute(nlmsghdr* nlh, int maxlen, int type, const void* data, int len) {
|
||||
int attrlen = RTA_LENGTH(len);
|
||||
if (NLMSG_ALIGN(nlh->nlmsg_len) + attrlen > static_cast<uint32_t>(maxlen)) {
|
||||
throw std::runtime_error("Netlink attribute too long for message buffer.");
|
||||
}
|
||||
struct rtattr* rta = (struct rtattr*)(((char*)nlh) + NLMSG_ALIGN(nlh->nlmsg_len));
|
||||
rta->rta_type = type;
|
||||
rta->rta_len = attrlen;
|
||||
memcpy(RTA_DATA(rta), data, len);
|
||||
nlh->nlmsg_len = NLMSG_ALIGN(nlh->nlmsg_len) + attrlen;
|
||||
}
|
||||
|
||||
vcan_netlink_manager::vcan_netlink_manager() {
|
||||
m_nl_socket_fd = socket(AF_NETLINK, SOCK_RAW, NETLINK_ROUTE);
|
||||
if (m_nl_socket_fd < 0) {
|
||||
throw std::runtime_error("Failed to create shared Netlink socket: " + std::string(strerror(errno)));
|
||||
}
|
||||
|
||||
struct timeval tv;
|
||||
tv.tv_sec = 5;
|
||||
tv.tv_usec = 0;
|
||||
if (setsockopt(m_nl_socket_fd, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof(tv)) < 0) {
|
||||
::close(m_nl_socket_fd);
|
||||
m_nl_socket_fd = -1;
|
||||
throw std::runtime_error("Could not set receive timeout on shared Netlink socket: " +
|
||||
std::string(strerror(errno)));
|
||||
}
|
||||
}
|
||||
|
||||
vcan_netlink_manager::~vcan_netlink_manager() {
|
||||
if (m_nl_socket_fd != -1) {
|
||||
::close(m_nl_socket_fd);
|
||||
m_nl_socket_fd = -1;
|
||||
}
|
||||
}
|
||||
|
||||
vcan_netlink_manager& vcan_netlink_manager::Instance() {
|
||||
static vcan_netlink_manager obj;
|
||||
return obj;
|
||||
}
|
||||
|
||||
void vcan_netlink_manager::send_message(NetlinkMessage const& msg, int flags) {
|
||||
iovec iov;
|
||||
iov.iov_base = (void*)&msg.header;
|
||||
iov.iov_len = msg.header.nlmsg_len;
|
||||
|
||||
msghdr message;
|
||||
message.msg_name = nullptr;
|
||||
message.msg_namelen = 0;
|
||||
message.msg_iov = &iov;
|
||||
message.msg_iovlen = 1;
|
||||
message.msg_control = nullptr;
|
||||
message.msg_controllen = 0;
|
||||
message.msg_flags = 0;
|
||||
|
||||
ssize_t bytes_sent = sendmsg(m_nl_socket_fd, &message, flags);
|
||||
if (bytes_sent < 0) {
|
||||
throw std::runtime_error("Failed to send Netlink message: " + std::string(strerror(errno)));
|
||||
}
|
||||
if (static_cast<std::uint32_t>(bytes_sent) != msg.header.nlmsg_len) {
|
||||
throw std::runtime_error("Partial Netlink message sent. Expected " + std::to_string(msg.header.nlmsg_len) +
|
||||
" bytes, sent " + std::to_string(bytes_sent));
|
||||
}
|
||||
}
|
||||
|
||||
void vcan_netlink_manager::receive_message(NetlinkMessage& msg) {
|
||||
iovec iov;
|
||||
iov.iov_base = (void*)&msg.header;
|
||||
iov.iov_len = sizeof(msg);
|
||||
|
||||
msghdr message;
|
||||
message.msg_name = nullptr;
|
||||
message.msg_namelen = 0;
|
||||
message.msg_iov = &iov;
|
||||
message.msg_iovlen = 1;
|
||||
message.msg_control = nullptr;
|
||||
message.msg_controllen = 0;
|
||||
message.msg_flags = 0;
|
||||
|
||||
ssize_t bytes_received = recvmsg(m_nl_socket_fd, &message, 0); // Use shared FD
|
||||
if (bytes_received < 0) {
|
||||
throw std::runtime_error("Failed to receive Netlink message: " + std::string(strerror(errno)));
|
||||
}
|
||||
if (bytes_received == 0) {
|
||||
throw std::runtime_error("Netlink socket closed during receive.");
|
||||
}
|
||||
if (bytes_received < static_cast<int>(sizeof(struct nlmsghdr))) {
|
||||
throw std::runtime_error("Incomplete Netlink message header.");
|
||||
}
|
||||
}
|
||||
|
||||
void vcan_netlink_manager::send_netlink_request_impl(int msg_type, int flags, cb_type const& callback) {
|
||||
NetlinkMessage req;
|
||||
memset(&req, 0, sizeof(req));
|
||||
|
||||
req.header.nlmsg_len = NLMSG_LENGTH(sizeof(struct ifinfomsg));
|
||||
req.header.nlmsg_type = msg_type;
|
||||
req.header.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK | flags;
|
||||
req.header.nlmsg_seq = ++s_netlink_seq_counter;
|
||||
req.header.nlmsg_pid = getpid();
|
||||
|
||||
struct ifinfomsg* ifi = (struct ifinfomsg*)NLMSG_DATA(&req.header);
|
||||
ifi->ifi_family = AF_UNSPEC;
|
||||
ifi->ifi_index = 0;
|
||||
ifi->ifi_flags = 0;
|
||||
ifi->ifi_change = 0xFFFFFFFF;
|
||||
|
||||
callback(&req.header, ifi, sizeof(req.buffer));
|
||||
send_message(req);
|
||||
|
||||
NetlinkMessage response;
|
||||
bool ack_received = false;
|
||||
int max_recv_attempts = 10;
|
||||
|
||||
for (int i = 0; i < max_recv_attempts; ++i) {
|
||||
ssize_t bytes_received = 0;
|
||||
try {
|
||||
memset(&response, 0, sizeof(response));
|
||||
receive_message(response);
|
||||
bytes_received = response.header.nlmsg_len;
|
||||
} catch (const std::runtime_error& e) {
|
||||
continue;
|
||||
}
|
||||
|
||||
for (struct nlmsghdr* nlh = &response.header; NLMSG_OK(nlh, bytes_received);
|
||||
nlh = NLMSG_NEXT(nlh, bytes_received)) {
|
||||
if (static_cast<int>(nlh->nlmsg_pid) != getpid() || nlh->nlmsg_seq != req.header.nlmsg_seq) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (nlh->nlmsg_type == NLMSG_ERROR) {
|
||||
struct nlmsgerr* err = (struct nlmsgerr*)NLMSG_DATA(nlh);
|
||||
if (err->error == 0) {
|
||||
ack_received = true;
|
||||
return;
|
||||
} else {
|
||||
throw std::runtime_error(std::string(strerror(-err->error)) +
|
||||
" (error code: " + std::to_string(err->error) + ")");
|
||||
}
|
||||
} else if (nlh->nlmsg_type == msg_type) {
|
||||
ack_received = true;
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!ack_received) {
|
||||
throw std::runtime_error("No valid ACK");
|
||||
}
|
||||
}
|
||||
|
||||
bool vcan_netlink_manager::send_netlink_request(int msg_type, int flags, cb_type const& callback,
|
||||
std::string const& interface_name, std::string const& caller) {
|
||||
try {
|
||||
send_netlink_request_impl(msg_type, flags, callback);
|
||||
return true;
|
||||
} catch (std::exception& e) {
|
||||
std::cerr << "[VCAN Netlink] (" << interface_name << ") '" << caller << "' -> " << e.what() << std::endl;
|
||||
|
||||
} catch (...) {
|
||||
std::cerr << "[VCAN Netlink] (" << interface_name << ") '" << caller << "' -> Unexpected exception"
|
||||
<< std::endl;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool vcan_netlink_manager::create(std::string const& interface_name) {
|
||||
return send_netlink_request(
|
||||
RTM_NEWLINK, NLM_F_CREATE | NLM_F_EXCL,
|
||||
[&](struct nlmsghdr* nlh, struct ifinfomsg* ifi, int max_buf_len) {
|
||||
ifi->ifi_type = AF_NETROM;
|
||||
|
||||
add_attribute(nlh, max_buf_len, IFLA_IFNAME, interface_name.c_str(), interface_name.length() + 1);
|
||||
|
||||
struct rtattr* linkinfo_attr = (struct rtattr*)(((char*)nlh) + NLMSG_ALIGN(nlh->nlmsg_len));
|
||||
linkinfo_attr->rta_type = IFLA_LINKINFO;
|
||||
linkinfo_attr->rta_len = RTA_LENGTH(0);
|
||||
|
||||
nlh->nlmsg_len = NLMSG_ALIGN(nlh->nlmsg_len) + RTA_ALIGN(RTA_LENGTH(0));
|
||||
|
||||
const char* vcan_kind = "vcan";
|
||||
add_attribute(nlh, max_buf_len, IFLA_INFO_KIND, vcan_kind, strlen(vcan_kind) + 1);
|
||||
|
||||
linkinfo_attr->rta_len = (char*)nlh + nlh->nlmsg_len - (char*)linkinfo_attr;
|
||||
},
|
||||
interface_name, "create");
|
||||
}
|
||||
|
||||
bool vcan_netlink_manager::bring_up(std::string const& interface_name) {
|
||||
return send_netlink_request(
|
||||
RTM_NEWLINK, 0,
|
||||
[&](struct nlmsghdr* nlh, struct ifinfomsg* ifi, int max_buf_len) {
|
||||
add_attribute(nlh, max_buf_len, IFLA_IFNAME, interface_name.c_str(), interface_name.length() + 1);
|
||||
ifi->ifi_flags = IFF_UP;
|
||||
ifi->ifi_change = IFF_UP;
|
||||
},
|
||||
interface_name, "bringUp");
|
||||
}
|
||||
|
||||
bool vcan_netlink_manager::bring_down(std::string const& interface_name) {
|
||||
return send_netlink_request(
|
||||
RTM_NEWLINK, 0,
|
||||
[&](struct nlmsghdr* nlh, struct ifinfomsg* ifi, int max_buf_len) {
|
||||
add_attribute(nlh, max_buf_len, IFLA_IFNAME, interface_name.c_str(), interface_name.length() + 1);
|
||||
ifi->ifi_flags = 0;
|
||||
ifi->ifi_change = IFF_UP;
|
||||
},
|
||||
interface_name, "bringDown");
|
||||
}
|
||||
|
||||
bool vcan_netlink_manager::destroy(std::string const& interface_name) {
|
||||
return send_netlink_request(
|
||||
RTM_DELLINK, 0,
|
||||
[&](struct nlmsghdr* nlh, struct ifinfomsg*, int max_buf_len) {
|
||||
add_attribute(nlh, max_buf_len, IFLA_IFNAME, interface_name.c_str(), interface_name.length() + 1);
|
||||
},
|
||||
interface_name, "destroy");
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::netlink
|
||||
70
tools/EVerest-main/lib/everest/io/src/raw/raw_socket.cpp
Normal file
70
tools/EVerest-main/lib/everest/io/src/raw/raw_socket.cpp
Normal file
@@ -0,0 +1,70 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2026 Pionix GmbH and Contributors to EVerest
|
||||
#include <chrono>
|
||||
#include <everest/io/raw/raw_socket.hpp>
|
||||
#include <everest/io/socket/socket.hpp>
|
||||
#include <exception>
|
||||
#include <netdb.h>
|
||||
#include <thread>
|
||||
|
||||
namespace everest::lib::io::raw {
|
||||
|
||||
bool raw_socket::open(std::string const& if_name) {
|
||||
try {
|
||||
auto socket = socket::open_raw_promiscuous_socket(if_name);
|
||||
socket::set_non_blocking(socket);
|
||||
m_fd = std::move(socket);
|
||||
return socket::get_pending_error(m_fd) == 0;
|
||||
} catch (...) {
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool raw_socket::tx(PayloadT& payload) {
|
||||
if (not is_open()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
auto status = ::send(m_fd, payload.data(), payload.size(), 0);
|
||||
if (status == -1) {
|
||||
return false;
|
||||
}
|
||||
if (status < static_cast<ssize_t>(payload.size())) {
|
||||
// We have a reference to the current data. Replace it with what is left to be written
|
||||
// and return false. This signals the current block cannot be removed from the buffer.
|
||||
payload = {payload.begin() + status, payload.end()};
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool raw_socket::rx(PayloadT& buffer) {
|
||||
if (not is_open()) {
|
||||
return false;
|
||||
}
|
||||
buffer.resize(default_buffer_size);
|
||||
auto status = ::recv(m_fd, buffer.data(), buffer.size(), 0);
|
||||
if (status <= 0) { // -1 is an error, 0 is a connection closed by the peer
|
||||
return false;
|
||||
}
|
||||
buffer.resize(status);
|
||||
return true;
|
||||
}
|
||||
|
||||
int raw_socket::get_fd() const {
|
||||
return m_fd;
|
||||
}
|
||||
|
||||
int raw_socket::get_error() const {
|
||||
return socket::get_pending_error(m_fd);
|
||||
}
|
||||
|
||||
bool raw_socket::is_open() const {
|
||||
return m_fd.is_fd();
|
||||
}
|
||||
|
||||
void raw_socket::close() {
|
||||
m_fd.close();
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::raw
|
||||
37
tools/EVerest-main/lib/everest/io/src/serial/event_pty.cpp
Normal file
37
tools/EVerest-main/lib/everest/io/src/serial/event_pty.cpp
Normal file
@@ -0,0 +1,37 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2025 Pionix GmbH and Contributors to EVerest
|
||||
|
||||
#include <cstring>
|
||||
#include <everest/io/serial/event_pty.hpp>
|
||||
#include <sys/types.h>
|
||||
#include <termios.h>
|
||||
#include <unistd.h>
|
||||
|
||||
namespace everest::lib::io::serial {
|
||||
|
||||
void event_pty::set_status_handler(cb_status const& handler) {
|
||||
m_status = handler;
|
||||
}
|
||||
|
||||
void event_pty::set_data_handler(cb_rx const& handler) {
|
||||
m_data = handler;
|
||||
set_rx_handler([this, handler](auto const& pl, auto& obj) {
|
||||
if (pl.size() > 0) {
|
||||
if (pl[0] == 0) {
|
||||
if (m_data) {
|
||||
m_data({pl.begin() + 1, pl.end()}, obj);
|
||||
}
|
||||
} else {
|
||||
if (m_status) {
|
||||
m_status(get_raw_handler()->get_status());
|
||||
}
|
||||
}
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
std::string event_pty::get_slave_path() {
|
||||
return get_raw_handler()->get_slave_path();
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::serial
|
||||
100
tools/EVerest-main/lib/everest/io/src/serial/pty_handler.cpp
Normal file
100
tools/EVerest-main/lib/everest/io/src/serial/pty_handler.cpp
Normal file
@@ -0,0 +1,100 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2025 Pionix GmbH and Contributors to EVerest
|
||||
|
||||
#include <cstring>
|
||||
#include <errno.h>
|
||||
#include <everest/io/serial/pty_handler.hpp>
|
||||
#include <iostream>
|
||||
#include <sys/types.h>
|
||||
#include <termios.h>
|
||||
#include <unistd.h>
|
||||
|
||||
namespace everest::lib::io::serial {
|
||||
|
||||
bool pty_handler::tx(PayloadT& data) {
|
||||
auto status = ::write(m_dev.master_fd, data.data(), data.size());
|
||||
if (status == -1) {
|
||||
error_id = errno;
|
||||
std::cout << "ERROR: Failed to write to pty master." << std::endl;
|
||||
return false;
|
||||
}
|
||||
if (status < static_cast<ssize_t>(data.size())) {
|
||||
// We have a reference to the current data. Replace it with what is left to be written
|
||||
// and return false. This signals the current block cannot be removed from the buffer.
|
||||
data = {data.begin() + status, data.end()};
|
||||
return false;
|
||||
}
|
||||
error_id = 0;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool pty_handler::rx(PayloadT& data) {
|
||||
// This should not be expensive, since capacity is only touched once, since
|
||||
// data is expected to stay the same object during the livetime of this instance
|
||||
data.resize(buffer_size_limit);
|
||||
auto n_bytes = ::read(m_dev.master_fd, data.data(), data.size());
|
||||
if (n_bytes == -1) {
|
||||
error_id = errno;
|
||||
return false;
|
||||
}
|
||||
data.resize(n_bytes);
|
||||
error_id = 0;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool pty_handler::open() {
|
||||
auto mypty = serial::openpty();
|
||||
if (not mypty.has_value()) {
|
||||
error_id = errno;
|
||||
return false;
|
||||
}
|
||||
|
||||
auto aware = serial::make_pty_mode_aware(mypty.value());
|
||||
if (not aware) {
|
||||
std::cout << "ERROR: Preparing pty for packet mode and extproc" << std::endl;
|
||||
error_id = errno;
|
||||
return false;
|
||||
}
|
||||
auto binary = serial::set_binary_mode(mypty->slave_fd);
|
||||
if (not binary) {
|
||||
std::cout << "ERROR: Preparing pty for binary mode" << std::endl;
|
||||
error_id = errno;
|
||||
return false;
|
||||
}
|
||||
m_dev = std::move(mypty.value());
|
||||
error_id = 0;
|
||||
return true;
|
||||
}
|
||||
|
||||
int pty_handler::get_fd() const {
|
||||
return m_dev.master_fd;
|
||||
}
|
||||
|
||||
pty_status pty_handler::get_status() {
|
||||
pty_status result;
|
||||
struct termios status;
|
||||
auto attr_res = tcgetattr(m_dev.master_fd, &status);
|
||||
if (attr_res == -1) {
|
||||
error_id = errno;
|
||||
std::cout << "Failed to get attributes" << std::endl;
|
||||
return result;
|
||||
}
|
||||
|
||||
result.ixon = status.c_iflag & IXON; // enable xon/xoff flow control on output
|
||||
result.ixoff = status.c_iflag & IXOFF; // enable xon/xoff flow control on input
|
||||
result.cstopb = status.c_cflag & CSTOPB; // two stop bits instead of one
|
||||
result.cbaud = cfgetospeed(&status);
|
||||
|
||||
error_id = 0;
|
||||
return result;
|
||||
}
|
||||
|
||||
int pty_handler::get_error() const {
|
||||
return error_id;
|
||||
}
|
||||
|
||||
std::string pty_handler::get_slave_path() const {
|
||||
return m_dev.slave_path;
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::serial
|
||||
76
tools/EVerest-main/lib/everest/io/src/serial/serial.cpp
Normal file
76
tools/EVerest-main/lib/everest/io/src/serial/serial.cpp
Normal file
@@ -0,0 +1,76 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2025 Pionix GmbH and Contributors to EVerest
|
||||
#include <cstdlib>
|
||||
#include <everest/io/serial/serial.hpp>
|
||||
#include <fcntl.h>
|
||||
#include <optional>
|
||||
#include <stdlib.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <termios.h>
|
||||
#include <unistd.h>
|
||||
|
||||
namespace everest::lib::io::serial {
|
||||
|
||||
std::optional<pty> openpty() {
|
||||
pty result;
|
||||
|
||||
result.master_fd = event::unique_fd(::posix_openpt(O_RDWR | O_NOCTTY));
|
||||
if (not result.master_fd.is_fd()) {
|
||||
return std::nullopt;
|
||||
}
|
||||
if (::grantpt(result.master_fd) == -1) {
|
||||
return std::nullopt;
|
||||
}
|
||||
if (::unlockpt(result.master_fd) == -1) {
|
||||
return std::nullopt;
|
||||
}
|
||||
auto mb_name = ::ptsname(result.master_fd);
|
||||
if (mb_name == 0) {
|
||||
return std::nullopt;
|
||||
}
|
||||
result.slave_path = mb_name;
|
||||
result.slave_fd = event::unique_fd(::open(mb_name, 0));
|
||||
if (not result.slave_fd.is_fd()) {
|
||||
return std::nullopt;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
bool set_packet_mode(int fd) {
|
||||
int nonzero = 1;
|
||||
return ioctl(fd, TIOCPKT, &nonzero) == 0;
|
||||
}
|
||||
|
||||
bool set_extproc_flag(int fd) {
|
||||
struct termios tio;
|
||||
|
||||
if (tcgetattr(fd, &tio) == -1) {
|
||||
return false;
|
||||
}
|
||||
tio.c_lflag |= EXTPROC;
|
||||
|
||||
if (tcsetattr(fd, TCSANOW, &tio) == -1) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool set_binary_mode(int fd) {
|
||||
struct termios tty;
|
||||
if (tcgetattr(fd, &tty) < 0) {
|
||||
return false;
|
||||
}
|
||||
cfmakeraw(&tty);
|
||||
|
||||
if (tcsetattr(fd, TCSAFLUSH, &tty) < 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool make_pty_mode_aware(pty const& item) {
|
||||
return set_packet_mode(item.master_fd) && set_extproc_flag(item.slave_fd);
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::serial
|
||||
925
tools/EVerest-main/lib/everest/io/src/socket/socket.cpp
Normal file
925
tools/EVerest-main/lib/everest/io/src/socket/socket.cpp
Normal file
@@ -0,0 +1,925 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2026 Pionix GmbH and Contributors to EVerest
|
||||
|
||||
#include <algorithm>
|
||||
#include <arpa/inet.h>
|
||||
#include <asm-generic/socket.h>
|
||||
#include <chrono>
|
||||
#include <cstring>
|
||||
#include <fcntl.h>
|
||||
#include <ifaddrs.h>
|
||||
#include <iostream>
|
||||
#include <linux/if_packet.h>
|
||||
#include <linux/if_tun.h>
|
||||
#include <net/if.h>
|
||||
#include <netdb.h>
|
||||
#include <netinet/in.h>
|
||||
#include <netinet/tcp.h>
|
||||
#include <poll.h>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
#include <string.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/socket.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <everest/io/event/unique_fd.hpp>
|
||||
#include <everest/io/socket/socket.hpp>
|
||||
#include <everest/io/udp/endpoint.hpp>
|
||||
|
||||
namespace {
|
||||
// a simple raii wrapper, which will call the given c-like deleter
|
||||
// function on its supplied pointer at the end of its lifetime
|
||||
template <typename T, void (*deleter)(T*)> class handle_disposer {
|
||||
public:
|
||||
handle_disposer(T* pointer) : m_pointer(pointer){};
|
||||
|
||||
handle_disposer(const handle_disposer&) = delete;
|
||||
handle_disposer(handle_disposer&&) = delete;
|
||||
handle_disposer& operator=(const handle_disposer&) = delete;
|
||||
handle_disposer& operator=(handle_disposer&&) = delete;
|
||||
~handle_disposer() {
|
||||
deleter(m_pointer);
|
||||
}
|
||||
|
||||
private:
|
||||
T* m_pointer;
|
||||
};
|
||||
|
||||
long get_remaining_time_ms(std::chrono::steady_clock::time_point const& init_time, uint32_t timeout_ms) {
|
||||
auto time_delta = std::chrono::steady_clock::now() - init_time;
|
||||
auto time_delta_ms = std::chrono::duration_cast<std::chrono::milliseconds>(time_delta).count();
|
||||
auto remaining_ms = timeout_ms - time_delta_ms;
|
||||
return remaining_ms;
|
||||
}
|
||||
|
||||
void set_ifr_name(struct ifreq& ifr, std::string const& dev_name) {
|
||||
memset(&ifr, 0, sizeof(ifr));
|
||||
strncpy(ifr.ifr_name, dev_name.c_str(),
|
||||
std::min(dev_name.size(), static_cast<std::string::size_type>(IFNAMSIZ - 1)));
|
||||
ifr.ifr_name[IFNAMSIZ - 1] = '\0';
|
||||
}
|
||||
|
||||
std::string build_errno_string(std::string const& msg) {
|
||||
std::stringstream str;
|
||||
str << msg << ": " << strerror(errno) << " (" << errno << ")";
|
||||
return str.str();
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
namespace everest::lib::io {
|
||||
namespace socket {
|
||||
|
||||
//
|
||||
// socket.hpp implementations
|
||||
//
|
||||
|
||||
namespace {
|
||||
// Returns true when SO_BINDTODEVICE succeeded. Returns false on EPERM/EACCES
|
||||
// (caller decides on a fallback). Throws on any other failure.
|
||||
bool apply_so_bindtodevice(int fd, std::string const& device) {
|
||||
if (setsockopt(fd, SOL_SOCKET, SO_BINDTODEVICE, device.c_str(), device.length()) == 0) {
|
||||
return true;
|
||||
}
|
||||
if (errno == EPERM || errno == EACCES) {
|
||||
return false;
|
||||
}
|
||||
throw std::runtime_error(build_errno_string("Failed to bind socket to device " + device));
|
||||
}
|
||||
|
||||
// Returns the socket's address family via getsockname(). Returns AF_UNSPEC if unknown.
|
||||
sa_family_t get_socket_family(int fd) {
|
||||
sockaddr_storage ss{};
|
||||
socklen_t len = sizeof(ss);
|
||||
if (getsockname(fd, reinterpret_cast<struct sockaddr*>(&ss), &len) < 0) {
|
||||
return AF_UNSPEC;
|
||||
}
|
||||
return ss.ss_family;
|
||||
}
|
||||
|
||||
// Restrict the outgoing interface for unicast packets without requiring CAP_NET_RAW.
|
||||
// IPv4: IP_UNICAST_IF takes uint32_t (ifindex in network byte order via in_addr layout).
|
||||
// IPv6: IPV6_UNICAST_IF takes int (ifindex in host byte order).
|
||||
// Returns true on success; false if the socket family is neither AF_INET nor AF_INET6.
|
||||
// Throws if the interface lookup fails or if setsockopt fails with a non-permission error.
|
||||
bool apply_unicast_if(int fd, std::string const& device) {
|
||||
unsigned int ifindex = if_nametoindex(device.c_str());
|
||||
if (ifindex == 0) {
|
||||
throw std::runtime_error(build_errno_string("if_nametoindex(\"" + device + "\") failed"));
|
||||
}
|
||||
sa_family_t family = get_socket_family(fd);
|
||||
if (family == AF_INET) {
|
||||
std::uint32_t opt = htonl(ifindex);
|
||||
if (setsockopt(fd, IPPROTO_IP, IP_UNICAST_IF, &opt, sizeof(opt)) == 0) {
|
||||
return true;
|
||||
}
|
||||
throw std::runtime_error(build_errno_string("setsockopt(IP_UNICAST_IF, " + device + ") failed"));
|
||||
}
|
||||
if (family == AF_INET6) {
|
||||
int opt = static_cast<int>(ifindex);
|
||||
if (setsockopt(fd, IPPROTO_IPV6, IPV6_UNICAST_IF, &opt, sizeof(opt)) == 0) {
|
||||
return true;
|
||||
}
|
||||
throw std::runtime_error(build_errno_string("setsockopt(IPV6_UNICAST_IF, " + device + ") failed"));
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// Steer multicast egress to @p device. SO_BINDTODEVICE and IP[V6]_UNICAST_IF do not
|
||||
// affect the multicast TX path; the kernel needs IP_MULTICAST_IF / IPV6_MULTICAST_IF
|
||||
// independently. For IPv6 we additionally populate sin6_scope_id on the destination
|
||||
// sockaddr so connect()/sendto() can resolve scope bound groups (ff02::/16). For IPv4
|
||||
// we use ip_mreqn so the interface is selected by index, avoiding the need for the
|
||||
// iface IPv4 address up front (works even before DHCP has assigned one). Returns
|
||||
// without action when not a multicast destination or device is empty.
|
||||
void set_multicast_if(int fd, std::string const& device, struct sockaddr* ai_addr) {
|
||||
if (device.empty() || ai_addr == nullptr) {
|
||||
return;
|
||||
}
|
||||
if (ai_addr->sa_family == AF_INET6) {
|
||||
auto* sin6 = reinterpret_cast<sockaddr_in6*>(ai_addr);
|
||||
if (!IN6_IS_ADDR_MULTICAST(&sin6->sin6_addr)) {
|
||||
return;
|
||||
}
|
||||
unsigned int ifindex = if_nametoindex(device.c_str());
|
||||
if (ifindex == 0) {
|
||||
throw std::runtime_error(build_errno_string("if_nametoindex(\"" + device + "\") failed"));
|
||||
}
|
||||
if (setsockopt(fd, IPPROTO_IPV6, IPV6_MULTICAST_IF, &ifindex, sizeof(ifindex)) != 0) {
|
||||
throw std::runtime_error(build_errno_string("setsockopt(IPV6_MULTICAST_IF, " + device + ") failed"));
|
||||
}
|
||||
sin6->sin6_scope_id = ifindex;
|
||||
return;
|
||||
}
|
||||
if (ai_addr->sa_family == AF_INET) {
|
||||
auto* sin = reinterpret_cast<sockaddr_in*>(ai_addr);
|
||||
if (!IN_MULTICAST(ntohl(sin->sin_addr.s_addr))) {
|
||||
return;
|
||||
}
|
||||
unsigned int ifindex = if_nametoindex(device.c_str());
|
||||
if (ifindex == 0) {
|
||||
throw std::runtime_error(build_errno_string("if_nametoindex(\"" + device + "\") failed"));
|
||||
}
|
||||
struct ip_mreqn mreq {};
|
||||
mreq.imr_ifindex = static_cast<int>(ifindex);
|
||||
if (setsockopt(fd, IPPROTO_IP, IP_MULTICAST_IF, &mreq, sizeof(mreq)) != 0) {
|
||||
throw std::runtime_error(build_errno_string("setsockopt(IP_MULTICAST_IF, " + device + ") failed"));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// IPv4 source-IP bind to the address belonging to @p device. Used as a fallback
|
||||
// when SO_BINDTODEVICE is not permitted. @p port == 0 picks an ephemeral port.
|
||||
void bind_socket_to_interface_address(int fd, std::string const& device, std::uint16_t port) {
|
||||
std::string ip = get_interface_address(device);
|
||||
if (ip.empty()) {
|
||||
throw std::runtime_error("Cannot bind socket to device " + device +
|
||||
": no IPv4 address on interface (and SO_BINDTODEVICE not permitted)");
|
||||
}
|
||||
sockaddr_in local{};
|
||||
local.sin_family = AF_INET;
|
||||
local.sin_port = htons(port);
|
||||
if (inet_pton(AF_INET, ip.c_str(), &local.sin_addr) != 1) {
|
||||
throw std::runtime_error("Failed to parse interface address " + ip);
|
||||
}
|
||||
if (::bind(fd, reinterpret_cast<struct sockaddr*>(&local), sizeof(local)) < 0) {
|
||||
throw std::runtime_error(build_errno_string("Fallback bind to interface " + device + " (" + ip + ":" +
|
||||
std::to_string(port) + ") failed"));
|
||||
}
|
||||
}
|
||||
} // namespace
|
||||
|
||||
void bind_socket_to_device(int fd, std::string const& device) {
|
||||
if (device.empty()) {
|
||||
return;
|
||||
}
|
||||
if (apply_so_bindtodevice(fd, device)) {
|
||||
return;
|
||||
}
|
||||
// Try IP[_V6]_UNICAST_IF for outgoing unicast routing without privilege. Works for both
|
||||
// IPv4 and IPv6 client sockets.
|
||||
if (apply_unicast_if(fd, device)) {
|
||||
return;
|
||||
}
|
||||
// Last resort for IPv4 sockets with an unusable family: bind a source IP belonging to the
|
||||
// interface (IPv4 only).
|
||||
bind_socket_to_interface_address(fd, device, 0);
|
||||
}
|
||||
|
||||
event::unique_fd open_udp_server_socket(std::uint16_t port, std::string const& device) {
|
||||
struct addrinfo hints {};
|
||||
hints.ai_family = AF_UNSPEC;
|
||||
hints.ai_socktype = SOCK_DGRAM;
|
||||
hints.ai_flags = AI_PASSIVE;
|
||||
|
||||
addrinfo* servinfo;
|
||||
|
||||
const auto err = getaddrinfo(nullptr, std::to_string(port).c_str(), &hints, &servinfo);
|
||||
if (err) {
|
||||
throw std::runtime_error("Failed to resolve endpoint (getaddrinfo): " + std::string(gai_strerror(err)));
|
||||
};
|
||||
|
||||
handle_disposer<addrinfo, freeaddrinfo> addrinfo_disposer(servinfo);
|
||||
|
||||
// open the first possible socket
|
||||
for (auto* p = servinfo; p != NULL; p = p->ai_next) {
|
||||
auto socket_fd = event::unique_fd(::socket(p->ai_family, p->ai_socktype, p->ai_protocol));
|
||||
if (socket_fd == -1) {
|
||||
continue;
|
||||
}
|
||||
set_reuse_address(socket_fd);
|
||||
|
||||
if (!device.empty()) {
|
||||
if (!apply_so_bindtodevice(socket_fd, device)) {
|
||||
// SO_BINDTODEVICE not permitted: bind to (interface_ip:port) instead of wildcard,
|
||||
// so the server only receives traffic addressed to that interface's IPv4.
|
||||
bind_socket_to_interface_address(socket_fd, device, port);
|
||||
return socket_fd;
|
||||
}
|
||||
}
|
||||
|
||||
if (bind(socket_fd, p->ai_addr, p->ai_addrlen) == -1) {
|
||||
continue;
|
||||
}
|
||||
|
||||
return socket_fd;
|
||||
}
|
||||
throw std::runtime_error(std::string("Could not open a socket for localhost:") + std::to_string(port));
|
||||
}
|
||||
|
||||
event::unique_fd open_udp_client_socket(std::string const& host, std::uint16_t port, std::string const& device) {
|
||||
struct addrinfo hints {};
|
||||
hints.ai_family = AF_UNSPEC;
|
||||
hints.ai_socktype = SOCK_DGRAM;
|
||||
|
||||
addrinfo* servinfo;
|
||||
|
||||
const auto err = getaddrinfo(host.c_str(), std::to_string(port).c_str(), &hints, &servinfo);
|
||||
if (err) {
|
||||
throw std::runtime_error("Failed to resolve endpoint (getaddrinfo): " + std::string(gai_strerror(err)));
|
||||
};
|
||||
|
||||
handle_disposer<addrinfo, freeaddrinfo> addrinfo_disposer(servinfo);
|
||||
|
||||
// open the first possible socket
|
||||
for (auto* p = servinfo; p != NULL; p = p->ai_next) {
|
||||
const auto socket_fd = ::socket(p->ai_family, p->ai_socktype, p->ai_protocol);
|
||||
|
||||
if (socket_fd == -1)
|
||||
continue;
|
||||
|
||||
try {
|
||||
bind_socket_to_device(socket_fd, device);
|
||||
set_multicast_if(socket_fd, device, p->ai_addr);
|
||||
} catch (...) {
|
||||
close(socket_fd);
|
||||
throw;
|
||||
}
|
||||
|
||||
if (-1 == ::connect(socket_fd, p->ai_addr, p->ai_addrlen)) {
|
||||
close(socket_fd);
|
||||
continue;
|
||||
}
|
||||
|
||||
return event::unique_fd{socket_fd};
|
||||
}
|
||||
|
||||
throw std::runtime_error(std::string("Could not open a socket for ") + host + ":" + std::to_string(port));
|
||||
}
|
||||
|
||||
event::unique_fd open_tcp_socket_with_timeout(const std::string& host, std::uint16_t port, unsigned int timeout_ms,
|
||||
const std::string& device) {
|
||||
struct addrinfo hints {};
|
||||
hints.ai_family = AF_UNSPEC;
|
||||
hints.ai_socktype = SOCK_STREAM;
|
||||
|
||||
addrinfo* servinfo;
|
||||
|
||||
const auto err = getaddrinfo(host.c_str(), std::to_string(port).c_str(), &hints, &servinfo);
|
||||
if (err) {
|
||||
throw std::runtime_error("Failed to resolve endpoint (getaddrinfo): " + std::string(gai_strerror(err)));
|
||||
};
|
||||
|
||||
handle_disposer<addrinfo, freeaddrinfo> addrinfo_disposer(servinfo);
|
||||
|
||||
// open the first possible socket
|
||||
for (auto* p = servinfo; p != NULL; p = p->ai_next) {
|
||||
const auto socket_fd = ::socket(p->ai_family, p->ai_socktype, p->ai_protocol);
|
||||
|
||||
if (socket_fd == -1)
|
||||
continue;
|
||||
|
||||
try {
|
||||
bind_socket_to_device(socket_fd, device);
|
||||
} catch (...) {
|
||||
close(socket_fd);
|
||||
throw;
|
||||
}
|
||||
|
||||
if (-1 == connect_with_timeout(socket_fd, p->ai_addr, p->ai_addrlen, timeout_ms)) {
|
||||
close(socket_fd);
|
||||
continue;
|
||||
}
|
||||
|
||||
return event::unique_fd{socket_fd};
|
||||
}
|
||||
|
||||
throw std::runtime_error(std::string("Could not open a socket for ") + host + ":" + std::to_string(port));
|
||||
}
|
||||
|
||||
event::unique_fd open_tcp_socket(const std::string& host, std::uint16_t port, const std::string& device) {
|
||||
struct addrinfo hints {};
|
||||
hints.ai_family = AF_UNSPEC;
|
||||
hints.ai_socktype = SOCK_STREAM;
|
||||
|
||||
addrinfo* servinfo;
|
||||
|
||||
const auto err = getaddrinfo(host.c_str(), std::to_string(port).c_str(), &hints, &servinfo);
|
||||
if (err) {
|
||||
throw std::runtime_error("Failed to resolve endpoint (getaddrinfo): " + std::string(gai_strerror(err)));
|
||||
};
|
||||
|
||||
handle_disposer<addrinfo, freeaddrinfo> addrinfo_disposer(servinfo);
|
||||
|
||||
// open the first possible socket
|
||||
for (auto* p = servinfo; p != NULL; p = p->ai_next) {
|
||||
const auto socket_fd = ::socket(p->ai_family, p->ai_socktype, p->ai_protocol);
|
||||
|
||||
if (socket_fd == -1)
|
||||
continue;
|
||||
|
||||
try {
|
||||
bind_socket_to_device(socket_fd, device);
|
||||
} catch (...) {
|
||||
close(socket_fd);
|
||||
throw;
|
||||
}
|
||||
|
||||
if (-1 == connect(socket_fd, p->ai_addr, p->ai_addrlen)) {
|
||||
close(socket_fd);
|
||||
continue;
|
||||
}
|
||||
|
||||
return event::unique_fd{socket_fd};
|
||||
}
|
||||
|
||||
throw std::runtime_error(std::string("Could not open a socket for ") + host + ":" + std::to_string(port));
|
||||
}
|
||||
|
||||
#ifndef EVEREST_NO_PACKET_IGNORE_OUTGOING
|
||||
event::unique_fd open_raw_promiscuous_socket(std::string const& if_name) {
|
||||
auto const socket_fd = ::socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
|
||||
if (socket_fd == -1) {
|
||||
auto msg = build_errno_string("Could not open raw socket");
|
||||
throw std::runtime_error(msg);
|
||||
}
|
||||
|
||||
int const if_index = if_nametoindex(if_name.c_str());
|
||||
if (if_index == 0) {
|
||||
close(socket_fd);
|
||||
throw std::runtime_error("Invalid interface name: " + if_name);
|
||||
}
|
||||
|
||||
int ignore_out = 1;
|
||||
if (setsockopt(socket_fd, SOL_PACKET, PACKET_IGNORE_OUTGOING, &ignore_out, sizeof(ignore_out)) == -1) {
|
||||
auto msg = build_errno_string("Could not set PACKET_IGNORE_OUTGOING for " + if_name);
|
||||
close(socket_fd);
|
||||
throw std::runtime_error(msg);
|
||||
}
|
||||
|
||||
struct sockaddr_ll sll;
|
||||
std::memset(&sll, 0, sizeof(sll));
|
||||
sll.sll_family = AF_PACKET;
|
||||
sll.sll_ifindex = if_index;
|
||||
sll.sll_protocol = htons(ETH_P_ALL);
|
||||
|
||||
if (::bind(socket_fd, (struct sockaddr*)&sll, sizeof(sll)) == -1) {
|
||||
auto msg = build_errno_string("Could not bind socket to " + if_name);
|
||||
close(socket_fd);
|
||||
throw std::runtime_error(msg);
|
||||
}
|
||||
|
||||
struct packet_mreq mreq;
|
||||
std::memset(&mreq, 0, sizeof(mreq));
|
||||
mreq.mr_ifindex = if_nametoindex(if_name.c_str());
|
||||
mreq.mr_type = PACKET_MR_PROMISC;
|
||||
auto error = setsockopt(socket_fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
|
||||
if (error) {
|
||||
auto msg = build_errno_string("Cannot set 'PROMISCUOUS MODE' for '" + if_name + "'");
|
||||
close(socket_fd);
|
||||
throw std::runtime_error(msg);
|
||||
}
|
||||
return event::unique_fd(socket_fd);
|
||||
}
|
||||
#endif
|
||||
|
||||
void enable_tcp_no_delay(int fd) {
|
||||
socklen_t enable = 1;
|
||||
auto err = setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &enable, sizeof(enable));
|
||||
|
||||
if (err) {
|
||||
throw std::runtime_error("Failed to setsockopt(TCP_NODELAY)");
|
||||
}
|
||||
}
|
||||
|
||||
void set_non_blocking(int fd) {
|
||||
const auto err = fcntl(fd, F_SETFL, fcntl(fd, F_GETFL) | O_NONBLOCK);
|
||||
|
||||
if (err) {
|
||||
throw std::runtime_error("Failed to fcntl(O_NONBLOCK)");
|
||||
}
|
||||
}
|
||||
|
||||
void set_keepalive(int fd, bool enable) {
|
||||
socklen_t value = static_cast<int>(enable);
|
||||
auto err = setsockopt(fd, SOL_SOCKET, SO_KEEPALIVE, &value, sizeof(value));
|
||||
|
||||
if (err) {
|
||||
throw std::runtime_error("Failed to setsockopt(SO_KEEPALIVE)");
|
||||
}
|
||||
}
|
||||
|
||||
void set_tcp_keepalive(int fd, uint32_t count, uint32_t idle_s, uint32_t intval_s) {
|
||||
set_keepalive(fd, true);
|
||||
auto err = setsockopt(fd, IPPROTO_TCP, TCP_KEEPCNT, &count, sizeof(count));
|
||||
if (err) {
|
||||
throw std::runtime_error("Failed to setsockopt(TCP_KEEPCNT)");
|
||||
}
|
||||
err = setsockopt(fd, IPPROTO_TCP, TCP_KEEPIDLE, &idle_s, sizeof(idle_s));
|
||||
if (err) {
|
||||
throw std::runtime_error("Failed to setsockopt(TCP_KEEPIDLE)");
|
||||
}
|
||||
err = setsockopt(fd, IPPROTO_TCP, TCP_KEEPINTVL, &intval_s, sizeof(intval_s));
|
||||
if (err) {
|
||||
throw std::runtime_error("Failed to setsockopt(TCP_INTVL)");
|
||||
}
|
||||
}
|
||||
|
||||
void set_tcp_user_timeout(int fd, uint32_t to_ms) {
|
||||
auto err = setsockopt(fd, IPPROTO_TCP, TCP_USER_TIMEOUT, &to_ms, sizeof(to_ms));
|
||||
|
||||
if (err) {
|
||||
throw std::runtime_error("Failed to setsockopt(TCP_USER_TIMEOUT)");
|
||||
}
|
||||
}
|
||||
|
||||
void set_socket_send_buffer_to_min(int fd) {
|
||||
int sndbuf = 0;
|
||||
auto err = setsockopt(fd, SOL_SOCKET, SO_SNDBUF, &sndbuf, sizeof(sndbuf));
|
||||
|
||||
if (err) {
|
||||
throw std::runtime_error("Failed to setsockopt(SO_SNDBUF)");
|
||||
}
|
||||
}
|
||||
|
||||
int get_pending_error(int fd) {
|
||||
int error = 0;
|
||||
socklen_t errlen = sizeof(error);
|
||||
if (getsockopt(fd, SOL_SOCKET, SO_ERROR, (void*)&error, &errlen)) {
|
||||
return errno;
|
||||
}
|
||||
return error;
|
||||
}
|
||||
|
||||
bool is_tcp_socket_alive(int sock_fd) {
|
||||
char buffer[1];
|
||||
ssize_t bytes_peeked = recv(sock_fd, buffer, sizeof(buffer), MSG_PEEK | MSG_DONTWAIT);
|
||||
|
||||
if (bytes_peeked == -1) {
|
||||
return errno == EAGAIN || errno == EWOULDBLOCK;
|
||||
}
|
||||
return bytes_peeked not_eq 0;
|
||||
}
|
||||
|
||||
void bring_device_up(int sock_fd, std::string const& dev_name) {
|
||||
struct ifreq ifr;
|
||||
set_ifr_name(ifr, dev_name);
|
||||
|
||||
if (ioctl(sock_fd, SIOCGIFFLAGS, &ifr) == -1) {
|
||||
throw std::runtime_error(std::string("Failed to get device flags for ") + dev_name + ": " + strerror(errno));
|
||||
}
|
||||
|
||||
ifr.ifr_flags |= IFF_UP;
|
||||
|
||||
if (ioctl(sock_fd, SIOCSIFFLAGS, &ifr) == -1) {
|
||||
throw std::runtime_error(std::string("Failed to bring device UP: ") + dev_name + ": " + strerror(errno));
|
||||
}
|
||||
}
|
||||
|
||||
void set_ip_address(int sock_fd, std::string const& dev_name, std::string const& ip_address_str) {
|
||||
struct ifreq ifr;
|
||||
set_ifr_name(ifr, dev_name);
|
||||
|
||||
struct sockaddr_in* sin = reinterpret_cast<struct sockaddr_in*>(&ifr.ifr_addr);
|
||||
sin->sin_family = AF_INET;
|
||||
|
||||
if (inet_pton(AF_INET, ip_address_str.c_str(), &sin->sin_addr) <= 0) {
|
||||
throw std::runtime_error(std::string("Invalid IP address format: ") + ip_address_str +
|
||||
" (Error: " + strerror(errno) + ")");
|
||||
}
|
||||
|
||||
if (ioctl(sock_fd, SIOCSIFADDR, &ifr) == -1) {
|
||||
throw std::runtime_error(std::string("Failed to set IP address ") + ip_address_str + " on " + dev_name + ": " +
|
||||
strerror(errno));
|
||||
}
|
||||
}
|
||||
|
||||
void set_netmask(int sock_fd, std::string const& dev_name, std::string const& netmask_str) {
|
||||
struct ifreq ifr;
|
||||
set_ifr_name(ifr, dev_name);
|
||||
|
||||
struct sockaddr_in* sin = reinterpret_cast<struct sockaddr_in*>(&ifr.ifr_netmask);
|
||||
sin->sin_family = AF_INET;
|
||||
|
||||
if (inet_pton(AF_INET, netmask_str.c_str(), &sin->sin_addr) <= 0) {
|
||||
throw std::runtime_error(std::string("Invalid netmask format: ") + netmask_str + " (Error: " + strerror(errno) +
|
||||
")");
|
||||
}
|
||||
|
||||
if (ioctl(sock_fd, SIOCSIFNETMASK, &ifr) == -1) {
|
||||
throw std::runtime_error(std::string("Failed to set netmask ") + netmask_str + " on " + dev_name + ": " +
|
||||
strerror(errno));
|
||||
}
|
||||
}
|
||||
|
||||
void set_mtu(int sock_fd, std::string const& dev_name, int mtu) {
|
||||
struct ifreq ifr;
|
||||
set_ifr_name(ifr, dev_name);
|
||||
ifr.ifr_mtu = mtu;
|
||||
|
||||
if (ioctl(sock_fd, SIOCSIFMTU, &ifr) == -1) {
|
||||
throw std::runtime_error(std::string("Failed to set MTU to ") + std::to_string(mtu) + " on " + dev_name + ": " +
|
||||
strerror(errno));
|
||||
}
|
||||
}
|
||||
|
||||
event::unique_fd create_tap_device(std::string const& desired_device_name) {
|
||||
event::unique_fd tap_fd(::open("/dev/net/tun", O_RDWR));
|
||||
if (!tap_fd.is_fd()) {
|
||||
throw std::runtime_error(std::string("Unable to open /dev/net/tun: ") + strerror(errno));
|
||||
}
|
||||
if (desired_device_name.empty()) {
|
||||
throw std::runtime_error("create_tap_device: No device name provided. ");
|
||||
}
|
||||
|
||||
struct ifreq ifr;
|
||||
set_ifr_name(ifr, desired_device_name);
|
||||
ifr.ifr_flags = IFF_TAP | IFF_NO_PI;
|
||||
|
||||
if (ioctl(tap_fd, TUNSETIFF, &ifr) == -1) {
|
||||
if (errno == EEXIST) {
|
||||
throw std::runtime_error(std::string("TAP device name is not available: '") + desired_device_name +
|
||||
"': " + strerror(errno));
|
||||
|
||||
} else {
|
||||
throw std::runtime_error(std::string("Unable to create TAP device '") + desired_device_name +
|
||||
"': " + strerror(errno));
|
||||
}
|
||||
}
|
||||
|
||||
return tap_fd;
|
||||
}
|
||||
|
||||
event::unique_fd open_control_socket() {
|
||||
event::unique_fd sock_fd(::socket(AF_INET, SOCK_DGRAM, 0));
|
||||
if (!sock_fd.is_fd()) {
|
||||
throw std::runtime_error(std::string("Unable to open control socket: ") + strerror(errno));
|
||||
}
|
||||
return sock_fd;
|
||||
}
|
||||
|
||||
bool configure_tap_device_properties(int tap_fd, std::string const& dev_name, std::string const& ip_addr_str,
|
||||
std::string const& netmask_str, int mtu) {
|
||||
event::unique_fd control_sock_fd = open_control_socket();
|
||||
|
||||
try {
|
||||
bring_device_up(control_sock_fd, dev_name);
|
||||
set_ip_address(control_sock_fd, dev_name, ip_addr_str);
|
||||
set_netmask(control_sock_fd, dev_name, netmask_str);
|
||||
set_mtu(control_sock_fd, dev_name, mtu);
|
||||
set_non_blocking(tap_fd);
|
||||
} catch (...) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
int get_socket_flags(int fd) {
|
||||
int result = fcntl(fd, F_GETFL, 0);
|
||||
if (result < 0) {
|
||||
throw std::runtime_error(std::string("Get flags for socket Error: ") + strerror(errno) + ")");
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
void set_socket_flags(int fd, int flags) {
|
||||
int result = fcntl(fd, F_SETFL, flags);
|
||||
if (result < 0) {
|
||||
throw std::runtime_error(std::string("Set flags for socket Error: ") + strerror(errno) + ")");
|
||||
}
|
||||
}
|
||||
|
||||
int poll_for_timeout_once(int fd, uint32_t timeout_ms) {
|
||||
struct pollfd pfd;
|
||||
pfd.fd = fd;
|
||||
pfd.events = POLLOUT;
|
||||
struct pollfd pfds[] = {pfd};
|
||||
return poll(pfds, 1, timeout_ms);
|
||||
}
|
||||
|
||||
int poll_until_timeout(int fd, uint32_t timeout_ms) {
|
||||
int result = 0;
|
||||
auto init_time = std::chrono::steady_clock::now();
|
||||
do {
|
||||
auto remaining_ms = get_remaining_time_ms(init_time, timeout_ms);
|
||||
if (remaining_ms < 0) {
|
||||
result = 0;
|
||||
break;
|
||||
}
|
||||
result = poll_for_timeout_once(fd, remaining_ms);
|
||||
if (result > 0) {
|
||||
errno = get_pending_error(fd);
|
||||
if (errno != 0) {
|
||||
result = -1;
|
||||
}
|
||||
}
|
||||
} while (result == -1 && errno == EINTR);
|
||||
if (result == 0) {
|
||||
errno = ETIMEDOUT;
|
||||
result = -1;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
int connect_with_timeout_impl(int sockfd, const struct sockaddr* addr, socklen_t addrlen, unsigned int timeout_ms) {
|
||||
int result = 0;
|
||||
int original_flags = get_socket_flags(sockfd);
|
||||
set_non_blocking(sockfd);
|
||||
if (connect(sockfd, addr, addrlen) < 0) {
|
||||
if ((errno != EWOULDBLOCK) && (errno != EINPROGRESS)) {
|
||||
result = -1;
|
||||
} else {
|
||||
result = poll_until_timeout(sockfd, timeout_ms);
|
||||
}
|
||||
}
|
||||
set_socket_flags(sockfd, original_flags);
|
||||
return result;
|
||||
}
|
||||
|
||||
int connect_with_timeout(int fd, const struct sockaddr* addr, uint32_t addrlen, unsigned int timeout_ms) {
|
||||
auto result = 0;
|
||||
try {
|
||||
result = connect_with_timeout_impl(fd, addr, addrlen, timeout_ms);
|
||||
} catch (...) {
|
||||
result = -1;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
std::string get_interface_address(std::string const& name) {
|
||||
if (name.empty()) {
|
||||
return "";
|
||||
}
|
||||
|
||||
struct ifaddrs* ifaddr{nullptr};
|
||||
struct ifaddrs* ifa{nullptr};
|
||||
if (getifaddrs(&ifaddr) == -1) {
|
||||
throw std::runtime_error("Cannot get interfaces: " + std::string(strerror(errno)));
|
||||
}
|
||||
|
||||
handle_disposer<ifaddrs, freeifaddrs> ifaddr_disposer(ifaddr);
|
||||
|
||||
for (ifa = ifaddr; ifa != nullptr; ifa = ifa->ifa_next) {
|
||||
if (ifa->ifa_addr == nullptr || ifa->ifa_addr->sa_family != AF_INET) {
|
||||
continue;
|
||||
}
|
||||
if (name == ifa->ifa_name) {
|
||||
char host[NI_MAXHOST];
|
||||
getnameinfo(ifa->ifa_addr, sizeof(struct sockaddr_in), host, NI_MAXHOST, NULL, 0, NI_NUMERICHOST);
|
||||
return host;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
throw std::runtime_error("Interface: " + name + " not found or has no IPv4");
|
||||
}
|
||||
|
||||
std::vector<if_info> get_all_interaces() {
|
||||
struct ifaddrs* ifaddr{nullptr};
|
||||
struct ifaddrs* ifa{nullptr};
|
||||
if (getifaddrs(&ifaddr) == -1) {
|
||||
throw std::runtime_error("Cannot get interfaces: " + std::string(strerror(errno)));
|
||||
}
|
||||
|
||||
handle_disposer<ifaddrs, freeifaddrs> ifaddr_disposer(ifaddr);
|
||||
|
||||
std::vector<if_info> interfaces;
|
||||
|
||||
for (ifa = ifaddr; ifa != nullptr; ifa = ifa->ifa_next) {
|
||||
if (ifa->ifa_addr == nullptr || ifa->ifa_addr->sa_family != AF_INET) {
|
||||
continue;
|
||||
}
|
||||
interfaces.push_back({ifa->ifa_name, get_interface_address(ifa->ifa_name)});
|
||||
}
|
||||
return interfaces;
|
||||
}
|
||||
|
||||
event::unique_fd open_udp_unconnected_socket(udp::endpoint const& target, std::string const& iface) {
|
||||
const auto family = target.family();
|
||||
if (family != AF_INET && family != AF_INET6) {
|
||||
throw std::runtime_error("open_udp_unconnected_socket: target has no usable address family");
|
||||
}
|
||||
|
||||
event::unique_fd sock(::socket(family, SOCK_DGRAM, 0));
|
||||
if (not sock.is_fd()) {
|
||||
throw std::runtime_error(build_errno_string("socket(SOCK_DGRAM) failed"));
|
||||
}
|
||||
set_non_blocking(sock);
|
||||
|
||||
// Wildcard bind on the target's family, ephemeral port. RX is by destination
|
||||
// address and port; interface confinement (if any) comes from the options below.
|
||||
if (family == AF_INET6) {
|
||||
sockaddr_in6 local{};
|
||||
local.sin6_family = AF_INET6;
|
||||
local.sin6_addr = in6addr_any;
|
||||
if (::bind(sock, reinterpret_cast<sockaddr*>(&local), sizeof(local)) < 0) {
|
||||
throw std::runtime_error(build_errno_string("bind([::]:0) failed"));
|
||||
}
|
||||
} else {
|
||||
sockaddr_in local{};
|
||||
local.sin_family = AF_INET;
|
||||
local.sin_addr.s_addr = htonl(INADDR_ANY);
|
||||
if (::bind(sock, reinterpret_cast<sockaddr*>(&local), sizeof(local)) < 0) {
|
||||
throw std::runtime_error(build_errno_string("bind(0.0.0.0:0) failed"));
|
||||
}
|
||||
}
|
||||
|
||||
const std::string& device = iface.empty() ? target.iface() : iface;
|
||||
|
||||
// Best-effort link confinement. Not required for the unicast-reply RX path
|
||||
// (delivery is by destination address and port on the wildcard bind).
|
||||
// Defense-in-depth only on multi-NIC hosts; unprivileged failure is tolerated.
|
||||
if (not device.empty()) {
|
||||
try {
|
||||
bind_socket_to_device(sock, device);
|
||||
} catch (const std::exception&) {
|
||||
// ignore: multicast TX interface is pinned by set_multicast_if below
|
||||
}
|
||||
}
|
||||
|
||||
// Pin multicast egress to the chosen interface (no-op for unicast targets).
|
||||
// A mutable copy is required: set_multicast_if writes sin6_scope_id, which
|
||||
// is irrelevant here since the socket option is what matters.
|
||||
sockaddr_storage ss{};
|
||||
std::memcpy(&ss, target.sa(), target.sa_len());
|
||||
set_multicast_if(sock, device, reinterpret_cast<sockaddr*>(&ss));
|
||||
|
||||
// No ::connect(), no group join: a connected datagram socket would drop a
|
||||
// unicast reply whose source differs from the configured target; a group
|
||||
// join is a receiver-side concern and is not needed to send to the group.
|
||||
return sock;
|
||||
}
|
||||
|
||||
event::unique_fd open_udp_dualstack_server_socket(std::uint16_t port, std::string const& device) {
|
||||
event::unique_fd sock(::socket(AF_INET6, SOCK_DGRAM, 0));
|
||||
if (not sock.is_fd() && errno == EAFNOSUPPORT) {
|
||||
// IPv6 unavailable on this host: v4-only fallback.
|
||||
event::unique_fd v4(::socket(AF_INET, SOCK_DGRAM, 0));
|
||||
if (not v4.is_fd()) {
|
||||
throw std::runtime_error(build_errno_string("socket(AF_INET, SOCK_DGRAM) failed"));
|
||||
}
|
||||
set_reuse_address(v4);
|
||||
set_non_blocking(v4);
|
||||
sockaddr_in local{};
|
||||
local.sin_family = AF_INET;
|
||||
local.sin_addr.s_addr = htonl(INADDR_ANY);
|
||||
local.sin_port = htons(port);
|
||||
if (::bind(v4, reinterpret_cast<sockaddr*>(&local), sizeof(local)) < 0) {
|
||||
throw std::runtime_error(build_errno_string("bind(0.0.0.0:" + std::to_string(port) + ") failed"));
|
||||
}
|
||||
if (not device.empty()) {
|
||||
try {
|
||||
bind_socket_to_device(v4, device);
|
||||
} catch (const std::exception&) {
|
||||
// best-effort; wildcard bind retained
|
||||
}
|
||||
}
|
||||
return v4;
|
||||
}
|
||||
|
||||
if (not sock.is_fd()) {
|
||||
throw std::runtime_error(build_errno_string("socket(AF_INET6, SOCK_DGRAM) failed"));
|
||||
}
|
||||
int v6only = 0;
|
||||
if (setsockopt(sock, IPPROTO_IPV6, IPV6_V6ONLY, &v6only, sizeof(v6only)) < 0) {
|
||||
throw std::runtime_error(build_errno_string("setsockopt(IPV6_V6ONLY=0) failed"));
|
||||
}
|
||||
set_reuse_address(sock);
|
||||
set_non_blocking(sock);
|
||||
sockaddr_in6 local{};
|
||||
local.sin6_family = AF_INET6;
|
||||
local.sin6_addr = in6addr_any;
|
||||
local.sin6_port = htons(port);
|
||||
if (::bind(sock, reinterpret_cast<sockaddr*>(&local), sizeof(local)) < 0) {
|
||||
throw std::runtime_error(build_errno_string("bind([::]:" + std::to_string(port) + ") failed"));
|
||||
}
|
||||
if (not device.empty()) {
|
||||
try {
|
||||
bind_socket_to_device(sock, device);
|
||||
} catch (const std::exception&) {
|
||||
// best-effort; wildcard bind retained
|
||||
}
|
||||
}
|
||||
return sock;
|
||||
}
|
||||
|
||||
event::unique_fd open_udp_multicast_socket(std::string const& multicast_group, std::uint16_t port,
|
||||
std::string interface_address, std::string listen_address,
|
||||
bool reuse_address, bool reuse_port) {
|
||||
auto sock = event::unique_fd(::socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP));
|
||||
set_non_blocking(sock);
|
||||
if (reuse_address) {
|
||||
set_reuse_address(sock);
|
||||
}
|
||||
if (reuse_port) {
|
||||
set_reuse_port(sock);
|
||||
}
|
||||
|
||||
bind_socket_ip4(sock, listen_address, port);
|
||||
set_udp_multicast(sock, multicast_group, interface_address);
|
||||
return sock;
|
||||
}
|
||||
|
||||
event::unique_fd open_mdns_socket(std::string const& interface_name) {
|
||||
auto ip = get_interface_address(interface_name);
|
||||
auto sock = open_udp_multicast_socket("224.0.0.251", 5353, ip, "0.0.0.0", true, true);
|
||||
if (setsockopt(sock, SOL_SOCKET, SO_BINDTODEVICE, interface_name.c_str(), interface_name.length()) < 0) {
|
||||
throw std::runtime_error("Failed to bind socket to device " + interface_name + " -> " + strerror(errno));
|
||||
}
|
||||
return sock;
|
||||
}
|
||||
|
||||
void set_reuse_address(int fd) {
|
||||
socklen_t enable = 1;
|
||||
auto err = setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &enable, sizeof(enable));
|
||||
if (err) {
|
||||
throw std::runtime_error("Failed to setsockopt(SO_REUSEADDRE)");
|
||||
}
|
||||
}
|
||||
|
||||
void set_reuse_port(int fd) {
|
||||
#ifdef SO_REUSEPORT
|
||||
socklen_t enable = 1;
|
||||
auto err = setsockopt(fd, SOL_SOCKET, SO_REUSEPORT, &enable, sizeof(enable));
|
||||
if (err) {
|
||||
throw std::runtime_error("Failed to setsockopt(SO_REUSEADDRE)");
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void bind_socket_ip4(int fd, std::string const& ip, std::uint16_t port) {
|
||||
sockaddr_in local_bind_addr{};
|
||||
int result = inet_pton(AF_INET, ip.c_str(), &local_bind_addr.sin_addr.s_addr);
|
||||
if (result not_eq 1) {
|
||||
throw std::runtime_error("Invalid IP address: " + ip);
|
||||
}
|
||||
|
||||
local_bind_addr.sin_family = AF_INET;
|
||||
local_bind_addr.sin_port = htons(port);
|
||||
local_bind_addr.sin_addr.s_addr = ip_to_s_addr(ip);
|
||||
if (bind(fd, (struct sockaddr*)&local_bind_addr, sizeof(local_bind_addr)) < 0) {
|
||||
throw std::runtime_error("Cannot bind socket to " + ip + ":" + std::to_string(port) + " -> " + strerror(errno));
|
||||
}
|
||||
}
|
||||
|
||||
void set_udp_multicast(int fd, std::string const& multicast_ip, std::string const& interface_ip) {
|
||||
struct in_addr out_addr;
|
||||
out_addr.s_addr = inet_addr(interface_ip.c_str());
|
||||
|
||||
auto err = setsockopt(fd, IPPROTO_IP, IP_MULTICAST_IF, (char*)&out_addr, sizeof(out_addr));
|
||||
if (err) {
|
||||
throw std::runtime_error("Failed to setsockopt(IP_MULTICAST_IF)");
|
||||
}
|
||||
|
||||
ip_mreq mreq;
|
||||
mreq.imr_multiaddr.s_addr = ip_to_s_addr(multicast_ip);
|
||||
mreq.imr_interface.s_addr = ip_to_s_addr(interface_ip);
|
||||
err = setsockopt(fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
|
||||
if (err) {
|
||||
throw std::runtime_error("Failed to setsockopt(IP_ADD_MEMBERSHIP)");
|
||||
}
|
||||
}
|
||||
|
||||
std::uint32_t ip_to_s_addr(std::string const& ip) {
|
||||
std::uint32_t s_addr;
|
||||
int result = inet_pton(AF_INET, ip.c_str(), &s_addr);
|
||||
if (result not_eq 1) {
|
||||
throw std::runtime_error("Invalid IP address: " + ip);
|
||||
}
|
||||
return s_addr;
|
||||
}
|
||||
|
||||
} // namespace socket
|
||||
|
||||
} // namespace everest::lib::io
|
||||
126
tools/EVerest-main/lib/everest/io/src/tcp/tcp_socket.cpp
Normal file
126
tools/EVerest-main/lib/everest/io/src/tcp/tcp_socket.cpp
Normal file
@@ -0,0 +1,126 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2026 Pionix GmbH and Contributors to EVerest
|
||||
#include <chrono>
|
||||
#include <everest/io/socket/socket.hpp>
|
||||
#include <everest/io/tcp/tcp_socket.hpp>
|
||||
#include <netdb.h>
|
||||
#include <thread>
|
||||
|
||||
namespace everest::lib::io::tcp {
|
||||
|
||||
bool tcp_socket::open(std::string const& remote, uint16_t port, std::string const& device) {
|
||||
m_remote = remote;
|
||||
m_port = port;
|
||||
m_timeout_ms = 1000;
|
||||
m_device = device;
|
||||
try {
|
||||
auto socket = socket::open_tcp_socket_with_timeout(remote, port, m_timeout_ms, m_device);
|
||||
socket::set_non_blocking(socket);
|
||||
m_fd = std::move(socket);
|
||||
return socket::get_pending_error(m_fd) == 0;
|
||||
} catch (...) {
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool tcp_socket::setup(std::string const& remote, uint16_t port, int timeout_ms, std::string const& device) {
|
||||
m_remote = remote;
|
||||
m_port = port;
|
||||
m_timeout_ms = timeout_ms;
|
||||
m_device = device;
|
||||
m_fd.close();
|
||||
return true;
|
||||
}
|
||||
|
||||
void tcp_socket::connect(std::function<void(bool, int)> const& setup_cb) {
|
||||
try {
|
||||
auto socket = socket::open_tcp_socket_with_timeout(m_remote, m_port, m_timeout_ms, m_device);
|
||||
socket::set_non_blocking(socket);
|
||||
const auto fd = static_cast<int>(socket);
|
||||
m_fd = std::move(socket);
|
||||
setup_cb(true, fd);
|
||||
} catch (...) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(m_timeout_ms));
|
||||
setup_cb(false, -1);
|
||||
}
|
||||
}
|
||||
|
||||
bool tcp_socket::tx(PayloadT& payload) {
|
||||
if (not is_open()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
auto status = ::send(m_fd, payload.data(), payload.size(), 0);
|
||||
if (status == -1) {
|
||||
return false;
|
||||
}
|
||||
if (status < static_cast<ssize_t>(payload.size())) {
|
||||
// We have a reference to the current data. Replace it with what is left to be written
|
||||
// and return false. This signals the current block cannot be removed from the buffer.
|
||||
payload = {payload.begin() + status, payload.end()};
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool tcp_socket::rx(PayloadT& buffer) {
|
||||
if (not is_open()) {
|
||||
return false;
|
||||
}
|
||||
buffer.resize(default_buffer_size);
|
||||
auto status = ::recv(m_fd, buffer.data(), buffer.size(), 0);
|
||||
if (status <= 0) { // -1 is an error, 0 is a connection closed by the peer
|
||||
return false;
|
||||
}
|
||||
buffer.resize(status);
|
||||
return true;
|
||||
}
|
||||
|
||||
int tcp_socket::get_fd() const {
|
||||
return m_fd;
|
||||
}
|
||||
|
||||
int tcp_socket::get_error() const {
|
||||
if (socket::is_tcp_socket_alive(m_fd)) {
|
||||
return socket::get_pending_error(m_fd);
|
||||
} else if (is_open()) {
|
||||
return ECONNRESET;
|
||||
}
|
||||
return socket::get_pending_error(m_fd);
|
||||
}
|
||||
|
||||
bool tcp_socket::is_open() const {
|
||||
return m_fd.is_fd();
|
||||
}
|
||||
|
||||
void tcp_socket::close() {
|
||||
m_fd.close();
|
||||
}
|
||||
|
||||
bool tcp_socket::set_keep_alive(uint32_t count, uint32_t idle_s, uint32_t intval_s) {
|
||||
if (not is_open()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
try {
|
||||
socket::set_tcp_keepalive(m_fd, count, idle_s, intval_s);
|
||||
return true;
|
||||
} catch (...) {
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool tcp_socket::set_user_timeout(uint32_t to_ms) {
|
||||
if (not is_open()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
try {
|
||||
socket::set_tcp_user_timeout(m_fd, to_ms);
|
||||
return true;
|
||||
} catch (...) {
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::tcp
|
||||
@@ -0,0 +1,65 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2026 Pionix GmbH and Contributors to EVerest
|
||||
#include <cstring>
|
||||
#include <everest/io/tun_tap/tap_handler.hpp>
|
||||
#include <fcntl.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/types.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <linux/if.h>
|
||||
#include <linux/if_tun.h>
|
||||
|
||||
#include <everest/io/socket/socket.hpp>
|
||||
|
||||
namespace everest::lib::io::tun_tap {
|
||||
|
||||
bool tap_handler::open(std::string const& device, std::string const& ip, std::string const& netmask, int mtu) {
|
||||
m_mtu = mtu;
|
||||
try {
|
||||
m_fd = socket::create_tap_device(device);
|
||||
} catch (...) {
|
||||
m_error = EPERM;
|
||||
return false;
|
||||
}
|
||||
if (not socket::configure_tap_device_properties(m_fd, device, ip, netmask, mtu)) {
|
||||
m_error = EPERM;
|
||||
m_fd.close();
|
||||
return false;
|
||||
}
|
||||
m_error = 0;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool tap_handler::tx(PayloadT const& data) {
|
||||
auto res = ::write(m_fd, data.data(), data.size());
|
||||
if (res != static_cast<ssize_t>(data.size())) {
|
||||
m_error = errno;
|
||||
return false;
|
||||
}
|
||||
m_error = 0;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool tap_handler::rx(PayloadT& data) {
|
||||
data.resize(m_mtu);
|
||||
auto res = ::read(m_fd, data.data(), data.size());
|
||||
if (res < 0) {
|
||||
m_error = errno;
|
||||
return false;
|
||||
}
|
||||
|
||||
data.resize(res);
|
||||
m_error = 0;
|
||||
return true;
|
||||
}
|
||||
|
||||
int tap_handler::get_fd() const {
|
||||
return m_fd;
|
||||
}
|
||||
|
||||
int tap_handler::get_error() const {
|
||||
return m_error;
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::tun_tap
|
||||
156
tools/EVerest-main/lib/everest/io/src/udp/endpoint.cpp
Normal file
156
tools/EVerest-main/lib/everest/io/src/udp/endpoint.cpp
Normal file
@@ -0,0 +1,156 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2026 Pionix GmbH and Contributors to EVerest
|
||||
|
||||
#include <everest/io/udp/endpoint.hpp>
|
||||
|
||||
#include <cstring>
|
||||
#include <stdexcept>
|
||||
|
||||
#include <arpa/inet.h>
|
||||
#include <net/if.h>
|
||||
#include <netdb.h>
|
||||
|
||||
namespace everest::lib::io::udp {
|
||||
|
||||
namespace {
|
||||
|
||||
// Set sin6_scope_id from @p iface for IPv6 addresses that need a scope
|
||||
// (link-local fe80::/10 or multicast ff00::/8). Throws if the interface is
|
||||
// unknown. No-op for global addresses or an empty interface.
|
||||
void apply_v6_scope(sockaddr_in6& sin6, std::string const& iface) {
|
||||
if (iface.empty()) {
|
||||
return;
|
||||
}
|
||||
if (not IN6_IS_ADDR_LINKLOCAL(&sin6.sin6_addr) && not IN6_IS_ADDR_MULTICAST(&sin6.sin6_addr)) {
|
||||
return;
|
||||
}
|
||||
unsigned int ifindex = if_nametoindex(iface.c_str());
|
||||
if (ifindex == 0) {
|
||||
throw std::runtime_error("if_nametoindex(\"" + iface + "\") failed");
|
||||
}
|
||||
sin6.sin6_scope_id = ifindex;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
endpoint::endpoint(std::string const& host, std::uint16_t port, std::string iface) : m_iface(std::move(iface)) {
|
||||
sockaddr_in v4{};
|
||||
if (inet_pton(AF_INET, host.c_str(), &v4.sin_addr) == 1) {
|
||||
v4.sin_family = AF_INET;
|
||||
v4.sin_port = htons(port);
|
||||
std::memcpy(&m_storage, &v4, sizeof(v4));
|
||||
m_len = sizeof(v4);
|
||||
return;
|
||||
}
|
||||
|
||||
sockaddr_in6 v6{};
|
||||
if (inet_pton(AF_INET6, host.c_str(), &v6.sin6_addr) == 1) {
|
||||
v6.sin6_family = AF_INET6;
|
||||
v6.sin6_port = htons(port);
|
||||
apply_v6_scope(v6, m_iface);
|
||||
std::memcpy(&m_storage, &v6, sizeof(v6));
|
||||
m_len = sizeof(v6);
|
||||
return;
|
||||
}
|
||||
|
||||
addrinfo hints{};
|
||||
hints.ai_family = AF_UNSPEC;
|
||||
hints.ai_socktype = SOCK_DGRAM;
|
||||
addrinfo* res = nullptr;
|
||||
const auto err = ::getaddrinfo(host.c_str(), std::to_string(port).c_str(), &hints, &res);
|
||||
if (err != 0 || res == nullptr) {
|
||||
throw std::runtime_error("Failed to resolve endpoint \"" + host + "\": " + ::gai_strerror(err));
|
||||
}
|
||||
std::memcpy(&m_storage, res->ai_addr, res->ai_addrlen);
|
||||
m_len = res->ai_addrlen;
|
||||
::freeaddrinfo(res);
|
||||
|
||||
if (m_storage.ss_family == AF_INET6) {
|
||||
sockaddr_in6 scoped{};
|
||||
std::memcpy(&scoped, &m_storage, sizeof(scoped));
|
||||
scoped.sin6_port = htons(port);
|
||||
apply_v6_scope(scoped, m_iface);
|
||||
std::memcpy(&m_storage, &scoped, sizeof(scoped));
|
||||
}
|
||||
}
|
||||
|
||||
endpoint::endpoint(sockaddr_storage const& src, socklen_t len) : m_len(len) {
|
||||
std::memcpy(&m_storage, &src, sizeof(m_storage));
|
||||
}
|
||||
|
||||
sa_family_t endpoint::family() const {
|
||||
return m_len == 0 ? AF_UNSPEC : m_storage.ss_family;
|
||||
}
|
||||
|
||||
std::uint16_t endpoint::port() const {
|
||||
if (m_storage.ss_family == AF_INET) {
|
||||
return ntohs(reinterpret_cast<sockaddr_in const*>(&m_storage)->sin_port);
|
||||
}
|
||||
if (m_storage.ss_family == AF_INET6) {
|
||||
return ntohs(reinterpret_cast<sockaddr_in6 const*>(&m_storage)->sin6_port);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
std::string endpoint::addr_str() const {
|
||||
char buf[INET6_ADDRSTRLEN]{};
|
||||
if (m_storage.ss_family == AF_INET) {
|
||||
inet_ntop(AF_INET, &reinterpret_cast<sockaddr_in const*>(&m_storage)->sin_addr, buf, sizeof(buf));
|
||||
} else if (m_storage.ss_family == AF_INET6) {
|
||||
inet_ntop(AF_INET6, &reinterpret_cast<sockaddr_in6 const*>(&m_storage)->sin6_addr, buf, sizeof(buf));
|
||||
}
|
||||
return std::string(buf);
|
||||
}
|
||||
|
||||
sockaddr const* endpoint::sa() const {
|
||||
return reinterpret_cast<sockaddr const*>(&m_storage);
|
||||
}
|
||||
|
||||
socklen_t endpoint::sa_len() const {
|
||||
return m_len;
|
||||
}
|
||||
|
||||
std::string const& endpoint::iface() const {
|
||||
return m_iface;
|
||||
}
|
||||
|
||||
bool endpoint::is_v4_mapped() const {
|
||||
if (m_storage.ss_family != AF_INET6) {
|
||||
return false;
|
||||
}
|
||||
auto const* s6 = reinterpret_cast<sockaddr_in6 const*>(&m_storage);
|
||||
return IN6_IS_ADDR_V4MAPPED(&s6->sin6_addr);
|
||||
}
|
||||
|
||||
endpoint endpoint::as_v4() const {
|
||||
if (not is_v4_mapped()) {
|
||||
return endpoint{};
|
||||
}
|
||||
auto const* s6 = reinterpret_cast<sockaddr_in6 const*>(&m_storage);
|
||||
sockaddr_storage ss{};
|
||||
auto* s4 = reinterpret_cast<sockaddr_in*>(&ss);
|
||||
s4->sin_family = AF_INET;
|
||||
s4->sin_port = s6->sin6_port; // network order preserved
|
||||
std::memcpy(&s4->sin_addr.s_addr, reinterpret_cast<uint8_t const*>(&s6->sin6_addr) + 12, 4);
|
||||
return endpoint(ss, sizeof(sockaddr_in));
|
||||
}
|
||||
|
||||
bool endpoint::operator==(endpoint const& other) const {
|
||||
if (m_storage.ss_family != other.m_storage.ss_family) {
|
||||
return false;
|
||||
}
|
||||
if (m_storage.ss_family == AF_INET) {
|
||||
auto const* a = reinterpret_cast<sockaddr_in const*>(&m_storage);
|
||||
auto const* b = reinterpret_cast<sockaddr_in const*>(&other.m_storage);
|
||||
return a->sin_port == b->sin_port && a->sin_addr.s_addr == b->sin_addr.s_addr;
|
||||
}
|
||||
if (m_storage.ss_family == AF_INET6) {
|
||||
auto const* a = reinterpret_cast<sockaddr_in6 const*>(&m_storage);
|
||||
auto const* b = reinterpret_cast<sockaddr_in6 const*>(&other.m_storage);
|
||||
return a->sin6_port == b->sin6_port && a->sin6_scope_id == b->sin6_scope_id &&
|
||||
std::memcmp(&a->sin6_addr, &b->sin6_addr, sizeof(in6_addr)) == 0;
|
||||
}
|
||||
return m_len == other.m_len;
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::udp
|
||||
@@ -0,0 +1,59 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2026 Pionix GmbH and Contributors to EVerest
|
||||
|
||||
#include <everest/io/udp/udp_dualstack_server_socket.hpp>
|
||||
|
||||
#include <exception>
|
||||
#include <utility>
|
||||
|
||||
#include <sys/socket.h>
|
||||
|
||||
#include <everest/io/socket/socket.hpp>
|
||||
|
||||
namespace everest::lib::io::udp {
|
||||
|
||||
bool udp_dualstack_server_socket::open(std::uint16_t port, std::string device) {
|
||||
try {
|
||||
m_owned_udp_fd = socket::open_udp_dualstack_server_socket(port, device);
|
||||
return socket::get_pending_error(m_owned_udp_fd) == 0;
|
||||
} catch (const std::exception&) {
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool udp_dualstack_server_socket::tx(udp_payload const& payload) {
|
||||
if (not m_owned_udp_fd.is_fd() or not m_last_source) {
|
||||
return false;
|
||||
}
|
||||
const auto nbytes = ::sendto(m_owned_udp_fd, payload.buffer.data(), payload.size(), 0, m_last_source->sa(),
|
||||
m_last_source->sa_len());
|
||||
return nbytes == static_cast<ssize_t>(payload.size());
|
||||
}
|
||||
|
||||
bool udp_dualstack_server_socket::rx(udp_payload& payload) {
|
||||
if (not m_owned_udp_fd.is_fd()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
sockaddr_storage src{};
|
||||
socklen_t src_len = sizeof(src);
|
||||
const auto nbytes = ::recvfrom(m_owned_udp_fd, m_rx_buffer.data(), m_rx_buffer.size(), 0,
|
||||
reinterpret_cast<sockaddr*>(&src), &src_len);
|
||||
if (nbytes < 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
payload.set_message(m_rx_buffer.data(), static_cast<size_t>(nbytes));
|
||||
m_last_source = endpoint(src, src_len);
|
||||
return true;
|
||||
}
|
||||
|
||||
int udp_dualstack_server_socket::get_fd() const {
|
||||
return m_owned_udp_fd;
|
||||
}
|
||||
|
||||
int udp_dualstack_server_socket::get_error() const {
|
||||
return socket::get_pending_error(m_owned_udp_fd);
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::udp
|
||||
42
tools/EVerest-main/lib/everest/io/src/udp/udp_payload.cpp
Normal file
42
tools/EVerest-main/lib/everest/io/src/udp/udp_payload.cpp
Normal file
@@ -0,0 +1,42 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2025 Pionix GmbH and Contributors to EVerest
|
||||
|
||||
#include <everest/io/udp/udp_payload.hpp>
|
||||
#include <vector>
|
||||
|
||||
namespace everest::lib::io::udp {
|
||||
|
||||
udp_payload::udp_payload(std::string const& msg) {
|
||||
set_message(msg);
|
||||
}
|
||||
|
||||
udp_payload::udp_payload(const char* msg) {
|
||||
set_message(msg);
|
||||
}
|
||||
|
||||
bool udp_payload::operator==(udp_payload const& other) const {
|
||||
return buffer == other.buffer;
|
||||
}
|
||||
|
||||
size_t udp_payload::size() const {
|
||||
return buffer.size();
|
||||
}
|
||||
|
||||
bool udp_payload::set_message(std::string const& msg) {
|
||||
if (msg.size() > max_size) {
|
||||
return false;
|
||||
}
|
||||
buffer = std::vector<uint8_t>(msg.begin(), msg.end());
|
||||
return true;
|
||||
}
|
||||
|
||||
bool udp_payload::set_message(void const* data, size_t size) {
|
||||
if (size > max_size) {
|
||||
return false;
|
||||
}
|
||||
auto ptr = reinterpret_cast<uint8_t const*>(data);
|
||||
buffer = std::vector<uint8_t>(ptr, ptr + size);
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::udp
|
||||
155
tools/EVerest-main/lib/everest/io/src/udp/udp_socket.cpp
Normal file
155
tools/EVerest-main/lib/everest/io/src/udp/udp_socket.cpp
Normal file
@@ -0,0 +1,155 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2026 Pionix GmbH and Contributors to EVerest
|
||||
|
||||
#include "everest/io/udp/udp_payload.hpp"
|
||||
#include <everest/io/event/fd_event_handler.hpp>
|
||||
#include <everest/io/event/unique_fd.hpp>
|
||||
#include <everest/io/socket/socket.hpp>
|
||||
#include <everest/io/udp/udp_socket.hpp>
|
||||
#include <iostream>
|
||||
#include <net/if.h>
|
||||
#include <netinet/in.h>
|
||||
#include <optional>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/socket.h>
|
||||
#include <unistd.h>
|
||||
|
||||
namespace everest::lib::io::udp {
|
||||
|
||||
bool udp_socket_base::open_as_client(std::string const& remote, uint16_t port, std::string const& device) {
|
||||
try {
|
||||
auto socket = socket::open_udp_client_socket(remote, port, device);
|
||||
socket::set_non_blocking(socket);
|
||||
m_owned_udp_fd = std::move(socket);
|
||||
return socket::get_pending_error(m_owned_udp_fd) == 0;
|
||||
} catch (...) {
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool udp_socket_base::open_as_server(uint16_t port, std::string const& device) {
|
||||
auto socket = socket::open_udp_server_socket(port, device);
|
||||
socket::set_non_blocking(socket);
|
||||
m_owned_udp_fd = std::move(socket);
|
||||
return socket::get_pending_error(m_owned_udp_fd) == 0;
|
||||
}
|
||||
|
||||
bool udp_socket_base::is_open() {
|
||||
return m_owned_udp_fd.is_fd();
|
||||
}
|
||||
|
||||
void udp_socket_base::close() {
|
||||
m_owned_udp_fd.close();
|
||||
}
|
||||
|
||||
int udp_socket_base::get_fd() const {
|
||||
return m_owned_udp_fd;
|
||||
}
|
||||
|
||||
int udp_socket_base::get_error() const {
|
||||
return socket::get_pending_error(m_owned_udp_fd);
|
||||
}
|
||||
|
||||
bool udp_socket_base::tx_impl(void const* payload, size_t size) {
|
||||
if (not is_open()) {
|
||||
return ENETDOWN;
|
||||
}
|
||||
size_t nbytes = ::send(m_owned_udp_fd, payload, size, 0);
|
||||
|
||||
return nbytes == size;
|
||||
}
|
||||
|
||||
bool udp_socket_base::tx_impl(void const* payload, size_t size, udp_info const& destination) {
|
||||
if (not is_open()) {
|
||||
return false;
|
||||
}
|
||||
socklen_t peer_addr_len = sizeof(struct sockaddr_in);
|
||||
struct sockaddr_in peer_addr;
|
||||
peer_addr.sin_port = destination.port;
|
||||
peer_addr.sin_addr.s_addr = destination.addr;
|
||||
peer_addr.sin_family = destination.family;
|
||||
size_t nbytes = ::sendto(m_owned_udp_fd, payload, size, 0, (struct sockaddr*)&peer_addr, peer_addr_len);
|
||||
|
||||
return nbytes == size;
|
||||
}
|
||||
|
||||
std::optional<udp_info> udp_socket_base::rx_impl(void* buffer, size_t buffer_size, ssize_t& payload_size) {
|
||||
socklen_t peer_addr_len = sizeof(struct sockaddr_in);
|
||||
struct sockaddr_in peer_addr;
|
||||
if (is_open()) {
|
||||
payload_size = ::recvfrom(m_owned_udp_fd, buffer, buffer_size, 0, (struct sockaddr*)&peer_addr, &peer_addr_len);
|
||||
if (payload_size >= 0) {
|
||||
return udp_info{peer_addr.sin_addr.s_addr, peer_addr.sin_port, peer_addr.sin_family};
|
||||
}
|
||||
}
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
/////////////////////////////////////////////////
|
||||
|
||||
bool udp_client_socket::setup(std::string const& remote, uint16_t port, int timeout_ms, std::string const& device) {
|
||||
m_remote = remote;
|
||||
m_port = port;
|
||||
m_timeout_ms = timeout_ms;
|
||||
m_device = device;
|
||||
m_owned_udp_fd.close();
|
||||
return true;
|
||||
}
|
||||
|
||||
void udp_client_socket::connect(std::function<void(bool, int)> const& setup_cb) {
|
||||
try {
|
||||
auto socket = socket::open_udp_client_socket(m_remote, m_port, m_device);
|
||||
socket::set_non_blocking(socket);
|
||||
setup_cb(true, socket);
|
||||
m_owned_udp_fd = std::move(socket);
|
||||
} catch (...) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(m_timeout_ms));
|
||||
setup_cb(false, -1);
|
||||
}
|
||||
}
|
||||
|
||||
bool udp_client_socket::open(std::string const& remote, uint16_t port, std::string const& device) {
|
||||
return open_as_client(remote, port, device);
|
||||
}
|
||||
|
||||
bool udp_client_socket::tx(udp_payload const& payload) {
|
||||
return tx_impl(payload.buffer.data(), payload.size());
|
||||
}
|
||||
|
||||
bool udp_client_socket::rx(udp_payload& payload) {
|
||||
ssize_t msg_size = 0;
|
||||
auto result = rx_impl(rx_buffer.data(), rx_buffer.size(), msg_size);
|
||||
if (result) {
|
||||
payload.set_message(rx_buffer.data(), msg_size);
|
||||
}
|
||||
return result.has_value();
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////
|
||||
|
||||
bool udp_server_socket::open(uint16_t port, std::string const& device) {
|
||||
return open_as_server(port, device);
|
||||
}
|
||||
|
||||
bool udp_server_socket::tx(udp_payload const& payload) {
|
||||
if (not m_last_source) {
|
||||
return false;
|
||||
}
|
||||
return tx_impl(payload.buffer.data(), payload.size(), *m_last_source);
|
||||
}
|
||||
|
||||
bool udp_server_socket::rx(udp_payload& payload) {
|
||||
ssize_t msg_size = 0;
|
||||
auto result = rx_impl(rx_buffer.data(), rx_buffer.size(), msg_size);
|
||||
if (not result) {
|
||||
return false;
|
||||
}
|
||||
payload.set_message(rx_buffer.data(), msg_size);
|
||||
m_last_source = result;
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::udp
|
||||
@@ -0,0 +1,61 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2026 Pionix GmbH and Contributors to EVerest
|
||||
|
||||
#include <everest/io/udp/udp_unconnected_socket.hpp>
|
||||
|
||||
#include <exception>
|
||||
#include <utility>
|
||||
|
||||
#include <sys/socket.h>
|
||||
|
||||
#include <everest/io/socket/socket.hpp>
|
||||
|
||||
namespace everest::lib::io::udp {
|
||||
|
||||
bool udp_unconnected_socket::open(endpoint target, std::string iface) {
|
||||
try {
|
||||
m_owned_udp_fd = socket::open_udp_unconnected_socket(target, iface);
|
||||
m_target = std::move(target);
|
||||
return socket::get_pending_error(m_owned_udp_fd) == 0;
|
||||
} catch (const std::exception&) {
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool udp_unconnected_socket::tx(udp_payload const& payload) {
|
||||
if (not m_owned_udp_fd.is_fd()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const auto nbytes =
|
||||
::sendto(m_owned_udp_fd, payload.buffer.data(), payload.size(), 0, m_target.sa(), m_target.sa_len());
|
||||
return nbytes == static_cast<ssize_t>(payload.size());
|
||||
}
|
||||
|
||||
bool udp_unconnected_socket::rx(udp_payload& payload) {
|
||||
if (not m_owned_udp_fd.is_fd()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
sockaddr_storage src{};
|
||||
socklen_t src_len = sizeof(src);
|
||||
const auto nbytes = ::recvfrom(m_owned_udp_fd, m_rx_buffer.data(), m_rx_buffer.size(), 0,
|
||||
reinterpret_cast<sockaddr*>(&src), &src_len);
|
||||
if (nbytes < 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
payload.set_message(m_rx_buffer.data(), static_cast<size_t>(nbytes));
|
||||
m_last_source = endpoint(src, src_len);
|
||||
return true;
|
||||
}
|
||||
|
||||
int udp_unconnected_socket::get_fd() const {
|
||||
return m_owned_udp_fd;
|
||||
}
|
||||
|
||||
int udp_unconnected_socket::get_error() const {
|
||||
return socket::get_pending_error(m_owned_udp_fd);
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::udp
|
||||
@@ -0,0 +1,45 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2025 Pionix GmbH and Contributors to EVerest
|
||||
#include <everest/io/utilities/generic_error_state.hpp>
|
||||
#include <string.h>
|
||||
|
||||
namespace everest::lib::io::utilities {
|
||||
|
||||
bool generic_error_state::set_error_status(int error_code) {
|
||||
m_current_error = error_code;
|
||||
auto on_error = error_code != 0;
|
||||
m_clear_error_pending = (not on_error) and m_on_error;
|
||||
m_on_error = on_error;
|
||||
return not m_on_error;
|
||||
}
|
||||
|
||||
bool generic_error_state::clear_error_pending() const {
|
||||
return m_clear_error_pending;
|
||||
}
|
||||
|
||||
bool generic_error_state::on_error() const {
|
||||
return m_on_error;
|
||||
}
|
||||
|
||||
int generic_error_state::current_error() const {
|
||||
return m_current_error;
|
||||
}
|
||||
|
||||
void generic_error_state::call_error_handler(cb_error& handler) const {
|
||||
if (handler) {
|
||||
handler(m_current_error, strerror(m_current_error));
|
||||
}
|
||||
}
|
||||
|
||||
void generic_error_state::clear_error_handler(cb_error& handler) {
|
||||
if (handler) {
|
||||
handler(0, strerror(0));
|
||||
}
|
||||
set_error_cleared();
|
||||
}
|
||||
|
||||
void generic_error_state::set_error_cleared() {
|
||||
m_clear_error_pending = false;
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::utilities
|
||||
@@ -0,0 +1,38 @@
|
||||
// SPDX-License-Identifier: Apache-2.0
|
||||
// Copyright 2020 - 2025 Pionix GmbH and Contributors to EVerest
|
||||
|
||||
#include <chrono>
|
||||
#include <everest/io/utilities/stop_watch.hpp>
|
||||
#include <iostream>
|
||||
|
||||
namespace everest::lib::io::utilities {
|
||||
|
||||
stop_watch::stop_watch(std::string const& id) : m_id(id) {
|
||||
m_start_time = clock::now();
|
||||
}
|
||||
|
||||
stop_watch::tp stop_watch::reset() {
|
||||
m_start_time = clock::now();
|
||||
return m_start_time;
|
||||
}
|
||||
|
||||
stop_watch::us stop_watch::stop() {
|
||||
m_end_time = clock::now();
|
||||
auto dura = std::chrono::duration_cast<us>(m_end_time - m_start_time);
|
||||
return dura;
|
||||
}
|
||||
|
||||
stop_watch::us stop_watch::lap() const {
|
||||
auto current = clock::now();
|
||||
auto dura = std::chrono::duration_cast<us>(current - m_start_time);
|
||||
return dura;
|
||||
}
|
||||
|
||||
stop_watch::~stop_watch() {
|
||||
if (not m_id.empty()) {
|
||||
auto dura = stop();
|
||||
std::cout << "StopWatch ( " << m_id << " ) duration " << dura.count() << "us" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace everest::lib::io::utilities
|
||||
Reference in New Issue
Block a user