X-Git-Url: http://rtime.felk.cvut.cz/gitweb/can-eth-gw.git/blobdiff_plain/ff3d57b5a70914d5728e49fd96d65e6e0fc1cad3..c6cc0dc64846347dbbec8dec2d23b256f0edb57a:/utils/cegw/cegw.c diff --git a/utils/cegw/cegw.c b/utils/cegw/cegw.c index c5c27b8..67953fe 100644 --- a/utils/cegw/cegw.c +++ b/utils/cegw/cegw.c @@ -1,521 +1,278 @@ +/* + * Copyright: (c) 2012 Czech Technical University in Prague + * + * Authors: + * Radek Matějka + * Michal Sojka + * + * Funded by: Volkswagen Group Research + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version + * 2 of the License, or (at your option) any later version. + */ + + #include #include -#include #include -#include -#include #include +#include #include -#include -#include -#include #include #include -#include +#include #include -#include "canethgw.h" - -/** - * ToDo: - * [ ] print usage, on -h and plain execution - * [ ] start/stop listening - * [ ] split to files - */ - -#define CEGW_CMD_ADD 1 -#define CEGW_CMD_LIST 2 -#define CEGW_CMD_FLUSH 4 -#define CEGW_CMD_LISTEN 8 - -enum -{ - IF_UNDEF, - IF_CAN, - IF_ETH_UDP -}; - -struct cegw_data -{ - int content; - int src_if, dst_if; - int can_ifidx; - struct in_addr eth_addr; - unsigned short eth_port; - struct in_addr eth_listen_addr; - unsigned short eth_listen_port; -}; - -struct cegw_nlmsg -{ - struct nlmsghdr nh; - struct rtmsg rt; - char buf[768]; /* enough? */ -}; - -struct list_item -{ - int type; - int can; - struct in_addr ip; - unsigned short port; -}; +#include +#include +#include +#include +#include unsigned int cegw_errno = 0; -enum -{ - CEGW_ERR_UNKNOWN, - CEGW_ERR_IF_UNSPEC, - CEGW_ERR_IF_SAME, - CEGW_ERR_IF_TYPE, - CEGW_ERR_IF_CAN, - CEGW_ERR_IF_ETH, - CEGW_ERR_COLON, - CEGW_ERR_ATON, - CEGW_ERR_PORT -}; - -char* cegw_errlist[] = -{ - [ CEGW_ERR_UNKNOWN ] = "", - [ CEGW_ERR_IF_UNSPEC ] = "source or destination not specified", - [ CEGW_ERR_IF_SAME ] = "source and destination have same interface type", - [ CEGW_ERR_IF_TYPE ] = "unknown interface type", - [ CEGW_ERR_IF_CAN ] = "invalid can interface", - [ CEGW_ERR_IF_ETH ] = "invalid eth interface", - [ CEGW_ERR_COLON ] = "expected ':' (:)", - [ CEGW_ERR_ATON ] = "ip address mismatch", - [ CEGW_ERR_PORT ] = "port number" +static const char help_msg[] = "usage:\n" + " %s [,filter]* : :\n" + " [list of additional udp recipients :]\n" + "example:\n" + " %s can0 192.168.0.1:10501 192.168.0.4:980 192.168.0.7:1160\n\n" + " Executing this command will set the gateway so that it will\n" + " listen for udp messages on 192.168.0.1:10501 and send them\n" + " to can0. Simultaneously, it will send all messages from can0\n" + " to 192.168.0.4:980 and 192.168.0.7:1160 via udp. The message is\n" + " therefore cloned. Notice that there can be more udp recipients.\n" + " The can filter is specified in the same way as in candump utility.\n"; + +struct addrinfo hints = { + .ai_socktype = SOCK_DGRAM }; -static void perr( char* s ) -{ - if( s ) - { - if( cegw_errno == 0 ) - { - fprintf( stderr, "error: %s\n", s ); - - } else - { - fprintf( stderr, "error: %s, %s\n", s, - cegw_errlist[ cegw_errno ] ); - } - return; - } - - fprintf( stderr, "error: %s\n", cegw_errlist[ cegw_errno ] ); -} - /** - * read_addrport - parses @in for eth address. - * Valid input is e.g. udp@127.0.0.1:10502 or can@vcan0. + * readsockaddr - parses @in for eth address. + * Valid input is e.g. 127.0.0.1:10502. If parsing fails + * the cause is stored in cegw_errno. Please note that + * the function modifies content of arg. * - * @param[in] in string to search in - * @param[out] addr ip address - * @param[out] port transport layer port + * @param[in] arg hostname:port string + * @param[out] addr filled sockaddr_in structure * @return 0 on success, -1 otherwise */ -int read_addrport( char* in, struct in_addr* addr, unsigned short* port ) +int readsockaddr(char *arg, struct sockaddr *addr, int ai_family) { - char* delim = NULL; - char addrstr[16]; - int addrlen; - - if( (delim = strchr( in, ':' )) == NULL ) - { - cegw_errno = CEGW_ERR_COLON; - return -1; - } + int ret; + char *delim; + struct addrinfo *res; - /* get address */ - addrlen = delim - in; - memcpy( addrstr, in, addrlen ); - addrstr[addrlen] = '\0'; - if( inet_aton( addrstr, addr ) == 0 ) - { - cegw_errno = CEGW_ERR_ATON; - return -1; - } + delim = strchr(arg, ':'); - /* get port */ - if( sscanf( delim, ":%hu", port ) != 1 ) /* ToDo: handle overflow */ - { - cegw_errno = CEGW_ERR_PORT; - return -1; + if (delim == NULL) { + fprintf(stderr, "expected ':' (:)"); + exit(1); } - return 0; -} + *delim = '\0'; + delim++; -/** - * read_iftype - reads @in for iftype - * Iftype type is e.g. "can@" or "udp@". - * - * @param[in] in string to search in - * @param[out] iftype iftype detected - * @return pointer to @in after iftype on success, NULL otherwise - */ -char* read_iftype( char* in, int* iftype ) -{ - char* ret = in+4; - - if( strncmp( "udp@", optarg, 4 ) == 0 ) - { - *iftype = IF_ETH_UDP; - return ret; - } - /* - if( strncmp( "tcp@", optarg, 4 ) == 0 ) - { - return NULL; - } - */ - if( strncmp( "can@", optarg, 4 ) == 0 ) - { - *iftype = IF_CAN; - return ret; - } - - cegw_errno = CEGW_ERR_IF_TYPE; - return NULL; -} - -/* ToDo: move to common */ - -/** - * read_if - reads interface from @in - * Function analyzes @in for interface specification in format - * @:, where is can or udp, is address in dotted - * format - * - * @param[in] in string to search in - * @param[out] iftype interface type (IF_CAN or IF_ETH_UDP) - * @param[out] d ip and port is stored to @d - */ -int read_if( char* in, int* iftype, struct cegw_data* d ) -{ - char* optstr = NULL; + hints.ai_family = ai_family; - if( (optstr = read_iftype( in, iftype )) == NULL ) - { - return -1; + ret = getaddrinfo(arg, delim, &hints, &res); + if (ret != 0) { + fprintf(stderr, "getaddrinfo failed: %s\n", gai_strerror(ret)); + exit(1); } - switch( *iftype ) - { - case IF_CAN: - d->can_ifidx = if_nametoindex( optstr ); - if( d->can_ifidx == 0 ) - { - cegw_errno = CEGW_ERR_IF_CAN; - return -1; - } - break; - case IF_ETH_UDP: - if( read_addrport( optstr, &d->eth_addr, &d->eth_port ) != 0 ) - { - return -1; - } - break; - default: - return -1; - break; - } + memcpy(addr, res->ai_addr, res->ai_addrlen); + freeaddrinfo(res); return 0; } -inline static int cegw_add( struct cegw_nlmsg* req, struct cegw_data* d ) +/** + * readfilter - reads can filter definition from nptr + */ +int readfilter(char *nptr, struct can_filter **filter, int *out_numfilter, can_err_mask_t *err_mask) { - int gwtype = 0; - - req->nh.nlmsg_type = RTM_NEWROUTE; - if( (d->src_if == 0 || d->dst_if == 0) ) - { - cegw_errno = CEGW_ERR_IF_UNSPEC; - return -cegw_errno; + char *ptr; + int numfilter; + struct can_filter *rfilter; + + numfilter = 0; + ptr = nptr; + while (ptr) { + numfilter++; + ptr++; /* hop behind the ',' */ + ptr = strchr(ptr, ','); /* exit condition */ } - if( d->src_if == d->dst_if ) - { - cegw_errno = CEGW_ERR_IF_SAME; - return -cegw_errno; + rfilter = malloc(numfilter * sizeof(*rfilter)); + if (!rfilter) { + fprintf(stderr, "filter malloc failed"); + exit(1); } - gwtype = (d->src_if == IF_CAN) ? CEGW_RULE_CAN_ETH : CEGW_RULE_ETH_CAN; - addattr_l( &req->nh, sizeof(*req), CEGW_CAN_IFINDEX, &d->can_ifidx, sizeof(d->can_ifidx) ); - addattr_l( &req->nh, sizeof(*req), CEGW_ETH_IP, &d->eth_addr, sizeof(d->eth_addr) ); - addattr_l( &req->nh, sizeof(*req), CEGW_ETH_PORT, &d->eth_port, sizeof(d->eth_port) ); - addattr32( &req->nh, sizeof(*req), CEGW_CMD_INFO, gwtype ); + numfilter = 0; + *err_mask = 0; + + while (nptr) { + + ptr = nptr+1; /* hop behind the ',' */ + nptr = strchr(ptr, ','); /* update exit condition */ + + if (sscanf(ptr, "%x:%x", + &rfilter[numfilter].can_id, + &rfilter[numfilter].can_mask) == 2) { + rfilter[numfilter].can_mask &= ~CAN_ERR_FLAG; + numfilter++; + } else if (sscanf(ptr, "%x~%x", + &rfilter[numfilter].can_id, + &rfilter[numfilter].can_mask) == 2) { + rfilter[numfilter].can_id |= CAN_INV_FILTER; + rfilter[numfilter].can_mask &= ~CAN_ERR_FLAG; + numfilter++; + } else if (sscanf(ptr, "#%x", err_mask) != 1) { + fprintf(stderr, "filter parsing failed"); + exit(1); + } + } + *filter = rfilter; + *out_numfilter = numfilter; return 0; } -inline static int cegw_listen( struct cegw_nlmsg* req, struct cegw_data* d ) +int main(int argc, char *argv[]) { - req->nh.nlmsg_type = RTM_NEWROUTE; - addattr32( &req->nh, sizeof(*req), CEGW_CMD_INFO, CEGW_LISTEN ); - addattr_l( &req->nh, sizeof(*req), CEGW_ETH_IP, &d->eth_listen_addr, sizeof(d->eth_listen_addr) ); - addattr_l( &req->nh, sizeof(*req), CEGW_ETH_PORT, &d->eth_listen_port, sizeof(d->eth_listen_port) ); + int i; + int fd; + int tmpi; + int dstcnt; + char *nptr; + int numfilter = 0; + int udp_sock, can_sock; + int addrlen; + can_err_mask_t err_mask = 0; + struct sockaddr_can can_addr; + struct sockaddr *dst = NULL; + struct cegw_ioctl *gwctl = NULL; + struct can_filter *filter = NULL; - return 0; -} + /* udp_addr can store both - in and in6 addresses */ + struct sockaddr_in6 udp6_addr; + struct sockaddr *udp_addr = (struct sockaddr *) &udp6_addr; -inline static int cegw_list( struct cegw_nlmsg* req, struct cegw_data* d ) -{ - req->nh.nlmsg_type = RTM_GETROUTE; - req->nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_DUMP; - - return 0; -} -inline static int cegw_flush( struct cegw_nlmsg* req, struct cegw_data* d ) -{ - addattr32( &req->nh, sizeof(*req), CEGW_CMD_INFO, CEGW_FLUSH ); - req->nh.nlmsg_type = RTM_DELROUTE; - return 0; -} - -void print_list_item( struct list_item* li ) -{ - char ifname[IF_NAMESIZE]; - const char src_width = 21; - char* dotaddr; - int tmp; - - if( if_indextoname( li->can, ifname ) == NULL ) - { - strncpy( ifname, "unknown", IF_NAMESIZE ); + if (argc == 1 || (argc == 2 && strcmp(argv[1], "-h") == 0)) { + printf(help_msg, argv[0], argv[0]); + return 0; } - /* ToDo listening at */ - switch( li->type ) - { - case CEGW_RULE_CAN_ETH: - printf( "can@%-*s -> udp@%s:%hu\n", src_width, ifname, \ - inet_ntoa(li->ip), li->port ); - break; - case CEGW_RULE_ETH_CAN: - dotaddr = inet_ntoa(li->ip); - tmp = src_width - strlen(dotaddr) - 1; - printf( "udp@%s:%-*hu -> can@%s\n", inet_ntoa(li->ip), \ - tmp, li->port, ifname ); - break; + if (argc < 4) { + fprintf(stderr, "not enough arguments\n"); + printf(help_msg, argv[0], argv[0]); + return 1; } -} -int main( int argc, char* argv[] ) -{ - int s; - int tmp = 0; - int cmd = 0; - char* optstr; - char opt; - struct sockaddr_nl nladdr; - int err = 0; - struct cegw_nlmsg req; - struct cegw_data d; - char rxbuf[8192]; /* ToDo: /linux/netlink.h? */ - int rsize = 0; - struct nlmsghdr* nlh; - struct nlmsgerr* rte; - struct rtattr* rta; - int len; - struct list_item li; - - memset( &d, 0, sizeof(d) ); - - struct option long_opt[] = - { - { "add" , 0, NULL, 'A' }, - { "flush" , 0, NULL, 'F' }, - { "list" , 0, NULL, 'L' }, - { "listen", 1, NULL, 'l' }, - { 0, 0, 0, 0 } - }; - - while( 1 ) - { - opt = getopt_long( argc, argv, "AFLl:s:d:", long_opt, NULL ); - if( opt == -1 ) - break; - - switch( opt ) - { - case 'A': - cmd |= CEGW_CMD_ADD; - break; - case 'F': - cmd |= CEGW_CMD_FLUSH; - break; - case 'L': - cmd |= CEGW_CMD_LIST; - break; - case 'l': - cmd |= CEGW_CMD_LISTEN; - if( (optstr = read_iftype( optarg, &tmp )) == NULL ) - { - perr( "'--listen'" ); - return -1; - } - if( tmp != IF_ETH_UDP ) - { - perr( "'--listen' expects udp interface" ); - return -1; - } - if( read_addrport( optstr, &d.eth_listen_addr, &d.eth_listen_port ) ) - { - perr( "'--listen'" ); - return -1; + dstcnt = argc-3; + + for (i=1; isa_family) { + case AF_INET: + addrlen = sizeof(struct sockaddr_in); + break; + case AF_INET6: + addrlen = sizeof(struct sockaddr_in6); + break; + default: + fprintf(stderr, "unexpected sockaddr family"); + break; } + gwctl = (struct cegw_ioctl*)malloc(sizeof(*gwctl) + dstcnt*addrlen); break; - case '?': - return -1; - break; - default: - perr( "unknown option" ); - return -1; + default: /* udp destination */ + dst = (struct sockaddr *)(gwctl->udp_dst + (i-3)*addrlen); + readsockaddr(argv[i], dst, udp_addr->sa_family); + break; - } + } } - /* prepare netlink message */ - req.nh.nlmsg_len = NLMSG_LENGTH( sizeof(struct rtmsg) ); - //req.nh.nlmsg_type; - req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK; - req.nh.nlmsg_seq = 0; - req.nh.nlmsg_pid = 0; /* ? */ - - memset( &req.rt, 0, sizeof(req.rt) ); - req.rt.rtm_family = AF_CAN; - - switch( cmd ) - { - case 0: - perr( "command not specified" ); - return -1; - break; - case CEGW_CMD_ADD: - if( cegw_add( &req, &d ) != 0 ) - { - perr( "'--add'" ); - return -1; - } - break; - case CEGW_CMD_FLUSH: - cegw_flush( &req, &d ); - break; - case CEGW_CMD_LIST: - cegw_list( &req, &d ); - break; - case CEGW_CMD_LISTEN: - cegw_listen( &req, &d ); - break; - default: - perr( "command mismatch" ); - break; + /* prepare udp socket */ + udp_sock = socket(udp_addr->sa_family, SOCK_DGRAM, IPPROTO_UDP); + if (udp_sock == -1) { + perror("udp socket(..)"); + return 1; } - /* send over netlink socket */ - s = socket( PF_NETLINK, SOCK_RAW, NETLINK_ROUTE ); /* chck */ - - memset( &nladdr, 0, sizeof(nladdr) ); - nladdr.nl_family = AF_NETLINK; - nladdr.nl_pad = 0; - nladdr.nl_pid = 0; - nladdr.nl_groups = 0; - - err = sendto( s, &req, req.nh.nlmsg_len, 0, - (struct sockaddr*)&nladdr, sizeof(nladdr) ); - if( err < 0 ) - { - perror( "netlink sendto" ); - return -1; + if (bind(udp_sock, udp_addr, addrlen) != 0) { + perror("bind(udp)"); + return 1; } - - /* recv */ - rsize = recv( s, &rxbuf, sizeof(rxbuf), 0 ); - if( rsize < 0 ) - { - perr( "recv" ); - return -1; + + /* prepare can socket */ + can_sock = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (can_sock == -1) { + perror("can socket(..)"); + return 1; } - nlh = (struct nlmsghdr*)rxbuf; - - if( nlh->nlmsg_type == NLMSG_ERROR ) - { - rte = (struct nlmsgerr*)NLMSG_DATA( nlh ); - err = rte->error; - - if( err == 0 ) - { - printf( "%s\n", strerror(abs(err)) ); - return 0; - } else - { - printf( "netlink error: %s\n", strerror(abs(err)) ); - return -1; - } + + if (bind(can_sock, (struct sockaddr *)&can_addr, sizeof(struct sockaddr_can)) != 0) { + perror("bind(can)"); + return 1; } - if( cmd & CEGW_CMD_LIST ) - { - /* ToDo recv while */ - printf( "%10ssource%20sdestination\n", "", "" ); - while( 1 ) - { - if( !NLMSG_OK( nlh, rsize ) ) - { - break; - } - if( nlh->nlmsg_type == NLMSG_DONE ) - { - break; - } - /* ToDo: NLMSG_ERR */ - rta = NLMSG_DATA( nlh ); - len = NLMSG_PAYLOAD( nlh, 0 ); - for( ;RTA_OK(rta, len); rta = RTA_NEXT(rta,len) ) - { - switch( rta->rta_type ) - { - case CEGW_TYPE: - li.type = *(int*)RTA_DATA(rta); - break; - case CEGW_CAN_IFINDEX: - li.can = *(int*)RTA_DATA(rta); - break; - case CEGW_ETH_IP: - li.ip = *(struct in_addr*)RTA_DATA(rta); - break; - case CEGW_ETH_PORT: - li.port = *(unsigned short*)RTA_DATA(rta); - break; - /* case CGW_ETH_PROTO */ - } - } + /* can filter */ + if (nptr) + readfilter(nptr, &filter, &numfilter, &err_mask); - print_list_item( &li ); + if (err_mask) + setsockopt(can_sock, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, + &err_mask, sizeof(err_mask)); - nlh = NLMSG_NEXT( nlh, rsize ); - } + if (numfilter) + setsockopt(can_sock, SOL_CAN_RAW, CAN_RAW_FILTER, + filter, numfilter * sizeof(struct can_filter)); + free(filter); + + /* send it to kernel gateway */ + fd = open("/dev/canethgw", O_RDONLY); + if (fd == -1) { + perror("/dev/canethgw"); + return 1; + } + + gwctl->can_sock = can_sock; + gwctl->udp_sock = udp_sock; + gwctl->udp_dstcnt = dstcnt; + gwctl->udp_addrlen = addrlen; + + if (ioctl(fd, CEGW_IOCTL_START, gwctl) != 0) { + perror("ioctl"); + return 1; } + printf("gateway successfully set and running\n"); + free(gwctl); + + /* sleep until someone kills me */ + pause(); return 0; } -