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 void 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, * udp_sock;
  46 static struct task_struct* eth_to_can, * can_to_eth;
  47 /* ToDo: protect with mutex */
  48 static int cegw_state = CEGW_STOPPED;
  49
  50 struct can_eth_gw
  51 {
  52         int src_ifindex;
  53         struct in_addr dst_addr;
  54         unsigned short dst_port;
  55         struct hlist_node list;
  56 };
  57
  58 struct eth_can_gw
  59 {
  60         int dst_if_idx;
  61         struct hlist_node list;
  62 };
  63
  64 HLIST_HEAD( can_eth_job );
  65 HLIST_HEAD( eth_can_job );
  66
  67 struct
  68 {
  69         struct can_filter filter;
  70         int can_idx;
  71         /* bind on if */
  72         struct in_addr eth_addr;
  73         unsigned short eth_port;
  74 } cegw_setting;
  75
  76 DECLARE_COMPLETION( cegw_compl_init );
  77 DECLARE_COMPLETION( cegw_compl_exit );
  78 DEFINE_MUTEX( cegw_setting_mutex );
  79
  80 inline static void cegw_udp_send( struct socket* udp_sock, struct can_frame* cf, struct in_addr ipaddr, u16 port )
  81 {
  82         struct msghdr mh;
  83         struct sockaddr_in addr;
  84         struct kvec vec;
  85
  86         addr.sin_family = AF_INET;
  87         addr.sin_port = htons( port );
  88         addr.sin_addr = ipaddr;
  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( udp_sock, &mh, &vec, 1, sizeof( *cf ) );
 100 }
 101
 102 inline static void cegw_can_send( struct socket* can_sock, struct can_frame* cf, int ifindex )
 103 {
 104         struct msghdr mh;
 105         struct kvec vec;
 106         struct sockaddr_can addr;
 107
 108         addr.can_family = AF_CAN;
 109         addr.can_ifindex = ifindex;
 110
 111         mh.msg_name = &addr;
 112         mh.msg_namelen = sizeof( addr );
 113         mh.msg_control = NULL;
 114         mh.msg_controllen = 0;
 115         mh.msg_flags = 0;
 116
 117         vec.iov_base = cf;
 118         vec.iov_len = sizeof( *cf );
 119
 120         kernel_sendmsg( can_sock, &mh, &vec, 1, sizeof( *cf ) ); /* ToDo: handle error */
 121 }
 122
 123 /**
 124  * cegw_udp_can - performs udp->can routing
 125  * This function is run as a thread.
 126  */
 127 static int cegw_udp_can( void* data )
 128 {
 129         struct can_frame cf;
 130         struct kvec vec;
 131         struct msghdr mh;
 132         struct eth_can_gw* job;
 133         struct hlist_node* pos;
 134         int can_ifidx;
 135         int recv_size;
 136         struct sockaddr_in udp_addr;
 137         struct sockaddr_can can_addr;
 138
 139         can_addr.can_family = AF_CAN;
 140         can_addr.can_ifindex = 0;
 141
 142         udp_addr.sin_family = AF_INET;
 143         mutex_lock( &cegw_setting_mutex );
 144         udp_addr.sin_port = htons( cegw_setting.eth_port );
 145         udp_addr.sin_addr = cegw_setting.eth_addr;
 146         mutex_unlock( &cegw_setting_mutex );
 147
 148         mh.msg_name = NULL;
 149         mh.msg_namelen = 0;
 150         mh.msg_iov = NULL;
 151         mh.msg_iovlen = 0;
 152         mh.msg_control = NULL;
 153         mh.msg_controllen = 0;
 154         mh.msg_flags = 0;
 155
 156         if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &udp_sock) != 0 )
 157         {
 158                 printk( KERN_ERR "canethgw: udp socket creation failed\n" );
 159                 return -1;
 160         }
 161
 162         if( sock_create_kern( PF_CAN, SOCK_RAW, CAN_RAW, &can_sock) != 0 )
 163         {
 164                 printk( KERN_ERR "canethgw: can socket creation failed\n" );
 165                 return -1;
 166         }
 167
 168         if( kernel_bind( udp_sock, (struct sockaddr*)&udp_addr, sizeof( udp_addr ) ) != 0 )
 169         {
 170                 printk( KERN_ERR "canethgw: udp socket binding failed\n" );
 171                 sock_release( udp_sock );
 172                 sock_release( can_sock );
 173                 return -1;
 174         }
 175
 176         if( kernel_bind( can_sock, (struct sockaddr*) &can_addr, sizeof(can_addr) ) != 0 )
 177         {
 178                 printk( KERN_ERR "canethgw: can socket binding failed\n" );
 179                 sock_release( udp_sock );
 180                 sock_release( can_sock );
 181                 return -1;
 182         }
 183
 184         pr_devel( "canethgw: cegw_udp_can init complete\n" );
 185         complete_all( &cegw_compl_init ); /* ToDo: why _all? */
 186         pr_devel( "canethgw: cegw_udp_can is entering working cycle\n" );
 187
 188         while( 1 )
 189         {
 190                 if( cegw_state == CEGW_STOPPED )
 191                         break;
 192                 vec.iov_base = &cf;
 193                 vec.iov_len = sizeof(cf);
 194                 recv_size = kernel_recvmsg( udp_sock, &mh, &vec, 1, sizeof(cf), 0 ); /* ToDo: handle error, size check */
 195                 if( recv_size == 0 )
 196                 {
 197                         continue;
 198                 }
 199                 hlist_for_each_entry_rcu( job, pos, &eth_can_job, list )
 200                 {
 201                         rcu_read_lock(); /**/
 202                         can_ifidx = job->dst_if_idx;
 203                         rcu_read_unlock();
 204                         /* ToDo: from filter */
 205                         cegw_can_send( can_sock, &cf, can_ifidx );
 206                 }
 207         }
 208
 209         pr_devel( "canethgw: cegw_udp_can is ready for termination\n" );
 210         wait_for_completion( &cegw_compl_exit );
 211         sock_release( udp_sock );
 212         sock_release( can_sock );
 213         cegw_state = CEGW_STOPPED;  /* ToDo */
 214         pr_devel( "canethgw: cegw_udp_can terminates\n" );
 215         return 0;
 216 }
 217
 218 /**
 219  * cegw_can_udp - performs can->udp routing
 220  */
 221 static int cegw_can_udp( void* data )
 222 {
 223         struct msghdr mh;
 224         struct kvec vec;
 225         struct can_frame cf;
 226         struct sockaddr_can ca;
 227         struct can_eth_gw* job;
 228         struct hlist_node* pos;
 229         struct in_addr eth_addr;
 230         u16 eth_port;
 231         int recv_size;
 232
 233         mh.msg_name = &ca;
 234         mh.msg_namelen = sizeof( ca );
 235         mh.msg_control = NULL;
 236         mh.msg_controllen = 0;
 237         mh.msg_flags = 0;
 238
 239         wait_for_completion( &cegw_compl_init );
 240         pr_devel( "canethgw: cegw_can_udp is entering working cycle\n" );
 241
 242         while( 1 )
 243         {
 244                 if( cegw_state == CEGW_STOPPED ) /**/
 245                         break;
 246                 vec.iov_base = &cf;
 247                 vec.iov_len = sizeof( cf );
 248
 249                 recv_size = kernel_recvmsg( can_sock, &mh, &vec, 1, sizeof( cf ), 0 );
 250                 if( recv_size == 0 )
 251                 {
 252                         continue;
 253                 }
 254                 hlist_for_each_entry_rcu( job, pos, &can_eth_job, list )
 255                 {
 256                         rcu_read_lock();
 257                         eth_addr = job->dst_addr;
 258                         eth_port = job->dst_port;
 259                         rcu_read_unlock();
 260                         if( job->src_ifindex == ca.can_ifindex )
 261                                 cegw_udp_send( udp_sock, &cf, eth_addr, eth_port );
 262                 }
 263         }
 264
 265         complete_all( &cegw_compl_exit );
 266         pr_devel( "canethgw: cegw_can_udp terminates\n" );
 267         return 0;
 268 }
 269
 270 /* NetLink */
 271
 272 static int cegw_create_job( struct sk_buff* skb, struct nlmsghdr* nlh, void* arg )
 273 {
 274         struct nlattr* tb[ CEGW_MAX+1 ];
 275         struct can_eth_gw* cethgw = NULL;
 276         struct eth_can_gw* ecangw = NULL;
 277         int ifindex;
 278         u32 ip;
 279         u16 port;
 280         int err = 0;
 281
 282         /* ToDo: size check
 283         if (nlmsg_len(nlh) < sizeof(*r))
 284                 return -EINVAL;
 285         */
 286
 287         err = nlmsg_parse( nlh, sizeof( struct rtmsg ), tb, CEGW_MAX, NULL );
 288         if( err < 0 )
 289         {
 290                 printk( KERN_ERR "canethgw: nlmsg_parse error\n" );
 291                 return err;
 292         }
 293
 294         if( tb[CEGW_CMD_INFO] == NULL )
 295         {
 296                 printk( KERN_ERR "canethgw: bad cmd error\n" );
 297                 return -EINVAL;
 298         }
 299
 300         switch( *(int*)nla_data( tb[CEGW_CMD_INFO] ) )
 301         {
 302                 case CEGW_LISTEN:
 303                         if( cegw_state == CEGW_RUNNING )
 304                         {
 305                                 mutex_lock( &cegw_setting_mutex );
 306                                 cegw_setting.eth_addr = *(struct in_addr*)nla_data( tb[CEGW_ETH_IP] );
 307                                 cegw_setting.eth_port = *(u16*)nla_data( tb[CEGW_ETH_PORT] );
 308                                 mutex_unlock( &cegw_setting_mutex );
 309                                 kthread_run( cegw_thread_restart, NULL, "canethgw" );
 310                         } else
 311                         {
 312                                 mutex_lock( &cegw_setting_mutex );
 313                                 cegw_setting.eth_addr = *(struct in_addr*)nla_data( tb[CEGW_ETH_IP] );
 314                                 cegw_setting.eth_port = *(u16*)nla_data( tb[CEGW_ETH_PORT] );
 315                                 mutex_unlock( &cegw_setting_mutex );
 316                                 cegw_thread_start();
 317                         }
 318                         break;
 319                 case CEGW_RULE_CAN_ETH: /* new can->eth rule */
 320                 {
 321                         ifindex = *(int*)nla_data( tb[CEGW_CAN_IFINDEX] );
 322                         ip = *(u32*)nla_data( tb[CEGW_ETH_IP] );
 323                         port = *(u16*)nla_data( tb[CEGW_ETH_PORT] );
 324                         pr_devel( "canethgw: new can->eth rule - (%d)->(%x:%hu)\n", ifindex, ip, port );
 325
 326                         cethgw = kmalloc( sizeof(struct can_eth_gw), GFP_KERNEL );
 327                         if( cethgw == NULL )
 328                         {
 329                                 break;
 330                         }
 331                         cethgw->src_ifindex = ifindex;
 332                         cethgw->dst_addr = *(struct in_addr*)nla_data( tb[CEGW_ETH_IP] );
 333                         cethgw->dst_port = *(u16*)nla_data( tb[CEGW_ETH_PORT] );
 334
 335                         hlist_add_head_rcu( &cethgw->list, &can_eth_job );
 336                 }
 337                 break;
 338                 case CEGW_RULE_ETH_CAN: /* new eth->can rule */
 339                         printk( KERN_INFO "can:%d\n", *(int*)nla_data( tb[CEGW_CAN_IFINDEX] ) );
 340                         ecangw = kmalloc( sizeof(struct eth_can_gw), GFP_KERNEL );
 341                         if( ecangw == NULL )
 342                         {
 343                                 break;
 344                         }
 345                         ecangw->dst_if_idx = *(int*)nla_data( tb[CEGW_CAN_IFINDEX] );
 346                         hlist_add_head_rcu( &ecangw->list, &eth_can_job );
 347                         break;
 348                 default:
 349                         pr_devel( "canethgw: unknown CEGW_CMD_INFO\n" );
 350                         /* ToDo undef operation */
 351                         break;
 352         }
 353
 354         return 0;
 355 }
 356
 357 static int cegw_remove_job( struct sk_buff* skb, struct nlmsghdr* nlh, void* arg )
 358 {
 359         struct rtmsg* r;
 360         struct nlattr* tb[ CEGW_MAX+1 ];
 361         struct hlist_node* pos,* n;
 362         struct can_eth_gw* ceth;
 363         struct eth_can_gw* ecan;
 364
 365         int err = 0;
 366
 367         if( nlmsg_len(nlh) < sizeof(*r) )
 368                 return -EINVAL;
 369
 370         r = nlmsg_data( nlh );
 371
 372         if( r->rtm_family != AF_CAN )
 373                 return -EPFNOSUPPORT;
 374
 375         /*
 376         if( r->gwtype != CGW_TYPE_CAN_ETH_UDP )
 377                 return -EINVAL;
 378         */
 379         printk( "attrsize=%d\n", nlmsg_attrlen(nlh, sizeof(struct rtmsg)) );
 380
 381         err = nlmsg_parse( nlh, sizeof(struct rtmsg), tb, CEGW_MAX, NULL );
 382         if( err != 0 )
 383                 return -EINVAL;
 384
 385         if( tb[CEGW_CMD_INFO] == NULL )
 386                 return -EINVAL;
 387
 388         if( *(int*)nla_data( tb[CEGW_CMD_INFO] ) == CEGW_FLUSH )
 389         {
 390                 hlist_for_each_entry_safe( ceth, pos, n, &can_eth_job, list )
 391                 {
 392                         hlist_del( &ceth->list );
 393                         kfree( ceth );
 394                 }
 395                 hlist_for_each_entry_safe( ecan, pos, n, &eth_can_job, list )
 396                 {
 397                         hlist_del( &ecan->list );
 398                         kfree( ecan );
 399                 }
 400         }
 401         //      tb[]
 402         return 0;
 403 }
 404
 405 static int cegw_dump_job( struct sk_buff* skb, struct netlink_callback* cb )
 406 {
 407         struct can_eth_gw* ceth;
 408         struct eth_can_gw* ecan;
 409         struct hlist_node* pos;
 410         struct nlmsghdr* nlh;
 411         int idx = 0;
 412         int s_idx = cb->args[0];
 413         int ifidx, type;
 414         struct in_addr dst_ip;
 415         unsigned short dst_port;
 416
 417         rcu_read_lock();
 418         hlist_for_each_entry_rcu( ecan, pos, &eth_can_job, list )
 419         {
 420
 421         //      if( idx < s_idx )
 422         //              goto cont1;
 423
 424                 nlh = nlmsg_put( skb, 0, 0, 0, 0, 0 );
 425
 426                 ifidx = ecan->dst_if_idx;
 427                 type = CEGW_RULE_ETH_CAN;
 428                 nla_put( skb, CEGW_TYPE, sizeof(type), &type );
 429                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(type) );
 430
 431                 nla_put( skb, CEGW_CAN_IFINDEX, sizeof(ifidx), &ifidx ); /* ToDo return */
 432                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(ifidx) );
 433 cont1:
 434                 idx++;
 435         }
 436         rcu_read_unlock();
 437
 438         rcu_read_lock();
 439         hlist_for_each_entry_rcu( ceth, pos, &can_eth_job, list )
 440         {
 441         //      if( idx < s_idx )
 442         //              goto cont2;
 443
 444                 nlh = nlmsg_put( skb, 0, 0, 0, 0, 0 );
 445
 446                 ifidx = ceth->src_ifindex;
 447                 type = CEGW_RULE_CAN_ETH;
 448                 dst_ip = ceth->dst_addr;
 449                 dst_port = ceth->dst_port;
 450
 451                 nla_put( skb, CEGW_TYPE, sizeof(type), &type );
 452                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(type) );
 453
 454                 nla_put( skb, CEGW_CAN_IFINDEX, sizeof(ifidx), &ifidx ); /* ToDo return */
 455                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(ifidx) );
 456
 457                 nla_put( skb, CEGW_ETH_IP, sizeof(dst_ip), &dst_ip ); /* ToDo return */
 458                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(dst_ip) );
 459
 460                 nla_put( skb, CEGW_ETH_PORT, sizeof(dst_port), &dst_port ); /* ToDo return */
 461                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(dst_port) );
 462
 463                 //nla_put( skb, CGW_ETH_IP, sizeof() IP_ADDR  )
 464 cont2:
 465                 idx++;
 466         }
 467         rcu_read_unlock();
 468
 469         /* ToDo nlmsg_cancel */
 470         cb->args[0] = idx;
 471
 472         return skb->len;
 473 }
 474
 475 static void cegw_thread_start( void )
 476 {
 477         cegw_state = CEGW_RUNNING;
 478
 479         INIT_COMPLETION( cegw_compl_init );
 480         INIT_COMPLETION( cegw_compl_exit );
 481
 482         eth_to_can = kthread_create( cegw_udp_can, NULL, "canethgw" );
 483         if( !IS_ERR( eth_to_can ) )
 484         {
 485                 get_task_struct( eth_to_can );
 486                 wake_up_process( eth_to_can );
 487         }
 488         can_to_eth = kthread_run( cegw_can_udp, NULL, "canethgw" );
 489         if( !IS_ERR( can_to_eth ) )
 490         {
 491                 get_task_struct( can_to_eth );
 492                 wake_up_process( can_to_eth );
 493         }
 494
 495         /* ToDo: kthread creation fail */
 496         printk( KERN_INFO "threads are running\n" );
 497 }
 498
 499 /* ToDo: stop when no threads started */
 500 static void cegw_thread_stop( void )
 501 {
 502         int how = SHUT_RDWR;
 503         struct sock* sk = NULL;
 504
 505         /* be sure sockets exist */
 506         wait_for_completion( &cegw_compl_init );
 507         cegw_state = CEGW_STOPPED;
 508
 509         /* shut down socket */
 510         sk = can_sock->sk;
 511         how++;
 512         lock_sock( sk );
 513         sk->sk_shutdown |= how;
 514         sk->sk_state_change( sk );
 515         release_sock( sk );
 516
 517         kernel_sock_shutdown( udp_sock, SHUT_RDWR );
 518
 519         /* wait for return to reuse port if restart */
 520         kthread_stop( eth_to_can );
 521         kthread_stop( can_to_eth );
 522 }
 523
 524 /**
 525  * cegw_thread_restart
 526  */
 527 static int cegw_thread_restart( void* data )
 528 {
 529         printk( "restart\n" );
 530
 531         cegw_thread_stop();
 532         cegw_thread_start();
 533
 534         return 0;
 535 }
 536
 537 /***********************
 538  *   module init/exit
 539  ***********************/
 540
 541 static int __init cangw_init( void )
 542 {
 543         /* subscribe to netlink */
 544         if( __rtnl_register( PF_CAN, RTM_GETROUTE, NULL, cegw_dump_job, NULL ) != 0 )
 545         {
 546                 printk( KERN_ERR "error: rtnl_register fail\n" );
 547                 return -1;
 548         }
 549         __rtnl_register( PF_CAN, RTM_NEWROUTE, cegw_create_job, NULL, NULL );
 550         __rtnl_register( PF_CAN, RTM_DELROUTE, cegw_remove_job, NULL, NULL );
 551
 552         return 0;
 553 }
 554
 555 static void __exit cangw_exit( void )
 556 {
 557         if( cegw_state == CEGW_RUNNING )
 558         {
 559                 cegw_thread_stop();
 560                 /* ToDo: frees mem_cache?    */
 561                 /*       udp must not exists */
 562         }
 563
 564         /* ToDo: unregister netlink
 565          *       free jobs          */
 566         printk( "cangw: exit\n" );
 567 }
 568
 569 module_init( cangw_init );
 570 module_exit( cangw_exit );
 571