kernel/canethgw.c

   1 #include "gw.h" /* override */
   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 "canethgw.h"
  16
  17 MODULE_LICENSE( "GPL" );
  18
  19 static struct task_struct* eth_to_can, * can_to_eth;
  20 static struct socket* udp_sock;
  21 static struct socket* can_sock;
  22 static struct net_device* can_dev;
  23
  24 struct can_eth_gw
  25 {
  26         int src_if_idx;
  27         struct in_addr dst_addr;
  28         unsigned short dst_port;
  29         struct hlist_node list;
  30 };
  31
  32 struct eth_can_gw
  33 {
  34         int dst_if_idx;
  35         struct hlist_node list;
  36 };
  37
  38 HLIST_HEAD( can_eth_job );
  39 HLIST_HEAD( eth_can_job );
  40
  41 struct cegw_setting
  42 {
  43         struct can_filter filter;
  44         int src_idx;
  45         /* bind on if */
  46         struct in_addr dst_addr;
  47         unsigned short dst_port;
  48 };
  49
  50 /***********************
  51  *   UDP
  52  ***********************/
  53
  54 static int gw_udp_recv( void* data )
  55 {
  56         struct can_frame cf;
  57         struct kvec vec;
  58         struct msghdr mh;
  59         struct eth_can_gw* job;
  60         struct hlist_node* pos;
  61
  62         vec.iov_base = &cf;
  63         vec.iov_len = sizeof(cf);
  64
  65         mh.msg_name = NULL;
  66         mh.msg_namelen = 0;
  67         mh.msg_iov = NULL;
  68         mh.msg_iovlen = 0;
  69         mh.msg_control = NULL;
  70         mh.msg_controllen = 0;
  71         mh.msg_flags = 0;
  72
  73         while( 1 )
  74         {
  75                 if( kthread_should_stop() ) /* up() ?, recv is blocking */
  76                         break;
  77                 kernel_recvmsg( udp_sock, &mh, &vec, 1, sizeof(cf), 0 ); /* todo: handle error */
  78                 printk( "received udp msg_id:%d\n", cf.can_id );
  79                 rcu_read_lock();
  80                 hlist_for_each_entry_rcu( job, pos, &eth_can_job, list )
  81                 {
  82                         /* ToDo from filter */
  83                         gw_can_send( &cf, job->dst_if_idx );
  84                 }
  85                 rcu_read_unlock();
  86         }
  87
  88         return 0;
  89 }
  90
  91 inline static void gw_udp_send( struct can_frame* cf, struct in_addr ipaddr, u16 port )
  92 {
  93         struct msghdr mh;
  94         struct sockaddr_in addr;
  95         struct kvec vec;
  96
  97         addr.sin_family = AF_INET;
  98         addr.sin_port = htons( port );
  99         addr.sin_addr = ipaddr;
 100
 101         mh.msg_name = &addr;
 102         mh.msg_namelen = sizeof( addr );
 103         mh.msg_control = NULL;
 104         mh.msg_controllen = 0;
 105         mh.msg_flags = 0;
 106
 107         vec.iov_base = cf;
 108         vec.iov_len = sizeof( *cf );
 109
 110         kernel_sendmsg( udp_sock, &mh, &vec, 1, sizeof( *cf ) );
 111 }
 112
 113 /***********************
 114  *   CAN
 115  ***********************/
 116
 117 static int gw_can_recv( void* data )
 118 {
 119         struct msghdr mh;
 120         struct kvec vec;
 121         struct can_frame cf;
 122         struct sockaddr_can ca;
 123         struct can_eth_gw* job;
 124         struct hlist_node* pos;
 125
 126         mh.msg_name = &ca;
 127         mh.msg_namelen = sizeof( ca );
 128         mh.msg_control = NULL;
 129         mh.msg_controllen = 0;
 130         mh.msg_flags = 0;
 131
 132         vec.iov_base = &cf;
 133         vec.iov_len = sizeof( cf );
 134
 135         while( 1 )
 136         {
 137                 if( kthread_should_stop() ) /**/
 138                         break;
 139                 kernel_recvmsg( can_sock, &mh, &vec, 1, sizeof( cf ), 0 );
 140                 printk( "received can msg_id:%d, from:%d\n", cf.can_id, ca.can_ifindex );
 141                 rcu_read_lock();
 142                 hlist_for_each_entry_rcu( job, pos, &can_eth_job, list )
 143                 {
 144                         printk( KERN_INFO "%x\n", job->dst_addr );
 145                         if( job->src_if_idx == ca.can_ifindex )
 146                                 gw_udp_send( &cf, job->dst_addr, job->dst_port );
 147                 }
 148                 rcu_read_unlock();
 149         }
 150
 151         return 0;
 152 }
 153
 154 inline static void gw_can_send( struct can_frame* cf, int ifidx )
 155 {
 156         struct msghdr mh;
 157         struct kvec vec;
 158         struct sockaddr_can ca =
 159         {
 160                 .can_family = AF_CAN,
 161                 .can_ifindex = ifidx
 162         };
 163
 164         mh.msg_name = &ca;
 165         mh.msg_namelen = sizeof( ca );
 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( can_sock, &mh, &vec, 1, sizeof( *cf ) );
 174 }
 175
 176 /* NetLink */
 177
 178 static int cgw_create_job( struct sk_buff* skb, struct nlmsghdr* nlh,
 179                      void* arg )
 180 {
 181         struct nlattr* tb[ CGW_MAX+1 ];
 182         struct rtcanmsg *r;
 183         struct cgw_job *gwj;
 184         struct can_eth_gw* cethgw = NULL;
 185         struct eth_can_gw* ecangw = NULL;
 186         int err = 0;
 187
 188         if (nlmsg_len(nlh) < sizeof(*r))
 189                 return -EINVAL;
 190
 191         r = nlmsg_data(nlh);
 192         if (r->can_family != AF_CAN)
 193                 return -EPFNOSUPPORT;
 194
 195         err = nlmsg_parse( nlh, sizeof( struct rtcanmsg ), tb, CGW_MAX, NULL );
 196         if( err < 0 )
 197         {
 198                 printk( KERN_ERR "error: nlmsg_parse\n" );
 199                 return err;
 200         }
 201
 202         /* so far we only support CAN -> CAN routings */
 203         switch( r->gwtype )
 204         {
 205                 case CGW_TYPE_CAN_ETH_UDP:
 206                         printk( KERN_INFO "can:%d\n", *(int*)nla_data( tb[CGW_CAN_IF] ) );
 207                         printk( KERN_INFO "eth addr:%x\n", *(u32*)nla_data( tb[CGW_ETH_IP] ) );
 208                         printk( KERN_INFO "eth port:%hu\n", *(u16*)nla_data( tb[CGW_ETH_PORT] ) );
 209                         cethgw = kmalloc( sizeof(struct can_eth_gw), GFP_KERNEL );
 210                         if( cethgw == NULL )
 211                         {
 212                                 printk( KERN_ERR "error: kmalloc\n" );
 213                                 break;
 214                         }
 215                         cethgw->src_if_idx = *(int*)nla_data( tb[CGW_CAN_IF] );
 216                         cethgw->dst_addr = *(struct in_addr*)nla_data( tb[CGW_ETH_IP] );
 217                         cethgw->dst_port = *(u16*)nla_data( tb[CGW_ETH_PORT] );
 218
 219                         hlist_add_head_rcu( &cethgw->list, &can_eth_job );
 220                         break;
 221                 case CGW_TYPE_ETH_CAN_UDP:
 222                         printk( KERN_INFO "can:%d\n", *(int*)nla_data( tb[CGW_CAN_IF] ) );
 223                         ecangw = kmalloc( sizeof(struct eth_can_gw), GFP_KERNEL );
 224                         if( ecangw == NULL )
 225                         {
 226                                 printk( KERN_ERR "error: kmalloc\n" );
 227                                 break;
 228                         }
 229                         ecangw->dst_if_idx = *(int*)nla_data( tb[CGW_CAN_IF] );
 230                         hlist_add_head_rcu( &ecangw->list, &eth_can_job );
 231                         break;
 232                 default:
 233                         /* ToDo undef operation */
 234                         break;
 235         }
 236
 237         return 0;
 238 }
 239
 240 /***********************
 241  *   module init/exit
 242  ***********************/
 243
 244 static int __init cangw_init( void )
 245 {
 246         struct sockaddr_in udp_addr;
 247         struct sockaddr_can can_addr;
 248         int ifidx = 0;
 249
 250         /* 1. create can socket and bind to it */
 251         can_dev = dev_get_by_name( &init_net, "vcan0" ); /* net ns?, release counter! */
 252         if( can_dev == NULL )
 253         {
 254                 printk( KERN_ERR "error: vcan0 not found\n" );
 255                 return -1;
 256         }
 257         ifidx = can_dev->ifindex;
 258         dev_put( can_dev );
 259
 260         if( sock_create_kern( PF_CAN, SOCK_RAW, CAN_RAW, &can_sock) != 0 )
 261         {
 262                 printk( KERN_ERR "error: can_sock creation failed\n" );
 263                 return -1;
 264         }
 265
 266         can_addr.can_family = AF_CAN;
 267         can_addr.can_ifindex = ifidx;
 268
 269         if( can_sock->ops->bind( can_sock, (struct sockaddr*) &can_addr, sizeof(can_addr) ) != 0 )
 270         {
 271                 printk( KERN_ERR "can_sock bind failed\n" );
 272                 return -1;
 273         }
 274
 275         /* 2. create udp socket and bind to it */
 276         if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &udp_sock ) != 0 )
 277         {
 278                 printk( "error: udp_sock creation failed\n" );
 279                 sock_release( can_sock );
 280                 return -1;
 281         }
 282
 283         udp_addr.sin_family = AF_INET;
 284         udp_addr.sin_port = htons( 10501 );
 285         udp_addr.sin_addr.s_addr = INADDR_ANY;
 286
 287         if( udp_sock->ops->bind( udp_sock, (struct sockaddr*)&udp_addr, sizeof( udp_addr ) ) != 0 ) /* ref impl ?!? */
 288         {
 289                 printk( "error: binding failed\n" );
 290                 sock_release( udp_sock );
 291                 sock_release( can_sock );
 292                 return -1;
 293         }
 294
 295         /* 3. subscribe to netlink */
 296         //if( __rtnl_register( PF_CAN, RTM_GETROUTE,  ) != 0 )
 297         __rtnl_register( PF_CAN, RTM_NEWROUTE, cgw_create_job, NULL, NULL );
 298         //__rtnl_register( PF_CAN, RTM_DELROUTE,  )
 299
 300         /* 4. run threads */
 301         eth_to_can = kthread_run( gw_udp_recv, NULL, "cangw" );
 302         can_to_eth = kthread_run( gw_can_recv, NULL, "cangw" );
 303
 304         /*
 305         if( sock_create_kern( AF_CAN, SOCK_RAW, CAN_RAW, &can_sock ) != 0 )
 306         {s
 307                 printk( "error: can_sock creation failed\n" );
 308         }
 309         */
 310
 311         return 0;
 312 }
 313
 314 static void __exit cangw_exit( void )
 315 {
 316         sock_release( udp_sock );
 317         sock_release( can_sock );
 318
 319         printk( "cangw: exit\n" );
 320         //kthread_stop( ts );
 321 }
 322
 323 module_init( cangw_init );
 324 module_exit( cangw_exit );
 325