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  *  [ ] check every input
  18  *  [ ] refactor
  19  */
  20
  21 MODULE_LICENSE( "GPL" );
  22
  23 static int  gw_udp_recv( void* data );
  24 static void gw_udp_send( struct can_frame* cf, struct in_addr ipaddr, u16 port );
  25 static int  gw_can_recv( void* data );
  26 static void gw_can_send( struct can_frame* cf, int ifidx );
  27 static int listen( int can_ifidx, struct in_addr eth_addr, u16 eth_port );
  28
  29 #define CEGW_STOPPED 0
  30 #define CEGW_RUNNING 1
  31
  32 static struct task_struct* eth_to_can, * can_to_eth;
  33 static struct socket* udp_sock = NULL;
  34 static struct socket* can_sock = NULL;
  35 static int gw_state = CEGW_STOPPED;
  36
  37 struct can_eth_gw
  38 {
  39         int src_if_idx;
  40         struct in_addr dst_addr;
  41         unsigned short dst_port;
  42         struct hlist_node list;
  43 };
  44
  45 struct eth_can_gw
  46 {
  47         int dst_if_idx;
  48         struct hlist_node list;
  49 };
  50
  51 HLIST_HEAD( can_eth_job );
  52 HLIST_HEAD( eth_can_job );
  53
  54 struct cegw_setting
  55 {
  56         struct can_filter filter;
  57         int src_idx;
  58         /* bind on if */
  59         struct in_addr dst_addr;
  60         unsigned short dst_port;
  61 };
  62
  63 /***********************
  64  *   UDP
  65  ***********************/
  66
  67 static int gw_udp_recv( void* data )
  68 {
  69         struct can_frame cf;
  70         struct kvec vec;
  71         struct msghdr mh;
  72         struct eth_can_gw* job;
  73         struct hlist_node* pos;
  74         int can_ifidx;
  75
  76         vec.iov_base = &cf;
  77         vec.iov_len = sizeof(cf);
  78
  79         mh.msg_name = NULL;
  80         mh.msg_namelen = 0;
  81         mh.msg_iov = NULL;
  82         mh.msg_iovlen = 0;
  83         mh.msg_control = NULL;
  84         mh.msg_controllen = 0;
  85         mh.msg_flags = 0;
  86
  87         while( 1 )
  88         {
  89                 if( kthread_should_stop() ) /* up() ?, recv is blocking */
  90                         break;
  91                 kernel_recvmsg( udp_sock, &mh, &vec, 1, sizeof(cf), 0 ); /* todo: handle error */
  92                 printk( "received udp msg_id:%d\n", cf.can_id );
  93                 hlist_for_each_entry_rcu( job, pos, &eth_can_job, list )
  94                 {
  95                         rcu_read_lock(); /**/
  96                         can_ifidx = job->dst_if_idx;
  97                         rcu_read_unlock();
  98                         /* ToDo from filter */
  99                         gw_can_send( &cf, can_ifidx );
 100                 }
 101         }
 102
 103         return 0;
 104 }
 105
 106 inline static void gw_udp_send( struct can_frame* cf, struct in_addr ipaddr, u16 port )
 107 {
 108         struct msghdr mh;
 109         struct sockaddr_in addr;
 110         struct kvec vec;
 111
 112         addr.sin_family = AF_INET;
 113         addr.sin_port = htons( port );
 114         addr.sin_addr = ipaddr;
 115
 116         mh.msg_name = &addr;
 117         mh.msg_namelen = sizeof( addr );
 118         mh.msg_control = NULL;
 119         mh.msg_controllen = 0;
 120         mh.msg_flags = 0;
 121
 122         vec.iov_base = cf;
 123         vec.iov_len = sizeof( *cf );
 124
 125         kernel_sendmsg( udp_sock, &mh, &vec, 1, sizeof( *cf ) );
 126 }
 127
 128 /***********************
 129  *   CAN
 130  ***********************/
 131
 132 static int gw_can_recv( void* data )
 133 {
 134         struct msghdr mh;
 135         struct kvec vec;
 136         struct can_frame cf;
 137         struct sockaddr_can ca;
 138         struct can_eth_gw* job;
 139         struct hlist_node* pos;
 140         struct in_addr eth_addr;
 141         u16 eth_port;
 142
 143         mh.msg_name = &ca;
 144         mh.msg_namelen = sizeof( ca );
 145         mh.msg_control = NULL;
 146         mh.msg_controllen = 0;
 147         mh.msg_flags = 0;
 148
 149         vec.iov_base = &cf;
 150         vec.iov_len = sizeof( cf );
 151
 152         while( 1 )
 153         {
 154                 if( kthread_should_stop() ) /**/
 155                         break;
 156                 kernel_recvmsg( can_sock, &mh, &vec, 1, sizeof( cf ), 0 );
 157                 printk( "received can msg_id:%d, from:%d\n", cf.can_id, ca.can_ifindex );
 158                 hlist_for_each_entry_rcu( job, pos, &can_eth_job, list )
 159                 {
 160                         rcu_read_lock();
 161                         eth_addr = job->dst_addr;
 162                         eth_port = job->dst_port;
 163                         rcu_read_unlock();
 164                         printk( KERN_INFO "%x\n", eth_addr );
 165                         if( job->src_if_idx == ca.can_ifindex )
 166                                 gw_udp_send( &cf, eth_addr, eth_port );
 167                 }
 168         }
 169
 170         return 0;
 171 }
 172
 173 inline static void gw_can_send( struct can_frame* cf, int ifidx )
 174 {
 175         struct msghdr mh;
 176         struct kvec vec;
 177         struct sockaddr_can ca =
 178         {
 179                 .can_family = AF_CAN,
 180                 .can_ifindex = ifidx
 181         };
 182
 183         mh.msg_name = &ca;
 184         mh.msg_namelen = sizeof( ca );
 185         mh.msg_control = NULL;
 186         mh.msg_controllen = 0;
 187         mh.msg_flags = 0;
 188
 189         vec.iov_base = cf;
 190         vec.iov_len = sizeof( *cf );
 191
 192         kernel_sendmsg( can_sock, &mh, &vec, 1, sizeof( *cf ) );
 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[ CGW_MAX+1 ];
 200         struct rtcanmsg *r;
 201         struct can_eth_gw* cethgw = NULL;
 202         struct eth_can_gw* ecangw = NULL;
 203         int err = 0;
 204
 205         if (nlmsg_len(nlh) < sizeof(*r))
 206                 return -EINVAL;
 207
 208         r = nlmsg_data(nlh);
 209         if (r->can_family != AF_CAN)
 210                 return -EPFNOSUPPORT;
 211
 212         err = nlmsg_parse( nlh, sizeof( struct rtcanmsg ), tb, CGW_MAX, NULL );
 213         if( err < 0 )
 214         {
 215                 printk( KERN_ERR "error: nlmsg_parse\n" );
 216                 return err;
 217         }
 218
 219         switch( r->gwtype )
 220         {
 221                 case CGW_TYPE_CONFIG:
 222                         listen( 0,  *(struct in_addr*)nla_data( tb[CGW_LISTEN_IP] ),
 223                                     *(u16*)nla_data( tb[CGW_LISTEN_PORT] ) );
 224                         break;
 225                 case CGW_TYPE_CAN_ETH_UDP:
 226                         printk( KERN_INFO "can:%d\n", *(int*)nla_data( tb[CGW_CAN_IF] ) );
 227                         printk( KERN_INFO "eth addr:%x\n", *(u32*)nla_data( tb[CGW_ETH_IP] ) );
 228                         printk( KERN_INFO "eth port:%hu\n", *(u16*)nla_data( tb[CGW_ETH_PORT] ) );
 229                         cethgw = kmalloc( sizeof(struct can_eth_gw), GFP_KERNEL );
 230                         if( cethgw == NULL )
 231                         {
 232                                 printk( KERN_ERR "error: kmalloc\n" );
 233                                 break;
 234                         }
 235                         cethgw->src_if_idx = *(int*)nla_data( tb[CGW_CAN_IF] );
 236                         cethgw->dst_addr = *(struct in_addr*)nla_data( tb[CGW_ETH_IP] );
 237                         cethgw->dst_port = *(u16*)nla_data( tb[CGW_ETH_PORT] );
 238
 239                         hlist_add_head_rcu( &cethgw->list, &can_eth_job );
 240                         break;
 241                 case CGW_TYPE_ETH_CAN_UDP:
 242                         printk( KERN_INFO "can:%d\n", *(int*)nla_data( tb[CGW_CAN_IF] ) );
 243                         ecangw = kmalloc( sizeof(struct eth_can_gw), GFP_KERNEL );
 244                         if( ecangw == NULL )
 245                         {
 246                                 printk( KERN_ERR "error: kmalloc\n" );
 247                                 break;
 248                         }
 249                         ecangw->dst_if_idx = *(int*)nla_data( tb[CGW_CAN_IF] );
 250                         hlist_add_head_rcu( &ecangw->list, &eth_can_job );
 251                         break;
 252                 default:
 253                         printk( "default" );
 254                         /* ToDo undef operation */
 255                         break;
 256         }
 257
 258         return 0;
 259 }
 260
 261 static int cegw_remove_job( struct sk_buff* skb, struct nlmsghdr* nlh, void* arg )
 262 {
 263         struct rtcanmsg* r;
 264         struct nlattr* tb[ CGW_MAX+1 ];
 265
 266         if( nlmsg_len(nlh) < sizeof(*r) )
 267                 return -EINVAL;
 268
 269         r = nlmsg_data( nlh );
 270
 271         if( r->can_family != AF_CAN )
 272                 return -EPFNOSUPPORT;
 273
 274         if( r->gwtype != CGW_TYPE_CAN_ETH_UDP )
 275                 return -EINVAL;
 276
 277         printk( "flags:%x",r->flags );
 278         //nlmsg_parse( nlh, sizeof(*r), tb, CGW_MAX, NULL );
 279
 280         //      tb[]
 281         return 0;
 282 }
 283
 284 static int listen( int can_ifidx, struct in_addr eth_addr, u16 eth_port )
 285 {
 286         struct sockaddr_in udp_addr;
 287         struct sockaddr_can can_addr;
 288         struct socket* tmp;
 289
 290         printk( KERN_INFO "listen called\n" );
 291
 292         if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &tmp) != 0 )
 293         {
 294                 printk( KERN_ERR "error: can_sock creation failed\n" );
 295                 return -1;
 296         }
 297
 298         can_addr.can_family = AF_CAN;
 299         can_addr.can_ifindex = can_ifidx;
 300
 301         if( can_sock->ops->bind( can_sock, (struct sockaddr*) &can_addr, sizeof(can_addr) ) != 0 )
 302         {
 303                 printk( KERN_ERR "can_sock bind failed\n" );
 304                 return -1;
 305         }
 306
 307         printk( KERN_INFO "can socket success\n" );
 308
 309         udp_addr.sin_family = AF_INET;
 310         udp_addr.sin_port = htons( eth_port );
 311         udp_addr.sin_addr = eth_addr;
 312
 313         printk( KERN_INFO "trying to bind\n" );
 314         if( udp_sock->ops->bind( udp_sock, (struct sockaddr*)&udp_addr, sizeof( udp_addr ) ) != 0 ) /* ref impl ?!? */
 315         {
 316                 printk( "error: binding failed\n" );
 317                 sock_release( udp_sock );
 318                 sock_release( can_sock );
 319                 return -1;
 320         }
 321
 322         printk( KERN_INFO "socket established\n" );
 323
 324         /* run threads */
 325         eth_to_can = kthread_run( gw_udp_recv, NULL, "ethcangw" );
 326         can_to_eth = kthread_run( gw_can_recv, NULL, "canethgw" );
 327
 328         printk( KERN_INFO "threads are running\n" );
 329
 330         gw_state = CEGW_RUNNING;
 331
 332         return 0;
 333 }
 334
 335 /***********************
 336  *   module init/exit
 337  ***********************/
 338
 339 static int __init cangw_init( void )
 340 {
 341         if( sock_create_kern( PF_CAN, SOCK_RAW, CAN_RAW, &can_sock) != 0 )
 342         {
 343                 printk( KERN_ERR "error: can_sock creation failed\n" );
 344                 return -1;
 345         }
 346
 347         if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &udp_sock ) != 0 )
 348         {
 349                 printk( KERN_ERR "error: udp_sock creation failed\n" );
 350                 sock_release( can_sock );
 351                 return -1;
 352         }
 353
 354         /* subscribe to netlink */
 355         //if( __rtnl_register( PF_CAN, RTM_GETROUTE,  ) != 0 )
 356         __rtnl_register( PF_CAN, RTM_NEWROUTE, cegw_create_job, NULL, NULL );
 357         __rtnl_register( PF_CAN, RTM_DELROUTE, cegw_remove_job, NULL, NULL );
 358         //__rtnl_register( PF_CAN, RTM_DELROUTE,  )
 359
 360         /*
 361         if( sock_create_kern( AF_CAN, SOCK_RAW, CAN_RAW, &can_sock ) != 0 )
 362         {s
 363                 printk( "error: can_sock creation failed\n" );
 364         }
 365         */
 366
 367         return 0;
 368 }
 369
 370 static void __exit cangw_exit( void )
 371 {
 372         if( gw_state == CEGW_RUNNING )
 373         {
 374                 sock_release( udp_sock );
 375                 sock_release( can_sock );
 376                 /* ToDo: stop threads */
 377         }
 378
 379         /* ToDo: unregister netlink */
 380         printk( "cangw: exit\n" );
 381         //kthread_stop( ts );
 382 }
 383
 384 module_init( cangw_init );
 385 module_exit( cangw_exit );
 386