334 lines
7.4 KiB
C
334 lines
7.4 KiB
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"
|
|
|
|
#define PORT 4321
|
|
//#define SERIALPORT "/dev/ttyS0"
|
|
//#define SERIALPORT "/dev/ttyUSB0"
|
|
#define SERIALPORT "foo.fifo"
|
|
|
|
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;
|
|
}
|
|
|
|
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));
|
|
|
|
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 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 = 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 handle_timerfd(int timerfd,
|
|
int sockfd,
|
|
struct sockaddr_in6 * dest_addr,
|
|
struct link_state * link_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);
|
|
|
|
int ret = packet_send_ping(sockfd, dest_addr);
|
|
if (ret == -1) {
|
|
return -1;
|
|
}
|
|
|
|
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)
|
|
{
|
|
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 = packet_send_stopwatch_event(sockfd, dest_addr, timer_state, link_state);
|
|
if (ret == -1) {
|
|
return -1;
|
|
}
|
|
break;
|
|
case EVENT_TYPE__TIME_START:
|
|
ret = packet_send_start_event(sockfd, dest_addr, 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;
|
|
}
|
|
|
|
{
|
|
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;
|
|
}
|
|
}
|
|
|
|
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);
|
|
assert(ret == 1);
|
|
|
|
struct parser_state parser_state = {0};
|
|
struct timer_state timer_state = {0};
|
|
struct link_state link_state = {0};
|
|
|
|
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, &dest_addr, &link_state);
|
|
if (ret == -1)
|
|
return -1;
|
|
} else if (events[n].data.fd == 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);
|
|
if (ret == -1) {
|
|
return -1;
|
|
}
|
|
} else {
|
|
assert(0);
|
|
}
|
|
}
|
|
|
|
//sleep(2);
|
|
}
|
|
|
|
close(sockfd);
|
|
close(serialfd);
|
|
close(timerfd);
|
|
close(epollfd);
|
|
|
|
return 0;
|
|
}
|