kernel/canethgw.c

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