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