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 cgw_create_job( struct sk_buff* skb, struct nlmsghdr* nlh,
 198                      void* arg )
 199 {
 200         struct nlattr* tb[ CGW_MAX+1 ];
 201         struct rtcanmsg *r;
 202         struct can_eth_gw* cethgw = NULL;
 203         struct eth_can_gw* ecangw = NULL;
 204         int err = 0;
 205
 206         if (nlmsg_len(nlh) < sizeof(*r))
 207                 return -EINVAL;
 208
 209         r = nlmsg_data(nlh);
 210         if (r->can_family != AF_CAN)
 211                 return -EPFNOSUPPORT;
 212
 213         err = nlmsg_parse( nlh, sizeof( struct rtcanmsg ), tb, CGW_MAX, NULL );
 214         if( err < 0 )
 215         {
 216                 printk( KERN_ERR "error: nlmsg_parse\n" );
 217                 return err;
 218         }
 219
 220         switch( r->gwtype )
 221         {
 222                 case CGW_TYPE_CONFIG:
 223                         listen( 0,  *(struct in_addr*)nla_data( tb[CGW_LISTEN_IP] ),
 224                                     *(u16*)nla_data( tb[CGW_LISTEN_PORT] ) );
 225                         break;
 226                 case CGW_TYPE_CAN_ETH_UDP:
 227                         printk( KERN_INFO "can:%d\n", *(int*)nla_data( tb[CGW_CAN_IF] ) );
 228                         printk( KERN_INFO "eth addr:%x\n", *(u32*)nla_data( tb[CGW_ETH_IP] ) );
 229                         printk( KERN_INFO "eth port:%hu\n", *(u16*)nla_data( tb[CGW_ETH_PORT] ) );
 230                         cethgw = kmalloc( sizeof(struct can_eth_gw), GFP_KERNEL );
 231                         if( cethgw == NULL )
 232                         {
 233                                 printk( KERN_ERR "error: kmalloc\n" );
 234                                 break;
 235                         }
 236                         cethgw->src_if_idx = *(int*)nla_data( tb[CGW_CAN_IF] );
 237                         cethgw->dst_addr = *(struct in_addr*)nla_data( tb[CGW_ETH_IP] );
 238                         cethgw->dst_port = *(u16*)nla_data( tb[CGW_ETH_PORT] );
 239
 240                         hlist_add_head_rcu( &cethgw->list, &can_eth_job );
 241                         break;
 242                 case CGW_TYPE_ETH_CAN_UDP:
 243                         printk( KERN_INFO "can:%d\n", *(int*)nla_data( tb[CGW_CAN_IF] ) );
 244                         ecangw = kmalloc( sizeof(struct eth_can_gw), GFP_KERNEL );
 245                         if( ecangw == NULL )
 246                         {
 247                                 printk( KERN_ERR "error: kmalloc\n" );
 248                                 break;
 249                         }
 250                         ecangw->dst_if_idx = *(int*)nla_data( tb[CGW_CAN_IF] );
 251                         hlist_add_head_rcu( &ecangw->list, &eth_can_job );
 252                         break;
 253                 default:
 254                         printk( "default" );
 255                         /* ToDo undef operation */
 256                         break;
 257         }
 258
 259         return 0;
 260 }
 261
 262 static int listen( int can_ifidx, struct in_addr eth_addr, u16 eth_port )
 263 {
 264         struct sockaddr_in udp_addr;
 265         struct sockaddr_can can_addr;
 266         struct socket* tmp;
 267
 268         printk( KERN_INFO "listen called\n" );
 269
 270         if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &tmp) != 0 )
 271         {
 272                 printk( KERN_ERR "error: can_sock creation failed\n" );
 273                 return -1;
 274         }
 275
 276         can_addr.can_family = AF_CAN;
 277         can_addr.can_ifindex = can_ifidx;
 278
 279         if( can_sock->ops->bind( can_sock, (struct sockaddr*) &can_addr, sizeof(can_addr) ) != 0 )
 280         {
 281                 printk( KERN_ERR "can_sock bind failed\n" );
 282                 return -1;
 283         }
 284
 285         printk( KERN_INFO "can socket success\n" );
 286
 287         udp_addr.sin_family = AF_INET;
 288         udp_addr.sin_port = htons( eth_port );
 289         udp_addr.sin_addr = eth_addr;
 290
 291         printk( KERN_INFO "trying to bind\n" );
 292         if( udp_sock->ops->bind( udp_sock, (struct sockaddr*)&udp_addr, sizeof( udp_addr ) ) != 0 ) /* ref impl ?!? */
 293         {
 294                 printk( "error: binding failed\n" );
 295                 sock_release( udp_sock );
 296                 sock_release( can_sock );
 297                 return -1;
 298         }
 299
 300         printk( KERN_INFO "socket established\n" );
 301
 302         /* run threads */
 303         eth_to_can = kthread_run( gw_udp_recv, NULL, "ethcangw" );
 304         can_to_eth = kthread_run( gw_can_recv, NULL, "canethgw" );
 305
 306         printk( KERN_INFO "threads are running\n" );
 307
 308         gw_state = CEGW_RUNNING;
 309
 310         return 0;
 311 }
 312
 313 /***********************
 314  *   module init/exit
 315  ***********************/
 316
 317 static int __init cangw_init( void )
 318 {
 319         if( sock_create_kern( PF_CAN, SOCK_RAW, CAN_RAW, &can_sock) != 0 )
 320         {
 321                 printk( KERN_ERR "error: can_sock creation failed\n" );
 322                 return -1;
 323         }
 324
 325         if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &udp_sock ) != 0 )
 326         {
 327                 printk( KERN_ERR "error: udp_sock creation failed\n" );
 328                 sock_release( can_sock );
 329                 return -1;
 330         }
 331
 332         /* subscribe to netlink */
 333         //if( __rtnl_register( PF_CAN, RTM_GETROUTE,  ) != 0 )
 334         __rtnl_register( PF_CAN, RTM_NEWROUTE, cgw_create_job, NULL, NULL );
 335         //__rtnl_register( PF_CAN, RTM_DELROUTE,  )
 336
 337         /*
 338         if( sock_create_kern( AF_CAN, SOCK_RAW, CAN_RAW, &can_sock ) != 0 )
 339         {s
 340                 printk( "error: can_sock creation failed\n" );
 341         }
 342         */
 343
 344         return 0;
 345 }
 346
 347 static void __exit cangw_exit( void )
 348 {
 349         if( gw_state == CEGW_RUNNING )
 350         {
 351                 sock_release( udp_sock );
 352                 sock_release( can_sock );
 353                 /* ToDo: stop threads */
 354         }
 355
 356         printk( "cangw: exit\n" );
 357         //kthread_stop( ts );
 358 }
 359
 360 module_init( cangw_init );
 361 module_exit( cangw_exit );
 362