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 /**
  22  * ToDo:
  23  * [ ] encapsule module - check inputs
  24  * [ ] refactor - chc .h
  25  * [ ] dump callback
  26  * [ ] rtnl vs nl functions
  27  * [ ] stop threads
  28  * [ ] change listening
  29  * [ ] clean exit - threads, jobs
  30  */
  31
  32 MODULE_LICENSE( "GPL" );
  33
  34 static int  cegw_udp_can( void* data );
  35 inline static void cegw_udp_send( struct socket* udp_sock, struct can_frame* cf, struct in_addr ipaddr, u16 port );
  36 static int  cegw_can_udp( void* data );
  37 inline static void cegw_can_send( struct socket* can_sock, struct can_frame* cf, int ifindex );
  38 static int cegw_thread_start( void );
  39 static void cegw_thread_stop( void );
  40 static int  cegw_thread_restart( void* arg );
  41
  42 #define CEGW_STOPPED 0
  43 #define CEGW_RUNNING 1
  44
  45 static struct socket* can_sock = NULL, * udp_sock = NULL;
  46 static struct task_struct* eth_to_can = NULL, * can_to_eth = NULL;
  47 /* ToDo: protect with mutex */
  48 static int cegw_state = CEGW_STOPPED;
  49
  50 struct cegw_rule
  51 {
  52         int can_ifindex;
  53         struct in_addr eth_ip;
  54         unsigned short eth_port;
  55         struct hlist_node list;
  56 };
  57
  58 HLIST_HEAD( cegw_rule_can_eth );
  59 HLIST_HEAD( cegw_rule_eth_can );
  60
  61 struct
  62 {
  63         int can_ifindex;
  64         struct in_addr eth_addr;
  65         unsigned short eth_port;
  66 } cegw_setting;
  67
  68 DEFINE_MUTEX( cegw_setting_mutex );
  69
  70 inline static void cegw_udp_send( struct socket* udp_sock, struct can_frame* cf, struct in_addr ipaddr, u16 port )
  71 {
  72         struct msghdr mh;
  73         struct sockaddr_in addr;
  74         struct kvec vec;
  75
  76         addr.sin_family = AF_INET;
  77         addr.sin_port = htons( port );
  78         addr.sin_addr = ipaddr;
  79
  80         mh.msg_name = &addr;
  81         mh.msg_namelen = sizeof( addr );
  82         mh.msg_control = NULL;
  83         mh.msg_controllen = 0;
  84         mh.msg_flags = 0;
  85
  86         vec.iov_base = cf;
  87         vec.iov_len = sizeof( *cf );
  88
  89         kernel_sendmsg( udp_sock, &mh, &vec, 1, sizeof( *cf ) );
  90 }
  91
  92 inline static void cegw_can_send( struct socket* can_sock, struct can_frame* cf, int ifindex )
  93 {
  94         struct msghdr mh;
  95         struct kvec vec;
  96         struct sockaddr_can addr;
  97
  98         addr.can_family = AF_CAN;
  99         addr.can_ifindex = ifindex;
 100
 101         mh.msg_name = &addr;
 102         mh.msg_namelen = sizeof( addr );
 103         mh.msg_control = NULL;
 104         mh.msg_controllen = 0;
 105         mh.msg_flags = 0;
 106
 107         vec.iov_base = cf;
 108         vec.iov_len = sizeof( *cf );
 109
 110         kernel_sendmsg( can_sock, &mh, &vec, 1, sizeof( *cf ) ); /* ToDo: handle error */
 111 }
 112
 113 /**
 114  * cegw_udp_can - performs udp->can routing
 115  * This function is run as a thread.
 116  */
 117 static int cegw_udp_can( void* data )
 118 {
 119         struct can_frame cf;
 120         struct kvec vec;
 121         struct msghdr mh;
 122         struct cegw_rule* job;
 123         struct hlist_node* pos;
 124         int can_ifidx;
 125         int recv_size;
 126
 127         mh.msg_name = NULL;
 128         mh.msg_namelen = 0;
 129         mh.msg_iov = NULL;
 130         mh.msg_iovlen = 0;
 131         mh.msg_control = NULL;
 132         mh.msg_controllen = 0;
 133         mh.msg_flags = 0;
 134
 135         while( 1 )
 136         {
 137                 if( cegw_state == CEGW_STOPPED )
 138                         break;
 139                 vec.iov_base = &cf;
 140                 vec.iov_len = sizeof(cf);
 141                 recv_size = kernel_recvmsg( udp_sock, &mh, &vec, 1, sizeof(cf), 0 ); /* ToDo: handle error, size check */
 142                 if( recv_size == 0 )
 143                 {
 144                         continue;
 145                 }
 146                 hlist_for_each_entry_rcu( job, pos, &cegw_rule_eth_can, list )
 147                 {
 148                         rcu_read_lock(); /**/
 149                         can_ifidx = job->can_ifindex;
 150                         rcu_read_unlock();
 151                         /* ToDo: from filter */
 152                         cegw_can_send( can_sock, &cf, can_ifidx );
 153                 }
 154         }
 155
 156         return 0;
 157 }
 158
 159 /**
 160  * cegw_can_udp - performs can->udp routing
 161  */
 162 static int cegw_can_udp( void* data )
 163 {
 164         struct msghdr mh;
 165         struct kvec vec;
 166         struct can_frame cf;
 167         struct sockaddr_can ca;
 168         struct cegw_rule* rule;
 169         struct hlist_node* pos;
 170         struct in_addr eth_addr;
 171         u16 eth_port;
 172         int recv_size;
 173
 174         mh.msg_name = &ca;
 175         mh.msg_namelen = sizeof( ca );
 176         mh.msg_control = NULL;
 177         mh.msg_controllen = 0;
 178         mh.msg_flags = 0;
 179
 180         while( 1 )
 181         {
 182                 if( cegw_state == CEGW_STOPPED ) /**/
 183                         break;
 184                 vec.iov_base = &cf;
 185                 vec.iov_len = sizeof( cf );
 186
 187                 recv_size = kernel_recvmsg( can_sock, &mh, &vec, 1, sizeof( cf ), 0 );
 188                 if( recv_size == 0 )
 189                 {
 190                         continue;
 191                 }
 192                 hlist_for_each_entry_rcu( rule, pos, &cegw_rule_can_eth, list )
 193                 {
 194                         rcu_read_lock();
 195                         eth_addr = rule->eth_ip;
 196                         eth_port = rule->eth_port;
 197                         rcu_read_unlock();
 198                         if( rule->can_ifindex == ca.can_ifindex )
 199                                 cegw_udp_send( udp_sock, &cf, eth_addr, eth_port );
 200                 }
 201         }
 202
 203         return 0;
 204 }
 205
 206 /* NetLink */
 207
 208 static int cegw_create_job( struct sk_buff* skb, struct nlmsghdr* nlh, void* arg )
 209 {
 210         struct nlattr* tb[ CEGW_MAX+1 ];
 211         struct cegw_rule* rule = NULL;
 212         int ifindex;
 213         struct rtmsg* r;
 214         struct in_addr ip;
 215         unsigned short port;
 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                         mutex_lock( &cegw_setting_mutex );
 249                         cegw_setting.eth_addr = *(struct in_addr*)nla_data( tb[CEGW_ETH_IP] );
 250                         cegw_setting.eth_port = *(unsigned short*)nla_data( tb[CEGW_ETH_PORT] );
 251                         kthread_run( cegw_thread_restart, NULL, "canethgw" );
 252                         mutex_unlock( &cegw_setting_mutex );
 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 int cegw_remove_job( struct sk_buff* skb, struct nlmsghdr* nlh, void* arg )
 311 {
 312         struct rtmsg* r;
 313         struct nlattr* tb[ CEGW_MAX+1 ];
 314         struct hlist_node* pos,* n;
 315         struct cegw_rule* rule;
 316
 317         int err = 0;
 318
 319         if( nlmsg_len(nlh) < sizeof(*r) )
 320                 return -EINVAL;
 321
 322         r = nlmsg_data( nlh );
 323
 324         if( r->rtm_family != AF_CAN )
 325                 return -EPFNOSUPPORT;
 326
 327         err = nlmsg_parse( nlh, sizeof(struct rtmsg), tb, CEGW_MAX, NULL );
 328         if( err != 0 )
 329                 return -EINVAL;
 330
 331         if( tb[CEGW_CMD_INFO] == NULL )
 332         {
 333                 pr_devel( "canethgw: CEGW_CMD_INFO is missing in rtmsg\n" );
 334                 return -EINVAL;
 335         }
 336
 337         if( *(int*)nla_data( tb[CEGW_CMD_INFO] ) != CEGW_FLUSH )
 338         {
 339                 return -EINVAL;
 340         }
 341
 342         hlist_for_each_entry_safe( rule, pos, n, &cegw_rule_can_eth, list )
 343         {
 344                 hlist_del( &rule->list );
 345                 kfree( rule );
 346         }
 347         hlist_for_each_entry_safe( rule, pos, n, &cegw_rule_eth_can, list )
 348         {
 349                 hlist_del( &rule->list );
 350                 kfree( rule );
 351         }
 352
 353         return 0;
 354 }
 355
 356 static int cegw_put_rule( struct sk_buff* skb, int type, struct cegw_rule* rule )
 357 {
 358         int ifindex;
 359         struct in_addr ip;
 360         unsigned short port;
 361         struct nlmsghdr* nlh;
 362
 363         ifindex = rule->can_ifindex;
 364         ip = rule->eth_ip;
 365         port = rule->eth_port;
 366
 367         nlh = nlmsg_put( skb, 0, 0, 0, 0, 0 );
 368         if( nlh == NULL )
 369                 return -EMSGSIZE;
 370
 371         /* type */
 372         if( nla_put( skb, CEGW_TYPE, sizeof(type), &type ) < 0 )
 373                 goto cancel;
 374         else
 375                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(type) );
 376
 377         /* can ifindex */
 378         if( nla_put( skb, CEGW_CAN_IFINDEX, sizeof(ifindex), &ifindex ) < 0 )
 379                 goto cancel;
 380         else
 381                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(ifindex) );
 382
 383         /* ip adress */
 384         if( nla_put( skb, CEGW_ETH_IP, sizeof(ip), &ip) < 0 )
 385                 goto cancel;
 386         else
 387                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(ip) );
 388
 389         /* port */
 390         if( nla_put( skb, CEGW_ETH_PORT, sizeof(port), &port ) < 0 )
 391                 goto cancel;
 392         else
 393                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(port) );
 394
 395         return skb->len;
 396
 397 cancel:
 398         nlmsg_cancel( skb, nlh );
 399         return -EMSGSIZE;
 400 }
 401
 402 static int cegw_dump_job( struct sk_buff* skb, struct netlink_callback* cb )
 403 {
 404         struct cegw_rule* rule;
 405         struct hlist_node* pos;
 406         int idx = 0;
 407         int s_idx = cb->args[0];
 408
 409         /* ToDo: skb max size */
 410
 411         rcu_read_lock();
 412         hlist_for_each_entry_rcu( rule, pos, &cegw_rule_eth_can, list )
 413         {
 414                 if( idx < s_idx )
 415                         goto cont1;
 416
 417                 if( cegw_put_rule( skb, CEGW_RULE_ETH_CAN, rule ) < 0 )
 418                         goto brk;
 419 cont1:
 420                 idx++;
 421         }
 422         rcu_read_unlock();
 423
 424         rcu_read_lock();
 425         hlist_for_each_entry_rcu( rule, pos, &cegw_rule_can_eth, list )
 426         {
 427                 if( idx < s_idx )
 428                         goto cont2;
 429
 430                 if( cegw_put_rule( skb, CEGW_RULE_CAN_ETH, rule ) < 0 )
 431                         goto brk;
 432
 433 cont2:
 434                 idx++;
 435         }
 436         rcu_read_unlock();
 437
 438 brk:
 439         cb->args[0] = idx;
 440
 441         return skb->len;
 442 }
 443
 444 static int cegw_thread_start( void )
 445 {
 446         struct sockaddr_in udp_addr;
 447         struct sockaddr_can can_addr;
 448
 449         can_addr.can_family = AF_CAN;
 450         can_addr.can_ifindex = 0;
 451
 452         udp_addr.sin_family = AF_INET;
 453         udp_addr.sin_port = htons( cegw_setting.eth_port );
 454         udp_addr.sin_addr = cegw_setting.eth_addr;
 455
 456         /* create and bind sockets */
 457         if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &udp_sock) != 0 )
 458         {
 459                 printk( KERN_ERR "canethgw: udp socket creation failed\n" );
 460                 return -1;
 461         }
 462
 463         if( sock_create_kern( PF_CAN, SOCK_RAW, CAN_RAW, &can_sock) != 0 )
 464         {
 465                 printk( KERN_ERR "canethgw: can socket creation failed\n" );
 466                 return -1;
 467         }
 468
 469         if( kernel_bind( udp_sock, (struct sockaddr*)&udp_addr, sizeof( udp_addr ) ) != 0 )
 470         {
 471                 printk( KERN_ERR "canethgw: udp socket binding failed\n" );
 472                 sock_release( udp_sock );
 473                 sock_release( can_sock );
 474                 return -1; /* ToDo: state==RUNNING and return? */
 475         }
 476
 477         if( kernel_bind( can_sock, (struct sockaddr*) &can_addr, sizeof(can_addr) ) != 0 )
 478         {
 479                 printk( KERN_ERR "canethgw: can socket binding failed\n" );
 480                 sock_release( udp_sock );
 481                 sock_release( can_sock );
 482                 return -1;
 483         }
 484
 485         /* start threads */
 486         cegw_state = CEGW_RUNNING;
 487         eth_to_can = kthread_create( cegw_udp_can, NULL, "canethgw" );
 488         if( !IS_ERR( eth_to_can ) )
 489         {
 490                 get_task_struct( eth_to_can );
 491                 wake_up_process( eth_to_can );
 492         }
 493         can_to_eth = kthread_create( cegw_can_udp, NULL, "canethgw" );
 494         if( !IS_ERR( can_to_eth ) )
 495         {
 496                 /* ToDo: free this? */
 497                 get_task_struct( can_to_eth );
 498                 wake_up_process( can_to_eth );
 499         }
 500
 501         /* ToDo: kthread creation fail */
 502         pr_devel( "threads are running\n" );
 503
 504         return 0;
 505 }
 506
 507 static void cegw_thread_stop( void )
 508 {
 509         int how = SHUT_RDWR;
 510         struct sock* sk = NULL;
 511
 512         /* no threads are running */
 513         if( !can_to_eth && !eth_to_can )
 514                 return;
 515
 516         cegw_state = CEGW_STOPPED;
 517         /* shut down socket */
 518         sk = can_sock->sk;
 519         how++;
 520         lock_sock( sk );
 521         sk->sk_shutdown |= how;
 522         sk->sk_state_change( sk );
 523         release_sock( sk );
 524
 525         kernel_sock_shutdown( udp_sock, SHUT_RDWR );
 526
 527         /* wait for return to reuse port if restart */
 528         kthread_stop( eth_to_can );
 529         kthread_stop( can_to_eth );
 530         sock_release( udp_sock );
 531         sock_release( can_sock );
 532         can_to_eth = NULL;
 533         eth_to_can = NULL;
 534 }
 535
 536 /**
 537  * cegw_thread_restart
 538  */
 539 static int cegw_thread_restart( void* data )
 540 {
 541         cegw_thread_stop();
 542         cegw_thread_start();
 543
 544         return 0;
 545 }
 546
 547 /***********************
 548  *   module init/exit
 549  ***********************/
 550
 551 static int __init cegw_init( void )
 552 {
 553         /* subscribe to netlink */
 554         rtnl_register( PF_CAN, RTM_GETROUTE, NULL, cegw_dump_job, NULL );
 555         rtnl_register( PF_CAN, RTM_NEWROUTE, cegw_create_job, NULL, NULL );
 556         rtnl_register( PF_CAN, RTM_DELROUTE, cegw_remove_job, NULL, NULL );
 557
 558         return 0;
 559 }
 560
 561 static void __exit cegw_exit( void )
 562 {
 563         /* ToDo: effect on others? */
 564         rtnl_unregister_all( PF_CAN );
 565
 566         /* ToDo: no rtnl pending? */
 567         cegw_thread_stop();
 568         /* ToDo: frees mem_cache?    */
 569         /*       udp must not exists */
 570
 571         printk( "cangw: exit\n" );
 572 }
 573
 574 module_init( cegw_init );
 575 module_exit( cegw_exit );
 576