utils/cegw/cegw.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <libgen.h>
   4 #include <string.h>
   5 #include <getopt.h>
   6 #include <errno.h>
   7 #include <sys/socket.h>
   8 #include <net/if.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>
  15 #include <unistd.h>
  16 #include "canethgw.h"
  17
  18 /**
  19  * ToDo
  20  * [ ] refactor error messages
  21  * [ ] start/stop listening
  22  * [ ] remove routing job
  23  * [ ] recv netlink reponse
  24  *
  25  */
  26
  27 #define CEGW_CMD_ADD     1
  28 #define CEGW_CMD_LIST    2
  29 #define CEGW_CMD_FLUSH   4
  30 #define CEGW_CMD_LISTEN  8
  31
  32 enum
  33 {
  34         IF_UNDEF,
  35         IF_CAN,
  36         IF_ETH_UDP
  37 };
  38
  39 struct cegw_data
  40 {
  41         int content;
  42         int src_if, dst_if;
  43         int can_ifidx;
  44         struct in_addr eth_addr;
  45         unsigned short eth_port;
  46         struct in_addr eth_listen_addr;
  47         unsigned short eth_listen_port;
  48 };
  49
  50 struct cegw_nlmsg
  51 {
  52         struct nlmsghdr nh;
  53         struct rtmsg rt;
  54         char buf[768]; /* enough? */
  55 };
  56
  57 unsigned int cegw_errno = 0;
  58
  59 enum
  60 {
  61         CEGW_ERR_UNKNOWN,
  62         CEGW_ERR_IF_UNSPEC,
  63         CEGW_ERR_IF_SAME,
  64         CEGW_ERR_IF_TYPE
  65 };
  66
  67 char* cegw_errlist[] =
  68 {
  69         [ CEGW_ERR_UNKNOWN ]   = "unknown error",
  70         [ CEGW_ERR_IF_UNSPEC ] = "source or destination not specified",
  71         [ CEGW_ERR_IF_SAME   ] = "source and destination have same interface type",
  72         [ CEGW_ERR_IF_TYPE    ] = "unknown interface type"
  73 };
  74
  75 static void perr( char* s )
  76 {
  77         if( s )
  78         {
  79                 fprintf( stderr, "error: %s; %s\n", s,
  80                 cegw_errlist[ cegw_errno ] );
  81                 return;
  82         }
  83
  84         fprintf( stderr, "error: %s\n", cegw_errlist[ cegw_errno ] );
  85 }
  86
  87 /**
  88  * Parses @in for eth address. Valid input is
  89  * e.g. udp@127.0.0.1:10502 or can@vcan0.
  90  *
  91  * @param[out] addr ip address
  92  * @param[out] port transport layer port
  93  * @return 0 on success
  94  */
  95 int read_addr_port( char* in, struct in_addr* addr, unsigned short* port )
  96 {
  97         char* delim = NULL;
  98         char addrstr[16];
  99         int addrlen;
 100
 101         if( (delim = strchr( in, ':' )) == NULL )
 102         {
 103                 fprintf( stderr, "error: ':'\n" );
 104                 return -1;
 105         }
 106
 107         /* get address */
 108         addrlen = delim - in;
 109         memcpy( addrstr, in, addrlen );
 110         addrstr[addrlen] = '\0';
 111         if( inet_aton( addrstr, addr ) == 0 )
 112         {
 113                 fprintf( stderr, "error: aton\n" );
 114                 return -1;
 115         }
 116
 117         /* get port */
 118         if( sscanf( delim, ":%hu", port ) != 1 ) /* todo: handle overflow */
 119         {
 120                 fprintf( stderr, "error: port\n" );
 121                 return -1;
 122         }
 123
 124         return 0;
 125 }
 126
 127 /**
 128  * Reads @in for iftype (e.g. "can@" or "udp@").
 129  *
 130  * @param[in] in string to search in
 131  * @param[out] iftype iftype detected
 132  * @return pointer to @in after iftype on success, NULL otherwise
 133  */
 134 char* read_iftype( char* in, int* iftype )
 135 {
 136         char* ret = in+4;
 137
 138         if( strncmp( "udp@", optarg, 4 ) == 0 )
 139         {
 140                 *iftype = IF_ETH_UDP;
 141                 return ret;
 142         }
 143         /*
 144         else if( strncmp( "tcp@", optarg, 4 ) == 0 )
 145         {
 146                 return NULL;
 147         }
 148         */
 149         else if( strncmp( "can@", optarg, 4 ) == 0 )
 150         {
 151                 *iftype = IF_CAN;
 152                 return ret;
 153         }
 154
 155         errno = CEGW_ERR_IF_TYPE;
 156         return NULL;
 157 }
 158
 159 int read_if( char* in, int* iftype, struct cegw_data* d )
 160 {
 161         char* optstr = NULL;
 162
 163         if( (optstr = read_iftype( in, iftype )) == NULL )
 164         {
 165                 return -1;
 166         }
 167
 168         switch( *iftype )
 169         {
 170                 case IF_CAN:
 171                         d->can_ifidx = if_nametoindex( optstr );
 172                         break;
 173                 case IF_ETH_UDP:
 174                         read_addr_port( optstr, &d->eth_addr, &d->eth_port );
 175                         break;
 176                 default:
 177                         return -1;
 178                         break;
 179         }
 180
 181         return 0;
 182 }
 183
 184 inline static int cegw_add( struct cegw_nlmsg* req, struct cegw_data* d )
 185 {
 186         int gwtype = 0;
 187
 188         req->nh.nlmsg_type  = RTM_NEWROUTE;
 189         if( (d->src_if == 0 || d->dst_if == 0) )
 190         {
 191                 cegw_errno = CEGW_ERR_IF_UNSPEC;
 192                 return -cegw_errno;
 193         }
 194
 195         if( d->src_if == d->dst_if )
 196         {
 197                 cegw_errno = CEGW_ERR_IF_SAME;
 198                 return -cegw_errno;
 199         }
 200
 201         gwtype = (d->src_if == IF_CAN) ? CGW_TYPE_CAN_ETH_UDP : CGW_TYPE_ETH_CAN_UDP;
 202         addattr_l( &req->nh, sizeof(*req), CGW_CAN_IF, &d->can_ifidx, sizeof(d->can_ifidx) );
 203
 204         switch( gwtype )
 205         {
 206                 case CGW_TYPE_CAN_ETH_UDP:
 207                         addattr_l( &req->nh, sizeof(*req), CGW_ETH_IP, &d->eth_addr, sizeof(d->eth_addr) );
 208                         addattr_l( &req->nh, sizeof(*req), CGW_ETH_PORT, &d->eth_port, sizeof(d->eth_port) );
 209                         break;
 210                 case CGW_TYPE_ETH_CAN_UDP:
 211                         break;
 212                 default:
 213                         break;
 214         }
 215
 216         addattr32( &req->nh, sizeof(*req), CGW_CMD_INFO, gwtype );
 217
 218         return 0;
 219 }
 220
 221 inline static int cegw_listen( struct cegw_nlmsg* req, struct cegw_data* d )
 222 {
 223         req->nh.nlmsg_type  = RTM_NEWROUTE;
 224         addattr32( &req->nh, sizeof(*req), CGW_CMD_INFO, CEGW_LISTEN );
 225         addattr_l( &req->nh, sizeof(*req), CGW_LISTEN_IP, &d->eth_listen_addr, sizeof(d->eth_listen_addr) );
 226         addattr_l( &req->nh, sizeof(*req), CGW_LISTEN_PORT, &d->eth_listen_port, sizeof(d->eth_listen_port) );
 227         printf( "start listen: %x, %hu\n", d->eth_listen_addr, d->eth_listen_port );
 228
 229         return 0;
 230 }
 231
 232 inline static int cegw_list( struct cegw_nlmsg* req, struct cegw_data* d )
 233 {
 234         return 0;
 235 }
 236
 237 inline static int cegw_flush( struct cegw_nlmsg* req, struct cegw_data* d )
 238 {
 239 //      char foo[2];
 240         addattr32( &req->nh, sizeof(*req), CGW_CMD_INFO, CEGW_FLUSH );
 241         req->nh.nlmsg_type  = RTM_DELROUTE;
 242 //      addattr_l( &req->nh, sizeof(*req), 0, &foo, sizeof(foo) );
 243         //addattr_l( &req->nh, sizeof(*req), CGW_CAN_IF, &d->can_ifidx, sizeof(d->can_ifidx) );
 244 //      req->rtcan.gwtype = CGW_TYPE_CAN_ETH_UDP;
 245 //      req->rtcan.flags = 5;//CEGW_FLUSH;
 246         return 0;
 247 }
 248
 249 int main( int argc, char* argv[] )
 250 {
 251         int s;
 252         int tmp = 0;
 253         int cmd = 0;
 254         char* optstr;
 255         char opt;
 256         struct sockaddr_nl nladdr;
 257         int err = 0;
 258         struct cegw_nlmsg req;
 259         struct cegw_data d;
 260         char rxbuf[256]; /* 8129+ ? */
 261         int rsize = 0;
 262         struct nlmsghdr* nlh;
 263         struct nlmsgerr* rte;
 264
 265         printf( "size=%d\n", NLMSG_LENGTH(sizeof(struct rtmsg)) );
 266
 267         memset( &d, 0, sizeof(d) );
 268
 269         struct option long_opt[] =
 270         {
 271                 { "add"   , 0, NULL, 'A' },
 272                 { "flush" , 0, NULL, 'F' },
 273                 { "list"  , 0, NULL, 'L' },
 274                 { "listen", 1, NULL, 'l' },
 275                 { 0, 0, 0, 0 }
 276         };
 277
 278         while( 1 )
 279         {
 280                 opt = getopt_long( argc, argv, "As:d:", long_opt, NULL );
 281                 if( opt == -1 )
 282                         break;
 283
 284                 switch( opt )
 285                 {
 286                         case 'A':
 287                                 cmd |= CEGW_CMD_ADD;
 288                                 break;
 289                         case 'F':
 290                                 cmd |= CEGW_CMD_FLUSH;
 291                                 break;
 292                         case 'L':
 293                                 cmd |= CEGW_CMD_FLUSH;
 294                                 break;
 295                         case 'l':
 296                                 cmd |= CEGW_CMD_LISTEN;
 297                                 if( (optstr = read_iftype( optarg, &tmp )) == NULL )
 298                                 {
 299                                         perr( "listen" );
 300                                         return -1;
 301                                 }
 302                                 if( tmp != IF_ETH_UDP )
 303                                 {
 304                                         fprintf( stderr, "error: -l bad input format\n" );
 305                                         return -1;
 306                                 }
 307                                 read_addr_port( optstr, &d.eth_listen_addr, &d.eth_listen_port ); /*chk*/
 308                                 break;
 309                         case 's':
 310                                 if( read_if( optarg, &d.src_if, &d ) != 0 )
 311                                 {
 312                                         fprintf( stderr, "error: bad input format\n" );
 313                                         goto syntax_error;
 314                                         break;
 315                                 }
 316                                 break;
 317                         case 'd':
 318                                 if( read_if( optarg, &d.dst_if, &d ) != 0 )
 319                                 {
 320                                         fprintf( stderr, "error: bad input format\n" );
 321                                         goto syntax_error;
 322                                         break;
 323                                 }
 324                                 break;
 325                         case '?':
 326                                 fprintf( stderr, "error: unknown option\n" );
 327                                 return -1;
 328                                 break;
 329                         default:
 330                                 fprintf( stderr, "error: unknown option\n" );
 331                                 return -1;
 332                                 break;
 333                 }
 334         }
 335
 336         /* prepare netlink message */
 337         req.nh.nlmsg_len   = NLMSG_LENGTH( sizeof(struct rtmsg) );
 338         req.nh.nlmsg_type  = RTM_DELROUTE;
 339         req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
 340         req.nh.nlmsg_seq   = 0;
 341         req.nh.nlmsg_pid   = 0; /* ? */
 342
 343         memset( &req.rt, 0, sizeof(req.rt) );
 344         req.rt.rtm_family = AF_CAN;
 345
 346         //req.rt.gwtype = CGW_TYPE_CAN_CAN;
 347         //req.rt.flags = 0;
 348
 349         switch( cmd )
 350         {
 351                 case 0:
 352                         fprintf( stderr, "error: command not specified\n" );
 353                         break;
 354                 case CEGW_CMD_ADD:
 355                         if( cegw_add( &req, &d ) != 0 )
 356                         {
 357                                 perr( "add" );
 358                                 return -1;
 359                         }
 360                         break;
 361                 case CEGW_CMD_FLUSH:
 362                         cegw_flush( &req, &d );
 363                         break;
 364                 case CEGW_CMD_LIST:
 365                         cegw_list( &req, &d );
 366                         break;
 367                 case CEGW_CMD_LISTEN:
 368                         cegw_listen( &req, &d );
 369                         break;
 370                 default:
 371                         fprintf( stderr, "error: command mismatch\n" );
 372                         break;
 373         }
 374
 375         /* send over netlink socket */
 376         s = socket( PF_NETLINK, SOCK_RAW, NETLINK_ROUTE ); /* chck */
 377
 378         memset( &nladdr, 0, sizeof(nladdr) );
 379         nladdr.nl_family = AF_NETLINK;
 380         nladdr.nl_pad = 0;
 381         nladdr.nl_pid = 0;
 382         nladdr.nl_groups = 0;
 383
 384         err = sendto( s, &req, req.nh.nlmsg_len, 0,
 385                       (struct sockaddr*)&nladdr, sizeof(nladdr) );
 386         if( err < 0 )
 387         {
 388                 perror( "netlink sendto" );
 389                 return err;
 390         }
 391
 392         /* recv */
 393         rsize = recv( s, &rxbuf, sizeof(rxbuf), 0 );
 394         if( rsize < 0 )
 395         {
 396                 fprintf( stderr, "error: recv\n" );
 397                 return -1;
 398         }
 399         nlh = (struct nlmsghdr*)rxbuf;
 400         if( nlh->nlmsg_type != NLMSG_ERROR )
 401         {
 402                 fprintf( stderr, "error: unexpected netlink answer=%d\n", nlh->nlmsg_type );
 403                 return -1;
 404         }
 405         rte = (struct nlmsgerr*)NLMSG_DATA( nlh );
 406         err = rte->error;
 407         if( err < 0 )
 408                 fprintf( stderr, "error: netlink(%d); %s\n", err, strerror(abs(err)) );
 409
 410         return 0;
 411 syntax_error:
 412         return -1;
 413 }
 414