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