From 3c3ea236f55e1f47336ae7d5779bd51ea1d613ea Mon Sep 17 00:00:00 2001 From: Zack Buhman Date: Tue, 24 Sep 2024 21:45:28 -0500 Subject: [PATCH] add support for multiple time displays --- gpio.c | 51 +++++++++--- gpio.h | 5 +- link.h | 2 + ping_pong.c | 5 +- ping_pong.h | 1 - serial_forwarder.c | 195 ++++++++++++++++++++++++--------------------- time_display.c | 33 ++++---- 7 files changed, 165 insertions(+), 127 deletions(-) diff --git a/gpio.c b/gpio.c index a37cbfc..43a7d14 100644 --- a/gpio.c +++ b/gpio.c @@ -45,7 +45,11 @@ int gpio_open(const char * path, .num_lines = 3, }; int ret1 = ioctl(gpio_state->chip_fd, GPIO_V2_GET_LINE_IOCTL, &request_leds); - assert(ret1 != -1); + if (ret1 == -1) { + gpio_state->chip_fd = -1; + perror("GPIO_V2_GET_LINE_IOCTL timer-leds"); + return -1; + } gpio_state->led_request_fd = request_leds.fd; struct gpio_v2_line_request request_switch = { @@ -57,7 +61,11 @@ int gpio_open(const char * path, .num_lines = 2, }; int ret2 = ioctl(gpio_state->chip_fd, GPIO_V2_GET_LINE_IOCTL, &request_switch); - assert(ret2 != -1); + if (ret2 == -1) { + gpio_state->chip_fd = -1; + perror("GPIO_V2_GET_LINE_IOCTL timer-switch"); + return -1; + } gpio_state->switch_request_fd = request_switch.fd; return 0; @@ -65,6 +73,10 @@ int gpio_open(const char * path, void gpio_set_values(struct gpio_state * gpio_state, uint64_t bits) { + if (gpio_state->chip_fd == -1) { + return; + } + struct gpio_v2_line_values values = { .bits = bits, .mask = 0b111, @@ -76,6 +88,10 @@ void gpio_set_values(struct gpio_state * gpio_state, uint64_t bits) uint64_t gpio_get_values(struct gpio_state * gpio_state) { + if (gpio_state->chip_fd == -1) { + return 0; + } + struct gpio_v2_line_values values = { .mask = 0b11, }; @@ -86,22 +102,33 @@ uint64_t gpio_get_values(struct gpio_state * gpio_state) return (~values.bits) & 0b11; } -void gpio_set_values_from_link_state(struct gpio_state * gpio_state, - struct link_state * link_state) +void gpio_set_values_from_link_states(struct gpio_state * gpio_state, + struct link_state * link_states, + int link_states_length) { struct timespec now; int ret = clock_gettime(CLOCK_MONOTONIC_RAW, &now); assert(ret != -1); - struct timespec delta = timespec_sub(&now, &link_state->last_pong); - if (delta.tv_sec >= LAST_PONG_TIMEOUT) { - printf("gpio pong timeout %ld\n", delta.tv_sec); - if (gpio_state->chip_fd != -1) - gpio_set_values(gpio_state, RED); + int error = 0; + + for (int i = 0; i < link_states_length; i++) { + struct timespec delta = timespec_sub(&now, &link_states[i].last_pong); + if (delta.tv_sec >= LAST_PONG_TIMEOUT) { + printf("[%d] gpio pong timeout %ld\n", i, delta.tv_sec); + error |= 1 << (i % 2); + } else { + printf("[%d] gpio pong ok %ld\n", i, delta.tv_sec); + } + } + + if (error == 0) { + gpio_set_values(gpio_state, GREEN); } else { - printf("gpio pong ok %ld\n", delta.tv_sec); - if (gpio_state->chip_fd != -1) - gpio_set_values(gpio_state, GREEN); + int color = 0; + if (error & 0b01) color |= RED; + if (error & 0b10) color |= BLUE; + gpio_set_values(gpio_state, color); } } diff --git a/gpio.h b/gpio.h index 1ccb7a0..2c8f792 100644 --- a/gpio.h +++ b/gpio.h @@ -15,5 +15,6 @@ void gpio_set_values(struct gpio_state * gpio_state, uint64_t bits); uint64_t gpio_get_values(struct gpio_state * gpio_state); -void gpio_set_values_from_link_state(struct gpio_state * gpio_state, - struct link_state * link_state); +void gpio_set_values_from_link_states(struct gpio_state * gpio_state, + struct link_state link_states[], + int link_states_length); diff --git a/link.h b/link.h index 50aa1b8..2eb3eef 100644 --- a/link.h +++ b/link.h @@ -2,6 +2,7 @@ #include #include +#include #define RTT_AVERAGE_SAMPLES 4 @@ -12,4 +13,5 @@ struct link_state { struct timespec ping_pong_rtt[RTT_AVERAGE_SAMPLES]; // relative struct timespec last_pong; // absolute int rtt_ix; + struct sockaddr_in6 dest_addr; }; diff --git a/ping_pong.c b/ping_pong.c index 8768330..85c0ab7 100644 --- a/ping_pong.c +++ b/ping_pong.c @@ -5,7 +5,6 @@ #include "packet.h" int handle_ping_pong(int sockfd, - struct sockaddr_in6 * dest_addr, void * buf, ssize_t length, struct link_state * link_state ) @@ -21,7 +20,7 @@ int handle_ping_pong(int sockfd, //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); + int ret = packet_send_pong(sockfd, &link_state->dest_addr, &ping_pkt->time); if (ret == -1) { return -1; } @@ -47,7 +46,7 @@ int handle_ping_pong(int sockfd, printf("pong rtt: %ld.%09ld\n", rtt->tv_sec, rtt->tv_nsec); struct timespec average_rtt = timespec_average(link_state->ping_pong_rtt, RTT_AVERAGE_SAMPLES); - ret = packet_send_average_rtt(sockfd, dest_addr, &average_rtt); + ret = packet_send_average_rtt(sockfd, &link_state->dest_addr, &average_rtt); if (ret == -1) { return -1; } diff --git a/ping_pong.h b/ping_pong.h index e72a2c1..6fc3281 100644 --- a/ping_pong.h +++ b/ping_pong.h @@ -7,7 +7,6 @@ #define PING_INTERVAL 1 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 283e9bf..5bee9f8 100644 --- a/serial_forwarder.c +++ b/serial_forwarder.c @@ -22,17 +22,24 @@ #include "ping_pong.h" #include "gpio.h" -#define DEST_PORT 1234 -#define DEST_ADDR "fd00::1" +struct addr_port { + const char * addr; + const int port; +}; + +const struct addr_port dests[] = { + {"fd00::10", 1234}, + {"fd00::11", 1234}, +}; +const int dests_length = (sizeof (dests)) / (sizeof (dests[0])); #define PORT 4321 #define SERIALPORT "/dev/ttyS0" //#define SERIALPORT "/dev/ttyUSB0" -//#define SERIALPORT "foo.fifo" +//#define SERIALPORT "test.fifo" int handle_buf(int sockfd, - struct sockaddr_in6 * dest_addr, void * buf, ssize_t length, struct link_state * link_state ) @@ -44,7 +51,6 @@ int handle_buf(int sockfd, case EVENT_TYPE__AVERAGE_RTT: { int ret = handle_ping_pong(sockfd, - dest_addr, buf, length, link_state); if (ret == -1) { @@ -65,8 +71,7 @@ int handle_buf(int sockfd, } int handle_sockfd(int sockfd, - struct sockaddr_in6 * dest_addr, - struct link_state * link_state) + struct link_state link_states[]) { struct sockaddr_in6 src_addr; socklen_t addrlen = (sizeof (struct sockaddr_in6)); @@ -88,15 +93,27 @@ int handle_sockfd(int sockfd, } } - //char src_addr_str[INET6_ADDRSTRLEN]; - //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); + char src_addr_str[INET6_ADDRSTRLEN]; + 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; + for (int i = 0; i < dests_length; i++) { + int cmp = + memcmp(src_addr.sin6_addr.s6_addr, + link_states[i].dest_addr.sin6_addr.s6_addr, + (sizeof (struct in6_addr))); + if (cmp == 0) { + printf("MATCH %d\n", i); + int ret = handle_buf(sockfd, buf, recv_len, &link_states[i]); + if (ret == -1) { + return -1; + } + return 0; + } } + printf("no link_state for %s:%d\n", src_addr_str, ntohs(src_addr.sin6_port)); + return -1; } } @@ -118,8 +135,7 @@ int rearm_timer(int timerfd) } int send_fake_stop_event(int sockfd, - struct sockaddr_in6 * dest_addr, - struct timer_state * timer_state, + struct timer_state * timer_state, // single struct link_state * link_state) { printf("fake stop event"); @@ -130,14 +146,13 @@ int send_fake_stop_event(int sockfd, fake_state.time.fraction_value = 0; fake_state.time.fraction_digits = 0; - int ret = packet_send_stopwatch_event(sockfd, dest_addr, &fake_state, link_state); + int ret = packet_send_stopwatch_event(sockfd, &link_state->dest_addr, &fake_state, link_state); return ret; } #define FAKE_STOP_BITS 0b10 int handle_event(int sockfd, - struct sockaddr_in6 * dest_addr, struct timer_state * timer_state, struct link_state * link_state, struct gpio_state * gpio_state, @@ -147,7 +162,7 @@ int handle_event(int sockfd, switch (type) { case EVENT_TYPE__TIME_STOP: printf("event time stop\n"); - ret = packet_send_stopwatch_event(sockfd, dest_addr, timer_state, link_state); + ret = packet_send_stopwatch_event(sockfd, &link_state->dest_addr, timer_state, link_state); if (ret == -1) { return -1; } @@ -155,9 +170,9 @@ int handle_event(int sockfd, case EVENT_TYPE__TIME_START: printf("event time start\n"); if (gpio_get_values(gpio_state) & FAKE_STOP_BITS) { - ret = send_fake_stop_event(sockfd, dest_addr, timer_state, link_state); + ret = send_fake_stop_event(sockfd, timer_state, link_state); } else { - ret = packet_send_start_event(sockfd, dest_addr, link_state); + ret = packet_send_start_event(sockfd, &link_state->dest_addr, link_state); } if (ret == -1) { return -1; @@ -166,9 +181,9 @@ int handle_event(int sockfd, case EVENT_TYPE__TIME_RESUME: printf("event time resume\n"); if (gpio_get_values(gpio_state) & FAKE_STOP_BITS) { - ret = send_fake_stop_event(sockfd, dest_addr, timer_state, link_state); + ret = send_fake_stop_event(sockfd, timer_state, link_state); } else { - ret = packet_send_resume_event(sockfd, dest_addr, timer_state, link_state); + ret = packet_send_resume_event(sockfd, &link_state->dest_addr, timer_state, link_state); } if (ret == -1) { return -1; @@ -180,43 +195,39 @@ int handle_event(int sockfd, return 0; } -int check_sequence(int sockfd, - struct sockaddr_in6 * dest_addr, - struct timer_state * timer_state, - struct link_state * link_state, - struct gpio_state * gpio_state) +void check_sequence(int sockfd, + struct timer_state * timer_state, + struct link_state link_states[], + struct gpio_state * gpio_state) { - printf("check_sequence send %ld recv: %ld\n", link_state->send_sequence, link_state->recv_sequence); + for (int i = 0; i < dests_length; i++) { + printf("[%d] check_sequence send %ld recv: %ld\n", i, link_states[i].send_sequence, link_states[i].recv_sequence); - if (link_state->send_sequence != link_state->recv_sequence) { - switch (timer_state->status) { - case TIMER_STOPPED: - handle_event(sockfd, - dest_addr, - timer_state, - link_state, - gpio_state, - EVENT_TYPE__TIME_STOP); - break; - case TIMER_RUNNING: - handle_event(sockfd, - dest_addr, - timer_state, - link_state, - gpio_state, - EVENT_TYPE__TIME_RESUME); - break; + if (link_states[i].send_sequence != link_states[i].recv_sequence) { + switch (timer_state->status) { + case TIMER_STOPPED: + handle_event(sockfd, + timer_state, + &link_states[i], + gpio_state, + EVENT_TYPE__TIME_STOP); + break; + case TIMER_RUNNING: + handle_event(sockfd, + timer_state, + &link_states[i], + gpio_state, + EVENT_TYPE__TIME_RESUME); + break; + } } } - - return 0; } int handle_timerfd(int timerfd, int sockfd, - struct sockaddr_in6 * dest_addr, struct timer_state * timer_state, - struct link_state * link_state, + struct link_state link_states[], struct gpio_state * gpio_state) { uint64_t expired_count = 0; @@ -236,37 +247,34 @@ int handle_timerfd(int timerfd, rearm_timer(timerfd); - int ret; - ret = packet_send_ping(sockfd, dest_addr); - if (ret == -1) { - return -1; + // broadcast ping + for (int i = 0; i < dests_length; i++) { + int ret = packet_send_ping(sockfd, &link_states[i].dest_addr); + (void)ret; // ignore error } - ret = check_sequence(sockfd, - dest_addr, - timer_state, - link_state, - gpio_state); - if (ret == -1) { - return -1; - } + check_sequence(sockfd, + timer_state, + link_states, + gpio_state); - gpio_set_values_from_link_state(gpio_state, - link_state); + gpio_set_values_from_link_states(gpio_state, + link_states, + dests_length); return 0; } 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, + struct link_state * link_states, struct gpio_state * gpio_state) { uint8_t buf[BUFSIZE]; + int error = 0; while (true) { ssize_t len = read(serialfd, buf, (sizeof (buf))); if (len == -1) { @@ -288,18 +296,19 @@ int handle_serialfd(int serialfd, uint32_t type = handle_parse(buf, len, parser_state, timer_state); - int ret = handle_event(sockfd, - dest_addr, - timer_state, - link_state, - gpio_state, - type); - if (ret == -1) { - return -1; + + // broadcast event + for (int i = 0; i < dests_length; i++) { + int ret = handle_event(sockfd, + timer_state, + &link_states[i], + gpio_state, + type); + error |= ret; } } - return 0; + return error; } int main(void) @@ -371,9 +380,11 @@ int main(void) return -1; } - ret = set_terminal_attributes(serialfd); - if (ret == -1) { - return -1; + if (strstr(SERIALPORT, "tty") != NULL) { + ret = set_terminal_attributes(serialfd); + if (ret == -1) { + return -1; + } } { @@ -387,17 +398,21 @@ int main(void) } } - struct sockaddr_in6 dest_addr; - dest_addr.sin6_family = AF_INET6; - dest_addr.sin6_port = htons(DEST_PORT); - ret = inet_pton(AF_INET6, DEST_ADDR, &dest_addr.sin6_addr); - assert(ret == 1); - struct parser_state parser_state = {0}; struct timer_state timer_state = {0}; - struct link_state link_state = {0}; + struct link_state link_states[dests_length]; + memset(link_states, 0, (sizeof (link_states))); struct gpio_state gpio_state = {0}; + for (int i = 0; i < dests_length; i++) { + link_states[i].dest_addr.sin6_family = AF_INET6; + link_states[i].dest_addr.sin6_port = htons(dests[i].port); + ret = inet_pton(AF_INET6, + dests[i].addr, + &link_states[i].dest_addr.sin6_addr); + assert(ret == 1); + } + gpio_open("/dev/gpiochip0", &gpio_state); // ignored gpio_open error @@ -410,15 +425,14 @@ int main(void) for (int n = 0; n < nfds; ++n) { if (events[n].data.fd == sockfd) { - ret = handle_sockfd(sockfd, &dest_addr, &link_state); + ret = handle_sockfd(sockfd, link_states); if (ret == -1) return -1; } else if (events[n].data.fd == timerfd) { ret = handle_timerfd(timerfd, sockfd, - &dest_addr, &timer_state, - &link_state, + link_states, &gpio_state); if (ret == -1) return -1; @@ -426,10 +440,9 @@ int main(void) fprintf(stderr, "handle_serialfd\n"); ret = handle_serialfd(serialfd, sockfd, - &dest_addr, &parser_state, &timer_state, - &link_state, + link_states, &gpio_state); if (ret == -1) { return -1; diff --git a/time_display.c b/time_display.c index 5833dda..b281523 100644 --- a/time_display.c +++ b/time_display.c @@ -20,7 +20,7 @@ #include "gpio.h" #define DEST_PORT 4321 -#define DEST_ADDR "fd00::2" +#define DEST_ADDR "fd00::1" #define PORT 1234 @@ -169,7 +169,6 @@ int max(int a, int b) { } 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) @@ -197,7 +196,7 @@ int handle_buf(int sockfd, 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; - packet_send_ack(sockfd, dest_addr, evt->sequence); + packet_send_ack(sockfd, &link_state->dest_addr, evt->sequence); } break; case EVENT_TYPE__TIME_START: @@ -212,7 +211,7 @@ int handle_buf(int sockfd, timer_state->counter.offset = timespec_div(&link_state->remote_average_rtt, 2); printf("counter_offset %ld.%09ld\n", timer_state->counter.offset.tv_sec, timer_state->counter.offset.tv_nsec); timer_state->status = TIMER_RUNNING; - packet_send_ack(sockfd, dest_addr, evt->sequence); + packet_send_ack(sockfd, &link_state->dest_addr, evt->sequence); } break; case EVENT_TYPE__TIME_RESUME: @@ -229,7 +228,7 @@ int handle_buf(int sockfd, timer_state->counter.offset = timespec_add(&timer_state->counter.offset, &resume_pkt->offset); printf("counter offset %ld.%09ld\n", timer_state->counter.offset.tv_sec, timer_state->counter.offset.tv_nsec); timer_state->status = TIMER_RUNNING; - packet_send_ack(sockfd, dest_addr, evt->sequence); + packet_send_ack(sockfd, &link_state->dest_addr, evt->sequence); } break; case EVENT_TYPE__PING: [[fallthrough]]; @@ -237,7 +236,6 @@ int handle_buf(int sockfd, case EVENT_TYPE__AVERAGE_RTT: { int ret = handle_ping_pong(sockfd, - dest_addr, buf, length, link_state); if (ret == -1) { @@ -254,7 +252,6 @@ int handle_buf(int sockfd, } int handle_sockfd(int sockfd, - struct sockaddr_in6 * dest_addr, struct timer_state * timer_state, struct link_state * link_state) { @@ -282,7 +279,7 @@ int handle_sockfd(int sockfd, //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, timer_state, link_state); + int ret = handle_buf(sockfd, buf, recv_len, timer_state, link_state); if (ret == -1) { return -1; } @@ -374,21 +371,20 @@ int main() /* */ - struct sockaddr_in6 dest_addr; - dest_addr.sin6_family = AF_INET6; - dest_addr.sin6_port = htons(DEST_PORT); - ret = inet_pton(AF_INET6, DEST_ADDR, &dest_addr.sin6_addr); - assert(ret == 1); - struct timer_state timer_state = {0}; struct link_state link_state = {0}; struct gpio_state gpio_state = {0}; + link_state.dest_addr.sin6_family = AF_INET6; + link_state.dest_addr.sin6_port = htons(DEST_PORT); + ret = inet_pton(AF_INET6, DEST_ADDR, &link_state.dest_addr.sin6_addr); + assert(ret == 1); + gpio_open("/dev/gpiochip0", &gpio_state); // ignored gpio_open error while (1) { - handle_sockfd(sockfd, &dest_addr, &timer_state, &link_state); + handle_sockfd(sockfd, &timer_state, &link_state); char str_buf[20 * 2 + 1 + 1]; switch (timer_state.status) { @@ -449,13 +445,14 @@ int main() ticks = SDL_GetTicks(); if (ticks - last_ping_tick > PING_INTERVAL * 1000) { //printf("send ping\n"); - int ret = packet_send_ping(sockfd, &dest_addr); + int ret = packet_send_ping(sockfd, &link_state.dest_addr); if (ret == -1) { return -1; } last_ping_tick = ticks; - gpio_set_values_from_link_state(&gpio_state, - &link_state); + gpio_set_values_from_link_states(&gpio_state, + &link_state, + 1); } SDL_Event event;