int send_can_ping(int can_socket, int verbose) {
if (frame_to_can(can_socket, M_CAN_PING) < 0) {
fprintf(stderr, "can't send CAN Ping\n");
syslog(LOG_ERR, "%s: can't send CAN Ping\n", __func__);
return -1;
} else {
if (verbose) {
/* printf(" CAN Ping sent\n"); */
print_can_frame(CAN_FORMAT_STRG, M_CAN_PING, verbose);
}
}
return 0;
}
int send_magic_start_60113_frame(int can_socket, int verbose) {
int ret;
ret = frame_to_can(can_socket, &M_GLEISBOX_MAGIC_START_SEQUENCE[0]);
if (ret < 0 ) {
perror("error CAN magic 60113 start write\n");
return -1;
} else {
if (verbose)
printf(" CAN magic 60113 start write\n");
print_can_frame(CAN_FORMAT_STRG, M_GLEISBOX_MAGIC_START_SEQUENCE);
}
return 0;
}
int send_start_60113_frames(int can_socket, int verbose) {
if (frame_to_can(can_socket, M_GLEISBOX_MAGIC_START_SEQUENCE) < 0) {
fprintf(stderr, "can't send CAN magic 60113 start sequence\n");
syslog(LOG_ERR, "%s: can't send CAN magic 60113 start sequence\n", __func__);
return -1;
} else {
if (verbose) {
printf(" CAN magic 60113 start written\n");
print_can_frame(CAN_FORMAT_STRG, M_GLEISBOX_MAGIC_START_SEQUENCE, verbose);
}
}
if (frame_to_can(can_socket, M_GLEISBOX_ALL_PROTO_ENABLE) < 0) {
fprintf(stderr, "can't enable all loco protos\n");
syslog(LOG_ERR, "%s: can't enable all loco protos\n", __func__);
return -1;
} else {
if (verbose) {
printf(" CAN enabled all loco protos\n");
print_can_frame(CAN_FORMAT_STRG, M_GLEISBOX_ALL_PROTO_ENABLE, verbose);
}
}
return 0;
}
int main(int argc, char **argv) {
pid_t pid;
int n, i, max_fds, opt, max_tcp_i, nready, conn_fd, timeout, tcp_client[MAX_TCP_CONN];
struct can_frame frame;
char timestamp[16];
/* UDP incoming socket , CAN socket, UDP broadcast socket, TCP socket */
int sa, sc, sb, st, st2, tcp_socket;
struct sockaddr_in saddr, baddr, tcp_addr, tcp_addr2;
/* vars for determing broadcast address */
struct ifaddrs *ifap, *ifa;
struct sockaddr_in *bsa;
struct sockaddr_can caddr;
struct ifreq ifr;
socklen_t caddrlen = sizeof(caddr);
socklen_t tcp_client_length = sizeof(tcp_addr);
fd_set all_fds, read_fds;
int s, ret;
struct timeval tv;
char *udp_dst_address;
char *bcast_interface;
struct cs2_config_data_t cs2_config_data;
int local_udp_port = 15731;
int local_tcp_port = 15731;
int local2_tcp_port = 15732;
int destination_port = 15730;
int background = 1;
/* const int off = 0; */
const int on = 1;
uint32_t canid;
char buffer[64];
/* clear timestamp for last CAN frame sent */
memset(&last_sent, 0, sizeof(last_sent));
memset(&cs2_config_data, 0, sizeof(cs2_config_data));
page_name = calloc(MAX_TRACK_PAGE, sizeof(char *));
if (!page_name) {
fprintf(stderr, "can't alloc memory for page_name: %s\n", strerror(errno));
exit(EXIT_FAILURE);
};
cs2_page_name = calloc(MAX_TRACK_PAGE, sizeof(char *));
if (!cs2_page_name) {
fprintf(stderr, "can't alloc memory for cs2_page_name: %s\n", strerror(errno));
exit(EXIT_FAILURE);
};
ms1_workaround = 0;
cs2fake_ping = 0;
cs2_config_data.verbose = 0;
cs2_config_data.state = CS2_STATE_INACTIVE;
cs2_config_data.page_name = cs2_page_name;
cs2_config_data.cs2_config_copy = 0;
cs2_config_data.cs2_tcp_socket = 0;
cs2_config_data.track_index = MAX_TRACK_PAGE;
memset(ifr.ifr_name, 0, sizeof(ifr.ifr_name));
strcpy(ifr.ifr_name, "can0");
memset(config_dir, 0, sizeof(config_dir));
udp_dst_address = (char *)calloc(MAXIPLEN, 1);
if (!udp_dst_address) {
fprintf(stderr, "can't alloc memory for udp_dst_address: %s\n", strerror(errno));
exit(EXIT_FAILURE);
};
bcast_interface = (char *)calloc(MAXIPLEN, 1);
if (!bcast_interface) {
fprintf(stderr, "can't alloc memory for bcast_interface: %s\n", strerror(errno));
exit(EXIT_FAILURE);
};
timeout = MAX_UDP_BCAST_RETRY;
strcpy(udp_dst_address, "255.255.255.255");
strcpy(bcast_interface, "br-lan");
config_file[0] = '\0';
while ((opt = getopt(argc, argv, "c:u:s:t:d:b:i:kT:gmvhf?")) != -1) {
switch (opt) {
case 'c':
if (strnlen(optarg, MAXLINE) < MAXLINE) {
strncpy(config_dir, optarg, sizeof(config_dir) - 1);
} else {
fprintf(stderr, "config file dir to long\n");
exit(EXIT_FAILURE);
}
break;
case 'u':
local_udp_port = strtoul(optarg, (char **)NULL, 10);
break;
case 't':
local_tcp_port = strtoul(optarg, (char **)NULL, 10);
break;
case 's':
local2_tcp_port = strtoul(optarg, (char **)NULL, 10);
break;
case 'd':
destination_port = strtoul(optarg, (char **)NULL, 10);
break;
case 'b':
if (strnlen(optarg, MAXIPLEN) <= MAXIPLEN - 1) {
/* IP address begins with a number */
if ((optarg[0] >= '0') && (optarg[0] <= '9')) {
memset(udp_dst_address, 0, MAXIPLEN);
strncpy(udp_dst_address, optarg, MAXIPLEN - 1);
} else {
memset(udp_dst_address, 0, MAXIPLEN);
memset(bcast_interface, 0, MAXIPLEN);
strncpy(bcast_interface, optarg, MAXIPLEN - 1);
}
} else {
fprintf(stderr, "UDP broadcast address or interface error: %s\n", optarg);
exit(EXIT_FAILURE);
}
break;
case 'i':
strncpy(ifr.ifr_name, optarg, sizeof(ifr.ifr_name) - 1);
break;
case 'k':
cs2_config_data.cs2_config_copy = 1;
break;
case 'T':
timeout = strtoul(optarg, (char **)NULL, 10);
break;
case 'g':
cs2fake_ping = 1;
break;
case 'm':
ms1_workaround = 1;
break;
case 'v':
cs2_config_data.verbose = 1;
break;
case 'f':
background = 0;
break;
case 'h':
case '?':
print_usage(basename(argv[0]));
exit(EXIT_SUCCESS);
default:
fprintf(stderr, "Unknown option %c\n", opt);
print_usage(basename(argv[0]));
exit(EXIT_FAILURE);
}
}
/* read track file */
if (config_dir[0] == 0) {
strcat(config_file, ".");
}
strcat(config_file, config_dir);
if (config_dir[strlen(config_dir) - 1] != '/') {
strcat(config_file, "/");
}
strncpy(config_dir, config_file, sizeof(config_dir) - 1);
strcat(config_file, "gleisbild.cs2");
cs2_config_data.dir = config_dir;
page_name = read_track_file(config_file, page_name);
/* we are trying to setup a UDP socket */
for (i = 0; i < timeout; i++) {
/* trying to get the broadcast address */
getifaddrs(&ifap);
for (ifa = ifap; ifa; ifa = ifa->ifa_next) {
if (ifa->ifa_addr) {
if (ifa->ifa_addr->sa_family == AF_INET) {
bsa = (struct sockaddr_in *)ifa->ifa_broadaddr;
if (strncmp(ifa->ifa_name, bcast_interface, strlen(bcast_interface)) == 0)
udp_dst_address = inet_ntoa(bsa->sin_addr);
}
}
}
/* try to prepare UDP sending socket struct */
memset(&baddr, 0, sizeof(baddr));
baddr.sin_family = AF_INET;
baddr.sin_port = htons(destination_port);
s = inet_pton(AF_INET, udp_dst_address, &baddr.sin_addr);
if (s > 0)
break;
sleep(1);
}
/* check if we got a real UDP socket after MAX_UDP_BCAST_TRY seconds */
if (s <= 0) {
if (s == 0) {
fprintf(stderr, "UDP IP address invalid\n");
} else {
fprintf(stderr, "invalid address family\n");
}
exit(EXIT_FAILURE);
}
if (cs2_config_data.verbose & !background)
printf("using broadcast address %s\n", udp_dst_address);
/* prepare UDP sending socket */
sb = socket(AF_INET, SOCK_DGRAM, 0);
if (sb < 0) {
fprintf(stderr, "error creating UDP sending socket: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
if (setsockopt(sb, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on)) < 0) {
fprintf(stderr, "error setup UDP broadcast option: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
/* prepare reading UDP socket */
memset(&saddr, 0, sizeof(saddr));
saddr.sin_family = AF_INET;
saddr.sin_addr.s_addr = htonl(INADDR_ANY);
saddr.sin_port = htons(local_udp_port);
sa = socket(PF_INET, SOCK_DGRAM, 0);
if (sa < 0) {
fprintf(stderr, "creating UDP reading socket error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
if (bind(sa, (struct sockaddr *)&saddr, sizeof(saddr)) < 0) {
fprintf(stderr, "binding UDP reading socket error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
/* prepare TCP socket */
st = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
if (st < 0) {
fprintf(stderr, "creating TCP socket error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
/* disable Nagle */
/*if (setsockopt(st, IPPROTO_TCP, TCP_NODELAY, &on, sizeof(on)) < 0) {
fprintf(stderr, "error disabling Nagle - TCP_NODELAY on: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
*/
/* disable TCP_CORK */
/*
if (setsockopt(st, IPPROTO_TCP, TCP_CORK, &off, sizeof(off)) < 0) {
fprintf(stderr, "error disabling Nagle - TCP_CORK off: %s\n", strerror(errno));
exit(EXIT_FAILURE);
} */
tcp_addr.sin_family = AF_INET;
tcp_addr.sin_addr.s_addr = htonl(INADDR_ANY);
tcp_addr.sin_port = htons(local_tcp_port);
if (bind(st, (struct sockaddr *)&tcp_addr, sizeof(tcp_addr)) < 0) {
fprintf(stderr, "binding TCP socket error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
if (listen(st, MAXPENDING) < 0) {
fprintf(stderr, "starting TCP listener error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
/* prepare TCP clients array */
max_tcp_i = -1; /* index into tcp_client[] array */
for (i = 0; i < MAX_TCP_CONN; i++)
tcp_client[i] = -1; /* -1 indicates available entry */
/* prepare second TCP socket */
st2 = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
if (st2 < 0) {
fprintf(stderr, "creating second TCP socket error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
tcp_addr2.sin_family = AF_INET;
tcp_addr2.sin_addr.s_addr = htonl(INADDR_ANY);
tcp_addr2.sin_port = htons(local2_tcp_port);
if (bind(st2, (struct sockaddr *)&tcp_addr2, sizeof(tcp_addr2)) < 0) {
fprintf(stderr, "binding second TCP socket error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
if (listen(st2, MAXPENDING) < 0) {
fprintf(stderr, "starting TCP listener error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
#if 0
/* prepare TCP clients array */
max_tcp_i = -1; /* index into tcp_client[] array */
for (i = 0; i < MAX_TCP_CONN; i++)
tcp_client[i] = -1; /* -1 indicates available entry */
#endif
/* prepare CAN socket */
memset(&caddr, 0, sizeof(caddr));
sc = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (sc < 0) {
fprintf(stderr, "creating CAN socket error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
caddr.can_family = AF_CAN;
if (ioctl(sc, SIOCGIFINDEX, &ifr) < 0) {
fprintf(stderr, "setup CAN error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
caddr.can_ifindex = ifr.ifr_ifindex;
if (bind(sc, (struct sockaddr *)&caddr, caddrlen) < 0) {
fprintf(stderr, "binding CAN socket error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
/* start Maerklin 60113 box */
if (send_start_60113_frames(sc, cs2_config_data.verbose))
exit(EXIT_FAILURE);
/* daemonize the process if requested */
if (background) {
/* fork off the parent process */
pid = fork();
if (pid < 0)
exit(EXIT_FAILURE);
/* if we got a good PID, then we can exit the parent process */
if (pid > 0) {
printf("Going into background ...\n");
exit(EXIT_SUCCESS);
}
}
setlogmask(LOG_UPTO(LOG_NOTICE));
openlog("can2lan", LOG_CONS | LOG_NDELAY, LOG_DAEMON);
/* set select timeout -> send periodic CAN Ping */
memset(&tv, 0, sizeof(tv));
tv.tv_sec = 1;
tv.tv_usec = 0;
FD_ZERO(&all_fds);
FD_SET(sc, &all_fds);
FD_SET(sa, &all_fds);
FD_SET(st, &all_fds);
FD_SET(st2, &all_fds);
max_fds = MAX(MAX(MAX(sc, sa), st), st2);
while (1) {
read_fds = all_fds;
nready = select(max_fds + 1, &read_fds, NULL, NULL, &tv);
if (nready == 0) {
/* send_can_ping(sc); */
tv.tv_sec = 1;
tv.tv_usec = 0;
continue;
} else if (nready < 0)
fprintf(stderr, "select error: %s\n", strerror(errno));
tv.tv_sec = 1;
tv.tv_usec = 0;
/* received a CAN frame */
if (FD_ISSET(sc, &read_fds)) {
/* reading via SockatCAN */
if (read(sc, &frame, sizeof(struct can_frame)) < 0) {
fprintf(stderr, "reading CAN frame error: %s\n", strerror(errno));
syslog(LOG_ERR, "%s: reading CAN frame error: %s\n", __func__, strerror(errno));
}
/* if CAN Frame is EFF do it */
if (frame.can_id & CAN_EFF_FLAG) { /* only EFF frames are valid */
/* send UDP frame */
frame_to_net(sb, (struct sockaddr *)&baddr, (struct can_frame *)&frame);
print_can_frame(UDP_FORMAT_STRG, netframe, cs2_config_data.verbose & !background);
/* send CAN frame to all connected TCP clients */
/* TODO: need all clients the packets ? */
for (i = 0; i <= max_tcp_i; i++) { /* check all clients for data */
tcp_socket = tcp_client[i];
if (tcp_socket < 0)
continue;
frame_to_net(tcp_socket, (struct sockaddr *)&tcp_addr, (struct can_frame *)&frame);
print_can_frame(CAN_TCP_FORMAT_STRG, netframe, cs2_config_data.verbose & !background);
}
}
}
/* received a UDP packet */
if (FD_ISSET(sa, &read_fds)) {
if (read(sa, netframe, MAXDG) == CAN_ENCAP_SIZE) {
/* check for S88 events on send them to TCP connected clients */
memcpy(&canid, netframe, 4);
canid = ntohl(canid);
if ((canid & 0x00230000) == 0x00230000) {
printf("UDP : canid 0x%08x\n", canid);
for (i = 0; i <= max_tcp_i; i++) {
tcp_socket = tcp_client[i];
if (tcp_socket < 0)
continue;
net_to_net(tcp_socket, NULL, netframe, CAN_ENCAP_SIZE);
print_can_frame(UDP_TCP_FORMAT_STRG, netframe, cs2_config_data.verbose & !background);
net_to_net(sb, (struct sockaddr *)&baddr, netframe, CAN_ENCAP_SIZE);
print_can_frame(UDP_UDP_FORMAT_STRG, netframe, cs2_config_data.verbose & !background);
}
} else {
/* send packet on CAN */
ret = frame_to_can(sc, netframe);
print_can_frame(NET_UDP_FORMAT_STRG, netframe, cs2_config_data.verbose & !background);
check_data_udp(sb, (struct sockaddr *)&baddr, &cs2_config_data, netframe);
}
}
}
/* received a TCP packet */
if (FD_ISSET(st, &read_fds)) {
conn_fd = accept(st, (struct sockaddr *)&tcp_addr, &tcp_client_length);
if (cs2_config_data.verbose && !background) {
printf("new client: %s, port %d conn fd: %d max fds: %d\n", inet_ntop(AF_INET, &(tcp_addr.sin_addr),
buffer, sizeof(buffer)), ntohs(tcp_addr.sin_port), conn_fd, max_fds);
}
syslog(LOG_NOTICE, "%s: new client: %s port %d conn fd: %d max fds: %d\n", __func__,
inet_ntop(AF_INET, &(tcp_addr.sin_addr), buffer, sizeof(buffer)), ntohs(tcp_addr.sin_port), conn_fd, max_fds);
for (i = 0; i < MAX_TCP_CONN; i++) {
if (tcp_client[i] < 0) {
tcp_client[i] = conn_fd; /* save new TCP client descriptor */
break;
}
}
if (i == MAX_TCP_CONN) {
fprintf(stderr, "too many TCP clients\n");
syslog(LOG_ERR, "%s: too many TCP clients\n", __func__);
}
FD_SET(conn_fd, &all_fds); /* add new descriptor to set */
max_fds = MAX(conn_fd, max_fds); /* for select */
max_tcp_i = MAX(i, max_tcp_i); /* max index in tcp_client[] array */
/* send embedded CAN ping */
memcpy(netframe, M_CAN_PING, CAN_ENCAP_SIZE);
net_to_net(conn_fd, NULL, netframe, CAN_ENCAP_SIZE);
if (cs2_config_data.verbose && !background)
printf("send embedded CAN ping\n");
if (--nready <= 0)
continue; /* no more readable descriptors */
}
/* received a packet on second TCP port */
if (FD_ISSET(st2, &read_fds)) {
conn_fd = accept(st2, (struct sockaddr *)&tcp_addr2, &tcp_client_length);
/* TODO : close missing */
if (cs2_config_data.verbose && !background) {
printf("new client: %s, port %d conn fd: %d max fds: %d\n", inet_ntop(AF_INET, &(tcp_addr2.sin_addr),
buffer, sizeof(buffer)), ntohs(tcp_addr2.sin_port), conn_fd, max_fds);
}
syslog(LOG_NOTICE, "%s: new client: %s port %d conn fd: %d max fds: %d\n", __func__,
inet_ntop(AF_INET, &(tcp_addr2.sin_addr), buffer, sizeof(buffer)), ntohs(tcp_addr2.sin_port), conn_fd, max_fds);
FD_SET(conn_fd, &all_fds); /* add new descriptor to set */
max_fds = MAX(conn_fd, max_fds); /* for select */
max_tcp_i = MAX(i, max_tcp_i); /* max index in tcp_client[] array */
}
/* check for already connected TCP clients */
for (i = 0; i <= max_tcp_i; i++) { /* check all clients for data */
tcp_socket = tcp_client[i];
if (tcp_socket < 0)
continue;
/* printf("%s tcp packet received from client #%d max_tcp_i:%d todo:%d\n", time_stamp(timestamp), i, max_tcp_i,nready); */
if (FD_ISSET(tcp_socket, &read_fds)) {
if (cs2_config_data.verbose && !background) {
time_stamp(timestamp);
printf("%s packet from: %s\n", timestamp, inet_ntop(AF_INET, &tcp_addr.sin_addr, buffer, sizeof(buffer)));
}
n = read(tcp_socket, netframe, MAXDG);
if (!n) {
/* connection closed by client */
if (cs2_config_data.verbose && !background) {
time_stamp(timestamp);
printf("%s client %s closed connection\n", timestamp, inet_ntop(AF_INET, &tcp_addr.sin_addr, buffer, sizeof(buffer)));
}
syslog(LOG_NOTICE, "%s: client %s closed connection\n", __func__, inet_ntop(AF_INET, &tcp_addr.sin_addr, buffer, sizeof(buffer)));
close(tcp_socket);
FD_CLR(tcp_socket, &all_fds);
tcp_client[i] = -1;
} else {
/* check the whole TCP packet, if there are more than one CAN frame included */
/* TCP packets with size modulo 13 !=0 are ignored though */
if (n % 13) {
time_stamp(timestamp);
if (!background)
fprintf(stderr, "%s received packet %% 13 : length %d - maybe close connection\n", timestamp, n);
syslog(LOG_ERR, "%s: received packet %% 13 : length %d - maybe close connection\n", __func__, n);
} else {
for (i = 0; i < n; i += CAN_ENCAP_SIZE) {
/* check if we need to forward the message to CAN */
if (!check_data(tcp_socket, &cs2_config_data, &netframe[i])) {
ret = frame_to_can(sc, &netframe[i]);
if (!ret) {
if (i > 0)
print_can_frame(TCP_FORMATS_STRG, &netframe[i], cs2_config_data.verbose & !background);
else
print_can_frame(TCP_FORMAT_STRG, &netframe[i], cs2_config_data.verbose & !background);
}
net_to_net(sb, (struct sockaddr *)&baddr, netframe, CAN_ENCAP_SIZE);
print_can_frame(UDP_FORMAT_STRG, netframe, cs2_config_data.verbose & !background);
}
}
}
}
if (--nready <= 0)
break; /* no more readable descriptors */
}
}
}
closelog();
close(sc);
close(sa);
close(sb);
close(st);
close(st2);
return 0;
}
int main(int argc, char **argv) {
pid_t pid;
int n, i, max_fds, opt, max_tcp_i, nready, conn_fd, tcp_client[MAX_TCP_CONN];
struct can_frame frame;
char timestamp[16];
/* UDP incoming socket , CAN socket, UDP broadcast socket, TCP socket */
int sc, st, tcp_socket;
struct sockaddr_in tcp_addr;
/* vars for determing broadcast address */
struct sockaddr_can caddr;
struct ifreq ifr;
socklen_t caddrlen = sizeof(caddr);
socklen_t tcp_client_length = sizeof(tcp_addr);
fd_set all_fds, read_fds;
int ret;
struct timeval tv;
int local_tcp_port = DEF_TCP_PORT;
int background = 1;
/* const int off = 0; */
char buffer[64];
/* clear timestamp for last CAN frame sent */
memset(&last_sent, 0, sizeof(last_sent));
memset(ifr.ifr_name, 0, sizeof(ifr.ifr_name));
strcpy(ifr.ifr_name, "can0");
while ((opt = getopt(argc, argv, "t:i:vhf?")) != -1) {
switch (opt) {
case 't':
local_tcp_port = strtoul(optarg, (char **)NULL, 10);
break;
case 'i':
strncpy(ifr.ifr_name, optarg, sizeof(ifr.ifr_name) - 1);
break;
case 'f':
background = 0;
break;
case 'h':
case '?':
print_usage(basename(argv[0]));
exit(EXIT_SUCCESS);
default:
fprintf(stderr, "Unknown option %c\n", opt);
print_usage(basename(argv[0]));
exit(EXIT_FAILURE);
}
}
/* prepare TCP socket */
st = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
if (st < 0) {
fprintf(stderr, "creating TCP socket error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
tcp_addr.sin_family = AF_INET;
tcp_addr.sin_addr.s_addr = htonl(INADDR_ANY);
tcp_addr.sin_port = htons(local_tcp_port);
if (bind(st, (struct sockaddr *)&tcp_addr, sizeof(tcp_addr)) < 0) {
fprintf(stderr, "binding TCP socket error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
if (listen(st, MAXPENDING) < 0) {
fprintf(stderr, "starting TCP listener error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
/* prepare TCP clients array */
max_tcp_i = -1; /* index into tcp_client[] array */
for (i = 0; i < MAX_TCP_CONN; i++)
tcp_client[i] = -1; /* -1 indicates available entry */
/* prepare CAN socket */
memset(&caddr, 0, sizeof(caddr));
sc = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (sc < 0) {
fprintf(stderr, "creating CAN socket error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
caddr.can_family = AF_CAN;
if (ioctl(sc, SIOCGIFINDEX, &ifr) < 0) {
fprintf(stderr, "setup CAN error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
caddr.can_ifindex = ifr.ifr_ifindex;
if (bind(sc, (struct sockaddr *)&caddr, caddrlen) < 0) {
fprintf(stderr, "binding CAN socket error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
/* daemonize the process if requested */
if (background) {
/* fork off the parent process */
pid = fork();
if (pid < 0)
exit(EXIT_FAILURE);
/* if we got a good PID, then we can exit the parent process */
if (pid > 0) {
printf("Going into background ...\n");
exit(EXIT_SUCCESS);
}
}
setlogmask(LOG_UPTO(LOG_NOTICE));
openlog("lan-schnitte", LOG_CONS | LOG_NDELAY, LOG_DAEMON);
/* set select timeout -> send periodic CAN Ping */
memset(&tv, 0, sizeof(tv));
tv.tv_sec = 1;
tv.tv_usec = 0;
FD_ZERO(&all_fds);
FD_SET(sc, &all_fds);
FD_SET(st, &all_fds);
max_fds = MAX(sc, st);
while (1) {
read_fds = all_fds;
nready = select(max_fds + 1, &read_fds, NULL, NULL, &tv);
if (nready == 0) {
/* send_can_ping(sc); */
tv.tv_sec = 1;
tv.tv_usec = 0;
continue;
} else if (nready < 0)
fprintf(stderr, "select error: %s\n", strerror(errno));
tv.tv_sec = 1;
tv.tv_usec = 0;
/* received a CAN frame */
if (FD_ISSET(sc, &read_fds)) {
/* reading via SockatCAN */
if (read(sc, &frame, sizeof(struct can_frame)) < 0) {
fprintf(stderr, "reading CAN frame error: %s\n", strerror(errno));
syslog(LOG_ERR, "%s: reading CAN frame error: %s\n", __func__, strerror(errno));
}
/* if CAN Frame is EFF do it */
if (frame.can_id & CAN_EFF_FLAG) { /* only EFF frames are valid */
/* send CAN frame to all connected TCP clients */
for (i = 0; i <= max_tcp_i; i++) { /* check all clients for data */
tcp_socket = tcp_client[i];
if (tcp_socket < 0)
continue;
frame_to_net(tcp_socket, (struct sockaddr *)&tcp_addr, (struct can_frame *)&frame);
print_can_frame(CAN_TCP_FORMAT_STRG, netframe, !background);
}
}
}
/* received a TCP packet */
if (FD_ISSET(st, &read_fds)) {
conn_fd = accept(st, (struct sockaddr *)&tcp_addr, &tcp_client_length);
if (!background) {
printf("new client: %s, port %d conn fd: %d max fds: %d\n", inet_ntop(AF_INET, &(tcp_addr.sin_addr),
buffer, sizeof(buffer)), ntohs(tcp_addr.sin_port), conn_fd, max_fds);
}
syslog(LOG_NOTICE, "%s: new client: %s port %d conn fd: %d max fds: %d\n", __func__,
inet_ntop(AF_INET, &(tcp_addr.sin_addr), buffer, sizeof(buffer)), ntohs(tcp_addr.sin_port),
conn_fd, max_fds);
for (i = 0; i < MAX_TCP_CONN; i++) {
if (tcp_client[i] < 0) {
tcp_client[i] = conn_fd; /* save new TCP client descriptor */
break;
}
}
if (i == MAX_TCP_CONN) {
fprintf(stderr, "too many TCP clients\n");
syslog(LOG_ERR, "%s: too many TCP clients\n", __func__);
}
FD_SET(conn_fd, &all_fds); /* add new descriptor to set */
max_fds = MAX(conn_fd, max_fds); /* for select */
max_tcp_i = MAX(i, max_tcp_i); /* max index in tcp_client[] array */
if (--nready <= 0)
continue; /* no more readable descriptors */
}
/* check for already connected TCP clients */
for (i = 0; i <= max_tcp_i; i++) { /* check all clients for data */
tcp_socket = tcp_client[i];
if (tcp_socket < 0)
continue;
if (FD_ISSET(tcp_socket, &read_fds)) {
if (!background) {
time_stamp(timestamp);
printf("%s packet from: %s\n", timestamp,
inet_ntop(AF_INET, &tcp_addr.sin_addr, buffer, sizeof(buffer)));
}
n = read(tcp_socket, netframe, MAXDG);
if (!n) {
/* connection closed by client */
if (!background) {
time_stamp(timestamp);
printf("%s client %s closed connection\n", timestamp,
inet_ntop(AF_INET, &tcp_addr.sin_addr, buffer, sizeof(buffer)));
}
syslog(LOG_NOTICE, "%s: client %s closed connection\n", __func__,
inet_ntop(AF_INET, &tcp_addr.sin_addr, buffer, sizeof(buffer)));
close(tcp_socket);
FD_CLR(tcp_socket, &all_fds);
tcp_client[i] = -1;
} else {
/* check the whole TCP packet, if there are more than one CAN frame included */
/* TCP packets with size modulo 13 !=0 are ignored though */
if (n % 13) {
time_stamp(timestamp);
if (!background)
fprintf(stderr, "%s received packet %% 13 : length %d - maybe close connection\n", timestamp, n);
syslog(LOG_ERR, "%s: received packet %% 13 : length %d - maybe close connection\n", __func__, n);
} else {
for (i = 0; i < n; i += CAN_ENCAP_SIZE) {
ret = frame_to_can(sc, &netframe[i]);
if (!ret) {
if (i > 0)
print_can_frame(TCP_FORMATS_STRG, &netframe[i], !background);
else
print_can_frame(TCP_FORMAT_STRG, &netframe[i], !background);
}
}
}
}
if (--nready <= 0)
break; /* no more readable descriptors */
}
}
}
closelog();
close(sc);
close(st);
return 0;
}
int main(int argc, char **argv) {
pid_t pid;
extern int optind, opterr, optopt;
int n, i, max_fds, opt, max_tcp_i, nready, conn_fd, tcp_client[MAX_TCP_CONN];;
struct can_frame frame;
int sa, sc, sb, st, tcp_socket; /* UDP socket , CAN socket, UDP Broadcast Socket, TCP Socket */
struct sockaddr_in saddr, baddr, tcp_addr;
struct sockaddr_can caddr;
struct ifreq ifr;
socklen_t caddrlen = sizeof(caddr);
socklen_t tcp_client_length = sizeof(tcp_addr);
fd_set all_fds, read_fds;
uint32_t canid;
int s, nbytes, ret;
int local_udp_port = 15731;
int local_tcp_port = 15731;
int destination_port = 15730;
int verbose = 1;
int background = 1;
const int on = 1;
char udp_dst_address[] = "255.255.255.255";
char buffer[64];
strcpy(ifr.ifr_name, "can0");
while ((opt = getopt(argc, argv, "u:t:d:b:i:vhf?")) != -1) {
switch (opt) {
case 'u':
local_udp_port = strtoul(optarg, (char **) NULL, 10);
break;
case 't':
local_tcp_port = strtoul(optarg, (char **) NULL, 10);
break;
case 'd':
destination_port = strtoul(optarg, (char **) NULL, 10);
break;
case 'b':
if ( strlen(optarg) <=15 ) {
strcpy(udp_dst_address, optarg);
} else {
fprintf(stderr, "UDP broadcast address error: %s\n", optarg);
}
break;
case 'i':
strcpy(ifr.ifr_name, optarg);
break;
case 'v':
verbose = 1;
break;
case 'f':
background = 0;
break;
case 'h':
case '?':
print_usage(basename(argv[0]));
exit(0);
break;
default:
fprintf(stderr, "Unknown option %c\n", opt);
print_usage(basename(argv[0]));
exit(1);
break;
}
}
/* prepare udp sending socket struct */
bzero(&baddr, sizeof(baddr));
baddr.sin_family = AF_INET;
baddr.sin_port = htons(destination_port);
s = inet_pton(AF_INET, udp_dst_address, &baddr.sin_addr);
if (s <= 0) {
if (s == 0) {
fprintf(stderr, "error: UDP sending port not in presentation format\n");
} else {
perror("inet_pton error\n");
}
exit(1);
}
/* prepare UDP sending socket */
if ((sb = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
perror("error creating UDP sending socket");
exit(1);
}
if (setsockopt(sb, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on)) < 0) {
perror("error setup UDP broadcast option\n");
exit(1);
}
/* prepare reading UDP socket */
bzero(&saddr, sizeof(saddr));
saddr.sin_family = AF_INET;
saddr.sin_addr.s_addr = htonl(INADDR_ANY);
saddr.sin_port = htons(local_udp_port);
if ((sa = socket(PF_INET, SOCK_DGRAM, 0)) < 0) {
perror("error creating UDP reading socket\n");
exit(1);
}
while (bind(sa, (struct sockaddr *) &saddr, sizeof(saddr)) < 0) {
printf(".");
fflush(NULL);
usleep(100000);
}
/* prepare TCP socket */
if ((st = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP)) < 0) {
perror("error creating TCP socket\n");
exit(1);
}
tcp_addr.sin_family = AF_INET;
tcp_addr.sin_addr.s_addr = htonl(INADDR_ANY);
tcp_addr.sin_port = htons(local_tcp_port);
if (bind(st, (struct sockaddr *)&tcp_addr, sizeof(tcp_addr)) < 0) {
perror("error binding TCP socket\n");
exit(1);
}
if (listen(st, MAXPENDING) < 0) {
perror("error starting TCP listener\n");
exit(1);
}
/* prepare TCP clients array */
max_tcp_i = -1; /* index into tcp_client[] array */
for (i = 0; i < MAX_TCP_CONN; i++)
tcp_client[i] = -1; /* -1 indicates available entry */
/* prepare CAN socket */
if ((sc = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
perror("error creating CAN socket\n");
exit(1);
}
caddr.can_family = AF_CAN;
if (ioctl(sc, SIOCGIFINDEX, &ifr) < 0) {
perror("SIOCGIFINDEX error\n");
exit(1);
}
caddr.can_ifindex = ifr.ifr_ifindex;
if (bind(sc, (struct sockaddr *) &caddr, caddrlen) < 0) {
perror("error binding CAN socket\n");
exit(1);
}
/* start Maerklin 60113 box */
send_magic_start_60113_frame(sc, verbose);
/* daemonize the process if requested */
if (background) {
/* fork off the parent process */
pid = fork();
if (pid < 0) {
exit(EXIT_FAILURE);
}
/* if we got a good PID, then we can exit the parent process */
if (pid > 0) {
printf("Going into background ...\n");
exit(EXIT_SUCCESS);
}
}
FD_ZERO(&all_fds);
FD_SET(sc, &all_fds);
FD_SET(sa, &all_fds);
FD_SET(st, &all_fds);
max_fds = MAX(MAX(sc, sa),st);
while (1) {
read_fds = all_fds;
nready = select(max_fds + 1 , &read_fds, NULL, NULL, NULL);
if (nready<0)
perror("select error\n");
/* received a CAN frame */
if (FD_ISSET(sc, &read_fds)) {
if ((nbytes = read(sc, &frame, sizeof(struct can_frame))) < 0) {
perror("error reading CAN frame\n");
} else if (frame.can_id & CAN_EFF_FLAG) { /* only EFF frames are valid */
/* send UDP frame */
frame_to_net(sb, (struct sockaddr *) &baddr, (struct can_frame *) &frame);
if (verbose && !background)
print_can_frame(UDP_FORMAT_STRG, netframe);
/* send CAN frame to all connected TCP clients */
/* TODO: need all clients the packets ? */
for (i = 0; i <= max_tcp_i; i++) { /* check all clients for data */
if ( (tcp_socket = tcp_client[i]) < 0)
continue;
frame_to_net(tcp_socket, (struct sockaddr *) &tcp_addr, (struct can_frame *) &frame);
if (verbose && !background)
print_can_frame(CAN_TCP_FORMAT_STRG, netframe);
}
// printf("%s tcp packet received from client #%d max_tcp_i:%d todo:%d\n", time_stamp(), i, max_tcp_i,nready);
}
}
/* received a UDP packet */
if (FD_ISSET(sa, &read_fds)) {
if (read(sa, netframe, MAXDG) == 13) {
/* send packet on CAN */
ret = frame_to_can(sc, netframe);
if (verbose && !background)
print_can_frame(NET_UDP_FORMAT_STRG, netframe);
memcpy(&canid, netframe, 4);
canid=ntohl(canid);
/* answer to encapsulated CAN ping from LAN to LAN */
if (((canid & 0x00FF0000UL) == 0x00310000UL)
&& (netframe[11] = 0xEE) && (netframe[12] = 0xEE)) {
printf(" received CAN ping\n");
netframe[0] = 0x00;
netframe[1] = 0x30;
netframe[2] = 0x00;
netframe[3] = 0x00;
netframe[4] = 0x00;
s = sendto(sb, netframe, 13, 0, (struct sockaddr *) &baddr, sizeof(baddr));
if (s != 13) {
perror("error sending UDP data (CAN Ping)\n");
} else if (verbose & !background) {
print_can_frame(NET_UDP_FORMAT_STRG, netframe);
printf(" replied CAN ping\n");
}
}
}
}
/* received a TCP packet */
if (FD_ISSET(st, &read_fds)) {
conn_fd = accept(st, (struct sockaddr *) &tcp_addr, &tcp_client_length);
if (verbose && !background) {
printf("new client: %s, port %d conn fd: %d max fds: %d\n", inet_ntop(AF_INET,
&(tcp_addr.sin_addr), buffer, sizeof(buffer)), ntohs(tcp_addr.sin_port), conn_fd, max_fds);
}
for (i = 0; i < MAX_TCP_CONN; i++) {
if (tcp_client[i] < 0) {
tcp_client[i] = conn_fd; /* save new TCP client descriptor */
break;
}
}
if (i == MAX_TCP_CONN)
perror("too many TCP clients\n");
FD_SET(conn_fd, &all_fds); /* add new descriptor to set */
max_fds = MAX(conn_fd,max_fds); /* for select */
max_tcp_i = MAX(i, max_tcp_i); /* max index in tcp_client[] array */
if (--nready <= 0)
continue; /* no more readable descriptors */
}
/* check for already connected TCP clients */
for (i = 0; i <= max_tcp_i; i++) { /* check all clients for data */
if ( (tcp_socket = tcp_client[i]) < 0)
continue;
// printf("%s tcp packet received from client #%d max_tcp_i:%d todo:%d\n", time_stamp(), i, max_tcp_i,nready);
if (FD_ISSET(tcp_socket, &read_fds)) {
if (verbose && !background) {
printf("%s packet from: %s\n", time_stamp(), inet_ntop(AF_INET,
&tcp_addr.sin_addr, buffer, sizeof(buffer)));
}
if ( (n = read(tcp_socket, netframe, MAXDG)) == 0) {
/* connection closed by client */
close(tcp_socket);
FD_CLR(tcp_socket, &all_fds);
tcp_client[i] = -1;
} else {
if (n == 13) {
ret = frame_to_can(sc, netframe);
if ((ret == 0) && (verbose && !background))
print_can_frame(TCP_FORMAT_STRG, netframe);
} else {
fprintf(stderr, "%s received package =!13 : %d\n", time_stamp(), n);
}
}
if (--nready <= 0)
break; /* no more readable descriptors */
}
}
}
close(sc);
close(sa);
close(sb);
close(st);
return 0;
}