diff --git a/Makefile b/Makefile index 663e5da..0d2ffe5 100644 --- a/Makefile +++ b/Makefile @@ -19,12 +19,16 @@ SERIAL_FORWARDER_OBJS = \ serial.o \ parse_serial.o \ packet.o \ - ping_pong.o + ping_pong.o \ + gpio.o \ + timespec.o TIME_DISPLAY_OBJS = \ time_display.o \ packet.o \ - ping_pong.o + ping_pong.o \ + gpio.o \ + timespec.o all: serial_forwarder time_display diff --git a/gpio.c b/gpio.c new file mode 100644 index 0000000..2d027fd --- /dev/null +++ b/gpio.c @@ -0,0 +1,86 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "gpio.h" +#include "timespec.h" +#include "ping_pong.h" + +#define LAST_PONG_TIMEOUT (PING_INTERVAL * 3) + +/* + 17 red (11) + 27 green (13) + 22 blue (15) +*/ + +#define RED 0b001 +#define GREEN 0b010 +#define BLUE 0b100 + +int gpio_open(const char * path, + struct gpio_state * gpio_state) +{ + gpio_state->chip_fd = open(path, O_RDWR); + if (gpio_state->chip_fd == -1) { + perror("open gpio"); + return -1; + } + + struct gpio_v2_line_request request = { + .offsets = { 17, 27, 22 }, + .consumer = "timer", + .config = { + .flags = GPIO_V2_LINE_FLAG_OUTPUT, + }, + .num_lines = 3, + }; + int ret = ioctl(gpio_state->chip_fd, GPIO_V2_GET_LINE_IOCTL, &request); + assert(ret != -1); + gpio_state->req_fd = request.fd; + + return 0; +} + +void gpio_set_values(struct gpio_state * gpio_state, uint64_t bits) +{ + struct gpio_v2_line_values values = { + .bits = bits, + .mask = 0b111, + }; + + int ret = ioctl(gpio_state->req_fd, GPIO_V2_LINE_SET_VALUES_IOCTL, &values); + assert(ret != -1); +} + +void gpio_set_values_from_link_state(struct gpio_state * gpio_state, + struct link_state * link_state) +{ + 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("pong timeout %ld\n", delta.tv_sec); + if (gpio_state->chip_fd != -1) + gpio_set_values(gpio_state, GREEN); + } else { + printf("pong ok %ld\n", delta.tv_sec); + if (gpio_state->chip_fd != -1) + gpio_set_values(gpio_state, RED); + } +} + +/* +int main() +{ + struct gpio_state gpio_state; + gpio_open("/dev/gpiochip0", &gpio_state); + set_gpio_values(&gpio_state, 0b000); +} +*/ diff --git a/gpio.h b/gpio.h new file mode 100644 index 0000000..dec1ba4 --- /dev/null +++ b/gpio.h @@ -0,0 +1,16 @@ +#pragma once + +#include "link.h" + +struct gpio_state { + int chip_fd; + int req_fd; +}; + +int gpio_open(const char * path, + struct gpio_state * gpio_state); + +void gpio_set_values(struct gpio_state * gpio_state, uint64_t bits); + +void gpio_set_values_from_link_state(struct gpio_state * gpio_state, + struct link_state * link_state); diff --git a/packet.c b/packet.c index a4cbb76..9242107 100644 --- a/packet.c +++ b/packet.c @@ -79,7 +79,6 @@ int packet_send_average_rtt(int sockfd, 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; @@ -99,6 +98,27 @@ int packet_send_start_event(int sockfd, return 0; } +int packet_send_resume_event(int sockfd, + struct sockaddr_in6 * dest_addr, + struct link_state * link_state) +{ + struct packet_start pkt; + pkt.event.type = EVENT_TYPE__TIME_RESUME; + 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_resume_event sendto"); + return -1; + } + + return 0; +} + int packet_send_stopwatch_event(int sockfd, struct sockaddr_in6 * dest_addr, struct timer_state * timer_state, diff --git a/packet.h b/packet.h index 915939d..c25aff1 100644 --- a/packet.h +++ b/packet.h @@ -10,6 +10,7 @@ #define EVENT_TYPE__INVALID 0x00000000 #define EVENT_TYPE__TIME_START 0x12858517 +#define EVENT_TYPE__TIME_RESUME 0x56100f0f #define EVENT_TYPE__TIME_STOP 0xdf75d3f8 #define EVENT_TYPE__AVERAGE_RTT 0xc7065931 #define EVENT_TYPE__PING 0x0ba0d4c2 @@ -66,8 +67,10 @@ int packet_send_average_rtt(int sockfd, 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_resume_event(int sockfd, + struct sockaddr_in6 * dest_addr, + struct link_state * link_state); int packet_send_stopwatch_event(int sockfd, struct sockaddr_in6 * dest_addr, struct timer_state * timer_state, diff --git a/parse_serial.c b/parse_serial.c index e83d864..5cc1b35 100644 --- a/parse_serial.c +++ b/parse_serial.c @@ -77,7 +77,9 @@ int min(int a, int b) { bool is_timer_resume(uint8_t const * const buf, int length) { - return false; + const char * s = "RESUME"; + size_t len = strlen(s); + return (len == length) && (strncmp((const char *)buf, s, length) == 0); } uint32_t handle_line(uint8_t const * const buf, int length, @@ -90,9 +92,10 @@ uint32_t handle_line(uint8_t const * const buf, int length, timer_state->counter.offset.tv_sec = 0; timer_state->counter.offset.tv_nsec = 0; return EVENT_TYPE__TIME_START; - } /*else if (is_timer_resume(buf, length)) { - return true; // fixme - } */ else { + } else if (is_timer_resume(buf, length)) { + timer_state->status = TIMER_RUNNING; + return EVENT_TYPE__TIME_START; + } else { int ret = parse_time(buf, length, &timer_state->time); if (ret == 0) { timer_state->status = TIMER_STOPPED; diff --git a/ping_pong.c b/ping_pong.c index 6f53680..473aab5 100644 --- a/ping_pong.c +++ b/ping_pong.c @@ -43,9 +43,9 @@ int handle_ping_pong(int sockfd, 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); + *rtt = timespec_sub(&now, &ping_pkt->time); printf("pong rtt: %ld.%09ld\n", rtt->tv_sec, rtt->tv_nsec); - struct timespec average_rtt = average_timespec(link_state->ping_pong_rtt, RTT_AVERAGE_SAMPLES); + struct timespec average_rtt = timespec_average(link_state->ping_pong_rtt, RTT_AVERAGE_SAMPLES); ret = packet_send_average_rtt(sockfd, dest_addr, &average_rtt); if (ret == -1) { diff --git a/serial_forwarder.c b/serial_forwarder.c index 4bdddbe..0c9cf0c 100644 --- a/serial_forwarder.c +++ b/serial_forwarder.c @@ -177,7 +177,13 @@ int handle_serialfd(int serialfd, } break; case EVENT_TYPE__TIME_START: - ret = packet_send_start_event(sockfd, dest_addr, timer_state, link_state); + ret = packet_send_start_event(sockfd, dest_addr, link_state); + if (ret == -1) { + return -1; + } + break; + case EVENT_TYPE__TIME_RESUME: + ret = packet_send_resume_event(sockfd, dest_addr, link_state); if (ret == -1) { return -1; } diff --git a/time_display.c b/time_display.c index 55bd69f..2b9a289 100644 --- a/time_display.c +++ b/time_display.c @@ -17,6 +17,7 @@ #include "packet.h" #include "timespec.h" #include "ping_pong.h" +#include "gpio.h" struct glyph { int32_t width; @@ -192,6 +193,14 @@ int handle_buf(int sockfd, 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__TIME_RESUME: + if (length != (sizeof (struct event))) { + printf("handle_buf: time resume: invalid length\n"); + return 0; + } + timer_state->status = TIMER_RUNNING; break; case EVENT_TYPE__PING: [[fallthrough]]; @@ -338,6 +347,10 @@ int main() struct timer_state timer_state = {0}; struct link_state link_state = {0}; + struct gpio_state gpio_state = {0}; + + gpio_open("/dev/gpiochip0", &gpio_state); + // ignored gpio_open error while (1) { handle_sockfd(sockfd, &dest_addr, &timer_state, &link_state); @@ -363,7 +376,7 @@ int main() int ret = clock_gettime(CLOCK_MONOTONIC_RAW, &now); assert(ret != -1); - struct timespec duration = diff_timespec(&now, &timer_state.counter.start); + struct timespec duration = timespec_sub(&now, &timer_state.counter.start); snprintf(str_buf, (sizeof (str_buf)) - 1, "%ld.%ld", @@ -404,6 +417,8 @@ int main() return -1; } last_ping_tick = ticks; + gpio_set_values_from_link_state(&gpio_state, + &link_state); } SDL_Event event; diff --git a/timespec.c b/timespec.c new file mode 100644 index 0000000..d73429a --- /dev/null +++ b/timespec.c @@ -0,0 +1,48 @@ +#include +#include +#include +#include + +struct timespec timespec_sub(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; +} + +struct timespec timespec_add(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; +} + +struct timespec timespec_average(const struct timespec * ts, + int length) +{ + struct timespec c = {0}; + for (int i = 0; i < length; i++) { + c = timespec_add(&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; +} diff --git a/timespec.h b/timespec.h index 85c58cc..2494920 100644 --- a/timespec.h +++ b/timespec.h @@ -1,50 +1,12 @@ #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; -} +struct timespec timespec_sub(const struct timespec * a, + const struct timespec * b); -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 - }; +struct timespec timespec_add(const struct timespec * a, + const struct timespec * b); - 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; -} +struct timespec timespec_average(const struct timespec * ts, + int length);