From 4eafadb1653b215883e555e4cd00cda0caaf5ab4 Mon Sep 17 00:00:00 2001 From: Roman Ilin Date: Wed, 24 Jun 2026 14:42:16 +0300 Subject: Initial commit --- src/main.c | 459 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 459 insertions(+) create mode 100644 src/main.c (limited to 'src') diff --git a/src/main.c b/src/main.c new file mode 100644 index 0000000..f0f589a --- /dev/null +++ b/src/main.c @@ -0,0 +1,459 @@ +// Copyright (c) 2026 Roman Ilin +// SPDX-License-Identifier: BUSL-1.1 +// +// Use of this software is governed by the Business Source License +// included in the LICENSE.txt file. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#if __STDC_ENDIAN_NATIVE__ == __STDC_ENDIAN_BIG__ +#error "Big-endian hardware is not supported" +// #define MESSAGE_NORMALIZE(v) ({ \ +// static_assert( \ +// sizeof(v) == 2 || \ +// sizeof(v) == 4 || \ +// sizeof(v) == 8 || \ +// sizeof(v) == 16, \ +// "MESSAGE_NORMALIZE: Unsupported value size" \ +// ); \ +// __builtin_choose_expr(sizeof(v) == 2, __builtin_bswap16(v), \ +// __builtin_choose_expr(sizeof(v) == 4, __builtin_bswap32(v), \ +// __builtin_choose_expr(sizeof(v) == 8, __builtin_bswap64(v), \ +// __builtin_choose_expr(sizeof(v) == 16, __builtin_bswap128(v), \ +// __builtin_unreachable())))); \ +// }) +// #else +// #define MESSAGE_NORMALIZE(v) (v) +#endif + +typedef enum : uint8_t { + MESSAGE_PING = 0, + MESSAGE_PONG = 1, +} Message_Type; + +typedef uint64_t Correlation_ID; + +typedef struct { + Correlation_ID correlation_id; + Message_Type type; + uint8_t _pad[7]; +} Message_Header; + +typedef uint64_t Node_ID; + +#define ID_FMT "016" PRIx64 + +typedef struct { + int64_t s; + int32_t ns; + uint8_t _pad[4]; +} Timestamp; + +static constexpr int32_t NS_PER_S = 1'000'000'000; + +static inline Timestamp timestamp_normalize(int64_t s, int64_t ns) +{ + Timestamp t; + int64_t s_adjust = ns / NS_PER_S; + ns = ns % NS_PER_S; + if (ns < 0) { + s_adjust--; + ns += NS_PER_S; + } + t.s = s + s_adjust; + t.ns = (int32_t)ns; + return t; +} + +static Timestamp timestamp_from_timespec(const struct timespec *ts_p) +{ + return timestamp_normalize((int64_t)ts_p->tv_sec, (int64_t)ts_p->tv_nsec); +} + +static Timestamp timestamp_now(clockid_t clock_id) { + struct timespec ts; + if (clock_gettime(clock_id, &ts) != 0) return (Timestamp){}; + return timestamp_from_timespec(&ts); +} + +typedef struct { + Node_ID node_id; +} Message_Ping; + +typedef struct { + Node_ID node_id; + Timestamp timestamp_rx; + Timestamp timestamp_tx; +} Message_Pong; + +typedef struct { + Message_Header header; + union { + Message_Ping ping; + Message_Pong pong; + } payload; +} Message; + +static size_t packet_handle( + Node_ID node_id, + int thread_id, + const struct sockaddr_storage *client_address_p, + socklen_t address_length, + uint8_t *buffer_p, + size_t rx_length, + size_t max_buffer_length, + Timestamp rx_timestamp +) +{ + (void)thread_id; + (void)client_address_p; + (void)address_length; + (void)max_buffer_length; + + if (rx_length < sizeof(Message_Header)) return 0; + + auto message_p = (Message *)buffer_p; + + switch (message_p->header.type) { + case MESSAGE_PING: { + if (rx_length != sizeof(Message_Header) + sizeof(Message_Ping)) { + fprintf(stderr, "Invalid PING message length %zu\n", rx_length); + return 0; + } + + Node_ID tx_node_id = message_p->payload.ping.node_id; + printf("Received PING from node %" ID_FMT "\n", tx_node_id); + + message_p->header.type = MESSAGE_PONG; + message_p->payload.pong.node_id = node_id; + message_p->payload.pong.timestamp_rx = rx_timestamp; + message_p->payload.pong.timestamp_tx = timestamp_now(CLOCK_REALTIME); + + return sizeof(Message_Header) + sizeof(Message_Pong); + } + + case MESSAGE_PONG: { + Timestamp tx_timestamp = message_p->payload.pong.timestamp_tx; + printf("Message type: %d, TX timestamp: %zu.%d\n", message_p->header.type, tx_timestamp.s, tx_timestamp.ns); + return 0; + } + + default: + printf("Unknown message type: %d\n", message_p->header.type); + return 0; + } +} + +static constexpr uint16_t PORT = 12421; +static constexpr int BGID = 1; +static constexpr size_t BUFFERS_LENGTH = 1024; +static constexpr size_t MTU = 1200; +static constexpr size_t BUFFER_LENGTH = 2048; +static constexpr uint32_t SQ_THREAD_IDLE_MS = 2000; + +static int shutdown_efd_g = -1; +static atomic_bool keep_running_g = true; + +typedef enum { + QE_RX, + QE_SEND, + QE_BUFFER, + QE_SHUTDOWN, +} QE_Type; + +typedef struct { + QE_Type type; + int bid; +} QE_Ctx; + +typedef union { + struct io_uring_recvmsg_out hdr; + uint8_t raw[BUFFER_LENGTH]; +} Aligned_Buffer; + +typedef struct { + Node_ID node_id; + int thread_id; + int sfd; + struct io_uring ring; + Aligned_Buffer buffers[BUFFERS_LENGTH]; + QE_Ctx ctxs[BUFFERS_LENGTH]; + struct msghdr send_messages[BUFFERS_LENGTH]; + struct iovec send_iovs[BUFFERS_LENGTH]; + struct msghdr rx_message_template; + QE_Ctx rx_ctx; + QE_Ctx shutdown_ctx; +} Worker_Ctx; + +static void signal_handle(int sig) +{ + (void)sig; + keep_running_g = false; + if (shutdown_efd_g >= 0) { + uint64_t val = 1; + ssize_t res = write(shutdown_efd_g, &val, sizeof(val)); + (void)res; + } +} + +static struct io_uring_sqe *sqe_get_safe(struct io_uring *ring_p) +{ + auto sqe_p = io_uring_get_sqe(ring_p); + if (!sqe_p) { + fprintf(stderr, "Fatal: SQ ring is full\n"); + abort(); + } + return sqe_p; +} + +static void buffer_provide(Worker_Ctx *worker_p, int bid) +{ + auto sqe_p = sqe_get_safe(&worker_p->ring); + worker_p->ctxs[bid].type = QE_BUFFER; + worker_p->ctxs[bid].bid = bid; + io_uring_prep_provide_buffers(sqe_p, worker_p->buffers[bid].raw, BUFFER_LENGTH, 1, BGID, bid); + io_uring_sqe_set_data(sqe_p, &worker_p->ctxs[bid]); +} + +static void *worker_thread(void *arg_p) +{ + Worker_Ctx *worker_p = calloc(1, sizeof(Worker_Ctx)); + defer free(worker_p); + + worker_p->node_id = 0; // TODO + worker_p->thread_id = *(int *)arg_p; + worker_p->rx_ctx.type = QE_RX; + worker_p->rx_ctx.bid = -1; + worker_p->shutdown_ctx.type = QE_SHUTDOWN; + worker_p->shutdown_ctx.bid = -1; + worker_p->rx_message_template.msg_namelen = sizeof(struct sockaddr_storage); + + worker_p->sfd = socket(AF_INET, SOCK_DGRAM, 0); + defer if (worker_p->sfd >= 0) close(worker_p->sfd); + if (worker_p->sfd < 0) { + perror("Failed to create socket"); + return nullptr; + } + + auto socket_option_reuse_port = 1; + if (setsockopt( + worker_p->sfd, + SOL_SOCKET, + SO_REUSEPORT, + &socket_option_reuse_port, + sizeof(socket_option_reuse_port) + ) < 0) { + perror("Failed to set SO_REUSEPORT"); + return nullptr; + } + + auto socket_option_timestamping = + SOF_TIMESTAMPING_TX_SOFTWARE | + SOF_TIMESTAMPING_SOFTWARE | + SOF_TIMESTAMPING_OPT_ID; + if (setsockopt( + worker_p->sfd, + SOL_SOCKET, + SO_TIMESTAMPING, + &socket_option_timestamping, + sizeof(socket_option_timestamping) + ) < 0) { + perror("Failed to set SO_TIMESTAMPING"); + return nullptr; + } + + struct sockaddr_in address = { + .sin_family = AF_INET, + .sin_port = htons(PORT), + .sin_addr.s_addr = INADDR_ANY + }; + if (bind(worker_p->sfd, (struct sockaddr *)&address, sizeof(address)) < 0) { + perror("Failed to bind socket"); + return nullptr; + } + + struct io_uring_params ring_params = {0}; + ring_params.flags = IORING_SETUP_SQPOLL; + ring_params.sq_thread_idle = SQ_THREAD_IDLE_MS; + + defer io_uring_queue_exit(&worker_p->ring); + if (io_uring_queue_init_params(BUFFERS_LENGTH * 2, &worker_p->ring, &ring_params) < 0) { + perror("Failed to setup I/O queues"); + return nullptr; + } + + auto sqe_p = sqe_get_safe(&worker_p->ring); + io_uring_prep_provide_buffers(sqe_p, worker_p->buffers, BUFFER_LENGTH, BUFFERS_LENGTH, BGID, 0); + io_uring_sqe_set_data(sqe_p, nullptr); + io_uring_submit(&worker_p->ring); + + struct io_uring_cqe *cqe_p; + io_uring_wait_cqe(&worker_p->ring, &cqe_p); + io_uring_cqe_seen(&worker_p->ring, cqe_p); + + sqe_p = sqe_get_safe(&worker_p->ring); + io_uring_prep_recvmsg_multishot(sqe_p, worker_p->sfd, &worker_p->rx_message_template, 0); + sqe_p->flags |= IOSQE_BUFFER_SELECT; + sqe_p->buf_group = BGID; + io_uring_sqe_set_data(sqe_p, &worker_p->rx_ctx); + + sqe_p = sqe_get_safe(&worker_p->ring); + io_uring_prep_poll_add(sqe_p, shutdown_efd_g, POLLIN); + io_uring_sqe_set_data(sqe_p, &worker_p->shutdown_ctx); + + io_uring_submit(&worker_p->ring); + + while (keep_running_g) { + int ret = io_uring_wait_cqe(&worker_p->ring, &cqe_p); + if (ret < 0) { + if (ret == -EINTR) continue; + perror("io_uring_wait_cqe error"); + break; + } + + unsigned head; + unsigned count = 0; + + io_uring_for_each_cqe(&worker_p->ring, head, cqe_p) { + QE_Ctx *ctx_p = io_uring_cqe_get_data(cqe_p); + + if (ctx_p && ctx_p->type == QE_RX) { + if (cqe_p->flags & IORING_CQE_F_BUFFER) { + auto bid = (int)(cqe_p->flags >> IORING_CQE_BUFFER_SHIFT); + + if (cqe_p->res > 0) { + auto hdr_p = (struct io_uring_recvmsg_out *)worker_p->buffers[bid].raw; + auto client_address_p = io_uring_recvmsg_name(hdr_p); + auto payload_p = (uint8_t *)io_uring_recvmsg_payload(hdr_p, &worker_p->rx_message_template); + + auto actual_payload_length = io_uring_recvmsg_payload_length( + hdr_p, cqe_p->res, &worker_p->rx_message_template + ); + + struct timespec rx_timespec = {0}; + struct cmsghdr *cmsg_p = io_uring_recvmsg_cmsg_firsthdr(hdr_p, &worker_p->rx_message_template); + while (cmsg_p != nullptr) { + if (cmsg_p->cmsg_level == SOL_SOCKET && cmsg_p->cmsg_type == SCM_TIMESTAMPNS) { + memcpy(&rx_timespec, CMSG_DATA(cmsg_p), sizeof(struct timespec)); + break; + } + cmsg_p = io_uring_recvmsg_cmsg_nexthdr(hdr_p, &worker_p->rx_message_template, cmsg_p); + } + Timestamp rx_timestamp = timestamp_from_timespec(&rx_timespec); + + size_t max_payload_length = BUFFER_LENGTH - (size_t)(payload_p - worker_p->buffers[bid].raw); + size_t response_length = packet_handle( + worker_p->node_id, + worker_p->thread_id, + (struct sockaddr_storage *)client_address_p, + hdr_p->namelen, + payload_p, + actual_payload_length, + max_payload_length, + rx_timestamp + ); + + if (response_length > 0) { + worker_p->send_iovs[bid].iov_base = payload_p; + worker_p->send_iovs[bid].iov_len = response_length; + + worker_p->send_messages[bid].msg_name = client_address_p; + worker_p->send_messages[bid].msg_namelen = hdr_p->namelen; + worker_p->send_messages[bid].msg_iov = &worker_p->send_iovs[bid]; + worker_p->send_messages[bid].msg_iovlen = 1; + + worker_p->ctxs[bid].type = QE_SEND; + worker_p->ctxs[bid].bid = bid; + + auto send_sqe_p = sqe_get_safe(&worker_p->ring); + io_uring_prep_sendmsg(send_sqe_p, worker_p->sfd, &worker_p->send_messages[bid], 0); + io_uring_sqe_set_data(send_sqe_p, &worker_p->ctxs[bid]); + } else { + buffer_provide(worker_p, bid); + } + } else { + buffer_provide(worker_p, bid); + } + } + + if (!(cqe_p->flags & IORING_CQE_F_MORE)) { + if (cqe_p->res < 0 && cqe_p->res != -ENOBUFS) { + usleep(50000); + } + if (keep_running_g) { + auto rearm_sqe_p = sqe_get_safe(&worker_p->ring); + io_uring_prep_recvmsg_multishot(rearm_sqe_p, worker_p->sfd, &worker_p->rx_message_template, 0); + rearm_sqe_p->flags |= IOSQE_BUFFER_SELECT; + rearm_sqe_p->buf_group = BGID; + io_uring_sqe_set_data(rearm_sqe_p, &worker_p->rx_ctx); + } + } + } else if (ctx_p && ctx_p->type == QE_SEND) { + buffer_provide(worker_p, ctx_p->bid); + } + + count++; + } + + io_uring_cq_advance(&worker_p->ring, count); + io_uring_submit(&worker_p->ring); + } + + return nullptr; +} + +int main() +{ + // E.g. for IDs generation + if (sodium_init() < 0) { + fprintf(stderr, "Failed to init libsodium\n"); + return EXIT_FAILURE; + } + + shutdown_efd_g = eventfd(0, EFD_NONBLOCK); + if (shutdown_efd_g < 0) { + perror("Failed to create eventfd"); + return EXIT_FAILURE; + } + defer close(shutdown_efd_g); + + signal(SIGINT, signal_handle); + signal(SIGTERM, signal_handle); + + auto cores_count = sysconf(_SC_NPROCESSORS_ONLN); + pthread_t threads[cores_count]; + int thread_ids[cores_count]; + + for (int i = 0; i < cores_count; i++) { + thread_ids[i] = i; + pthread_create(&threads[i], nullptr, worker_thread, &thread_ids[i]); + } + + for (int i = 0; i < cores_count; i++) { + pthread_join(threads[i], nullptr); + } + + return EXIT_SUCCESS; +} -- cgit