kernel/canethgw.c

   1 #define DEBUG 1
   2
   3 #include <linux/module.h>
   4 #include <linux/kernel.h>
   5 #include <linux/kthread.h>
   6 #include <linux/sched.h>
   7 #include <linux/delay.h>
   8 #include <linux/wait.h>
   9 #include <linux/netdevice.h>
  10 #include <linux/socket.h>
  11 #include <linux/if_arp.h>
  12 #include <linux/net.h>
  13 #include <linux/netdevice.h>
  14 #include <linux/can/core.h>
  15 #include <linux/can.h>
  16 #include <net/rtnetlink.h>
  17 #include <net/sock.h>
  18 #include "canethgw.h"
  19 #include <linux/completion.h>
  20 #include <linux/mutex.h>
  21 #include <net/inet_common.h>
  22
  23 MODULE_LICENSE( "GPL" );
  24
  25 static int  cegw_udp_can( void* data );
  26 inline static void cegw_udp_send( struct socket* udp_sock, struct can_frame* cf, struct in_addr ipaddr, u16 port );
  27 static int  cegw_can_udp( void* data );
  28 inline static void cegw_can_send( struct socket* can_sock, struct can_frame* cf, int ifindex );
  29 static int cegw_thread_start( void* data );
  30 static int cegw_thread_stop( void );
  31
  32 enum __cegw_state
  33 {
  34         CEGW_RUN,
  35         CEGW_STOP,
  36         CEGW_EXIT
  37 };
  38
  39 struct cegw_rule
  40 {
  41         int can_ifindex;
  42         struct in_addr eth_ip;
  43         unsigned short eth_port;
  44         struct hlist_node list;
  45 };
  46
  47 struct cegw_setting
  48 {
  49         struct in_addr eth_ip;
  50         unsigned short eth_port;
  51 };
  52
  53 static int cegw_state = CEGW_STOP;
  54 static struct socket* can_sock = NULL, * udp_sock = NULL;
  55 static struct task_struct* eth_to_can = NULL, * can_to_eth = NULL;
  56 static struct notifier_block notifier;
  57
  58 HLIST_HEAD( cegw_rule_can_eth );
  59 HLIST_HEAD( cegw_rule_eth_can );
  60 DEFINE_MUTEX( cegw_mutex );
  61
  62 inline static void cegw_udp_send( struct socket* udp_sock, struct can_frame* cf, struct in_addr ipaddr, u16 port )
  63 {
  64         struct msghdr mh;
  65         struct sockaddr_in addr;
  66         struct kvec vec;
  67
  68         addr.sin_family = AF_INET;
  69         addr.sin_port = htons( port );
  70         addr.sin_addr = ipaddr;
  71
  72         mh.msg_name = &addr;
  73         mh.msg_namelen = sizeof( addr );
  74         mh.msg_control = NULL;
  75         mh.msg_controllen = 0;
  76         mh.msg_flags = 0;
  77
  78         vec.iov_base = cf;
  79         vec.iov_len = sizeof( *cf );
  80
  81         kernel_sendmsg( udp_sock, &mh, &vec, 1, sizeof( *cf ) );
  82 }
  83
  84 inline static void cegw_can_send( struct socket* can_sock, struct can_frame* cf, int ifindex )
  85 {
  86         struct msghdr mh;
  87         struct kvec vec;
  88         struct sockaddr_can addr;
  89
  90         addr.can_family = AF_CAN;
  91         addr.can_ifindex = ifindex;
  92
  93         mh.msg_name = &addr;
  94         mh.msg_namelen = sizeof( addr );
  95         mh.msg_control = NULL;
  96         mh.msg_controllen = 0;
  97         mh.msg_flags = 0;
  98
  99         vec.iov_base = cf;
 100         vec.iov_len = sizeof( *cf );
 101
 102         kernel_sendmsg( can_sock, &mh, &vec, 1, sizeof( *cf ) );
 103 }
 104
 105 /**
 106  * cegw_udp_can - performs udp->can routing
 107  * This function is run as a thread.
 108  */
 109 static int cegw_udp_can( void* data )
 110 {
 111         struct can_frame cf;
 112         struct kvec vec;
 113         struct msghdr mh;
 114         struct cegw_rule* rule;
 115         struct hlist_node* pos;
 116         int can_ifidx;
 117         int recv_size;
 118
 119         mh.msg_name = NULL;
 120         mh.msg_namelen = 0;
 121         mh.msg_iov = NULL;
 122         mh.msg_iovlen = 0;
 123         mh.msg_control = NULL;
 124         mh.msg_controllen = 0;
 125         mh.msg_flags = 0;
 126
 127         while( 1 )
 128         {
 129                 if( cegw_state == CEGW_STOP )
 130                         break;
 131                 vec.iov_base = &cf;
 132                 vec.iov_len = sizeof(cf);
 133                 recv_size = kernel_recvmsg( udp_sock, &mh, &vec, 1, sizeof(cf), 0 );
 134                 /* recv_size == 0 when shutting down */
 135                 if( recv_size != sizeof(cf) || recv_size == 0 )
 136                 {
 137                         continue;
 138                 } else if( recv_size < 0 )
 139                 {
 140                         return -1;
 141                 }
 142
 143                 hlist_for_each_entry_rcu( rule, pos, &cegw_rule_eth_can, list )
 144                 {
 145                         rcu_read_lock();
 146                         can_ifidx = rule->can_ifindex;
 147                         rcu_read_unlock();
 148                         /* ToDo: from filter */
 149                         cegw_can_send( can_sock, &cf, can_ifidx );
 150                 }
 151         }
 152
 153         return 0;
 154 }
 155
 156 /**
 157  * cegw_can_udp - performs can->udp routing
 158  */
 159 static int cegw_can_udp( void* data )
 160 {
 161         struct msghdr mh;
 162         struct kvec vec;
 163         struct can_frame cf;
 164         struct sockaddr_can ca;
 165         struct cegw_rule* rule;
 166         struct hlist_node* pos;
 167         struct in_addr eth_ip;
 168         u16 eth_port;
 169         int recv_size;
 170
 171         mh.msg_name = &ca;
 172         mh.msg_namelen = sizeof( ca );
 173         mh.msg_control = NULL;
 174         mh.msg_controllen = 0;
 175         mh.msg_flags = 0;
 176
 177         while( 1 )
 178         {
 179                 if( cegw_state == CEGW_STOP )
 180                         break;
 181                 vec.iov_base = &cf;
 182                 vec.iov_len = sizeof( cf );
 183
 184                 recv_size = kernel_recvmsg( can_sock, &mh, &vec, 1, sizeof( cf ), 0 );
 185                 if( recv_size != sizeof(cf) || recv_size == 0 )
 186                 {
 187                         continue;
 188                 } else if( recv_size < 0 )
 189                 {
 190                         return -1;
 191                 }
 192
 193                 hlist_for_each_entry_rcu( rule, pos, &cegw_rule_can_eth, list )
 194                 {
 195                         rcu_read_lock();
 196                         eth_ip = rule->eth_ip;
 197                         eth_port = rule->eth_port;
 198                         rcu_read_unlock();
 199                         if( rule->can_ifindex == ca.can_ifindex )
 200                                 cegw_udp_send( udp_sock, &cf, eth_ip, eth_port );
 201                 }
 202         }
 203
 204         return 0;
 205 }
 206
 207 static int cegw_newroute( struct sk_buff* skb, struct nlmsghdr* nlh, void* arg )
 208 {
 209         struct nlattr* tb[ CEGW_MAX+1 ];
 210         struct cegw_rule* rule = NULL;
 211         int ifindex;
 212         struct rtmsg* r;
 213         struct in_addr ip;
 214         unsigned short port;
 215         struct cegw_setting* set;
 216         int err = 0;
 217
 218         if( nlmsg_len(nlh) < sizeof(*r) )
 219                 return -EINVAL;
 220
 221         r = nlmsg_data( nlh );
 222
 223         if( r->rtm_family != AF_CAN )
 224                 return -EPFNOSUPPORT;
 225
 226         err = nlmsg_parse( nlh, sizeof(*r), tb, CEGW_MAX, NULL );
 227         if( err < 0 )
 228         {
 229                 pr_devel( "canethgw: nlmsg_parse error\n" );
 230                 return err;
 231         }
 232
 233         if( tb[CEGW_CMD_INFO] == NULL )
 234         {
 235                 pr_devel( "canethgw: CEGW_CMD_INFO is missing in rtmsg\n" );
 236                 return -EINVAL;
 237         }
 238
 239         switch( *(int*)nla_data( tb[CEGW_CMD_INFO] ) )
 240         {
 241                 case CEGW_LISTEN:
 242                         if( !tb[CEGW_ETH_IP] || !tb[CEGW_ETH_PORT] )
 243                         {
 244                                 pr_devel( "canethgw: missing attribute for CEGW_LISTEN\n" );
 245                                 return -EINVAL;
 246                         }
 247
 248                         /* ToDo: valid listen address */
 249                         set = kmalloc( sizeof(*set), GFP_KERNEL );
 250                         set->eth_ip   = *(struct in_addr*)nla_data( tb[CEGW_ETH_IP] );
 251                         set->eth_port = *(unsigned short*)nla_data( tb[CEGW_ETH_PORT] );
 252                         kthread_run( cegw_thread_start, set, "canethgw" );
 253                         break;
 254                 case CEGW_RULE_CAN_ETH:
 255                         if( !tb[CEGW_ETH_IP] || !tb[CEGW_ETH_PORT] || !tb[CEGW_CAN_IFINDEX] )
 256                         {
 257                                 pr_devel( "canethgw: missing attribute for CEGW_RULE_CAN_ETH\n" );
 258                                 return -EINVAL;
 259                         }
 260
 261                         ifindex = *(int*)nla_data( tb[CEGW_CAN_IFINDEX] );
 262                         ip = *(struct in_addr*)nla_data( tb[CEGW_ETH_IP] );
 263                         port = *(unsigned short*)nla_data( tb[CEGW_ETH_PORT] );
 264                         pr_devel( "canethgw: new can->eth rule - (%d)->(%x:%hu)\n", ifindex, ip.s_addr, port );
 265
 266                         rule = kmalloc( sizeof(struct cegw_rule), GFP_KERNEL );
 267                         if( rule == NULL )
 268                         {
 269                                 break;
 270                         }
 271
 272                         rule->can_ifindex = ifindex;
 273                         rule->eth_ip = ip;
 274                         rule->eth_port = port;
 275
 276                         hlist_add_head_rcu( &rule->list, &cegw_rule_can_eth );
 277                         break;
 278                 case CEGW_RULE_ETH_CAN:
 279                         if( !tb[CEGW_ETH_IP] || !tb[CEGW_ETH_PORT] || !tb[CEGW_CAN_IFINDEX] )
 280                         {
 281                                 pr_devel( "canethgw: missing attribute for CEGW_RULE_ETH_CAN\n" );
 282                                 return -EINVAL;
 283                         }
 284
 285                         ifindex = *(int*)nla_data( tb[CEGW_CAN_IFINDEX] );
 286                         ip = *(struct in_addr*)nla_data( tb[CEGW_ETH_IP] );
 287                         port = *(unsigned short*)nla_data( tb[CEGW_ETH_PORT] );
 288                         pr_devel( "canethgw: new eth->can rule - (%x:%hu)->(%d)\n", ip.s_addr, port, ifindex );
 289
 290                         rule = kmalloc( sizeof(struct cegw_rule), GFP_KERNEL );
 291                         if( rule == NULL )
 292                         {
 293                                 break;
 294                         }
 295
 296                         rule->can_ifindex = ifindex;
 297                         rule->eth_ip = ip;
 298                         rule->eth_port = port;
 299
 300                         hlist_add_head_rcu( &rule->list, &cegw_rule_eth_can );
 301                         break;
 302                 default:
 303                         pr_devel( "canethgw: unknown CEGW_CMD_INFO\n" );
 304                         break;
 305         }
 306
 307         return 0;
 308 }
 309
 310 static void cegw_flush( void )
 311 {
 312         struct cegw_rule* rule;
 313         struct hlist_node* pos,* n;
 314
 315         hlist_for_each_entry_safe( rule, pos, n, &cegw_rule_can_eth, list )
 316         {
 317                 hlist_del( &rule->list );
 318                 kfree( rule );
 319         }
 320         hlist_for_each_entry_safe( rule, pos, n, &cegw_rule_eth_can, list )
 321         {
 322                 hlist_del( &rule->list );
 323                 kfree( rule );
 324         }
 325 }
 326
 327 static int cegw_delroute( struct sk_buff* skb, struct nlmsghdr* nlh, void* arg )
 328 {
 329         struct rtmsg* r;
 330         struct nlattr* tb[ CEGW_MAX+1 ];
 331         int err = 0;
 332
 333         if( nlmsg_len(nlh) < sizeof(*r) )
 334                 return -EINVAL;
 335
 336         r = nlmsg_data( nlh );
 337
 338         if( r->rtm_family != AF_CAN )
 339                 return -EPFNOSUPPORT;
 340
 341         err = nlmsg_parse( nlh, sizeof(struct rtmsg), tb, CEGW_MAX, NULL );
 342         if( err != 0 )
 343                 return -EINVAL;
 344
 345         if( tb[CEGW_CMD_INFO] == NULL )
 346         {
 347                 pr_devel( "canethgw: CEGW_CMD_INFO is missing in rtmsg\n" );
 348                 return -EINVAL;
 349         }
 350
 351         if( *(int*)nla_data( tb[CEGW_CMD_INFO] ) != CEGW_FLUSH )
 352         {
 353                 return -EINVAL;
 354         }
 355
 356         cegw_flush();
 357
 358         return 0;
 359 }
 360
 361 static int cegw_put_rule( struct sk_buff* skb, int type, struct cegw_rule* rule )
 362 {
 363         int ifindex;
 364         struct in_addr ip;
 365         unsigned short port;
 366         struct nlmsghdr* nlh;
 367
 368         ifindex = rule->can_ifindex;
 369         ip = rule->eth_ip;
 370         port = rule->eth_port;
 371
 372         nlh = nlmsg_put( skb, 0, 0, 0, 0, 0 );
 373         if( nlh == NULL )
 374                 return -EMSGSIZE;
 375
 376         /* type */
 377         if( nla_put( skb, CEGW_TYPE, sizeof(type), &type ) < 0 )
 378                 goto cancel;
 379         else
 380                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(type) );
 381
 382         /* can ifindex */
 383         if( nla_put( skb, CEGW_CAN_IFINDEX, sizeof(ifindex), &ifindex ) < 0 )
 384                 goto cancel;
 385         else
 386                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(ifindex) );
 387
 388         /* ip adress */
 389         if( nla_put( skb, CEGW_ETH_IP, sizeof(ip), &ip) < 0 )
 390                 goto cancel;
 391         else
 392                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(ip) );
 393
 394         /* port */
 395         if( nla_put( skb, CEGW_ETH_PORT, sizeof(port), &port ) < 0 )
 396                 goto cancel;
 397         else
 398                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(port) );
 399
 400         return skb->len;
 401
 402 cancel:
 403         nlmsg_cancel( skb, nlh );
 404         return -EMSGSIZE;
 405 }
 406
 407 static int cegw_getroute( struct sk_buff* skb, struct netlink_callback* cb )
 408 {
 409         struct cegw_rule* rule;
 410         struct hlist_node* pos;
 411         int idx = 0;
 412         int s_idx = cb->args[0];
 413
 414         /* ToDo: skb max size */
 415
 416         rcu_read_lock();
 417         hlist_for_each_entry_rcu( rule, pos, &cegw_rule_eth_can, list )
 418         {
 419                 if( idx < s_idx )
 420                         goto cont1;
 421
 422                 if( cegw_put_rule( skb, CEGW_RULE_ETH_CAN, rule ) < 0 )
 423                         goto brk;
 424 cont1:
 425                 idx++;
 426         }
 427         rcu_read_unlock();
 428
 429         rcu_read_lock();
 430         hlist_for_each_entry_rcu( rule, pos, &cegw_rule_can_eth, list )
 431         {
 432                 if( idx < s_idx )
 433                         goto cont2;
 434
 435                 if( cegw_put_rule( skb, CEGW_RULE_CAN_ETH, rule ) < 0 )
 436                         goto brk;
 437
 438 cont2:
 439                 idx++;
 440         }
 441         rcu_read_unlock();
 442
 443 brk:
 444         cb->args[0] = idx;
 445
 446         return skb->len;
 447 }
 448
 449 static int cegw_notifier( struct notifier_block* nb, unsigned long msg, void* data )
 450 {
 451         struct net_device* dev = (struct net_device*)data;
 452         struct cegw_rule* rule;
 453         struct hlist_node* pos,* n;
 454
 455         if( !net_eq(dev_net(dev), &init_net) )
 456                 return NOTIFY_DONE;
 457         if( dev->type != ARPHRD_CAN )
 458                 return NOTIFY_DONE;
 459
 460         if( msg == NETDEV_UNREGISTER )
 461         {
 462                 hlist_for_each_entry_safe( rule, pos, n, &cegw_rule_eth_can, list )
 463                 {
 464                         if( rule->can_ifindex == dev->ifindex )
 465                         {
 466                                 hlist_del( &rule->list );
 467                                 kfree( rule );
 468                         }
 469                 }
 470
 471                 hlist_for_each_entry_safe( rule, pos, n, &cegw_rule_can_eth, list )
 472                 {
 473                         if( rule->can_ifindex == dev->ifindex )
 474                         {
 475                                 hlist_del( &rule->list );
 476                                 kfree( rule );
 477                         }
 478                 }
 479         }
 480
 481         return NOTIFY_DONE;
 482 }
 483
 484 /**
 485  * cegw_thread_start - start working threads
 486  * Two threads are started. One is serving udp->can routing and the other
 487  * can->udp.
 488  *
 489  * @return 0 on success, -1 otherwise
 490  */
 491 static int cegw_thread_start( void* data )
 492 {
 493         struct sockaddr_in udp_addr;
 494         struct sockaddr_can can_addr;
 495         struct cegw_setting* set;
 496
 497         set = (struct cegw_setting*)data;
 498
 499         can_addr.can_family = AF_CAN;
 500         can_addr.can_ifindex = 0;
 501
 502         udp_addr.sin_family = AF_INET;
 503         udp_addr.sin_port = htons( set->eth_port );
 504         udp_addr.sin_addr = set->eth_ip;
 505
 506         kfree( set );
 507         mutex_lock( &cegw_mutex );
 508         if( cegw_state == CEGW_EXIT )
 509                 return -1;
 510         /* stops threads if exist */
 511         cegw_thread_stop();
 512
 513         /* create and bind sockets */
 514         if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &udp_sock) != 0 )
 515         {
 516                 printk( KERN_ERR "canethgw: udp socket creation failed\n" );
 517                 return -1;
 518         }
 519
 520         if( sock_create_kern( PF_CAN, SOCK_RAW, CAN_RAW, &can_sock) != 0 )
 521         {
 522                 printk( KERN_ERR "canethgw: can socket creation failed\n" );
 523                 return -1;
 524         }
 525
 526         if( kernel_bind( udp_sock, (struct sockaddr*)&udp_addr, sizeof( udp_addr ) ) != 0 )
 527         {
 528                 printk( KERN_ERR "canethgw: udp socket binding failed\n" );
 529                 sock_release( udp_sock );
 530                 sock_release( can_sock );
 531                 return -1;
 532         }
 533
 534         if( kernel_bind( can_sock, (struct sockaddr*) &can_addr, sizeof(can_addr) ) != 0 )
 535         {
 536                 printk( KERN_ERR "canethgw: can socket binding failed\n" );
 537                 kernel_sock_shutdown( udp_sock, SHUT_RDWR );
 538                 sock_release( udp_sock );
 539                 sock_release( can_sock );
 540                 return -1;
 541         }
 542
 543         /* start threads */
 544         cegw_state = CEGW_RUN;
 545
 546         eth_to_can = kthread_create( cegw_udp_can, NULL, "canethgw" );
 547         if( IS_ERR( eth_to_can ) )
 548         {
 549                 cegw_state = CEGW_STOP;
 550                 sock_release( udp_sock );
 551                 sock_release( can_sock );
 552                 return -1;
 553         }
 554         get_task_struct( eth_to_can );
 555         wake_up_process( eth_to_can );
 556
 557         can_to_eth = kthread_create( cegw_can_udp, NULL, "canethgw" );
 558         if( IS_ERR( can_to_eth ) )
 559         {
 560                 cegw_state = CEGW_STOP;
 561                 kernel_sock_shutdown( udp_sock, SHUT_RDWR );
 562                 kthread_stop( eth_to_can );
 563                 sock_release( udp_sock );
 564                 sock_release( can_sock );
 565                 return -1;
 566         }
 567         /* ToDo: free this? */
 568         get_task_struct( can_to_eth );
 569         wake_up_process( can_to_eth );
 570
 571         mutex_unlock( &cegw_mutex );
 572         pr_devel( "threads are running\n" );
 573         return 0;
 574 }
 575
 576 /**
 577  * cegw_thread_stop
 578  * Waits for threads to stop. Does nothing if cegw_state == CEGW_STOP.
 579  *
 580  * @return 0
 581  */
 582 static int cegw_thread_stop( void )
 583 {
 584         int how = SHUT_RDWR;
 585         struct sock* sk = NULL;
 586
 587         if( cegw_state == CEGW_STOP )
 588                 return 0;
 589
 590         cegw_state = CEGW_STOP;
 591         /* shut down socket */
 592         sk = can_sock->sk;
 593         how++;
 594         lock_sock( sk );
 595         sk->sk_shutdown |= how;
 596         sk->sk_state_change( sk );
 597         release_sock( sk );
 598
 599         kernel_sock_shutdown( udp_sock, SHUT_RDWR );
 600
 601         /* wait for return to reuse port if restart */
 602         kthread_stop( eth_to_can );
 603         kthread_stop( can_to_eth );
 604         sock_release( udp_sock );
 605         sock_release( can_sock );
 606         can_to_eth = NULL;
 607         eth_to_can = NULL;
 608
 609         return 0;
 610 }
 611
 612 static int __init cegw_init( void )
 613 {
 614         notifier.notifier_call = cegw_notifier;
 615         register_netdevice_notifier( &notifier );
 616
 617         /* subscribe to netlink */
 618         rtnl_register( PF_CAN, RTM_GETROUTE, NULL, cegw_getroute, NULL );
 619         rtnl_register( PF_CAN, RTM_NEWROUTE, cegw_newroute, NULL, NULL );
 620         rtnl_register( PF_CAN, RTM_DELROUTE, cegw_delroute, NULL, NULL );
 621
 622         return 0;
 623 }
 624
 625 static void __exit cegw_exit( void )
 626 {
 627         /* ToDo: effect on cangw? */
 628         rtnl_unregister_all( PF_CAN );
 629
 630         /* wait for rtnl callbacks */
 631         rtnl_lock();
 632         rtnl_unlock();
 633
 634         mutex_lock( &cegw_mutex );
 635         cegw_thread_stop();
 636         cegw_state = CEGW_EXIT;
 637         mutex_unlock( &cegw_mutex );
 638
 639         unregister_netdevice_notifier( &notifier );
 640         cegw_flush();
 641 }
 642
 643 module_init( cegw_init );
 644 module_exit( cegw_exit );
 645