timer/serial_forwarder.c

#include <arpa/inet.h>
#include <assert.h>
#include <errno.h>
#include <fcntl.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/epoll.h>
#include <sys/socket.h>
#include <sys/timerfd.h>
#include <netinet/in.h>
#include <unistd.h>
#include <string.h>

#include "bufsize.h"
#include "serial.h"
#include "timer.h"
#include "parse_serial.h"
#include "packet.h"
#include "link.h"
#include "timespec.h"
#include "ping_pong.h"
#include "gpio.h"

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 "test.fifo"

int handle_buf(int sockfd,
               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,
                                 buf, length,
                                 link_state);
      if (ret == -1) {
        return -1;
      }
    }
    break;
  case EVENT_TYPE__ACK:
    printf("recv ack %d\n", evt->sequence);
    link_state->recv_sequence = evt->sequence;
    break;
  default:
    printf("unhandled event %d\n", evt->type);
    break;
  }

  return 0;
}

int handle_sockfd(int sockfd,
                  struct link_state link_states[])
{
  struct sockaddr_in6 src_addr;
  socklen_t addrlen = (sizeof (struct sockaddr_in6));

  char buf[BUFSIZE];

  while (true) {
    ssize_t recv_len = recvfrom(sockfd,
				buf, (sizeof (buf)),
				0,
				(struct sockaddr *)&src_addr, &addrlen);
    if (recv_len == -1) {
      if (errno == EAGAIN || errno == EWOULDBLOCK) {
	//perror("sock eagain");
	return 0;
      } else {
	perror("recvfrom sock");
	return -1;
      }
    }

    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);

    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;
  }
}

int rearm_timer(int timerfd)
{
  struct itimerspec value;
  value.it_value.tv_sec = PING_INTERVAL;
  value.it_value.tv_nsec = 0;
  value.it_interval.tv_sec = 0;
  value.it_interval.tv_nsec = 0;

  int ret = timerfd_settime(timerfd, 0, &value, NULL);
  if (ret == -1) {
    perror("timerfd_settime");
    return -1;
  }

  return 0;
}

int send_fake_stop_event(int sockfd,
                         struct timer_state * timer_state, // single
                         struct link_state * link_state)
{
  printf("fake stop event");
  struct timer_state fake_state = *timer_state;

  fake_state.time.integer_value = 0;
  fake_state.time.integer_digits = 0;
  fake_state.time.fraction_value = 0;
  fake_state.time.fraction_digits = 0;

  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 timer_state * timer_state,
		 struct link_state * link_state,
                 struct gpio_state * gpio_state,
		 uint32_t type)
{
  int ret;
  switch (type) {
  case EVENT_TYPE__TIME_STOP:
    printf("event time stop\n");
    ret = packet_send_stopwatch_event(sockfd, &link_state->dest_addr, timer_state, link_state);
    if (ret == -1) {
      return -1;
    }
    break;
  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, timer_state, link_state);
    } else {
      ret = packet_send_start_event(sockfd, &link_state->dest_addr, link_state);
    }
    if (ret == -1) {
      return -1;
    }
    break;
  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, timer_state, link_state);
    } else {
      ret = packet_send_resume_event(sockfd, &link_state->dest_addr, timer_state, link_state);
    }
    if (ret == -1) {
      return -1;
    }
    break;
  default:
    break;
  }
  return 0;
}

void check_sequence(int sockfd,
		    struct timer_state * timer_state,
		    struct link_state link_states[],
		    struct gpio_state * gpio_state)
{
  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_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;
      }
    }
  }
}

int handle_timerfd(int timerfd,
                   int sockfd,
		   struct timer_state * timer_state,
                   struct link_state link_states[],
		   struct gpio_state * gpio_state)
{
  uint64_t expired_count = 0;
  while (true) {
    ssize_t len = read(timerfd, &expired_count, (sizeof (expired_count)));
    if (len == -1) {
      if (errno == EAGAIN || errno == EWOULDBLOCK) {
        //fprintf(stderr, "timerfd eagain\n");
	break;
      } else {
	perror("read timerfd");
	return -1;
      }
    }
    assert(len == (sizeof (expired_count)));
  }

  rearm_timer(timerfd);

  // 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
  }

  check_sequence(sockfd,
		 timer_state,
		 link_states,
		 gpio_state);

  gpio_set_values_from_link_states(gpio_state,
				   link_states,
				   dests_length);

  return 0;
}

