7 #include <sys/socket.h>
9 //#include <libnetlink.h>
10 #include <linux/netlink.h>
11 #include <linux/rtnetlink.h>
12 #include <arpa/inet.h>
13 #include <linux/can.h>
14 #include <linux/types.h>
20 * [ ] print usage, on -h and plain execution
21 * [ ] start/stop listening
25 #define CEGW_CMD_ADD 1
26 #define CEGW_CMD_LIST 2
27 #define CEGW_CMD_FLUSH 4
28 #define CEGW_CMD_LISTEN 8
31 #define RTCAN_RTA(r) ((struct rtattr*)(((char*)(r)) + NLMSG_ALIGN(sizeof(struct rtcanmsg))))
32 #define RTCAN_PAYLOAD(n) NLMSG_PAYLOAD(n,sizeof(struct rtcanmsg))
34 /* some netlink helpers stolen from iproute2 package */
35 #define NLMSG_TAIL(nmsg) \
36 ((struct rtattr *)(((void *) (nmsg)) + NLMSG_ALIGN((nmsg)->nlmsg_len)))
39 int addattr_l(struct nlmsghdr *n, int maxlen, int type, const void *data,
42 int len = RTA_LENGTH(alen);
45 if (NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len) > maxlen) {
46 fprintf(stderr, "addattr_l: message exceeded bound of %d\n",
53 memcpy(RTA_DATA(rta), data, alen);
54 n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len);
58 int addattr32(struct nlmsghdr *n, int maxlen, int type, __u32 data)
60 return addattr_l(n, maxlen, type, &data, sizeof(__u32));
75 struct in_addr eth_addr;
76 unsigned short eth_port;
77 struct in_addr eth_listen_addr;
78 unsigned short eth_listen_port;
85 char buf[768]; /* enough? */
96 unsigned int cegw_errno = 0;
111 char* cegw_errlist[] =
113 [ CEGW_ERR_UNKNOWN ] = "",
114 [ CEGW_ERR_IF_UNSPEC ] = "source or destination not specified",
115 [ CEGW_ERR_IF_SAME ] = "source and destination have same interface type",
116 [ CEGW_ERR_IF_TYPE ] = "unknown interface type",
117 [ CEGW_ERR_IF_CAN ] = "invalid can interface",
118 [ CEGW_ERR_IF_ETH ] = "invalid eth interface",
119 [ CEGW_ERR_COLON ] = "expected ':' (<ip>:<port>)",
120 [ CEGW_ERR_ATON ] = "ip address mismatch",
121 [ CEGW_ERR_PORT ] = "port number"
124 static void perr( char* s )
128 if( cegw_errno == 0 )
130 fprintf( stderr, "error: %s\n", s );
134 fprintf( stderr, "error: %s, %s\n", s,
135 cegw_errlist[ cegw_errno ] );
140 fprintf( stderr, "error: %s\n", cegw_errlist[ cegw_errno ] );
144 * read_addrport - parses @in for eth address.
145 * Valid input is e.g. udp@127.0.0.1:10502 or can@vcan0.
147 * @param[in] in string to search in
148 * @param[out] addr ip address
149 * @param[out] port transport layer port
150 * @return 0 on success, -1 otherwise
152 int read_addrport( char* in, struct in_addr* addr, unsigned short* port )
158 if( (delim = strchr( in, ':' )) == NULL )
160 cegw_errno = CEGW_ERR_COLON;
166 addrlen = delim - in;
167 memcpy( addrstr, in, addrlen );
168 addrstr[addrlen] = '\0';
169 if( inet_aton( addrstr, addr ) == 0 )
171 cegw_errno = CEGW_ERR_ATON;
176 if( sscanf( delim, ":%hu", port ) != 1 ) /* ToDo: handle overflow */
178 cegw_errno = CEGW_ERR_PORT;
186 * read_iftype - reads @in for iftype
187 * Iftype type is e.g. "can@" or "udp@".
189 * @param[in] in string to search in
190 * @param[out] iftype iftype detected
191 * @return pointer to @in after iftype on success, NULL otherwise
193 char* read_iftype( char* in, int* iftype )
197 if( strncmp( "udp@", optarg, 4 ) == 0 )
199 *iftype = IF_ETH_UDP;
203 if( strncmp( "tcp@", optarg, 4 ) == 0 )
208 if( strncmp( "can@", optarg, 4 ) == 0 )
214 cegw_errno = CEGW_ERR_IF_TYPE;
218 /* ToDo: move to common */
221 * read_if - reads interface from @in
222 * Function analyzes @in for interface specification in format
223 * <if>@<ip>:<port>, where <if> is can or udp, <ip> is address in dotted
226 * @param[in] in string to search in
227 * @param[out] iftype interface type (IF_CAN or IF_ETH_UDP)
228 * @param[out] d ip and port is stored to @d
230 int read_if( char* in, int* iftype, struct cegw_data* d )
234 if( (optstr = read_iftype( in, iftype )) == NULL )
242 d->can_ifidx = if_nametoindex( optstr );
243 if( d->can_ifidx == 0 )
245 cegw_errno = CEGW_ERR_IF_CAN;
250 if( read_addrport( optstr, &d->eth_addr, &d->eth_port ) != 0 )
263 inline static int cegw_add( struct cegw_nlmsg* req, struct cegw_data* d )
267 req->nh.nlmsg_type = RTM_NEWROUTE;
268 if( (d->src_if == 0 || d->dst_if == 0) )
270 cegw_errno = CEGW_ERR_IF_UNSPEC;
274 if( d->src_if == d->dst_if )
276 cegw_errno = CEGW_ERR_IF_SAME;
280 gwtype = (d->src_if == IF_CAN) ? CEGW_RULE_CAN_ETH : CEGW_RULE_ETH_CAN;
281 addattr_l( &req->nh, sizeof(*req), CEGW_CAN_IFINDEX, &d->can_ifidx, sizeof(d->can_ifidx) );
282 addattr_l( &req->nh, sizeof(*req), CEGW_ETH_IP, &d->eth_addr, sizeof(d->eth_addr) );
283 addattr_l( &req->nh, sizeof(*req), CEGW_ETH_PORT, &d->eth_port, sizeof(d->eth_port) );
284 addattr32( &req->nh, sizeof(*req), CEGW_CMD_INFO, gwtype );
289 inline static int cegw_listen( struct cegw_nlmsg* req, struct cegw_data* d )
291 req->nh.nlmsg_type = RTM_NEWROUTE;
292 addattr32( &req->nh, sizeof(*req), CEGW_CMD_INFO, CEGW_LISTEN );
293 addattr_l( &req->nh, sizeof(*req), CEGW_ETH_IP, &d->eth_listen_addr, sizeof(d->eth_listen_addr) );
294 addattr_l( &req->nh, sizeof(*req), CEGW_ETH_PORT, &d->eth_listen_port, sizeof(d->eth_listen_port) );
299 inline static int cegw_list( struct cegw_nlmsg* req, struct cegw_data* d )
301 req->nh.nlmsg_type = RTM_GETROUTE;
302 req->nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_DUMP;
307 inline static int cegw_flush( struct cegw_nlmsg* req, struct cegw_data* d )
309 addattr32( &req->nh, sizeof(*req), CEGW_CMD_INFO, CEGW_FLUSH );
310 req->nh.nlmsg_type = RTM_DELROUTE;
314 void print_list_item( struct list_item* li )
316 char ifname[IF_NAMESIZE];
317 const char src_width = 21;
321 if( if_indextoname( li->can, ifname ) == NULL )
323 strncpy( ifname, "unknown", IF_NAMESIZE );
326 /* ToDo listening at */
329 case CEGW_RULE_CAN_ETH:
330 printf( "can@%-*s -> udp@%s:%hu\n", src_width, ifname, \
331 inet_ntoa(li->ip), li->port );
333 case CEGW_RULE_ETH_CAN:
334 dotaddr = inet_ntoa(li->ip);
335 tmp = src_width - strlen(dotaddr) - 1;
336 printf( "udp@%s:%-*hu -> can@%s\n", inet_ntoa(li->ip), \
337 tmp, li->port, ifname );
342 int main( int argc, char* argv[] )
349 struct sockaddr_nl nladdr;
351 struct cegw_nlmsg req;
353 char rxbuf[8192]; /* ToDo: /linux/netlink.h? */
355 struct nlmsghdr* nlh;
356 struct nlmsgerr* rte;
361 memset( &d, 0, sizeof(d) );
363 struct option long_opt[] =
365 { "add" , 0, NULL, 'A' },
366 { "flush" , 0, NULL, 'F' },
367 { "list" , 0, NULL, 'L' },
368 { "listen", 1, NULL, 'l' },
374 opt = getopt_long( argc, argv, "AFLl:s:d:", long_opt, NULL );
378 //printf( "optarg=%s\n", optarg );
385 cmd |= CEGW_CMD_FLUSH;
388 cmd |= CEGW_CMD_LIST;
391 cmd |= CEGW_CMD_LISTEN;
392 if( (optstr = read_iftype( optarg, &tmp )) == NULL )
394 perr( "'--listen'" );
397 if( tmp != IF_ETH_UDP )
399 perr( "'--listen' expects udp interface" );
402 if( read_addrport( optstr, &d.eth_listen_addr, &d.eth_listen_port ) )
404 perr( "'--listen'" );
409 if( read_if( optarg, &d.src_if, &d ) != 0 )
416 if( read_if( optarg, &d.dst_if, &d ) != 0 )
426 //fprintf( stderr, "unknown option: %u\n", opt );
435 /* prepare netlink message */
436 req.nh.nlmsg_len = NLMSG_LENGTH( sizeof(struct rtmsg) );
438 req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
439 req.nh.nlmsg_seq = 0;
440 req.nh.nlmsg_pid = 0; /* ? */
442 memset( &req.rt, 0, sizeof(req.rt) );
443 req.rt.rtm_family = AF_CAN;
448 perr( "command not specified" );
452 if( cegw_add( &req, &d ) != 0 )
459 cegw_flush( &req, &d );
462 cegw_list( &req, &d );
464 case CEGW_CMD_LISTEN:
465 cegw_listen( &req, &d );
468 perr( "command mismatch" );
472 /* send over netlink socket */
473 s = socket( PF_NETLINK, SOCK_RAW, NETLINK_ROUTE ); /* chck */
475 memset( &nladdr, 0, sizeof(nladdr) );
476 nladdr.nl_family = AF_NETLINK;
479 nladdr.nl_groups = 0;
481 err = sendto( s, &req, req.nh.nlmsg_len, 0,
482 (struct sockaddr*)&nladdr, sizeof(nladdr) );
485 perror( "netlink sendto" );
490 rsize = recv( s, &rxbuf, sizeof(rxbuf), 0 );
496 nlh = (struct nlmsghdr*)rxbuf;
498 if( nlh->nlmsg_type == NLMSG_ERROR )
500 rte = (struct nlmsgerr*)NLMSG_DATA( nlh );
505 printf( "%s\n", strerror(abs(err)) );
509 printf( "netlink error: %s\n", strerror(abs(err)) );
514 if( cmd & CEGW_CMD_LIST )
516 /* ToDo recv while */
517 printf( "%10ssource%20sdestination\n", "", "" );
520 if( !NLMSG_OK( nlh, rsize ) )
524 if( nlh->nlmsg_type == NLMSG_DONE )
528 /* ToDo: NLMSG_ERR */
529 rta = NLMSG_DATA( nlh );
530 len = NLMSG_PAYLOAD( nlh, 0 );
531 for( ;RTA_OK(rta, len); rta = RTA_NEXT(rta,len) )
533 switch( rta->rta_type )
536 li.type = *(int*)RTA_DATA(rta);
538 case CEGW_CAN_IFINDEX:
539 li.can = *(int*)RTA_DATA(rta);
542 li.ip = *(struct in_addr*)RTA_DATA(rta);
545 li.port = *(unsigned short*)RTA_DATA(rta);
547 /* case CGW_ETH_PROTO */
551 print_list_item( &li );
553 nlh = NLMSG_NEXT( nlh, rsize );