diff --git a/Makefile b/Makefile index 880c721..663e5da 100644 --- a/Makefile +++ b/Makefile @@ -3,6 +3,7 @@ SDL ?= ../SDL DEBUG = -g -gdwarf-4 CFLAGS += -Wall -Werror -Wfatal-errors +CFLAGS += -Wno-error=unused-function CFLAGS += -std=c2x CFLAGS += -I$(SDL)/include -D_REENTRANT CFLAGS += $(shell pkg-config --cflags freetype2) @@ -16,10 +17,14 @@ OPT = -O3 -march=native SERIAL_FORWARDER_OBJS = \ serial_forwarder.o \ serial.o \ - parse_serial.o + parse_serial.o \ + packet.o \ + ping_pong.o TIME_DISPLAY_OBJS = \ - time_display.o + time_display.o \ + packet.o \ + ping_pong.o all: serial_forwarder time_display diff --git a/link.c b/link.c index e69de29..0097c6d 100644 --- a/link.c +++ b/link.c @@ -0,0 +1,11 @@ +#include "link.h" + +void handle_link_pong(int sockfd, + struct link_state * link_state, + struct timespec * time) +{ + int ret = clock_gettime(CLOCK_MONOTONIC_RAW, &link_state->recv_pong); + assert(ret != -1); + + printf("pong\n"); +} diff --git a/link.h b/link.h index 7ac7d2c..50aa1b8 100644 --- a/link.h +++ b/link.h @@ -1,10 +1,15 @@ #pragma once #include +#include + +#define RTT_AVERAGE_SAMPLES 4 struct link_state { uint64_t send_sequence; uint64_t recv_sequence; - struct timespec send_ping; - struct timespec recv_ping; + struct timespec remote_average_rtt; // relative + struct timespec ping_pong_rtt[RTT_AVERAGE_SAMPLES]; // relative + struct timespec last_pong; // absolute + int rtt_ix; }; diff --git a/packet.c b/packet.c new file mode 100644 index 0000000..a4cbb76 --- /dev/null +++ b/packet.c @@ -0,0 +1,123 @@ +#include +#include +#include + +#include "link.h" +#include "timer.h" +#include "packet.h" + +int packet_send_ping(int sockfd, + struct sockaddr_in6 * dest_addr) +{ + struct packet_ping_pong pkt; + pkt.event.type = EVENT_TYPE__PING; + pkt.event.sequence = 0; + { + int ret = clock_gettime(CLOCK_MONOTONIC_RAW, &pkt.time); + assert(ret != -1); + } + + int ret = sendto(sockfd, + &pkt, (sizeof (pkt)), + 0, + (struct sockaddr *)dest_addr, + (sizeof (struct sockaddr_in6))); + + if (ret == -1) { + perror("packet_send_ping sendto"); + return -1; + } + + return 0; +} + +int packet_send_pong(int sockfd, + struct sockaddr_in6 * dest_addr, + struct timespec * time) +{ + struct packet_ping_pong pkt; + pkt.event.type = EVENT_TYPE__PONG; + pkt.event.sequence = 0; + memcpy(&pkt.time, time, (sizeof (struct timespec))); + + int ret = sendto(sockfd, + &pkt, (sizeof (pkt)), + 0, + (struct sockaddr *)dest_addr, + (sizeof (struct sockaddr_in6))); + + if (ret == -1) { + perror("packet_send_pong sendto"); + return -1; + } + + return 0; +} + +int packet_send_average_rtt(int sockfd, + struct sockaddr_in6 * dest_addr, + struct timespec * time) +{ + struct packet_ping_pong pkt; + pkt.event.type = EVENT_TYPE__AVERAGE_RTT; + pkt.event.sequence = 0; + memcpy(&pkt.time, time, (sizeof (struct timespec))); + + int ret = sendto(sockfd, + &pkt, (sizeof (pkt)), + 0, + (struct sockaddr *)dest_addr, + (sizeof (struct sockaddr_in6))); + + if (ret == -1) { + perror("packet_send_average_rtt sendto"); + return -1; + } + + return 0; +} + +int packet_send_start_event(int sockfd, + struct sockaddr_in6 * dest_addr, + struct timer_state * timer_state, + struct link_state * link_state) +{ + struct packet_start pkt; + pkt.event.type = EVENT_TYPE__TIME_START; + pkt.event.sequence = link_state->send_sequence++; + + int ret = sendto(sockfd, + &pkt, (sizeof (pkt)), + 0, + (struct sockaddr *)dest_addr, + (sizeof (struct sockaddr_in6))); + if (ret == -1) { + perror("packet_send_start_event sendto"); + return -1; + } + + return 0; +} + +int packet_send_stopwatch_event(int sockfd, + struct sockaddr_in6 * dest_addr, + struct timer_state * timer_state, + struct link_state * link_state) +{ + struct packet_stopwatch pkt; + pkt.event.type = EVENT_TYPE__TIME_STOP; + pkt.event.sequence = link_state->send_sequence++; + memcpy(&pkt.stopwatch_time, &timer_state->time, (sizeof (struct stopwatch_time))); + + int ret = sendto(sockfd, + &pkt, (sizeof (pkt)), + 0, + (struct sockaddr *)dest_addr, + (sizeof (struct sockaddr_in6))); + if (ret == -1) { + perror("packet_send_stopwatch_event sendto"); + return -1; + } + + return 0; +} diff --git a/packet.h b/packet.h index 98d40d1..915939d 100644 --- a/packet.h +++ b/packet.h @@ -2,15 +2,19 @@ #include #include +#include +#include #include "timer.h" +#include "link.h" -#define EVENT_TYPE__INVALID 0x00000000 -#define EVENT_TYPE__TIME_START 0x12858517 -#define EVENT_TYPE__TIME_STOP 0xdf75d3f8 -#define EVENT_TYPE__RTT_DELAY 0xc7065931 -#define EVENT_TYPE__PING 0x0ba0d4c2 -#define EVENT_TYPE__ACK 0xfde7959f +#define EVENT_TYPE__INVALID 0x00000000 +#define EVENT_TYPE__TIME_START 0x12858517 +#define EVENT_TYPE__TIME_STOP 0xdf75d3f8 +#define EVENT_TYPE__AVERAGE_RTT 0xc7065931 +#define EVENT_TYPE__PING 0x0ba0d4c2 +#define EVENT_TYPE__PONG 0x49a87c9d +#define EVENT_TYPE__ACK 0xfde7959f static_assert((sizeof (struct timespec)) == 8 + 8); @@ -45,3 +49,26 @@ struct packet_rtt_delay { }; static_assert((sizeof (struct packet_rtt_delay)) == (sizeof (struct event)) + (sizeof (struct timespec))); + +struct packet_ping_pong { + struct event event; + struct timespec time; +}; + +int packet_send_ping(int sockfd, + struct sockaddr_in6 * dest_addr); +int packet_send_pong(int sockfd, + struct sockaddr_in6 * dest_addr, + struct timespec * time); +int packet_send_average_rtt(int sockfd, + struct sockaddr_in6 * dest_addr, + struct timespec * time); + +int packet_send_start_event(int sockfd, + struct sockaddr_in6 * dest_addr, + struct timer_state * timer_state, + struct link_state * link_state); +int packet_send_stopwatch_event(int sockfd, + struct sockaddr_in6 * dest_addr, + struct timer_state * timer_state, + struct link_state * link_state); diff --git a/ping_pong.c b/ping_pong.c new file mode 100644 index 0000000..4ad4402 --- /dev/null +++ b/ping_pong.c @@ -0,0 +1,71 @@ +#include + +#include "ping_pong.h" +#include "timespec.h" +#include "packet.h" + +int handle_ping_pong(int sockfd, + struct sockaddr_in6 * dest_addr, + void * buf, ssize_t length, + struct link_state * link_state + ) +{ + struct event * evt = (struct event *)buf; + switch (evt->type) { + case EVENT_TYPE__PING: + { + if (length != (sizeof (struct packet_ping_pong))) { + printf("handle_ping_pong: ping: invalid length\n"); + return 0; // recoverable + } + + printf("recv ping; send pong\n"); + struct packet_ping_pong * ping_pkt = (struct packet_ping_pong *)buf; + int ret = packet_send_pong(sockfd, dest_addr, &ping_pkt->time); + if (ret == -1) { + return -1; + } + } + break; + case EVENT_TYPE__PONG: + { + if (length != (sizeof (struct packet_ping_pong))) { + printf("handle_ping_pong: pong: invalid length\n"); + return 0; // recoverable + } + + printf("recv pong\n"); + struct packet_ping_pong * ping_pkt = (struct packet_ping_pong *)buf; + struct timespec now; + int ret = clock_gettime(CLOCK_MONOTONIC_RAW, &now); + assert(ret != -1); + link_state->last_pong = now; + + struct timespec * rtt = &link_state->ping_pong_rtt[link_state->rtt_ix]; + link_state->rtt_ix = (link_state->rtt_ix + 1) % RTT_AVERAGE_SAMPLES; + *rtt = diff_timespec(&now, &ping_pkt->time); + printf("rtt: %ld.%09ld\n", rtt->tv_sec, rtt->tv_nsec); + struct timespec average_rtt = average_timespec(link_state->ping_pong_rtt, RTT_AVERAGE_SAMPLES); + + printf("send_average_rtt\n"); + ret = packet_send_average_rtt(sockfd, dest_addr, &average_rtt); + if (ret == -1) { + return -1; + } + } + break; + case EVENT_TYPE__AVERAGE_RTT: + { + if (length != (sizeof (struct packet_ping_pong))) { + printf("handle_ping_pong: average_rtt: invalid length\n"); + return 0; // recoverable + } + + printf("recv average_rtt\n"); + } + break; + default: + break; + } + return 0; +} diff --git a/ping_pong.h b/ping_pong.h new file mode 100644 index 0000000..c83b7f1 --- /dev/null +++ b/ping_pong.h @@ -0,0 +1,11 @@ +#pragma once + +#include + +#include "link.h" + +int handle_ping_pong(int sockfd, + struct sockaddr_in6 * dest_addr, + void * buf, ssize_t length, + struct link_state * link_state + ); diff --git a/serial_forwarder.c b/serial_forwarder.c index 6029ea6..3894659 100644 --- a/serial_forwarder.c +++ b/serial_forwarder.c @@ -17,15 +17,47 @@ #include "timer.h" #include "parse_serial.h" #include "packet.h" +#include "link.h" +#include "timespec.h" +#include "ping_pong.h" #define PORT 4321 //#define SERIALPORT "/dev/ttyS0" //#define SERIALPORT "/dev/ttyUSB0" #define SERIALPORT "foo.fifo" -static struct sockaddr_in6 dest_addr; +int handle_buf(int sockfd, + struct sockaddr_in6 * dest_addr, + void * buf, ssize_t length, + struct link_state * link_state + ) +{ + struct event * evt = (struct event *)buf; + switch (evt->type) { + case EVENT_TYPE__PING: [[fallthrough]]; + case EVENT_TYPE__PONG: [[fallthrough]]; + case EVENT_TYPE__AVERAGE_RTT: + { + int ret = handle_ping_pong(sockfd, + dest_addr, + buf, length, + link_state); + if (ret == -1) { + return -1; + } + } + break; + default: + printf("unhandled event %d\n", evt->type); + break; + } -int handle_sockfd(int sockfd) + return 0; +} + +int handle_sockfd(int sockfd, + struct sockaddr_in6 * dest_addr, + struct link_state * link_state) { struct sockaddr_in6 src_addr; socklen_t addrlen = (sizeof (struct sockaddr_in6)); @@ -51,13 +83,18 @@ int handle_sockfd(int sockfd) inet_ntop(AF_INET6, &src_addr.sin6_addr, src_addr_str, INET6_ADDRSTRLEN); printf("Received packet from %s:%d\n", src_addr_str, ntohs(src_addr.sin6_port)); printf("length: %ld\n", recv_len); + + int ret = handle_buf(sockfd, dest_addr, buf, recv_len, link_state); + if (ret == -1) { + return -1; + } } } int rearm_timer(int timerfd) { struct itimerspec value; - value.it_value.tv_sec = 1; + value.it_value.tv_sec = 5; value.it_value.tv_nsec = 0; value.it_interval.tv_sec = 0; value.it_interval.tv_nsec = 0; @@ -71,7 +108,10 @@ int rearm_timer(int timerfd) return 0; } -int handle_timerfd(int timerfd) +int handle_timerfd(int timerfd, + int sockfd, + struct sockaddr_in6 * dest_addr, + struct link_state * link_state) { uint64_t expired_count = 0; while (true) { @@ -92,45 +132,9 @@ int handle_timerfd(int timerfd) rearm_timer(timerfd); printf("do timer stuff\n"); - return 0; -} - -int send_stopwatch_event(int sockfd, - struct timer_state * timer_state, - struct link_state * link_state) -{ - struct packet_stopwatch pkt; - pkt.event.type = EVENT_TYPE__TIME_STOP; - pkt.event.sequence = link_state.send_sequence++; - memcpy(&pkt.stopwatch_time, &timer_state->time, (sizeof (struct stopwatch_time))); - - int ret = sendto(sockfd, - &pkt, (sizeof (struct packet_stopwatch)), - 0, - (struct sockaddr *)&dest_addr, - (sizeof (struct sockaddr_in6))); + int ret = packet_send_ping(sockfd, dest_addr); if (ret == -1) { - perror("sendto()"); - } - - return 0; -} - -int send_start_event(int sockfd, - struct timer_state * timer_state, - struct link_state * link_state) -{ - struct packet_start pkt; - pkt.event.type = EVENT_TYPE__TIME_START; - pkt.event.sequence = link_state.send_sequence++; - - int ret = sendto(sockfd, - &pkt, (sizeof (struct packet_stopwatch)), - 0, - (struct sockaddr *)&dest_addr, - (sizeof (struct sockaddr_in6))); - if (ret == -1) { - perror("sendto()"); + return -1; } return 0; @@ -138,6 +142,7 @@ int send_start_event(int sockfd, int handle_serialfd(int serialfd, int sockfd, + struct sockaddr_in6 * dest_addr, struct parser_state * parser_state, struct timer_state * timer_state, struct link_state * link_state) @@ -168,13 +173,13 @@ int handle_serialfd(int serialfd, int ret; switch (type) { case EVENT_TYPE__TIME_STOP: - ret = send_stopwatch_event(sockfd, timer_state, link_state); + ret = packet_send_stopwatch_event(sockfd, dest_addr, timer_state, link_state); if (ret == -1) { return -1; } break; case EVENT_TYPE__TIME_START: - ret = send_start_event(sockfd, timer_state, link_state); + ret = packet_send_start_event(sockfd, dest_addr, timer_state, link_state); if (ret == -1) { return -1; } @@ -239,7 +244,7 @@ int main(void) return -1; } - if (0) { + { struct epoll_event ev; ev.events = EPOLLIN | EPOLLET; ev.data.fd = timerfd; @@ -273,6 +278,7 @@ int main(void) } } + struct sockaddr_in6 dest_addr; dest_addr.sin6_family = AF_INET6; dest_addr.sin6_port = htons(1234); ret = inet_pton(AF_INET6, "::1", &dest_addr.sin6_addr); @@ -293,17 +299,21 @@ int main(void) for (int n = 0; n < nfds; ++n) { if (events[n].data.fd == sockfd) { - ret = handle_sockfd(sockfd); + ret = handle_sockfd(sockfd, &dest_addr, &link_state); if (ret == -1) return -1; } else if (events[n].data.fd == timerfd) { - ret = handle_timerfd(timerfd); + ret = handle_timerfd(timerfd, + sockfd, + &dest_addr, + &link_state); if (ret == -1) return -1; } else if (events[n].data.fd == serialfd) { fprintf(stderr, "handle_serialfd\n"); ret = handle_serialfd(serialfd, sockfd, + &dest_addr, &parser_state, &timer_state, &link_state); diff --git a/time_display.c b/time_display.c index 2d709ba..78ecb13 100644 --- a/time_display.c +++ b/time_display.c @@ -15,6 +15,8 @@ #include "timer.h" #include "bufsize.h" #include "packet.h" +#include "timespec.h" +#include "ping_pong.h" struct glyph { int32_t width; @@ -162,35 +164,61 @@ int max(int a, int b) { #define PORT 1234 -void handle_buf(void * buf, ssize_t length, - struct timer_state * timer_state) +int handle_buf(int sockfd, + struct sockaddr_in6 * dest_addr, + void * buf, ssize_t length, + struct timer_state * timer_state, + struct link_state * link_state) { struct event * evt = (struct event *)buf; switch (evt->type) { case EVENT_TYPE__TIME_STOP: if (length != (sizeof (struct packet_stopwatch))) { - printf("handle_buf: event_stopwatch: invalid length\n"); + printf("handle_buf: packet_stopwatch: invalid length\n"); + return 0; } - printf("handle_buf: event_stopwatch: valid length\n"); - struct packet_stopwatch * time_evt = (struct packet_stopwatch *)buf; - memcpy(&timer_state->time, &time_evt->stopwatch_time, (sizeof (struct stopwatch_time))); + struct packet_stopwatch * time_pkt = (struct packet_stopwatch *)buf; + memcpy(&timer_state->time, &time_pkt->stopwatch_time, (sizeof (struct stopwatch_time))); timer_state->status = TIMER_STOPPED; break; case EVENT_TYPE__TIME_START: + if (length != (sizeof (struct event))) { + printf("handle_buf: time start: invalid length\n"); + return 0; + } + int ret = clock_gettime(CLOCK_MONOTONIC_RAW, &timer_state->counter.start); assert(ret != -1); timer_state->counter.offset.tv_sec = 0; timer_state->counter.offset.tv_nsec = 0; timer_state->status = TIMER_RUNNING; break; + case EVENT_TYPE__PING: [[fallthrough]]; + case EVENT_TYPE__PONG: [[fallthrough]]; + case EVENT_TYPE__AVERAGE_RTT: + { + int ret = handle_ping_pong(sockfd, + dest_addr, + buf, length, + link_state); + if (ret == -1) { + return -1; + } + } + break; default: printf("unhandled event %d\n", evt->type); break; } + + return 0; } -int handle_sockfd(int sockfd, struct timer_state * timer_state) +int handle_sockfd(int sockfd, + struct sockaddr_in6 * dest_addr, + struct timer_state * timer_state, + struct link_state * link_state) { char buf[BUFSIZE]; struct sockaddr_in6 src_addr; @@ -215,12 +243,33 @@ int handle_sockfd(int sockfd, struct timer_state * timer_state) inet_ntop(AF_INET6, &src_addr.sin6_addr, src_addr_str, INET6_ADDRSTRLEN); printf("Received packet from %s:%d\n", src_addr_str, ntohs(src_addr.sin6_port)); printf("length: %ld\n", recv_len); - handle_buf(buf, recv_len, timer_state); + int ret = handle_buf(sockfd, dest_addr, buf, recv_len, timer_state, link_state); + if (ret == -1) { + return -1; + } } return 0; } +char * format_base10(char * buf, int value, int digits) +{ + static int table[10] = { + 1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000 + }; + + if (digits <= 0) + return buf; + + int power = table[digits - 1]; + for (int i = 0; i < digits; i++) { + buf[i] = '0' + ((value / power) % 10); + power /= 10; + } + + return buf + digits; +} + int main() { int ret; @@ -279,15 +328,32 @@ int main() /* */ + struct sockaddr_in6 dest_addr; + dest_addr.sin6_family = AF_INET6; + dest_addr.sin6_port = htons(4321); + ret = inet_pton(AF_INET6, "::1", &dest_addr.sin6_addr); + assert(ret == 1); + struct timer_state timer_state = {0}; + struct link_state link_state = {0}; while (1) { - handle_sockfd(sockfd, &timer_state); + handle_sockfd(sockfd, &dest_addr, &timer_state, &link_state); - char str_buf[16]; + char str_buf[20 * 2 + 1 + 1]; switch (timer_state.status) { case TIMER_STOPPED: - snprintf(str_buf, (sizeof (str_buf)) - 1, "%d.%02d", timer_state.time.integer_value, timer_state.time.fraction_value); + { + char * bufi = str_buf; + bufi = format_base10(bufi, + timer_state.time.integer_value, + timer_state.time.integer_digits); + *bufi++ = '.'; + bufi = format_base10(bufi, + timer_state.time.fraction_value, + timer_state.time.fraction_digits); + *bufi = 0; + } break; case TIMER_RUNNING: { @@ -295,12 +361,12 @@ int main() int ret = clock_gettime(CLOCK_MONOTONIC_RAW, &now); assert(ret != -1); - double now_d = (double)now.tv_sec + ((double)now.tv_nsec / 1000000000.f); - struct timespec * start = &timer_state.counter.start; - double start_d = (double)start->tv_sec + ((double)start->tv_nsec / 1000000000.f); - double duration = now_d - start_d; + struct timespec duration = diff_timespec(&now, &timer_state.counter.start); - snprintf(str_buf, (sizeof (str_buf)) - 1, "%.02f", duration); + snprintf(str_buf, (sizeof (str_buf)) - 1, + "%ld.%ld", + duration.tv_sec, + duration.tv_nsec / (1000000000 / 100)); // 2 digits } break; default: @@ -318,8 +384,8 @@ int main() max_length = max(min_length, length); last_width = window_width; - int divisor = max(1, (max_length * 8 / 10)); - int font_size = (window_width * 95 / 100) / divisor; + int divisor = max(1, max_length); + int font_size = (window_width * 150 / 100) / divisor; printf("fs %d %d max_length: %d\n", font_size, window_width, max_length); load_font(renderer, font_size); } diff --git a/timespec.h b/timespec.h new file mode 100644 index 0000000..85c58cc --- /dev/null +++ b/timespec.h @@ -0,0 +1,50 @@ +#pragma once + +#include +#include +#include +#include + +static struct timespec diff_timespec(const struct timespec * a, + const struct timespec * b) +{ + struct timespec c = { + .tv_sec = a->tv_sec - b->tv_sec, + .tv_nsec = a->tv_nsec - b->tv_nsec + }; + if (c.tv_nsec < 0) { + c.tv_nsec += 1000000000; + c.tv_sec -= 1; + } + return c; +} + +static struct timespec add_timespec(const struct timespec * a, + const struct timespec * b) +{ + struct timespec c = { + .tv_sec = a->tv_sec + b->tv_sec, + .tv_nsec = a->tv_nsec + b->tv_nsec + }; + + int64_t rem = c.tv_nsec / 1000000000; + c.tv_nsec = c.tv_nsec % 1000000000; + c.tv_sec += rem; + return c; +} + +static struct timespec average_timespec(const struct timespec * ts, + int length) +{ + struct timespec c = {0}; + for (int i = 0; i < length; i++) { + c = add_timespec(&c, &ts[i]); + //printf("rtt[%d]: %ld.%09ld\n", i, ts[i].tv_sec, ts[i].tv_nsec); + } + int64_t rem = c.tv_sec % length; + c.tv_sec /= length; + c.tv_nsec = (c.tv_nsec / length) + (rem * 1000000000 / length); + assert(c.tv_nsec < 1000000000); + //printf("average: %ld.%09ld\n", c.tv_sec, c.tv_nsec); + return c; +}