int handle_serialfd(int serialfd,
		    int sockfd,
                    struct parser_state * parser_state,
                    struct timer_state * timer_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) {
      if (errno == EAGAIN || errno == EWOULDBLOCK) {
        fprintf(stderr, "serialfd eagain\n");
        break;
      } else {
	perror("read serialfd");
	return -1;
      }
    }

    /*
    for (int i = 0; i < len; i++) {
      fprintf(stderr, "%x ", buf[i]);
    }
    fprintf(stderr, "\n");
    */
    uint32_t type = handle_parse(buf, len,
				 parser_state,
				 timer_state);

    // 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 error;
}

int main(void)
{
  int ret;

  int sockfd = socket(AF_INET6, SOCK_DGRAM | SOCK_NONBLOCK, IPPROTO_UDP);
  if (sockfd == -1) {
    perror("socket");
    return -1;
  }

  struct sockaddr_in6 sockaddr = {0};
  sockaddr.sin6_family = AF_INET6;
  sockaddr.sin6_port = htons(PORT);
  sockaddr.sin6_addr = in6addr_any;

  ret = bind(sockfd, (struct sockaddr *)&sockaddr, (sizeof (struct sockaddr_in6)));
  if (ret == -1) {
    perror("sockfd bind");
    return -1;
  }

#define MAX_EVENTS 5
  struct epoll_event events[MAX_EVENTS];

  int epollfd = epoll_create1(0);
  if (epollfd == -1) {
    perror("epoll_create1");
    return -1;
  }

  {
    struct epoll_event ev;
    ev.events = EPOLLIN | EPOLLET;
    ev.data.fd = sockfd;
    ret = epoll_ctl(epollfd, EPOLL_CTL_ADD, sockfd, &ev);
    if (ret == -1) {
      perror("epoll_ctl: sock");
      return -1;
    }
  }

  int timerfd = timerfd_create(CLOCK_MONOTONIC, TFD_NONBLOCK);
  if (timerfd == -1) {
    perror("timerfd_create");
    return -1;
  }

  ret = rearm_timer(timerfd);
  if (ret == -1) {
    return -1;
  }

  {
    struct epoll_event ev;
    ev.events = EPOLLIN | EPOLLET;
    ev.data.fd = timerfd;
    ret = epoll_ctl(epollfd, EPOLL_CTL_ADD, timerfd, &ev);
    if (ret == -1) {
      perror("epoll_ctl: timerfd");
      return -1;
    }
  }

  int serialfd = open(SERIALPORT, O_RDWR | O_NOCTTY | O_NONBLOCK);
  if (serialfd == -1) {
    perror("open: serialport");
    return -1;
  }

  if (strstr(SERIALPORT, "tty") != NULL) {
    ret = set_terminal_attributes(serialfd);
    if (ret == -1) {
      return -1;
    }
  }

  {
    struct epoll_event ev;
    ev.events = EPOLLIN | EPOLLET;
    ev.data.fd = serialfd;
    ret = epoll_ctl(epollfd, EPOLL_CTL_ADD, serialfd, &ev);
    if (ret == -1) {
      perror("epoll_ctl: timerfd");
      return -1;
    }
  }

  struct parser_state parser_state = {0};
  struct timer_state timer_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

  while (1) {
    int nfds = epoll_wait(epollfd, events, MAX_EVENTS, -1);
    if (nfds == -1) {
      perror("epoll_wait");
      return -1;
    }

    for (int n = 0; n < nfds; ++n) {
      if (events[n].data.fd == sockfd) {
	ret = handle_sockfd(sockfd, link_states);
	if (ret == -1)
	  return -1;
      } else if (events[n].data.fd == timerfd) {
        ret = handle_timerfd(timerfd,
                             sockfd,
			     &timer_state,
                             link_states,
			     &gpio_state);
        if (ret == -1)
          return -1;
      } else if (events[n].data.fd == serialfd) {
        fprintf(stderr, "handle_serialfd\n");
        ret = handle_serialfd(serialfd,
			      sockfd,
                              &parser_state,
                              &timer_state,
			      link_states,
                              &gpio_state);
        if (ret == -1) {
          return -1;
	}
      } else {
        assert(0);
      }
    }

    //sleep(2);
  }

  close(sockfd);
  close(serialfd);
  close(timerfd);
  close(epollfd);

  return 0;
}