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