+#include <stdio.h>
+#include <stdlib.h>
+#include <libgen.h>
+#include <string.h>
+#include <getopt.h>
+#include <errno.h>
+#include <sys/socket.h>
+#include <net/if.h>
+//#include <libnetlink.h>
+#include <linux/netlink.h>
+#include <linux/rtnetlink.h>
+#include <arpa/inet.h>
+#include <linux/can.h>
+#include <linux/types.h>
+#include <unistd.h>
+#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
+
+/* PowerPC encore */
+#define RTCAN_RTA(r) ((struct rtattr*)(((char*)(r)) + NLMSG_ALIGN(sizeof(struct rtcanmsg))))
+#define RTCAN_PAYLOAD(n) NLMSG_PAYLOAD(n,sizeof(struct rtcanmsg))
+
+/* some netlink helpers stolen from iproute2 package */
+#define NLMSG_TAIL(nmsg) \
+ ((struct rtattr *)(((void *) (nmsg)) + NLMSG_ALIGN((nmsg)->nlmsg_len)))
+#define AF_CAN 29
+
+int addattr_l(struct nlmsghdr *n, int maxlen, int type, const void *data,
+ int alen)
+{
+ int len = RTA_LENGTH(alen);
+ struct rtattr *rta;
+
+ if (NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len) > maxlen) {
+ fprintf(stderr, "addattr_l: message exceeded bound of %d\n",
+ maxlen);
+ return -1;
+ }
+ rta = NLMSG_TAIL(n);
+ rta->rta_type = type;
+ rta->rta_len = len;
+ memcpy(RTA_DATA(rta), data, alen);
+ n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len);
+ return 0;
+}
+
+int addattr32(struct nlmsghdr *n, int maxlen, int type, __u32 data)
+{
+ return addattr_l(n, maxlen, type, &data, sizeof(__u32));
+}
+
+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;
+};
+
+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 ':' (<ip>:<port>)",
+ [ CEGW_ERR_ATON ] = "ip address mismatch",
+ [ CEGW_ERR_PORT ] = "port number"
+};
+
+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.
+ *
+ * @param[in] in string to search in
+ * @param[out] addr ip address
+ * @param[out] port transport layer port
+ * @return 0 on success, -1 otherwise
+ */
+int read_addrport( char* in, struct in_addr* addr, unsigned short* port )
+{
+ char* delim = NULL;
+ char addrstr[16];
+ int addrlen;
+
+ if( (delim = strchr( in, ':' )) == NULL )
+ {
+ cegw_errno = CEGW_ERR_COLON;
+ return -1;
+ }
+
+ /* get address */
+ /* ToDo: overflow */
+ addrlen = delim - in;
+ memcpy( addrstr, in, addrlen );
+ addrstr[addrlen] = '\0';
+ if( inet_aton( addrstr, addr ) == 0 )
+ {
+ cegw_errno = CEGW_ERR_ATON;
+ return -1;
+ }
+
+ /* get port */
+ if( sscanf( delim, ":%hu", port ) != 1 ) /* ToDo: handle overflow */
+ {
+ cegw_errno = CEGW_ERR_PORT;
+ return -1;
+ }
+
+ return 0;
+}
+
+/**
+ * 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
+ * <if>@<ip>:<port>, where <if> is can or udp, <ip> 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;
+
+ if( (optstr = read_iftype( in, iftype )) == NULL )
+ {
+ return -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;
+ }
+
+ return 0;
+}
+
+inline static int cegw_add( struct cegw_nlmsg* req, struct cegw_data* d )
+{
+ 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;
+ }
+
+ if( d->src_if == d->dst_if )
+ {
+ cegw_errno = CEGW_ERR_IF_SAME;
+ return -cegw_errno;
+ }
+
+ 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 );
+
+ return 0;
+}
+
+inline static int cegw_listen( struct cegw_nlmsg* req, struct cegw_data* d )
+{
+ 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) );
+
+ return 0;
+}
+
+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 );
+ }
+
+ /* 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;
+ }
+}
+
+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;
+
+ //printf( "optarg=%s\n", optarg );
+ 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;
+ }
+ break;
+ case 's':
+ if( read_if( optarg, &d.src_if, &d ) != 0 )
+ {
+ perr( "'-s'" );
+ return -1;
+ }
+ break;
+ case 'd':
+ if( read_if( optarg, &d.dst_if, &d ) != 0 )
+ {
+ perr( "'-d'" );
+ return -1;
+ }
+ break;
+ case '?':
+ return -1;
+ break;
+ default:
+ //fprintf( stderr, "unknown option: %u\n", opt );
+ goto while_break;
+ //return -1;
+ break;
+ }
+ }
+
+ while_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;
+ }
+
+ /* 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;
+ }
+
+ /* recv */
+ rsize = recv( s, &rxbuf, sizeof(rxbuf), 0 );
+ if( rsize < 0 )
+ {
+ perr( "recv" );
+ 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( 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 */
+ }
+ }
+
+ print_list_item( &li );
+
+ nlh = NLMSG_NEXT( nlh, rsize );
+ }
+ }
+
+ return 0;
+}
+