int check_data_udp(int udp_socket, struct sockaddr *baddr, struct cs2_config_data_t *cs2_config_data, unsigned char *netframe) {
uint32_t canid;
memcpy(&canid, netframe, 4);
canid = ntohl(canid);
switch (canid & 0xFFFF0000UL) {
case (0x00300000UL):
if (cs2fake_ping) {
if (cs2_config_data->verbose)
printf(" received CAN ping\n");
memcpy(netframe, M_CAN_PING_CS2, 13);
if (net_to_net(udp_socket, baddr, netframe, CAN_ENCAP_SIZE)) {
fprintf(stderr, "sending UDP data (CAN Ping fake CS2) error:%s \n", strerror(errno));
syslog(LOG_ERR, "%s: sending UDP data (CAN Ping fake CS2) error:%s\n", __func__, strerror(errno));
} else {
print_can_frame(NET_UDP_FORMAT_STRG, netframe, cs2_config_data->verbose);
if (cs2_config_data->verbose)
printf(" replied CAN ping (fake CS2)\n");
}
}
break;
case (0x00310000UL):
if ((netframe[11] == 0xEE) && (netframe[12] == 0xEE)) {
if (cs2_config_data->verbose)
printf(" received CAN ping\n");
memcpy(netframe, M_PING_RESPONSE, 5);
if (net_to_net(udp_socket, baddr, netframe, CAN_ENCAP_SIZE)) {
fprintf(stderr, "sending UDP data (CAN Ping) error:%s \n", strerror(errno));
syslog(LOG_ERR, "%s: sending UDP data (CAN Ping) error:%s\n", __func__, strerror(errno));
} else {
print_can_frame(NET_UDP_FORMAT_STRG, netframe, cs2_config_data->verbose);
if (cs2_config_data->verbose)
printf(" replied CAN ping\n");
}
if (cs2_config_data->cs2_config_copy)
copy_cs2_config(cs2_config_data);
}
break;
case (0x00400000UL):
/* check for initiated config request */
if (canid == 0x0040af7e) {
if (cs2_config_data->verbose)
printf("copy config request\n");
syslog(LOG_NOTICE, "%s %d: copy config request\n", __func__, __LINE__);
cs2_config_data->cs2_config_copy = 1;
copy_cs2_config(cs2_config_data);
}
break;
default:
break;
}
return 0;
}
int send_can_frame(int can_socket, struct can_frame *frame, int verbose) {
frame->can_id &= CAN_EFF_MASK;
frame->can_id |= CAN_EFF_FLAG;
/* send CAN frame */
if (write(can_socket, frame, sizeof(*frame)) != sizeof(*frame)) {
fprintf(stderr, "error writing CAN frame: %s\n", strerror(errno));
return -1;
}
if (verbose)
print_can_frame(T_CAN_FORMAT_STRG, frame);
return 0;
}
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;
}
/*! \fn void app_task_uarttocan(void *pdata)
*
* \brief 串口转换CAN任务
*
* \param 无
*
* \exception 无
*
* \return 无
*/
void app_task_uarttocan(void *pdata)
{
/* 防止编译器告警 */
pdata = pdata;
/*! 初始化CAN */
can_init();
/*! 任务循环 */
for( ; ; )
{
/*! 通信协议 */
if(EXIT_FAILURE == protocol_process())
{
debug_msg("E:Protocol process error!", __FILE__, __LINE__);
}
else
{
/* 任务延时 */
OSTimeDlyHMSM(0, 0, 0, 1);
}
print_can_frame();
}
}
int main(int argc, char **argv) {
pthread_t pth;
int opt;
struct ifreq ifr;
struct trigger_t trigger_data;
struct sockaddr_can caddr;
fd_set readfds, exceptfds;
struct can_frame frame;
uint16_t member;
uint8_t buffer[MAXLEN];
memset(&trigger_data, 0, sizeof(trigger_data));
memset(ifr.ifr_name, 0, sizeof(ifr.ifr_name));
strcpy(ifr.ifr_name, "can0");
trigger_data.led_pin = -1;
trigger_data.pb_pin = -1;
trigger_data.interval = DEF_INTERVAL;
while ((opt = getopt(argc, argv, "i:l:p:t:fvh?")) != -1) {
switch (opt) {
case 'i':
strncpy(ifr.ifr_name, optarg, sizeof(ifr.ifr_name));
break;
case 't':
trigger_data.interval = atoi(optarg);
break;
case 'l':
trigger_data.led_pin = atoi(optarg);
break;
case 'p':
trigger_data.pb_pin = atoi(optarg);
break;
case 'v':
trigger_data.verbose = 1;
break;
case 'f':
trigger_data.background = 0;
break;
case 'h':
case '?':
usage(basename(argv[0]));
exit(EXIT_SUCCESS);
default:
usage(basename(argv[0]));
exit(EXIT_FAILURE);
}
}
/* prepare CAN socket */
if ((trigger_data.socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
fprintf(stderr, "error creating CAN socket: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
memset(&caddr, 0, sizeof(caddr));
caddr.can_family = AF_CAN;
if (ioctl(trigger_data.socket, SIOCGIFINDEX, &ifr) < 0) {
fprintf(stderr, "setup CAN socket error: %s %s\n", strerror(errno), ifr.ifr_name);
exit(EXIT_FAILURE);
}
caddr.can_ifindex = ifr.ifr_ifindex;
if (bind(trigger_data.socket, (struct sockaddr *)&caddr, sizeof(caddr)) < 0) {
fprintf(stderr, "error binding CAN socket: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
trigger_data.caddr = caddr;
/* Create thread if LED pin defined */
if ((trigger_data.led_pin) > 0) {
trigger_data.led_pattern = LED_ST_HB_SLOW;
gpio_export(trigger_data.led_pin);
gpio_direction(trigger_data.led_pin, 0);
if (pthread_mutex_init(&lock, NULL)) {
fprintf(stderr, "can't init mutex %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
if (pthread_create(&pth, NULL, LEDMod, &trigger_data)) {
fprintf(stderr, "can't create pthread %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
if (!trigger_data.background && trigger_data.verbose)
printf("created LED thread\n");
}
if (trigger_data.background) {
pid_t pid;
/* 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) {
if (trigger_data.verbose)
printf("Going into background ...\n");
exit(EXIT_SUCCESS);
}
}
/* initialize push button */
if ((trigger_data.pb_pin) > 0) {
/* first free pin */
gpio_unexport(trigger_data.pb_pin);
gpio_export(trigger_data.pb_pin);
gpio_direction(trigger_data.pb_pin, 1);
gpio_edge(trigger_data.pb_pin, EDGE_FALLING);
trigger_data.pb_fd = gpio_open(trigger_data.pb_pin);
}
FD_ZERO(&readfds);
FD_ZERO(&exceptfds);
/* delete pending push button event */
if ((trigger_data.pb_pin) > 0) {
read(trigger_data.pb_fd, NULL, 100);
lseek(trigger_data.pb_fd, 0, SEEK_SET);
}
/* loop forever TODO: if interval is set */
while (1) {
FD_SET(trigger_data.socket, &readfds);
/* extend FD_SET only if push button pin is set */
if (trigger_data.pb_pin > 0)
FD_SET(trigger_data.pb_fd, &exceptfds);
if (select(MAX(trigger_data.socket, trigger_data.pb_fd) + 1, &readfds, NULL, &exceptfds, NULL) < 0) {
fprintf(stderr, "select error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
/* CAN frame event */
if (FD_ISSET(trigger_data.socket, &readfds)) {
if (read(trigger_data.socket, &frame, sizeof(struct can_frame)) < 0)
fprintf(stderr, "error reading CAN frame: %s\n", strerror(errno));
if (frame.can_id & CAN_EFF_FLAG) {
switch ((frame.can_id & 0x00FF0000UL) >> 16) {
case 0x31:
if (trigger_data.verbose)
print_can_frame(F_CAN_FORMAT_STRG, &frame);
memcpy(&member, frame.data, sizeof(member));
member = ntohs(member);
/* look for MS2 */
if ((member & 0xfff0) == 0x4d50)
get_ms2_dbsize(&trigger_data);
break;
case 0x41:
if (trigger_data.verbose)
print_can_frame(F_CAN_FORMAT_STRG, &frame);
break;
case 0x42:
get_data(&trigger_data, &frame);
/* if (trigger_data.verbose)
print_can_frame(F_CAN_FORMAT_STRG, &frame); */
break;
default:
if (trigger_data.verbose)
print_can_frame(F_CAN_FORMAT_STRG, &frame);
break;
}
}
}
/* push button event */
if (FD_ISSET(trigger_data.pb_fd, &exceptfds)) {
set_led_pattern(&trigger_data, LED_ST_HB_FAST);
/* wait some time for LED pattern change */
usec_sleep(1000 * 1000);
/* send CAN Member Ping */
frame.can_id = 0x00300300;
frame.can_dlc = 0;
memset(frame.data, 0, 8);
if (send_can_frame(trigger_data.socket, &frame, trigger_data.verbose) < 0)
fprintf(stderr, "can't send CAN Member Ping: %s\n", strerror(errno));
lseek(trigger_data.pb_fd, 0, SEEK_SET);
if (read(trigger_data.pb_fd, buffer, sizeof(buffer)) < 0)
fprintf(stderr, "error reading GPIO status: %s\n", strerror(errno));
printf("push button event\n");
}
}
/* TODO : wait until copy is done */
if ((trigger_data.pb_pin) > 0)
gpio_unexport(trigger_data.pb_pin);
if ((trigger_data.led_pin) > 0) {
gpio_unexport(trigger_data.led_pin);
pthread_join(pth, (void *)&trigger_data);
pthread_mutex_unlock(&lock);
}
return 0;
}
int main(int argc, char **argv) {
pid_t pid;
int opt;
canmsg_t frame;
int sa,sb; /* UDP socket UDP Broadcast Socket */
int fdc; /* can4linux FD */
struct sockaddr_in saddr, baddr;
fd_set readfds;
int s;
int local_port = 15730;
int destination_port = 15731;
int verbose = 0;
int background = 1;
int canid = 0;
char can_interface[64];
const int on = 1;
const char broadcast_address[] = "255.255.255.255";
memset(&saddr, 0, sizeof(saddr));
memset(&baddr, 0, sizeof(baddr));
memset(&frame, 0, sizeof(frame));
memset(udpframe, 0, sizeof(udpframe));
memset(udpframe_reply, 0, sizeof(udpframe_reply));
/* prepare udp destination struct with defaults */
baddr.sin_family = AF_INET;
baddr.sin_port = htons(destination_port);
s = inet_pton(AF_INET, broadcast_address, &baddr.sin_addr);
if (s <= 0) {
if (s == 0) {
fprintf(stderr, "UDP IP invalid\n");
} else {
fprintf(stderr, "invalid address family\n");
}
exit(EXIT_FAILURE);
}
while ((opt = getopt(argc, argv, "l:d:b:i:hfv?")) != -1) {
switch (opt) {
case 'l':
local_port = strtoul(optarg, (char **) NULL, 10);
break;
case 'd':
destination_port = strtoul(optarg, (char **) NULL, 10);
baddr.sin_port = htons(destination_port);
break;
case 'b':
s = inet_pton(AF_INET, optarg, &baddr.sin_addr);
if (s <= 0) {
if (s == 0) {
fprintf(stderr, "invalid IP address: %s\n", strerror(errno));
} else {
fprintf(stderr, "inet_pton error: %s\n", strerror(errno));
}
exit(EXIT_FAILURE);
}
break;
case 'i':
strncpy(can_interface, optarg, sizeof(can_interface) - 1);
break;
case 'v':
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);
}
}
if ((sa = socket(PF_INET, SOCK_DGRAM, 0)) < 0) {
fprintf(stderr, "UDP socket error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
saddr.sin_family = AF_INET;
saddr.sin_addr.s_addr = htonl(INADDR_ANY);
saddr.sin_port = htons(local_port);
if (bind(sa, (struct sockaddr *) &saddr, sizeof(saddr)) < 0) {
fprintf(stderr, "UDP bind error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
if ((fdc = open(can_interface, O_RDWR)) < 0 ) {
fprintf(stderr, "can't open CAN interface: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
/* prepare UDP sending socket */
if ((sb = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
fprintf(stderr, "Send UDP socket error %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
if (setsockopt(sb, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on)) < 0) {
fprintf(stderr, "UDP set socket option error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
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);
}
}
while (1) {
FD_ZERO(&readfds);
FD_SET(fdc, &readfds);
FD_SET(sa, &readfds);
if (select((fdc > sa) ? fdc + 1 : sa + 1, &readfds, NULL, NULL, NULL) <0 ) {
fprintf(stderr, "select error: %s\n", strerror(errno));
};
/* received a CAN frame */
if (FD_ISSET(fdc, &readfds)) {
if (read(fdc, &frame, 1) < 0)
fprintf(stderr, "CAN read error: %s\n", strerror(errno));
/* prepare UDP packet - extended format */
canid = htonl(frame.id);
memcpy(udpframe, &canid, 4);
udpframe[4] = frame.length;
memcpy(&udpframe[5], &frame.data, 8);
/* send UDP frame */
if (sendto(sb, udpframe, 13, 0, (struct sockaddr *) &baddr, sizeof(baddr)) !=13)
fprintf(stderr, "UDP write error: %s\n", strerror(errno));
print_can_frame("->CAN>UDP CANID 0x%08lX R", &frame, verbose & !background);
}
/* received a UDP packet */
if (FD_ISSET(sa, &readfds)) {
if (read(sa, udpframe, MAXDG) == 13) {
/* prepare CAN frame - extended format only */
memcpy(&canid, &udpframe[0], 4);
frame.id = ntohl(canid);
frame.id &= CAN_EFF_MASK;
frame.flags |= MSG_EXT;
frame.length = udpframe[4];
memcpy(&frame.data, &udpframe[5], 8);
/* send CAN frame */
if (write(fdc, &frame, 1) != 1)
fprintf(stderr, "CAN write error: %s\n", strerror(errno));
print_can_frame("->UDP>CAN CANID 0x%08lX ", &frame, verbose & !background);
}
}
}
close(sa);
close(sb);
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 check_data(int tcp_socket, struct cs2_config_data_t *cs2_config_data, unsigned char *netframe) {
uint32_t canid;
char config_name[9];
char gbs_name[MAXLINE];
int ret;
gbs_name[0] = '\0';
ret = 0;
memcpy(&canid, netframe, 4);
canid = ntohl(canid);
switch (canid & 0xFFFF0000UL) {
case (0x00310000UL): /* CAN ping */
ret = 0;
/* looking for CS2.exe ping answer */
print_can_frame(NET_TCP_FORMAT_STRG, netframe, cs2_config_data->verbose);
if ((netframe[11] == 0xFF) && (netframe[12] == 0xFF)) {
if (cs2_config_data->verbose)
printf("got CS2 TCP ping - copy config var: %d\n", cs2_config_data->cs2_config_copy);
syslog(LOG_NOTICE, "%s: got CS2 TCP ping - copy config var: %d\n", __func__, cs2_config_data->cs2_config_copy);
cs2_config_data->cs2_tcp_socket = tcp_socket;
if (cs2_config_data->cs2_config_copy)
copy_cs2_config(cs2_config_data);
}
break;
case (0x00400000UL): /* config data */
/* mark frame to send over CAN */
ret = 0;
/* check for special copy request */
if (canid == 0x0040af7e) {
ret = 1;
netframe[1] |= 1;
netframe[4] = 4;
strcpy((char *)&netframe[5], "copy");
net_to_net(tcp_socket, NULL, netframe, CAN_ENCAP_SIZE);
if (cs2_config_data->verbose)
printf("CS2 copy request\n");
syslog(LOG_NOTICE, "%s: CS2 copy request\n", __func__);
cs2_config_data->cs2_config_copy = 1;
} else {
strncpy(config_name, (char *)&netframe[5], 8);
syslog(LOG_NOTICE, "%s: config request >%s<\n", __func__, config_name);
config_name[8] = '\0';
if (cs2_config_data->verbose)
printf("%s ID 0x%08x %s\n", __func__, canid, (char *)&netframe[5]);
netframe[1] |= 1;
net_to_net(tcp_socket, NULL, netframe, CAN_ENCAP_SIZE);
if (strcmp("loks", config_name) == 0) {
ret = 0;
syslog(LOG_NOTICE, "%s: sending lokomotive.cs2\n", __func__);
send_tcp_config_data("lokomotive.cs2", config_dir, canid, tcp_socket, CRC | COMPRESSED);
break;
} else if (strcmp("mags", config_name) == 0) {
ret = 1;
syslog(LOG_NOTICE, "%s: sending magnetartikel.cs2\n", __func__);
send_tcp_config_data("magnetartikel.cs2", config_dir, canid, tcp_socket, CRC | COMPRESSED);
break;
} else if (strncmp("gbs-", config_name, 4) == 0) {
int page_number;
ret = 1;
page_number = atoi(&config_name[4]);
strcat(gbs_name, "gleisbilder/");
if (page_number <= MAX_TRACK_PAGE) {
if (page_name[page_number]) {
strcat(gbs_name, page_name[page_number]);
syslog(LOG_NOTICE, "%s: sending %s\n", __func__, gbs_name);
send_tcp_config_data(gbs_name, config_dir, canid, tcp_socket, CRC | COMPRESSED);
break;
}
}
syslog(LOG_NOTICE, "%s: cant't send gbs-%d\n", __func__, page_number);
break;
} else if (strcmp("gbs", config_name) == 0) {
ret = 1;
syslog(LOG_NOTICE, "%s: sending gleisbild.cs2\n", __func__);
send_tcp_config_data("gleisbild.cs2", config_dir, canid, tcp_socket, CRC | COMPRESSED);
break;
} else if (strcmp("fs", config_name) == 0) {
ret = 1;
syslog(LOG_NOTICE, "%s: sending fahrstrassen.cs2\n", __func__);
send_tcp_config_data("fahrstrassen.cs2", config_dir, canid, tcp_socket, CRC | COMPRESSED);
break;
}
/* TODO : these files depends on different internal states */
else if (strcmp("lokstat", config_name) == 0) {
ret = 1;
/* fprintf(stderr, "%s: lokstat (lokomotive.sr2) not implemented yet\n", __func__); */
syslog(LOG_NOTICE, "%s: sending lokomotive.sr2\n", __func__);
send_tcp_config_data("lokomotive.sr2", config_dir, canid, tcp_socket, CRC | COMPRESSED);
break;
} else if (strcmp("magstat", config_name) == 0) {
ret = 1;
/* fprintf(stderr, "%s: magstat (magnetartikel.sr2) not implemented yet\n\n", __func__); */
syslog(LOG_NOTICE, "%s: sending magnetartikel.sr2\n", __func__);
send_tcp_config_data("magnetartikel.sr2", config_dir, canid, tcp_socket, CRC | COMPRESSED);
break;
} else if (strcmp("gbsstat", config_name) == 0) {
ret = 1;
/* fprintf(stderr, "%s: gbsstat (gbsstat.sr2) not implemented yet\n\n", __func__); */
syslog(LOG_NOTICE, "%s: sending gbsstat.sr2\n", __func__);
send_tcp_config_data("gbsstat.sr2", config_dir, canid, tcp_socket, CRC | COMPRESSED);
break;
} else if (strcmp("fsstat", config_name) == 0) {
ret = 1;
/* fprintf(stderr, "%s: fsstat (fahrstrassen.sr2) not implemented yet\n\n", __func__); */
syslog(LOG_NOTICE, "%s: sending fahrstrassen.sr2\n", __func__);
send_tcp_config_data("fahrstrassen.sr2", config_dir, canid, tcp_socket, CRC | COMPRESSED);
break;
}
break;
}
case (0x00420000UL):
/* check for bordcast config change: DLC = 7 */
if (netframe[4] == 7) {
/* TODO */
syslog(LOG_NOTICE, "%s: broadcast config change", __func__);
cs2_config_data->name = BROADCAST_C0NFIG_UPDATE;
cs2_config_data->state = CS2_STATE_BROADCAST_UPDATE;
}
/* check for initiated copy request or brodcast update */
reassemble_data(cs2_config_data, netframe);
print_can_frame(NET_TCP_FORMAT_STRG, netframe, cs2_config_data->verbose);
/* none CS2 copy request needs to be send over CAN */
if (canid & 0x0000fcff)
ret = 0;
else
ret = 1;
break;
/* fake cyclic MS1 slave monitoring response */
case (0x0C000000UL):
/* mark CAN frame to send */
ret = 0;
if (ms1_workaround)
netframe[5] = 0x03;
break;
default:
break;
}
return ret;
}
int main(int argc, char **argv)
{
pid_t pid;
int ret, s, opt;
struct can_frame frame;
/* UDP socket , CAN socket, UDP broadcast socket */
int sa, sc, sb;
struct sockaddr_in saddr, baddr;
struct sockaddr_can caddr;
struct ifreq ifr;
socklen_t caddrlen = sizeof(caddr);
fd_set readfds;
int local_port = DEF_UDP_RECV_PORT;
int destination_port = DEF_UDP_SEND_PORT;
int foreground = 0;
uint32_t canid = 0;
const int on = 1;
const char broadcast_address[] = "255.255.255.255";
memset(ifr.ifr_name, 0, sizeof(ifr.ifr_name));
strcpy(ifr.ifr_name, "can0");
memset(&saddr, 0, sizeof(saddr));
memset(&baddr, 0, sizeof(baddr));
memset(&caddr, 0, sizeof(caddr));
memset(&frame, 0, sizeof(frame));
memset(udpframe, 0, sizeof(udpframe));
/* prepare udp destination struct with defaults */
baddr.sin_family = AF_INET;
baddr.sin_port = htons(destination_port);
s = inet_pton(AF_INET, broadcast_address, &baddr.sin_addr);
if (s <= 0) {
if (s == 0)
fprintf(stderr, "UDP IP invalid\n");
else
fprintf(stderr, "invalid address family\n");
exit(EXIT_FAILURE);
}
while ((opt = getopt(argc, argv, "l:d:b:i:hf?")) != -1) {
switch (opt) {
case 'l':
local_port = strtoul(optarg, (char **)NULL, 10);
break;
case 'd':
destination_port = strtoul(optarg, (char **)NULL, 10);
baddr.sin_port = htons(destination_port);
break;
case 'b':
s = inet_pton(AF_INET, optarg, &baddr.sin_addr);
if (s <= 0) {
if (s == 0)
fprintf(stderr, "invalid IP address: %s\n", strerror(errno));
else
fprintf(stderr, "inet_pton error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
break;
case 'i':
strncpy(ifr.ifr_name, optarg, sizeof(ifr.ifr_name) - 1);
break;
case 'f':
foreground = 1;
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);
}
}
sa = socket(PF_INET, SOCK_DGRAM, 0);
if (sa < 0) {
fprintf(stderr, "UDP socket error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
saddr.sin_family = AF_INET;
saddr.sin_addr.s_addr = htonl(INADDR_ANY);
saddr.sin_port = htons(local_port);
if (bind(sa, (struct sockaddr *)&saddr, sizeof(saddr)) < 0) {
fprintf(stderr, "UDP bind error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
sc = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (sc < 0) {
fprintf(stderr, "CAN socket error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
caddr.can_family = AF_CAN;
if (ioctl(sc, SIOCGIFINDEX, &ifr) < 0) {
fprintf(stderr, "CAN setup error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
caddr.can_ifindex = ifr.ifr_ifindex;
if (bind(sc, (struct sockaddr *)&caddr, caddrlen) < 0) {
fprintf(stderr, "CAN bind error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
/* prepare UDP sending socket */
sb = socket(AF_INET, SOCK_DGRAM, 0);
if (sb < 0) {
fprintf(stderr, "Send UDP socket error %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
if (setsockopt(sb, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on)) < 0) {
fprintf(stderr, "UDP set socket option error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
if (!foreground) {
/* 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(&readfds);
while (1) {
FD_SET(sc, &readfds);
FD_SET(sa, &readfds);
if (select((sc > sa) ? sc + 1 : sa + 1, &readfds, NULL, NULL, NULL) < 0) {
fprintf(stderr, "select error: %s\n", strerror(errno));
break;
}
/* received a CAN frame */
if (FD_ISSET(sc, &readfds)) {
if (read(sc, &frame, sizeof(struct can_frame)) < 0) {
fprintf(stderr, "CAN read error: %s\n", strerror(errno));
break;
} else {
/* prepare UDP packet */
memset(udpframe, 0, 13);
canid = htonl(frame.can_id);
memcpy(udpframe, &canid, 4);
udpframe[4] = frame.can_dlc;
memcpy(&udpframe[5], &frame.data, frame.can_dlc);
/* send UDP packet */
if (sendto(sb, udpframe, 13, 0, (struct sockaddr *)&baddr, sizeof(baddr)) != 13) {
fprintf(stderr, "UDP write error: %s\n", strerror(errno));
break;
}
print_can_frame(&frame, CAN_SRC_STRG, foreground);
}
}
/* received a UDP packet */
if (FD_ISSET(sa, &readfds)) {
ret = read(sa, udpframe, MAXDG);
if (ret != 13) {
if (ret < 0) {
fprintf(stderr, "UDP read error: %s\n", strerror(errno));
break;
} else {
fprintf(stderr, "UDP packet size error: got %d bytes\n", ret);
}
} else {
/* prepare CAN frame */
memcpy(&canid, &udpframe[0], 4);
frame.can_id = ntohl(canid);
frame.can_dlc = udpframe[4];
memcpy(&frame.data, &udpframe[5], 8);
/* send CAN frame */
if (write(sc, &frame, sizeof(frame)) != sizeof(frame)) {
fprintf(stderr, "CAN write error: %s\n", strerror(errno));
break;
}
print_can_frame(&frame, UDP_SRC_STRG, foreground);
}
}
}
close(sc);
close(sa);
close(sb);
/* if we reach this point, there was an error */
return EXIT_FAILURE;
}
int main(int argc, char **argv) {
pid_t pid;
int max_fds, opt;
struct can_frame frame;
int sc;
struct sockaddr_can caddr;
struct ifreq ifr;
socklen_t caddrlen = sizeof(caddr);
fd_set read_fds;
int background = 0;
int verbose = 1;
strcpy(ifr.ifr_name, "vcan1");
while ((opt = getopt(argc, argv, "i:dh?")) != -1) {
switch (opt) {
case 'i':
strncpy(ifr.ifr_name, optarg, sizeof(ifr.ifr_name) - 1);
break;
case 'd':
verbose = 0;
background = 1;
break;
case 'h':
case '?':
print_usage(basename(argv[0]));
exit(EXIT_SUCCESS);
break;
default:
fprintf(stderr, "Unknown option %c\n", opt);
print_usage(basename(argv[0]));
exit(EXIT_FAILURE);
}
}
memset(&caddr, 0, sizeof(caddr));
/* prepare CAN socket */
if ((sc = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
fprintf(stderr, "error creating CAN socket: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
caddr.can_family = AF_CAN;
if (ioctl(sc, SIOCGIFINDEX, &ifr) < 0) {
fprintf(stderr, "setup CAN socket error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
caddr.can_ifindex = ifr.ifr_ifindex;
if (bind(sc, (struct sockaddr *)&caddr, caddrlen) < 0) {
fprintf(stderr, "error binding CAN socket: %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);
}
}
FD_ZERO(&read_fds);
FD_SET(sc, &read_fds);
max_fds = sc;
while (1) {
if (select(max_fds + 1, &read_fds, NULL, NULL, NULL) < 0) {
fprintf(stderr, "select error: %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
/* received a CAN frame */
if (FD_ISSET(sc, &read_fds)) {
if (read(sc, &frame, sizeof(struct can_frame)) < 0) {
fprintf(stderr, "error reading CAN frame: %s\n", strerror(errno));
} else if (frame.can_id & CAN_EFF_FLAG) { /* only EFF frames are valid */
if (verbose) {
print_can_frame(F_CAN_FORMAT_STRG, &frame);
}
switch ((frame.can_id & 0x00FF0000UL) >> 16) {
case 0x00:
if ((memcmp(&frame.data[0], &M_GLEISBOX_ID[5], 4) == 0) ||
(memcmp(&frame.data[0], M_ALL_ID, 4) == 0)) {
if (frame.data[4] == 0x0b) {
switch (frame.data[5]) {
case 0x01:
send_defined_can_frame(sc, M_GLEISBOX_VAL1, verbose);
break;
case 0x03:
send_defined_can_frame(sc, M_GLEISBOX_VAL3, verbose);
break;
case 0x04:
send_defined_can_frame(sc, M_GLEISBOX_VAL4, verbose);
break;
default:
break;
}
} else {
/* frame.can_id &= 0xFFFF0000UL; */
frame.can_id |= 0x00010000UL;
send_can_frame(sc, &frame, verbose);
}
}
break;
case 0x08: /* loco speed */
case 0x0A: /* loco direction */
case 0x0C: /* loco function */
case 0x16: /* extension */
/* frame.can_id &= 0xFFFF0000UL; */
frame.can_id |= 0x00010000UL;
send_can_frame(sc, &frame, verbose);
break;
case 0x30: /* ping / ID /software */
send_defined_can_frame(sc, M_GLEISBOX_ID, verbose);
break;
case 0x3A: /* status */
if (memcmp(&frame.data[0], &M_GLEISBOX_ID[5], 4) == 0) {
switch (frame.data[4]) {
case 0x00:
send_defined_can_frame(sc, M_GLEISBOX_STATUS1, verbose);
send_defined_can_frame(sc, M_GLEISBOX_STATUS2, verbose);
send_defined_can_frame(sc, M_GLEISBOX_STATUS3, verbose);
send_defined_can_frame(sc, M_GLEISBOX_STATUS4, verbose);
send_defined_can_frame(sc, M_GLEISBOX_STATUS5, verbose);
send_defined_can_frame(sc, M_GLEISBOX_STATUS6, verbose);
break;
case 0x01:
send_defined_can_frame(sc, M_GLEISBOX_INDEX1_1, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX1_2, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX1_3, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX1_4, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX1_5, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX1_6, verbose);
break;
case 0x02:
send_defined_can_frame(sc, M_GLEISBOX_INDEX2_1, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX2_2, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX2_3, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX2_4, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX2_5, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX2_6, verbose);
break;
case 0x03:
send_defined_can_frame(sc, M_GLEISBOX_INDEX3_1, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX3_2, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX3_3, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX3_4, verbose);
send_defined_can_frame(sc, M_GLEISBOX_INDEX3_5, verbose);
break;
default:
break;
}
}
case 0x36: /* upgrade process) */
if (frame.can_dlc == 0)
send_defined_can_frame(sc, M_GLEISBOX_BL_INIT, verbose);
if ((frame.can_dlc == 6) && (frame.data[4] == 0x44)) {
frame.can_id = 0x00379B32UL;
send_can_frame(sc, &frame, verbose);
}
if ((frame.can_dlc == 7) && (frame.data[4] == 0x88)) {
frame.can_dlc = 5;
frame.can_id = 0x00379B32UL;
/* test case : crc fault */
/* frame.data[4] = 0xf2; */
send_can_frame(sc, &frame, verbose);
}
break;
default:
break;
}
} else
print_can_frame(F_S_CAN_FORMAT_STRG, &frame);
}
}
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;
}
