/*
|
* RTnet test server loop
|
*
|
* Copyright (C) 2015 Gilles Chanteperdrix <gch@xenomai.org>
|
*
|
* SPDX-License-Identifier: MIT
|
*/
|
|
#include <stdio.h>
|
#include <stdlib.h>
|
#include <stdint.h>
|
#include <limits.h>
|
#include <errno.h>
|
#include <string.h>
|
|
#include <unistd.h>
|
#include <signal.h>
|
#include <pthread.h>
|
#include <sched.h>
|
#include <fcntl.h>
|
#include <sys/socket.h>
|
#include <netinet/in.h>
|
#include <netpacket/packet.h>
|
#include <net/ethernet.h>
|
|
#include <sys/cobalt.h>
|
#include <smokey/smokey.h>
|
#include "smokey_net.h"
|
#include "smokey_net_server.h"
|
|
struct smokey_server {
|
sem_t sync;
|
int fds[0];
|
};
|
|
struct proto {
|
int config_flag;
|
int (*create_socket)(void);
|
void (*serve)(int fd);
|
};
|
|
static int udp_create_socket(void);
|
static void udp_serve(int fd);
|
static int packet_dgram_socket(void);
|
static void packet_dgram_serve(int fd);
|
|
static const struct proto protos[] = {
|
{
|
.config_flag = _CC_COBALT_NET_UDP,
|
.create_socket = &udp_create_socket,
|
.serve = &udp_serve,
|
},
|
{
|
.config_flag = _CC_COBALT_NET_AF_PACKET,
|
.create_socket = &packet_dgram_socket,
|
.serve = &packet_dgram_serve,
|
},
|
};
|
|
static int udp_create_socket(void)
|
{
|
struct sockaddr_in name;
|
int fd;
|
|
fd = check_unix(__RT(socket(PF_INET, SOCK_DGRAM, 0)));
|
|
name.sin_family = AF_INET;
|
name.sin_port = htons(7); /* UDP echo service */
|
name.sin_addr.s_addr = htonl(INADDR_ANY);
|
|
check_unix(__RT(bind(fd, (struct sockaddr *)&name, sizeof(name))));
|
|
return fd;
|
}
|
|
static void udp_serve(int fd)
|
{
|
struct smokey_net_payload pkt;
|
struct sockaddr_in peer;
|
socklen_t peer_len;
|
int err;
|
|
peer_len = sizeof(peer);
|
err = check_unix(
|
__RT(recvfrom(fd, &pkt, sizeof(pkt), 0,
|
(struct sockaddr *)&peer, &peer_len)));
|
|
check_unix(
|
__RT(sendto(fd, &pkt, err, 0,
|
(struct sockaddr *)&peer, peer_len)));
|
}
|
|
static int packet_dgram_socket(void)
|
{
|
return check_unix(
|
__RT(socket(PF_PACKET, SOCK_DGRAM, htons(ETH_P_802_EX1))));
|
}
|
|
static void packet_dgram_serve(int fd)
|
{
|
struct smokey_net_payload pkt;
|
struct sockaddr_ll peer;
|
socklen_t peer_len;
|
int err;
|
|
peer_len = sizeof(peer);
|
err = check_unix(
|
__RT(recvfrom(fd, &pkt, sizeof(pkt), 0,
|
(struct sockaddr *)&peer, &peer_len)));
|
|
peer.sll_protocol = htons(ETH_P_802_EX1 + 1);
|
check_unix(
|
__RT(sendto(fd, &pkt, err, 0,
|
(struct sockaddr *)&peer, peer_len)));
|
}
|
|
static void server_loop_cleanup(void *cookie)
|
{
|
int *fds = cookie;
|
int i;
|
|
for (i = 0; i < sizeof(protos)/sizeof(protos[0]); i++)
|
__RT(close(fds[i]));
|
free(fds);
|
}
|
|
void smokey_net_server_loop(int net_config)
|
{
|
struct sched_param prio;
|
const struct proto *p;
|
int i, maxfd, *fds;
|
fd_set rfds;
|
|
fds = malloc(sizeof(*fds) * sizeof(protos)/sizeof(protos[0]));
|
if (fds == NULL)
|
pthread_exit((void *)(long)-ENOMEM);
|
|
pthread_cleanup_push(server_loop_cleanup, fds);
|
|
FD_ZERO(&rfds);
|
maxfd = 0;
|
for (i = 0; i < sizeof(protos)/sizeof(protos[0]); i++) {
|
p = &protos[i];
|
|
if ((net_config & p->config_flag) == 0) {
|
fds[i] = -1;
|
continue;
|
}
|
|
fds[i] = p->create_socket();
|
FD_SET(fds[i], &rfds);
|
if (fds[i] > maxfd)
|
maxfd = fds[i];
|
}
|
|
prio.sched_priority = 20;
|
check_pthread(
|
__RT(pthread_setschedparam(pthread_self(), SCHED_FIFO, &prio)));
|
|
for (;;) {
|
fd_set tfds;
|
|
tfds = rfds;
|
|
check_unix(__RT(select(maxfd + 1, &tfds, NULL, NULL, NULL)));
|
|
for (i = 0; i < sizeof(protos)/sizeof(protos[0]); i++) {
|
p = &protos[i];
|
|
if (fds[i] < 0 || !FD_ISSET(fds[i], &tfds))
|
continue;
|
|
p->serve(fds[i]);
|
}
|
}
|
|
pthread_cleanup_pop(1);
|
}
|
