From 199685ef6ef788cbcc81bfd102063ed2bdf37b9f Mon Sep 17 00:00:00 2001 From: Radek Matejka Date: Wed, 11 Jul 2012 17:36:49 +0200 Subject: [PATCH] new features added filter, signal SIGINT exit, getopt --- user/Makefile | 4 +- user/canethgw.c | 132 ++++++++++++++++++++++++++++++++++++++---------- 2 files changed, 107 insertions(+), 29 deletions(-) diff --git a/user/Makefile b/user/Makefile index bbab6d0..aea5dbc 100644 --- a/user/Makefile +++ b/user/Makefile @@ -1,2 +1,4 @@ +PROGRAM=canethgw + all: - gcc -Wall -o canethgw canethgw.c + gcc -Wall -o ${PROGRAM} canethgw.c diff --git a/user/canethgw.c b/user/canethgw.c index 8048a1f..5331a55 100644 --- a/user/canethgw.c +++ b/user/canethgw.c @@ -1,11 +1,14 @@ #include +#include #include #include #include #include +#include #include #include #include +#include #include #include #include @@ -13,14 +16,41 @@ #define GW_POLL_ETH 0 #define GW_POLL_CAN 1 +/**/ +#define GW_IF_BIT 1 +#define GW_SRC_PORT_BIT 2 +#define GW_DEST_PORT_BIT 4 +#define GW_FILTER_BIT 8 +unsigned int opt_flag = 0; int sock_udp; -int sock_can; +int sock_can; +struct can_filter flt; +struct sockaddr_in udp_dest; + +/** + * + * @return true if frame should be dropped + */ +bool filter_check( struct can_frame* cf ) +{ + if( !(opt_flag & GW_FILTER_BIT) ) + return false; + + return ( (cf->can_id & flt.can_mask) == (flt.can_id & flt.can_mask) ); +} void send_to_can() { struct can_frame cf; read( sock_udp, &cf, sizeof(cf) ); + + if( !filter_check( &cf ) ) + { + printf( "filter: drop\n" ); + return; + } + write( sock_can, &cf, sizeof(cf) ); printf( "sending to can\n" ); @@ -29,32 +59,90 @@ void send_to_can() void send_to_eth() { struct can_frame cf; - struct sockaddr_in udp_dest; - - udp_dest.sin_family = AF_INET; - udp_dest.sin_addr.s_addr = inet_addr("127.0.0.1"); - udp_dest.sin_port = htons(10502); read( sock_can, &cf, sizeof(cf) ); + + if( !filter_check( &cf ) ) + { + printf( "filter: drop\n" ); + return; + } + sendto( sock_udp, &cf, sizeof(cf), 0, (struct sockaddr*) &udp_dest, sizeof(udp_dest) ); - //write( sock_eth, &cf, sizeof(cf) ); printf( "sending to eth\n" ); } +void signal_exit( int sig ) +{ + close( sock_udp ); + close( sock_can ); + printf( "exiting\n" ); + + exit( 0 ); +} + int main( int argc, char* argv[] ) { struct sockaddr_in udp_addr; struct sockaddr_can can_addr; struct ifreq ifr; struct pollfd pfd[2]; + int polret; + int opt; + int src_port = 0, dest_port = 0; + + flt.can_id = 0; + flt.can_mask = 0; - if( argc != 2 ) + while( (opt = getopt( argc, argv, "s:d:f:i:" )) != -1 ) { - printf( "usage example: %s can1\n", argv[0] ); + switch( opt ) + { + case 's': + src_port = atoi( optarg ); + opt_flag |= GW_SRC_PORT_BIT; + break; + case 'd': + dest_port = atoi( optarg ); + opt_flag |= GW_DEST_PORT_BIT; + break; + case 'f': + if( sscanf( optarg, "%x:%x", &flt.can_id, + &flt.can_mask ) != 2 ) + { + fprintf( stderr, "error:\ + bad filter format\n"); + goto gw_error; + } + opt_flag |= GW_FILTER_BIT; + break; + case 'i': + ifr.ifr_ifindex = if_nametoindex( optarg ); + if( ifr.ifr_ifindex == 0 ) + { + fprintf( stderr, "error: if_nametoindex\n" ); + goto gw_error; + } + opt_flag |= GW_IF_BIT; + break; + } + } + + if( !((opt_flag & GW_SRC_PORT_BIT) && (opt_flag & GW_DEST_PORT_BIT) + && (opt_flag & GW_DEST_PORT_BIT)) ) + { + fprintf( stdout, "bad usage, example -i vcan0 -s 10501 -d 10502 -f 007:00000007\n" ); return -1; } + signal( SIGINT, signal_exit ); + + /* prepare udp destination */ + udp_dest.sin_family = AF_INET; + udp_dest.sin_addr.s_addr = inet_addr("127.0.0.1"); + udp_dest.sin_port = htons(dest_port); + /* udp socket */ sock_udp = socket( PF_INET, SOCK_DGRAM, IPPROTO_UDP ); if( sock_udp < 0 ) @@ -64,7 +152,7 @@ int main( int argc, char* argv[] ) } udp_addr.sin_family = AF_INET; - udp_addr.sin_port = htons(10501); + udp_addr.sin_port = htons(src_port); udp_addr.sin_addr.s_addr = INADDR_ANY; if( bind( sock_udp, (struct sockaddr*) &udp_addr, sizeof(udp_addr) ) < 0 ) @@ -74,7 +162,7 @@ int main( int argc, char* argv[] ) goto gw_error; } - /* can socket*/ + /* can socket */ sock_can = socket( PF_CAN, SOCK_RAW, CAN_RAW ); if( sock_can < 0 ) { @@ -83,16 +171,8 @@ int main( int argc, char* argv[] ) goto gw_error; } - strcpy( ifr.ifr_name, argv[1] ); - if( ioctl( sock_can, SIOCGIFINDEX, &ifr ) < 0 ) - { - fprintf( stderr, "error: ioctl(), get if index\n" ); - goto gw_error; - } - can_addr.can_family = AF_CAN; can_addr.can_ifindex = ifr.ifr_ifindex; -// setsockopt( sock_can, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0 ); /*?!?*/ if( bind( sock_can, (struct sockaddr*) &can_addr, sizeof(can_addr) ) < 0 ) { @@ -109,21 +189,14 @@ int main( int argc, char* argv[] ) pfd[GW_POLL_CAN].events = POLLIN | POLLPRI; pfd[GW_POLL_CAN].revents = 0; -/* - unsigned char buf[1]; - struct can_frame cf; - printf( "block\n" ); - read( sock_can, &cf, sizeof(cf) ); again, return 0 - printf( "can read\n" ); -*/ - while(1) { printf( "polling\n" ); - int polret = poll( pfd, 2, -1 ); + polret = poll( pfd, 2, -1 ); if( polret <= 0 ) { fprintf( stderr, "error: poll returned 0\n" ); + goto gw_clean; } if( pfd[GW_POLL_ETH].revents != 0 ) @@ -140,6 +213,9 @@ int main( int argc, char* argv[] ) } return 0; +gw_clean: + close( sock_udp ); + close( sock_can ); gw_error: return -1; } -- 2.39.2