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"

#define PORT 4321
//#define SERIALPORT "/dev/ttyS0"
//#define SERIALPORT "/dev/ttyUSB0"
#define SERIALPORT "foo.fifo"

static struct sockaddr_in6 dest_addr;

int handle_sockfd(int sockfd)
{
  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);
  }
}

int rearm_timer(int timerfd)
{
  struct itimerspec value;
  value.it_value.tv_sec = 1;
  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 handle_timerfd(int timerfd)
{
  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)));
    printf("len %ld\n", len);
  }

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

int handle_serialfd(int serialfd,
		    int sockfd,
                    struct parser_state * parser_state,
                    struct timer_state * timer_state,
		    struct link_state * link_state)
{
  uint8_t buf[BUFSIZE];

  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);
    int ret;
    switch (type) {
    case EVENT_TYPE__TIME_STOP:
      ret = send_stopwatch_event(sockfd, timer_state, link_state);
      if (ret == -1) {
	return -1;
      }
      break;
    case EVENT_TYPE__TIME_START:
      ret = send_start_event(sockfd, timer_state, link_state);
      if (ret == -1) {
	return -1;
      }
      break;
    default:
      break;
    }
  }

  return 0;
}

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

  if (0) {
    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;
  }
  /*
  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;
    }
  }

  dest_addr.sin6_family = AF_INET6;
  dest_addr.sin6_port = htons(1234);
  ret = inet_pton(AF_INET6, "::1", &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};

  while (1) {
    printf("Wait for datagram\n");

    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);
	if (ret == -1)
	  return -1;
      } else if (events[n].data.fd == timerfd) {
        ret = handle_timerfd(timerfd);
        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_state);
        if (ret == -1) {
          return -1;
	}
      } else {
        assert(0);
      }
    }

    //sleep(2);
  }

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

  return 0;
}