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 <getopt.h>
   9 #include <sys/ioctl.h>
  10 #include <sys/types.h>
  11 #include <sys/socket.h>
  12 #include <arpa/inet.h>
  13 #include <net/if.h>
  14 #include <linux/can.h>
  15 #include <linux/can/raw.h>
  16 #include <poll.h>
  17 #include "readif.h"
  18 #include "cegwerr.h"
  19
  20 /**
  21  * ToDo:
  22  * [ ] print usage
  23  */
  24 //#define GW_DEBUG
  25 #ifdef GW_DEBUG
  26 #define printdbg(x) printf(x)
  27 #else
  28 #define printdbg(x)
  29 #endif
  30
  31 #define GW_POLL_ETH 0
  32 #define GW_POLL_CAN 1
  33
  34 #define GW_FLAG_SRC 1
  35 #define GW_FLAG_DST 2
  36 #define GW_FLAG_LISTEN 4
  37 #define GW_FLAG_FILTER 8
  38
  39 unsigned int opt_flag = 0;
  40 int sock_udp;
  41 int sock_can;
  42 struct can_filter flt;
  43 struct sockaddr_in udp_dest;
  44
  45 /**
  46  * filter_check
  47  * @return true if frame should be dropped
  48  */
  49 bool filter_check( struct can_frame* cf )
  50 {
  51         if( !(opt_flag & GW_FLAG_FILTER) )
  52                 return false;
  53
  54         return ( (cf->can_id & flt.can_mask) == (flt.can_id & flt.can_mask) );
  55 }
  56
  57 void send_to_can()
  58 {
  59         struct can_frame cf;
  60
  61         read( sock_udp, &cf, sizeof(cf) );
  62
  63         if( filter_check( &cf ) )
  64         {
  65                 printdbg( "filter: drop\n" );
  66                 return;
  67         }
  68
  69         write( sock_can, &cf, sizeof(cf) );
  70
  71         printdbg( "eth -> can\n" );
  72 }
  73
  74 void send_to_eth()
  75 {
  76         struct can_frame cf;
  77
  78         read( sock_can, &cf, sizeof(cf) );
  79
  80         if( filter_check( &cf ) )
  81         {
  82                 printdbg( "filter: drop\n" );
  83                 return;
  84         }
  85
  86         sendto( sock_udp, &cf, sizeof(cf), 0, (struct sockaddr*) &udp_dest, sizeof(udp_dest) );
  87
  88         printdbg( "can -> eth\n" );
  89 }
  90
  91 void signal_exit( int sig )
  92 {
  93         close( sock_udp );
  94         close( sock_can );
  95         printf( "exiting\n" );
  96
  97         exit( 0 );
  98 }
  99
 100 int main( int argc, char* argv[] )
 101 {
 102         struct sockaddr_in udp_addr;
 103         struct sockaddr_can can_addr;
 104         struct pollfd pfd[2];
 105         int polret;
 106         int opt;
 107         struct cegw_if ceif[2], iflisten;
 108         struct cegw_if* ifcan,* ifeth;
 109
 110         flt.can_id = 0;
 111         flt.can_mask = 0;
 112
 113         struct option longopt[] =
 114         {
 115                 {"listen", 1, NULL, 'l'},
 116                 { 0, 0, 0, 0 }
 117         };
 118
 119         while( (opt = getopt_long( argc, argv, "s:d:l:f:i:", longopt, NULL )) != -1 )
 120         {
 121                 switch( opt )
 122                 {
 123                         case 's':
 124                                 if( read_if(optarg, &ceif[0]) != 0 )
 125                                 {
 126                                         perr( "'-s'" );
 127                                         return -1;
 128                                 }
 129                                 opt_flag |= GW_FLAG_SRC;
 130                                 break;
 131                         case 'd':
 132                                 if( read_if(optarg, &ceif[1]) != 0 )
 133                                 {
 134                                         perr( "'-d'" );
 135                                         return -1;
 136                                 }
 137                                 opt_flag |= GW_FLAG_DST;
 138                                 break;
 139                         case 'l':
 140                                 if( read_if(optarg, &iflisten) != 0 )
 141                                 {
 142                                         perr( "'-l'" );
 143                                         return -1;
 144                                 }
 145                                 if( iflisten.type != IF_ETH_UDP )
 146                                 {
 147                                         perr( "'-l' udp interface expected" );
 148                                         return -1;
 149                                 }
 150                                 opt_flag |= GW_FLAG_LISTEN;
 151                                 break;
 152                         case 'f':
 153                                 if( sscanf( optarg, "%x:%x", &flt.can_id,
 154                                    &flt.can_mask ) != 2 )
 155                                 {
 156                                         perr( "bad filter format");
 157                                         return -1;
 158                                 }
 159                                 opt_flag |= GW_FLAG_FILTER;
 160                                 break;
 161                         case '?':
 162                                 return -1;
 163                                 break;
 164                 }
 165         }
 166
 167         if( !((opt_flag & GW_FLAG_SRC) && (opt_flag & GW_FLAG_DST)
 168                 && (opt_flag & GW_FLAG_LISTEN)) )
 169         {
 170                 perr( "'s', 'd' and 'l' are mandatory" );
 171                 /* ToDo: usage? */
 172                 return -1;
 173         }
 174
 175         if( !(ceif[0].type == IF_CAN     && ceif[1].type == IF_ETH_UDP) &&
 176             !(ceif[0].type == IF_ETH_UDP && ceif[1].type == IF_CAN    )    )
 177         {
 178                 perr( "'-s' and '-d' should be different interface type" );
 179                 return -1;
 180         }
 181
 182         if( ceif[0].type == IF_CAN )
 183         {
 184                 ifcan = &ceif[0];
 185                 ifeth = &ceif[1];
 186         } else
 187         {
 188                 ifcan = &ceif[1];
 189                 ifeth = &ceif[0];
 190         }
 191
 192         signal( SIGINT, signal_exit );
 193
 194         /* prepare udp destination */
 195         udp_dest.sin_family = AF_INET;
 196         udp_dest.sin_port = htons(ifeth->eth.port);
 197         udp_dest.sin_addr = ifeth->eth.ip;
 198
 199         /* udp socket */
 200         sock_udp = socket( PF_INET, SOCK_DGRAM, IPPROTO_UDP );
 201         if( sock_udp < 0 )
 202         {
 203                 perr( "udp socket creation failed" );
 204                 return -1;
 205         }
 206
 207         udp_addr.sin_family = AF_INET;
 208         udp_addr.sin_port = htons(iflisten.eth.port);
 209         udp_addr.sin_addr = iflisten.eth.ip;
 210
 211         if( bind( sock_udp, (struct sockaddr*) &udp_addr, sizeof(udp_addr) ) < 0 )
 212         {
 213                 perr( "udp socket binding failed" );
 214                 close( sock_udp );
 215                 return -1;
 216         }
 217
 218         /* can socket */
 219         sock_can = socket( PF_CAN, SOCK_RAW, CAN_RAW );
 220         if( sock_can < 0 )
 221         {
 222                 perr( "can socket creation failed");
 223                 close( sock_can );
 224                 return -1;
 225         }
 226
 227         can_addr.can_family = AF_CAN;
 228         can_addr.can_ifindex = ifcan->can.ifindex;
 229
 230         if( bind( sock_can, (struct sockaddr*) &can_addr, sizeof(can_addr) ) < 0 )
 231         {
 232                 perr( "can socket binding failed" );
 233                 return -1;
 234         }
 235
 236         /* poll */
 237         pfd[GW_POLL_ETH].fd = sock_udp;
 238         pfd[GW_POLL_ETH].events = POLLIN | POLLPRI;
 239         pfd[GW_POLL_ETH].revents = 0;
 240
 241         pfd[GW_POLL_CAN].fd = sock_can;
 242         pfd[GW_POLL_CAN].events = POLLIN | POLLPRI;
 243         pfd[GW_POLL_CAN].revents = 0;
 244
 245         printf( "canethgw is running\n" );
 246
 247         while( 1 )
 248         {
 249                 printdbg( "polling\n" );
 250                 polret = poll( pfd, 2, -1 );
 251                 if( polret < 0 )
 252                 {
 253                         perr( "poll(..) failed" );
 254                         close( sock_udp );
 255                         close( sock_can );
 256                         return -1;
 257                 }
 258
 259                 if( pfd[GW_POLL_ETH].revents != 0 )
 260                 {
 261                         send_to_can();
 262                 }
 263                 if( pfd[GW_POLL_CAN].revents != 0 )
 264                 {
 265                         send_to_eth();
 266                 }
 267
 268                 pfd[GW_POLL_ETH].revents = 0;
 269                 pfd[GW_POLL_CAN].revents = 0;
 270         }
 271
 272         return 0;
 273 }
 274