user/canethgw.c

   1 #include <stdio.h>
   2 #include <stdbool.h>
   3 #include <stdlib.h>
   4 #include <string.h>
   5 #include <unistd.h>
   6 #include <netinet/in.h>
   7 #include <signal.h>
   8 #include <sys/ioctl.h>
   9 #include <sys/types.h>
  10 #include <sys/socket.h>
  11 #include <arpa/inet.h>
  12 #include <net/if.h>
  13 #include <linux/can.h>
  14 #include <linux/can/raw.h>
  15 #include <poll.h>
  16
  17 #define GW_POLL_ETH 0
  18 #define GW_POLL_CAN 1
  19 /**/
  20 #define GW_IF_BIT 1
  21 #define GW_SRC_PORT_BIT 2
  22 #define GW_DEST_PORT_BIT 4
  23 #define GW_FILTER_BIT 8
  24
  25 unsigned int opt_flag = 0;
  26 int sock_udp;
  27 int sock_can;
  28 struct can_filter flt;
  29 struct sockaddr_in udp_dest;
  30
  31 /**
  32  *
  33  * @return true if frame should be dropped
  34  */
  35 bool filter_check( struct can_frame* cf )
  36 {
  37         if( !(opt_flag & GW_FILTER_BIT) )
  38                 return false;
  39
  40         return ( (cf->can_id & flt.can_mask) == (flt.can_id & flt.can_mask) );
  41 }
  42
  43 void send_to_can()
  44 {
  45         struct can_frame cf;
  46         read( sock_udp, &cf, sizeof(cf) );
  47
  48         if( !filter_check( &cf ) )
  49         {
  50                 printf( "filter: drop\n" );
  51                 return;
  52         }
  53
  54         write( sock_can, &cf, sizeof(cf) );
  55
  56         printf( "sending to can\n" );
  57 }
  58
  59 void send_to_eth()
  60 {
  61         struct can_frame cf;
  62
  63         read( sock_can, &cf, sizeof(cf) );
  64
  65         if( !filter_check( &cf ) )
  66         {
  67                 printf( "filter: drop\n" );
  68                 return;
  69         }
  70
  71         sendto( sock_udp, &cf, sizeof(cf), 0, (struct sockaddr*) &udp_dest, sizeof(udp_dest) );
  72
  73         printf( "sending to eth\n" );
  74 }
  75
  76 void signal_exit( int sig )
  77 {
  78         close( sock_udp );
  79         close( sock_can );
  80         printf( "exiting\n" );
  81
  82         exit( 0 );
  83 }
  84
  85 int main( int argc, char* argv[] )
  86 {
  87         struct sockaddr_in udp_addr;
  88         struct sockaddr_can can_addr;
  89         struct ifreq ifr;
  90         struct pollfd pfd[2];
  91         int polret;
  92         int opt;
  93         int src_port = 0, dest_port = 0;
  94
  95         flt.can_id = 0;
  96         flt.can_mask = 0;
  97
  98         while( (opt = getopt( argc, argv, "s:d:f:i:" )) != -1 )
  99         {
 100                 switch( opt )
 101                 {
 102                         case 's':
 103                                 src_port = atoi( optarg );
 104                                 opt_flag |= GW_SRC_PORT_BIT;
 105                                 break;
 106                         case 'd':
 107                                 dest_port = atoi( optarg );
 108                                 opt_flag |= GW_DEST_PORT_BIT;
 109                                 break;
 110                         case 'f':
 111                                 if( sscanf( optarg, "%x:%x", &flt.can_id,
 112                                    &flt.can_mask ) != 2 )
 113                                 {
 114                                         fprintf( stderr, "error:\
 115                                                 bad filter format\n");
 116                                         goto gw_error;
 117                                 }
 118                                 opt_flag |= GW_FILTER_BIT;
 119                                 break;
 120                         case 'i':
 121                                 ifr.ifr_ifindex = if_nametoindex( optarg );
 122                                 if( ifr.ifr_ifindex == 0 )
 123                                 {
 124                                         fprintf( stderr, "error: if_nametoindex\n" );
 125                                         goto gw_error;
 126                                 }
 127                                 opt_flag |= GW_IF_BIT;
 128                                 break;
 129                 }
 130         }
 131
 132         if( !((opt_flag & GW_SRC_PORT_BIT) && (opt_flag & GW_DEST_PORT_BIT)
 133                 && (opt_flag & GW_DEST_PORT_BIT)) )
 134         {
 135                 fprintf( stdout, "bad usage, example -i vcan0 -s 10501 -d 10502 -f 007:00000007\n" );
 136                 return -1;
 137         }
 138
 139         signal( SIGINT, signal_exit );
 140
 141         /* prepare udp destination */
 142         udp_dest.sin_family = AF_INET;
 143         udp_dest.sin_addr.s_addr = inet_addr("127.0.0.1");
 144         udp_dest.sin_port = htons(dest_port);
 145
 146         /* udp socket */
 147         sock_udp = socket( PF_INET, SOCK_DGRAM, IPPROTO_UDP );
 148         if( sock_udp < 0 )
 149         {
 150                 fprintf( stderr, "error: socket(), udp\n" );
 151                 goto gw_error;
 152         }
 153
 154         udp_addr.sin_family = AF_INET;
 155         udp_addr.sin_port = htons(src_port);
 156         udp_addr.sin_addr.s_addr = INADDR_ANY;
 157
 158         if( bind( sock_udp, (struct sockaddr*) &udp_addr, sizeof(udp_addr) ) < 0 )
 159         {
 160                 fprintf( stderr, "error: bind(), udp\n" );
 161                 close( sock_udp );
 162                 goto gw_error;
 163         }
 164
 165         /* can socket */
 166         sock_can = socket( PF_CAN, SOCK_RAW, CAN_RAW );
 167         if( sock_can < 0 )
 168         {
 169                 fprintf( stderr, "error: socket(), can\n");
 170                 close( sock_can );
 171                 goto gw_error;
 172         }
 173
 174         can_addr.can_family = AF_CAN;
 175         can_addr.can_ifindex = ifr.ifr_ifindex;
 176
 177         if( bind( sock_can, (struct sockaddr*) &can_addr, sizeof(can_addr) ) < 0 )
 178         {
 179                 fprintf( stderr, "error: bind(), can" );
 180                 goto gw_error;
 181         }
 182
 183         /* poll */
 184         pfd[GW_POLL_ETH].fd = sock_udp;
 185         pfd[GW_POLL_ETH].events = POLLIN | POLLPRI;
 186         pfd[GW_POLL_ETH].revents = 0;
 187
 188         pfd[GW_POLL_CAN].fd = sock_can;
 189         pfd[GW_POLL_CAN].events = POLLIN | POLLPRI;
 190         pfd[GW_POLL_CAN].revents = 0;
 191
 192         while(1)
 193         {
 194                 printf( "polling\n" );
 195                 polret = poll( pfd, 2, -1 );
 196                 if( polret <= 0 )
 197                 {
 198                         fprintf( stderr, "error: poll returned 0\n" );
 199                         goto gw_clean;
 200                 }
 201
 202                 if( pfd[GW_POLL_ETH].revents != 0 )
 203                 {
 204                         send_to_can();
 205                 }
 206                 if( pfd[GW_POLL_CAN].revents != 0 )
 207                 {
 208                         send_to_eth();
 209                 }
 210
 211                 pfd[GW_POLL_ETH].revents = 0;
 212                 pfd[GW_POLL_CAN].revents = 0;
 213         }
 214
 215         return 0;
 216 gw_clean:
 217         close( sock_udp );
 218         close( sock_can );
 219 gw_error:
 220         return -1;
 221 }
 222