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