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